Correct i2c stub implementation