diff -r 219766126afb -r f1c85c2500f2 src/main.cpp
--- a/src/main.cpp	Tue Sep 13 21:32:48 2016 +0200
+++ b/src/main.cpp	Tue Sep 20 23:50:45 2016 +0200
@@ -1,8 +1,8 @@
 #include "stdio.h"
 
 #include "mbed.h"
+#include "rtos.h"
 #include "string"
-#include "CircularBuffer.h"
 
 #include "Terminal6x8.h"
 #include "Mono19x27.h"
@@ -10,18 +10,15 @@
 #include "Arial12x12.h"
 
 #include "SSD1322.h"
+#include "hp34comm.h"
 
 Serial pc(USBTX, USBRX);
 
-//BufferedSerial hp(NC, PA_1, 64, 0); // serial4
-#define HP_SERIAL_TX PC_7 // serial6 RX
-#define HP_SERIAL_RX PA_1 // serial4 RX
+#define DEBUG
 
-SSD1322 dsp(SPI_8, 10000000, PB_15, PB_14, PB_13, PB_12, D11, D12, "SSD1322", 256, 64); 
+SSD1322 dsp(SPI_8, 20000000, PB_15, PB_14, PB_13, PB_12, D11, D12, "SSD1322", 256, 64); 
 
-#define BUF_SIZE 32
-#define MAX_BUFF 15
-#define MAX_ERRS 10
+
 uint8_t curchar;
 uint8_t cmd;
 uint8_t nchars;
@@ -34,7 +31,7 @@
   uint8_t bgcolor;
   uint8_t x0;
   uint8_t y0;
-  uint8_t fmt; // 0=>ascii, 1=>hex, 2=>bits, 3=>flags
+  uint8_t fmt; // 0x01=>ascii, 0x02=>hex, 0x04=>bits, 0x08=>flags, 0x80=>ignore
   uint8_t maxsize;
   uint8_t width;
   const unsigned char* font;
@@ -43,13 +40,12 @@
 
 static DSP table[] =
 {
-  { 0x00, 0xF,   0x0,  0,  0, 0, MAX_BUFF, 245, Mono19x27}, // main display
-  { 0x0C, 0xF,   0x0,204, 38, 0, 3,        45, Mono15x22}, // channels display
-  { 0x01, 0xF,   0x0,  0,  0, 4, MAX_BUFF, 0, Terminal6x8}, 
-  { 0x02, 0xF,   0x0,  0,  0, 4, MAX_BUFF, 0, Terminal6x8},
-  { 0x0A, 0xF,   0x0,  0, 57, 2, 4,        0, Terminal6x8},
-  { 0x0A, 0xF,   0x0,  0,  0, 3, 4,        0, Terminal6x8}, // flags
-  { 0xFF, 0xF,   0x0,  0,  0, 4, MAX_BUFF, 0, Terminal6x8},
+  { 0x00, 0xF,   0x0,  0,  0, 0x01, MAX_BUFF, 245, Mono19x27}, // main display
+  { 0x0C, 0xF,   0x0,204, 38, 0x01, 3,        45, Mono15x22}, // channels display
+  { 0x01, 0xF,   0x0,  0,  0, 0x80, MAX_BUFF, 0, Terminal6x8}, 
+  { 0x02, 0xF,   0x0,  0,  0, 0x80, MAX_BUFF, 0, Terminal6x8},
+  { 0x0A, 0xF,   0x0,  0, 57, 0x0C, 4,        0, Terminal6x8}, // flags + bits
+  { 0xFF, 0xF,   0x0,  0,  0, 0x80, MAX_BUFF, 0, Terminal6x8},
 };
 
 // 9x10
@@ -93,16 +89,16 @@
 static const FLAG flags[] =
   {
     //{ 0x00, 160, 30, "Alarm"},  // for the 'alarm' box
-    { 0x00, 246, 0,  NULL, icon_alarm},      // F1.1
-    { 0x01, 246, 11, NULL, icon_curve},      // F1.2
-    { 0x03, 204, 30, "Channel"},    // F1.4
-    { 0x14, 68,  44, "4W"},         // F2.5
-    { 0x33, 40, 44,  "VIEW"},       // F4.4
-    { 0x34, 0, 30,   "MON"},        // F4.5
-    { 0x36, 0, 44,   "CONFIG"},     // F4.7
+    { 0x00, 246, 0,  NULL, icon_alarm}, // F1.1
+    { 0x01, 246, 11, NULL, icon_curve}, // F1.2
+    { 0x03, 204, 30, "Channel"}, // F1.4
+    { 0x14, 68, 44,  "4W"},      // F2.5
+    { 0x32, 40, 44,  "*"},       // F4.3
+    { 0x33, 40, 44,  "VIEW"},    // F4.4
+    { 0x34, 0,  30,  "MON"},     // F4.5
+    { 0x35, 0,  44,  "SCAN"},    // F4.6
+    { 0x36, 0,  44,  "CONFIG"},  // F4.7
     
-    { 0xFF, 0, 0,  "SCAN"},  // F4.6 or F4.3
-    { 0xFF, 0, 0,  "*"},
     { 0xFF, 0, 0,  "ADRS"},
     { 0xFF, 0, 0,  "RMT"},
     { 0xFF, 0, 0,  "ERROR"},
@@ -125,218 +121,49 @@
     
   };
 
