i2c: add error checking and do some clean-ups.

Change-Id: I50a54357b4a2b94f43f25521a123b00fc73957c2