--- a/master/fsm_sii.c	Fri Aug 10 15:08:44 2007 +0000
+++ b/master/fsm_sii.c	Fri Aug 10 15:27:08 2007 +0000
@@ -105,7 +105,7 @@
 void ec_fsm_sii_write(ec_fsm_sii_t *fsm, /**< finite state machine */
                       ec_slave_t *slave, /**< slave to read from */
                       uint16_t offset, /**< offset to read from */
-                      uint16_t *value, /**< pointer to 2 bytes of data */
+                      const uint16_t *value, /**< pointer to 2 bytes of data */
                       ec_fsm_sii_addressing_t mode /**< addressing scheme */
                       )
 {
@@ -168,7 +168,6 @@
     EC_WRITE_U8 (datagram->data,     0x00); // read-only access
     EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation
     EC_WRITE_U16(datagram->data + 2, fsm->offset);
-    ec_master_queue_datagram(fsm->slave->master, datagram);
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_sii_read_check;
 }
@@ -184,15 +183,14 @@
 {
     ec_datagram_t *datagram = fsm->datagram;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
-        ec_master_queue_datagram(fsm->slave->master, datagram);
-        return;
-    }
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_sii_error;
-        EC_ERR("Failed to receive SII read datagram from slave %i.\n",
-               fsm->slave->ring_position);
+        EC_ERR("Failed to receive SII read datagram from slave %i"
+                " (datagram state %i).\n",
+               fsm->slave->ring_position, datagram->state);
         return;
     }
 
@@ -215,7 +213,7 @@
             ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10);
             break;
     }
-    ec_master_queue_datagram(fsm->slave->master, datagram);
+
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_sii_read_fetch;
 }
@@ -231,15 +229,14 @@
 {
     ec_datagram_t *datagram = fsm->datagram;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
-        ec_master_queue_datagram(fsm->slave->master, datagram);
-        return;
-    }
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_sii_error;
-        EC_ERR("Failed to receive SII check/fetch datagram from slave %i.\n",
-               fsm->slave->ring_position);
+        EC_ERR("Failed to receive SII check/fetch datagram from slave %i"
+                " (datagram state %i).\n",
+               fsm->slave->ring_position, datagram->state);
         return;
     }
 
@@ -279,7 +276,6 @@
                 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10);
                 break;
         }
-        ec_master_queue_datagram(fsm->slave->master, datagram);
         fsm->retries = EC_FSM_RETRIES;
         return;
     }
@@ -314,7 +310,7 @@
     EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation
     EC_WRITE_U32(datagram->data + 2, fsm->offset);
     memcpy(datagram->data + 6, fsm->value, 2);
-    ec_master_queue_datagram(fsm->slave->master, datagram);
+
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_sii_write_check;
 }
@@ -329,15 +325,14 @@
 {
     ec_datagram_t *datagram = fsm->datagram;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
-        ec_master_queue_datagram(fsm->slave->master, datagram);
-        return;
-    }
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_sii_error;
-        EC_ERR("Failed to receive SII write datagram for slave %i.\n",
-               fsm->slave->ring_position);
+        EC_ERR("Failed to receive SII write datagram for slave %i"
+                " (datagram state %i).\n",
+               fsm->slave->ring_position, datagram->state);
         return;
     }
 
@@ -353,7 +348,6 @@
 
     // issue check/fetch datagram
     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 2);
-    ec_master_queue_datagram(fsm->slave->master, datagram);
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_sii_write_check2;
 }
@@ -368,15 +362,14 @@
 {
     ec_datagram_t *datagram = fsm->datagram;
 
-    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
-        ec_master_queue_datagram(fsm->slave->master, datagram);
-        return;
-    }
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
 
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_sii_error;
-        EC_ERR("Failed to receive SII write check datagram from slave %i.\n",
-               fsm->slave->ring_position);
+        EC_ERR("Failed to receive SII write check datagram from slave %i"
+                " (datagram state %i).\n",
+               fsm->slave->ring_position, datagram->state);
         return;
     }
 
@@ -400,7 +393,6 @@
         }
 
         // issue check/fetch datagram again
-        ec_master_queue_datagram(fsm->slave->master, datagram);
         fsm->retries = EC_FSM_RETRIES;
         return;
     }