From 2296b5581217571b435ee862b526ac37dbe96520 Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Mon, 22 Jun 2020 15:01:06 +0200 Subject: [PATCH] Add/extend support for upload and download callbacks --- soes/ecat_slv.c | 58 ++++++++++++++++++++++++++++++++++++++++++------- soes/ecat_slv.h | 6 ++++- soes/esc.c | 2 ++ soes/esc.h | 21 ++++++++++++++++-- soes/esc_coe.c | 53 ++++++++++++++++++++++++++++++++++++++------ soes/esc_coe.h | 10 +++++++-- 6 files changed, 130 insertions(+), 20 deletions(-) diff --git a/soes/ecat_slv.c b/soes/ecat_slv.c index c28055d..e634428 100644 --- a/soes/ecat_slv.c +++ b/soes/ecat_slv.c @@ -40,14 +40,12 @@ extern uint8_t * txpdo; * @param[in] sub-index = sub-index of SDO download request to check * @return SDO abort code, or 0 on success */ -uint32_t ESC_pre_objecthandler (uint16_t index, +uint32_t ESC_download_pre_objecthandler (uint16_t index, uint8_t subindex, void * data, size_t size, uint16_t flags) { - uint32_t abort = 0; - if (IS_RXPDO (index) || IS_TXPDO (index) || index == RX_PDO_OBJIDX || @@ -55,20 +53,20 @@ uint32_t ESC_pre_objecthandler (uint16_t index, { if (subindex > 0 && COE_maxSub (index) != 0) { - abort = ABORT_SUBINDEX0_NOT_ZERO; + return ABORT_SUBINDEX0_NOT_ZERO; } } if (ESCvar.pre_object_download_hook) { - abort = (ESCvar.pre_object_download_hook) (index, + return (ESCvar.pre_object_download_hook) (index, subindex, data, size, flags); } - return abort; + return 0; } /** Hook called from the slave stack SDO Download handler to act on @@ -76,13 +74,57 @@ uint32_t ESC_pre_objecthandler (uint16_t index, * * @param[in] index = index of SDO download request to handle * @param[in] sub-index = sub-index of SDO download request to handle + * @return SDO abort code, or 0 on success */ -void ESC_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags) +uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags) { if (ESCvar.post_object_download_hook != NULL) { - (ESCvar.post_object_download_hook)(index, subindex, flags); + return (ESCvar.post_object_download_hook)(index, subindex, flags); } + + return 0; +} + +/** Function to pre-qualify the incoming SDO upload. + * + * @param[in] index = index of SDO upload request to handle + * @param[in] sub-index = sub-index of SDO upload request to handle + * @return SDO abort code, or 0 on success + */ +uint32_t ESC_upload_pre_objecthandler (uint16_t index, + uint8_t subindex, + void * data, + size_t size, + uint16_t flags) +{ + if (ESCvar.pre_object_upload_hook != NULL) + { + return (ESCvar.pre_object_upload_hook) (index, + subindex, + data, + size, + flags); + } + + return 0; +} + +/** Hook called from the slave stack SDO Upload handler to act on + * user specified Index and Sub-index. + * + * @param[in] index = index of SDO upload request to handle + * @param[in] sub-index = sub-index of SDO upload request to handle + * @return SDO abort code, or 0 on success + */ +uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags) +{ + if (ESCvar.post_object_upload_hook != NULL) + { + return (ESCvar.post_object_upload_hook)(index, subindex, flags); + } + + return 0; } /** Hook called from the slave stack ESC_stopoutputs to act on state changes diff --git a/soes/ecat_slv.h b/soes/ecat_slv.h index fb15514..160f296 100644 --- a/soes/ecat_slv.h +++ b/soes/ecat_slv.h @@ -1,7 +1,11 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + #ifndef __ECAT_SLV_H__ #define __ECAT_SLV_H__ -#include "ecat_slv.h" #include "options.h" #include "esc.h" diff --git a/soes/esc.c b/soes/esc.c index c1d1342..7c8c12b 100644 --- a/soes/esc.c +++ b/soes/esc.c @@ -1165,6 +1165,8 @@ void ESC_config (esc_cfg_t * cfg) ESCvar.safeoutput_override = cfg->safeoutput_override; ESCvar.pre_object_download_hook = cfg->pre_object_download_hook; ESCvar.post_object_download_hook = cfg->post_object_download_hook; + ESCvar.pre_object_upload_hook = cfg->pre_object_upload_hook; + ESCvar.post_object_upload_hook = cfg->post_object_upload_hook; ESCvar.rxpdo_override = cfg->rxpdo_override; ESCvar.txpdo_override = cfg->txpdo_override; ESCvar.esc_hw_interrupt_enable = cfg->esc_hw_interrupt_enable; diff --git a/soes/esc.h b/soes/esc.h index 684e0d7..1f70de2 100644 --- a/soes/esc.h +++ b/soes/esc.h @@ -287,7 +287,15 @@ typedef struct esc_cfg void * data, size_t size, uint16_t flags); - void (*post_object_download_hook) (uint16_t index, + uint32_t (*post_object_download_hook) (uint16_t index, + uint8_t subindex, + uint16_t flags); + uint32_t (*pre_object_upload_hook) (uint16_t index, + uint8_t subindex, + void * data, + size_t size, + uint16_t flags); + uint32_t (*post_object_upload_hook) (uint16_t index, uint8_t subindex, uint16_t flags); void (*rxpdo_override) (void); @@ -402,7 +410,15 @@ typedef struct void * data, size_t size, uint16_t flags); - void (*post_object_download_hook) (uint16_t index, + uint32_t (*post_object_download_hook) (uint16_t index, + uint8_t subindex, + uint16_t flags); + uint32_t (*pre_object_upload_hook) (uint16_t index, + uint8_t subindex, + void * data, + size_t size, + uint16_t flags); + uint32_t (*post_object_upload_hook) (uint16_t index, uint8_t subindex, uint16_t flags); void (*rxpdo_override) (void); @@ -436,6 +452,7 @@ typedef struct uint16_t entries; uint16_t frags; uint16_t fragsleft; + uint16_t flags; uint8_t toggle; diff --git a/soes/esc_coe.c b/soes/esc_coe.c index f5d1840..ec8cece 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -245,6 +245,7 @@ void SDO_upload (void) uint8_t MBXout; uint32_t size; uint8_t dss; + uint32_t abort = 1; const _objd *objd; coesdo = (_COEsdo *) &MBX[0]; index = etohs (coesdo->index); @@ -295,8 +296,17 @@ void SDO_upload (void) { /* convert bits to bytes */ size = (size + 7) >> 3; - /* use dynamic data */ - copy2mbx ((objd + nsub)->data, &(coeres->size), size); + abort = ESC_upload_pre_objecthandler (index, subindex, + (objd + nsub)->data, size, (objd + nsub)->flags); + if (abort == 0) + { + /* use dynamic data */ + copy2mbx ((objd + nsub)->data, &(coeres->size), size); + } + else + { + SDO_abort (index, subindex, abort); + } } } else @@ -319,14 +329,33 @@ void SDO_upload (void) /* signal segmented transfer */ ESCvar.segmented = MBXSEU; ESCvar.data = (objd + nsub)->data; + ESCvar.flags = (objd + nsub)->flags; } else { ESCvar.segmented = 0; } coeres->mbxheader.length = htoes (COE_HEADERSIZE + size); - /* use dynamic data */ - copy2mbx ((objd + nsub)->data, (&(coeres->size)) + 1, size); + abort = ESC_upload_pre_objecthandler (index, subindex, + (objd + nsub)->data, size, (objd + nsub)->flags); + if (abort == 0) + { + /* use dynamic data */ + copy2mbx ((objd + nsub)->data, (&(coeres->size)) + 1, size); + } + else + { + SDO_abort (index, subindex, abort); + } + } + if ((abort == 0) && (ESCvar.segmented == 0)) + { + abort = ESC_upload_post_objecthandler (index, subindex, + (objd + nsub)->flags); + if (abort != 0) + { + SDO_abort (index, subindex, abort); + } } MBXcontrol[MBXout].state = MBXstate_outreq; } @@ -352,7 +381,7 @@ void SDO_uploadsegment (void) { _COEsdo *coesdo, *coeres; uint8_t MBXout; - uint32_t size, offset; + uint32_t size, offset, abort; coesdo = (_COEsdo *) &MBX[0]; MBXout = ESC_claimbuffer (); if (MBXout) @@ -392,6 +421,12 @@ void SDO_uploadsegment (void) } copy2mbx ((uint8_t *) ESCvar.data + offset, (&(coeres->command)) + 1, size); //copy to mailbox + abort = ESC_upload_post_objecthandler (etohs (coesdo->index), + coesdo->subindex, ESCvar.flags); + if (abort != 0) + { + SDO_abort (etohs (coesdo->index), coesdo->subindex, abort); + } MBXcontrol[MBXout].state = MBXstate_outreq; } MBXcontrol[0].state = MBXstate_idle; @@ -444,7 +479,7 @@ void SDO_download (void) actsize = ((objd + nsub)->bitlength + 7) >> 3; if (actsize == size) { - abort = ESC_pre_objecthandler ( + abort = ESC_download_pre_objecthandler ( index, subindex, mbxdata, @@ -469,7 +504,11 @@ void SDO_download (void) MBXcontrol[MBXout].state = MBXstate_outreq; } /* external object write handler */ - ESC_objecthandler (index, subindex, (objd + nsub)->flags); + abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags); + if (abort != 0) + { + SDO_abort (index, subindex, abort); + } } else { diff --git a/soes/esc_coe.h b/soes/esc_coe.h index ab18c49..064b737 100644 --- a/soes/esc_coe.h +++ b/soes/esc_coe.h @@ -106,12 +106,18 @@ void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * sm); void COE_pdoUnpack (uint8_t * buffer, int nmappings, _SMmap * sm); uint8_t COE_maxSub (uint16_t index); -extern void ESC_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags); -extern uint32_t ESC_pre_objecthandler (uint16_t index, +extern uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags); +extern uint32_t ESC_download_pre_objecthandler (uint16_t index, uint8_t subindex, void * data, size_t size, uint16_t flags); +extern uint32_t ESC_upload_pre_objecthandler (uint16_t index, + uint8_t subindex, + void * data, + size_t size, + uint16_t flags); +extern uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags); extern const _objectlist SDOobjects[]; #endif