Adding some i2c register reading, totally untested...