nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

Some updates to CO_memcpySwap and in SDO master.

Browse source
This commit is contained in:
Janez 2015-12-11 13:47:06 +01:00
parent b8266e9e20
commit a5224176d2
6 changed files with 159 additions and 68 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
stack/CO_Emergency.c
View file

@ -297,10 +297,10 @@ void CO_errorReport(CO_EM_t *em, const uint8_t errorBit, const uint16_t errorCod
uint8_t bufCopy[8];
/* prepare data for emergency message */
CO_memcpySwap2(&bufCopy[0], (uint8_t*)&errorCode);
CO_memcpySwap2(&bufCopy[0], &errorCode);
bufCopy[2] = 0; /* error register will be set later */
bufCopy[3] = errorBit;
CO_memcpySwap4(&bufCopy[4], (uint8_t*)&infoCode);
CO_memcpySwap4(&bufCopy[4], &infoCode);
/* copy data to the buffer, increment writePtr and verify buffer full */
CO_LOCK_EMCY();
@ -359,7 +359,7 @@ void CO_errorReset(CO_EM_t *em, const uint8_t errorBit, const uint32_t infoCode)
bufCopy[1] = 0;
bufCopy[2] = 0; /* error register will be set later */
bufCopy[3] = errorBit;
CO_memcpySwap4(&bufCopy[4], (uint8_t*)&infoCode);
CO_memcpySwap4(&bufCopy[4], &infoCode);
/* copy data to the buffer, increment writePtr and verify buffer full */
CO_LOCK_EMCY();

90
stack/CO_SDO.c
View file

@ -86,27 +86,71 @@ void CO_setUint32(uint8_t data[], const uint32_t value){
}
#ifdef CO_LITTLE_ENDIAN
void CO_memcpySwap2(uint8_t dest[], const uint8_t src[]){
dest[0] = src[0];
dest[1] = src[1];
void CO_memcpySwap2(void* dest, const void* src){
char *cdest;
char *csrc;
cdest = (char *) dest;
csrc = (char *) src;
cdest[0] = csrc[0];
cdest[1] = csrc[1];
}
void CO_memcpySwap4(uint8_t dest[], const uint8_t src[]){
dest[0] = src[0];
dest[1] = src[1];
dest[2] = src[2];
dest[3] = src[3];
void CO_memcpySwap4(void* dest, const void* src){
char *cdest;
char *csrc;
cdest = (char *) dest;
csrc = (char *) src;
cdest[0] = csrc[0];
cdest[1] = csrc[1];
cdest[2] = csrc[2];
cdest[3] = csrc[3];
}
void CO_memcpySwap8(void* dest, const void* src){
char *cdest;
char *csrc;
cdest = (char *) dest;
csrc = (char *) src;
cdest[0] = csrc[0];
cdest[1] = csrc[1];
cdest[2] = csrc[2];
cdest[3] = csrc[3];
cdest[4] = csrc[4];
cdest[5] = csrc[5];
cdest[6] = csrc[6];
cdest[7] = csrc[7];
}
#endif
#ifdef CO_BIG_ENDIAN
void CO_memcpySwap2(uint8_t dest[], const uint8_t src[]){
dest[0] = src[1];
dest[1] = src[0];
void CO_memcpySwap2(void* dest, const void* src){
char *cdest;
char *csrc;
cdest = (char *) dest;
csrc = (char *) src;
cdest[0] = csrc[1];
cdest[1] = csrc[0];
}
void CO_memcpySwap4(uint8_t dest[], const uint8_t src[]){
dest[0] = src[3];
dest[1] = src[2];
dest[2] = src[1];
dest[3] = src[0];
void CO_memcpySwap4(void* dest, const void* src){
char *cdest;
char *csrc;
cdest = (char *) dest;
csrc = (char *) src;
cdest[0] = csrc[3];
cdest[1] = csrc[2];
cdest[2] = csrc[1];
cdest[3] = csrc[0];
}
void CO_memcpySwap8(void* dest, const void* src){
char *cdest;
char *csrc;
cdest = (char *) dest;
csrc = (char *) src;
cdest[0] = csrc[7];
cdest[1] = csrc[6];
cdest[2] = csrc[5];
cdest[3] = csrc[4];
cdest[4] = csrc[3];
cdest[5] = csrc[2];
cdest[6] = csrc[1];
cdest[7] = csrc[0];
}
#endif
@ -661,7 +705,7 @@ static void CO_SDO_abort(CO_SDO_t *SDO, uint32_t code){
SDO->CANtxBuff->data[1] = SDO->ODF_arg.index & 0xFF;
SDO->CANtxBuff->data[2] = (SDO->ODF_arg.index>>8) & 0xFF;
SDO->CANtxBuff->data[3] = SDO->ODF_arg.subIndex;
CO_memcpySwap4(&SDO->CANtxBuff->data[4], (uint8_t*)&code);
CO_memcpySwap4(&SDO->CANtxBuff->data[4], &code);
SDO->state = CO_SDO_ST_IDLE;
SDO->CANrxNew = false;
CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
@ -836,7 +880,7 @@ int8_t CO_SDO_process(
/* verify length if size is indicated */
if((SDO->CANrxData[0]&0x01) != 0){
uint32_t lenRx;
CO_memcpySwap4((uint8_t*)&lenRx, &SDO->CANrxData[4]);
CO_memcpySwap4(&lenRx, &SDO->CANrxData[4]);
SDO->ODF_arg.dataLengthTotal = lenRx;
/* verify length except for domain data type */
@ -938,7 +982,7 @@ int8_t CO_SDO_process(
/* verify length if size is indicated */
if((SDO->CANrxData[0]&0x02) != 0U){
uint32_t lenRx;
CO_memcpySwap4((uint8_t*)&lenRx, &SDO->CANrxData[4]);
CO_memcpySwap4(&lenRx, &SDO->CANrxData[4]);
SDO->ODF_arg.dataLengthTotal = lenRx;
/* verify length except for domain data type */
@ -1030,7 +1074,7 @@ int8_t CO_SDO_process(
uint16_t crc;
SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->bufferOffset, SDO->crc);
CO_memcpySwap2((uint8_t*)&crc, &SDO->CANrxData[1]);
CO_memcpySwap2(&crc, &SDO->CANrxData[1]);
if(SDO->crc != crc){
CO_SDO_abort(SDO, CO_SDO_AB_CRC); /* CRC error (block mode only). */
@ -1079,7 +1123,7 @@ int8_t CO_SDO_process(
/* indicate data size, if known */
if(SDO->ODF_arg.dataLengthTotal != 0U){
uint32_t len = SDO->ODF_arg.dataLengthTotal;
CO_memcpySwap4(&SDO->CANtxBuff->data[4], (uint8_t*)&len);
CO_memcpySwap4(&SDO->CANtxBuff->data[4], &len);
SDO->CANtxBuff->data[0] = 0x41U;
}
else{
@ -1191,7 +1235,7 @@ int8_t CO_SDO_process(
/* indicate data size, if known */
if(SDO->ODF_arg.dataLengthTotal != 0U){
uint32_t len = SDO->ODF_arg.dataLengthTotal;
CO_memcpySwap4(&SDO->CANtxBuff->data[4], (uint8_t*)&len);
CO_memcpySwap4(&SDO->CANtxBuff->data[4], &len);
SDO->CANtxBuff->data[0] = 0xC6U;
}
else{
@ -1246,7 +1290,7 @@ int8_t CO_SDO_process(
/* CRC */
if(SDO->crcEnabled)
CO_memcpySwap2(&SDO->CANtxBuff->data[1], (uint8_t*)&SDO->crc);
CO_memcpySwap2(&SDO->CANtxBuff->data[1], &SDO->crc);
SDO->state = CO_SDO_ST_UPLOAD_BL_END;

16
stack/CO_SDO.h
View file

@ -671,17 +671,27 @@ void CO_setUint32(uint8_t data[], const uint32_t value);
* @param dest Destination location.
* @param src Source location.
*/
void CO_memcpySwap2(uint8_t dest[], const uint8_t src[]);
void CO_memcpySwap2(void* dest, const void* src);
/**
* Copy 2 data bytes from source to destination. Swap bytes if
* Copy 4 data bytes from source to destination. Swap bytes if
* microcontroller is big-endian.
*
* @param dest Destination location.
* @param src Source location.
*/
void CO_memcpySwap4(uint8_t dest[], const uint8_t src[]);
void CO_memcpySwap4(void* dest, const void* src);
/**
* Copy 8 data bytes from source to destination. Swap bytes if
* microcontroller is big-endian.
*
* @param dest Destination location.
* @param src Source location.
*/
void CO_memcpySwap8(void* dest, const void* src);
/**

105
stack/CO_SDOmaster.c
View file

@ -200,6 +200,8 @@ CO_ReturnError_t CO_SDOclient_init(
SDO_C->CANdevTx = CANdevTx;
SDO_C->CANdevTxIdx = CANdevTxIdx;
SDO_C->COB_IDClientToServerPrev = 0;
SDO_C->COB_IDServerToClientPrev = 0;
CO_SDOclient_setup(SDO_C, 0, 0, 0);
return CO_ERROR_NO;
@ -224,51 +226,66 @@ CO_SDOclient_return_t CO_SDOclient_setup(
uint32_t COB_IDServerToClient,
uint8_t nodeIDOfTheSDOServer)
{
uint32_t idCtoS, idStoC;
uint8_t idNode;
/* verify parameters */
if((COB_IDClientToServer&0x7FFFF800L) || (COB_IDServerToClient&0x7FFFF800L) ||
nodeIDOfTheSDOServer > 127) return CO_SDOcli_wrongArguments;
if(SDO_C == NULL || (COB_IDClientToServer&0x7FFFF800L) != 0 ||
(COB_IDServerToClient&0x7FFFF800L) != 0 || nodeIDOfTheSDOServer > 127)
{
return CO_SDOcli_wrongArguments;
}
/* Configure object variables */
SDO_C->state = SDO_STATE_NOTDEFINED;
SDO_C->CANrxNew = false;
/* setup Object Dictionary variables */
if((COB_IDClientToServer & 0x80000000L) || (COB_IDServerToClient & 0x80000000L) || nodeIDOfTheSDOServer == 0){
if((COB_IDClientToServer & 0x80000000L) != 0 || (COB_IDServerToClient & 0x80000000L) != 0 || nodeIDOfTheSDOServer == 0){
/* SDO is NOT used */
SDO_C->SDOClientPar->COB_IDClientToServer = 0x80000000L;
SDO_C->SDOClientPar->COB_IDServerToClient = 0x80000000L;
SDO_C->SDOClientPar->nodeIDOfTheSDOServer = 0;
idCtoS = 0x80000000L;
idStoC = 0x80000000L;
idNode = 0;
}
else{
if(COB_IDClientToServer == 0 || COB_IDServerToClient == 0){
SDO_C->SDOClientPar->COB_IDClientToServer = 0x600 + nodeIDOfTheSDOServer;
SDO_C->SDOClientPar->COB_IDServerToClient = 0x580 + nodeIDOfTheSDOServer;
idCtoS = 0x600 + nodeIDOfTheSDOServer;
idStoC = 0x580 + nodeIDOfTheSDOServer;
}
else{
SDO_C->SDOClientPar->COB_IDClientToServer = COB_IDClientToServer;
SDO_C->SDOClientPar->COB_IDServerToClient = COB_IDServerToClient;
idCtoS = COB_IDClientToServer;
idStoC = COB_IDServerToClient;
}
SDO_C->SDOClientPar->nodeIDOfTheSDOServer = nodeIDOfTheSDOServer;
idNode = nodeIDOfTheSDOServer;
}
/* configure SDO client CAN reception */
CO_CANrxBufferInit(
SDO_C->CANdevRx, /* CAN device */
SDO_C->CANdevRxIdx, /* rx buffer index */
(uint16_t)SDO_C->SDOClientPar->COB_IDServerToClient,/* CAN identifier */
0x7FF, /* mask */
0, /* rtr */
(void*)SDO_C, /* object passed to receive function */
CO_SDOclient_receive); /* this function will process received message */
SDO_C->SDOClientPar->COB_IDClientToServer = idCtoS;
SDO_C->SDOClientPar->COB_IDServerToClient = idStoC;
SDO_C->SDOClientPar->nodeIDOfTheSDOServer = idNode;
/* configure SDO client CAN transmission */
SDO_C->CANtxBuff = CO_CANtxBufferInit(
SDO_C->CANdevTx, /* CAN device */
SDO_C->CANdevTxIdx, /* index of specific buffer inside CAN module */
(uint16_t)SDO_C->SDOClientPar->COB_IDClientToServer,/* CAN identifier */
0, /* rtr */
8, /* number of data bytes */
0); /* synchronous message flag bit */
/* configure SDO client CAN reception, if differs. */
if(SDO_C->COB_IDClientToServerPrev != idCtoS || SDO_C->COB_IDServerToClientPrev != idStoC) {
CO_CANrxBufferInit(
SDO_C->CANdevRx, /* CAN device */
SDO_C->CANdevRxIdx, /* rx buffer index */
(uint16_t)idStoC, /* CAN identifier */
0x7FF, /* mask */
0, /* rtr */
(void*)SDO_C, /* object passed to receive function */
CO_SDOclient_receive); /* this function will process received message */
/* configure SDO client CAN transmission */
SDO_C->CANtxBuff = CO_CANtxBufferInit(
SDO_C->CANdevTx, /* CAN device */
SDO_C->CANdevTxIdx, /* index of specific buffer inside CAN module */
(uint16_t)idCtoS, /* CAN identifier */
0, /* rtr */
8, /* number of data bytes */
0); /* synchronous message flag bit */
SDO_C->COB_IDClientToServerPrev = idCtoS;
SDO_C->COB_IDServerToClientPrev = idStoC;
}
return CO_SDOcli_ok_communicationEnd;
}
@ -280,7 +297,7 @@ static void CO_SDOclient_abort(CO_SDOclient_t *SDO_C, uint32_t code){
SDO_C->CANtxBuff->data[1] = SDO_C->index & 0xFF;
SDO_C->CANtxBuff->data[2] = (SDO_C->index>>8) & 0xFF;
SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;
CO_memcpySwap4(&SDO_C->CANtxBuff->data[4], (uint8_t*)&code);
CO_memcpySwap4(&SDO_C->CANtxBuff->data[4], &code);
CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
SDO_C->state = SDO_STATE_NOTDEFINED;
SDO_C->CANrxNew = false;
@ -313,7 +330,9 @@ CO_SDOclient_return_t CO_SDOclientDownloadInitiate(
uint8_t blockEnable)
{
/* verify parameters */
if(dataTx == 0 || dataSize == 0) return CO_SDOcli_wrongArguments;
if(SDO_C == NULL || dataTx == 0 || dataSize == 0) {
return CO_SDOcli_wrongArguments;
}
/* save parameters */
SDO_C->buffer = dataTx;
@ -366,7 +385,7 @@ CO_SDOclient_return_t CO_SDOclientDownloadInitiate(
/* segmented transfer */
SDO_C->CANtxBuff->data[0] = 0x21;
len = dataSize;
CO_memcpySwap4(&SDO_C->CANtxBuff->data[4], (uint8_t*)&len);
CO_memcpySwap4(&SDO_C->CANtxBuff->data[4], &len);
}
/* empty receive buffer, reset timeout timer and send message */
@ -387,6 +406,11 @@ CO_SDOclient_return_t CO_SDOclientDownload(
{
CO_SDOclient_return_t ret = CO_SDOcli_waitingServerResponse;
/* verify parameters */
if(SDO_C == NULL) {
return CO_SDOcli_wrongArguments;
}
/* clear abort code */
*pSDOabortCode = CO_SDO_AB_NONE;
@ -426,7 +450,7 @@ CO_SDOclient_return_t CO_SDOclientDownload(
/* ABORT */
if (SDO_C->CANrxData[0] == (SCS_ABORT<<5)){
SDO_C->state = SDO_STATE_NOTDEFINED;
CO_memcpySwap4((uint8_t*)pSDOabortCode , &SDO_C->CANrxData[4]);
CO_memcpySwap4(pSDOabortCode , &SDO_C->CANrxData[4]);
SDO_C->CANrxNew = false;
return CO_SDOcli_endedWithServerAbort;
}
@ -713,7 +737,7 @@ CO_SDOclient_return_t CO_SDOclientUploadInitiate(
uint8_t blockEnable)
{
/* verify parameters */
if(dataRx == 0 || dataRxSize < 4) {
if(SDO_C == NULL || dataRx == 0 || dataRxSize < 4) {
return CO_SDOcli_wrongArguments;
}
@ -785,6 +809,11 @@ CO_SDOclient_return_t CO_SDOclientUpload(
uint32_t tmp32;
CO_SDOclient_return_t ret = CO_SDOcli_waitingServerResponse;
/* verify parameters */
if(SDO_C == NULL) {
return CO_SDOcli_wrongArguments;
}
/* clear abort code */
*pSDOabortCode = CO_SDO_AB_NONE;
@ -837,7 +866,7 @@ CO_SDOclient_return_t CO_SDOclientUpload(
if (SDO_C->CANrxData[0] == (SCS_ABORT<<5)){
SDO_C->state = SDO_STATE_NOTDEFINED;
SDO_C->CANrxNew = false;
CO_memcpySwap4((uint8_t*)pSDOabortCode , &SDO_C->CANrxData[4]);
CO_memcpySwap4(pSDOabortCode , &SDO_C->CANrxData[4]);
return CO_SDOcli_endedWithServerAbort;
}
switch (SDO_C->state){
@ -942,7 +971,7 @@ CO_SDOclient_return_t CO_SDOclientUpload(
/* set length */
if(SDO_C->CANrxData[0]&0x02){
uint32_t len;
CO_memcpySwap4((uint8_t*)&len, &SDO_C->CANrxData[4]);
CO_memcpySwap4(&len, &SDO_C->CANrxData[4]);
SDO_C->dataSize = len;
}
else{
@ -1030,7 +1059,7 @@ CO_SDOclient_return_t CO_SDOclientUpload(
SDO_C->state = SDO_STATE_BLOCKUPLOAD_BLOCK_END;
if (SDO_C->crcEnabled){
uint16_t tmp16;
CO_memcpySwap2((uint8_t*)&tmp16, &SDO_C->CANrxData[1]);
CO_memcpySwap2(&tmp16, &SDO_C->CANrxData[1]);
if (tmp16 != crc16_ccitt((unsigned char *)SDO_C->buffer, (unsigned int)SDO_C->dataSizeTransfered, 0)){
*pSDOabortCode = CO_SDO_AB_CRC;
@ -1196,5 +1225,7 @@ CO_SDOclient_return_t CO_SDOclientUpload(
/******************************************************************************/
void CO_SDOclientClose(CO_SDOclient_t *SDO_C){
SDO_C->state = SDO_STATE_NOTDEFINED;
if(SDO_C != NULL) {
SDO_C->state = SDO_STATE_NOTDEFINED;
}
}

8
stack/CO_SDOmaster.h
View file

@ -151,6 +151,10 @@ typedef struct{
uint8_t block_noData;
/** Server CRC support in block transfer */
uint8_t crcEnabled;
/** Previous value of the COB_IDClientToServer */
uint32_t COB_IDClientToServerPrev;
/** Previous value of the COB_IDServerToClient */
uint32_t COB_IDServerToClientPrev;
}CO_SDOclient_t;
@ -281,8 +285,8 @@ CO_SDOclient_return_t CO_SDOclientDownload(
* @param SDO_C This object.
* @param index Index of object in object dictionary in remote node.
* @param subIndex Subindex of object in object dictionary in remote node.
* @param dataRx Pointer to data buffer data will be written. Buffer must be
* valid until end of communication. Note that data are aligned
* @param dataRx Pointer to data buffer, into which received data will be written.
* Buffer must be valid until end of communication. Note that data are aligned
* in little-endian format, because CANopen itself uses
* little-endian. Take care, when using processors with big-endian.
* @param dataRxSize Size of dataRx.

2
stack/socketCAN/CO_driver.h
View file

@ -157,11 +157,13 @@ typedef struct{
/* Endianes */
#ifdef BYTE_ORDER
#if BYTE_ORDER == LITTLE_ENDIAN
#define CO_LITTLE_ENDIAN
#else
#define CO_BIG_ENDIAN
#endif
#endif
/* Helper function, must be defined externally. */