diff --git a/firmware/drivers/generic_i2c.c b/firmware/drivers/generic_i2c.c
index 2e5f202307..0dcc957260 100644
--- a/firmware/drivers/generic_i2c.c
+++ b/firmware/drivers/generic_i2c.c
@@ -23,14 +23,17 @@
 #include <stdbool.h>
 #include <stdlib.h>
 #include "debug.h"
-#include "logf.h"
 #include "generic_i2c.h"
 
+#define MAX_I2C_INTERFACES 5
+
 int i2c_num_ifs = 0;
-struct i2c_interface *i2c_if[5];
+struct i2c_interface *i2c_if[MAX_I2C_INTERFACES];
 
 static void i2c_start(struct i2c_interface *iface)
 {
+    iface->sda_output();
+
     iface->sda_hi();
     iface->scl_hi();
     iface->sda_lo();
@@ -40,6 +43,8 @@ static void i2c_start(struct i2c_interface *iface)
 
 static void i2c_stop(struct i2c_interface *iface)
 {
+    iface->sda_output();
+
     iface->sda_lo();
     iface->scl_hi();
     iface->delay_su_sto();
@@ -48,6 +53,7 @@ static void i2c_stop(struct i2c_interface *iface)
 
 static void i2c_ack(struct i2c_interface *iface, bool ack)
 {
+    iface->sda_output();
     iface->scl_lo();
     if ( ack )
         iface->sda_lo();
@@ -67,10 +73,10 @@ static int i2c_getack(struct i2c_interface *iface)
     iface->sda_input();
     iface->delay_su_dat();
     iface->scl_hi();
-    
+
     if (iface->sda())
         ret = 0; /* ack failed */
-    
+
     iface->delay_thigh();
     iface->scl_lo();
     iface->sda_hi();
@@ -83,7 +89,9 @@ static unsigned char i2c_inb(struct i2c_interface *iface, bool ack)
 {
     int i;
     unsigned char byte = 0;
-    
+
+    iface->sda_input();
+
     /* clock in each bit, MSB first */
     for ( i=0x80; i; i>>=1 ) {
         iface->sda_input();
@@ -95,27 +103,29 @@ static unsigned char i2c_inb(struct i2c_interface *iface, bool ack)
         iface->delay_hd_dat();
         iface->sda_output();
     }
-    
+
     i2c_ack(iface, ack);
-    
+
     return byte;
 }
 
 static void i2c_outb(struct i2c_interface *iface, unsigned char byte)
 {
-   int i;
+    int i;
 
-   /* clock out each bit, MSB first */
-   for (i=0x80; i; i>>=1) {
-       if (i & byte)
-           iface->sda_hi();
-       else
-           iface->sda_lo();
-       iface->delay_su_dat();
-       iface->scl_hi();
-       iface->delay_thigh();
-       iface->scl_lo();
-       iface->delay_hd_dat();
+    iface->sda_output();
+
+    /* clock out each bit, MSB first */
+    for (i=0x80; i; i>>=1) {
+        if (i & byte)
+            iface->sda_hi();
+        else
+            iface->sda_lo();
+        iface->delay_su_dat();
+        iface->scl_hi();
+        iface->delay_thigh();
+        iface->scl_lo();
+        iface->delay_hd_dat();
    }
 
    iface->sda_hi();
@@ -140,27 +150,36 @@ int i2c_write_data(int bus_address, int address,
     struct i2c_interface *iface = find_if(bus_address);
     if(!iface)
         return -1;
-    
+
     i2c_start(iface);
     i2c_outb(iface, iface->address & 0xfe);
-    if (i2c_getack(iface)) {
-        i2c_outb(iface, address);
-        if (i2c_getack(iface)) {
-            for(i = 0;i < count;i++) {
-                i2c_outb(iface, buf[i]);
-                if (!i2c_getack(iface)) {
-                    ret = -3;
-                    break;
-                }
-            }
-        } else {
-            ret = -2;
-        }
-    } else {
-        logf("i2c_write_data() - no ack\n");
-        ret = -1;
+    if (!i2c_getack(iface))
+    {
+        ret = -2;
+        goto end;
     }
-    
+
+    if (address != -1)
+    {
+        i2c_outb(iface, address);
+        if (!i2c_getack(iface))
+        {
+            ret = -3;
+            goto end;
+        }
+    }
+
+    for(i = 0;i < count;i++)
+    {
+        i2c_outb(iface, buf[i]);
+        if (!i2c_getack(iface))
+        {
+            ret = -4;
+            break;
+        }
+    }
+
+end:
     i2c_stop(iface);
     return ret;
 }
@@ -173,34 +192,50 @@ int i2c_read_data(int bus_address, int address,
     struct i2c_interface *iface = find_if(bus_address);
     if(!iface)
         return -1;
-    
-    i2c_start(iface);
-    i2c_outb(iface, iface->address & 0xfe);
-    if (i2c_getack(iface)) {
-        i2c_outb(iface, address);
-        if (i2c_getack(iface)) {
-            i2c_start(iface);
-            i2c_outb(iface, iface->address | 1);
-            if (i2c_getack(iface)) {
-                for(i = 0;i < count-1;i++)
-                    buf[i] = i2c_inb(iface, true);
-                
-                buf[i] = i2c_inb(iface, false);
-            } else {
-                ret = -3;
-            }
-        } else {
+
+    if (address != -1)
+    {
+        i2c_start(iface);
+        i2c_outb(iface, iface->address & 0xfe);
+        if (!i2c_getack(iface))
+        {
             ret = -2;
+            goto end;
+        }
+        i2c_outb(iface, address);
+        if (!i2c_getack(iface))
+        {
+            ret = -3;
+            goto end;
         }
-    } else {
-        ret = -1;
     }
-    
+
+    i2c_start(iface);
+    i2c_outb(iface, iface->address | 1);
+    if (!i2c_getack(iface))
+    {
+        ret = -4;
+        goto end;
+    }
+
+    for(i = 0;i < count-1;i++)
+        buf[i] = i2c_inb(iface, true);
+
+    buf[i] = i2c_inb(iface, false);
+
+end:
     i2c_stop(iface);
     return ret;
 }
 
-void i2c_add_node(struct i2c_interface *iface)
+int i2c_add_node(struct i2c_interface *iface)
 {
+    if (i2c_num_ifs == MAX_I2C_INTERFACES)
+        return -1;
+
     i2c_if[i2c_num_ifs++] = iface;
+
+    iface->scl_output();
+
+    return 0;
 }
diff --git a/firmware/export/generic_i2c.h b/firmware/export/generic_i2c.h
index 7b598039fa..f38728589d 100644
--- a/firmware/export/generic_i2c.h
+++ b/firmware/export/generic_i2c.h
@@ -47,7 +47,7 @@ struct i2c_interface
     void (*delay_thigh)(void);  /* SCL high period (tHIGH)     4.0us/0.6us */
 };
 
-extern void i2c_add_node(struct i2c_interface *iface);
+extern int i2c_add_node(struct i2c_interface *iface);
 extern int i2c_write_data(int bus_address, int address,
                           const unsigned char* buf, int count);
 extern int i2c_read_data(int bus_address, int address,