diff --git a/sys/can/dll.c b/sys/can/dll.c index 13012147e3..1d0f5098a8 100644 --- a/sys/can/dll.c +++ b/sys/can/dll.c @@ -56,9 +56,7 @@ static int _get_ifnum(kernel_pid_t pid) int _send_pkt(can_pkt_t *pkt) { - if (!pkt) { - return -ENOMEM; - } + assert(pkt); msg_t msg; int handle = pkt->handle; @@ -71,6 +69,9 @@ int _send_pkt(can_pkt_t *pkt) msg.content.ptr = (void*) pkt; if (msg_send(&msg, candev_list[pkt->entry.ifnum]->pid) <= 0) { + mutex_lock(&tx_lock); + LL_DELETE(tx_list[pkt->entry.ifnum], &pkt->entry); + mutex_unlock(&tx_lock); return -EOVERFLOW; } @@ -85,11 +86,18 @@ int raw_can_send(int ifnum, const struct can_frame *frame, kernel_pid_t pid) assert(ifnum < candev_nb); pkt = can_pkt_alloc_tx(ifnum, frame, pid); + if (!pkt) { + return -ENOMEM; + } DEBUG("raw_can_send: ifnum=%d, id=0x%" PRIx32 " from pid=%" PRIkernel_pid ", handle=%d\n", ifnum, frame->can_id, pid, pkt->handle); - return _send_pkt(pkt); + int ret = _send_pkt(pkt); + if (ret < 0) { + can_pkt_free(pkt); + } + return ret; } #ifdef MODULE_CAN_MBOX @@ -101,10 +109,17 @@ int raw_can_send_mbox(int ifnum, const struct can_frame *frame, mbox_t *mbox) assert(ifnum < candev_nb); pkt = can_pkt_alloc_mbox_tx(ifnum, frame, mbox); + if (!pkt) { + return -ENOMEM; + } DEBUG("raw_can_send: ifnum=%d, id=0x%" PRIx32 ", handle=%d\n", ifnum, frame->can_id, pkt->handle); - return _send_pkt(pkt); + int ret = _send_pkt(pkt); + if (ret < 0) { + can_pkt_free(pkt); + } + return ret; } #endif @@ -113,6 +128,7 @@ int raw_can_abort(int ifnum, int handle) msg_t msg, reply; can_pkt_t *pkt = NULL; can_reg_entry_t *entry = NULL; + can_reg_entry_t *prev = tx_list[ifnum]; assert(ifnum < candev_nb); @@ -122,11 +138,16 @@ int raw_can_abort(int ifnum, int handle) LL_FOREACH(tx_list[ifnum], entry) { pkt = container_of(entry, can_pkt_t, entry); if (pkt->handle == handle) { + if (prev == tx_list[ifnum]) { + tx_list[ifnum] = entry->next; + } + else { + prev->next = entry->next; + } break; } + prev = entry; } - - LL_DELETE(tx_list[ifnum], entry); mutex_unlock(&tx_lock); if (pkt == NULL) { @@ -336,32 +357,56 @@ int can_dll_dispatch_rx_frame(struct can_frame *frame, kernel_pid_t pid) return can_router_dispatch_rx_indic(pkt); } +static int _remove_entry_from_list(can_reg_entry_t **list, can_reg_entry_t *entry) +{ + assert(list); + int res = -1; + can_reg_entry_t *_tmp; + if (*list == entry) { + res = 0; + *list = (*list)->next; + } + else if (*list != NULL) { + _tmp = *list; + while (_tmp->next && (_tmp->next != entry)) { + _tmp = _tmp->next; + } + if (_tmp->next) { + _tmp->next = entry->next; + res = 0; + } + } + return res; +} + int can_dll_dispatch_tx_conf(can_pkt_t *pkt) { - DEBUG("can_dll_dispatch_tx_conf: pkt=0x%p\n", (void*)pkt); - - can_router_dispatch_tx_conf(pkt); + DEBUG("can_dll_dispatch_tx_conf: pkt=%p\n", (void*)pkt); mutex_lock(&tx_lock); - LL_DELETE(tx_list[pkt->entry.ifnum], &pkt->entry); + int res = _remove_entry_from_list(&tx_list[pkt->entry.ifnum], &pkt->entry); mutex_unlock(&tx_lock); - can_pkt_free(pkt); + if (res == 0) { + can_router_dispatch_tx_conf(pkt); + can_pkt_free(pkt); + } return 0; } int can_dll_dispatch_tx_error(can_pkt_t *pkt) { - DEBUG("can_dll_dispatch_tx_error: pkt=0x%p\n", (void*)pkt); - - can_router_dispatch_tx_error(pkt); + DEBUG("can_dll_dispatch_tx_error: pkt=%p\n", (void*)pkt); mutex_lock(&tx_lock); - LL_DELETE(tx_list[pkt->entry.ifnum], &pkt->entry); + int res = _remove_entry_from_list(&tx_list[pkt->entry.ifnum], &pkt->entry); mutex_unlock(&tx_lock); - can_pkt_free(pkt); + if (res == 0) { + can_router_dispatch_tx_error(pkt); + can_pkt_free(pkt); + } return 0; diff --git a/sys/can/pkt.c b/sys/can/pkt.c index 31107cd1b6..57d2a2b815 100644 --- a/sys/can/pkt.c +++ b/sys/can/pkt.c @@ -69,21 +69,21 @@ static can_pkt_t *_pkt_alloc(int ifnum, const struct can_frame *frame) return pkt; } -static void _init_pkt(can_pkt_t *pkt, int tx) +static void _init_rx_pkt(can_pkt_t *pkt) { - if (tx) { - mutex_lock(&_mutex); - pkt->handle = handle++; - if (handle == INT_MAX) { - handle = 1; - } - pkt->entry.next = NULL; - mutex_unlock(&_mutex); - } - else { - pkt->handle = 0; - atomic_store(&pkt->ref_count, 0); + pkt->handle = 0; + atomic_store(&pkt->ref_count, 0); +} + +static void _init_tx_pkt(can_pkt_t *pkt) +{ + mutex_lock(&_mutex); + pkt->handle = handle++; + if (handle == INT_MAX) { + handle = 1; } + pkt->entry.next = NULL; + mutex_unlock(&_mutex); } can_pkt_t *can_pkt_alloc_tx(int ifnum, const struct can_frame *frame, kernel_pid_t tx_pid) @@ -94,7 +94,7 @@ can_pkt_t *can_pkt_alloc_tx(int ifnum, const struct can_frame *frame, kernel_pid return NULL; } - _init_pkt(pkt, 1); + _init_tx_pkt(pkt); pkt->entry.target.pid = tx_pid; #ifdef MODULE_CAN_MBOX pkt->entry.type = CAN_TYPE_DEFAULT; @@ -111,7 +111,7 @@ can_pkt_t *can_pkt_alloc_rx(int ifnum, const struct can_frame *frame) return NULL; } - _init_pkt(pkt, 0); + _init_rx_pkt(pkt); return pkt; } @@ -125,7 +125,7 @@ can_pkt_t *can_pkt_alloc_mbox_tx(int ifnum, const struct can_frame *frame, mbox_ return NULL; } - _init_pkt(pkt, 1); + _init_tx_pkt(pkt); pkt->entry.target.mbox = tx_mbox; pkt->entry.type = CAN_TYPE_MBOX; @@ -135,9 +135,7 @@ can_pkt_t *can_pkt_alloc_mbox_tx(int ifnum, const struct can_frame *frame, mbox_ void can_pkt_free(can_pkt_t *pkt) { - if (!pkt) { - return; - } + assert(pkt); DEBUG("can_pkt_free: free pkt=%p\n", (void*)pkt); @@ -173,6 +171,8 @@ void can_pkt_free_rx_data(can_rx_data_t *data) return; } + DEBUG("can_pkt_free_rx_data: rx=%p\n", (void *)data); + mutex_lock(&_mutex); memarray_free(&_pkt_array, data); mutex_unlock(&_mutex);