i2c: further improvements and working test case
TODO: update