Serveral rewrite and performance optimization to make I2C running.