From cb7d5b159a42a8124dc618e26c0d86cf2370971e Mon Sep 17 00:00:00 2001 From: Tadas Aleksonis Date: Thu, 14 May 2026 18:30:32 -0700 Subject: [PATCH 1/2] DAM_MPDO --- 301/CO_MPDO.c | 331 ++++++++++++++++++++++++++++++++++++++++++++++++ 301/CO_MPDO.h | 246 +++++++++++++++++++++++++++++++++++ 301/CO_config.h | 26 ++++ CANopen.c | 97 +++++++++++++- CANopen.h | 53 ++++++++ 5 files changed, 751 insertions(+), 2 deletions(-) create mode 100644 301/CO_MPDO.c create mode 100644 301/CO_MPDO.h diff --git a/301/CO_MPDO.c b/301/CO_MPDO.c new file mode 100644 index 000000000..14f63e346 --- /dev/null +++ b/301/CO_MPDO.c @@ -0,0 +1,331 @@ +/* + * CANopen Multiplexed Process Data Object (MPDO) protocol — CiA 301 §7.2.5. + * + * @file CO_MPDO.c + * @ingroup CO_MPDO + * + * This file is part of CANopenNode, an opensource CANopen Stack. + * Project home page is . + * For more information on CANopen see . + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + */ + +#include + +#include "301/CO_MPDO.h" + +#if (CO_CONFIG_MPDO) != 0 + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) +/* + * Per-frame RX callback. Runs on whatever context CO_CANprocessCANread runs + * from — task context on both CP (NuttX signal timer thread) and VC (main + * loop). Defers OD work to CO_process_MPDO_RX so that handling stays uniform + * with regular RPDOs. + */ +static void CO_MPDO_receive(void *object, void *msg) { + CO_MPDO_rx_t *rx = (CO_MPDO_rx_t *) object; + uint8_t DLC = CO_CANrxMsg_readDLC(msg); + uint8_t *data = CO_CANrxMsg_readData(msg); + + /* MPDO frames are always 8 bytes (CiA 301 §7.2.5). Drop runts. */ + if (!rx->valid || DLC != 8U) { + return; + } + + memcpy(rx->CANrxData, data, sizeof(rx->CANrxData)); + CO_FLAG_SET(rx->CANrxNew); +} +#endif + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM) +/* + * Apply one DAM frame to the OD. Returns true if the write succeeded so the + * caller can decide whether to emit EMCY for spec-mandated failures. + */ +static bool_t CO_MPDO_applyDAM(CO_MPDO_t *MPDO, const uint8_t *frame) { + uint8_t byte0 = frame[0]; + + /* DAM iff bit 7 is clear. */ + if ((byte0 & CO_MPDO_BYTE0_SAM_MASK) != 0U) { + return false; + } + + uint8_t targetNodeId = byte0 & CO_MPDO_BYTE0_NODEID_MASK; + + /* DAM accepts broadcast (nodeId == 0) and our own node ID. */ + if (targetNodeId != CO_MPDO_DAM_BROADCAST + && targetNodeId != MPDO->ownNodeId + ) { + return true; /* not for us — silently ignore, not an error */ + } + + uint16_t idx = (uint16_t)frame[1] | ((uint16_t)frame[2] << 8); + uint8_t sub = frame[3]; + + /* CiA 301 §7.2.5.2: target must be PDO-writable (ODA_RPDO). */ + OD_entry_t *entry = OD_find(MPDO->OD, idx); + if (entry == NULL) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, ((uint32_t)idx << 16) | ((uint32_t)sub << 8)); + return false; + } + + OD_IO_t io; + ODR_t odRet = OD_getSub(entry, sub, &io, false); + if (odRet != ODR_OK) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, ((uint32_t)idx << 16) | ((uint32_t)sub << 8)); + return false; + } + + if ((io.stream.attribute & ODA_RPDO) == 0U) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, ((uint32_t)idx << 16) | ((uint32_t)sub << 8)); + return false; + } + + /* DAM payload is the entire 4-byte tail. Spec: the destination OD entry + * must have exactly the length the producer is writing. Without a length + * field on the wire, we treat the OD entry length as authoritative and + * write exactly that many bytes, capped at 4. Anything longer than 4 is + * a violation of the DAM contract. */ + OD_size_t writeLen = io.stream.dataLength; + if (writeLen == 0U || writeLen > 4U) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, ((uint32_t)idx << 16) | ((uint32_t)sub << 8)); + return false; + } + + /* Reset stream offset before each write — io->write may be an extension. */ + io.stream.dataOffset = 0; + OD_size_t countWritten = 0; + + CO_LOCK_OD(MPDO->CANdev); + ODR_t writeRet = io.write(&io.stream, &frame[4], writeLen, &countWritten); + CO_UNLOCK_OD(MPDO->CANdev); + + if (writeRet != ODR_OK || countWritten != writeLen) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, ((uint32_t)idx << 16) | ((uint32_t)sub << 8)); + return false; + } + + return true; +} +#endif /* (CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM */ + + +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_init(CO_MPDO_t *MPDO, + OD_t *OD, + CO_EM_t *em, + CO_CANmodule_t *CANdev, + uint8_t ownNodeId, + uint16_t rxIdxBase, + uint16_t txIdxBase) +{ + if (MPDO == NULL || OD == NULL || em == NULL || CANdev == NULL + || ownNodeId < 1U || ownNodeId > 127U + ) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + + memset(MPDO, 0, sizeof(*MPDO)); + MPDO->OD = OD; + MPDO->em = em; + MPDO->CANdev = CANdev; + MPDO->ownNodeId = ownNodeId; +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) + MPDO->rxIdxBase = rxIdxBase; +#else + (void) rxIdxBase; +#endif +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM)) + MPDO->txIdxBase = txIdxBase; +#else + (void) txIdxBase; +#endif + + return CO_ERROR_NO; +} + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM) +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_configRX_DAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId) +{ + if (MPDO == NULL || slotIdx >= CO_CONFIG_MPDO_RX_COUNT || canId == 0U + || (canId & ~0x7FFU) != 0U + ) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + + CO_MPDO_rx_t *rx = &MPDO->rx[slotIdx]; + rx->canId = canId; + CO_FLAG_CLEAR(rx->CANrxNew); + + CO_ReturnError_t ret = CO_CANrxBufferInit(MPDO->CANdev, + MPDO->rxIdxBase + slotIdx, + canId, + 0x7FFU, + 0, + (void *) rx, + CO_MPDO_receive); + if (ret == CO_ERROR_NO) { + rx->valid = true; + } + return ret; +} +#endif + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM) +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_configTX_DAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId, + uint32_t inhibitTime_us) +{ + if (MPDO == NULL || slotIdx >= CO_CONFIG_MPDO_TX_COUNT || canId == 0U + || (canId & ~0x7FFU) != 0U + ) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + + CO_MPDO_tx_t *tx = &MPDO->tx[slotIdx]; + tx->CANtxBuff = CO_CANtxBufferInit(MPDO->CANdev, + MPDO->txIdxBase + slotIdx, + canId, + 0, + 8, + false); + if (tx->CANtxBuff == NULL) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + tx->inhibitTime_us = inhibitTime_us; + tx->inhibitTimer = 0; + tx->sendRequest = false; + tx->valid = true; + return CO_ERROR_NO; +} + + +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_send_DAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint8_t targetNodeId, + uint16_t idx, + uint8_t sub, + const void *data, + uint8_t len) +{ + if (MPDO == NULL || slotIdx >= CO_CONFIG_MPDO_TX_COUNT + || targetNodeId > 127U || data == NULL || len == 0U || len > 4U + ) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + + CO_MPDO_tx_t *tx = &MPDO->tx[slotIdx]; + if (!tx->valid) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + if (tx->sendRequest) { + return CO_ERROR_TX_OVERFLOW; + } + + /* Build the 8-byte payload. Unused tail bytes are zeroed. */ + uint8_t *frame = tx->pendingFrame; + frame[0] = targetNodeId & CO_MPDO_BYTE0_NODEID_MASK; /* DAM: bit 7 = 0 */ + frame[1] = (uint8_t)(idx & 0xFFU); + frame[2] = (uint8_t)(idx >> 8); + frame[3] = sub; + memset(&frame[4], 0, 4); + memcpy(&frame[4], data, len); + + tx->sendRequest = true; + return CO_ERROR_NO; +} +#endif /* (CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM */ + + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) +/******************************************************************************/ +void CO_MPDO_processRX(CO_MPDO_t *MPDO) { + if (MPDO == NULL) { + return; + } + + for (uint8_t i = 0; i < CO_CONFIG_MPDO_RX_COUNT; i++) { + CO_MPDO_rx_t *rx = &MPDO->rx[i]; + if (!rx->valid || !CO_FLAG_READ(rx->CANrxNew)) { + continue; + } + + uint8_t frame[8]; + memcpy(frame, rx->CANrxData, sizeof(frame)); + CO_FLAG_CLEAR(rx->CANrxNew); + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM) + (void) CO_MPDO_applyDAM(MPDO, frame); +#else + (void) frame; /* SAM-only build: stub for now */ +#endif + } +} +#endif + + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM)) +/******************************************************************************/ +void CO_MPDO_processTX(CO_MPDO_t *MPDO, + uint32_t timeDifference_us, + uint32_t *timerNext_us) +{ + (void) timerNext_us; + if (MPDO == NULL) { + return; + } + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM) + for (uint8_t i = 0; i < CO_CONFIG_MPDO_TX_COUNT; i++) { + CO_MPDO_tx_t *tx = &MPDO->tx[i]; + if (!tx->valid) { + continue; + } + + if (tx->inhibitTimer > timeDifference_us) { + tx->inhibitTimer -= timeDifference_us; + } else { + tx->inhibitTimer = 0; + } + + if (tx->sendRequest && tx->inhibitTimer == 0U) { + memcpy(&tx->CANtxBuff->data[0], tx->pendingFrame, 8); + CO_ReturnError_t ret = CO_CANsend(MPDO->CANdev, tx->CANtxBuff); + if (ret == CO_ERROR_NO) { + tx->sendRequest = false; + tx->inhibitTimer = tx->inhibitTime_us; + } + /* On TX overflow, leave sendRequest set so the next tick retries. */ + } + + if (timerNext_us != NULL && tx->sendRequest + && *timerNext_us > tx->inhibitTimer + ) { + *timerNext_us = tx->inhibitTimer; + } + } +#endif +} +#endif + +#endif /* (CO_CONFIG_MPDO) != 0 */ diff --git a/301/CO_MPDO.h b/301/CO_MPDO.h new file mode 100644 index 000000000..2c96479af --- /dev/null +++ b/301/CO_MPDO.h @@ -0,0 +1,246 @@ +/** + * CANopen Multiplexed Process Data Object (MPDO) protocol — CiA 301 §7.2.5. + * + * @file CO_MPDO.h + * @ingroup CO_MPDO + * + * This file is part of CANopenNode, an opensource CANopen Stack. + * Project home page is . + * For more information on CANopen see . + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + */ + +#ifndef CO_MPDO_H +#define CO_MPDO_H + +#include "301/CO_ODinterface.h" +#include "301/CO_Emergency.h" +#include "301/CO_driver.h" + +/* Default to compiled out unless the target overrides it. */ +#ifndef CO_CONFIG_MPDO +#define CO_CONFIG_MPDO (0) +#endif + +#if (CO_CONFIG_MPDO) != 0 || defined CO_DOXYGEN + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @defgroup CO_MPDO MPDO + * CANopen Multiplexed Process Data Object — CiA 301 §7.2.5. + * + * @ingroup CO_CANopen_301 + * @{ + * + * MPDO solves "I want to push many distinct OD entries through one PDO slot" + * without consuming one of the 8 mapping slots per target entry. The producer + * builds a frame that names the destination (DAM) or names the source (SAM) + * inline, so a single COB-ID can move payload into arbitrary destinations. + * + * Frame layout, 8 data bytes: + * - byte 0 addressing byte; bit 7 = 0 ⇒ DAM, bit 7 = 1 ⇒ SAM. + * For DAM, bits 6..0 are the target node ID (0 ⇒ broadcast). + * For SAM, bits 6..0 are the producer node ID. + * - bytes 1..2 multiplexor index (little-endian). + * - byte 3 multiplexor sub-index. + * - bytes 4..7 1..4 bytes of payload, left-aligned (high bytes unused). + * + * Transmission type for MPDO is fixed at event-driven (≥ 0xFE). + */ + +/** Number of MPDO consumer RX slots. */ +#ifndef CO_CONFIG_MPDO_RX_COUNT +#define CO_CONFIG_MPDO_RX_COUNT 1 +#endif + +/** Number of MPDO producer TX slots. */ +#ifndef CO_CONFIG_MPDO_TX_COUNT +#define CO_CONFIG_MPDO_TX_COUNT 1 +#endif + +/** MPDO frame addressing mode. */ +typedef enum { + CO_MPDO_MODE_DAM = 0, + CO_MPDO_MODE_SAM = 1 +} CO_MPDO_mode_t; + +/** Bit 7 of byte 0 distinguishes SAM from DAM on the wire. */ +#define CO_MPDO_BYTE0_SAM_MASK 0x80U +/** Bits 6..0 of byte 0 are the (target/source) node ID. */ +#define CO_MPDO_BYTE0_NODEID_MASK 0x7FU +/** Broadcast destination encoding for DAM. */ +#define CO_MPDO_DAM_BROADCAST 0U + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) || defined CO_DOXYGEN +/** + * Per-slot MPDO consumer state. One slot is bound to one CAN-ID — typically + * the producer's COB-ID for its MPDO-TPDO. The receive callback only buffers + * the frame; CO_process_MPDO_RX does the OD work in task context. + */ +typedef struct { + bool_t valid; /**< Slot is configured. */ + uint16_t canId; /**< 11-bit COB-ID, 0 ⇒ slot disabled. */ + volatile void *CANrxNew; /**< Same flag-style as CO_RPDO_t. */ + uint8_t CANrxData[8]; /**< Buffered payload. */ +} CO_MPDO_rx_t; +#endif + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM)) || defined CO_DOXYGEN +/** + * Per-slot MPDO producer state. + */ +typedef struct { + bool_t valid; /**< Slot is configured. */ + CO_CANtx_t *CANtxBuff; /**< Tx buffer, returned by CO_CANtxBufferInit. */ + uint32_t inhibitTime_us; /**< Inhibit interval. */ + uint32_t inhibitTimer; /**< Counts down to 0 between sends. */ + volatile bool_t sendRequest; /**< Caller scheduled a frame. */ + uint8_t pendingFrame[8]; /**< Built by CO_MPDO_send_DAM(). */ +} CO_MPDO_tx_t; +#endif + +/** + * Top-level MPDO state, one instance per @ref CO_t. + */ +typedef struct { + CO_EM_t *em; + CO_CANmodule_t *CANdev; + OD_t *OD; + uint8_t ownNodeId; +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) || defined CO_DOXYGEN + CO_MPDO_rx_t rx[CO_CONFIG_MPDO_RX_COUNT]; + uint16_t rxIdxBase; /**< First rxArray index owned by MPDO. */ +#endif +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM)) || defined CO_DOXYGEN + CO_MPDO_tx_t tx[CO_CONFIG_MPDO_TX_COUNT]; + uint16_t txIdxBase; /**< First txArray index owned by MPDO. */ +#endif +} CO_MPDO_t; + + +/** + * Initialize the MPDO container. + * + * Resets all slot state; call once after CO_CANopenInitPDO(). Per-slot + * configuration is applied separately via CO_MPDO_configRX_DAM() and + * CO_MPDO_configTX_DAM(). + * + * @param MPDO Container object. + * @param OD Object Dictionary (used by RX-DAM to resolve writes). + * @param em Emergency object. + * @param CANdev CAN module. + * @param ownNodeId Node ID of this node (1..127); used for DAM target match. + * @param rxIdxBase First rxArray index reserved for MPDO RX slots. + * @param txIdxBase First txArray index reserved for MPDO TX slots. + * + * @return CO_ERROR_NO on success. + */ +CO_ReturnError_t CO_MPDO_init(CO_MPDO_t *MPDO, + OD_t *OD, + CO_EM_t *em, + CO_CANmodule_t *CANdev, + uint8_t ownNodeId, + uint16_t rxIdxBase, + uint16_t txIdxBase); + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM) || defined CO_DOXYGEN +/** + * Configure one MPDO consumer slot for DAM reception. + * + * @param MPDO Container. + * @param slotIdx 0..CO_CONFIG_MPDO_RX_COUNT-1. + * @param canId 11-bit COB-ID to subscribe to. + * + * @return CO_ERROR_NO on success. + */ +CO_ReturnError_t CO_MPDO_configRX_DAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId); +#endif + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM) || defined CO_DOXYGEN +/** + * Configure one MPDO producer slot. + * + * @param MPDO Container. + * @param slotIdx 0..CO_CONFIG_MPDO_TX_COUNT-1. + * @param canId 11-bit COB-ID to publish on. + * @param inhibitTime_us Minimum spacing between sends on this slot. + * + * @return CO_ERROR_NO on success. + */ +CO_ReturnError_t CO_MPDO_configTX_DAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId, + uint32_t inhibitTime_us); + + +/** + * Fire-and-forget DAM send. Schedules one MPDO frame; the actual CO_CANsend + * happens inside @ref CO_process_MPDO_TX once the inhibit window has elapsed. + * + * @param MPDO Container. + * @param slotIdx Producer slot to send on. + * @param targetNodeId 1..127, or 0 for broadcast. + * @param idx Destination OD index on the consumer. + * @param sub Destination OD sub-index on the consumer. + * @param data Payload (1..4 bytes). + * @param len 1..4. + * + * @return CO_ERROR_NO on success, CO_ERROR_ILLEGAL_ARGUMENT on bad args, + * CO_ERROR_TX_OVERFLOW if a previous send is still pending. + */ +CO_ReturnError_t CO_MPDO_send_DAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint8_t targetNodeId, + uint16_t idx, + uint8_t sub, + const void *data, + uint8_t len); +#endif + + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) || defined CO_DOXYGEN +/** + * Drain pending MPDO frames into the OD. Call once per RT tick after + * CO_process_RPDO(). + * + * @param MPDO Container. + */ +void CO_MPDO_processRX(CO_MPDO_t *MPDO); +#endif + + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM)) || defined CO_DOXYGEN +/** + * Send any scheduled MPDO frames whose inhibit window has elapsed. Call once + * per RT tick after CO_process_TPDO(). + * + * @param MPDO Container. + * @param timeDifference_us Time since last call. + * @param [out] timerNext_us info to OS — see CO_process(); may be NULL. + */ +void CO_MPDO_processTX(CO_MPDO_t *MPDO, + uint32_t timeDifference_us, + uint32_t *timerNext_us); +#endif + +/** @} */ /* CO_MPDO */ + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* (CO_CONFIG_MPDO) != 0 */ + +#endif /* CO_MPDO_H */ diff --git a/301/CO_config.h b/301/CO_config.h index a80932586..2928a815e 100644 --- a/301/CO_config.h +++ b/301/CO_config.h @@ -474,6 +474,32 @@ extern "C" { #define CO_CONFIG_TPDO_TIMERS_ENABLE 0x08 #define CO_CONFIG_PDO_SYNC_ENABLE 0x10 #define CO_CONFIG_PDO_OD_IO_ACCESS 0x20 + +/** + * Configuration of @ref CO_MPDO (Multiplexed PDO, CiA 301 §7.2.5) + * + * MPDO provides one PDO COB-ID that carries (idx, sub, data[1..4]) tuples so + * the producer can write to many destination OD entries through a single + * mapping slot. There are two flavors: DAM (Destination Address Mode, target + * picked by the producer per-frame) and SAM (Source Address Mode, consumer + * looks up local destination via the OD 0x1FD0 dispatcher). + * + * Possible flags, can be ORed: + * - CO_CONFIG_MPDO_RX_DAM - Enable DAM consumer. + * - CO_CONFIG_MPDO_TX_DAM - Enable DAM producer. + * - CO_CONFIG_MPDO_RX_SAM - Enable SAM consumer (0x1FD0 dispatcher). + * - CO_CONFIG_MPDO_TX_SAM - Enable SAM producer (0x1FA0 scanner). + * + * When CO_CONFIG_MPDO == 0 every byte of CO_MPDO.c is compiled out and no + * fields are added to any struct. + */ +#ifdef CO_DOXYGEN +#define CO_CONFIG_MPDO (0) +#endif +#define CO_CONFIG_MPDO_RX_DAM 0x01 +#define CO_CONFIG_MPDO_TX_DAM 0x02 +#define CO_CONFIG_MPDO_RX_SAM 0x04 +#define CO_CONFIG_MPDO_TX_SAM 0x08 /** @} */ /* CO_STACK_CONFIG_SYNC_PDO */ diff --git a/CANopen.c b/CANopen.c index 3dcad69c9..bc0dcc6a6 100644 --- a/CANopen.c +++ b/CANopen.c @@ -276,7 +276,8 @@ #define CO_RX_IDX_GFC (CO_RX_IDX_TIME + CO_RX_CNT_TIME) #define CO_RX_IDX_SRDO (CO_RX_IDX_GFC + CO_RX_CNT_GFC) #define CO_RX_IDX_RPDO (CO_RX_IDX_SRDO + CO_RX_CNT_SRDO * 2) -#define CO_RX_IDX_SDO_SRV (CO_RX_IDX_RPDO + CO_RX_CNT_RPDO) +#define CO_RX_IDX_MPDO (CO_RX_IDX_RPDO + CO_RX_CNT_RPDO) +#define CO_RX_IDX_SDO_SRV (CO_RX_IDX_MPDO + CO_RX_CNT_MPDO) #define CO_RX_IDX_SDO_CLI (CO_RX_IDX_SDO_SRV + CO_RX_CNT_SDO_SRV) #define CO_RX_IDX_HB_CONS (CO_RX_IDX_SDO_CLI + CO_RX_CNT_SDO_CLI) #define CO_RX_IDX_LSS_SLV (CO_RX_IDX_HB_CONS + CO_RX_CNT_HB_CONS) @@ -290,7 +291,8 @@ #define CO_TX_IDX_GFC (CO_TX_IDX_TIME + CO_TX_CNT_TIME) #define CO_TX_IDX_SRDO (CO_TX_IDX_GFC + CO_TX_CNT_GFC) #define CO_TX_IDX_TPDO (CO_TX_IDX_SRDO + CO_TX_CNT_SRDO * 2) -#define CO_TX_IDX_SDO_SRV (CO_TX_IDX_TPDO + CO_TX_CNT_TPDO) +#define CO_TX_IDX_MPDO (CO_TX_IDX_TPDO + CO_TX_CNT_TPDO) +#define CO_TX_IDX_SDO_SRV (CO_TX_IDX_MPDO + CO_TX_CNT_MPDO) #define CO_TX_IDX_SDO_CLI (CO_TX_IDX_SDO_SRV + CO_TX_CNT_SDO_SRV) #define CO_TX_IDX_HB_PROD (CO_TX_IDX_SDO_CLI + CO_TX_CNT_SDO_CLI) #define CO_TX_IDX_LSS_SLV (CO_TX_IDX_HB_PROD + CO_TX_CNT_HB_PROD) @@ -299,6 +301,26 @@ #endif /* #ifdef #else CO_MULTIPLE_OD */ +/* MPDO buffer counts depend only on the compile-time CO_CONFIG_MPDO flags + * (there is no per-OD count source for them), so they live outside the + * single-OD vs CO_MULTIPLE_OD discriminator. */ +#if (CO_CONFIG_MPDO) != 0 + #if (CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM) + #define CO_RX_CNT_MPDO CO_CONFIG_MPDO_RX_COUNT + #else + #define CO_RX_CNT_MPDO 0 + #endif + #if (CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM) + #define CO_TX_CNT_MPDO CO_CONFIG_MPDO_TX_COUNT + #else + #define CO_TX_CNT_MPDO 0 + #endif +#else + #define CO_RX_CNT_MPDO 0 + #define CO_TX_CNT_MPDO 0 +#endif + + /* Objects from heap **********************************************************/ #ifndef CO_USE_GLOBALS #include @@ -473,6 +495,10 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { } #endif +#if (CO_CONFIG_MPDO) != 0 + CO_alloc_break_on_fail(co->MPDO, 1, sizeof(*co->MPDO)); +#endif + #if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE if (CO_GET_CNT(LEDS) == 1) { CO_alloc_break_on_fail(co->LEDs, CO_GET_CNT(LEDS), sizeof(*co->LEDs)); @@ -553,6 +579,9 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { #endif #if (CO_CONFIG_PDO) & CO_CONFIG_RPDO_ENABLE co->RX_IDX_RPDO = idxRx; idxRx += RX_CNT_RPDO; +#endif +#if (CO_CONFIG_MPDO) != 0 + co->RX_IDX_MPDO = idxRx; idxRx += CO_RX_CNT_MPDO; #endif co->RX_IDX_SDO_SRV = idxRx; idxRx += RX_CNT_SDO_SRV; #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE @@ -586,6 +615,9 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { #endif #if (CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE co->TX_IDX_TPDO = idxTx; idxTx += TX_CNT_TPDO; +#endif +#if (CO_CONFIG_MPDO) != 0 + co->TX_IDX_MPDO = idxTx; idxTx += CO_TX_CNT_MPDO; #endif co->TX_IDX_SDO_SRV = idxTx; idxTx += TX_CNT_SDO_SRV; #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE @@ -665,6 +697,10 @@ void CO_delete(CO_t *co) { CO_free(co->LEDs); #endif +#if (CO_CONFIG_MPDO) != 0 + CO_free(co->MPDO); +#endif + #if (CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE CO_free(co->TPDO); #endif @@ -742,6 +778,9 @@ void CO_delete(CO_t *co) { #if (CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE static CO_TPDO_t COO_TPDO[OD_CNT_TPDO]; #endif +#if (CO_CONFIG_MPDO) != 0 + static CO_MPDO_t COO_MPDO; +#endif #if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE static CO_LEDs_t COO_LEDs; #endif @@ -804,6 +843,9 @@ CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed) { #if (CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE co->TPDO = &COO_TPDO[0]; #endif +#if (CO_CONFIG_MPDO) != 0 + co->MPDO = &COO_MPDO; +#endif #if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE co->LEDs = &COO_LEDs; #endif @@ -1342,6 +1384,57 @@ CO_ReturnError_t CO_CANopenInitPDO(CO_t *co, } +/******************************************************************************/ +#if (CO_CONFIG_MPDO) != 0 +CO_ReturnError_t CO_CANopenInitMPDO(CO_t *co, + CO_EM_t *em, + OD_t *od, + uint8_t nodeId) +{ + if (co == NULL || od == NULL) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + if (nodeId < 1 || nodeId > 127 || co->nodeIdUnconfigured) { + return co->nodeIdUnconfigured + ? CO_ERROR_NODE_ID_UNCONFIGURED_LSS : CO_ERROR_ILLEGAL_ARGUMENT; + } + if (em == NULL) { + em = co->em; + } + return CO_MPDO_init(co->MPDO, + od, + em, + co->CANmodule, + nodeId, + CO_GET_CO(RX_IDX_MPDO), + CO_GET_CO(TX_IDX_MPDO)); +} + + +#if (CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM) +void CO_process_MPDO_RX(CO_t *co) { + if (co == NULL || co->nodeIdUnconfigured) { + return; + } + CO_MPDO_processRX(co->MPDO); +} +#endif + + +#if (CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM) +void CO_process_MPDO_TX(CO_t *co, + uint32_t timeDifference_us, + uint32_t *timerNext_us) +{ + if (co == NULL || co->nodeIdUnconfigured) { + return; + } + CO_MPDO_processTX(co->MPDO, timeDifference_us, timerNext_us); +} +#endif +#endif /* (CO_CONFIG_MPDO) != 0 */ + + /******************************************************************************/ CO_NMT_reset_cmd_t CO_process(CO_t *co, bool_t enableGateway, diff --git a/CANopen.h b/CANopen.h index de03169e7..d0303fdf7 100644 --- a/CANopen.h +++ b/CANopen.h @@ -37,6 +37,7 @@ #include "301/CO_SDOclient.h" #include "301/CO_SYNC.h" #include "301/CO_PDO.h" +#include "301/CO_MPDO.h" #include "301/CO_TIME.h" #include "303/CO_LEDs.h" #include "304/CO_GFC.h" @@ -359,6 +360,14 @@ typedef struct { uint16_t TX_IDX_TPDO; /**< Start index in CANtx. */ #endif #endif +#if (CO_CONFIG_MPDO) != 0 || defined CO_DOXYGEN + /** MPDO container, initialised by @ref CO_CANopenInitMPDO() */ + CO_MPDO_t *MPDO; + #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN + uint16_t RX_IDX_MPDO; /**< Start index in CANrx. */ + uint16_t TX_IDX_MPDO; /**< Start index in CANtx. */ + #endif +#endif #if ((CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE) || defined CO_DOXYGEN /** LEDs object, initialised by @ref CO_LEDs_init() */ CO_LEDs_t *LEDs; @@ -649,6 +658,50 @@ void CO_process_TPDO(CO_t *co, #endif +#if (CO_CONFIG_MPDO) != 0 || defined CO_DOXYGEN +/** + * Initialize CANopenNode MPDO container. + * + * Call after @ref CO_CANopenInitPDO(). The container is created empty; + * application configures per-slot RX/TX via @ref CO_MPDO_configRX_DAM / + * @ref CO_MPDO_configTX_DAM after this returns. + * + * @param co CANopen object. + * @param em Emergency object (or NULL to use co->em). + * @param od Object Dictionary. + * @param nodeId 1..127. + * + * @return CO_ERROR_NO in case of success. + */ +CO_ReturnError_t CO_CANopenInitMPDO(CO_t *co, + CO_EM_t *em, + OD_t *od, + uint8_t nodeId); + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) || defined CO_DOXYGEN +/** + * Process MPDO consumer side. Call cyclically from the RT thread after + * @ref CO_process_RPDO(). + */ +void CO_process_MPDO_RX(CO_t *co); +#endif + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM)) || defined CO_DOXYGEN +/** + * Process MPDO producer side. Call cyclically from the RT thread after + * @ref CO_process_TPDO(). + * + * @param co CANopen object. + * @param timeDifference_us Time since last call. + * @param [out] timerNext_us info to OS — see CO_process(); may be NULL. + */ +void CO_process_MPDO_TX(CO_t *co, + uint32_t timeDifference_us, + uint32_t *timerNext_us); +#endif +#endif /* (CO_CONFIG_MPDO) != 0 */ + + #if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN /** * Process CANopen SRDO objects. From 9ca38401a04e07df33cd49b1862b310609c836a2 Mon Sep 17 00:00:00 2001 From: Tadas Aleksonis Date: Thu, 14 May 2026 18:45:02 -0700 Subject: [PATCH 2/2] SAM_MPDO MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add SAM (Source Address Mode) consumer and producer alongside the DAM first-pass: - CO_MPDO_applySAM: peer to applyDAM; dispatcher table looks up (producerNodeId, srcIdx, srcSub) -> (dstIdx, dstSub) and writes the payload into the local OD. - CO_MPDO_scanWrite: OD-extension write callback that flags the scanned entry dirty; CO_MPDO_processTX SAM walker emits one frame per dirty row on the carrier TX slot, with clear-before-read race ordering. - Public API: CO_MPDO_configRX_SAM, CO_MPDO_dispatchAdd_SAM, CO_MPDO_configTX_SAM, CO_MPDO_scanAdd_SAM. Programmatic registration (no 0x1FA0 / 0x1FD0 OD parsing yet); the OD-driven path is a future enhancement layered on these primitives. - CO_MPDO_processTX: inhibit decrement lifted out of the DAM-only block so SAM-only builds tick the timers. Compile-tested for CO_CONFIG_MPDO ∈ {0, RX_DAM|TX_DAM, RX_SAM|TX_SAM, all four, RX_SAM only, TX_SAM only} with -Wall -Wextra -Wpedantic -Wshadow -Wstrict-prototypes -Wmissing-prototypes -std=c11. Co-Authored-By: Claude Opus 4.7 (1M context) --- 301/CO_MPDO.c | 350 ++++++++++++++++++++++++++++++++++++++++++++++++-- 301/CO_MPDO.h | 154 +++++++++++++++++++++- 2 files changed, 489 insertions(+), 15 deletions(-) diff --git a/301/CO_MPDO.c b/301/CO_MPDO.c index 14f63e346..b34e0855e 100644 --- a/301/CO_MPDO.c +++ b/301/CO_MPDO.c @@ -122,6 +122,123 @@ static bool_t CO_MPDO_applyDAM(CO_MPDO_t *MPDO, const uint8_t *frame) { #endif /* (CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM */ +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_SAM) +/* + * Apply one SAM frame to the local OD. + * + * CiA 301 §7.2.5.3 — byte 0 bit 7 set, low 7 bits = producer node ID; the + * dispatcher table re-maps (producerNodeId, srcIdx, srcSub) to a local + * (dstIdx, dstSub). Frames with no matching row are silently dropped; only + * downstream OD-write failures raise EMCY. + */ +static bool_t CO_MPDO_applySAM(CO_MPDO_t *MPDO, const uint8_t *frame) { + uint8_t byte0 = frame[0]; + + /* SAM iff bit 7 is set. */ + if ((byte0 & CO_MPDO_BYTE0_SAM_MASK) == 0U) { + return false; + } + + uint8_t srcNodeId = byte0 & CO_MPDO_BYTE0_NODEID_MASK; + uint16_t srcIdx = (uint16_t)frame[1] | ((uint16_t)frame[2] << 8); + uint8_t srcSub = frame[3]; + + /* First-match-wins linear scan; dispatch tables stay small enough that + * a hash buys nothing — see docs/mpdo-implementation-plan.md §3.3. */ + CO_MPDO_dispatch_t *match = NULL; + for (uint16_t i = 0; i < CO_CONFIG_MPDO_DISPATCH_COUNT; i++) { + CO_MPDO_dispatch_t *d = &MPDO->dispatch[i]; + if (!d->valid) { + continue; + } + if (d->srcNodeId == srcNodeId + && d->srcIdx == srcIdx + && d->srcSub == srcSub + ) { + match = d; + break; + } + } + + /* Dispatcher miss: drop silently. The producer is broadcasting whatever + * it scans; only rows we have explicitly subscribed to are routed. */ + if (match == NULL) { + return true; + } + + OD_entry_t *entry = OD_find(MPDO->OD, match->dstIdx); + if (entry == NULL) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, + ((uint32_t)match->dstIdx << 16) | ((uint32_t)match->dstSub << 8)); + return false; + } + + OD_IO_t io; + ODR_t odRet = OD_getSub(entry, match->dstSub, &io, false); + if (odRet != ODR_OK) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, + ((uint32_t)match->dstIdx << 16) | ((uint32_t)match->dstSub << 8)); + return false; + } + + if ((io.stream.attribute & ODA_RPDO) == 0U) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, + ((uint32_t)match->dstIdx << 16) | ((uint32_t)match->dstSub << 8)); + return false; + } + + OD_size_t writeLen = io.stream.dataLength; + if (writeLen == 0U || writeLen > 4U) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, + ((uint32_t)match->dstIdx << 16) | ((uint32_t)match->dstSub << 8)); + return false; + } + + io.stream.dataOffset = 0; + OD_size_t countWritten = 0; + + CO_LOCK_OD(MPDO->CANdev); + ODR_t writeRet = io.write(&io.stream, &frame[4], writeLen, &countWritten); + CO_UNLOCK_OD(MPDO->CANdev); + + if (writeRet != ODR_OK || countWritten != writeLen) { + CO_errorReport(MPDO->em, CO_EM_RPDO_WRONG_LENGTH, + CO_EMC_DAM_MPDO, + ((uint32_t)match->dstIdx << 16) | ((uint32_t)match->dstSub << 8)); + return false; + } + + return true; +} +#endif /* (CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_SAM */ + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM) +/* + * OD extension write callback installed on each scanned local OD entry. + * + * Lets the underlying storage absorb the write via OD_writeOriginal, then + * flags the row dirty so CO_MPDO_processTX emits a SAM frame next tick. + */ +static ODR_t CO_MPDO_scanWrite(OD_stream_t *stream, + const void *buf, + OD_size_t count, + OD_size_t *countWritten) +{ + ODR_t ret = OD_writeOriginal(stream, buf, count, countWritten); + CO_MPDO_scan_t *scan = (CO_MPDO_scan_t *)(stream != NULL ? stream->object : NULL); + if (ret == ODR_OK && scan != NULL) { + scan->dirty = true; + } + return ret; +} +#endif /* (CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM */ + + /******************************************************************************/ CO_ReturnError_t CO_MPDO_init(CO_MPDO_t *MPDO, OD_t *OD, @@ -157,11 +274,13 @@ CO_ReturnError_t CO_MPDO_init(CO_MPDO_t *MPDO, } -#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM) -/******************************************************************************/ -CO_ReturnError_t CO_MPDO_configRX_DAM(CO_MPDO_t *MPDO, - uint8_t slotIdx, - uint16_t canId) +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) +/* RX-slot subscription is mode-agnostic — the per-frame mode dispatcher in + * processRX picks DAM vs SAM by byte 0. Public configRX_DAM / configRX_SAM + * wrappers exist for self-documenting call sites. */ +static CO_ReturnError_t CO_MPDO_configRX(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId) { if (MPDO == NULL || slotIdx >= CO_CONFIG_MPDO_RX_COUNT || canId == 0U || (canId & ~0x7FFU) != 0U @@ -188,12 +307,64 @@ CO_ReturnError_t CO_MPDO_configRX_DAM(CO_MPDO_t *MPDO, #endif -#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM) +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM) /******************************************************************************/ -CO_ReturnError_t CO_MPDO_configTX_DAM(CO_MPDO_t *MPDO, +CO_ReturnError_t CO_MPDO_configRX_DAM(CO_MPDO_t *MPDO, uint8_t slotIdx, - uint16_t canId, - uint32_t inhibitTime_us) + uint16_t canId) +{ + return CO_MPDO_configRX(MPDO, slotIdx, canId); +} +#endif + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_SAM) +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_configRX_SAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId) +{ + return CO_MPDO_configRX(MPDO, slotIdx, canId); +} + + +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_dispatchAdd_SAM(CO_MPDO_t *MPDO, + uint8_t producerNodeId, + uint16_t srcIdx, + uint8_t srcSub, + uint16_t dstIdx, + uint8_t dstSub) +{ + if (MPDO == NULL || producerNodeId < 1U || producerNodeId > 127U) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + + for (uint16_t i = 0; i < CO_CONFIG_MPDO_DISPATCH_COUNT; i++) { + CO_MPDO_dispatch_t *d = &MPDO->dispatch[i]; + if (d->valid) { + continue; + } + d->srcNodeId = producerNodeId; + d->srcIdx = srcIdx; + d->srcSub = srcSub; + d->dstIdx = dstIdx; + d->dstSub = dstSub; + d->valid = true; + return CO_ERROR_NO; + } + return CO_ERROR_OUT_OF_MEMORY; +} +#endif + + +#if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_TX_DAM | CO_CONFIG_MPDO_TX_SAM)) +/* TX-slot subscription is mode-agnostic — the public configTX_DAM / + * configTX_SAM wrappers exist for self-documenting call sites. */ +static CO_ReturnError_t CO_MPDO_configTX(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId, + uint32_t inhibitTime_us) { if (MPDO == NULL || slotIdx >= CO_CONFIG_MPDO_TX_COUNT || canId == 0U || (canId & ~0x7FFU) != 0U @@ -217,6 +388,18 @@ CO_ReturnError_t CO_MPDO_configTX_DAM(CO_MPDO_t *MPDO, tx->valid = true; return CO_ERROR_NO; } +#endif + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM) +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_configTX_DAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId, + uint32_t inhibitTime_us) +{ + return CO_MPDO_configTX(MPDO, slotIdx, canId, inhibitTime_us); +} /******************************************************************************/ @@ -257,6 +440,78 @@ CO_ReturnError_t CO_MPDO_send_DAM(CO_MPDO_t *MPDO, #endif /* (CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM */ +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM) +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_configTX_SAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId, + uint32_t inhibitTime_us) +{ + return CO_MPDO_configTX(MPDO, slotIdx, canId, inhibitTime_us); +} + + +/******************************************************************************/ +CO_ReturnError_t CO_MPDO_scanAdd_SAM(CO_MPDO_t *MPDO, + uint8_t txSlotIdx, + uint16_t srcIdx, + uint8_t srcSub, + uint8_t length) +{ + if (MPDO == NULL || txSlotIdx >= CO_CONFIG_MPDO_TX_COUNT + || length == 0U || length > 4U + ) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + if (!MPDO->tx[txSlotIdx].valid) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + + OD_entry_t *entry = OD_find(MPDO->OD, srcIdx); + if (entry == NULL) { + return CO_ERROR_OD_PARAMETERS; + } + + /* Probe the storage length via odOrig so we don't trip extensions. */ + OD_IO_t io; + ODR_t odRet = OD_getSub(entry, srcSub, &io, true); + if (odRet != ODR_OK) { + return CO_ERROR_OD_PARAMETERS; + } + if (io.stream.dataLength != length) { + return CO_ERROR_ILLEGAL_ARGUMENT; + } + + CO_MPDO_scan_t *scan = NULL; + for (uint16_t i = 0; i < CO_CONFIG_MPDO_SCAN_COUNT; i++) { + if (!MPDO->scan[i].valid) { + scan = &MPDO->scan[i]; + break; + } + } + if (scan == NULL) { + return CO_ERROR_OUT_OF_MEMORY; + } + + scan->srcIdx = srcIdx; + scan->srcSub = srcSub; + scan->length = length; + scan->txSlotIdx = txSlotIdx; + scan->entry = entry; + scan->dirty = false; + scan->scanExt.object = scan; + scan->scanExt.read = OD_readOriginal; + scan->scanExt.write = CO_MPDO_scanWrite; + + if (OD_extension_init(entry, &scan->scanExt) != ODR_OK) { + return CO_ERROR_OD_PARAMETERS; + } + scan->valid = true; + return CO_ERROR_NO; +} +#endif /* (CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM */ + + #if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) /******************************************************************************/ void CO_MPDO_processRX(CO_MPDO_t *MPDO) { @@ -276,8 +531,9 @@ void CO_MPDO_processRX(CO_MPDO_t *MPDO) { #if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_DAM) (void) CO_MPDO_applyDAM(MPDO, frame); -#else - (void) frame; /* SAM-only build: stub for now */ +#endif +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_SAM) + (void) CO_MPDO_applySAM(MPDO, frame); #endif } } @@ -290,23 +546,30 @@ void CO_MPDO_processTX(CO_MPDO_t *MPDO, uint32_t timeDifference_us, uint32_t *timerNext_us) { - (void) timerNext_us; if (MPDO == NULL) { return; } -#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM) + /* Drain inhibit timers once per tick regardless of mode — both DAM and + * SAM share the same per-slot CANtxBuff and inhibit window. */ for (uint8_t i = 0; i < CO_CONFIG_MPDO_TX_COUNT; i++) { CO_MPDO_tx_t *tx = &MPDO->tx[i]; if (!tx->valid) { continue; } - if (tx->inhibitTimer > timeDifference_us) { tx->inhibitTimer -= timeDifference_us; } else { tx->inhibitTimer = 0; } + } + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_DAM) + for (uint8_t i = 0; i < CO_CONFIG_MPDO_TX_COUNT; i++) { + CO_MPDO_tx_t *tx = &MPDO->tx[i]; + if (!tx->valid) { + continue; + } if (tx->sendRequest && tx->inhibitTimer == 0U) { memcpy(&tx->CANtxBuff->data[0], tx->pendingFrame, 8); @@ -325,6 +588,65 @@ void CO_MPDO_processTX(CO_MPDO_t *MPDO, } } #endif + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM) + for (uint16_t i = 0; i < CO_CONFIG_MPDO_SCAN_COUNT; i++) { + CO_MPDO_scan_t *scan = &MPDO->scan[i]; + if (!scan->valid || !scan->dirty) { + continue; + } + CO_MPDO_tx_t *tx = &MPDO->tx[scan->txSlotIdx]; + if (!tx->valid) { + scan->dirty = false; + continue; + } + if (tx->inhibitTimer != 0U) { + if (timerNext_us != NULL && *timerNext_us > tx->inhibitTimer) { + *timerNext_us = tx->inhibitTimer; + } + continue; + } + + /* Clear dirty before reading so concurrent writes during the read + * re-arm us for next tick rather than being lost. */ + scan->dirty = false; + + OD_IO_t io; + ODR_t odRet = OD_getSub(scan->entry, scan->srcSub, &io, true); + if (odRet != ODR_OK) { + continue; + } + + uint8_t payload[4]; + memset(payload, 0, sizeof(payload)); + OD_size_t countRead = 0; + io.stream.dataOffset = 0; + + CO_LOCK_OD(MPDO->CANdev); + ODR_t readRet = io.read(&io.stream, payload, scan->length, &countRead); + CO_UNLOCK_OD(MPDO->CANdev); + + if (readRet != ODR_OK || countRead != scan->length) { + continue; + } + + uint8_t *frame = tx->CANtxBuff->data; + frame[0] = (uint8_t)(CO_MPDO_BYTE0_SAM_MASK + | (MPDO->ownNodeId & CO_MPDO_BYTE0_NODEID_MASK)); + frame[1] = (uint8_t)(scan->srcIdx & 0xFFU); + frame[2] = (uint8_t)(scan->srcIdx >> 8); + frame[3] = scan->srcSub; + memset(&frame[4], 0, 4); + memcpy(&frame[4], payload, scan->length); + + if (CO_CANsend(MPDO->CANdev, tx->CANtxBuff) == CO_ERROR_NO) { + tx->inhibitTimer = tx->inhibitTime_us; + } else { + /* TX queue full — re-arm for retry next tick. */ + scan->dirty = true; + } + } +#endif } #endif diff --git a/301/CO_MPDO.h b/301/CO_MPDO.h index 2c96479af..67262319a 100644 --- a/301/CO_MPDO.h +++ b/301/CO_MPDO.h @@ -66,6 +66,16 @@ extern "C" { #define CO_CONFIG_MPDO_TX_COUNT 1 #endif +/** Maximum number of SAM dispatch entries (consumer side, 0x1FD0 equivalent). */ +#ifndef CO_CONFIG_MPDO_DISPATCH_COUNT +#define CO_CONFIG_MPDO_DISPATCH_COUNT 8 +#endif + +/** Maximum number of SAM scan entries (producer side, 0x1FA0 equivalent). */ +#ifndef CO_CONFIG_MPDO_SCAN_COUNT +#define CO_CONFIG_MPDO_SCAN_COUNT 8 +#endif + /** MPDO frame addressing mode. */ typedef enum { CO_MPDO_MODE_DAM = 0, @@ -102,11 +112,47 @@ typedef struct { CO_CANtx_t *CANtxBuff; /**< Tx buffer, returned by CO_CANtxBufferInit. */ uint32_t inhibitTime_us; /**< Inhibit interval. */ uint32_t inhibitTimer; /**< Counts down to 0 between sends. */ - volatile bool_t sendRequest; /**< Caller scheduled a frame. */ + volatile bool_t sendRequest; /**< Caller scheduled a frame (DAM). */ uint8_t pendingFrame[8]; /**< Built by CO_MPDO_send_DAM(). */ } CO_MPDO_tx_t; #endif +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_SAM) || defined CO_DOXYGEN +/** + * One row of the SAM dispatcher table. CiA 301 §7.2.5.3 OD 0x1FD0 equivalent. + * On receive of a SAM frame, the table is searched for a row whose + * (srcNodeId, srcIdx, srcSub) matches the frame, and the payload is then + * written to (dstIdx, dstSub) on this node's OD. + */ +typedef struct { + bool_t valid; /**< Slot is configured. */ + uint8_t srcNodeId; /**< Producer node ID (1..127). */ + uint16_t srcIdx; /**< Producer-side OD index. */ + uint8_t srcSub; /**< Producer-side OD sub-index. */ + uint16_t dstIdx; /**< Local OD index to write into. */ + uint8_t dstSub; /**< Local OD sub-index to write into. */ +} CO_MPDO_dispatch_t; +#endif + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM) || defined CO_DOXYGEN +/** + * One row of the SAM scanner table. CiA 301 §7.2.5.3 OD 0x1FA0 equivalent. + * Bound to a local OD entry via OD_extension_init(); writes to that entry + * raise the dirty flag, and CO_MPDO_processTX emits one SAM frame per dirty + * row on the carrier TX slot. + */ +typedef struct { + bool_t valid; /**< Slot is configured. */ + uint16_t srcIdx; /**< Local OD index being scanned. */ + uint8_t srcSub; /**< Local OD sub-index being scanned. */ + uint8_t length; /**< 1..4 bytes of payload to emit. */ + uint8_t txSlotIdx; /**< Carrier TX slot in CO_MPDO_t::tx[]. */ + OD_entry_t *entry; /**< Cached OD_find() result, for read on emit. */ + OD_extension_t scanExt; /**< Installed on the scanned OD entry. */ + volatile bool_t dirty; /**< Set on local write, cleared on emit. */ +} CO_MPDO_scan_t; +#endif + /** * Top-level MPDO state, one instance per @ref CO_t. */ @@ -123,6 +169,12 @@ typedef struct { CO_MPDO_tx_t tx[CO_CONFIG_MPDO_TX_COUNT]; uint16_t txIdxBase; /**< First txArray index owned by MPDO. */ #endif +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_SAM) || defined CO_DOXYGEN + CO_MPDO_dispatch_t dispatch[CO_CONFIG_MPDO_DISPATCH_COUNT]; +#endif +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM) || defined CO_DOXYGEN + CO_MPDO_scan_t scan[CO_CONFIG_MPDO_SCAN_COUNT]; +#endif } CO_MPDO_t; @@ -210,6 +262,106 @@ CO_ReturnError_t CO_MPDO_send_DAM(CO_MPDO_t *MPDO, #endif +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_RX_SAM) || defined CO_DOXYGEN +/** + * Configure one MPDO consumer slot for SAM reception. Behaves identically to + * CO_MPDO_configRX_DAM — same CAN-ID subscription — but is provided as a + * separate symbol so callers self-document the slot's intent. When both RX + * modes are compiled in, a single subscribed CAN-ID delivers DAM and SAM + * frames to the same slot; the mode is picked apart by byte 0 of the payload. + * + * @param MPDO Container. + * @param slotIdx 0..CO_CONFIG_MPDO_RX_COUNT-1. + * @param canId 11-bit COB-ID to subscribe to. + * + * @return CO_ERROR_NO on success. + */ +CO_ReturnError_t CO_MPDO_configRX_SAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId); + + +/** + * Append one entry to the SAM dispatcher table. + * + * On receipt of a SAM frame whose byte 0 names @p producerNodeId and whose + * (srcIdx, srcSub) match this row, the payload is written into this node's + * (dstIdx, dstSub). The local OD entry must have ODA_RPDO set and a length + * in 1..4; payload that does not match the destination length raises + * CO_EMC_DAM_MPDO. Frames with no matching row are silently dropped. + * + * First-match-wins on duplicates; rows are scanned in registration order. + * + * @param MPDO Container. + * @param producerNodeId 1..127 — producer node we accept frames from. + * @param srcIdx Producer OD index named in the SAM frame. + * @param srcSub Producer OD sub-index named in the SAM frame. + * @param dstIdx Local OD index to write into. + * @param dstSub Local OD sub-index to write into. + * + * @return CO_ERROR_NO on success, + * CO_ERROR_OUT_OF_MEMORY if dispatch table is full, + * CO_ERROR_ILLEGAL_ARGUMENT on bad args. + */ +CO_ReturnError_t CO_MPDO_dispatchAdd_SAM(CO_MPDO_t *MPDO, + uint8_t producerNodeId, + uint16_t srcIdx, + uint8_t srcSub, + uint16_t dstIdx, + uint8_t dstSub); +#endif + + +#if ((CO_CONFIG_MPDO) & CO_CONFIG_MPDO_TX_SAM) || defined CO_DOXYGEN +/** + * Configure one MPDO producer slot to carry SAM frames. + * + * @param MPDO Container. + * @param slotIdx 0..CO_CONFIG_MPDO_TX_COUNT-1. + * @param canId 11-bit COB-ID to publish on. + * @param inhibitTime_us Minimum spacing between sends on this slot. + * + * @return CO_ERROR_NO on success. + */ +CO_ReturnError_t CO_MPDO_configTX_SAM(CO_MPDO_t *MPDO, + uint8_t slotIdx, + uint16_t canId, + uint32_t inhibitTime_us); + + +/** + * Register a local OD entry for SAM scanning. + * + * Installs an OD_extension_t on (srcIdx, srcSub) that flags the row dirty on + * write while preserving the original storage via OD_writeOriginal. The + * @ref CO_MPDO_processTX walker then emits one SAM frame per dirty row on + * the carrier @p txSlotIdx, subject to that slot's inhibit window. + * + * Warning: this overwrites any extension previously installed on the entry. + * Applications that need to coexist with custom OD extensions must wrap the + * scan plumbing manually for now. + * + * @param MPDO Container. + * @param txSlotIdx Carrier TX slot, must have been configured via + * CO_MPDO_configTX_SAM(). + * @param srcIdx Local OD index to scan. + * @param srcSub Local OD sub-index to scan. + * @param length 1..4 — payload size to emit per frame; must match the OD + * entry's storage length. + * + * @return CO_ERROR_NO on success, + * CO_ERROR_OUT_OF_MEMORY if scan table is full, + * CO_ERROR_OD_PARAMETERS if (srcIdx, srcSub) is not in the OD, + * CO_ERROR_ILLEGAL_ARGUMENT on bad length / TX slot. + */ +CO_ReturnError_t CO_MPDO_scanAdd_SAM(CO_MPDO_t *MPDO, + uint8_t txSlotIdx, + uint16_t srcIdx, + uint8_t srcSub, + uint8_t length); +#endif + + #if ((CO_CONFIG_MPDO) & (CO_CONFIG_MPDO_RX_DAM | CO_CONFIG_MPDO_RX_SAM)) || defined CO_DOXYGEN /** * Drain pending MPDO frames into the OD. Call once per RT tick after