Fixed I2C write/read functions