+typedef struct _FRAME
+{
+  uint8_t flag;
+  uint8_t x0;
+  uint8_t y0;
+  uint8_t x1;
+  uint8_t y1;
+} FRAME;
+
+static const FRAME frames[] =
+  {
+    { 0x02, 203, 35, 248, 55}, // F1.3
+  };
+
 
 #ifdef DEBUG
 #define DBGPIN PC_0
 
 DigitalOut dbgpin(DBGPIN);
-inline void DebugPulse(uint8_t count=1)
+inline void pulse(uint8_t count=1, bool stayup=false)
 {
+  dbgpin = 0; 
+  wait_us(2);
   while (count--)
   {
     dbgpin = 1;
-    dbgpin = 0;
-  }
+    wait_us(2);
+    dbgpin = 0; 
+    wait_us(2);
+ }
+  if (stayup)
+    dbgpin = 1;
+    
 }
 #else
-inline void DebugPulse(uint8_t count=1)
+inline void pulse(uint8_t count=1, bool stayup=false)
 {}
 #endif
 
 
-
-/***** HP 34970A communication class ***/
-
-class HPSerial {
-
-public:
-  enum TrState {
-        Idle = 0,
-        Tx,
-        Rx,
-    };
-  typedef struct _CMD
-  {
-    TrState direction;
-    uint8_t cmd;
-    uint8_t size;
-    char value[MAX_BUFF+1];
-    unsigned long id;
-  } CMD;
-
-
-  
-  HPSerial(): ncmd(0), serial_tx(NC, HP_SERIAL_TX), serial_rx(NC, HP_SERIAL_RX) {
-    pc.printf("HPSerial init\n");
-    for(uint8_t i=0; i<MAX_ERRS; i++)
-      errs[i] = 0;
-    reset();
-
-    serial_tx.baud(187500);
-    serial_tx.attach(this, &HPSerial::txIrq, Serial::RxIrq); //sic!
-
-    serial_rx.baud(187500);
-    serial_rx.attach(this, &HPSerial::rxIrq, Serial::RxIrq);
-  }
-
-  bool cmd_available(void) {
-    return !cmdbuf.empty();
-  }
-
-  bool pop(CMD& cmd) {
-    return cmdbuf.pop(cmd);
-  }
-
-  bool cmd_buf_full(void) {
-    return cmdbuf.full();
-  }
-
-  unsigned int nerrors(uint8_t errorno) {
-    return errs[errorno];
-  }
-    
-		     
-  
-private:
-  void reset(uint8_t errorno=0xFF) {
-    head = 0;
-    tx_state = Idle;
-    tx_cmd = 0xFF;
-    tx_ack = false;
-    tx_len = 0xFF;
-    memset(buf, 0, BUF_SIZE);
-    if (errorno != 0xFF)
-      errs[errorno]++;
-  }
-
-  void handleAck(uint8_t val) {
-    if (tx_ack == true)
-      reset(0);
-
-    else 
-      if (tx_cmd == 0xFF) // still at the beginning of a packet, expect 0x99 as ack
-	if (val == 0x99)
-	  tx_ack = true;
-	else 
-	  reset(1);
-
-      else // expect 0x00 as ack
-	if (val == 0x00)
-	  tx_ack = true;
-	else
-	  reset(2);
-  }
-
-  void pushCmd(TrState direction, uint8_t cmd, uint8_t size, char *payload) {
-    CMD val;
-    uint8_t i;
-    val.id = ncmd++;
-    val.direction = direction;
-    val.cmd = cmd;
-    val.size = size;
-    for(i=0; i<size; i++)
-      val.value[i] = payload[i];
-    val.value[i] = 0x00;
-    cmdbuf.push(val);
-  }
-  
-  void handleChar(uint8_t val) {
-    if (tx_ack == false)
-      reset(3);
-    else // remaining of the state machine
-      if (tx_cmd == 0xFF) {
-	// begin of transmission, expect a cmd
-	tx_cmd = val;
-	tx_ack = false;
-	tx_len = 0xFF;
-      }
-      else if (tx_len == 0xFF) {
-	// got a cmd, expect a payload size
-	tx_len = val;
-	tx_ack = false;
-      }
-      else if (tx_len > 0) {
-	// a payload char
-	buf[head++] = val;
-	tx_len--;
-	tx_ack = false;
-      }
-      else {
-	uint8_t cur_state = tx_state;
-	pushCmd(tx_state, tx_cmd, head, buf); 
-	reset();
-	
-	if (val != 0x55) { // not an end of transmission
-	  // should be another cmd I think
-	  tx_cmd = val;
-	  tx_state = cur_state;
-	}
-      }
-  }
-
-  
-  void rxIrq(void) {
-    uint8_t val;
-    if(serial_rx.readable()) { // no reason why we would end here without this condition, but hey
-      val = serial_rx.getc();
-      
-      if (tx_state == Idle)
-	if (val == 0x66) {
-	  // no transmission in progress, expect a start of transmission
-	  tx_state = Rx;
-	  tx_ack = false;
-	}
-	else
-	  reset(4);
-    
-      else if (tx_state == Tx)  // manage the acks
-	handleAck(val);
-      
-      else 
-	handleChar(val);
-    }
-  }
-  
-  void txIrq(void) {
-    uint8_t val;
-    if(serial_tx.readable()) { // no reason why we would end here without this condition, but hey
-      val = serial_tx.getc();
-      
-      if (tx_state == Idle)
-	if (val == 0x66) {
-	  // no transmission in progress, expect a start of transmission
-	  tx_state = Tx;
-	  tx_ack = false;
-	}
-	else
-	  reset(5);
-    
-      else if (tx_state == Rx)  // manage the acks
-	handleAck(val);
-      
-      else 
-	handleChar(val);
-    }
-  }
-  
-private:
-  RawSerial serial_tx;
-  RawSerial serial_rx;
-  uint8_t buf[BUF_SIZE];
-  uint8_t head;
-  uint8_t tx_state;
-  uint8_t tx_cmd;
-  uint8_t tx_len;
-  bool tx_ack;
-  CircularBuffer<CMD, 32> cmdbuf;
-  unsigned long ncmd;
-  unsigned int errs[MAX_ERRS];
-};
-
-
 HPSerial hp;
