Fix i2c implementation