diff --git a/master/datagram.c b/master/datagram.c index b834216b..5f478c96 100644 --- a/master/datagram.c +++ b/master/datagram.c @@ -116,6 +116,8 @@ void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */) */ void ec_datagram_clear(ec_datagram_t *datagram /**< EtherCAT datagram. */) { + ec_datagram_unqueue(datagram); + if (datagram->data_origin == EC_ORIG_INTERNAL && datagram->data) { kfree(datagram->data); datagram->data = NULL; @@ -124,6 +126,17 @@ void ec_datagram_clear(ec_datagram_t *datagram /**< EtherCAT datagram. */) /*****************************************************************************/ +/** Unqueue datagram. + */ +void ec_datagram_unqueue(ec_datagram_t *datagram /**< EtherCAT datagram. */) +{ + if (!list_empty(&datagram->queue)) { + list_del_init(&datagram->queue); + } +} + +/*****************************************************************************/ + /** Allocates internal payload memory. * * If the allocated memory is already larger than requested, nothing ist done. diff --git a/master/datagram.h b/master/datagram.h index 06d988ab..42bba49f 100644 --- a/master/datagram.h +++ b/master/datagram.h @@ -115,6 +115,7 @@ typedef struct { void ec_datagram_init(ec_datagram_t *); void ec_datagram_clear(ec_datagram_t *); +void ec_datagram_unqueue(ec_datagram_t *); int ec_datagram_prealloc(ec_datagram_t *, size_t); void ec_datagram_zero(ec_datagram_t *); diff --git a/master/domain.c b/master/domain.c index 2cb66b06..0abbe7ec 100644 --- a/master/domain.c +++ b/master/domain.c @@ -80,8 +80,6 @@ void ec_domain_clear(ec_domain_t *domain /**< EtherCAT domain */) // dequeue and free datagrams list_for_each_entry_safe(datagram, next, &domain->datagrams, list) { - if (!list_empty(&datagram->queue)) // datagram queued? - list_del(&datagram->queue); ec_datagram_clear(datagram); kfree(datagram); }