1 files changed, 38 insertions, 10 deletions

diff --git a/app/oled.c b/app/oled.c
index 51cfd9a..69b2580 100644
--- a/app/oled.c
+++ b/app/oled.c
@@ -8,27 +8,41 @@ uint8_t update_buf[DMA_BUF_SZ];
 static int dma_in_progress = 0;
 static int refresh_enabled = 0;
 static uint32_t refresh_wdt = 0;
+static int oled_sad=0;
 
 
 int
 ssd1306_cmds (uint8_t * buf, size_t len, int delay)
 {
 
+  int ret=-1;
+
   while (i2c_lock ());
 
-  i2cp_start_transaction (SSD1306_I2C_ADDRESS, I2C_WRITE);
-  i2cp_send (SSD1306_COMMAND);
+ do {
 
+  if (i2cp_start_transaction (SSD1306_I2C_ADDRESS, I2C_WRITE))
+	break;
+
+  if (i2cp_send (SSD1306_COMMAND))
+	break;
+	
   while (len--)
-    i2cp_send (*(buf++));
+    if (i2cp_send (*(buf++)))
+	  break;
 
   i2cp_stop ();
 
   if (delay)
     delay_us (delay);
+
+  ret=0;
+  } while(0);
+
   i2c_unlock ();
 
-  return 0;
+
+  return ret;
 }
 
 int
@@ -50,15 +64,22 @@ start_dma (void)
   if (dma_in_progress)
     return;
 
-  refresh_wdt = 0;
-
-  dma_in_progress = 1;
 
   memcpy (dma_buf, update_buf, DMA_BUF_SZ);
 
-  ssd1306_cmds (cmds, sizeof (cmds), 0);
+  if (ssd1306_cmds (cmds, sizeof (cmds), 0))  {
+	oled_sad++;
+	return;
+   }
+	
+  if (i2cp_start_transaction (SSD1306_I2C_ADDRESS, I2C_WRITE)) {
+	oled_sad++;
+	return;
+   }
+
+  refresh_wdt = 0;
+  dma_in_progress = 1;
 
-  i2cp_start_transaction (SSD1306_I2C_ADDRESS, I2C_WRITE);
   i2cp_start_dma (dma_buf, DMA_BUF_SZ);
 }
 
@@ -85,10 +106,12 @@ oled_refresh_wdt (void)
   if (!refresh_enabled)
     return;
 
+
+
   refresh_wdt++;
 
 
-  if (refresh_wdt < 1000)
+  if ((refresh_wdt < 1000) && (!oled_sad))
     return;
 
   refresh_wdt = 0;
@@ -99,6 +122,11 @@ oled_refresh_wdt (void)
   i2cp_stop ();
   dma_in_progress = 0;
 
+  if (oled_sad)  {
+	oled_sad=0;
+	i2cp_reset_sm();
+  }
+ 
   start_dma ();
 
 }