--- a/master/fsm_change.c	Tue Feb 13 13:36:31 2007 +0000
+++ b/master/fsm_change.c	Tue Feb 13 13:42:37 2007 +0000
@@ -159,6 +159,7 @@
     ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);
     EC_WRITE_U16(datagram->data, fsm->requested_state);
     ec_master_queue_datagram(fsm->slave->master, datagram);
+    fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_change_state_check;
 }
 
@@ -174,9 +175,14 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_master_queue_datagram(fsm->slave->master, datagram);
+        return;
+    }
+
     if (datagram->state != EC_DATAGRAM_RECEIVED) {
         fsm->state = ec_fsm_change_state_error;
-        EC_ERR("Failed to send state datagram to slave %i!\n",
+        EC_ERR("Failed to receive state datagram from slave %i!\n",
                fsm->slave->ring_position);
         return;
     }
@@ -200,6 +206,7 @@
         ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);
         EC_WRITE_U16(datagram->data, fsm->requested_state);
         ec_master_queue_datagram(fsm->slave->master, datagram);
+        fsm->retries = EC_FSM_RETRIES;
         return;
     }
 
@@ -208,6 +215,7 @@
     // read AL status from slave
     ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
     ec_master_queue_datagram(fsm->slave->master, datagram);
+    fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_change_state_status;
 }
 
@@ -223,8 +231,19 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED
-        || datagram->working_counter != 1) {
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_master_queue_datagram(fsm->slave->master, datagram);
+        return;
+    }
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_change_state_error;
+        EC_ERR("Failed to receive state checking datagram from slave %i.\n",
+               slave->ring_position);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
         char req_state[EC_STATE_STRING_SIZE];
         ec_state_string(fsm->requested_state, req_state);
         fsm->state = ec_fsm_change_state_error;
@@ -271,6 +290,7 @@
         // fetch AL status error code
         ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2);
         ec_master_queue_datagram(fsm->slave->master, datagram);
+        fsm->retries = EC_FSM_RETRIES;
         fsm->state = ec_fsm_change_state_code;
         return;
     }
@@ -291,6 +311,7 @@
     // no timeout yet. check again
     ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
     ec_master_queue_datagram(fsm->slave->master, datagram);
+    fsm->retries = EC_FSM_RETRIES;
 }
 
 /*****************************************************************************/
@@ -346,8 +367,19 @@
     uint32_t code;
     const ec_code_msg_t *al_msg;
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED
-        || datagram->working_counter != 1) {
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_master_queue_datagram(fsm->slave->master, datagram);
+        return;
+    }
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_change_state_error;
+        EC_ERR("Failed to receive AL status code datagram from slave %i.\n",
+               fsm->slave->ring_position);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
         EC_WARN("Reception of AL status code datagram failed.\n");
     }
     else {
@@ -382,6 +414,7 @@
     ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);
     EC_WRITE_U16(datagram->data, slave->current_state);
     ec_master_queue_datagram(fsm->slave->master, datagram);
+    fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_change_state_ack;
 }
 
@@ -396,10 +429,22 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED
-        || datagram->working_counter != 1) {
-        fsm->state = ec_fsm_change_state_error;
-        EC_ERR("Reception of state ack datagram failed.\n");
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_master_queue_datagram(fsm->slave->master, datagram);
+        return;
+    }
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_change_state_error;
+        EC_ERR("Failed to receive state ack datagram for slave %i.\n",
+               slave->ring_position);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
+        fsm->state = ec_fsm_change_state_error;
+        EC_ERR("Reception of state ack datagram failed - slave %i did not"
+               " respond.\n", slave->ring_position);
         return;
     }
 
@@ -408,6 +453,7 @@
     // read new AL status
     ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
     ec_master_queue_datagram(fsm->slave->master, datagram);
+    fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_change_state_check_ack;
 }
 
@@ -423,10 +469,22 @@
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
 
-    if (datagram->state != EC_DATAGRAM_RECEIVED
-        || datagram->working_counter != 1) {
-        fsm->state = ec_fsm_change_state_error;
-        EC_ERR("Reception of state ack check datagram failed.\n");
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        ec_master_queue_datagram(fsm->slave->master, datagram);
+        return;
+    }
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_change_state_error;
+        EC_ERR("Failed to receive state ack check datagram from slave %i.\n",
+               slave->ring_position);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
+        fsm->state = ec_fsm_change_state_error;
+        EC_ERR("Reception of state ack check datagram failed - slave %i did"
+               " not respond.\n", slave->ring_position);
         return;
     }
 
@@ -464,6 +522,7 @@
     // reread new AL status
     ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2);
     ec_master_queue_datagram(fsm->slave->master, datagram);
+    fsm->retries = EC_FSM_RETRIES;
 }
 
 /*****************************************************************************/