Embedded_SDK
/
MindSDK_MM32F5270
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
MindSDK_MM32F5270/components/canopen/CO_LSSmaster.c

1108 lines
37 KiB
C
Raw Blame History

/*
* CANopen LSS Master protocol.
*
* @file CO_LSSmaster.c
* @ingroup CO_LSS
* @author Martin Wagner
* @copyright 2017 - 2020 Neuberger Gebaeudeautomation GmbH
*
*
* This file is part of CANopenNode, an opensource CANopen Stack.
* Project home page is <https://github.com/CANopenNode/CANopenNode>.
* For more information on CANopen see <http://www.can-cia.org/>.
*
* 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 "CANopen.h"
#include "CO_LSSmaster.h"
#if CO_NO_LSS_CLIENT == 1
/*
* LSS master slave select state machine. Compared to #CO_LSS_state_t this
* has information if we currently have selected one or all slaves. This
* allows for some basic error checking.
*/
typedef enum {
CO_LSSmaster_STATE_WAITING = 0,
CO_LSSmaster_STATE_CFG_SLECTIVE,
CO_LSSmaster_STATE_CFG_GLOBAL,
} CO_LSSmaster_state_t;
/*
* LSS master slave command state machine
*/
typedef enum {
CO_LSSmaster_COMMAND_WAITING = 0,
CO_LSSmaster_COMMAND_SWITCH_STATE,
CO_LSSmaster_COMMAND_CFG_BIT_TIMING,
CO_LSSmaster_COMMAND_CFG_NODE_ID,
CO_LSSmaster_COMMAND_CFG_STORE,
CO_LSSmaster_COMMAND_INQUIRE_VENDOR,
CO_LSSmaster_COMMAND_INQUIRE_PRODUCT,
CO_LSSmaster_COMMAND_INQUIRE_REV,
CO_LSSmaster_COMMAND_INQUIRE_SERIAL,
CO_LSSmaster_COMMAND_INQUIRE_NODE_ID,
CO_LSSmaster_COMMAND_IDENTIFY_FASTSCAN,
} CO_LSSmaster_command_t;
/*
* LSS master fastscan state machine
*/
typedef enum {
CO_LSSmaster_FS_STATE_CHECK,
CO_LSSmaster_FS_STATE_SCAN,
CO_LSSmaster_FS_STATE_VERIFY
} CO_LSSmaster_fs_t;
/*
* 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_LSSmaster_receive(void *object, const CO_CANrxMsg_t *msg)
{
CO_LSSmaster_t *LSSmaster;
LSSmaster = (CO_LSSmaster_t*)object; /* this is the correct pointer type of the first argument */
/* verify message length and message overflow (previous message was not processed yet) */
if(msg->DLC==8 && !IS_CANrxNew(LSSmaster->CANrxNew) &&
LSSmaster->command!=CO_LSSmaster_COMMAND_WAITING){
/* copy data and set 'new message' flag */
LSSmaster->CANrxData[0] = msg->data[0];
LSSmaster->CANrxData[1] = msg->data[1];
LSSmaster->CANrxData[2] = msg->data[2];
LSSmaster->CANrxData[3] = msg->data[3];
LSSmaster->CANrxData[4] = msg->data[4];
LSSmaster->CANrxData[5] = msg->data[5];
LSSmaster->CANrxData[6] = msg->data[6];
LSSmaster->CANrxData[7] = msg->data[7];
SET_CANrxNew(LSSmaster->CANrxNew);
/* Optional signal to RTOS, which can resume task, which handles SDO client. */
if(LSSmaster->pFunctSignal != NULL) {
LSSmaster->pFunctSignal(LSSmaster->functSignalObject);
}
}
}
/*
* Check LSS timeout.
*
* Generally, we do not really care if the message has been received before
* or after the timeout expired. Only if no message has been received we have
* to check for timeouts
*/
static CO_LSSmaster_return_t CO_LSSmaster_check_timeout(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_WAIT_SLAVE;
LSSmaster->timeoutTimer += timeDifference_ms;
if (LSSmaster->timeoutTimer >= LSSmaster->timeout) {
LSSmaster->timeoutTimer = 0;
ret = CO_LSSmaster_TIMEOUT;
}
return ret;
}
/******************************************************************************/
CO_ReturnError_t CO_LSSmaster_init(
CO_LSSmaster_t *LSSmaster,
uint16_t timeout_ms,
CO_CANmodule_t *CANdevRx,
uint16_t CANdevRxIdx,
uint32_t CANidLssSlave,
CO_CANmodule_t *CANdevTx,
uint16_t CANdevTxIdx,
uint32_t CANidLssMaster)
{
/* verify arguments */
if (LSSmaster==NULL || CANdevRx==NULL || CANdevTx==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
LSSmaster->timeout = timeout_ms;
LSSmaster->state = CO_LSSmaster_STATE_WAITING;
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
LSSmaster->timeoutTimer = 0;
CLEAR_CANrxNew(LSSmaster->CANrxNew);
CO_memset(LSSmaster->CANrxData, 0, sizeof(LSSmaster->CANrxData));
LSSmaster->pFunctSignal = NULL;
LSSmaster->functSignalObject = NULL;
/* configure LSS CAN Slave response message reception */
CO_CANrxBufferInit(
CANdevRx, /* CAN device */
CANdevRxIdx, /* rx buffer index */
CANidLssSlave, /* CAN identifier */
0x7FF, /* mask */
0, /* rtr */
(void*)LSSmaster, /* object passed to receive function */
CO_LSSmaster_receive);/* this function will process received message */
/* configure LSS CAN Master message transmission */
LSSmaster->CANdevTx = CANdevTx;
LSSmaster->TXbuff = CO_CANtxBufferInit(
CANdevTx, /* CAN device */
CANdevTxIdx, /* index of specific buffer inside CAN module */
CANidLssMaster, /* CAN identifier */
0, /* rtr */
8, /* number of data bytes */
0); /* synchronous message flag bit */
return CO_ERROR_NO;
}
/******************************************************************************/
void CO_LSSmaster_changeTimeout(
CO_LSSmaster_t *LSSmaster,
uint16_t timeout_ms)
{
if (LSSmaster != NULL) {
LSSmaster->timeout = timeout_ms;
}
}
/******************************************************************************/
void CO_LSSmaster_initCallback(
CO_LSSmaster_t *LSSmaster,
void *object,
void (*pFunctSignal)(void *object))
{
if(LSSmaster != NULL){
LSSmaster->functSignalObject = object;
LSSmaster->pFunctSignal = pFunctSignal;
}
}
/*
* Helper function - initiate switch state
*/
static CO_LSSmaster_return_t CO_LSSmaster_switchStateSelectInitiate(
CO_LSSmaster_t *LSSmaster,
CO_LSS_address_t *lssAddress)
{
CO_LSSmaster_return_t ret;
if (lssAddress != NULL) {
/* switch state select specific using LSS address */
LSSmaster->state = CO_LSSmaster_STATE_CFG_SLECTIVE;
LSSmaster->command = CO_LSSmaster_COMMAND_SWITCH_STATE;
LSSmaster->timeoutTimer = 0;
CLEAR_CANrxNew(LSSmaster->CANrxNew);
CO_memset(&LSSmaster->TXbuff->data[6], 0, 3);
LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_VENDOR;
CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.vendorID);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_PRODUCT;
CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.productCode);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_REV;
CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.revisionNumber);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_SERIAL;
CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.serialNumber);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
ret = CO_LSSmaster_WAIT_SLAVE;
}
else {
/* switch state global */
LSSmaster->state = CO_LSSmaster_STATE_CFG_GLOBAL;
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_GLOBAL;
LSSmaster->TXbuff->data[1] = CO_LSS_STATE_CONFIGURATION;
CO_memset(&LSSmaster->TXbuff->data[2], 0, 6);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
/* This is non-confirmed service! */
ret = CO_LSSmaster_OK;
}
return ret;
}
/*
* Helper function - wait for confirmation
*/
static CO_LSSmaster_return_t CO_LSSmaster_switchStateSelectWait(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms)
{
CO_LSSmaster_return_t ret;
if (IS_CANrxNew(LSSmaster->CANrxNew)) {
uint8_t cs = LSSmaster->CANrxData[0];
CLEAR_CANrxNew(LSSmaster->CANrxNew);
if (cs == CO_LSS_SWITCH_STATE_SEL) {
/* confirmation received */
ret = CO_LSSmaster_OK;
}
else {
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
}
}
else {
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
CO_LSS_address_t *lssAddress)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
if (LSSmaster == NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* Initiate select */
if (LSSmaster->state==CO_LSSmaster_STATE_WAITING &&
LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
ret = CO_LSSmaster_switchStateSelectInitiate(LSSmaster, lssAddress);
}
/* Wait for confirmation */
else if (LSSmaster->command == CO_LSSmaster_COMMAND_SWITCH_STATE) {
ret = CO_LSSmaster_switchStateSelectWait(LSSmaster, timeDifference_ms);
}
if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
/* finished */
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
if (ret < CO_LSSmaster_OK) {
/* switching failed, go back to waiting */
LSSmaster->state=CO_LSSmaster_STATE_WAITING;
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect(
CO_LSSmaster_t *LSSmaster)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
if (LSSmaster == NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* We can always send this command to get into a clean state on the network.
* If no slave is selected, this command is ignored. */
LSSmaster->state = CO_LSSmaster_STATE_WAITING;
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
LSSmaster->timeoutTimer = 0;
/* switch state global */
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_GLOBAL;
LSSmaster->TXbuff->data[1] = CO_LSS_STATE_WAITING;
CO_memset(&LSSmaster->TXbuff->data[2], 0, 6);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
/* This is non-confirmed service! */
ret = CO_LSSmaster_OK;
return ret;
}
/*
* Helper function - wait for confirmation, check for returned error code
*
* This uses the nature of the configure confirmation message design:
* - byte 0 -> cs
* - byte 1 -> Error Code, where
* - 0 = OK
* - 1 .. FE = Values defined by CiA. All currently defined values
* are slave rejects. No further distinction on why the
* slave did reject the request.
* - FF = Manufacturer Error Code in byte 2
* - byte 2 -> Manufacturer Error, currently not used
*
* enums for the errorCode are
* - CO_LSS_cfgNodeId_t
* - CO_LSS_cfgBitTiming_t
* - CO_LSS_cfgStore_t
*/
static CO_LSSmaster_return_t CO_LSSmaster_configureCheckWait(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
uint8_t csWait)
{
CO_LSSmaster_return_t ret;
if (IS_CANrxNew(LSSmaster->CANrxNew)) {
uint8_t cs = LSSmaster->CANrxData[0];
uint8_t errorCode = LSSmaster->CANrxData[1];
CLEAR_CANrxNew(LSSmaster->CANrxNew);
if (cs == csWait) {
if (errorCode == 0) {
ret = CO_LSSmaster_OK;
}
else if (errorCode == 0xff) {
ret = CO_LSSmaster_OK_MANUFACTURER;
}
else {
ret = CO_LSSmaster_OK_ILLEGAL_ARGUMENT;
}
}
else {
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
}
}
else {
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
}
if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
/* finished */
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
uint16_t bit)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
uint8_t bitTiming;
if (LSSmaster == NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
switch (bit) {
case 1000: bitTiming = CO_LSS_BIT_TIMING_1000; break;
case 800: bitTiming = CO_LSS_BIT_TIMING_800; break;
case 500: bitTiming = CO_LSS_BIT_TIMING_500; break;
case 250: bitTiming = CO_LSS_BIT_TIMING_250; break;
case 125: bitTiming = CO_LSS_BIT_TIMING_125; break;
case 50: bitTiming = CO_LSS_BIT_TIMING_50; break;
case 20: bitTiming = CO_LSS_BIT_TIMING_20; break;
case 10: bitTiming = CO_LSS_BIT_TIMING_10; break;
case 0: bitTiming = CO_LSS_BIT_TIMING_AUTO; break;
default: return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* Initiate config bit */
if (LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE &&
LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
LSSmaster->command = CO_LSSmaster_COMMAND_CFG_BIT_TIMING;
LSSmaster->timeoutTimer = 0;
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = CO_LSS_CFG_BIT_TIMING;
LSSmaster->TXbuff->data[1] = 0;
LSSmaster->TXbuff->data[2] = bitTiming;
CO_memset(&LSSmaster->TXbuff->data[3], 0, 5);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
ret = CO_LSSmaster_WAIT_SLAVE;
}
/* Wait for confirmation */
else if (LSSmaster->command == CO_LSSmaster_COMMAND_CFG_BIT_TIMING) {
ret = CO_LSSmaster_configureCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_CFG_BIT_TIMING);
}
if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
/* finished */
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_configureNodeId(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
uint8_t nodeId)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
if (LSSmaster==NULL || !CO_LSS_NODE_ID_VALID(nodeId)){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* Initiate config node ID */
if ((LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE ||
/* Let un-config node ID also be run in global mode for unconfiguring all nodes */
(LSSmaster->state==CO_LSSmaster_STATE_CFG_GLOBAL &&
nodeId == CO_LSS_NODE_ID_ASSIGNMENT)) &&
LSSmaster->command==CO_LSSmaster_COMMAND_WAITING) {
LSSmaster->command = CO_LSSmaster_COMMAND_CFG_NODE_ID;
LSSmaster->timeoutTimer = 0;
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = CO_LSS_CFG_NODE_ID;
LSSmaster->TXbuff->data[1] = nodeId;
CO_memset(&LSSmaster->TXbuff->data[2], 0, 6);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
ret = CO_LSSmaster_WAIT_SLAVE;
}
/* Wait for confirmation */
else if (LSSmaster->command == CO_LSSmaster_COMMAND_CFG_NODE_ID) {
ret = CO_LSSmaster_configureCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_CFG_NODE_ID);
}
if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
/* finished */
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_configureStore(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
if (LSSmaster == NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* Initiate config store */
if (LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE &&
LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
LSSmaster->command = CO_LSSmaster_COMMAND_CFG_STORE;
LSSmaster->timeoutTimer = 0;
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = CO_LSS_CFG_STORE;
CO_memset(&LSSmaster->TXbuff->data[1], 0, 7);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
ret = CO_LSSmaster_WAIT_SLAVE;
}
/* Wait for confirmation */
else if (LSSmaster->command == CO_LSSmaster_COMMAND_CFG_STORE) {
ret = CO_LSSmaster_configureCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_CFG_STORE);
}
if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
/* finished */
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_ActivateBit(
CO_LSSmaster_t *LSSmaster,
uint16_t switchDelay_ms)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
if (LSSmaster == NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* for activating bit timing, we need to have all slaves set to config
* state. This check makes it a bit harder to shoot ourselves in the foot */
if (LSSmaster->state==CO_LSSmaster_STATE_CFG_GLOBAL &&
LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = CO_LSS_CFG_ACTIVATE_BIT_TIMING;
CO_setUint16(&LSSmaster->TXbuff->data[1], switchDelay_ms);
CO_memset(&LSSmaster->TXbuff->data[3], 0, 5);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
/* This is non-confirmed service! */
ret = CO_LSSmaster_OK;
}
return ret;
}
/*
* Helper function - send request
*/
static CO_LSSmaster_return_t CO_LSSmaster_inquireInitiate(
CO_LSSmaster_t *LSSmaster,
uint8_t cs)
{
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = cs;
CO_memset(&LSSmaster->TXbuff->data[1], 0, 7);
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
return CO_LSSmaster_WAIT_SLAVE;
}
/*
* Helper function - wait for confirmation
*/
static CO_LSSmaster_return_t CO_LSSmaster_inquireCheckWait(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
uint8_t csWait,
uint32_t *value)
{
CO_LSSmaster_return_t ret;
if (IS_CANrxNew(LSSmaster->CANrxNew)) {
uint8_t cs = LSSmaster->CANrxData[0];
*value = CO_getUint32(&LSSmaster->CANrxData[1]);
CLEAR_CANrxNew(LSSmaster->CANrxNew);
if (cs == csWait) {
ret = CO_LSSmaster_OK;
}
else {
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
}
}
else {
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
CO_LSS_address_t *lssAddress)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
CO_LSSmaster_command_t next = CO_LSSmaster_COMMAND_WAITING;
if (LSSmaster==NULL || lssAddress==NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* Check for reply */
if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_VENDOR) {
ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_INQUIRE_VENDOR, &lssAddress->identity.vendorID);
if (ret == CO_LSSmaster_OK) {
/* Start next request */
next = CO_LSSmaster_COMMAND_INQUIRE_PRODUCT;
ret = CO_LSSmaster_WAIT_SLAVE;
}
}
else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_PRODUCT) {
ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_INQUIRE_PRODUCT, &lssAddress->identity.productCode);
if (ret == CO_LSSmaster_OK) {
/* Start next request */
next = CO_LSSmaster_COMMAND_INQUIRE_REV;
ret = CO_LSSmaster_WAIT_SLAVE;
}
}
else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_REV) {
ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_INQUIRE_REV, &lssAddress->identity.revisionNumber);
if (ret == CO_LSSmaster_OK) {
/* Start next request */
next = CO_LSSmaster_COMMAND_INQUIRE_SERIAL;
ret = CO_LSSmaster_WAIT_SLAVE;
}
}
else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_SERIAL) {
ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_INQUIRE_SERIAL, &lssAddress->identity.serialNumber);
}
/* Check for next request */
if (LSSmaster->state == CO_LSSmaster_STATE_CFG_SLECTIVE ||
LSSmaster->state == CO_LSSmaster_STATE_CFG_GLOBAL) {
if (LSSmaster->command == CO_LSSmaster_COMMAND_WAITING) {
LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_VENDOR;
LSSmaster->timeoutTimer = 0;
ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_VENDOR);
}
else if (next == CO_LSSmaster_COMMAND_INQUIRE_PRODUCT) {
LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_PRODUCT;
LSSmaster->timeoutTimer = 0;
ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_PRODUCT);
}
else if (next == CO_LSSmaster_COMMAND_INQUIRE_REV) {
LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_REV;
LSSmaster->timeoutTimer = 0;
ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_REV);
}
else if (next == CO_LSSmaster_COMMAND_INQUIRE_SERIAL) {
LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_SERIAL;
LSSmaster->timeoutTimer = 0;
ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_SERIAL);
}
}
if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
/* finished */
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_InquireNodeId(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
uint8_t *nodeId)
{
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
if (LSSmaster==NULL || nodeId==NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
/* send request */
if ((LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE ||
LSSmaster->state==CO_LSSmaster_STATE_CFG_GLOBAL) &&
LSSmaster->command == CO_LSSmaster_COMMAND_WAITING) {
LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_NODE_ID;
LSSmaster->timeoutTimer = 0;
ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_NODE_ID);
}
/* Check for reply */
else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_NODE_ID) {
uint32_t tmp = 0;
ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_ms,
CO_LSS_INQUIRE_NODE_ID, &tmp);
*nodeId = tmp & 0xff;
}
if (ret != CO_LSSmaster_WAIT_SLAVE) {
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
/*
* Helper function - send request
*/
static void CO_LSSmaster_FsSendMsg(
CO_LSSmaster_t *LSSmaster,
uint32_t idNumber,
uint8_t bitCheck,
uint8_t lssSub,
uint8_t lssNext)
{
LSSmaster->timeoutTimer = 0;
CLEAR_CANrxNew(LSSmaster->CANrxNew);
LSSmaster->TXbuff->data[0] = CO_LSS_IDENT_FASTSCAN;
CO_setUint32(&LSSmaster->TXbuff->data[1], idNumber);
LSSmaster->TXbuff->data[5] = bitCheck;
LSSmaster->TXbuff->data[6] = lssSub;
LSSmaster->TXbuff->data[7] = lssNext;
CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
}
/*
* Helper function - wait for confirmation
*/
static CO_LSSmaster_return_t CO_LSSmaster_FsCheckWait(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms)
{
CO_LSSmaster_return_t ret;
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
if (ret == CO_LSSmaster_TIMEOUT) {
ret = CO_LSSmaster_SCAN_NOACK;
if (IS_CANrxNew(LSSmaster->CANrxNew)) {
uint8_t cs = LSSmaster->CANrxData[0];
CLEAR_CANrxNew(LSSmaster->CANrxNew);
if (cs == CO_LSS_IDENT_SLAVE) {
/* At least one node is waiting for fastscan */
ret = CO_LSSmaster_SCAN_FINISHED;
}
}
}
return ret;
}
/*
* Helper function - initiate scan for 32 bit part of LSS address
*/
static CO_LSSmaster_return_t CO_LSSmaster_FsScanInitiate(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
CO_LSSmaster_scantype_t scan,
CO_LSS_fastscan_lss_sub_next lssSub)
{
LSSmaster->fsLssSub = lssSub;
LSSmaster->fsIdNumber = 0;
switch (scan) {
case CO_LSSmaster_FS_SCAN:
break;
case CO_LSSmaster_FS_MATCH:
/* No scanning requested */
return CO_LSSmaster_SCAN_FINISHED;
case CO_LSSmaster_FS_SKIP:
default:
return CO_LSSmaster_SCAN_FAILED;
}
LSSmaster->fsBitChecked = CO_LSS_FASTSCAN_BIT31;
/* trigger scan procedure by sending first message */
CO_LSSmaster_FsSendMsg(LSSmaster, LSSmaster->fsIdNumber,
LSSmaster->fsBitChecked, LSSmaster->fsLssSub, LSSmaster->fsLssSub);
return CO_LSSmaster_WAIT_SLAVE;
}
/*
* Helper function - scan for 32 bits of LSS address, one by one
*/
static CO_LSSmaster_return_t CO_LSSmaster_FsScanWait(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
CO_LSSmaster_scantype_t scan)
{
CO_LSSmaster_return_t ret;
switch (scan) {
case CO_LSSmaster_FS_SCAN:
break;
case CO_LSSmaster_FS_MATCH:
/* No scanning requested */
return CO_LSSmaster_SCAN_FINISHED;
case CO_LSSmaster_FS_SKIP:
default:
return CO_LSSmaster_SCAN_FAILED;
}
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
if (ret == CO_LSSmaster_TIMEOUT) {
ret = CO_LSSmaster_WAIT_SLAVE;
if (IS_CANrxNew(LSSmaster->CANrxNew)) {
uint8_t cs = LSSmaster->CANrxData[0];
CLEAR_CANrxNew(LSSmaster->CANrxNew);
if (cs != CO_LSS_IDENT_SLAVE) {
/* wrong response received. Can not continue */
return CO_LSSmaster_SCAN_FAILED;
}
}
else {
/* no response received, assumption is wrong */
LSSmaster->fsIdNumber |= 1UL << LSSmaster->fsBitChecked;
}
if (LSSmaster->fsBitChecked == CO_LSS_FASTSCAN_BIT0) {
/* Scanning cycle is finished, we now have 32 bit address data */
ret = CO_LSSmaster_SCAN_FINISHED;
}
else {
LSSmaster->fsBitChecked --;
CO_LSSmaster_FsSendMsg(LSSmaster,
LSSmaster->fsIdNumber, LSSmaster->fsBitChecked,
LSSmaster->fsLssSub, LSSmaster->fsLssSub);
}
}
return ret;
}
/*
* Helper function - initiate check for 32 bit part of LSS address
*/
static CO_LSSmaster_return_t CO_LSSmaster_FsVerifyInitiate(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
CO_LSSmaster_scantype_t scan,
uint32_t idNumberCheck,
CO_LSS_fastscan_lss_sub_next lssNext)
{
switch (scan) {
case CO_LSSmaster_FS_SCAN:
/* ID obtained by scan */
break;
case CO_LSSmaster_FS_MATCH:
/* ID given by user */
LSSmaster->fsIdNumber = idNumberCheck;
break;
case CO_LSSmaster_FS_SKIP:
default:
return CO_LSSmaster_SCAN_FAILED;
}
LSSmaster->fsBitChecked = CO_LSS_FASTSCAN_BIT0;
/* send request */
CO_LSSmaster_FsSendMsg(LSSmaster, LSSmaster->fsIdNumber,
LSSmaster->fsBitChecked, LSSmaster->fsLssSub, lssNext);
return CO_LSSmaster_WAIT_SLAVE;
}
/*
* Helper function - verify 32 bit LSS address, request node(s) to switch
* their state machine to the next state
*/
static CO_LSSmaster_return_t CO_LSSmaster_FsVerifyWait(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
CO_LSSmaster_scantype_t scan,
uint32_t *idNumberRet)
{
CO_LSSmaster_return_t ret;
if (scan == CO_LSSmaster_FS_SKIP) {
return CO_LSSmaster_SCAN_FAILED;
}
ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_ms);
if (ret == CO_LSSmaster_TIMEOUT) {
*idNumberRet = 0;
ret = CO_LSSmaster_SCAN_NOACK;
if (IS_CANrxNew(LSSmaster->CANrxNew)) {
uint8_t cs = LSSmaster->CANrxData[0];
CLEAR_CANrxNew(LSSmaster->CANrxNew);
if (cs == CO_LSS_IDENT_SLAVE) {
*idNumberRet = LSSmaster->fsIdNumber;
ret = CO_LSSmaster_SCAN_FINISHED;
} else {
ret = CO_LSSmaster_SCAN_FAILED;
}
}
}
return ret;
}
/*
* Helper function - check which 32 bit to scan for next, if any
*/
static CO_LSS_fastscan_lss_sub_next CO_LSSmaster_FsSearchNext(
CO_LSSmaster_t *LSSmaster,
const CO_LSSmaster_fastscan_t *fastscan)
{
int i;
/* we search for the next LSS address part to scan for, beginning with the
* one after the current one. If there is none remaining, scanning is
* finished */
for (i = LSSmaster->fsLssSub + 1; i <= CO_LSS_FASTSCAN_SERIAL; i++) {
if (fastscan->scan[i] != CO_LSSmaster_FS_SKIP) {
return (CO_LSS_fastscan_lss_sub_next)i;
}
}
/* node selection is triggered by switching node state machine back
* to initial state */
return CO_LSS_FASTSCAN_VENDOR_ID;
}
/******************************************************************************/
CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan(
CO_LSSmaster_t *LSSmaster,
uint16_t timeDifference_ms,
CO_LSSmaster_fastscan_t *fastscan)
{
uint8_t i;
uint8_t count;
CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
CO_LSS_fastscan_lss_sub_next next;
/* parameter validation */
if (LSSmaster==NULL || fastscan==NULL){
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
if (fastscan->scan[0] == CO_LSSmaster_FS_SKIP) {
/* vendor ID scan cannot be skipped */
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
count = 0;
for (i = 0; i < (sizeof(fastscan->scan) / sizeof(fastscan->scan[0])); i++) {
if (fastscan->scan[i] == CO_LSSmaster_FS_SKIP) {
count ++;
}
if (count > 2) {
/* Node selection needs the Vendor ID and at least one other value */
return CO_LSSmaster_ILLEGAL_ARGUMENT;
}
}
/* state machine validation */
if (LSSmaster->state!=CO_LSSmaster_STATE_WAITING ||
(LSSmaster->command!=CO_LSSmaster_COMMAND_WAITING &&
LSSmaster->command!=CO_LSSmaster_COMMAND_IDENTIFY_FASTSCAN)) {
/* state machine not ready, other command is already processed */
return CO_LSSmaster_INVALID_STATE;
}
/* evaluate LSS state machine */
switch (LSSmaster->command) {
case CO_LSSmaster_COMMAND_WAITING:
/* start fastscan */
LSSmaster->command = CO_LSSmaster_COMMAND_IDENTIFY_FASTSCAN;
/* check if any nodes are waiting, if yes fastscan is reset */
LSSmaster->fsState = CO_LSSmaster_FS_STATE_CHECK;
CO_LSSmaster_FsSendMsg(LSSmaster, 0, CO_LSS_FASTSCAN_CONFIRM, 0, 0);
return CO_LSSmaster_WAIT_SLAVE;
default:
/* continue with evaluating fastscan state machine */
break;
}
/* evaluate fastscan state machine. The state machine is evaluated as following
* - check for non-configured nodes
* - scan for vendor ID
* - verify vendor ID, switch node state
* - scan for product code
* - verify product code, switch node state
* - scan for revision number
* - verify revision number, switch node state
* - scan for serial number
* - verify serial number, switch node to LSS configuration mode
* Certain steps can be skipped as mentioned in the function description.
* If one step is not ack'ed by a node, the scanning process is terminated
* and the correspondign error is returned. */
switch (LSSmaster->fsState) {
case CO_LSSmaster_FS_STATE_CHECK:
ret = CO_LSSmaster_FsCheckWait(LSSmaster, timeDifference_ms);
if (ret == CO_LSSmaster_SCAN_FINISHED) {
CO_memset((uint8_t*)&fastscan->found, 0, sizeof(fastscan->found));
/* start scanning procedure by triggering vendor ID scan */
CO_LSSmaster_FsScanInitiate(LSSmaster, timeDifference_ms,
fastscan->scan[CO_LSS_FASTSCAN_VENDOR_ID],
CO_LSS_FASTSCAN_VENDOR_ID);
ret = CO_LSSmaster_WAIT_SLAVE;
LSSmaster->fsState = CO_LSSmaster_FS_STATE_SCAN;
}
break;
case CO_LSSmaster_FS_STATE_SCAN:
ret = CO_LSSmaster_FsScanWait(LSSmaster, timeDifference_ms,
fastscan->scan[LSSmaster->fsLssSub]);
if (ret == CO_LSSmaster_SCAN_FINISHED) {
/* scanning finished, initiate verifcation. The verification
* message also contains the node state machine "switch to
* next state" request */
next = CO_LSSmaster_FsSearchNext(LSSmaster, fastscan);
ret = CO_LSSmaster_FsVerifyInitiate(LSSmaster, timeDifference_ms,
fastscan->scan[LSSmaster->fsLssSub],
fastscan->match.addr[LSSmaster->fsLssSub], next);
LSSmaster->fsState = CO_LSSmaster_FS_STATE_VERIFY;
}
break;
case CO_LSSmaster_FS_STATE_VERIFY:
ret = CO_LSSmaster_FsVerifyWait(LSSmaster, timeDifference_ms,
fastscan->scan[LSSmaster->fsLssSub],
&fastscan->found.addr[LSSmaster->fsLssSub]);
if (ret == CO_LSSmaster_SCAN_FINISHED) {
/* verification successful:
* - assumed node id is correct
* - node state machine has switched to the requested state,
* mirror that in the local copy */
next = CO_LSSmaster_FsSearchNext(LSSmaster, fastscan);
if (next == CO_LSS_FASTSCAN_VENDOR_ID) {
/* fastscan finished, one node is now in LSS configuration
* mode */
LSSmaster->state = CO_LSSmaster_STATE_CFG_SLECTIVE;
}
else {
/* initiate scan for next part of LSS address */
ret = CO_LSSmaster_FsScanInitiate(LSSmaster,
timeDifference_ms, fastscan->scan[next], next);
if (ret == CO_LSSmaster_SCAN_FINISHED) {
/* Scanning is not requested. Initiate verification
* step in next function call */
ret = CO_LSSmaster_WAIT_SLAVE;
}
LSSmaster->fsState = CO_LSSmaster_FS_STATE_SCAN;
}
}
break;
default:
break;
}
if (ret != CO_LSSmaster_WAIT_SLAVE) {
/* finished */
LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
}
return ret;
}
#endif
Reference in New Issue View Git Blame Copy Permalink