diff --git a/master/datagram.c b/master/datagram.c
index d7d59113..6e14e3bc 100644
--- a/master/datagram.c
+++ b/master/datagram.c
@@ -87,7 +87,10 @@ static const char *type_strings[] = {
  */
 void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */)
 {
+    INIT_LIST_HEAD(&datagram->list); // mark as unqueued
     INIT_LIST_HEAD(&datagram->queue); // mark as unqueued
+    INIT_LIST_HEAD(&datagram->fsm_queue); // mark as unqueued
+    INIT_LIST_HEAD(&datagram->sent); // mark as unqueued
     datagram->type = EC_DATAGRAM_NONE;
     memset(datagram->address, 0x00, EC_ADDR_LEN);
     datagram->data = NULL;
@@ -131,8 +134,8 @@ void ec_datagram_clear(ec_datagram_t *datagram /**< EtherCAT datagram. */)
  */
 void ec_datagram_unqueue(ec_datagram_t *datagram /**< EtherCAT datagram. */)
 {
-    if (!list_empty(&datagram->queue)) {
-        list_del_init(&datagram->queue);
+    if (!list_empty(&datagram->fsm_queue)) {
+        list_del_init(&datagram->fsm_queue);
     }
 }
 
diff --git a/master/datagram.h b/master/datagram.h
index b8f729b8..e4ec1cf5 100644
--- a/master/datagram.h
+++ b/master/datagram.h
@@ -86,7 +86,8 @@ typedef enum {
  */
 typedef struct {
     struct list_head list; /**< Needed by domain datagram lists. */
-    struct list_head queue; /**< Master datagram queue item. */
+    struct list_head queue; /**< Master datagram send-receive queue item. */
+    struct list_head fsm_queue; /**< Master datagram fsm queue item. */
     struct list_head sent; /**< Master list item for sent datagrams. */
     ec_datagram_type_t type; /**< Datagram type (APRD, BWR, etc.). */
     uint8_t address[EC_ADDR_LEN]; /**< Recipient address. */
diff --git a/master/master.c b/master/master.c
index 24edd207..9fb8ccee 100644
--- a/master/master.c
+++ b/master/master.c
@@ -416,11 +416,18 @@ void ec_master_clear_slaves(ec_master_t *master)
         wake_up(&master->reg_queue);
     }
 
+    // we must lock the fsm_queue here because the slave's fsm_datagram will be unqueued
+    if (master->fsm_queue_lock_cb)
+        master->fsm_queue_lock_cb(master->fsm_queue_locking_data);
+    ec_mutex_lock(&master->fsm_queue_mutex);
     for (slave = master->slaves;
             slave < master->slaves + master->slave_count;
             slave++) {
         ec_slave_clear(slave);
     }
+    ec_mutex_unlock(&master->fsm_queue_mutex);
+    if (master->fsm_queue_unlock_cb)
+        master->fsm_queue_unlock_cb(master->fsm_queue_locking_data);
 
     if (master->slaves) {
         kfree(master->slaves);
@@ -668,8 +675,11 @@ void ec_master_inject_fsm_datagrams(
 
     if (master->fsm_queue_lock_cb)
         master->fsm_queue_lock_cb(master->fsm_queue_locking_data);
-    if (ec_mutex_trylock(&master->fsm_queue_mutex) == 0)
-        return;
+    if (ec_mutex_trylock(&master->fsm_queue_mutex) == 0) {
+           if (master->fsm_queue_unlock_cb)
+               master->fsm_queue_unlock_cb(master->fsm_queue_locking_data);
+           return;
+    }
     if (list_empty(&master->fsm_datagram_queue)) {
         ec_mutex_unlock(&master->fsm_queue_mutex);
         if (master->fsm_queue_unlock_cb)
@@ -681,10 +691,10 @@ void ec_master_inject_fsm_datagrams(
     }
 
     list_for_each_entry_safe(datagram, n, &master->fsm_datagram_queue,
-            queue) {
+            fsm_queue) {
         queue_size += datagram->data_size;
         if (queue_size <= master->max_queue_size) {
-            list_del_init(&datagram->queue);
+            list_del_init(&datagram->fsm_queue);
 #if DEBUG_INJECT
             EC_MASTER_DBG(master, 2, "Injecting fsm datagram %p"
                     " size=%zu, queue_size=%zu\n", datagram,
@@ -698,7 +708,7 @@ void ec_master_inject_fsm_datagrams(
         }
         else {
             if (datagram->data_size > master->max_queue_size) {
-                list_del_init(&datagram->queue);
+                list_del_init(&datagram->fsm_queue);
                 datagram->state = EC_DATAGRAM_ERROR;
                 EC_MASTER_ERR(master, "Fsm datagram %p is too large,"
                         " size=%zu, max_queue_size=%zu\n",
@@ -717,7 +727,7 @@ void ec_master_inject_fsm_datagrams(
                 {
                     unsigned int time_us;
 
-                    list_del_init(&datagram->queue);
+                    list_del_init(&datagram->fsm_queue);
                     datagram->state = EC_DATAGRAM_ERROR;
 #ifdef EC_HAVE_CYCLES
                     time_us = (unsigned int)
@@ -794,7 +804,7 @@ void ec_master_queue_fsm_datagram(
 
     // check, if the datagram is already queued
     list_for_each_entry(queued_datagram, &master->fsm_datagram_queue,
-            queue) {
+            fsm_queue) {
         if (queued_datagram == datagram) {
             datagram->state = EC_DATAGRAM_QUEUED;
             ec_mutex_unlock(&master->fsm_queue_mutex);
@@ -809,7 +819,7 @@ void ec_master_queue_fsm_datagram(
             datagram, datagram->data_size);
 #endif
 
-    list_add_tail(&datagram->queue, &master->fsm_datagram_queue);
+    list_add_tail(&datagram->fsm_queue, &master->fsm_datagram_queue);
     datagram->state = EC_DATAGRAM_QUEUED;
 #ifdef EC_HAVE_CYCLES
     datagram->cycles_sent = get_cycles();