Add I2C Rx code

This is not working correctly yet