Added I2C init code

Signed-off-by: Dimitar Marinov <dimitar.m.marinov@abv.bg>