diff --git a/soes/esc.c b/soes/esc.c index f8be93c..093ff68 100644 --- a/soes/esc.c +++ b/soes/esc.c @@ -395,6 +395,9 @@ void ESC_stopmbx (void) ESCvar.frags = 0; ESCvar.fragsleft = 0; ESCvar.txcue = 0; + ESCvar.index = 0; + ESCvar.subindex = 0; + ESCvar.flags = 0; } /** Read Receive mailbox and store data in local ESCvar.MBX variable. diff --git a/soes/esc.h b/soes/esc.h index 51bf720..b14eaad 100644 --- a/soes/esc.h +++ b/soes/esc.h @@ -161,17 +161,41 @@ #define MBXERR_INVALIDSIZE 0x0008 #define ABORT_NOTOGGLE 0x05030000 +#define ABORT_TRANSFER_TIMEOUT 0x05040000 #define ABORT_UNKNOWN 0x05040001 +#define ABORT_INVALID_BLOCK_SIZE 0x05040002 +#define ABORT_INVALID_SEQUENCE_NUMBER 0x05040003 +#define ABORT_BLOCK_CRC_ERROR 0x05040004 +#define ABORT_OUT_OF_MEMORY 0x05040005 #define ABORT_UNSUPPORTED 0x06010000 #define ABORT_WRITEONLY 0x06010001 #define ABORT_READONLY 0x06010002 #define ABORT_SUBINDEX0_NOT_ZERO 0x06010003 +#define ABORT_CA_NOT_SUPPORTED 0x06010004 #define ABORT_EXCEEDS_MBOX_SIZE 0x06010005 +#define ABORT_SDO_DOWNLOAD_BLOCKED 0x06010006 #define ABORT_NOOBJECT 0x06020000 +#define ABORT_MAPPING_OBJECT_ERROR 0x06040041 +#define ABORT_MAPPING_LENGTH_ERROR 0x06040042 +#define ABORT_GENERAL_PARAMETER_ERROR 0x06040043 +#define ABORT_GENERAL_DEVICE_ERROR 0x06040047 +#define ABORT_HARDWARE_ERROR 0x06060000 #define ABORT_TYPEMISMATCH 0x06070010 +#define ABORT_DATATYPE_TOO_HIGH 0x06070012 +#define ABORT_DATATYPE_TOO_LOW 0x06070013 #define ABORT_NOSUBINDEX 0x06090011 +#define ABORT_VALUE_EXCEEDED 0x06090030 +#define ABORT_VALUE_TOO_HIGH 0x06090031 +#define ABORT_VALUE_TOO_LOW 0x06090032 +#define ABORT_MODULE_LIST_MISMATCH 0x06090033 +#define ABORT_MAX_VAL_LESS_THAN_MIN_VAL 0x06090036 +#define ABORT_RESOURCE_NOT_AVAILABLE 0x060A0023 #define ABORT_GENERALERROR 0x08000000 +#define ABORT_DATA_STORE_ERROR 0x08000020 +#define ABORT_DATA_STORE_LOCAL_ERROR 0x08000021 #define ABORT_NOTINTHISSTATE 0x08000022 +#define ABORT_OBJECT_DICTIONARY_ERROR 0x08000023 +#define ABORT_NO_DATA_AVAILABLE 0x08000024 #define MBXstate_idle 0x00 #define MBXstate_inclaim 0x01 @@ -194,6 +218,8 @@ #define COE_COMMAND_UPLOADSEGREQ 0x60 #define COE_COMMAND_DOWNLOADREQUEST 0x20 #define COE_COMMAND_DOWNLOADRESPONSE 0x60 +#define COE_COMMAND_DOWNLOADSEGREQ 0x00 +#define COE_COMMAND_DOWNLOADSEGRESP 0x20 #define COE_COMMAND_LASTSEGMENTBIT 0x01 #define COE_SIZE_INDICATOR 0x01 #define COE_EXPEDITED_INDICATOR 0x02 @@ -456,6 +482,8 @@ typedef struct uint16_t entries; uint16_t frags; uint16_t fragsleft; + uint16_t index; + uint8_t subindex; uint16_t flags; uint8_t toggle; diff --git a/soes/esc_coe.c b/soes/esc_coe.c index 9f53a41..73156fd 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -296,26 +296,28 @@ void SDO_upload (void) /* expedited response i.e. length<=4 bytes */ coeres->command = COE_COMMAND_UPLOADRESPONSE + COE_SIZE_INDICATOR + COE_EXPEDITED_INDICATOR + dss; - if ((objd + nsub)->data == NULL) + /* convert bits to bytes */ + size = BITS2BYTES(size); + void *dataptr = ((objd + nsub)->data) ? + (objd + nsub)->data : (void *)&((objd + nsub)->value); + abort = ESC_upload_pre_objecthandler (index, subindex, + dataptr, size, (objd + nsub)->flags); + if (abort == 0) { - /* use constant value */ - coeres->size = htoel ((objd + nsub)->value); - } - else - { - /* convert bits to bytes */ - size = BITS2BYTES(size); - abort = ESC_upload_pre_objecthandler (index, subindex, - (objd + nsub)->data, size, (objd + nsub)->flags); - if (abort == 0) + if ((objd + nsub)->data == NULL) + { + /* use constant value */ + coeres->size = htoel ((objd + nsub)->value); + } + else { /* use dynamic data */ copy2mbx ((objd + nsub)->data, &(coeres->size), size); } - else - { - SDO_abort (index, subindex, abort); - } + } + else + { + SDO_abort (index, subindex, abort); } } else @@ -325,12 +327,12 @@ void SDO_upload (void) COE_SIZE_INDICATOR; /* convert bits to bytes */ size = BITS2BYTES(size); + /* set total size in bytes */ + ESCvar.frags = size; coeres->size = htoel (size); if ((size + COE_HEADERSIZE) > ESC_MBXDSIZE) { /* segmented transfer needed */ - /* set total size in bytes */ - ESCvar.frags = size; /* limit to mailbox size */ size = ESC_MBXDSIZE - COE_HEADERSIZE; /* number of bytes done */ @@ -346,7 +348,7 @@ void SDO_upload (void) } coeres->mbxheader.length = htoes (COE_HEADERSIZE + size); abort = ESC_upload_pre_objecthandler (index, subindex, - (objd + nsub)->data, size, (objd + nsub)->flags); + (objd + nsub)->data, ESCvar.frags, (objd + nsub)->flags); if (abort == 0) { /* use dynamic data */ @@ -434,7 +436,7 @@ static uint32_t complete_access_subindex_loop(const _objd *objd, (objd->datatype == DTYPE_OCTET_STRING) || (objd->datatype == DTYPE_UNICODE_STRING)) { - return ABORT_UNSUPPORTED; + return ABORT_CA_NOT_SUPPORTED; } uint32_t size = 0; @@ -604,6 +606,7 @@ static void SDO_upload_complete_access (void) /* signal segmented transfer */ ESCvar.segmented = MBXSEU; ESCvar.data = ESCvar.mbxdata; + ESCvar.flags = COMPLETE_ACCESS_FLAG; } coeres->mbxheader.length = htoes (COE_HEADERSIZE + size); @@ -629,7 +632,7 @@ static void SDO_upload_complete_access (void) /** Function for handling the following SDO Upload if previous SDOUpload * response was flagged it needed to be segmented. */ -static void SDO_uploadsegment (bool complete_access) +static void SDO_uploadsegment (void) { _COEsdo *coesdo, *coeres; uint8_t MBXout; @@ -643,10 +646,6 @@ static void SDO_uploadsegment (bool complete_access) size = ESCvar.frags - ESCvar.fragsleft; uint8_t command = COE_COMMAND_UPLOADSEGMENT + (coesdo->command & COE_TOGGLEBIT); /* copy toggle bit */ - if (complete_access) - { - command |= COE_COMPLETEACCESS; - } init_coesdo(coeres, COE_SDORESPONSE, command, coesdo->index, coesdo->subindex); if ((size + COE_SEGMENTHEADERSIZE) > ESC_MBXDSIZE) @@ -680,15 +679,8 @@ static void SDO_uploadsegment (bool complete_access) if (ESCvar.segmented == 0) { - uint16_t flags = ESCvar.flags; - - if (complete_access) - { - flags |= COMPLETE_ACCESS_FLAG; - } - abort = ESC_upload_post_objecthandler (etohs (coesdo->index), - coesdo->subindex, flags); + coesdo->subindex, ESCvar.flags); if (abort != 0) { SDO_abort (etohs (coesdo->index), coesdo->subindex, abort); @@ -756,6 +748,21 @@ void SDO_download (void) ); if (abort == 0) { + if ((size > 4) && + (size > (coesdo->mbxheader.length - COE_HEADERSIZE))) + { + size = coesdo->mbxheader.length - COE_HEADERSIZE; + /* signal segmented transfer */ + ESCvar.segmented = MBXSED; + ESCvar.data = (objd + nsub)->data + size; + ESCvar.index = index; + ESCvar.subindex = subindex; + ESCvar.flags = (objd + nsub)->flags; + } + else + { + ESCvar.segmented = 0; + } copy2mbx (mbxdata, (objd + nsub)->data, size); MBXout = ESC_claimbuffer (); if (MBXout) @@ -771,11 +778,14 @@ void SDO_download (void) coeres->size = htoel (0); MBXcontrol[MBXout].state = MBXstate_outreq; } - /* external object write handler */ - abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags); - if (abort != 0) + if (ESCvar.segmented == 0) { - SDO_abort (index, subindex, abort); + /* external object write handler */ + abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags); + if (abort != 0) + { + SDO_abort (index, subindex, abort); + } } } else @@ -906,6 +916,51 @@ static void SDO_download_complete_access (void) set_state_idle (index, subindex, 0); } +static void SDO_downloadsegment (void) +{ + _COEsdo *coesdo = (_COEsdo *) &MBX[0]; + uint8_t MBXout = ESC_claimbuffer (); + if (MBXout) + { + _COEsdo *coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE]; + uint32_t size = coesdo->mbxheader.length - 3; + if (size == 7) + { + size = 7 - ((coesdo->command >> 1) & 7); + } + uint8_t command = COE_COMMAND_DOWNLOADSEGRESP + + (coesdo->command & COE_TOGGLEBIT); /* copy toggle bit */ + init_coesdo(coeres, COE_SDORESPONSE, command, 0, 0); + + uint32_t *mbxdata = (uint32_t *)&(coesdo->index); /* data pointer */ + copy2mbx (mbxdata, (uint8_t *)ESCvar.data, size); + + if (coesdo->command & COE_COMMAND_LASTSEGMENTBIT) + { + /* last segment */ + ESCvar.segmented = 0; + + /* external object write handler */ + uint32_t abort = ESC_download_post_objecthandler + (ESCvar.index, ESCvar.subindex, ESCvar.flags); + if (abort != 0) + { + set_state_idle (ESCvar.index, ESCvar.subindex, abort); + return; + } + } + else + { + /* more segmented transfer needed: increase offset */ + ESCvar.data += size; + } + + MBXcontrol[MBXout].state = MBXstate_outreq; + } + + set_state_idle (0, 0, 0); +} + /** Function for sending an SDO Info Error reply. * * @param[in] abortcode = = abort code to send in reply @@ -1258,34 +1313,31 @@ void ESC_coeprocess (void) coesdo = (_COEsdo *) &MBX[0]; coeobjdesc = (_COEobjdesc *) &MBX[0]; service = etohs (coesdo->coeheader.numberservice) >> 12; - /* initiate SDO upload request */ - if ((service == COE_SDOREQUEST) - && (SDO_COMMAND(coesdo->command) == COE_COMMAND_UPLOADREQUEST) - && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE)) + if (service == COE_SDOREQUEST) { - if (SDO_COMPLETE_ACCESS(coesdo->command)) + if ((SDO_COMMAND(coesdo->command) == COE_COMMAND_UPLOADREQUEST) + && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE)) { - SDO_upload_complete_access (); + /* initiate SDO upload request */ + if (SDO_COMPLETE_ACCESS(coesdo->command)) + { + SDO_upload_complete_access (); + } + else + { + SDO_upload (); + } } - else + else if (((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ) + && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE) + && (ESCvar.segmented == MBXSEU)) { - SDO_upload (); + /* SDO upload segment request */ + SDO_uploadsegment (); } - } - /* SDO upload segment request */ - if ((service == COE_SDOREQUEST) - && ((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ) - && (etohs (coesdo->mbxheader.length) == COE_HEADERSIZE) - && (ESCvar.segmented == MBXSEU)) - { - SDO_uploadsegment (SDO_COMPLETE_ACCESS(coesdo->command)); - } - /* initiate SDO download request */ - else - { - if ((service == COE_SDOREQUEST) - && (SDO_COMMAND(coesdo->command) == COE_COMMAND_DOWNLOADREQUEST)) + else if (SDO_COMMAND(coesdo->command) == COE_COMMAND_DOWNLOADREQUEST) { + /* initiate SDO download request */ if (SDO_COMPLETE_ACCESS(coesdo->command)) { SDO_download_complete_access (); @@ -1295,46 +1347,51 @@ void ESC_coeprocess (void) SDO_download (); } } - /* initiate SDO get OD list */ + else if (SDO_COMMAND(coesdo->command) == COE_COMMAND_DOWNLOADSEGREQ) + { + /* SDO download segment request */ + SDO_downloadsegment (); + } + } + /* initiate SDO get OD list */ + else + { + if ((service == COE_SDOINFORMATION) + && (coeobjdesc->infoheader.opcode == 0x01)) + { + SDO_getodlist (); + } + /* initiate SDO get OD */ else { if ((service == COE_SDOINFORMATION) - && (coeobjdesc->infoheader.opcode == 0x01)) + && (coeobjdesc->infoheader.opcode == 0x03)) { - SDO_getodlist (); + SDO_getod (); } - /* initiate SDO get OD */ + /* initiate SDO get ED */ else { if ((service == COE_SDOINFORMATION) - && (coeobjdesc->infoheader.opcode == 0x03)) + && (coeobjdesc->infoheader.opcode == 0x05)) { - SDO_getod (); + SDO_geted (); } - /* initiate SDO get ED */ else { - if ((service == COE_SDOINFORMATION) - && (coeobjdesc->infoheader.opcode == 0x05)) + /* COE not recognised above */ + if (ESCvar.xoe == MBXCOE) { - SDO_geted (); - } - else - { - /* COE not recognised above */ - if (ESCvar.xoe == MBXCOE) + if (service == 0) { - if (service == 0) - { - MBX_error (MBXERR_INVALIDHEADER); - } - else - { - SDO_abort (etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED); - } - MBXcontrol[0].state = MBXstate_idle; - ESCvar.xoe = 0; + MBX_error (MBXERR_INVALIDHEADER); } + else + { + SDO_abort (etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED); + } + MBXcontrol[0].state = MBXstate_idle; + ESCvar.xoe = 0; } } } @@ -1439,17 +1496,20 @@ static uint64_t COE_getValue (const _objd * obj) case DTYPE_BOOLEAN: case DTYPE_UNSIGNED8: case DTYPE_INTEGER8: + case DTYPE_BITARR8: value = *(uint8_t *)obj->data; break; case DTYPE_UNSIGNED16: case DTYPE_INTEGER16: + case DTYPE_BITARR16: value = *(uint16_t *)obj->data; break; case DTYPE_REAL32: case DTYPE_UNSIGNED32: case DTYPE_INTEGER32: + case DTYPE_BITARR32: value = *(uint32_t *)obj->data; break; @@ -1490,17 +1550,20 @@ static void COE_setValue (const _objd * obj, uint64_t value) case DTYPE_BOOLEAN: case DTYPE_UNSIGNED8: case DTYPE_INTEGER8: + case DTYPE_BITARR8: *(uint8_t *)obj->data = value & UINT8_MAX; break; case DTYPE_UNSIGNED16: case DTYPE_INTEGER16: + case DTYPE_BITARR16: *(uint16_t *)obj->data = value & UINT16_MAX; break; case DTYPE_REAL32: case DTYPE_UNSIGNED32: case DTYPE_INTEGER32: + case DTYPE_BITARR32: *(uint32_t *)obj->data = value & UINT32_MAX; break; diff --git a/soes/esc_coe.h b/soes/esc_coe.h index 221e195..20e25d5 100644 --- a/soes/esc_coe.h +++ b/soes/esc_coe.h @@ -72,6 +72,9 @@ typedef struct #define DTYPE_REAL64 0x0011 #define DTYPE_PDO_MAPPING 0x0021 #define DTYPE_IDENTITY 0x0023 +#define DTYPE_BITARR8 0x002D +#define DTYPE_BITARR16 0x002E +#define DTYPE_BITARR32 0x002F #define DTYPE_BIT1 0x0030 #define DTYPE_BIT2 0x0031 #define DTYPE_BIT3 0x0032