Add i2c1 and implement timeout to avoid infinite loop in case of problem.