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
