--- a/master/fsm_slave.c	Thu Dec 17 17:21:03 2009 +0100
+++ b/master/fsm_slave.c	Fri Dec 18 11:09:07 2009 +0100
@@ -62,7 +62,9 @@
     fsm->slave = slave;
     fsm->datagram = datagram;
     fsm->datagram->data_size = 0;
-    fsm->state = ec_fsm_slave_state_idle;
+	if (slave->master->debug_level)
+		EC_DBG("init fsm for slave %u...\n",slave->ring_position);
+	fsm->state = ec_fsm_slave_state_idle;
 
     // init sub-state-machines
     ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
@@ -177,20 +179,26 @@
     // search the first external request to be processed
     list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) {
 
+        list_del_init(&request->list); // dequeue
 		if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
-			EC_WARN("Postponing SDO request, slave %u has ERROR.\n",
+			EC_WARN("Aborting SDO request, slave %u has ERROR.\n",
 					slave->ring_position);
+			request->req.state = EC_INT_REQUEST_FAILURE;
+			wake_up(&slave->sdo_queue);
+			fsm->sdo_request = NULL;
 			fsm->state = ec_fsm_slave_state_idle;
 			return 0;
 		}
 		if (slave->current_state == EC_SLAVE_STATE_INIT) {
-            EC_WARN("Postponing SDO request, slave %u is in INIT.\n",
-                    slave->ring_position);
+			EC_WARN("Aborting SDO request, slave %u is in INIT.\n",
+					slave->ring_position);
+			request->req.state = EC_INT_REQUEST_FAILURE;
+			wake_up(&slave->sdo_queue);
+			fsm->sdo_request = NULL;
 			fsm->state = ec_fsm_slave_state_idle;
 			return 0;
-        }
-        list_del_init(&request->list); // dequeue
-        request->req.state = EC_INT_REQUEST_BUSY;
+		}
+		request->req.state = EC_INT_REQUEST_BUSY;
 
         // Found pending SDO request. Execute it!
         if (master->debug_level)
@@ -226,8 +234,11 @@
     // search the first request to be processed
     list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
 		if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
-			EC_WARN("Postponing FOE request, slave %u has ERROR.\n",
+			EC_WARN("Aborting FOE request, slave %u has ERROR.\n",
 					slave->ring_position);
+			request->req.state = EC_INT_REQUEST_FAILURE;
+			wake_up(&slave->sdo_queue);
+			fsm->sdo_request = NULL;
 			fsm->state = ec_fsm_slave_state_idle;
 			return 0;
 		}
@@ -271,9 +282,9 @@
         EC_DBG("Failed to process SDO request for slave %u.\n",
                 fsm->slave->ring_position);
         request->state = EC_INT_REQUEST_FAILURE;
-        wake_up(&slave->sdo_queue);
-        fsm->sdo_request = NULL;
-        fsm->state = ec_fsm_slave_state_idle;
+		wake_up(&slave->foe_queue);
+		fsm->foe_request = NULL;
+		fsm->state = ec_fsm_slave_state_idle;
         return;
     }
 

--- a/master/master.c	Thu Dec 17 17:21:03 2009 +0100
+++ b/master/master.c	Fri Dec 18 11:09:07 2009 +0100
@@ -849,12 +849,11 @@
 
 /** Sends the datagrams in the queue.
  *
- * \return 0 in case of success, else < 0
  */
 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
 {
     ec_datagram_t *datagram, *next;
-    size_t datagram_size;
+	size_t datagram_size;
     uint8_t *frame_data, *cur_data;
     void *follows_word;
 #ifdef EC_HAVE_CYCLES
@@ -1187,6 +1186,7 @@
     ec_master_t *master = (ec_master_t *) priv_data;
     ec_slave_t *slave = NULL;
     int fsm_exec;
+	size_t sent_bytes;
 	ec_master_set_send_interval(master,1000000 / HZ); // send interval in IDLE phase
 	if (master->debug_level)
 		EC_DBG("Idle thread running with send interval = %d us, max data size=%d\n",master->send_interval,master->max_queue_size);
@@ -1218,12 +1218,13 @@
         }
         ec_master_inject_external_datagrams(master);
         ecrt_master_send(master);
+		sent_bytes = master->main_device.tx_skb[master->main_device.tx_ring_index]->len;
         up(&master->io_sem);
 
 		if (ec_fsm_master_idle(&master->fsm))
 			ec_master_nanosleep(master->send_interval*1000);
 		else
-			schedule();
+			ec_master_nanosleep(sent_bytes*EC_BYTE_TRANSMITION_TIME);
     }
     
     if (master->debug_level)
@@ -2002,7 +2003,7 @@
     }
 
     // send frames
-    ec_master_send_datagrams(master);
+	ec_master_send_datagrams(master);
 }
 
 /*****************************************************************************/