AtguiguCorp/linear-Slide
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
linear-Slide/Middleware/CANopenNode/309/CO_gateway_ascii.c

2010 lines
81 KiB
C
Raw Permalink Normal View History

第一版代码，为了在EEPROM保存参数的时候走STM32的CRC，让Codex修改了一下，现在的效果是无法存储，codex表示原因是CRC方法不同，修改到一半今天的额度使用完了，有待后续解决CRC的bug
2026-02-28 17:36:05 +08:00
/*
* CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)
*
* @file CO_gateway_ascii.c
* @ingroup CO_CANopen_309_3
* @author Janez Paternoster
* @author Martin Wagner
* @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 "309/CO_gateway_ascii.h"
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII) != 0
#include <stdlib.h>
#include <ctype.h>
#include <inttypes.h>
#include <stdio.h>
/* verify configuration */
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ENABLE) == 0
#error CO_CONFIG_FIFO_ENABLE must be enabled.
#endif
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ASCII_COMMANDS) == 0
#error CO_CONFIG_FIFO_ASCII_COMMANDS must be enabled.
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ASCII_DATATYPES) == 0
#error CO_CONFIG_FIFO_ASCII_DATATYPES must be enabled.
#endif
#endif
CO_ReturnError_t
CO_GTWA_init(CO_GTWA_t* gtwa,
#if (((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0) || defined CO_DOXYGEN
CO_SDOclient_t* SDO_C, uint16_t SDOclientTimeoutTime_ms, bool_t SDOclientBlockTransfer,
#endif
#if (((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_NMT) != 0) || defined CO_DOXYGEN
CO_NMT_t* NMT,
#endif
#if (((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LSS) != 0) || defined CO_DOXYGEN
CO_LSSmaster_t* LSSmaster,
#endif
#if (((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_LEDS) != 0) || defined CO_DOXYGEN
CO_LEDs_t* LEDs,
#endif
uint8_t dummy) {
(void)dummy;
/* verify arguments */
if ((gtwa == NULL)
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
|| (SDO_C == NULL) || (SDOclientTimeoutTime_ms == 0U)
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_NMT) != 0
|| (NMT == NULL)
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LSS) != 0
|| (LSSmaster == NULL)
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_LEDS) != 0
|| (LEDs == NULL)
#endif
) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* clear the object */
(void)memset(gtwa, 0, sizeof(CO_GTWA_t));
/* initialize variables */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
gtwa->SDO_C = SDO_C;
gtwa->SDOtimeoutTime = SDOclientTimeoutTime_ms;
gtwa->SDOblockTransferEnable = SDOclientBlockTransfer;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_NMT) != 0
gtwa->NMT = NMT;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LSS) != 0
gtwa->LSSmaster = LSSmaster;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_LEDS) != 0
gtwa->LEDs = LEDs;
#endif
gtwa->net_default = -1;
gtwa->node_default = -1;
gtwa->state = CO_GTWA_ST_IDLE;
gtwa->respHold = false;
CO_fifo_init(&gtwa->commFifo, &gtwa->commBuf[0], CO_CONFIG_GTWA_COMM_BUF_SIZE + 1);
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LOG) != 0
CO_fifo_init(&gtwa->logFifo, &gtwa->logBuf[0], CO_CONFIG_GTWA_LOG_BUF_SIZE + 1);
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) */
return CO_ERROR_NO;
}
void
CO_GTWA_initRead(CO_GTWA_t* gtwa,
size_t (*readCallback)(void* object, const char* buf, size_t count, uint8_t* connectionOK),
void* readCallbackObject) {
if (gtwa != NULL) {
gtwa->readCallback = readCallback;
gtwa->readCallbackObject = readCallbackObject;
}
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LOG) != 0
void
CO_GTWA_log_print(CO_GTWA_t* gtwa, const char* message) {
if ((gtwa != NULL) && (message != NULL)) {
const char* c;
/* add newline between messages */
if (CO_fifo_getOccupied(&gtwa->logFifo) > 0U) {
CO_fifo_putc_ov(&gtwa->logFifo, '\n');
}
for (c = &message[0]; *c != '\0'; c++) {
CO_fifo_putc_ov(&gtwa->logFifo, (const uint8_t)*c);
}
}
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
/*******************************************************************************
* HELPER FUNCTIONS
******************************************************************************/
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_HELP) != 0
/* help strings ("\n" is used between string, "\r\n" closes the response.) */
static const char CO_GTWA_helpString[] =
"\nCommand strings start with '\"[\"<sequence>\"]\"' followed by:\n"
"[[<net>] <node>] r[ead] <index> <subindex> [<datatype>] # SDO upload.\n"
"[[<net>] <node>] w[rite] <index> <subindex> <datatype> <value> # SDO download.\n"
"\n"
"[[<net>] <node>] start # NMT Start node.\n"
"[[<net>] <node>] stop # NMT Stop node.\n"
"[[<net>] <node>] preop[erational] # NMT Set node to pre-operational.\n"
"[[<net>] <node>] reset node # NMT Reset node.\n"
"[[<net>] <node>] reset comm[unication] # NMT Reset communication.\n"
"\n"
"[<net>] set network <value> # Set default net.\n"
"[<net>] set node <value> # Set default node.\n"
"[<net>] set sdo_timeout <value> # Configure SDO client time-out in ms.\n"
"[<net>] set sdo_block <0|1> # Enable/disable SDO block transfer.\n"
"\n"
"help [datatype|lss] # Print this or datatype or lss help.\n"
"led # Print status LEDs of this device.\n"
"log # Print message log.\n"
"\n"
"Response:\n"
"\"[\"<sequence>\"]\" OK | <value> |\n"
" ERROR:<SDO-abort-code> | ERROR:<internal-error-code>\n"
"\n"
"* Every command must be terminated with <CR><LF> ('\\r\\n'). characters. Same\n"
" is response. String is not null terminated, <CR> is optional in command.\n"
"* Comments started with '#' are ignored. They may be on the beginning of the\n"
" line or after the command string.\n"
"* 'sdo_timeout' is in milliseconds, 500 by default. Block transfer is\n"
" disabled by default.\n"
"* If '<net>' or '<node>' is not specified within commands, then value defined\n"
" by 'set network' or 'set node' command is used.\r\n";
static const char CO_GTWA_helpStringDatatypes[] =
"\nDatatypes:\n"
"b # Boolean.\n"
"i8, i16, i32, i64 # Signed integers.\n"
"u8, u16, u32, u64 # Unsigned integers.\n"
"x8, x16, x32, x64 # Unsigned integers, displayed as hexadecimal, non-standard.\n"
"r32, r64 # Real numbers.\n"
"vs # Visible string (between double quotes if multi-word).\n"
"os, us # Octet, unicode string, (mime-base64 (RFC2045) based, line).\n"
"d # domain (mime-base64 (RFC2045) based, one line).\n"
"hex # Hexagonal data, optionally space separated, non-standard.\r\n";
static const char CO_GTWA_helpStringLss[] =
"\nLSS commands:\n"
"lss_switch_glob <0|1> # Switch state global command.\n"
"lss_switch_sel <vendorID> <product code> \\\n"
" <revisionNo> <serialNo> #Switch state selective.\n"
"lss_set_node <node> # Configure node-ID.\n"
"lss_conf_bitrate <table_selector=0> \\\n"
" <table_index> # Configure bit-rate.\n"
"lss_activate_bitrate <switch_delay_ms> # Activate new bit-rate.\n"
"lss_store # LSS store configuration.\n"
"lss_inquire_addr [<LSSSUB=0..3>] # Inquire LSS address.\n"
"lss_get_node # Inquire node-ID.\n"
"_lss_fastscan [<timeout_ms>] # Identify fastscan, non-standard.\n"
"lss_allnodes [<timeout_ms> [<nodeStart=1..127> <store=0|1>\\\n"
" [<scanType0> <vendorId> <scanType1> <productCode>\\\n"
" <scanType2> <revisionNo> <scanType3> <serialNo>]]]\n"
" # Node-ID configuration of all nodes.\n"
"\n"
"* All LSS commands start with '\"[\"<sequence>\"]\" [<net>]'.\n"
"* <table_index>: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,\n"
" 4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto\n"
"* <scanType>: 0=fastscan, 1=ignore, 2=match value in next parameter\r\n";
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_LEDS) != 0
#define CO_GTWA_LED_PRINTOUTS_SIZE 5U
static const char* CO_GTWA_LED_PRINTOUTS[CO_GTWA_LED_PRINTOUTS_SIZE] = {
" CANopen status LEDs: R G \r", " CANopen status LEDs: R G* \r",
" CANopen status LEDs: R* G \r", " CANopen status LEDs: R* G* \r",
" \r"};
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS */
/* Get uint32 number from token, verify limits and set *err if necessary */
static inline uint32_t
getU32(char* token, uint32_t min, uint32_t max, bool_t* err) {
char* sRet;
uint32_t num = strtoul(token, &sRet, 0);
if ((sRet != strchr(token, (int32_t)'\0')) || (num < min) || (num > max)) {
*err = true;
}
return num;
}
/* Verify net and node, return true on error */
static bool_t
checkNetNode(CO_GTWA_t* gtwa, int32_t net, int16_t node, uint8_t NodeMin, CO_GTWA_respErrorCode_t* errCode) {
bool_t e = false;
CO_GTWA_respErrorCode_t eCode;
if (node == -1) {
eCode = CO_GTWA_respErrorNoDefaultNodeSet;
e = true;
} else if ((node < (int16_t)NodeMin) || (node > (int16_t)127)) {
eCode = CO_GTWA_respErrorUnsupportedNode;
e = true;
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_MULTI_NET) != 0
else if (net == -1) {
eCode = CO_GTWA_respErrorNoDefaultNetSet;
e = true;
}
/* not implemented */
else if ((net < CO_CONFIG_GTW_NET_MIN) || (net > CO_CONFIG_GTW_NET_MAX)) {
eCode = CO_GTWA_respErrorUnsupportedNet;
e = true;
}
#endif
else {
gtwa->net = (uint16_t)net;
gtwa->node = (uint8_t)node;
}
if (e) {
*errCode = eCode;
}
return e;
}
/* Verify net, return true on error */
static bool_t
checkNet(CO_GTWA_t* gtwa, int32_t net, CO_GTWA_respErrorCode_t* errCode) {
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_MULTI_NET) != 0
bool_t e = false;
CO_GTWA_respErrorCode_t eCode;
if (net == -1) {
eCode = CO_GTWA_respErrorNoDefaultNetSet;
e = true;
}
/* not implemented */
else if ((net < CO_CONFIG_GTW_NET_MIN) || (net > CO_CONFIG_GTW_NET_MAX)) {
eCode = CO_GTWA_respErrorUnsupportedNet;
e = true;
} else {
gtwa->net = (uint16_t)net;
}
if (e) {
*errCode = eCode;
}
return e;
#else
(void)errCode; /* unused */
#define CO_CONFIG_GTW_NET_MIN 0
#define CO_CONFIG_GTW_NET_MAX 0xFFFF
gtwa->net = (uint16_t)net;
return false;
#endif
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
/* data types for SDO read or write */
static const CO_GTWA_dataType_t dataTypes[] = {
{(char*)"hex", 0, CO_fifo_readHex2a, CO_fifo_cpyTok2Hex}, /* hex, non-standard */
{(char*)"b", 1, CO_fifo_readU82a, CO_fifo_cpyTok2U8}, /* BOOLEAN */
{(char*)"i8", 1, CO_fifo_readI82a, CO_fifo_cpyTok2I8}, /* INTEGER8 */
{(char*)"i16", 2, CO_fifo_readI162a, CO_fifo_cpyTok2I16}, /* INTEGER16 */
{(char*)"i32", 4, CO_fifo_readI322a, CO_fifo_cpyTok2I32}, /* INTEGER32 */
{(char*)"i64", 8, CO_fifo_readI642a, CO_fifo_cpyTok2I64}, /* INTEGER64 */
{(char*)"u8", 1, CO_fifo_readU82a, CO_fifo_cpyTok2U8}, /* UNSIGNED8 */
{(char*)"u16", 2, CO_fifo_readU162a, CO_fifo_cpyTok2U16}, /* UNSIGNED16 */
{(char*)"u32", 4, CO_fifo_readU322a, CO_fifo_cpyTok2U32}, /* UNSIGNED32 */
{(char*)"u64", 8, CO_fifo_readU642a, CO_fifo_cpyTok2U64}, /* UNSIGNED64 */
{(char*)"x8", 1, CO_fifo_readX82a, CO_fifo_cpyTok2U8}, /* UNSIGNED8 */
{(char*)"x16", 2, CO_fifo_readX162a, CO_fifo_cpyTok2U16}, /* UNSIGNED16 */
{(char*)"x32", 4, CO_fifo_readX322a, CO_fifo_cpyTok2U32}, /* UNSIGNED32 */
{(char*)"x64", 8, CO_fifo_readX642a, CO_fifo_cpyTok2U64}, /* UNSIGNED64 */
{(char*)"r32", 4, CO_fifo_readR322a, CO_fifo_cpyTok2R32}, /* REAL32 */
{(char*)"r64", 8, CO_fifo_readR642a, CO_fifo_cpyTok2R64}, /* REAL64 */
{(char*)"vs", 0, CO_fifo_readVs2a, CO_fifo_cpyTok2Vs}, /* VISIBLE_STRING */
{(char*)"os", 0, CO_fifo_readB642a, CO_fifo_cpyTok2B64}, /* OCTET_STRING base64 */
{(char*)"us", 0, CO_fifo_readB642a, CO_fifo_cpyTok2B64}, /* UNICODE_STRING base64 */
{(char*)"d", 0, CO_fifo_readB642a, CO_fifo_cpyTok2B64} /* DOMAIN - base64 */
};
/* get data type from token */
static const CO_GTWA_dataType_t*
CO_GTWA_getDataType(char* token, bool_t* err) {
if ((token != NULL) && (*err == false)) {
uint32_t i;
uint32_t len = sizeof(dataTypes) / sizeof(CO_GTWA_dataType_t);
for (i = 0; i < len; i++) {
const CO_GTWA_dataType_t* dt = &dataTypes[i];
if (strcmp(token, dt->syntax) == 0) {
return dt;
}
}
}
*err = true;
return NULL;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
/* transfer response buffer and verify if all bytes was read. Return true on
* success, or false, if communication is broken. */
static bool_t
respBufTransfer(CO_GTWA_t* gtwa) {
uint8_t connectionOK = 1;
if (gtwa->readCallback == NULL) {
/* no callback registered, just purge the response */
gtwa->respBufOffset = 0;
gtwa->respBufCount = 0;
gtwa->respHold = false;
} else {
/* transfer response to the application */
size_t countRead = gtwa->readCallback(gtwa->readCallbackObject,
(const char*)&gtwa->respBuf[gtwa->respBufOffset], gtwa->respBufCount,
&connectionOK);
if (countRead < gtwa->respBufCount) {
gtwa->respBufOffset += countRead;
gtwa->respBufCount -= countRead;
gtwa->respHold = true;
} else {
gtwa->respBufOffset = 0;
gtwa->respBufCount = 0;
gtwa->respHold = false;
}
}
return connectionOK != 0U;
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_ERROR_DESC) != 0
#ifndef CO_CONFIG_GTW_ASCII_ERROR_DESC_STRINGS
#define CO_CONFIG_GTW_ASCII_ERROR_DESC_STRINGS
typedef struct {
const uint32_t code;
const char* desc;
} errorDescs_t;
static const errorDescs_t errorDescs[] = {{100, "Request not supported."},
{101, "Syntax error."},
{102, "Request not processed due to internal state."},
{103, "Time-out."},
{104, "No default net set."},
{105, "No default node set."},
{106, "Unsupported net."},
{107, "Unsupported node."},
{200, "Lost guarding message."},
{201, "Lost connection."},
{202, "Heartbeat started."},
{203, "Heartbeat lost."},
{204, "Wrong NMT state."},
{205, "Boot-up."},
{300, "Error passive."},
{301, "Bus off."},
{303, "CAN buffer overflow."},
{304, "CAN init."},
{305, "CAN active (at init or start-up)."},
{400, "PDO already used."},
{401, "PDO length exceeded."},
{501, "LSS implementation- / manufacturer-specific error."},
{502, "LSS node-ID not supported."},
{503, "LSS bit-rate not supported."},
{504, "LSS parameter storing failed."},
{505, "LSS command failed because of media error."},
{600, "Running out of memory."}};
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
static const errorDescs_t errorDescsSDO[] = {
{0x00000000, "No abort."},
{0x05030000, "Toggle bit not altered."},
{0x05040000, "SDO protocol timed out."},
{0x05040001, "Command specifier not valid or unknown."},
{0x05040002, "Invalid block size in block mode."},
{0x05040003, "Invalid sequence number in block mode."},
{0x05040004, "CRC error (block mode only)."},
{0x05040005, "Out of memory."},
{0x06010000, "Unsupported access to an object."},
{0x06010001, "Attempt to read a write only object."},
{0x06010002, "Attempt to write a read only object."},
{0x06020000, "Object does not exist."},
{0x06040041, "Object cannot be mapped to the PDO."},
{0x06040042, "Number and length of object to be mapped exceeds PDO length."},
{0x06040043, "General parameter incompatibility reasons."},
{0x06040047, "General internal incompatibility in device."},
{0x06060000, "Access failed due to hardware error."},
{0x06070010, "Data type does not match, length of service parameter does not match."},
{0x06070012, "Data type does not match, length of service parameter too high."},
{0x06070013, "Data type does not match, length of service parameter too short."},
{0x06090011, "Sub index does not exist."},
{0x06090030, "Invalid value for parameter (download only)."},
{0x06090031, "Value range of parameter written too high."},
{0x06090032, "Value range of parameter written too low."},
{0x06090036, "Maximum value is less than minimum value."},
{0x060A0023, "Resource not available: SDO connection."},
{0x08000000, "General error."},
{0x08000020, "Data cannot be transferred or stored to application."},
{0x08000021, "Data cannot be transferred or stored to application because of local control."},
{0x08000022, "Data cannot be transferred or stored to application because of present device state."},
{0x08000023, "Object dictionary not present or dynamic generation fails."},
{0x08000024, "No data available."}};
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
#endif /* CO_CONFIG_GTW_ASCII_ERROR_DESC_STRINGS */
static void
responseWithError(CO_GTWA_t* gtwa, CO_GTWA_respErrorCode_t respErrorCode) {
uint32_t i;
uint32_t len = sizeof(errorDescs) / sizeof(errorDescs_t);
const char* desc = "-";
for (i = 0; i < len; i++) {
const errorDescs_t* ed = &errorDescs[i];
if ((CO_GTWA_respErrorCode_t)ed->code == respErrorCode) {
desc = ed->desc;
}
}
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "[%" PRId32 "] ERROR:%d #%s\r\n",
(int32_t)gtwa->sequence, (int32_t)respErrorCode, desc);
(void)respBufTransfer(gtwa);
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
static void
responseWithErrorSDO(CO_GTWA_t* gtwa, CO_SDO_abortCode_t abortCode, bool_t postponed) {
uint32_t i;
uint32_t len = sizeof(errorDescsSDO) / sizeof(errorDescs_t);
const char* desc = "-";
for (i = 0; i < len; i++) {
const errorDescs_t* ed = &errorDescsSDO[i];
if ((CO_SDO_abortCode_t)ed->code == abortCode) {
desc = ed->desc;
}
}
if (!postponed) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%" PRId32 "] ERROR:0x%08X #%s\r\n", (int32_t)gtwa->sequence,
(uint32_t)abortCode, desc);
} else {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "\n...ERROR:0x%08X #%s\r\n",
(uint32_t)abortCode, desc);
}
(void)respBufTransfer(gtwa);
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
#else /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_ERROR_DESC */
static inline void
responseWithError(CO_GTWA_t* gtwa, CO_GTWA_respErrorCode_t respErrorCode) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "[%" PRId32 "] ERROR:%d\r\n",
gtwa->sequence, respErrorCode);
(void)respBufTransfer(gtwa);
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
static inline void
responseWithErrorSDO(CO_GTWA_t* gtwa, CO_SDO_abortCode_t abortCode, bool_t postponed) {
if (!postponed) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "[%" PRId32 "] ERROR:0x%08X\r\n",
gtwa->sequence, abortCode);
} else {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "\n...ERROR:0x%08X\r\n", abortCode);
}
(void)respBufTransfer(gtwa);
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_ERROR_DESC */
static inline void
responseWithOK(CO_GTWA_t* gtwa) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "[%" PRId32 "] OK\r\n",
(int32_t)gtwa->sequence);
(void)respBufTransfer(gtwa);
}
static inline void
responseWithEmpty(CO_GTWA_t* gtwa) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "\r\n");
(void)respBufTransfer(gtwa);
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LSS) != 0
static void
responseLSS(CO_GTWA_t* gtwa, CO_LSSmaster_return_t lss_ret) {
if (lss_ret == CO_LSSmaster_OK) {
responseWithOK(gtwa);
} else {
CO_GTWA_respErrorCode_t respErrorCode;
if ((lss_ret == CO_LSSmaster_TIMEOUT) || (lss_ret == CO_LSSmaster_SCAN_NOACK)) {
respErrorCode = CO_GTWA_respErrorTimeOut;
} else if (lss_ret == CO_LSSmaster_OK_MANUFACTURER) {
respErrorCode = CO_GTWA_respErrorLSSmanufacturer;
} else {
respErrorCode = CO_GTWA_respErrorInternalState;
}
responseWithError(gtwa, respErrorCode);
}
}
#endif
static inline void
convertToLower(char* token, size_t maxCount) {
size_t i;
char* c = &token[0];
for (i = 0; i < maxCount; i++) {
if (*c == '\0') {
break;
} else {
*c = (char)tolower((int32_t)*c);
}
c++;
}
}
/*******************************************************************************
* PROCESS FUNCTION
******************************************************************************/
void
CO_GTWA_process(CO_GTWA_t* gtwa, bool_t enable, uint32_t timeDifference_us, uint32_t* timerNext_us) {
(void)timerNext_us; /* may be unused */
bool_t err = false; /* syntax or other error, true or false, I/O variable */
uint8_t closed; /* indication of command delimiter, I/O variable */
CO_GTWA_respErrorCode_t respErrorCode = CO_GTWA_respErrorNone;
if (gtwa == NULL) {
return;
}
if (!enable) {
gtwa->state = CO_GTWA_ST_IDLE;
CO_fifo_reset(&gtwa->commFifo);
return;
}
/* If there is some more output data for application, read them first. Hold on this state, if necessary. */
if (gtwa->respHold) {
timeDifference_us += gtwa->timeDifference_us_cumulative;
(void)respBufTransfer(gtwa);
if (gtwa->respHold) {
gtwa->timeDifference_us_cumulative = timeDifference_us;
return;
} else {
gtwa->timeDifference_us_cumulative = 0;
}
}
/***************************************************************************
* COMMAND PARSER
***************************************************************************/
/* if idle, search for new command, skip comments or empty lines */
while (CO_fifo_CommSearch(&gtwa->commFifo, false) && (gtwa->state == CO_GTWA_ST_IDLE)) {
char tok[20];
size_t n;
uint32_t ui[3];
int32_t i;
int32_t net = gtwa->net_default;
int16_t node = gtwa->node_default;
/* parse mandatory token '"["<sequence>"]"' */
closed = 0xFFU;
n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
/* Break if error in token or token was found, but closed with command delimiter. */
if (err || ((n > 0U) && (closed != 0U))) {
err = true;
break;
}
/* If empty line or just comment, continue with next command */
else if ((n == 0U) && (closed != 0U)) {
responseWithEmpty(gtwa);
continue;
} else { /* MISRA C 2004 14.10 */
}
if (tok[0] != '[') {
err = true;
break;
}
if (tok[strlen(tok) - 1U] != ']') {
err = true;
break;
}
tok[strlen(tok) - 1U] = '\0';
gtwa->sequence = getU32(tok + 1, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
/* parse optional tokens '[[<net>] <node>]', both numerical. Then
* follows mandatory token <command>, which is not numerical. */
for (i = 0; i < 3; i++) {
closed = 0xFFU;
n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
if (err || (n == 0U)) {
/* empty token, break on error */
err = true;
break;
} else if ((int32_t)isdigit((int)tok[0]) == 0) {
/* <command> found */
break;
} else if (closed != 0U) {
/* numerical value must not be closed */
err = true;
break;
} else { /* MISRA C 2004 14.10 */
}
ui[i] = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
}
if (err) {
break;
}
switch (i) {
case 0: /* only <command> (pointed by token) */ break;
case 1: /* <node> and <command> tokens */
if (ui[0] > 127U) {
err = true;
respErrorCode = CO_GTWA_respErrorUnsupportedNode;
} else {
node = (int16_t)ui[0];
}
break;
case 2: /* <net>, <node> and <command> tokens */
if (ui[0] > 0xFFFFU) {
err = true;
respErrorCode = CO_GTWA_respErrorUnsupportedNet;
} else if (ui[1] > 127U) {
err = true;
respErrorCode = CO_GTWA_respErrorUnsupportedNode;
} else {
net = (int32_t)ui[0];
node = (int16_t)ui[1];
}
break;
case 3: /* <command> token contains digit */ err = true; break;
default:
/* MISRA C 2004 15.3 */
break;
}
if (err) {
break;
}
/* command is case insensitive */
convertToLower(tok, sizeof(tok));
bool_t tok_is_set = strcmp(tok, "set") == 0;
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
bool_t tok_is_read = strcmp(tok, "r") == 0;
tok_is_read = (strcmp(tok, "read") == 0) || tok_is_read;
bool_t tok_is_write = strcmp(tok, "w") == 0;
tok_is_write = (strcmp(tok, "write") == 0) || tok_is_write;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_NMT) != 0
bool_t tok_is_start = strcmp(tok, "start") == 0;
bool_t tok_is_stop = strcmp(tok, "stop") == 0;
bool_t tok_is_preop = strcmp(tok, "preop") == 0;
tok_is_preop = (strcmp(tok, "preoperational") == 0) || tok_is_preop;
bool_t tok_is_reset = strcmp(tok, "reset") == 0;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LSS) != 0
bool_t tok_is_lss_switch_glob = strcmp(tok, "lss_switch_glob") == 0;
bool_t tok_is_lss_switch_sel = strcmp(tok, "lss_switch_sel") == 0;
bool_t tok_is_lss_set_node = strcmp(tok, "lss_set_node") == 0;
bool_t tok_is_lss_conf_bitrate = strcmp(tok, "lss_conf_bitrate") == 0;
bool_t tok_is_lss_activate_bitrate = strcmp(tok, "lss_activate_bitrate") == 0;
bool_t tok_is_lss_store = strcmp(tok, "lss_store") == 0;
bool_t tok_is_lss_inquire_addr = strcmp(tok, "lss_inquire_addr") == 0;
bool_t tok_is_lss_get_node = strcmp(tok, "lss_get_node") == 0;
bool_t tok_is__lss_fastscan = strcmp(tok, "_lss_fastscan") == 0;
bool_t tok_is_lss_allnodes = strcmp(tok, "lss_allnodes") == 0;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LOG) != 0
bool_t tok_is_log = strcmp(tok, "log") == 0;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_HELP) != 0
bool_t tok_is_help = strcmp(tok, "help") == 0;
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_LEDS) != 0
bool_t tok_is_led = strcmp(tok, "led") == 0;
#endif
/* set command - multiple sub commands */
if (tok_is_set) {
if (closed != 0U) {
err = true;
break;
}
/* command 2 */
closed = 0xFFU;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
if (err) {
break;
}
convertToLower(tok, sizeof(tok));
/* 'set network <value>' */
if (strcmp(tok, "network") == 0) {
uint16_t value;
if (closed != 0U) {
err = true;
break;
}
/* value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
value = (uint16_t)getU32(tok, CO_CONFIG_GTW_NET_MIN, CO_CONFIG_GTW_NET_MAX, &err);
if (err) {
break;
}
gtwa->net_default = (int32_t)value;
responseWithOK(gtwa);
}
/* 'set node <value>' */
else if (strcmp(tok, "node") == 0) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint8_t value;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
value = (uint8_t)getU32(tok, 1, 127, &err);
if (err) {
break;
}
gtwa->node_default = (int16_t)value;
responseWithOK(gtwa);
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
/* 'set sdo_timeout <value_ms>' */
else if (strcmp(tok, "sdo_timeout") == 0) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint16_t value;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
value = (uint16_t)getU32(tok, 1, 0xFFFF, &err);
if (err) {
break;
}
gtwa->SDOtimeoutTime = value;
responseWithOK(gtwa);
}
/* 'set sdo_timeout <0|1>' */
else if (strcmp(tok, "sdo_block") == 0) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint16_t value;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
value = (uint16_t)getU32(tok, 0, 1, &err);
if (err) {
break;
}
gtwa->SDOblockTransferEnable = (value == 1U) ? true : false;
responseWithOK(gtwa);
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
else {
respErrorCode = CO_GTWA_respErrorReqNotSupported;
err = true;
break;
}
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
/* Upload SDO command - 'r[ead] <index> <subindex> <datatype>' */
else if (tok_is_read) {
uint16_t idx;
uint8_t subidx;
CO_SDO_return_t SDO_ret;
bool_t NodeErr = checkNetNode(gtwa, net, node, 1, &respErrorCode);
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* index */
closed = 0U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
idx = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
if (err) {
break;
}
/* subindex */
closed = 0xFFU;
n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
subidx = (uint8_t)getU32(tok, 0, 0xFF, &err);
if (err || (n == 0U)) {
err = true;
break;
}
/* optional data type */
if (closed == 0U) {
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
convertToLower(tok, sizeof(tok));
gtwa->SDOdataType = CO_GTWA_getDataType(tok, &err);
if (err) {
break;
}
} else {
gtwa->SDOdataType = &dataTypes[0]; /* use generic data type */
}
/* setup client */
SDO_ret = CO_SDOclient_setup(gtwa->SDO_C, (uint32_t)CO_CAN_ID_SDO_CLI + gtwa->node,
(uint32_t)CO_CAN_ID_SDO_SRV + gtwa->node, gtwa->node);
if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
/* initiate upload */
SDO_ret = CO_SDOclientUploadInitiate(gtwa->SDO_C, idx, subidx, gtwa->SDOtimeoutTime,
gtwa->SDOblockTransferEnable);
if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
/* indicate that gateway response didn't start yet */
gtwa->SDOdataCopyStatus = false;
/* continue with state machine */
timeDifference_us = 0;
gtwa->state = CO_GTWA_ST_READ;
}
/* Download SDO comm. - w[rite] <index> <subindex> <datatype> <value> */
else if (tok_is_write) {
uint16_t idx;
uint8_t subidx;
uint8_t status;
CO_SDO_return_t SDO_ret;
size_t size;
bool_t NodeErr = checkNetNode(gtwa, net, node, 1, &respErrorCode);
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* index */
closed = 0U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
idx = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
if (err) {
break;
}
/* subindex */
closed = 0U;
n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
subidx = (uint8_t)getU32(tok, 0, 0xFF, &err);
if (err) {
break;
}
/* data type */
closed = 0U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
convertToLower(tok, sizeof(tok));
gtwa->SDOdataType = CO_GTWA_getDataType(tok, &err);
if (err) {
break;
}
/* setup client */
SDO_ret = CO_SDOclient_setup(gtwa->SDO_C, (uint32_t)CO_CAN_ID_SDO_CLI + gtwa->node,
(uint32_t)CO_CAN_ID_SDO_SRV + gtwa->node, gtwa->node);
if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
/* initiate download */
SDO_ret = CO_SDOclientDownloadInitiate(gtwa->SDO_C, idx, subidx, gtwa->SDOdataType->length,
gtwa->SDOtimeoutTime, gtwa->SDOblockTransferEnable);
if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
/* copy data from comm to the SDO buffer, according to data type */
size = gtwa->SDOdataType->dataTypeScan(&gtwa->SDO_C->bufFifo, &gtwa->commFifo, &status);
/* set to true, if command delimiter was found */
closed = ((status & CO_fifo_st_closed) == 0U) ? 0U : 1U;
/* set to true, if data are copied only partially */
gtwa->SDOdataCopyStatus = (status & CO_fifo_st_partial) != 0U;
/* is syntax error in command or size is zero or not the last token
* in command */
if (((status & CO_fifo_st_errMask) != 0U) || (size == 0U)
|| ((gtwa->SDOdataCopyStatus == false) && (closed != 1U))) {
err = true;
break;
}
/* if data size was not known before and is known now, update SDO */
if ((gtwa->SDOdataType->length == 0U) && !gtwa->SDOdataCopyStatus) {
CO_SDOclientDownloadInitSize(gtwa->SDO_C, size);
}
/* continue with state machine */
gtwa->stateTimeoutTmr = 0;
timeDifference_us = 0;
gtwa->state = CO_GTWA_ST_WRITE;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_NMT) != 0
/* NMT start node - 'start' */
else if (tok_is_start) {
CO_ReturnError_t ret;
bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
CO_NMT_command_t command2 = CO_NMT_ENTER_OPERATIONAL;
if ((closed != 1U) || NodeErr) {
err = true;
break;
}
ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
if (ret == CO_ERROR_NO) {
responseWithOK(gtwa);
} else {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
}
/* NMT stop node - 'stop' */
else if (tok_is_stop) {
CO_ReturnError_t ret;
bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
CO_NMT_command_t command2 = CO_NMT_ENTER_STOPPED;
if ((closed != 1U) || NodeErr) {
err = true;
break;
}
ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
if (ret == CO_ERROR_NO) {
responseWithOK(gtwa);
} else {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
}
/* NMT Set node to pre-operational - 'preop[erational]' */
else if (tok_is_preop) {
CO_ReturnError_t ret;
bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
CO_NMT_command_t command2 = CO_NMT_ENTER_PRE_OPERATIONAL;
if ((closed != 1U) || NodeErr) {
err = true;
break;
}
ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
if (ret == CO_ERROR_NO) {
responseWithOK(gtwa);
} else {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
}
/* NMT reset (node or communication) - 'reset <node|comm[unication]>' */
else if (tok_is_reset) {
CO_ReturnError_t ret;
bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
CO_NMT_command_t command2;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* command 2 */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
if (err) {
break;
}
convertToLower(tok, sizeof(tok));
if (strcmp(tok, "node") == 0) {
command2 = CO_NMT_RESET_NODE;
} else if (strcmp(tok, "comm") == 0) {
command2 = CO_NMT_RESET_COMMUNICATION;
} else if (strcmp(tok, "communication") == 0) {
command2 = CO_NMT_RESET_COMMUNICATION;
} else {
err = true;
break;
}
ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
if (ret == CO_ERROR_NO) {
responseWithOK(gtwa);
} else {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LSS) != 0
/* Switch state global command - 'lss_switch_glob <0|1>' */
else if (tok_is_lss_switch_glob) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint8_t select;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* get value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
select = (uint8_t)getU32(tok, 0, 1, &err);
if (err) {
break;
}
if (select == 0U) {
/* send non-confirmed message */
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_swStateDeselect(gtwa->LSSmaster);
if (ret == CO_LSSmaster_OK) {
responseWithOK(gtwa);
} else {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
} else {
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_SWITCH_GLOB;
}
}
/* Switch state selective command -
* 'lss_switch_sel <vendorID> <product code> <revisionNo> <serialNo>' */
else if (tok_is_lss_switch_sel) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
CO_LSS_address_t* addr = &gtwa->lssAddress;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* get values */
closed = 0U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
addr->identity.vendorID = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
addr->identity.productCode = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
addr->identity.revisionNumber = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
addr->identity.serialNumber = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_SWITCH_SEL;
}
/* LSS configure node-ID command - 'lss_set_node <node>' */
else if (tok_is_lss_set_node) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* get value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
gtwa->lssNID = (uint8_t)getU32(tok, 0, 0xFF, &err);
if ((gtwa->lssNID > 0x7FU) && (gtwa->lssNID < 0xFFU)) {
err = true;
}
if (err) {
break;
}
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_SET_NODE;
}
/* LSS configure bit-rate command -
* 'lss_conf_bitrate <table_selector=0> <table_index>'
* table_index: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,
* 4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto */
else if (tok_is_lss_conf_bitrate) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint8_t tableIndex;
uint32_t maxIndex = (sizeof(CO_LSS_bitTimingTableLookup) / sizeof(CO_LSS_bitTimingTableLookup[0])) - 1U;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* First parameter is table selector. We only support the CiA bit timing table from CiA301 ("0") */
closed = 0U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
(void)getU32(tok, 0, 0, &err);
/* get value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
tableIndex = (uint8_t)getU32(tok, 0, maxIndex, &err);
if (tableIndex == 5U) {
err = true;
}
if (err) {
break;
}
gtwa->lssBitrate = CO_LSS_bitTimingTableLookup[tableIndex];
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_CONF_BITRATE;
}
/* LSS activate new bit-rate command -
* 'lss_activate_bitrate <switch_delay_ms>' */
else if (tok_is_lss_activate_bitrate) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint16_t switchDelay;
CO_LSSmaster_return_t ret;
if ((closed != 0U) || NodeErr) {
err = true;
break;
}
/* get value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
switchDelay = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
if (err) {
break;
}
/* send non-confirmed message */
ret = CO_LSSmaster_ActivateBit(gtwa->LSSmaster, switchDelay);
if (ret == CO_LSSmaster_OK) {
responseWithOK(gtwa);
} else {
respErrorCode = CO_GTWA_respErrorInternalState;
err = true;
break;
}
}
/* LSS store configuration command - 'lss_store' */
else if (tok_is_lss_store) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
if ((closed != 1U) || NodeErr) {
err = true;
break;
}
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_STORE;
}
/* Inquire LSS address command - 'lss_inquire_addr [<LSSSUB=0..3>]' */
else if (tok_is_lss_inquire_addr) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
if (NodeErr) {
err = true;
break;
}
if (closed == 0U) {
uint8_t lsssub;
/* get value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
lsssub = (uint8_t)getU32(tok, 0, 3, &err);
if (err) {
break;
}
switch (lsssub) {
case 0: gtwa->lssInquireCs = CO_LSS_INQUIRE_VENDOR; break;
case 1: gtwa->lssInquireCs = CO_LSS_INQUIRE_PRODUCT; break;
case 2: gtwa->lssInquireCs = CO_LSS_INQUIRE_REV; break;
default: gtwa->lssInquireCs = CO_LSS_INQUIRE_SERIAL; break;
}
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_INQUIRE;
} else {
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL;
}
}
/* LSS inquire node-ID command - 'lss_get_node' */
else if (tok_is_lss_get_node) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
if ((closed != 1U) || NodeErr) {
err = true;
break;
}
/* continue with state machine */
gtwa->lssInquireCs = CO_LSS_INQUIRE_NODE_ID;
gtwa->state = CO_GTWA_ST_LSS_INQUIRE;
}
/* LSS identify fastscan. This is a manufacturer specific command as
* the one in DSP309 is quite useless - '_lss_fastscan [<timeout_ms>]' */
else if (tok_is__lss_fastscan) {
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint16_t timeout_ms = 0;
if (NodeErr) {
err = true;
break;
}
if (closed == 0U) {
/* get value */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
timeout_ms = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
if (err) {
break;
}
}
/* If timeout not specified, use 100ms. Should work in most cases */
if (timeout_ms == 0U) {
timeout_ms = 100;
}
CO_LSSmaster_changeTimeout(gtwa->LSSmaster, timeout_ms);
/* prepare lssFastscan, all zero */
(void)memset(&gtwa->lssFastscan, 0, sizeof(gtwa->lssFastscan));
/* continue with state machine */
gtwa->state = CO_GTWA_ST__LSS_FASTSCAN;
}
/* LSS complete node-ID configuration command - 'lss_allnodes
* [<timeout_ms> [<nodeStart=1..127> <store=0|1>
* <scanType0=0..2> <vendorId> <scanType1=0..2> <productCode>
* <scanType2=0..2> <revisionNo> <scanType3=0..2> <serialNo>]]' */
else if (tok_is_lss_allnodes) {
/* Request node enumeration by LSS identify fastscan. This initiates node enumeration
* by the means of LSS fastscan mechanism. When this function is finished:
* - All nodes that match the given criteria are assigned a node ID beginning with nodeId.
* If 127 is reached, the process is stopped, no matter if there are nodes remaining or not.
* - No IDs are assigned because:
* - the given criteria do not match any node,
* - all nodes are already configured.
* This function needs that no node is selected when starting the scan process. */
bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
uint16_t timeout_ms = 0;
if (NodeErr) {
err = true;
break;
}
if (closed == 0U) {
/* get optional token timeout (non standard) */
closed = 0xFFU;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
timeout_ms = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
if (err) {
break;
}
}
/* If timeout not specified, use 100ms. Should work in most cases */
gtwa->lssTimeout_ms = (timeout_ms == 0U) ? 100U : timeout_ms;
CO_LSSmaster_changeTimeout(gtwa->LSSmaster, gtwa->lssTimeout_ms);
gtwa->lssNodeCount = 0;
gtwa->lssSubState = 0;
if (closed == 1U) {
/* No other arguments, as by CiA specification for this command. Do full scan. */
/* use start node ID 2. Should work in most cases */
gtwa->lssNID = 2;
/* store node ID in node's NVM */
gtwa->lssStore = true;
/* prepare lssFastscan, all zero */
(void)memset(&gtwa->lssFastscan, 0, sizeof(gtwa->lssFastscan));
}
if (closed == 0U) {
/* more arguments follow */
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
gtwa->lssNID = (uint8_t)getU32(tok, 1, 127, &err);
if (err) {
break;
}
closed = 0xFFU;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
gtwa->lssStore = (bool_t)getU32(tok, 0, 1, &err);
if (err) {
break;
}
if (closed == 1U) {
/* No other arguments, prepare lssFastscan, all zero */
(void)memset(&gtwa->lssFastscan, 0, sizeof(gtwa->lssFastscan));
}
}
if (closed == 0U) {
/* more arguments follow */
CO_LSSmaster_fastscan_t* fs = &gtwa->lssFastscan;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->scan[CO_LSS_FASTSCAN_VENDOR_ID] = (CO_LSSmaster_scantype_t)getU32(tok, 0, 2, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->match.identity.vendorID = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->scan[CO_LSS_FASTSCAN_PRODUCT] = (CO_LSSmaster_scantype_t)getU32(tok, 0, 2, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->match.identity.productCode = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->scan[CO_LSS_FASTSCAN_REV] = (CO_LSSmaster_scantype_t)getU32(tok, 0, 2, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->match.identity.revisionNumber = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->scan[CO_LSS_FASTSCAN_SERIAL] = (CO_LSSmaster_scantype_t)getU32(tok, 0, 2, &err);
if (err) {
break;
}
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
fs->match.identity.serialNumber = getU32(tok, 0, 0xFFFFFFFFU, &err);
if (err) {
break;
}
}
/* continue with state machine */
gtwa->state = CO_GTWA_ST_LSS_ALLNODES;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LOG) != 0
/* Print message log */
else if (tok_is_log) {
if (closed == 0U) {
err = true;
break;
}
gtwa->state = CO_GTWA_ST_LOG;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_HELP) != 0
/* Print help */
else if (tok_is_help) {
if (closed == 1U) {
gtwa->helpString = CO_GTWA_helpString;
} else {
/* get second token */
closed = 1U;
(void)CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
if (err) {
break;
}
convertToLower(tok, sizeof(tok));
if (strcmp(tok, "datatype") == 0) {
gtwa->helpString = CO_GTWA_helpStringDatatypes;
} else if (strcmp(tok, "lss") == 0) {
gtwa->helpString = CO_GTWA_helpStringLss;
} else {
err = true;
break;
}
}
/* continue with state machine */
gtwa->helpStringOffset = 0;
gtwa->state = CO_GTWA_ST_HELP;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_LEDS) != 0
/* Print status led diodes */
else if (tok_is_led) {
if (closed == 0U) {
err = true;
break;
}
gtwa->ledStringPreviousIndex = 0xFF;
gtwa->state = CO_GTWA_ST_LED;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS */
/* Unrecognized command */
else {
respErrorCode = CO_GTWA_respErrorReqNotSupported;
err = true;
break;
}
} /* while CO_GTWA_ST_IDLE && CO_fifo_CommSearch */
/***************************************************************************
* STATE MACHINE
***************************************************************************/
/* If error, generate error response */
if (err) {
if (respErrorCode == CO_GTWA_respErrorNone) {
respErrorCode = CO_GTWA_respErrorSyntax;
}
responseWithError(gtwa, respErrorCode);
/* delete command, if it was only partially read */
if (closed == 0U) {
(void)CO_fifo_CommSearch(&gtwa->commFifo, true);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
else {
switch (gtwa->state) {
case CO_GTWA_ST_IDLE: {
return; /* skip timerNext_us calculation */
break;
}
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_SDO) != 0
/* SDO upload state */
case CO_GTWA_ST_READ: {
CO_SDO_abortCode_t abortCode;
size_t sizeTransferred;
CO_SDO_return_t ret;
ret = CO_SDOclientUpload(gtwa->SDO_C, timeDifference_us, false, &abortCode, NULL, &sizeTransferred,
timerNext_us);
if (ret < CO_SDO_RT_ok_communicationEnd) {
responseWithErrorSDO(gtwa, abortCode, gtwa->SDOdataCopyStatus);
gtwa->state = CO_GTWA_ST_IDLE;
}
/* Response data must be read, partially or whole */
else if ((ret == CO_SDO_RT_uploadDataBufferFull) || (ret == CO_SDO_RT_ok_communicationEnd)) {
size_t fifoRemain;
/* write response head first */
if (!gtwa->SDOdataCopyStatus) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE - 2U,
"[%" PRId32 "] ", (int32_t)gtwa->sequence);
gtwa->SDOdataCopyStatus = true;
}
/* Empty SDO fifo buffer in multiple cycles. Repeat until
* application runs out of space (respHold) or fifo empty. */
do {
/* read SDO fifo (partially) and print specific data type as
* ascii into intermediate respBuf */
gtwa->respBufCount += gtwa->SDOdataType->dataTypePrint(
&gtwa->SDO_C->bufFifo, &gtwa->respBuf[gtwa->respBufCount],
(CO_GTWA_RESP_BUF_SIZE - 2U) - gtwa->respBufCount, ret == CO_SDO_RT_ok_communicationEnd);
fifoRemain = CO_fifo_getOccupied(&gtwa->SDO_C->bufFifo);
/* end of communication, print newline and enter idle state */
if ((ret == CO_SDO_RT_ok_communicationEnd) && (fifoRemain == 0U)) {
gtwa->respBufCount += (size_t)sprintf(&gtwa->respBuf[gtwa->respBufCount], "\r\n");
gtwa->state = CO_GTWA_ST_IDLE;
}
/* transfer response to the application */
if (respBufTransfer(gtwa) == false) {
/* broken communication, send SDO abort and force finish. */
abortCode = CO_SDO_AB_DATA_TRANSF;
(void)CO_SDOclientUpload(gtwa->SDO_C, 0, true, &abortCode, NULL, NULL, NULL);
gtwa->state = CO_GTWA_ST_IDLE;
break;
}
} while ((gtwa->respHold == false) && (fifoRemain > 0U));
} else { /* MISRA C 2004 14.10 */
}
break;
}
/* SDO download state */
case CO_GTWA_ST_WRITE:
case CO_GTWA_ST_WRITE_ABORTED: {
CO_SDO_abortCode_t abortCode;
size_t sizeTransferred;
bool_t abort_comm = false;
bool_t hold = false;
CO_SDO_return_t ret;
/* copy data to the SDO buffer if previous dataTypeScan was partial */
if (gtwa->SDOdataCopyStatus) {
uint8_t status;
gtwa->SDOdataType->dataTypeScan(&gtwa->SDO_C->bufFifo, &gtwa->commFifo, &status);
/* set to true, if command delimiter was found */
closed = ((status & CO_fifo_st_closed) == 0U) ? 0U : 1U;
/* set to true, if data are copied only partially */
gtwa->SDOdataCopyStatus = (status & CO_fifo_st_partial) != 0U;
/* is syntax error in command or not the last token in command */
if (((status & CO_fifo_st_errMask) != 0U)
|| ((gtwa->SDOdataCopyStatus == false) && (closed != 1U))) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
abort_comm = true; /* abort SDO communication */
/* clear the rest of the command, if necessary */
if (closed != 1U) {
(void)CO_fifo_CommSearch(&gtwa->commFifo, true);
}
}
if (gtwa->state == CO_GTWA_ST_WRITE_ABORTED) {
/* Stay in this state, until all data transferred via commFifo will be purged. */
if (!CO_fifo_purge(&gtwa->SDO_C->bufFifo) || (closed == 1U)) {
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
}
/* If not all data were transferred, make sure, there is enough data in
* SDO buffer, to continue communication. Otherwise wait and check for timeout */
if (gtwa->SDOdataCopyStatus) {
if (CO_fifo_getOccupied(&gtwa->SDO_C->bufFifo) < (CO_CONFIG_GTW_BLOCK_DL_LOOP * 7U)) {
if (gtwa->stateTimeoutTmr > CO_GTWA_STATE_TIMEOUT_TIME_US) {
abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
abort_comm = true;
} else {
gtwa->stateTimeoutTmr += timeDifference_us;
hold = true;
}
}
}
if (!hold || abort_comm) {
/* if OS has CANtx queue, speedup block transfer */
uint32_t loop = 0;
do {
ret = CO_SDOclientDownload(gtwa->SDO_C, timeDifference_us, abort_comm, gtwa->SDOdataCopyStatus,
&abortCode, &sizeTransferred, timerNext_us);
if (++loop >= CO_CONFIG_GTW_BLOCK_DL_LOOP) {
break;
}
} while (ret == CO_SDO_RT_blockDownldInProgress);
/* send response in case of error or finish */
if (ret < CO_SDO_RT_ok_communicationEnd) {
responseWithErrorSDO(gtwa, abortCode, false);
/* purge remaining data if necessary */
gtwa->state = gtwa->SDOdataCopyStatus ? CO_GTWA_ST_WRITE_ABORTED : CO_GTWA_ST_IDLE;
} else if (ret == CO_SDO_RT_ok_communicationEnd) {
responseWithOK(gtwa);
gtwa->state = CO_GTWA_ST_IDLE;
} else { /* MISRA C 2004 14.10 */
}
}
break;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LSS) != 0
case CO_GTWA_ST_LSS_SWITCH_GLOB: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_swStateSelect(gtwa->LSSmaster, timeDifference_us, NULL);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
responseLSS(gtwa, ret);
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST_LSS_SWITCH_SEL: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_swStateSelect(gtwa->LSSmaster, timeDifference_us, &gtwa->lssAddress);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
responseLSS(gtwa, ret);
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST_LSS_SET_NODE: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_configureNodeId(gtwa->LSSmaster, timeDifference_us, gtwa->lssNID);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
respErrorCode = CO_GTWA_respErrorLSSnodeIdNotSupported;
responseWithError(gtwa, respErrorCode);
} else {
responseLSS(gtwa, ret);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST_LSS_CONF_BITRATE: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_configureBitTiming(gtwa->LSSmaster, timeDifference_us, gtwa->lssBitrate);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
respErrorCode = CO_GTWA_respErrorLSSbitRateNotSupported;
responseWithError(gtwa, respErrorCode);
} else {
responseLSS(gtwa, ret);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST_LSS_STORE: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_configureStore(gtwa->LSSmaster, timeDifference_us);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
respErrorCode = CO_GTWA_respErrorLSSparameterStoringFailed;
responseWithError(gtwa, respErrorCode);
} else {
responseLSS(gtwa, ret);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST_LSS_INQUIRE: {
CO_LSSmaster_return_t ret;
uint32_t value;
ret = CO_LSSmaster_Inquire(gtwa->LSSmaster, timeDifference_us, gtwa->lssInquireCs, &value);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if (ret == CO_LSSmaster_OK) {
if (gtwa->lssInquireCs == CO_LSS_INQUIRE_NODE_ID) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%" PRId32 "] 0x%02" PRIX32 "\r\n",
(int32_t)gtwa->sequence, value & 0xFFU);
} else {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%" PRId32 "] 0x%08" PRIX32 "\r\n",
(int32_t)gtwa->sequence, value);
}
(void)respBufTransfer(gtwa);
} else {
responseLSS(gtwa, ret);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_InquireLssAddress(gtwa->LSSmaster, timeDifference_us, &gtwa->lssAddress);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if (ret == CO_LSSmaster_OK) {
gtwa->respBufCount = (size_t)snprintf(
gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%" PRId32 "] 0x%08" PRIX32 " 0x%08" PRIX32 " 0x%08" PRIX32 " 0x%08" PRIX32 "\r\n",
(int32_t)gtwa->sequence, gtwa->lssAddress.identity.vendorID,
gtwa->lssAddress.identity.productCode, gtwa->lssAddress.identity.revisionNumber,
gtwa->lssAddress.identity.serialNumber);
(void)respBufTransfer(gtwa);
} else {
responseLSS(gtwa, ret);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST__LSS_FASTSCAN: {
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_IdentifyFastscan(gtwa->LSSmaster, timeDifference_us, &gtwa->lssFastscan);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if ((ret == CO_LSSmaster_OK) || (ret == CO_LSSmaster_SCAN_FINISHED)) {
gtwa->respBufCount = (size_t)snprintf(
gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"[%" PRId32 "] 0x%08" PRIX32 " 0x%08" PRIX32 " 0x%08" PRIX32 " 0x%08" PRIX32 "\r\n",
(int32_t)gtwa->sequence, gtwa->lssFastscan.found.identity.vendorID,
gtwa->lssFastscan.found.identity.productCode,
gtwa->lssFastscan.found.identity.revisionNumber,
gtwa->lssFastscan.found.identity.serialNumber);
(void)respBufTransfer(gtwa);
} else {
responseLSS(gtwa, ret);
}
CO_LSSmaster_changeTimeout(gtwa->LSSmaster, CO_LSSmaster_DEFAULT_TIMEOUT);
gtwa->state = CO_GTWA_ST_IDLE;
}
break;
}
case CO_GTWA_ST_LSS_ALLNODES: {
CO_LSSmaster_return_t ret;
if (gtwa->lssSubState == 0U) { /* _lss_fastscan */
ret = CO_LSSmaster_IdentifyFastscan(gtwa->LSSmaster, timeDifference_us, &gtwa->lssFastscan);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
CO_LSSmaster_changeTimeout(gtwa->LSSmaster, CO_LSSmaster_DEFAULT_TIMEOUT);
if ((ret == CO_LSSmaster_OK) || (ret == CO_LSSmaster_SCAN_NOACK)) {
/* no (more) nodes found, send report sum and finish */
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"# Found %d nodes, search finished.\n"
"[%" PRId32 "] OK\r\n",
gtwa->lssNodeCount, (int32_t)gtwa->sequence);
(void)respBufTransfer(gtwa);
gtwa->state = CO_GTWA_ST_IDLE;
} else if (ret == CO_LSSmaster_SCAN_FINISHED) {
/* next sub-step */
gtwa->lssSubState++;
} else {
/* error occurred */
responseLSS(gtwa, ret);
gtwa->state = CO_GTWA_ST_IDLE;
}
}
}
if (gtwa->lssSubState == 1U) { /* lss_set_node */
ret = CO_LSSmaster_configureNodeId(gtwa->LSSmaster, timeDifference_us, gtwa->lssNID);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if (ret == CO_LSSmaster_OK) {
/* next sub-step */
gtwa->lssSubState += gtwa->lssStore ? 1U : 2U;
} else {
/* error occurred */
if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
respErrorCode = CO_GTWA_respErrorLSSnodeIdNotSupported;
responseWithError(gtwa, respErrorCode);
} else {
responseLSS(gtwa, ret);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
}
}
if (gtwa->lssSubState == 2U) { /* lss_store */
ret = CO_LSSmaster_configureStore(gtwa->LSSmaster, timeDifference_us);
if (ret != CO_LSSmaster_WAIT_SLAVE) {
if (ret == CO_LSSmaster_OK) {
/* next sub-step */
gtwa->lssSubState++;
} else {
/* error occurred */
if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
respErrorCode = CO_GTWA_respErrorLSSparameterStoringFailed;
responseWithError(gtwa, respErrorCode);
} else {
responseLSS(gtwa, ret);
}
gtwa->state = CO_GTWA_ST_IDLE;
}
}
}
if (gtwa->lssSubState >= 3U) { /* lss_switch_glob 0 */
/* send non-confirmed message */
ret = CO_LSSmaster_swStateDeselect(gtwa->LSSmaster);
if (ret != CO_LSSmaster_OK) {
/* error occurred */
responseLSS(gtwa, ret);
gtwa->state = CO_GTWA_ST_IDLE;
} else {
/* cycle finished successfully, send report */
uint8_t lssNidAssigned = gtwa->lssNID;
const char msg2Fmt[] = "# Not all nodes scanned!\n"
"[%" PRId32 "] OK\r\n";
char msg2[sizeof(msg2Fmt) + 10U] = {0};
/* increment variables, check end-of-nodeId */
gtwa->lssNodeCount++;
if (gtwa->lssNID < 127U) {
/* repeat cycle with next node-id */
gtwa->lssNID++;
CO_LSSmaster_changeTimeout(gtwa->LSSmaster, gtwa->lssTimeout_ms);
gtwa->lssSubState = 0;
} else {
/* If we can't assign more node IDs, quit scanning */
sprintf(msg2, msg2Fmt, (int32_t)gtwa->sequence);
gtwa->state = CO_GTWA_ST_IDLE;
}
/* send report */
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
"# Node-ID %d assigned to: 0x%08" PRIX32 " 0x%08" PRIX32
" 0x%08" PRIX32 " 0x%08" PRIX32 "\n%s",
lssNidAssigned, gtwa->lssFastscan.found.identity.vendorID,
gtwa->lssFastscan.found.identity.productCode,
gtwa->lssFastscan.found.identity.revisionNumber,
gtwa->lssFastscan.found.identity.serialNumber, msg2);
(void)respBufTransfer(gtwa);
}
}
break;
} /* CO_GTWA_ST_LSS_ALLNODES */
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_LOG) != 0
/* print message log */
case CO_GTWA_ST_LOG: {
CO_fifo_putc_ov(&gtwa->logFifo, '\r');
CO_fifo_putc_ov(&gtwa->logFifo, '\n');
do {
gtwa->respBufCount = CO_fifo_read(&gtwa->logFifo, (uint8_t*)gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
NULL);
(void)respBufTransfer(gtwa);
if (CO_fifo_getOccupied(&gtwa->logFifo) == 0U) {
gtwa->state = CO_GTWA_ST_IDLE;
break;
}
} while (gtwa->respHold == false);
break;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_HELP) != 0
/* Print help string (in multiple segments if necessary) */
case CO_GTWA_ST_HELP: {
size_t lenBuf = CO_GTWA_RESP_BUF_SIZE;
size_t lenHelp = strlen(gtwa->helpString);
do {
size_t lenHelpRemain = lenHelp - gtwa->helpStringOffset;
size_t lenCopied = (lenBuf < lenHelpRemain) ? lenBuf : lenHelpRemain;
(void)memcpy(gtwa->respBuf, &gtwa->helpString[gtwa->helpStringOffset], lenCopied);
gtwa->respBufCount = lenCopied;
gtwa->helpStringOffset += lenCopied;
(void)respBufTransfer(gtwa);
if (gtwa->helpStringOffset == lenHelp) {
gtwa->state = CO_GTWA_ST_IDLE;
break;
}
} while (gtwa->respHold == false);
break;
}
#endif
#if ((CO_CONFIG_GTW)&CO_CONFIG_GTW_ASCII_PRINT_LEDS) != 0
/* print CANopen status LED diodes */
case CO_GTWA_ST_LED: {
uint8_t i;
if (CO_fifo_CommSearch(&gtwa->commFifo, false)) {
gtwa->state = CO_GTWA_ST_IDLE;
i = 4;
} else {
i = (CO_LED_RED(gtwa->LEDs, CO_LED_CANopen) * 2U) + CO_LED_GREEN(gtwa->LEDs, CO_LED_CANopen);
}
if (i > (CO_GTWA_LED_PRINTOUTS_SIZE - 1U)) {
i = CO_GTWA_LED_PRINTOUTS_SIZE - 1U;
}
if (i != gtwa->ledStringPreviousIndex) {
gtwa->respBufCount = (size_t)snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE, "%s",
CO_GTWA_LED_PRINTOUTS[i]);
(void)respBufTransfer(gtwa);
gtwa->ledStringPreviousIndex = i;
}
break;
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS */
/* illegal state */
default: {
respErrorCode = CO_GTWA_respErrorInternalState;
responseWithError(gtwa, respErrorCode);
gtwa->state = CO_GTWA_ST_IDLE;
break;
}
} /* switch (gtwa->state) */
}
/* execute next CANopen processing immediately, if idle and more commands available */
if ((timerNext_us != NULL) && (gtwa->state == CO_GTWA_ST_IDLE)) {
if (CO_fifo_CommSearch(&gtwa->commFifo, false)) {
*timerNext_us = 0;
}
}
}
#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
Reference in New Issue Copy Permalink