Working on systick i2c drivers