diff -r c850674a3101 -r b3c3d54d2c7c src/hp34comm.cpp
--- a/src/hp34comm.cpp	Sun Nov 01 22:16:33 2020 +0100
+++ b/src/hp34comm.cpp	Sun Nov 01 22:21:19 2020 +0100
@@ -5,31 +5,22 @@
 
 /***** HP 34970A communication class ***/
 
-#ifdef DEBUG2
-
-DigitalOut inrx(D9);
-
-#endif
-
-DigitalOut lled(LED3);
-
 #define RXTIMEOUT 50ms
 #define STARTUPRETRY 0.5
 
 uint8_t startup_seq[] = {
   0x33,
-  0x02,
+  0x02,  // 0x02?
   0x00,
-  0x00,  // to be replaced by the actual keycode, if any
+  0x00,
 };
 
 HPSerial::statemethod HPSerial::state_table[NUM_STATES] = {
-  &HPSerial::do_state_initial,
-  &HPSerial::do_state_command,
-  &HPSerial::do_state_payload_size,
-  &HPSerial::do_state_payload,
-  &HPSerial::do_state_sending,
-  &HPSerial::do_state_eot,
+  &HPSerial::do_state_initial,      // STATE_IDLE
+  &HPSerial::do_state_command,      // STATE_COMMAND
+  &HPSerial::do_state_payload_size, // STATE_PAYLOAD_SIZE
+  &HPSerial::do_state_payload,      // STATE_PAYLOAD
+  &HPSerial::do_state_sending,      // STATE_SENDING
 };
 
 
@@ -39,17 +30,21 @@
   cur_gstate(GSTATE_IDLE)
 {
   serial.baud(187500);
+  serial.format(8, BufferedSerial::Even, 1);
   cur_state = STATE_IDLE;
-  serial.attach(callback(this, &HPSerial::rxIrq), SerialBase::RxIrq);
+  serial.attach(callback(this, &HPSerial::rx_irq), SerialBase::RxIrq);
 }
 
 void HPSerial::startup(uint8_t keycode) {
+  cur_gstate = GSTATE_STARTING;
+
   if (keycode != 0xFF) {
-      startup_sed[2] = 0xFF;
-      startup_sed[3] = keycode;
+	  printf("Set startup keycode to %X\n", keycode);
+      startup_seq[2] = 0xFF;
+      startup_seq[3] = keycode;
   }
-
-  cur_gstate = GSTATE_STARTING;
+  else
+      startup_seq[2] = 0x00;
   set_timer(10ms); // launch the startup in 10ms
 }
 
@@ -65,18 +60,28 @@
       tr_data.size = 3; // sizeof(startup_seq);
   for(uint8_t i=0; i<tr_data.size; i++)
     tr_data.payload[i] = startup_seq[i];
-
   cur_state = do_state_sending();
 }
 
 void HPSerial::sendkey(uint8_t keycode)
 {
-  tr_data.size = 2;
-  tr_data.cmd = 0xFF;
-  tr_data.pos = 0;
-  tr_data.payload[0] = 0x66;
-  tr_data.payload[1] = keycode;
-  cur_state = do_state_sending();
+  if (!sendbuf.full())
+    sendbuf.push(keycode);
+}
+
+void HPSerial::send_key_when_idle() {
+  if (!sendbuf.empty() && cur_gstate == GSTATE_IDLE)
+  {
+    uint8_t keycode;
+	cur_gstate = GSTATE_TX;
+	sendbuf.pop(keycode);
+	tr_data.size = 2;
+	tr_data.cmd = 0xFF;
+	tr_data.pos = 0;
+	tr_data.payload[0] = 0x66;
+	tr_data.payload[1] = keycode;
+	cur_state = do_state_sending();
+  }
 }
 
 bool HPSerial::cmd_available(void)
@@ -112,8 +117,8 @@
 }
 
 void HPSerial::send_ack(uint8_t c) {
-	serial.write(&c, 1);
-  set_timer(RXTIMEOUT); // if nothing else happen in the next ms, reset
+  serial.write(&c, 1);
+  set_timer(RXTIMEOUT); // if nothing else happen in the next RXTIMEOUT ms, reset
 }
 
 HPSerial::state_t HPSerial::do_state_initial(uint8_t c)
@@ -122,7 +127,7 @@
   // knwon handcheck values are 0x66 and 0x33
   set_timer(RXTIMEOUT); // reset the watchdog
   switch (c) {
-  case 0x33:
+  case 0x33:  // XXX? when are we expecting a 0x33 here?
     send_ack(0xCC);
     return HPSerial::STATE_PAYLOAD_SIZE;
     break;
@@ -173,9 +178,9 @@
   send_ack(0x00);
   if (tr_data.pos >= tr_data.size) {
     pushCmd(tr_data.cmd, tr_data.size, tr_data.payload);
-    return HPSerial::STATE_IDLE;
+    return STATE_IDLE;
   }
-  return HPSerial::STATE_PAYLOAD;
+  return STATE_PAYLOAD;
 }
 
 HPSerial::state_t HPSerial::do_state_sending(uint8_t c)
@@ -184,48 +189,41 @@
   if (c == 0xFF)
   { // resend current char
     tr_data.pos--;
-	tr_data.payload[tr_data.pos] += 1;
   }
   // TODO: check ACK values (c is the received ack)
   if (tr_data.pos >= tr_data.size)
   {
-    return do_state_eot();
+	  c = 0x55;
+	  serial.write(&c, 1); // EoT
+	  cur_gstate = GSTATE_IDLE;
+	  set_timer(); // We are IDLE, detach the timeouter
+	  return STATE_IDLE;
   }
-  serial.write(&tr_data.payload[tr_data.pos++], 1);
-  set_timer(RXTIMEOUT);
-  return HPSerial::STATE_SENDING;
-}
-
-HPSerial::state_t HPSerial::do_state_eot(uint8_t c)
-{
-  serial.write(&c, 1); // EoT
-  cur_gstate = GSTATE_IDLE;
-  set_timer(); // We are IDLE, detach the timeouter
-  return STATE_IDLE;
+  else
+  {
+	  serial.write(&tr_data.payload[tr_data.pos++], 1);
+	  set_timer(RXTIMEOUT);
+	  return STATE_SENDING;
+  }
 }
 
 HPSerial::state_t HPSerial::run_state(HPSerial::state_t cur_state,
-				      uint8_t c)
+									  uint8_t c)
 {
   return (this->*(HPSerial::state_table[cur_state]))(c);
 };
 
-void HPSerial::rxIrq(void) {
+void HPSerial::rx_irq(void) {
   uint8_t val;
   if(serial.readable())
   { // no reason why we would end here without
 	// this condition, but hey
-#ifdef DEBUG2
-    inrx=1;
-#endif
-    //lled = 1;
-    //val = serial.getc();
+	if (cur_gstate == GSTATE_IDLE)
+	  // occurs when the CPU starts a new transmission
+	  // at this point, cur_state should be STATE_IDLE also (TODO add a check?)
+	  cur_gstate = GSTATE_RX;
 	serial.read(&val, 1);
     cur_state = run_state(cur_state, val);
-    //lled = 0;
-#ifdef DEBUG2
-    inrx=0;
-#endif
   }
 }
 
@@ -234,7 +232,7 @@
   set_timer(); // detach the timeouter
   if (cur_gstate == GSTATE_STARTING)
     _startup();
-  else // reset
+  else // CPU took too long to reply, reset
     {
     cur_gstate = GSTATE_IDLE;
     cur_state = STATE_IDLE;