-Ticker dsp_refresher;
 volatile bool must_refresh;
+Thread tdsp, tloop;
 
 void copy_to_lcd(void);
 void test_dsp();
@@ -362,7 +189,9 @@
     curchar = 0;
     nchars = 0;
 
-    
+    for (uint8_t i=0; i<sizeof(table)/sizeof(table[0]); ++i)
+      memset(table[i].buffer, 0, MAX_BUFF+1);
+      
     dsp.printf("HP34970A");
     dsp.set_font(Terminal6x8);
     for(uint8_t i=0; i<4;  i++) {
@@ -375,16 +204,23 @@
     wait(2);
     dsp.cls();
     
-    dsp_refresher.attach(&copy_to_lcd, 0.1);
     test_dsp();
+    dsp.copy_to_lcd();
     wait(2);
     dsp.cls();
     
 }
 
 void copy_to_lcd(void) {
-  if (must_refresh)
-    dsp.copy_to_lcd();
+  while(1) {
+    pulse(0, true);
+    if (must_refresh) {
+      must_refresh = false;
+      dsp.copy_to_lcd();
+    }
+    pulse(0, false);
+    Thread::wait(100);
+  }
 }
 
 void show(uint8_t cmd, char *txt, uint8_t nchar=0) {
@@ -392,6 +228,8 @@
   uint16_t bgcolor, fgcolor;
   char *oldv;
 
+  pulse(1, true);
+  
   must_refresh = false;
   
   len = MAX_BUFF;
@@ -406,13 +244,34 @@
       dsp.set_font((unsigned char*) table[i].font);
       oldv = table[i].buffer;
       
-      switch (table[i].fmt) {
-      case 0: //ascii
+
+      if (table[i].fmt & 0x01 ) {
+	// ascii
 	if (table[i].width > 0)
 	  dsp.fillrect(table[i].x0, table[i].y0, table[i].x0 + table[i].width, table[i].y0 + table[i].font[2], bgcolor);
-	dsp.printf(txt);
-	break;
-      case 1: // hex
+	for (uint8_t k=0; ;k++) {
+	  if (txt[k] == 0x00)
+	    {
+	      dsp.printf(txt);
+	      break;
+	    }
+	  if (txt[k] == 0x09) { // \t is a special char for 'unselected' display value
+	    txt[k] = 0x00;
+	    dsp.printf(txt);
+	    
+	    if (fgcolor == table[i].color)
+	      fgcolor /= 2;
+	    else
+	      fgcolor = table[i].color;
+	    dsp.foreground(fgcolor);
+	    txt = &(txt[k+1]);
+	    k = 0;
+	  }
+	}
+      }
+	
+      if (table[i].fmt & 0x02 ) {
+      // hex
 	for (uint8_t j=0;; j++) {
 	  if (txt[j] == 0x00)
 	    break;
@@ -420,8 +279,10 @@
 	}
 	for (uint8_t j=3*strlen(txt); j<table[i].maxsize; j++)
 	  dsp.printf(" ");
-	break;
-      case 2: // binary
+      }
+      
+      if (table[i].fmt & 0x04 ) {
+      // binary
 	dsp.foreground(fgcolor);
 	dsp.printf(" [");
 	for (uint8_t j=0; j<max(nchar, table[i].maxsize) ; j++) {
@@ -439,8 +300,10 @@
 	}
 	dsp.foreground(fgcolor);
 	dsp.printf("]");
-	break;
-      case 3: // flags
+      }
+
+      if (table[i].fmt & 0x08 ) {
+      // flags
 	for (uint8_t j=0; j<max(nchar, table[i].maxsize) ; j++) {
 	  for (uint8_t k=0; k<8; k++) {
 	    if ((txt[j] & (1 << k) ) != (oldv[j] & (1 << k))) {
@@ -449,6 +312,20 @@
 		dsp.foreground(fgcolor);
 	      else
 		dsp.foreground(bgcolor);
+
+	      for (uint8_t l=0;
+		   l<(sizeof(frames)/sizeof(frames[0])); ++l) {
+		if (frames[l].flag & ((j<<4) + k))
+		  dsp.rect(frames[l].x0, frames[l].y0,
+			   frames[l].x1, frames[l].y1,
+			   fgcolor); 
+		else
+		  dsp.rect(frames[l].x0, frames[l].y0,
+			   frames[l].x1, frames[l].y1,
+			   bgcolor); 
+		
+	      }
+	      
 	      for (uint8_t l=0;
 		   l<(sizeof(flags)/sizeof(flags[0])); ++l) {
 		if (flags[l].flag == ((j<<4) + k)) {
@@ -459,44 +336,47 @@
 		    Bitmap_s pic = {9, 10, 2, flags[l].icon};
 		    dsp.Bitmap_BW(pic, flags[l].x, flags[l].y);
 		  }
-		  break;
+		  //break;
 		}
-	      }
+	      }	      
 	    }
 	  }
-	  oldv[j] = txt[j];
 	}
-	break;
-      case 4: //ignore
-	break;
       }
