1 /*
2  * CANopen LSS Slave protocol.
3  *
4  * @file        CO_LSSslave.c
5  * @ingroup     CO_LSS
6  * @author      Martin Wagner
7  * @copyright   2017 - 2020 Neuberger Gebaeudeautomation GmbH
8  *
9  *
10  * This file is part of CANopenNode, an opensource CANopen Stack.
11  * Project home page is <https://github.com/CANopenNode/CANopenNode>.
12  * For more information on CANopen see <http://www.can-cia.org/>.
13  *
14  * Licensed under the Apache License, Version 2.0 (the "License");
15  * you may not use this file except in compliance with the License.
16  * You may obtain a copy of the License at
17  *
18  *     http://www.apache.org/licenses/LICENSE-2.0
19  *
20  * Unless required by applicable law or agreed to in writing, software
21  * distributed under the License is distributed on an "AS IS" BASIS,
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23  * See the License for the specific language governing permissions and
24  * limitations under the License.
25  */
26 
27 #include "CANopen.h"
28 #include "CO_LSSslave.h"
29 
30 #if CO_NO_LSS_SERVER == 1
31 
32 /*
33  * Helper function - Handle service "switch state global"
34  */
CO_LSSslave_serviceSwitchStateGlobal(CO_LSSslave_t * LSSslave,CO_LSS_cs_t service,const CO_CANrxMsg_t * msg)35 static void CO_LSSslave_serviceSwitchStateGlobal(
36     CO_LSSslave_t *LSSslave,
37     CO_LSS_cs_t service,
38     const CO_CANrxMsg_t *msg)
39 {
40     uint8_t mode = msg->data[1];
41 
42     switch (mode) {
43         case CO_LSS_STATE_WAITING:
44             LSSslave->lssState = CO_LSS_STATE_WAITING;
45             CO_memset((uint8_t*)&LSSslave->lssSelect, 0, sizeof(LSSslave->lssSelect));
46             break;
47         case CO_LSS_STATE_CONFIGURATION:
48             LSSslave->lssState = CO_LSS_STATE_CONFIGURATION;
49             break;
50         default:
51             break;
52     }
53 }
54 
55 /*
56  * Helper function - Handle service "switch state selective"
57  */
CO_LSSslave_serviceSwitchStateSelective(CO_LSSslave_t * LSSslave,CO_LSS_cs_t service,const CO_CANrxMsg_t * msg)58 static void CO_LSSslave_serviceSwitchStateSelective(
59     CO_LSSslave_t *LSSslave,
60     CO_LSS_cs_t service,
61     const CO_CANrxMsg_t *msg)
62 {
63     uint32_t value;
64     CO_memcpySwap4(&value, &msg->data[1]);
65 
66     if(LSSslave->lssState != CO_LSS_STATE_WAITING) {
67         return;
68     }
69 
70     switch (service) {
71         case CO_LSS_SWITCH_STATE_SEL_VENDOR:
72             LSSslave->lssSelect.identity.vendorID = value;
73             break;
74         case CO_LSS_SWITCH_STATE_SEL_PRODUCT:
75             LSSslave->lssSelect.identity.productCode = value;
76             break;
77         case CO_LSS_SWITCH_STATE_SEL_REV:
78             LSSslave->lssSelect.identity.revisionNumber = value;
79             break;
80         case CO_LSS_SWITCH_STATE_SEL_SERIAL:
81             LSSslave->lssSelect.identity.serialNumber = value;
82 
83             if (CO_LSS_ADDRESS_EQUAL(LSSslave->lssAddress, LSSslave->lssSelect)) {
84                 LSSslave->lssState = CO_LSS_STATE_CONFIGURATION;
85 
86                 /* send confirmation */
87                 LSSslave->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL;
88                 CO_memset(&LSSslave->TXbuff->data[1], 0, 7);
89                 CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
90             }
91             break;
92         default:
93             break;
94     }
95 }
96 
97 /*
98  * Helper function - Handle service "configure"
99  *
100  * values inside message have different meaning, depending on the selected
101  * configuration type
102  */
CO_LSSslave_serviceConfig(CO_LSSslave_t * LSSslave,CO_LSS_cs_t service,const CO_CANrxMsg_t * msg)103 static void CO_LSSslave_serviceConfig(
104     CO_LSSslave_t *LSSslave,
105     CO_LSS_cs_t service,
106     const CO_CANrxMsg_t *msg)
107 {
108     uint8_t nid;
109     uint8_t tableSelector;
110     uint8_t tableIndex;
111     uint8_t errorCode;
112 
113     if(LSSslave->lssState != CO_LSS_STATE_CONFIGURATION) {
114         return;
115     }
116 
117     switch (service) {
118         case CO_LSS_CFG_NODE_ID:
119             nid = msg->data[1];
120             errorCode = CO_LSS_CFG_NODE_ID_OK;
121 
122             if (CO_LSS_NODE_ID_VALID(nid)) {
123                 LSSslave->pendingNodeID = nid;
124             }
125             else {
126                 errorCode = CO_LSS_CFG_NODE_ID_OUT_OF_RANGE;
127             }
128 
129             /* send confirmation */
130             LSSslave->TXbuff->data[0] = CO_LSS_CFG_NODE_ID;
131             LSSslave->TXbuff->data[1] = errorCode;
132             /* we do not use spec-error, always 0 */
133             CO_memset(&LSSslave->TXbuff->data[2], 0, 6);
134             CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
135             break;
136         case CO_LSS_CFG_BIT_TIMING:
137             if (LSSslave->pFunctLSScheckBitRate == NULL) {
138                 /* setting bit timing is not supported. Drop request */
139                 break;
140             }
141 
142             tableSelector = msg->data[1];
143             tableIndex = msg->data[2];
144             errorCode = CO_LSS_CFG_BIT_TIMING_OK;
145 
146             if (tableSelector==0 && CO_LSS_BIT_TIMING_VALID(tableIndex)) {
147                 uint16_t bit = CO_LSS_bitTimingTableLookup[tableIndex];
148                 bool_t bit_rate_supported  = LSSslave->pFunctLSScheckBitRate(
149                     LSSslave->functLSScheckBitRateObject, bit);
150 
151                 if (bit_rate_supported) {
152                     LSSslave->pendingBitRate = bit;
153                 }
154                 else {
155                     errorCode = CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE;
156                 }
157             }
158             else {
159                 /* we currently only support CiA301 bit timing table */
160                 errorCode = CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE;
161             }
162 
163             /* send confirmation */
164             LSSslave->TXbuff->data[0] = CO_LSS_CFG_BIT_TIMING;
165             LSSslave->TXbuff->data[1] = errorCode;
166             /* we do not use spec-error, always 0 */
167             CO_memset(&LSSslave->TXbuff->data[2], 0, 6);
168             CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
169             break;
170         case CO_LSS_CFG_ACTIVATE_BIT_TIMING:
171             if (LSSslave->pFunctLSScheckBitRate == NULL) {
172                 /* setting bit timing is not supported. Drop request */
173                 break;
174             }
175 
176             /* notify application */
177             if (LSSslave->pFunctLSSactivateBitRate != NULL) {
178               uint16_t delay;
179               CO_memcpySwap2(&delay, &msg->data[1]);
180               LSSslave->pFunctLSSactivateBitRate(
181                   LSSslave->functLSSactivateBitRateObject, delay);
182             }
183             break;
184         case CO_LSS_CFG_STORE:
185             errorCode = CO_LSS_CFG_STORE_OK;
186 
187             if (LSSslave->pFunctLSScfgStore == NULL) {
188                 /* storing is not supported. Reply error */
189                 errorCode = CO_LSS_CFG_STORE_NOT_SUPPORTED;
190             }
191             else {
192                 bool_t result;
193                 /* Store "pending" to "persistent" */
194                 result = LSSslave->pFunctLSScfgStore(LSSslave->functLSScfgStore,
195                     LSSslave->pendingNodeID, LSSslave->pendingBitRate);
196                 if (!result) {
197                     errorCode = CO_LSS_CFG_STORE_FAILED;
198                 }
199             }
200 
201             /* send confirmation */
202             LSSslave->TXbuff->data[0] = CO_LSS_CFG_STORE;
203             LSSslave->TXbuff->data[1] = errorCode;
204             /* we do not use spec-error, always 0 */
205             CO_memset(&LSSslave->TXbuff->data[2], 0, 6);
206             CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
207             break;
208         default:
209             break;
210     }
211 }
212 
213 /*
214  * Helper function - Handle service "inquire"
215  */
CO_LSSslave_serviceInquire(CO_LSSslave_t * LSSslave,CO_LSS_cs_t service,const CO_CANrxMsg_t * msg)216 static void CO_LSSslave_serviceInquire(
217     CO_LSSslave_t *LSSslave,
218     CO_LSS_cs_t service,
219     const CO_CANrxMsg_t *msg)
220 {
221     uint32_t value;
222 
223     if(LSSslave->lssState != CO_LSS_STATE_CONFIGURATION) {
224         return;
225     }
226 
227     switch (service) {
228         case CO_LSS_INQUIRE_VENDOR:
229             value = LSSslave->lssAddress.identity.vendorID;
230             break;
231         case CO_LSS_INQUIRE_PRODUCT:
232             value = LSSslave->lssAddress.identity.productCode;
233             break;
234         case CO_LSS_INQUIRE_REV:
235             value = LSSslave->lssAddress.identity.revisionNumber;
236             break;
237         case CO_LSS_INQUIRE_SERIAL:
238             value = LSSslave->lssAddress.identity.serialNumber;
239             break;
240         case CO_LSS_INQUIRE_NODE_ID:
241             value = (uint32_t)LSSslave->activeNodeID;
242             break;
243         default:
244             return;
245     }
246     /* send response */
247     LSSslave->TXbuff->data[0] = service;
248     CO_memcpySwap4(&LSSslave->TXbuff->data[1], &value);
249     CO_memset(&LSSslave->TXbuff->data[5], 0, 3);
250     CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
251 }
252 
253 /*
254  * Helper function - Handle service "identify"
255  */
CO_LSSslave_serviceIdent(CO_LSSslave_t * LSSslave,CO_LSS_cs_t service,const CO_CANrxMsg_t * msg)256 static void CO_LSSslave_serviceIdent(
257     CO_LSSslave_t *LSSslave,
258     CO_LSS_cs_t service,
259     const CO_CANrxMsg_t *msg)
260 {
261     uint32_t idNumber;
262     uint8_t bitCheck;
263     uint8_t lssSub;
264     uint8_t lssNext;
265     bool_t ack;
266 
267     if (LSSslave->lssState != CO_LSS_STATE_WAITING) {
268         /* fastscan is only allowed in waiting state */
269         return;
270     }
271     if (service != CO_LSS_IDENT_FASTSCAN) {
272         /* we only support "fastscan" identification */
273         return;
274     }
275     if (LSSslave->pendingNodeID!=CO_LSS_NODE_ID_ASSIGNMENT ||
276         LSSslave->activeNodeID!=CO_LSS_NODE_ID_ASSIGNMENT) {
277         /* fastscan is only active on unconfigured nodes */
278         return;
279     }
280 
281     CO_memcpySwap4(&idNumber, &msg->data[1]);
282     bitCheck = msg->data[5];
283     lssSub = msg->data[6];
284     lssNext = msg->data[7];
285 
286     if (!CO_LSS_FASTSCAN_BITCHECK_VALID(bitCheck) ||
287         !CO_LSS_FASTSCAN_LSS_SUB_NEXT_VALID(lssSub) ||
288         !CO_LSS_FASTSCAN_LSS_SUB_NEXT_VALID(lssNext)) {
289         /* Invalid request */
290         return;
291     }
292 
293     ack = false;
294     if (bitCheck == CO_LSS_FASTSCAN_CONFIRM) {
295         /* Confirm, Reset */
296         ack = true;
297         LSSslave->fastscanPos = CO_LSS_FASTSCAN_VENDOR_ID;
298         CO_memset((uint8_t*)&LSSslave->lssFastscan, 0,
299             sizeof(LSSslave->lssFastscan));
300     }
301     else if (LSSslave->fastscanPos == lssSub) {
302         uint32_t mask = 0xFFFFFFFF << bitCheck;
303 
304         if ((LSSslave->lssAddress.addr[lssSub] & mask) == (idNumber & mask)) {
305             /* all requested bits match */
306             ack = true;
307             LSSslave->fastscanPos = lssNext;
308 
309             if (bitCheck==0 && lssNext<lssSub) {
310                 /* complete match, enter configuration state */
311                 LSSslave->lssState = CO_LSS_STATE_CONFIGURATION;
312             }
313         }
314     }
315     if (ack) {
316         LSSslave->TXbuff->data[0] = CO_LSS_IDENT_SLAVE;
317         CO_memset(&LSSslave->TXbuff->data[1], 0, 7);
318         CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
319     }
320 }
321 
322 
323 /*
324  * Read received message from CAN module.
325  *
326  * Function will be called (by CAN receive interrupt) every time, when CAN
327  * message with correct identifier will be received. For more information and
328  * description of parameters see file CO_driver.h.
329  */
CO_LSSslave_receive(void * object,const CO_CANrxMsg_t * msg)330 static void CO_LSSslave_receive(void *object, const CO_CANrxMsg_t *msg)
331 {
332     CO_LSSslave_t *LSSslave;
333 
334     LSSslave = (CO_LSSslave_t*)object;   /* this is the correct pointer type of the first argument */
335 
336     if(msg->DLC == 8){
337         CO_LSS_cs_t cs = msg->data[0];
338 
339         if (CO_LSS_CS_SERVICE_IS_SWITCH_GLOBAL(cs)) {
340             CO_LSSslave_serviceSwitchStateGlobal(LSSslave, cs, msg);
341         }
342         else if (CO_LSS_CS_SERVICE_IS_SWITCH_STATE_SELECTIVE(cs)) {
343             CO_LSSslave_serviceSwitchStateSelective(LSSslave, cs, msg);
344         }
345         else if (CO_LSS_CS_SERVICE_IS_CONFIG(cs)) {
346             CO_LSSslave_serviceConfig(LSSslave, cs, msg);
347         }
348         else if (CO_LSS_CS_SERVICE_IS_INQUIRE(cs)) {
349             CO_LSSslave_serviceInquire(LSSslave, cs, msg);
350         }
351         else if (CO_LSS_CS_SERVICE_IS_IDENT(cs)) {
352             CO_LSSslave_serviceIdent(LSSslave, cs, msg);
353         }
354         else {
355             /* No Ack -> Unsupported commands are dropped */
356         }
357     }
358 }
359 
360 
361 /******************************************************************************/
CO_LSSslave_init(CO_LSSslave_t * LSSslave,CO_LSS_address_t lssAddress,uint16_t pendingBitRate,uint8_t pendingNodeID,CO_CANmodule_t * CANdevRx,uint16_t CANdevRxIdx,uint32_t CANidLssMaster,CO_CANmodule_t * CANdevTx,uint16_t CANdevTxIdx,uint32_t CANidLssSlave)362 CO_ReturnError_t CO_LSSslave_init(
363         CO_LSSslave_t          *LSSslave,
364         CO_LSS_address_t        lssAddress,
365         uint16_t                pendingBitRate,
366         uint8_t                 pendingNodeID,
367         CO_CANmodule_t         *CANdevRx,
368         uint16_t                CANdevRxIdx,
369         uint32_t                CANidLssMaster,
370         CO_CANmodule_t         *CANdevTx,
371         uint16_t                CANdevTxIdx,
372         uint32_t                CANidLssSlave)
373 {
374     /* verify arguments */
375     if (LSSslave==NULL || CANdevRx==NULL || CANdevTx==NULL ||
376         !CO_LSS_NODE_ID_VALID(pendingNodeID)) {
377         return CO_ERROR_ILLEGAL_ARGUMENT;
378     }
379 
380     /* check LSS address for plausibility. As a bare minimum, the vendor
381      * ID and serial number must be set */
382     if (lssAddress.identity.vendorID==0 || lssAddress.identity.serialNumber==0) {
383         return CO_ERROR_ILLEGAL_ARGUMENT;
384     }
385 
386     CO_memcpy((uint8_t*)&LSSslave->lssAddress, (uint8_t*)&lssAddress, sizeof(LSSslave->lssAddress));
387     LSSslave->lssState = CO_LSS_STATE_WAITING;
388     CO_memset((uint8_t*)&LSSslave->lssSelect, 0, sizeof(LSSslave->lssSelect));
389 
390     CO_memset((uint8_t*)&LSSslave->lssFastscan, 0, sizeof(LSSslave->lssFastscan));
391     LSSslave->fastscanPos = CO_LSS_FASTSCAN_VENDOR_ID;
392 
393     LSSslave->pendingBitRate = pendingBitRate;
394     LSSslave->pendingNodeID = pendingNodeID;
395     LSSslave->activeNodeID = CO_LSS_NODE_ID_ASSIGNMENT;
396     LSSslave->pFunctLSScheckBitRate = NULL;
397     LSSslave->functLSScheckBitRateObject = NULL;
398     LSSslave->pFunctLSSactivateBitRate = NULL;
399     LSSslave->functLSSactivateBitRateObject = NULL;
400     LSSslave->pFunctLSScfgStore = NULL;
401     LSSslave->functLSScfgStore = NULL;
402 
403     /* configure LSS CAN Master message reception */
404     CO_CANrxBufferInit(
405             CANdevRx,             /* CAN device */
406             CANdevRxIdx,          /* rx buffer index */
407             CANidLssMaster,       /* CAN identifier */
408             0x7FF,                /* mask */
409             0,                    /* rtr */
410             (void*)LSSslave,      /* object passed to receive function */
411             CO_LSSslave_receive); /* this function will process received message */
412 
413     /* configure LSS CAN Slave response message transmission */
414     LSSslave->CANdevTx = CANdevTx;
415     LSSslave->TXbuff = CO_CANtxBufferInit(
416             CANdevTx,             /* CAN device */
417             CANdevTxIdx,          /* index of specific buffer inside CAN module */
418             CANidLssSlave,        /* CAN identifier */
419             0,                    /* rtr */
420             8,                    /* number of data bytes */
421             0);                   /* synchronous message flag bit */
422 
423     return CO_ERROR_NO;
424 }
425 
426 
427 /******************************************************************************/
CO_LSSslave_initCheckBitRateCallback(CO_LSSslave_t * LSSslave,void * object,bool_t (* pFunctLSScheckBitRate)(void * object,uint16_t bitRate))428 void CO_LSSslave_initCheckBitRateCallback(
429         CO_LSSslave_t          *LSSslave,
430         void                   *object,
431         bool_t                (*pFunctLSScheckBitRate)(void *object, uint16_t bitRate))
432 {
433     if(LSSslave != NULL){
434         LSSslave->functLSScheckBitRateObject = object;
435         LSSslave->pFunctLSScheckBitRate = pFunctLSScheckBitRate;
436     }
437 }
438 
439 
440 /******************************************************************************/
CO_LSSslave_initActivateBitRateCallback(CO_LSSslave_t * LSSslave,void * object,void (* pFunctLSSactivateBitRate)(void * object,uint16_t delay))441 void CO_LSSslave_initActivateBitRateCallback(
442         CO_LSSslave_t          *LSSslave,
443         void                   *object,
444         void                  (*pFunctLSSactivateBitRate)(void *object, uint16_t delay))
445 {
446     if(LSSslave != NULL){
447         LSSslave->functLSSactivateBitRateObject = object;
448         LSSslave->pFunctLSSactivateBitRate = pFunctLSSactivateBitRate;
449     }
450 }
451 
452 
453 /******************************************************************************/
CO_LSSslave_initCfgStoreCallback(CO_LSSslave_t * LSSslave,void * object,bool_t (* pFunctLSScfgStore)(void * object,uint8_t id,uint16_t bitRate))454 void CO_LSSslave_initCfgStoreCallback(
455         CO_LSSslave_t          *LSSslave,
456         void                   *object,
457         bool_t                (*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate))
458 {
459     if(LSSslave != NULL){
460         LSSslave->functLSScfgStore = object;
461         LSSslave->pFunctLSScfgStore = pFunctLSScfgStore;
462     }
463 }
464 
465 
466 /******************************************************************************/
CO_LSSslave_process(CO_LSSslave_t * LSSslave,uint16_t activeBitRate,uint8_t activeNodeId,uint16_t * pendingBitRate,uint8_t * pendingNodeId)467 void CO_LSSslave_process(
468         CO_LSSslave_t          *LSSslave,
469         uint16_t                activeBitRate,
470         uint8_t                 activeNodeId,
471         uint16_t               *pendingBitRate,
472         uint8_t                *pendingNodeId)
473 {
474     LSSslave->activeNodeID = activeNodeId;
475     *pendingBitRate = LSSslave->pendingBitRate;
476     *pendingNodeId = LSSslave->pendingNodeID;
477 }
478 
479 
480 /******************************************************************************/
CO_LSSslave_getState(CO_LSSslave_t * LSSslave)481 CO_LSS_state_t CO_LSSslave_getState(
482         CO_LSSslave_t          *LSSslave)
483 {
484   if(LSSslave != NULL){
485       return LSSslave->lssState;
486   }
487   return CO_LSS_STATE_WAITING;
488 }
489 
490 
491 /******************************************************************************/
CO_LSSslave_LEDprocess(CO_LSSslave_t * LSSslave,uint16_t timeDifference_ms,bool_t * LEDon)492 bool_t CO_LSSslave_LEDprocess(
493         CO_LSSslave_t          *LSSslave,
494         uint16_t                timeDifference_ms,
495         bool_t *LEDon)
496 {
497     static uint16_t ms50 = 0;
498     static int8_t flash1, flash2;
499 
500     if (LSSslave == NULL || LEDon == NULL)
501         return false;
502     ms50 += timeDifference_ms;
503     if(ms50 >= 50) {
504         ms50 -= 50;
505         /* 4 cycles on, 50 cycles off */
506         if(++flash1 >= 4) flash1 = -50;
507 
508         /* 4 cycles on, 4 cycles off, 4 cycles on, 50 cycles off */
509         switch(++flash2){
510             case    4:  flash2 = -104; break;
511             case -100:  flash2 =  100; break;
512             case  104:  flash2 =  -50; break;
513         }
514     }
515     if (LSSslave->lssState == CO_LSS_STATE_CONFIGURATION)
516     {
517         *LEDon = (flash2 >= 0);
518         return true;
519     }
520     else if (LSSslave->activeNodeID == CO_LSS_NODE_ID_ASSIGNMENT)
521     {
522         *LEDon = (flash1 >= 0);
523         return true;
524     }
525     return false;
526 }
527 
528 
529 #endif
530