Rewrite i2c.h with MUX support