From 6380a3b64648f53960d71193253f28f62db4a3ee Mon Sep 17 00:00:00 2001 From: Andreas Fritiofson Date: Thu, 19 Nov 2020 17:39:53 +0100 Subject: [PATCH 1/2] Add DAM-MPDO consumer support Basic support for MPDO reception in destination address mode only. Signed-off-by: Andreas Fritiofson Change-Id: I1b793daac63a5a08d428fb4d37fb28440cfbe3c3 --- src/co_pdo.c | 49 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/src/co_pdo.c b/src/co_pdo.c index debf9d9..c6e4ef1 100644 --- a/src/co_pdo.c +++ b/src/co_pdo.c @@ -95,11 +95,53 @@ void co_pdo_pack (co_net_t * net, co_pdo_t * pdo) } } +static int co_mpdo_unpack (co_net_t * net, co_pdo_t * pdo) +{ + bool is_dam = bitslice_get (&pdo->frame, 7, 1); + uint8_t node = bitslice_get (&pdo->frame, 0, 7); + uint16_t index = bitslice_get (&pdo->frame, 8, 16); + uint8_t subindex = bitslice_get (&pdo->frame, 24, 8); + uint32_t value = bitslice_get (&pdo->frame, 32, 32); + const co_obj_t * obj; + const co_entry_t * entry; + + if (is_dam) + { + if (node != 0 && node != net->node) + return 0; + + obj = co_obj_find (net, index); + if (obj == NULL) + return co_emcy_tx (net, 0x8230, 0, NULL); + + entry = co_entry_find (net, obj, subindex); + if (entry == NULL || !(entry->flags & OD_WRITE)) + return co_emcy_tx (net, 0x8230, 0, NULL); + + if (co_od_set_value (net, obj, entry, subindex, value)) + return co_emcy_tx (net, 0x8230, 0, NULL); + + } else { + /* TODO: Support SAM-MPDO Consumer using dispatcher list. */ + } + + return 0; +} + void co_pdo_unpack (co_net_t * net, co_pdo_t * pdo) { unsigned int ix; unsigned int offset = 0; + if (pdo->number_of_mappings >= 0xFE) + { + /* TODO: Each MPDO frame contains the info whether it's SAM or DAM, + * so what difference does it make if the configuration here is 0xFE + * or 0xFF?? */ + co_mpdo_unpack(net, pdo); + return; + } + for (ix = 0; ix < pdo->number_of_mappings; ix++) { const co_entry_t * entry = pdo->entries[ix]; @@ -121,6 +163,13 @@ static uint32_t co_pdo_mapping_validate (co_pdo_t * pdo, uint8_t number_of_mappi { int ix; + /* Check if MPDO */ + if (number_of_mappings >= 0xFE) + { + pdo->bitlength = 64; + return 0; + } + /* Mappings array bounds check */ if (number_of_mappings > MAX_PDO_ENTRIES) return CO_SDO_ABORT_PDO_LENGTH; From 8dc104ef87dcc24999226059bebc1d48a9bc2e48 Mon Sep 17 00:00:00 2001 From: Andreas Fritiofson Date: Thu, 3 Dec 2020 10:47:44 +0100 Subject: [PATCH 2/2] Don't send EMCY for broadcast MPDO failures Signed-off-by: Andreas Fritiofson Change-Id: I81a392f96c3db6fc0e3ffe70460f80de9ef3cb01 --- src/co_pdo.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/co_pdo.c b/src/co_pdo.c index c6e4ef1..4083c14 100644 --- a/src/co_pdo.c +++ b/src/co_pdo.c @@ -112,20 +112,26 @@ static int co_mpdo_unpack (co_net_t * net, co_pdo_t * pdo) obj = co_obj_find (net, index); if (obj == NULL) - return co_emcy_tx (net, 0x8230, 0, NULL); + goto error; entry = co_entry_find (net, obj, subindex); if (entry == NULL || !(entry->flags & OD_WRITE)) - return co_emcy_tx (net, 0x8230, 0, NULL); + goto error; if (co_od_set_value (net, obj, entry, subindex, value)) - return co_emcy_tx (net, 0x8230, 0, NULL); + goto error; } else { /* TODO: Support SAM-MPDO Consumer using dispatcher list. */ } return 0; + +error: + if (node != 0) + return co_emcy_tx (net, 0x8230, 0, NULL); + + return 0; } void co_pdo_unpack (co_net_t * net, co_pdo_t * pdo)