+      
+      for(uint8_t j=0; j<table[i].maxsize; j++)
+	oldv[j] = txt[j];
     }
   }
+
   must_refresh = true;
   //dsp.copy_to_lcd();
+  pulse(1, false);
 }
 
 void test_dsp()
 {
   show(0x00, "8g8g8g8g8g8g8", 13); // main dsp
   show(0x0C, "888", 3); // channel dsp
-  show(0x0A, "\xFF\xFF\xFF\xFF", 4); // flags
+  show(0x0A, "\xFF\xFF\xFF\xFF", 4); // all flags
 }
 
 
 void loop() { // run over and over
+  while(1){
   if (hp.cmd_available())
     {
       HPSerial::CMD cmd;
       if (hp.pop(cmd))
 	{
-	  pc.printf("CMD[%s:%d %d/%d/%d/%d/%d/%d] %X\n", (cmd.direction==HPSerial::Rx)?"Rx":"Tx", cmd.id,
+	  pc.printf("CMD[%s:%d %d/%d/%d/%d/%d/%d/%d] %X\n", (cmd.direction==HPSerial::Rx)?"Rx":"Tx", cmd.id,
 		    hp.nerrors(0), 
 		    hp.nerrors(1), 
 		    hp.nerrors(2), 
 		    hp.nerrors(3), 
 		    hp.nerrors(4), 
 		    hp.nerrors(5), 
+		    hp.nerrors(6), 
 		    cmd.cmd);
 
 	  if (cmd.direction == HPSerial::Rx) {
@@ -507,11 +387,15 @@
 	  }
 	}
     }
+  Thread::wait(0.01);
+  }
 }
 
 int main()
 {
   setup();
-  while(1)
-    loop();
+  tdsp.start(copy_to_lcd);
+  tloop.start(loop);
+  while(1);
+  //loop();
 }