1803 lines
66 KiB
C
1803 lines
66 KiB
C
/*
|
|
* CANopen Service Data Object - server.
|
|
*
|
|
* @file CO_SDOserver.c
|
|
* @ingroup CO_SDOserver
|
|
* @author Janez Paternoster
|
|
* @copyright 2020 Janez Paternoster
|
|
*
|
|
* This file is part of <https://github.com/CANopenNode/CANopenNode>, a CANopen Stack.
|
|
*
|
|
* 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
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software distributed under the License is
|
|
* distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and limitations under the License.
|
|
*/
|
|
|
|
#include <string.h>
|
|
|
|
#include "301/CO_SDOserver.h"
|
|
#include "301/crc16-ccitt.h"
|
|
|
|
/* verify configuration */
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
#if CO_CONFIG_SDO_SRV_BUFFER_SIZE < 20
|
|
#error CO_CONFIG_SDO_SRV_BUFFER_SIZE must be greater or equal than 20.
|
|
#endif
|
|
#endif
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) == 0
|
|
#error CO_CONFIG_SDO_SRV_SEGMENTED must be enabled.
|
|
#endif
|
|
#if ((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE) == 0
|
|
#error CO_CONFIG_CRC16_ENABLE must be enabled.
|
|
#endif
|
|
#if CO_CONFIG_SDO_SRV_BUFFER_SIZE < 900
|
|
#error CO_CONFIG_SDO_SRV_BUFFER_SIZE must be greater or equal than 900.
|
|
#endif
|
|
#endif
|
|
|
|
/*
|
|
* Read received message from CAN module.
|
|
*
|
|
* Function will be called (by CAN receive interrupt) every time, when CAN message with correct identifier
|
|
* will be received. For more information and description of parameters see file CO_driver.h.
|
|
*/
|
|
static void
|
|
CO_SDO_receive(void *object, void *msg)
|
|
{
|
|
CO_SDOserver_t *SDO = (CO_SDOserver_t *)object;
|
|
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
|
|
const uint8_t *data = CO_CANrxMsg_readData(msg);
|
|
|
|
/* ignore messages with wrong length */
|
|
if (DLC == 8U)
|
|
{
|
|
if (data[0] == 0x80U)
|
|
{
|
|
/* abort from client, just make idle */
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
}
|
|
else if (CO_FLAG_READ(SDO->CANrxNew))
|
|
{
|
|
/* ignore message if previous message was not processed yet */
|
|
}
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
else if (SDO->state == CO_SDO_ST_UPLOAD_BLK_END_CRSP && data[0] == 0xA1)
|
|
{
|
|
/* SDO block download successfully transferred, just make idle */
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
}
|
|
else if (SDO->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ)
|
|
{
|
|
/* just in case, condition should always pass */
|
|
if (SDO->bufOffsetWr <= (CO_CONFIG_SDO_SRV_BUFFER_SIZE - (7 + 2)))
|
|
{
|
|
/* block download, copy data directly */
|
|
CO_SDO_state_t state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
|
|
uint8_t seqno = data[0] & 0x7F;
|
|
SDO->timeoutTimer = 0;
|
|
SDO->block_timeoutTimer = 0;
|
|
|
|
/* verify if sequence number is correct */
|
|
if (seqno <= SDO->block_blksize && seqno == (SDO->block_seqno + 1))
|
|
{
|
|
SDO->block_seqno = seqno;
|
|
|
|
/* Copy data. There is always enough space in buffer,
|
|
* because block_blksize was calculated before */
|
|
(void)memcpy(SDO->buf + SDO->bufOffsetWr, &data[1], 7);
|
|
SDO->bufOffsetWr += 7;
|
|
SDO->sizeTran += 7;
|
|
|
|
/* is this the last segment? */
|
|
if ((data[0] & 0x80) != 0)
|
|
{
|
|
SDO->finished = true;
|
|
state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
|
|
}
|
|
else if (seqno == SDO->block_blksize)
|
|
{
|
|
/* all segments in sub-block has been transferred */
|
|
state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
|
|
}
|
|
}
|
|
/* If message is duplicate or sequence didn't start yet, ignore it. Otherwise seqno is wrong,
|
|
* so break sub-block. Data after last good seqno will be re-transmitted. */
|
|
else if (seqno != SDO->block_seqno && SDO->block_seqno != 0U)
|
|
{
|
|
state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
|
|
#ifdef CO_DEBUG_SDO_SERVER
|
|
char msg[80];
|
|
sprintf(msg, "sub-block, rx WRONG: sequno=%02X, previous=%02X", seqno, SDO->block_seqno);
|
|
CO_DEBUG_SDO_SERVER(msg);
|
|
#endif
|
|
}
|
|
#ifdef CO_DEBUG_SDO_SERVER
|
|
else
|
|
{
|
|
char msg[80];
|
|
sprintf(msg, "sub-block, rx ignored: sequno=%02X, expected=%02X", seqno, SDO->block_seqno + 1);
|
|
CO_DEBUG_SDO_SERVER(msg);
|
|
}
|
|
#endif
|
|
|
|
if (state != CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ)
|
|
{
|
|
/* SDO->state has changed, processing will continue in another thread.
|
|
* Make memory barrier here with CO_FLAG_CLEAR() call. */
|
|
CO_FLAG_CLEAR(SDO->CANrxNew);
|
|
SDO->state = state;
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE) != 0
|
|
/* Optional signal to RTOS, which can resume task, which handles SDO server processing. */
|
|
if (SDO->pFunctSignalPre != NULL)
|
|
{
|
|
SDO->pFunctSignalPre(SDO->functSignalObjectPre);
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
else if (SDO->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP)
|
|
{
|
|
/* ignore subsequent server messages, if response was requested */
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK */
|
|
else
|
|
{
|
|
/* copy data and set 'new message' flag, data will be processed in CO_SDOserver_process() */
|
|
(void)memcpy(SDO->CANrxData, data, DLC);
|
|
CO_FLAG_SET(SDO->CANrxNew);
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE) != 0
|
|
/* Optional signal to RTOS, which can resume task, which handles SDO server processing. */
|
|
if (SDO->pFunctSignalPre != NULL)
|
|
{
|
|
SDO->pFunctSignalPre(SDO->functSignalObjectPre);
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
|
|
/* helper for configuring CANrx and CANtx */
|
|
static CO_ReturnError_t
|
|
CO_SDOserver_init_canRxTx(CO_SDOserver_t *SDO, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, uint16_t CANdevTxIdx,
|
|
uint32_t COB_IDClientToServer, uint32_t COB_IDServerToClient)
|
|
{
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_OD_DYNAMIC) != 0
|
|
/* proceed only, if parameters change */
|
|
if ((COB_IDClientToServer == SDO->COB_IDClientToServer) && (COB_IDServerToClient == SDO->COB_IDServerToClient))
|
|
{
|
|
return CO_ERROR_NO;
|
|
}
|
|
/* store variables */
|
|
SDO->COB_IDClientToServer = COB_IDClientToServer;
|
|
SDO->COB_IDServerToClient = COB_IDServerToClient;
|
|
#endif
|
|
|
|
/* verify valid bit */
|
|
uint16_t idC2S = ((COB_IDClientToServer & 0x80000000UL) == 0U) ? (uint16_t)COB_IDClientToServer : 0U;
|
|
uint16_t idS2C = ((COB_IDServerToClient & 0x80000000UL) == 0U) ? (uint16_t)COB_IDServerToClient : 0U;
|
|
if ((idC2S != 0U) && (idS2C != 0U))
|
|
{
|
|
SDO->valid = true;
|
|
}
|
|
else
|
|
{
|
|
idC2S = 0;
|
|
idS2C = 0;
|
|
SDO->valid = false;
|
|
}
|
|
|
|
/* configure SDO server CAN reception */
|
|
CO_ReturnError_t ret = CO_CANrxBufferInit(CANdevRx, CANdevRxIdx, idC2S, 0x7FF, false, (void *)SDO, CO_SDO_receive);
|
|
|
|
/* configure SDO server CAN transmission */
|
|
SDO->CANtxBuff = CO_CANtxBufferInit(SDO->CANdevTx, CANdevTxIdx, idS2C, false, 8, false);
|
|
|
|
if (SDO->CANtxBuff == NULL)
|
|
{
|
|
ret = CO_ERROR_ILLEGAL_ARGUMENT;
|
|
SDO->valid = false;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_OD_DYNAMIC) != 0
|
|
/*
|
|
* Custom function for writing OD object _SDO server parameter_, additional channels
|
|
*
|
|
* For more information see file CO_ODinterface.h, OD_IO_t.
|
|
*/
|
|
static ODR_t
|
|
OD_write_1201_additional(OD_stream_t *stream, const void *buf, OD_size_t count, OD_size_t *countWritten)
|
|
{
|
|
/* "count" is already verified in *_init() function */
|
|
if ((stream == NULL) || (buf == NULL) || (countWritten == NULL))
|
|
{
|
|
return ODR_DEV_INCOMPAT;
|
|
}
|
|
|
|
CO_SDOserver_t *SDO = (CO_SDOserver_t *)stream->object;
|
|
|
|
switch (stream->subIndex)
|
|
{
|
|
case 0: /* Highest sub-index supported */
|
|
return ODR_READONLY;
|
|
|
|
case 1:
|
|
{ /* COB-ID client -> server */
|
|
uint32_t COB_ID = CO_getUint32(buf);
|
|
uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
|
|
uint16_t CAN_ID_cur = (uint16_t)(SDO->COB_IDClientToServer & 0x7FFU);
|
|
bool_t valid = (COB_ID & 0x80000000U) == 0U;
|
|
|
|
/* SDO client must not be valid when changing COB_ID */
|
|
if (((COB_ID & 0x3FFFF800U) != 0U) || ((valid && SDO->valid) && (CAN_ID != CAN_ID_cur)) || (valid && CO_IS_RESTRICTED_CAN_ID(CAN_ID)))
|
|
{
|
|
return ODR_INVALID_VALUE;
|
|
}
|
|
(void)CO_SDOserver_init_canRxTx(SDO, SDO->CANdevRx, SDO->CANdevRxIdx, SDO->CANdevTxIdx, COB_ID,
|
|
SDO->COB_IDServerToClient);
|
|
break;
|
|
}
|
|
|
|
case 2:
|
|
{ /* COB-ID server -> client */
|
|
uint32_t COB_ID = CO_getUint32(buf);
|
|
uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
|
|
uint16_t CAN_ID_cur = (uint16_t)(SDO->COB_IDServerToClient & 0x7FFU);
|
|
bool_t valid = (COB_ID & 0x80000000U) == 0U;
|
|
|
|
/* SDO client must not be valid when changing COB_ID */
|
|
if (((COB_ID & 0x3FFFF800U) != 0U) || (valid && (SDO->valid && (CAN_ID != CAN_ID_cur))) || (valid && CO_IS_RESTRICTED_CAN_ID(CAN_ID)))
|
|
{
|
|
return ODR_INVALID_VALUE;
|
|
}
|
|
(void)CO_SDOserver_init_canRxTx(SDO, SDO->CANdevRx, SDO->CANdevRxIdx, SDO->CANdevTxIdx,
|
|
SDO->COB_IDClientToServer, COB_ID);
|
|
break;
|
|
}
|
|
|
|
case 3:
|
|
{ /* Node-ID of the SDO server */
|
|
if (count != 1U)
|
|
{
|
|
return ODR_TYPE_MISMATCH;
|
|
}
|
|
uint8_t nodeId = CO_getUint8(buf);
|
|
if ((nodeId < 1U) || (nodeId > 127U))
|
|
{
|
|
return ODR_INVALID_VALUE;
|
|
}
|
|
break;
|
|
}
|
|
|
|
default:
|
|
return ODR_SUB_NOT_EXIST;
|
|
}
|
|
|
|
/* write value to the original location in the Object Dictionary */
|
|
return OD_writeOriginal(stream, buf, count, countWritten);
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_OD_DYNAMIC */
|
|
|
|
CO_ReturnError_t
|
|
CO_SDOserver_init(CO_SDOserver_t *SDO, OD_t *OD, OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId,
|
|
uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx,
|
|
uint16_t CANdevTxIdx, uint32_t *errInfo)
|
|
{
|
|
/* verify arguments */
|
|
if ((SDO == NULL) || (OD == NULL) || (CANdevRx == NULL) || (CANdevTx == NULL))
|
|
{
|
|
return CO_ERROR_ILLEGAL_ARGUMENT;
|
|
}
|
|
|
|
/* Configure object variables */
|
|
SDO->OD = OD;
|
|
SDO->nodeId = nodeId;
|
|
#if (((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED)) != 0
|
|
SDO->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000U;
|
|
#endif
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
SDO->block_SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 700;
|
|
#endif
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE) != 0
|
|
SDO->pFunctSignalPre = NULL;
|
|
SDO->functSignalObjectPre = NULL;
|
|
#endif
|
|
|
|
/* configure CAN identifiers and SDO server parameters if available */
|
|
uint16_t CanId_ClientToServer, CanId_ServerToClient;
|
|
|
|
if (OD_1200_SDOsrvPar == NULL)
|
|
{
|
|
/* configure default SDO channel */
|
|
if ((nodeId < 1U) || (nodeId > 127U))
|
|
{
|
|
return CO_ERROR_ILLEGAL_ARGUMENT;
|
|
}
|
|
|
|
CanId_ClientToServer = CO_CAN_ID_SDO_CLI + nodeId;
|
|
CanId_ServerToClient = CO_CAN_ID_SDO_SRV + nodeId;
|
|
SDO->valid = true;
|
|
}
|
|
else
|
|
{
|
|
uint16_t OD_SDOsrvParIdx = OD_getIndex(OD_1200_SDOsrvPar);
|
|
|
|
if (OD_SDOsrvParIdx == (uint16_t)OD_H1200_SDO_SERVER_1_PARAM)
|
|
{
|
|
/* configure default SDO channel and SDO server parameters for it */
|
|
if ((nodeId < 1U) || (nodeId > 127U))
|
|
{
|
|
return CO_ERROR_ILLEGAL_ARGUMENT;
|
|
}
|
|
|
|
CanId_ClientToServer = CO_CAN_ID_SDO_CLI + nodeId;
|
|
CanId_ServerToClient = CO_CAN_ID_SDO_SRV + nodeId;
|
|
SDO->valid = true;
|
|
|
|
(void)OD_set_u32(OD_1200_SDOsrvPar, 1, CanId_ClientToServer, true);
|
|
(void)OD_set_u32(OD_1200_SDOsrvPar, 2, CanId_ServerToClient, true);
|
|
}
|
|
else if ((OD_SDOsrvParIdx > (uint16_t)OD_H1200_SDO_SERVER_1_PARAM) && (OD_SDOsrvParIdx <= ((uint16_t)OD_H1200_SDO_SERVER_1_PARAM + 0x7FU)))
|
|
{
|
|
/* configure additional SDO channel and SDO server parameters for it */
|
|
uint8_t maxSubIndex;
|
|
uint32_t COB_IDClientToServer32, COB_IDServerToClient32;
|
|
|
|
/* get and verify parameters from Object Dictionary (initial values) */
|
|
ODR_t odRet0 = OD_get_u8(OD_1200_SDOsrvPar, 0, &maxSubIndex, true);
|
|
ODR_t odRet1 = OD_get_u32(OD_1200_SDOsrvPar, 1, &COB_IDClientToServer32, true);
|
|
ODR_t odRet2 = OD_get_u32(OD_1200_SDOsrvPar, 2, &COB_IDServerToClient32, true);
|
|
|
|
if ((odRet0 != ODR_OK) || ((maxSubIndex != 2U) && (maxSubIndex != 3U)) || (odRet1 != ODR_OK) || (odRet2 != ODR_OK))
|
|
{
|
|
if (errInfo != NULL)
|
|
{
|
|
*errInfo = OD_SDOsrvParIdx;
|
|
}
|
|
return CO_ERROR_OD_PARAMETERS;
|
|
}
|
|
|
|
CanId_ClientToServer = ((COB_IDClientToServer32 & 0x80000000U) == 0U)
|
|
? (uint16_t)(COB_IDClientToServer32 & 0x7FFU)
|
|
: 0U;
|
|
CanId_ServerToClient = ((COB_IDServerToClient32 & 0x80000000U) == 0U)
|
|
? (uint16_t)(COB_IDServerToClient32 & 0x7FFU)
|
|
: 0U;
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_OD_DYNAMIC) != 0
|
|
SDO->OD_1200_extension.object = SDO;
|
|
SDO->OD_1200_extension.read = OD_readOriginal;
|
|
SDO->OD_1200_extension.write = OD_write_1201_additional;
|
|
ODR_t odRetE = OD_extension_init(OD_1200_SDOsrvPar, &SDO->OD_1200_extension);
|
|
if (odRetE != ODR_OK)
|
|
{
|
|
if (errInfo != NULL)
|
|
{
|
|
*errInfo = OD_SDOsrvParIdx;
|
|
}
|
|
return CO_ERROR_OD_PARAMETERS;
|
|
}
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
return CO_ERROR_ILLEGAL_ARGUMENT;
|
|
}
|
|
}
|
|
CO_FLAG_CLEAR(SDO->CANrxNew);
|
|
|
|
/* store the parameters and configure CANrx and CANtx */
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_OD_DYNAMIC) != 0
|
|
SDO->CANdevRx = CANdevRx;
|
|
SDO->CANdevRxIdx = CANdevRxIdx;
|
|
SDO->CANdevTxIdx = CANdevTxIdx;
|
|
/* set to zero to make sure CO_SDOserver_init_canRxTx() will reconfig CAN */
|
|
SDO->COB_IDClientToServer = 0;
|
|
SDO->COB_IDServerToClient = 0;
|
|
#endif
|
|
SDO->CANdevTx = CANdevTx;
|
|
|
|
return CO_SDOserver_init_canRxTx(SDO, CANdevRx, CANdevRxIdx, CANdevTxIdx, CanId_ClientToServer,
|
|
CanId_ServerToClient);
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE) != 0
|
|
void CO_SDOserver_initCallbackPre(CO_SDOserver_t *SDO, void *object, void (*pFunctSignalPre)(void *object))
|
|
{
|
|
if (SDO != NULL)
|
|
{
|
|
SDO->functSignalObjectPre = object;
|
|
SDO->pFunctSignalPre = pFunctSignalPre;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#ifdef CO_BIG_ENDIAN
|
|
static inline void
|
|
reverseBytes(void *start, OD_size_t size)
|
|
{
|
|
uint8_t *lo = (uint8_t *)start;
|
|
uint8_t *hi = (uint8_t *)start + size - 1;
|
|
while (lo < hi)
|
|
{
|
|
uint8_t swap = *lo;
|
|
*lo++ = *hi;
|
|
*hi-- = swap;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
/* Helper function for writing data to Object dictionary. Function swaps data if necessary,
|
|
* calcualtes (and verifies CRC) writes data to OD and verifies data lengths.
|
|
*
|
|
* @param SDO SDO server
|
|
* @param [out] abortCode SDO abort code in case of error
|
|
* @param crcOperation 0=none, 1=calculate, 2=calculate and compare
|
|
* @parma crcClient crc checksum to campare with
|
|
*
|
|
* Returns true on success, otherwise write also abortCode and sets state to CO_SDO_ST_ABORT */
|
|
static bool_t
|
|
validateAndWriteToOD(CO_SDOserver_t *SDO, CO_SDO_abortCode_t *abortCode, uint8_t crcOperation, uint16_t crcClient)
|
|
{
|
|
OD_size_t bufOffsetWrOrig = SDO->bufOffsetWr;
|
|
|
|
if (SDO->finished)
|
|
{
|
|
/* Verify if size of data downloaded matches size indicated. */
|
|
if ((SDO->sizeInd > 0U) && (SDO->sizeTran != SDO->sizeInd))
|
|
{
|
|
*abortCode = (SDO->sizeTran > SDO->sizeInd) ? CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
|
|
#ifdef CO_BIG_ENDIAN
|
|
/* swap int16_t .. uint64_t data if necessary */
|
|
if ((SDO->OD_IO.stream.attribute & ODA_MB) != 0)
|
|
{
|
|
reverseBytes(SDO->buf, SDO->bufOffsetWr);
|
|
}
|
|
#endif
|
|
|
|
OD_size_t sizeInOd = SDO->OD_IO.stream.dataLength;
|
|
|
|
/* If dataType is string, then size of data downloaded may be shorter than size of the
|
|
* OD data buffer. If so, add two zero bytes to terminate (unicode) string. Shorten
|
|
* also OD data size, (temporary, send information about EOF into OD_IO.write) */
|
|
if (((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_STR) != 0U) && ((sizeInOd == 0U) || (SDO->sizeTran < sizeInOd)) && ((SDO->bufOffsetWr + 2U) <= CO_CONFIG_SDO_SRV_BUFFER_SIZE))
|
|
{
|
|
SDO->buf[SDO->bufOffsetWr] = 0;
|
|
SDO->bufOffsetWr++;
|
|
SDO->sizeTran++;
|
|
if ((sizeInOd == 0U) || (SDO->sizeTran < sizeInOd))
|
|
{
|
|
SDO->buf[SDO->bufOffsetWr] = 0;
|
|
SDO->bufOffsetWr++;
|
|
SDO->sizeTran++;
|
|
}
|
|
SDO->OD_IO.stream.dataLength = SDO->sizeTran;
|
|
}
|
|
/* Indicate OD data size, if not indicated. Can be used for EOF check. */
|
|
else if (sizeInOd == 0U)
|
|
{
|
|
SDO->OD_IO.stream.dataLength = SDO->sizeTran;
|
|
}
|
|
/* Verify if size of data downloaded matches data size in OD. */
|
|
else if (SDO->sizeTran != sizeInOd)
|
|
{
|
|
*abortCode = (SDO->sizeTran > sizeInOd) ? CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* Verify if size of data downloaded is not too large. */
|
|
if ((SDO->sizeInd > 0U) && (SDO->sizeTran > SDO->sizeInd))
|
|
{
|
|
*abortCode = CO_SDO_AB_DATA_LONG;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
/* calculate crc on current data */
|
|
if (SDO->block_crcEnabled && crcOperation > 0)
|
|
{
|
|
SDO->block_crc = crc16_ccitt(SDO->buf, bufOffsetWrOrig, SDO->block_crc);
|
|
if (crcOperation == 2 && crcClient != SDO->block_crc)
|
|
{
|
|
*abortCode = CO_SDO_AB_CRC;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
}
|
|
#endif
|
|
/* may be unused */
|
|
(void)crcOperation;
|
|
(void)crcClient;
|
|
(void)bufOffsetWrOrig;
|
|
|
|
/* write data */
|
|
OD_size_t countWritten = 0;
|
|
ODR_t odRet;
|
|
|
|
CO_LOCK_OD(SDO->CANdevTx);
|
|
odRet = SDO->OD_IO.write(&SDO->OD_IO.stream, SDO->buf, SDO->bufOffsetWr, &countWritten);
|
|
CO_UNLOCK_OD(SDO->CANdevTx);
|
|
|
|
SDO->bufOffsetWr = 0;
|
|
|
|
/* verify write error value */
|
|
if ((odRet != ODR_OK) && (odRet != ODR_PARTIAL))
|
|
{
|
|
*abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
else if (SDO->finished && (odRet == ODR_PARTIAL))
|
|
{
|
|
/* OD variable was not written completely, but SDO download finished */
|
|
*abortCode = CO_SDO_AB_DATA_SHORT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
else if (!SDO->finished && (odRet == ODR_OK))
|
|
{
|
|
/* OD variable was written completely, but SDO download still has data */
|
|
*abortCode = CO_SDO_AB_DATA_LONG;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Helper function for reading data from Object dictionary. Function also swaps data if necessary and calcualtes CRC.
|
|
*
|
|
* @param SDO SDO server
|
|
* @param [out] abortCode SDO abort code in case of error
|
|
* @parma countMinimum if data size in buffer is less than countMinimum, then buffer is refilled from OD variable
|
|
* @param calculateCrc if true, crc is calculated
|
|
*
|
|
* Returns true on success, otherwise write also abortCode and sets state to CO_SDO_ST_ABORT */
|
|
static bool_t
|
|
readFromOd(CO_SDOserver_t *SDO, CO_SDO_abortCode_t *abortCode, OD_size_t countMinimum, bool_t calculateCrc)
|
|
{
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) == 0
|
|
(void)calculateCrc; /* may be unused */
|
|
#endif
|
|
OD_size_t countRemain = SDO->bufOffsetWr - SDO->bufOffsetRd;
|
|
|
|
if (!SDO->finished && (countRemain < countMinimum))
|
|
{
|
|
/* first move remaining data to the start of the buffer */
|
|
(void)memmove(SDO->buf, SDO->buf + SDO->bufOffsetRd, countRemain);
|
|
SDO->bufOffsetRd = 0;
|
|
SDO->bufOffsetWr = countRemain;
|
|
|
|
/* Get size of free data buffer */
|
|
OD_size_t countRdRequest = CO_CONFIG_SDO_SRV_BUFFER_SIZE - countRemain;
|
|
|
|
/* load data from OD variable into the buffer */
|
|
OD_size_t countRd = 0;
|
|
ODR_t odRet;
|
|
|
|
CO_LOCK_OD(SDO->CANdevTx);
|
|
odRet = SDO->OD_IO.read(&SDO->OD_IO.stream, &SDO->buf[countRemain], countRdRequest, &countRd);
|
|
CO_UNLOCK_OD(SDO->CANdevTx);
|
|
|
|
if ((odRet != ODR_OK) && (odRet != ODR_PARTIAL))
|
|
{
|
|
*abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
|
|
/* if data is string, send only data up to null termination */
|
|
OD_size_t lastRd = countRd + countRemain;
|
|
if ((countRd > 0U) && ((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_STR) != 0U))
|
|
{
|
|
SDO->buf[lastRd] = 0; /* (SDO->buf is one byte larger) */
|
|
OD_size_t countStr = (OD_size_t)strlen((char *)&SDO->buf[countRemain]);
|
|
if (countStr == 0U)
|
|
{
|
|
countStr = 1;
|
|
} /* zero length is not allowed */
|
|
if (countStr < countRd)
|
|
{
|
|
/* string terminator found, read is finished, shorten data */
|
|
countRd = countStr;
|
|
odRet = ODR_OK;
|
|
SDO->OD_IO.stream.dataLength = SDO->sizeTran + countRd;
|
|
}
|
|
}
|
|
|
|
/* partial or finished read */
|
|
SDO->bufOffsetWr = countRemain + countRd;
|
|
if ((SDO->bufOffsetWr == 0U) || (odRet == ODR_PARTIAL))
|
|
{
|
|
SDO->finished = false;
|
|
if (SDO->bufOffsetWr < countMinimum)
|
|
{
|
|
*abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SDO->finished = true;
|
|
}
|
|
|
|
#ifdef CO_BIG_ENDIAN
|
|
/* swap data if necessary */
|
|
if ((SDO->OD_IO.stream.attribute & ODA_MB) != 0)
|
|
{
|
|
if (SDO->finished)
|
|
{
|
|
/* int16_t .. uint64_t */
|
|
reverseBytes(&SDO->buf[countRemain], countRd);
|
|
}
|
|
else
|
|
{
|
|
*abortCode = CO_SDO_AB_PRAM_INCOMPAT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
return false;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
/* update the crc */
|
|
if (calculateCrc && SDO->block_crcEnabled)
|
|
{
|
|
SDO->block_crc = crc16_ccitt(&SDO->buf[countRemain], countRd, SDO->block_crc);
|
|
}
|
|
#endif
|
|
}
|
|
return true;
|
|
}
|
|
#endif
|
|
|
|
CO_SDO_return_t
|
|
CO_SDOserver_process(CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us,
|
|
uint32_t *timerNext_us)
|
|
{
|
|
if (SDO == NULL)
|
|
{
|
|
return CO_SDO_RT_wrongArguments;
|
|
}
|
|
|
|
(void)timerNext_us; /* may be unused */
|
|
|
|
CO_SDO_return_t ret = CO_SDO_RT_waitingResponse;
|
|
CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE;
|
|
bool_t isNew = CO_FLAG_READ(SDO->CANrxNew);
|
|
|
|
if ((SDO->state == CO_SDO_ST_IDLE) && SDO->valid && !isNew)
|
|
{
|
|
/* Idle and nothing new */
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
}
|
|
else if (!NMTisPreOrOperational || !SDO->valid)
|
|
{
|
|
/* SDO is allowed only in operational or pre-operational NMT state and must be valid */
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
CO_FLAG_CLEAR(SDO->CANrxNew);
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
}
|
|
/* CAN data received ******************************************************/
|
|
else if (isNew)
|
|
{
|
|
if (SDO->state == CO_SDO_ST_IDLE)
|
|
{ /* new SDO communication? */
|
|
bool_t upload = false;
|
|
|
|
if ((SDO->CANrxData[0] & 0xF0U) == 0x20U)
|
|
{
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ;
|
|
}
|
|
else if (SDO->CANrxData[0] == 0x40U)
|
|
{
|
|
upload = true;
|
|
SDO->state = CO_SDO_ST_UPLOAD_INITIATE_REQ;
|
|
}
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
else if ((SDO->CANrxData[0] & 0xF9) == 0xC0)
|
|
{
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ;
|
|
}
|
|
else if ((SDO->CANrxData[0] & 0xFB) == 0xA0)
|
|
{
|
|
upload = true;
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ;
|
|
}
|
|
#endif
|
|
else
|
|
{
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
|
|
/* if no error search object dictionary for new SDO request */
|
|
if (abortCode == CO_SDO_AB_NONE)
|
|
{
|
|
ODR_t odRet;
|
|
SDO->index = (uint16_t)((((uint16_t)SDO->CANrxData[2]) << 8) | SDO->CANrxData[1]);
|
|
SDO->subIndex = SDO->CANrxData[3];
|
|
odRet = OD_getSub(OD_find(SDO->OD, SDO->index), SDO->subIndex, &SDO->OD_IO, false);
|
|
if (odRet != ODR_OK)
|
|
{
|
|
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
else
|
|
{
|
|
/* verify read/write attributes */
|
|
if ((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_RW) == 0U)
|
|
{
|
|
abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
else if (upload && ((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_R) == 0U))
|
|
{
|
|
abortCode = CO_SDO_AB_WRITEONLY;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
else if (!upload && ((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_W) == 0U))
|
|
{
|
|
abortCode = CO_SDO_AB_READONLY;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
}
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
/* load data from object dictionary, if upload and no error */
|
|
if (upload && (abortCode == CO_SDO_AB_NONE))
|
|
{
|
|
SDO->bufOffsetRd = 0;
|
|
SDO->bufOffsetWr = 0;
|
|
SDO->sizeTran = 0;
|
|
SDO->finished = false;
|
|
|
|
if (readFromOd(SDO, &abortCode, 7, false))
|
|
{
|
|
/* Size of variable in OD (may not be known yet) */
|
|
if (SDO->finished)
|
|
{
|
|
/* OD variable was completely read, its size is known */
|
|
|
|
SDO->sizeInd = SDO->OD_IO.stream.dataLength;
|
|
|
|
if (SDO->sizeInd == 0U)
|
|
{
|
|
SDO->sizeInd = SDO->bufOffsetWr;
|
|
}
|
|
else if (SDO->sizeInd != SDO->bufOffsetWr)
|
|
{
|
|
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* If data type is string, size is not known */
|
|
SDO->sizeInd = ((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_STR) == 0U)
|
|
? SDO->OD_IO.stream.dataLength
|
|
: 0U;
|
|
}
|
|
}
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED */
|
|
} /* (SDO->state == CO_SDO_ST_IDLE) */
|
|
|
|
bool isOKstate = (SDO->state != CO_SDO_ST_IDLE);
|
|
isOKstate = (SDO->state != CO_SDO_ST_ABORT) && isOKstate;
|
|
if (isOKstate)
|
|
{
|
|
switch (SDO->state)
|
|
{
|
|
case CO_SDO_ST_DOWNLOAD_INITIATE_REQ:
|
|
{
|
|
if ((SDO->CANrxData[0] & 0x02U) != 0U)
|
|
{
|
|
/* Expedited transfer, max 4 bytes of data */
|
|
|
|
/* Size of OD variable (>0 if indicated) */
|
|
OD_size_t sizeInOd = SDO->OD_IO.stream.dataLength;
|
|
|
|
/* Get SDO data size (indicated by SDO client or get from OD) */
|
|
OD_size_t dataSizeToWrite = 4;
|
|
if ((SDO->CANrxData[0] & 0x01U) != 0U)
|
|
{
|
|
dataSizeToWrite -= ((OD_size_t)(SDO->CANrxData[0]) >> 2) & 0x03U;
|
|
}
|
|
else if ((sizeInOd > 0U) && (sizeInOd < 4U))
|
|
{
|
|
dataSizeToWrite = sizeInOd;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
|
|
/* copy data to the temp buffer, swap data if necessary */
|
|
uint8_t buf[6] = {0};
|
|
(void)memcpy(buf, &SDO->CANrxData[4], dataSizeToWrite);
|
|
#ifdef CO_BIG_ENDIAN
|
|
if ((SDO->OD_IO.stream.attribute & ODA_MB) != 0)
|
|
{
|
|
reverseBytes(buf, dataSizeToWrite);
|
|
}
|
|
#endif
|
|
|
|
/* If dataType is string, then size of data downloaded may be shorter as size of
|
|
* the OD data buffer. If so, add two zero bytes to terminate (unicode) string.
|
|
* Shorten also OD data size, (temporary, send information about EOF into OD_IO.write) */
|
|
if (((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_STR) != 0U) && ((sizeInOd == 0U) || (dataSizeToWrite < sizeInOd)))
|
|
{
|
|
OD_size_t delta = sizeInOd - dataSizeToWrite;
|
|
dataSizeToWrite += (delta == 1U) ? 1U : 2U;
|
|
SDO->OD_IO.stream.dataLength = dataSizeToWrite;
|
|
}
|
|
else if (sizeInOd == 0U)
|
|
{
|
|
SDO->OD_IO.stream.dataLength = dataSizeToWrite;
|
|
}
|
|
/* Verify if size of data downloaded matches data size in OD. */
|
|
else if (dataSizeToWrite != sizeInOd)
|
|
{
|
|
abortCode = (dataSizeToWrite > sizeInOd) ? CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
|
|
/* Copy data */
|
|
OD_size_t countWritten = 0;
|
|
ODR_t odRet;
|
|
|
|
CO_LOCK_OD(SDO->CANdevTx);
|
|
odRet = SDO->OD_IO.write(&SDO->OD_IO.stream, buf, dataSizeToWrite, &countWritten);
|
|
CO_UNLOCK_OD(SDO->CANdevTx);
|
|
|
|
if (odRet != ODR_OK)
|
|
{
|
|
abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_INITIATE_RSP;
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
SDO->finished = true;
|
|
#endif
|
|
}
|
|
}
|
|
else
|
|
{
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
/* segmented transfer, is size indicated? */
|
|
if ((SDO->CANrxData[0] & 0x01U) != 0U)
|
|
{
|
|
uint32_t size;
|
|
OD_size_t sizeInOd = SDO->OD_IO.stream.dataLength;
|
|
|
|
(void)memcpy((void *)(&size), (const void *)(&SDO->CANrxData[4]), sizeof(size));
|
|
SDO->sizeInd = CO_SWAP_32(size);
|
|
|
|
/* Indicated size of SDO matches sizeof OD variable? */
|
|
if (sizeInOd > 0U)
|
|
{
|
|
if (SDO->sizeInd > sizeInOd)
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_LONG;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
/* strings are allowed to be shorter */
|
|
else if ((SDO->sizeInd < sizeInOd) && ((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_STR) == 0U))
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_SHORT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SDO->sizeInd = 0;
|
|
}
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_INITIATE_RSP;
|
|
SDO->finished = false;
|
|
#else
|
|
abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
#endif
|
|
}
|
|
break;
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ:
|
|
{
|
|
if ((SDO->CANrxData[0] & 0xE0U) == 0x00U)
|
|
{
|
|
SDO->finished = (SDO->CANrxData[0] & 0x01U) != 0U;
|
|
|
|
/* verify and alternate toggle bit */
|
|
uint8_t toggle = SDO->CANrxData[0] & 0x10U;
|
|
if (toggle != SDO->toggle)
|
|
{
|
|
abortCode = CO_SDO_AB_TOGGLE_BIT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
|
|
/* get data size and write data to the buffer */
|
|
OD_size_t count = (OD_size_t)(7U - (((OD_size_t)(SDO->CANrxData[0]) >> 1) & 0x07U));
|
|
(void)memcpy(SDO->buf + SDO->bufOffsetWr, &SDO->CANrxData[1], count);
|
|
SDO->bufOffsetWr += count;
|
|
SDO->sizeTran += count;
|
|
|
|
/* if data size exceeds variable size, abort */
|
|
if ((SDO->OD_IO.stream.dataLength > 0U) && (SDO->sizeTran > SDO->OD_IO.stream.dataLength))
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_LONG;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
|
|
/* if necessary, empty the buffer */
|
|
if (SDO->finished || ((CO_CONFIG_SDO_SRV_BUFFER_SIZE - SDO->bufOffsetWr) < (7U + 2U)))
|
|
{
|
|
if (!validateAndWriteToOD(SDO, &abortCode, 0, 0))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_SEGMENT_RSP;
|
|
}
|
|
else
|
|
{
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
break;
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED */
|
|
|
|
case CO_SDO_ST_UPLOAD_INITIATE_REQ:
|
|
{
|
|
SDO->state = CO_SDO_ST_UPLOAD_INITIATE_RSP;
|
|
break;
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
case CO_SDO_ST_UPLOAD_SEGMENT_REQ:
|
|
{
|
|
if ((SDO->CANrxData[0] & 0xEFU) == 0x60U)
|
|
{
|
|
/* verify and alternate toggle bit */
|
|
uint8_t toggle = SDO->CANrxData[0] & 0x10U;
|
|
if (toggle != SDO->toggle)
|
|
{
|
|
abortCode = CO_SDO_AB_TOGGLE_BIT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
SDO->state = CO_SDO_ST_UPLOAD_SEGMENT_RSP;
|
|
}
|
|
else
|
|
{
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
break;
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED */
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ:
|
|
{
|
|
SDO->block_crcEnabled = (SDO->CANrxData[0] & 0x04) != 0;
|
|
|
|
/* is size indicated? */
|
|
if ((SDO->CANrxData[0] & 0x02) != 0)
|
|
{
|
|
uint32_t size;
|
|
OD_size_t sizeInOd = SDO->OD_IO.stream.dataLength;
|
|
|
|
(void)memcpy(&size, &SDO->CANrxData[4], sizeof(size));
|
|
SDO->sizeInd = CO_SWAP_32(size);
|
|
|
|
/* Indicated size of SDO matches sizeof OD variable? */
|
|
if (sizeInOd > 0)
|
|
{
|
|
if (SDO->sizeInd > sizeInOd)
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_LONG;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
/* strings are allowed to be shorter */
|
|
else if (SDO->sizeInd < sizeInOd && (SDO->OD_IO.stream.attribute & ODA_STR) == 0)
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_SHORT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SDO->sizeInd = 0;
|
|
}
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP;
|
|
SDO->finished = false;
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
|
|
{
|
|
/* data are copied directly in the receive function */
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_DOWNLOAD_BLK_END_REQ:
|
|
{
|
|
if ((SDO->CANrxData[0] & 0xE3) == 0xC1)
|
|
{
|
|
/* Get number of data bytes in last segment, that do not contain data. Then reduce buffer. */
|
|
uint8_t noData = ((SDO->CANrxData[0] >> 2) & 0x07);
|
|
if (SDO->bufOffsetWr <= noData)
|
|
{
|
|
/* just in case, should never happen */
|
|
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
SDO->sizeTran -= noData;
|
|
SDO->bufOffsetWr -= noData;
|
|
|
|
uint16_t crcClient = 0;
|
|
if (SDO->block_crcEnabled)
|
|
{
|
|
crcClient = ((uint16_t)SDO->CANrxData[2]) << 8;
|
|
crcClient |= SDO->CANrxData[1];
|
|
}
|
|
|
|
if (!validateAndWriteToOD(SDO, &abortCode, 2, crcClient))
|
|
{
|
|
break;
|
|
}
|
|
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_END_RSP;
|
|
}
|
|
else
|
|
{
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ:
|
|
{
|
|
/* if pst (protocol switch threshold, byte5) is larger than data
|
|
* size of OD variable, then switch to segmented transfer */
|
|
if (SDO->sizeInd > 0 && SDO->CANrxData[5] > 0 && SDO->CANrxData[5] >= SDO->sizeInd)
|
|
{
|
|
SDO->state = CO_SDO_ST_UPLOAD_INITIATE_RSP;
|
|
}
|
|
else
|
|
{
|
|
/* data were already loaded from OD variable, verify crc */
|
|
if ((SDO->CANrxData[0] & 0x04) != 0)
|
|
{
|
|
SDO->block_crcEnabled = true;
|
|
SDO->block_crc = crc16_ccitt(SDO->buf, SDO->bufOffsetWr, 0);
|
|
}
|
|
else
|
|
{
|
|
SDO->block_crcEnabled = false;
|
|
}
|
|
|
|
/* get blksize and verify it */
|
|
SDO->block_blksize = SDO->CANrxData[4];
|
|
if (SDO->block_blksize < 1 || SDO->block_blksize > 127)
|
|
{
|
|
abortCode = CO_SDO_AB_BLOCK_SIZE;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
|
|
/* verify, if there is enough data */
|
|
if (!SDO->finished && SDO->bufOffsetWr < SDO->block_blksize * 7U)
|
|
{
|
|
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2:
|
|
{
|
|
if (SDO->CANrxData[0] == 0xA3)
|
|
{
|
|
SDO->block_seqno = 0;
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
|
|
}
|
|
else
|
|
{
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP:
|
|
{
|
|
if (SDO->CANrxData[0] == 0xA2)
|
|
{
|
|
SDO->block_blksize = SDO->CANrxData[2];
|
|
if (SDO->block_blksize < 1 || SDO->block_blksize > 127)
|
|
{
|
|
abortCode = CO_SDO_AB_BLOCK_SIZE;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
|
|
/* check number of segments */
|
|
if (SDO->CANrxData[1] < SDO->block_seqno)
|
|
{
|
|
/* NOT all segments transferred successfully. Re-transmit data after erroneous segment. */
|
|
OD_size_t cntFailed = SDO->block_seqno - SDO->CANrxData[1];
|
|
cntFailed = cntFailed * 7 - SDO->block_noData;
|
|
SDO->bufOffsetRd -= cntFailed;
|
|
SDO->sizeTran -= cntFailed;
|
|
}
|
|
else if (SDO->CANrxData[1] > SDO->block_seqno)
|
|
{
|
|
/* something strange from server, break transmission */
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
|
|
/* refill data buffer if necessary */
|
|
if (!readFromOd(SDO, &abortCode, SDO->block_blksize * 7, true))
|
|
{
|
|
break;
|
|
}
|
|
|
|
if (SDO->bufOffsetWr == SDO->bufOffsetRd)
|
|
{
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_END_SREQ;
|
|
}
|
|
else
|
|
{
|
|
SDO->block_seqno = 0;
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
break;
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK */
|
|
|
|
case CO_SDO_ST_IDLE:
|
|
case CO_SDO_ST_ABORT:
|
|
case CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER:
|
|
case CO_SDO_ST_DOWNLOAD_INITIATE_RSP:
|
|
case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP:
|
|
case CO_SDO_ST_UPLOAD_LOCAL_TRANSFER:
|
|
case CO_SDO_ST_UPLOAD_INITIATE_RSP:
|
|
case CO_SDO_ST_UPLOAD_SEGMENT_RSP:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_END_RSP:
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP:
|
|
case CO_SDO_ST_UPLOAD_BLK_END_SREQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_END_CRSP:
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) == 0
|
|
case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ:
|
|
case CO_SDO_ST_UPLOAD_SEGMENT_REQ:
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED */
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) == 0
|
|
case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_END_REQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2:
|
|
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP:
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK */
|
|
default:
|
|
{
|
|
/* unknown message received */
|
|
abortCode = CO_SDO_AB_CMD;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
} /* switch (SDO->state) */
|
|
} /* if (SDO->state != CO_SDO_ST_IDLE && SDO->state != CO_SDO_ST_ABORT) */
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
SDO->timeoutTimer = 0;
|
|
#endif
|
|
timeDifference_us = 0;
|
|
CO_FLAG_CLEAR(SDO->CANrxNew);
|
|
} /* else if (isNew) */
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
|
|
/* Timeout timers and transmit bufferFull flag ****************************/
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
if (ret == CO_SDO_RT_waitingResponse)
|
|
{
|
|
if (SDO->timeoutTimer < SDO->SDOtimeoutTime_us)
|
|
{
|
|
SDO->timeoutTimer += timeDifference_us;
|
|
}
|
|
if (SDO->timeoutTimer >= SDO->SDOtimeoutTime_us)
|
|
{
|
|
abortCode = CO_SDO_AB_TIMEOUT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
}
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_TIMERNEXT) != 0
|
|
else if (timerNext_us != NULL)
|
|
{
|
|
/* check again after timeout time elapsed */
|
|
uint32_t diff = SDO->SDOtimeoutTime_us - SDO->timeoutTimer;
|
|
if (*timerNext_us > diff)
|
|
{
|
|
*timerNext_us = diff;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
/* Timeout for sub-block transmission */
|
|
if (SDO->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ)
|
|
{
|
|
if (SDO->block_timeoutTimer < SDO->block_SDOtimeoutTime_us)
|
|
{
|
|
SDO->block_timeoutTimer += timeDifference_us;
|
|
}
|
|
if (SDO->block_timeoutTimer >= SDO->block_SDOtimeoutTime_us)
|
|
{
|
|
/* SDO->state will change, processing will continue in this
|
|
* thread. Make memory barrier here with CO_FLAG_CLEAR() call. */
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
|
|
CO_FLAG_CLEAR(SDO->CANrxNew);
|
|
}
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_TIMERNEXT) != 0
|
|
else if (timerNext_us != NULL)
|
|
{
|
|
/* check again after timeout time elapsed */
|
|
uint32_t diff = SDO->block_SDOtimeoutTime_us - SDO->block_timeoutTimer;
|
|
if (*timerNext_us > diff)
|
|
{
|
|
*timerNext_us = diff;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK */
|
|
|
|
if (SDO->CANtxBuff->bufferFull)
|
|
{
|
|
ret = CO_SDO_RT_transmittBufferFull;
|
|
}
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED */
|
|
|
|
/* Transmit CAN data ******************************************************/
|
|
if (ret == CO_SDO_RT_waitingResponse)
|
|
{
|
|
/* clear response buffer */
|
|
(void)memset(SDO->CANtxBuff->data, 0, sizeof(SDO->CANtxBuff->data));
|
|
|
|
switch (SDO->state)
|
|
{
|
|
case CO_SDO_ST_DOWNLOAD_INITIATE_RSP:
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0x60;
|
|
SDO->CANtxBuff->data[1] = (uint8_t)SDO->index;
|
|
SDO->CANtxBuff->data[2] = (uint8_t)(SDO->index >> 8);
|
|
SDO->CANtxBuff->data[3] = SDO->subIndex;
|
|
|
|
/* reset timeout timer and send message */
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
SDO->timeoutTimer = 0;
|
|
#endif
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
if (SDO->finished)
|
|
{
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
}
|
|
else
|
|
{
|
|
SDO->toggle = 0x00;
|
|
SDO->sizeTran = 0;
|
|
SDO->bufOffsetWr = 0;
|
|
SDO->bufOffsetRd = 0;
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ;
|
|
}
|
|
#else
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
#endif
|
|
break;
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP:
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0x20U | SDO->toggle;
|
|
SDO->toggle = (SDO->toggle == 0x00U) ? 0x10U : 0x00U;
|
|
|
|
/* reset timeout timer and send message */
|
|
SDO->timeoutTimer = 0;
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
if (SDO->finished)
|
|
{
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
}
|
|
else
|
|
{
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ;
|
|
}
|
|
break;
|
|
}
|
|
#endif
|
|
|
|
case CO_SDO_ST_UPLOAD_INITIATE_RSP:
|
|
{
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
/* data were already loaded from OD variable */
|
|
if ((SDO->sizeInd > 0U) && (SDO->sizeInd <= 4U))
|
|
{
|
|
/* expedited transfer */
|
|
SDO->CANtxBuff->data[0] = (uint8_t)(0x43U | ((4U - SDO->sizeInd) << 2U));
|
|
(void)memcpy((void *)(&SDO->CANtxBuff->data[4]), (const void *)(&SDO->buf[0]), SDO->sizeInd);
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
}
|
|
else
|
|
{
|
|
/* data will be transferred with segmented transfer */
|
|
if (SDO->sizeInd > 0U)
|
|
{
|
|
/* indicate data size, if known */
|
|
uint32_t sizeInd = SDO->sizeInd;
|
|
uint32_t sizeIndSw = CO_SWAP_32(sizeInd);
|
|
SDO->CANtxBuff->data[0] = 0x41;
|
|
(void)memcpy((void *)(&SDO->CANtxBuff->data[4]), (const void *)(&sizeIndSw), sizeof(sizeIndSw));
|
|
}
|
|
else
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0x40;
|
|
}
|
|
SDO->toggle = 0x00;
|
|
SDO->timeoutTimer = 0;
|
|
SDO->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
|
|
}
|
|
#else /* Expedited transfer only */
|
|
/* load data from OD variable */
|
|
OD_size_t count = 0;
|
|
ODR_t odRet;
|
|
|
|
CO_LOCK_OD(SDO->CANdevTx);
|
|
odRet = SDO->OD_IO.read(&SDO->OD_IO.stream, &SDO->CANtxBuff->data[4], 4, &count);
|
|
CO_UNLOCK_OD(SDO->CANdevTx);
|
|
|
|
/* strings are allowed to be shorter */
|
|
if (odRet == ODR_PARTIAL && (SDO->OD_IO.stream.attribute & ODA_STR) != 0)
|
|
{
|
|
odRet = ODR_OK;
|
|
}
|
|
|
|
if (odRet != ODR_OK || count == 0)
|
|
{
|
|
abortCode = (odRet == ODR_OK) ? CO_SDO_AB_DEVICE_INCOMPAT
|
|
: (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
|
|
#ifdef CO_BIG_ENDIAN
|
|
/* swap data if necessary */
|
|
if ((SDO->OD_IO.stream.attribute & ODA_MB) != 0)
|
|
{
|
|
reverseBytes(buf, dataSizeToWrite);
|
|
}
|
|
#endif
|
|
SDO->CANtxBuff->data[0] = 0x43 | ((4 - count) << 2);
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED */
|
|
|
|
/* send message */
|
|
SDO->CANtxBuff->data[1] = (uint8_t)SDO->index;
|
|
SDO->CANtxBuff->data[2] = (uint8_t)(SDO->index >> 8);
|
|
SDO->CANtxBuff->data[3] = SDO->subIndex;
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
break;
|
|
}
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) != 0
|
|
case CO_SDO_ST_UPLOAD_SEGMENT_RSP:
|
|
{
|
|
/* refill the data buffer if necessary */
|
|
if (!readFromOd(SDO, &abortCode, 7, false))
|
|
{
|
|
break;
|
|
}
|
|
|
|
/* SDO command specifier with toggle bit */
|
|
SDO->CANtxBuff->data[0] = SDO->toggle;
|
|
SDO->toggle = (SDO->toggle == 0x00U) ? 0x10U : 0x00U;
|
|
|
|
OD_size_t count = SDO->bufOffsetWr - SDO->bufOffsetRd;
|
|
/* verify, if this is the last segment */
|
|
if ((count < 7U) || (SDO->finished && (count == 7U)))
|
|
{
|
|
/* indicate last segment and nnn */
|
|
SDO->CANtxBuff->data[0] |= (uint8_t)(((7U - count) << 1U) | 0x01U);
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
}
|
|
else
|
|
{
|
|
SDO->timeoutTimer = 0;
|
|
SDO->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
|
|
count = 7;
|
|
}
|
|
|
|
/* copy data segment to CAN message */
|
|
(void)memcpy(&SDO->CANtxBuff->data[1], SDO->buf + SDO->bufOffsetRd, count);
|
|
SDO->bufOffsetRd += count;
|
|
SDO->sizeTran += count;
|
|
|
|
/* verify if sizeTran is too large or too short if last segment */
|
|
if (SDO->sizeInd > 0U)
|
|
{
|
|
if (SDO->sizeTran > SDO->sizeInd)
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_LONG;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
else if ((ret == CO_SDO_RT_ok_communicationEnd) && (SDO->sizeTran < SDO->sizeInd))
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_SHORT;
|
|
ret = CO_SDO_RT_waitingResponse;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
else
|
|
{ /* MISRA C 2004 14.10 */
|
|
}
|
|
}
|
|
|
|
/* send message */
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
break;
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED */
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP:
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0xA4;
|
|
SDO->CANtxBuff->data[1] = (uint8_t)SDO->index;
|
|
SDO->CANtxBuff->data[2] = (uint8_t)(SDO->index >> 8);
|
|
SDO->CANtxBuff->data[3] = SDO->subIndex;
|
|
|
|
/* calculate number of block segments from free buffer space */
|
|
OD_size_t count = (CO_CONFIG_SDO_SRV_BUFFER_SIZE - 2) / 7;
|
|
if (count > 127)
|
|
{
|
|
count = 127;
|
|
}
|
|
SDO->block_blksize = (uint8_t)count;
|
|
SDO->CANtxBuff->data[4] = SDO->block_blksize;
|
|
|
|
/* reset variables */
|
|
SDO->sizeTran = 0;
|
|
SDO->finished = false;
|
|
SDO->bufOffsetWr = 0;
|
|
SDO->bufOffsetRd = 0;
|
|
SDO->block_seqno = 0;
|
|
SDO->block_crc = 0;
|
|
SDO->timeoutTimer = 0;
|
|
SDO->block_timeoutTimer = 0;
|
|
|
|
/* Block segments will be received in different thread. Make memory
|
|
* barrier here with CO_FLAG_CLEAR() call. */
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
|
|
CO_FLAG_CLEAR(SDO->CANrxNew);
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP:
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0xA2;
|
|
SDO->CANtxBuff->data[1] = SDO->block_seqno;
|
|
#ifdef CO_DEBUG_SDO_SERVER
|
|
bool_t transferShort = SDO->block_seqno != SDO->block_blksize;
|
|
uint8_t seqnoStart = SDO->block_seqno;
|
|
#endif
|
|
|
|
/* Is last segment? */
|
|
if (SDO->finished)
|
|
{
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_END_REQ;
|
|
}
|
|
else
|
|
{
|
|
/* calculate number of block segments from free buffer space */
|
|
OD_size_t count;
|
|
count = (CO_CONFIG_SDO_SRV_BUFFER_SIZE - 2 - SDO->bufOffsetWr) / 7;
|
|
if (count >= 127)
|
|
{
|
|
count = 127;
|
|
}
|
|
else if (SDO->bufOffsetWr > 0)
|
|
{
|
|
/* it is necessary to empty the buffer */
|
|
if (!validateAndWriteToOD(SDO, &abortCode, 1, 0))
|
|
{
|
|
break;
|
|
}
|
|
|
|
count = (CO_CONFIG_SDO_SRV_BUFFER_SIZE - 2 - SDO->bufOffsetWr) / 7;
|
|
if (count >= 127)
|
|
{
|
|
count = 127;
|
|
}
|
|
}
|
|
|
|
SDO->block_blksize = (uint8_t)count;
|
|
SDO->block_seqno = 0;
|
|
/* Block segments will be received in different thread. Make
|
|
* memory barrier here with CO_FLAG_CLEAR() call. */
|
|
SDO->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
|
|
CO_FLAG_CLEAR(SDO->CANrxNew);
|
|
}
|
|
|
|
SDO->CANtxBuff->data[2] = SDO->block_blksize;
|
|
|
|
/* reset block_timeoutTimer, but not SDO->timeoutTimer */
|
|
SDO->block_timeoutTimer = 0;
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
#ifdef CO_DEBUG_SDO_SERVER
|
|
if (transferShort && !SDO->finished)
|
|
{
|
|
char msg[80];
|
|
sprintf(msg, "sub-block restarted: sequnoPrev=%02X, blksize=%02X", seqnoStart, SDO->block_blksize);
|
|
CO_DEBUG_SDO_SERVER(msg);
|
|
}
|
|
#endif
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_DOWNLOAD_BLK_END_RSP:
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0xA1;
|
|
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_ok_communicationEnd;
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP:
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0xC4;
|
|
SDO->CANtxBuff->data[1] = (uint8_t)SDO->index;
|
|
SDO->CANtxBuff->data[2] = (uint8_t)(SDO->index >> 8);
|
|
SDO->CANtxBuff->data[3] = SDO->subIndex;
|
|
|
|
/* indicate data size */
|
|
if (SDO->sizeInd > 0)
|
|
{
|
|
uint32_t size = CO_SWAP_32(SDO->sizeInd);
|
|
SDO->CANtxBuff->data[0] |= 0x02;
|
|
(void)memcpy(&SDO->CANtxBuff->data[4], &size, sizeof(size));
|
|
}
|
|
|
|
/* reset timeout timer and send message */
|
|
SDO->timeoutTimer = 0;
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2;
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
|
|
{
|
|
/* write header and get current count */
|
|
SDO->CANtxBuff->data[0] = ++SDO->block_seqno;
|
|
OD_size_t count = SDO->bufOffsetWr - SDO->bufOffsetRd;
|
|
/* verify, if this is the last segment */
|
|
if (count < 7 || (SDO->finished && count == 7))
|
|
{
|
|
SDO->CANtxBuff->data[0] |= 0x80;
|
|
}
|
|
else
|
|
{
|
|
count = 7;
|
|
}
|
|
|
|
/* copy data segment to CAN message */
|
|
(void)memcpy(&SDO->CANtxBuff->data[1], SDO->buf + SDO->bufOffsetRd, count);
|
|
SDO->bufOffsetRd += count;
|
|
SDO->block_noData = (uint8_t)(7 - count);
|
|
SDO->sizeTran += count;
|
|
|
|
/* verify if sizeTran is too large or too short if last segment */
|
|
if (SDO->sizeInd > 0)
|
|
{
|
|
if (SDO->sizeTran > SDO->sizeInd)
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_LONG;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
else if (SDO->bufOffsetWr == SDO->bufOffsetRd && SDO->sizeTran < SDO->sizeInd)
|
|
{
|
|
abortCode = CO_SDO_AB_DATA_SHORT;
|
|
SDO->state = CO_SDO_ST_ABORT;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* is last segment or all segments in current block transferred? */
|
|
if (SDO->bufOffsetWr == SDO->bufOffsetRd || SDO->block_seqno >= SDO->block_blksize)
|
|
{
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
|
|
}
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_TIMERNEXT) != 0
|
|
else
|
|
{
|
|
/* Inform OS to call this function again without delay. */
|
|
if (timerNext_us != NULL)
|
|
{
|
|
*timerNext_us = 0;
|
|
}
|
|
}
|
|
#endif
|
|
/* reset timeout timer and send message */
|
|
SDO->timeoutTimer = 0;
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
break;
|
|
}
|
|
|
|
case CO_SDO_ST_UPLOAD_BLK_END_SREQ:
|
|
{
|
|
SDO->CANtxBuff->data[0] = 0xC1 | (SDO->block_noData << 2);
|
|
SDO->CANtxBuff->data[1] = (uint8_t)SDO->block_crc;
|
|
SDO->CANtxBuff->data[2] = (uint8_t)(SDO->block_crc >> 8);
|
|
|
|
/* reset timeout timer and send message */
|
|
SDO->timeoutTimer = 0;
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
SDO->state = CO_SDO_ST_UPLOAD_BLK_END_CRSP;
|
|
break;
|
|
}
|
|
#endif /* (CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK */
|
|
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) == 0
|
|
case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP:
|
|
case CO_SDO_ST_UPLOAD_SEGMENT_RSP:
|
|
#endif
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) == 0
|
|
case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_END_RSP:
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP:
|
|
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_END_SREQ:
|
|
#endif
|
|
case CO_SDO_ST_IDLE:
|
|
case CO_SDO_ST_ABORT:
|
|
case CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER:
|
|
case CO_SDO_ST_DOWNLOAD_INITIATE_REQ:
|
|
case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ:
|
|
case CO_SDO_ST_UPLOAD_LOCAL_TRANSFER:
|
|
case CO_SDO_ST_UPLOAD_INITIATE_REQ:
|
|
case CO_SDO_ST_UPLOAD_SEGMENT_REQ:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
|
|
case CO_SDO_ST_DOWNLOAD_BLK_END_REQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ:
|
|
case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2:
|
|
case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP:
|
|
case CO_SDO_ST_UPLOAD_BLK_END_CRSP:
|
|
default:
|
|
{
|
|
/* none */
|
|
break;
|
|
}
|
|
} /* switch (SDO->state) */
|
|
}
|
|
|
|
if (ret == CO_SDO_RT_waitingResponse)
|
|
{
|
|
if (SDO->state == CO_SDO_ST_ABORT)
|
|
{
|
|
uint32_t code = CO_SWAP_32((uint32_t)abortCode);
|
|
/* Send SDO abort message */
|
|
SDO->CANtxBuff->data[0] = 0x80;
|
|
SDO->CANtxBuff->data[1] = (uint8_t)SDO->index;
|
|
SDO->CANtxBuff->data[2] = (uint8_t)(SDO->index >> 8);
|
|
SDO->CANtxBuff->data[3] = SDO->subIndex;
|
|
|
|
(void)memcpy((void *)(&SDO->CANtxBuff->data[4]), (const void *)(&code), sizeof(code));
|
|
(void)CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
|
|
SDO->state = CO_SDO_ST_IDLE;
|
|
ret = CO_SDO_RT_endedWithServerAbort;
|
|
}
|
|
#if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) != 0
|
|
else if (SDO->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ)
|
|
{
|
|
ret = CO_SDO_RT_blockDownldInProgress;
|
|
}
|
|
else if (SDO->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ)
|
|
{
|
|
ret = CO_SDO_RT_blockUploadInProgress;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
return ret;
|
|
}
|