--- a/master/fsm_foe.c	Fri Jan 22 10:11:58 2016 +0100
+++ b/master/fsm_foe.c	Fri Jan 22 13:09:43 2016 +0100
@@ -42,9 +42,10 @@
 
 /*****************************************************************************/
 
-/** Maximum time in ms to wait for responses when reading out the dictionary.
- */
-#define EC_FSM_FOE_TIMEOUT 3000
+/** Maximum time in jiffies to wait for responses when reading out the
+ * dictionary.
+ */
+#define EC_FSM_FOE_TIMEOUT_JIFFIES (3 * HZ)
 
 /** Size of the FoE header.
  */
@@ -256,6 +257,10 @@
 
     EC_WRITE_U16(data, EC_FOE_OPCODE_DATA);    // OpCode = DataBlock req.
     EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password
+#ifdef DEBUG_FOE
+    EC_SLAVE_DBG(fsm->slave, 0, "sending opcode %u packet %u\n",
+            EC_FOE_OPCODE_DATA, fsm->tx_packet_no);
+#endif
 
     memcpy(data + EC_FOE_HEADER_SIZE,
             fsm->tx_buffer + fsm->tx_buffer_offset, current_size);
@@ -293,6 +298,10 @@
 
     EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request
     EC_WRITE_U32( data + 2, fsm->tx_packet_no );
+#ifdef DEBUG_FOE
+    EC_SLAVE_DBG(fsm->slave, 0, "sending opcode %u packet %u\n",
+            EC_FOE_OPCODE_WRQ, fsm->tx_packet_no);
+#endif
 
     memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_filename, current_size);
 
@@ -365,9 +374,8 @@
 
     if (!ec_slave_mbox_check(fsm->datagram)) {
         // slave did not put anything in the mailbox yet
-        unsigned long diff_ms = (fsm->datagram->jiffies_received -
-                fsm->jiffies_start) * 1000 / HZ;
-        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
+        if (time_after(fsm->datagram->jiffies_received,
+                    fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) {
             ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
             EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
             return;
@@ -431,6 +439,9 @@
     }
 
     opCode = EC_READ_U8(data);
+#ifdef DEBUG_FOE
+    EC_SLAVE_DBG(fsm->slave, 0, "received opcode %u\n", opCode);
+#endif
 
     if (opCode == EC_FOE_OPCODE_BUSY) {
         // slave not ready
@@ -443,6 +454,20 @@
         return;
     }
 
+    if (opCode == EC_FOE_OPCODE_ERR) {
+        fsm->request->error_code = EC_READ_U32(data + 2);
+        EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n",
+                fsm->request->error_code);
+        if (rec_size > 6 && data[6]) {
+            uint8_t text[256];
+            strncpy(text, data + 6, min(rec_size - 6, sizeof(text)));
+            text[sizeof(text)-1] = 0;
+            EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text);
+        }
+        ec_foe_set_tx_error(fsm, FOE_OPCODE_ERROR);
+        return;
+    }
+
     if (opCode == EC_FOE_OPCODE_ACK) {
         fsm->tx_packet_no++;
         fsm->tx_buffer_offset += fsm->tx_current_size;
@@ -566,6 +591,9 @@
     EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
     EC_WRITE_U32(data + 2, 0x00000000); // no passwd
     memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size);
+#ifdef DEBUG_FOE
+    EC_SLAVE_DBG(fsm->slave, 0, "sending opcode %u\n", EC_FOE_OPCODE_RRQ);
+#endif
 
     if (fsm->slave->master->debug_level) {
         EC_SLAVE_DBG(fsm->slave, 1, "FoE Read Request:\n");
@@ -596,6 +624,10 @@
 
     EC_WRITE_U16(data, EC_FOE_OPCODE_ACK);
     EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no);
+#ifdef DEBUG_FOE
+    EC_SLAVE_DBG(fsm->slave, 0, "sending opcode %u packet %u\n",
+            EC_FOE_OPCODE_ACK, fsm->rx_expected_packet_no);
+#endif
 
     return 0;
 }
@@ -704,9 +736,8 @@
     }
 
     if (!ec_slave_mbox_check(fsm->datagram)) {
-        unsigned long diff_ms = (fsm->datagram->jiffies_received -
-                fsm->jiffies_start) * 1000 / HZ;
-        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
+        if (time_after(fsm->datagram->jiffies_received,
+                    fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) {
             ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
             EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
             return;
@@ -734,7 +765,8 @@
         )
 {
     size_t rec_size;
-    uint8_t *data, opCode, packet_no, mbox_prot;
+    uint32_t packet_no;
+    uint8_t *data, opCode, mbox_prot;
 
     ec_slave_t *slave = fsm->slave;
 
@@ -770,11 +802,21 @@
     }
 
     opCode = EC_READ_U8(data);
+#ifdef DEBUG_FOE
+    EC_SLAVE_DBG(fsm->slave, 0, "received opcode %u\n", opCode);
+#endif
 
     if (opCode == EC_FOE_OPCODE_BUSY) {
+        fsm->rx_expected_packet_no--;
         if (ec_foe_prepare_send_ack(fsm, datagram)) {
             ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
+        } else {
+            fsm->state = ec_fsm_foe_state_sent_ack;
         }
+#ifdef DEBUG_FOE
+        EC_SLAVE_DBG(fsm->slave, 0, "%s() busy. Next pkt %u\n", __func__,
+                fsm->rx_expected_packet_no);
+#endif
         return;
     }
 
@@ -782,9 +824,10 @@
         fsm->request->error_code = EC_READ_U32(data + 2);
         EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n",
                 fsm->request->error_code);
-        if (rec_size > 6) {
+        if (rec_size > 6 && data[6]) {
             uint8_t text[256];
             strncpy(text, data + 6, min(rec_size - 6, sizeof(text)));
+            text[sizeof(text)-1] = 0;
             EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text);
         }
         ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR);
@@ -799,9 +842,10 @@
         return;
     }
 
-    packet_no = EC_READ_U16(data + 2);
+    packet_no = EC_READ_U32(data + 2);
     if (packet_no != fsm->rx_expected_packet_no) {
-        EC_SLAVE_ERR(slave, "Received unexpected packet number.\n");
+        EC_SLAVE_ERR(slave, "Received packet number %u, expected %u.\n",
+                packet_no, fsm->rx_expected_packet_no);
         ec_foe_set_rx_error(fsm, FOE_PACKETNO_ERROR);
         return;
     }