1 /*
2  * Main CANopen stack file. It combines Object dictionary (CO_OD) and all other
3  * CANopen source files. Configuration information are read from CO_OD.h file.
4  *
5  * @file        CANopen.c
6  * @ingroup     CO_CANopen
7  * @author      Janez Paternoster
8  * @copyright   2010 - 2020 Janez Paternoster
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 
28 #include "CANopen.h"
29 
30 
31 /* If defined, global variables will be used, otherwise CANopen objects will
32    be generated with calloc(). */
33 /* #define CO_USE_GLOBALS */
34 
35 /* If defined, the user provides an own implemetation for calculating the
36  * CRC16 CCITT checksum. */
37 /* #define CO_USE_OWN_CRC16 */
38 
39 #ifndef CO_USE_GLOBALS
40     #include <stdlib.h> /*  for malloc, free */
41     static uint32_t CO_memoryUsed = 0; /* informative */
42 #endif
43 
44 
45 /* Global variables ***********************************************************/
46     extern const CO_OD_entry_t CO_OD[CO_OD_NoOfElements];  /* Object Dictionary array */
47     static CO_t COO;
48     CO_t *CO = NULL;
49 
50     static CO_CANrx_t          *CO_CANmodule_rxArray0;
51     static CO_CANtx_t          *CO_CANmodule_txArray0;
52     static CO_OD_extension_t   *CO_SDO_ODExtensions;
53     static CO_HBconsNode_t     *CO_HBcons_monitoredNodes;
54 #if CO_NO_TRACE > 0
55     static uint32_t            *CO_traceTimeBuffers[CO_NO_TRACE];
56     static int32_t             *CO_traceValueBuffers[CO_NO_TRACE];
57   #ifdef CO_USE_GLOBALS
58   #ifndef CO_TRACE_BUFFER_SIZE_FIXED
59     #define CO_TRACE_BUFFER_SIZE_FIXED 100
60   #endif
61   #endif
62 #endif
63 
64 
65 /* Verify features from CO_OD *************************************************/
66     /* generate error, if features are not correctly configured for this project */
67     #if        CO_NO_NMT_MASTER                           >  1     \
68             || CO_NO_SYNC                                 >  1     \
69             || CO_NO_EMERGENCY                            != 1     \
70             || CO_NO_SDO_SERVER                           == 0     \
71             || CO_NO_TIME                                 >  1     \
72             || CO_NO_SDO_CLIENT                           > 128    \
73             || (CO_NO_RPDO < 1 || CO_NO_RPDO > 0x200)              \
74             || (CO_NO_TPDO < 1 || CO_NO_TPDO > 0x200)              \
75             || ODL_consumerHeartbeatTime_arrayLength      == 0     \
76             || ODL_errorStatusBits_stringLength           < 10     \
77             || CO_NO_LSS_SERVER                           >  1     \
78             || CO_NO_LSS_CLIENT                           >  1     \
79             || (CO_NO_LSS_SERVER > 0 && CO_NO_LSS_CLIENT > 0)
80         #error Features from CO_OD.h file are not corectly configured for this project!
81     #endif
82 
83 
84 /* Indexes for CANopenNode message objects ************************************/
85     #ifdef ODL_consumerHeartbeatTime_arrayLength
86         #define CO_NO_HB_CONS   ODL_consumerHeartbeatTime_arrayLength
87     #else
88         #define CO_NO_HB_CONS   0
89     #endif
90     #define CO_NO_HB_PROD      1                                      /*  Producer Heartbeat Cont */
91 
92     #define CO_RXCAN_NMT       0                                      /*  index for NMT message */
93     #define CO_RXCAN_SYNC      1                                      /*  index for SYNC message */
94     #define CO_RXCAN_EMERG    (CO_RXCAN_SYNC+CO_NO_SYNC)              /*  index for Emergency message */
95     #define CO_RXCAN_TIME     (CO_RXCAN_EMERG+CO_NO_EMERGENCY)        /*  index for TIME message */
96     #define CO_RXCAN_RPDO     (CO_RXCAN_TIME+CO_NO_TIME)              /*  start index for RPDO messages */
97     #define CO_RXCAN_SDO_SRV  (CO_RXCAN_RPDO+CO_NO_RPDO)              /*  start index for SDO server message (request) */
98     #define CO_RXCAN_SDO_CLI  (CO_RXCAN_SDO_SRV+CO_NO_SDO_SERVER)     /*  start index for SDO client message (response) */
99     #define CO_RXCAN_CONS_HB  (CO_RXCAN_SDO_CLI+CO_NO_SDO_CLIENT)     /*  start index for Heartbeat Consumer messages */
100     #define CO_RXCAN_LSS      (CO_RXCAN_CONS_HB+CO_NO_HB_CONS)        /*  index for LSS rx message */
101     /* total number of received CAN messages */
102     #define CO_RXCAN_NO_MSGS (\
103         1 + \
104         CO_NO_SYNC + \
105         CO_NO_EMERGENCY + \
106         CO_NO_TIME + \
107         CO_NO_RPDO + \
108         CO_NO_SDO_SERVER + \
109         CO_NO_SDO_CLIENT + \
110         CO_NO_HB_CONS + \
111         CO_NO_LSS_SERVER + \
112         CO_NO_LSS_CLIENT + \
113         0 \
114     )
115 
116     #define CO_TXCAN_NMT       0                                      /*  index for NMT master message */
117     #define CO_TXCAN_SYNC      CO_TXCAN_NMT+CO_NO_NMT_MASTER          /*  index for SYNC message */
118     #define CO_TXCAN_EMERG    (CO_TXCAN_SYNC+CO_NO_SYNC)              /*  index for Emergency message */
119     #define CO_TXCAN_TIME     (CO_TXCAN_EMERG+CO_NO_EMERGENCY)        /*  index for TIME message */
120     #define CO_TXCAN_TPDO     (CO_TXCAN_TIME+CO_NO_TIME)              /*  start index for TPDO messages */
121     #define CO_TXCAN_SDO_SRV  (CO_TXCAN_TPDO+CO_NO_TPDO)              /*  start index for SDO server message (response) */
122     #define CO_TXCAN_SDO_CLI  (CO_TXCAN_SDO_SRV+CO_NO_SDO_SERVER)     /*  start index for SDO client message (request) */
123     #define CO_TXCAN_HB       (CO_TXCAN_SDO_CLI+CO_NO_SDO_CLIENT)     /*  index for Heartbeat message */
124     #define CO_TXCAN_LSS      (CO_TXCAN_HB+CO_NO_HB_PROD)             /*  index for LSS tx message */
125     /* total number of transmitted CAN messages */
126     #define CO_TXCAN_NO_MSGS ( \
127         CO_NO_NMT_MASTER + \
128         CO_NO_SYNC + \
129         CO_NO_EMERGENCY + \
130         CO_NO_TIME + \
131         CO_NO_TPDO + \
132         CO_NO_SDO_SERVER + \
133         CO_NO_SDO_CLIENT + \
134         CO_NO_HB_PROD + \
135         CO_NO_LSS_SERVER + \
136         CO_NO_LSS_CLIENT + \
137         0\
138     )
139 
140 
141 #ifdef CO_USE_GLOBALS
142     static CO_CANmodule_t       COO_CANmodule;
143     static CO_CANrx_t           COO_CANmodule_rxArray0[CO_RXCAN_NO_MSGS];
144     static CO_CANtx_t           COO_CANmodule_txArray0[CO_TXCAN_NO_MSGS];
145     static CO_SDO_t             COO_SDO[CO_NO_SDO_SERVER];
146     static CO_OD_extension_t    COO_SDO_ODExtensions[CO_OD_NoOfElements];
147     static CO_EM_t              COO_EM;
148     static CO_EMpr_t            COO_EMpr;
149     static CO_NMT_t             COO_NMT;
150 #if CO_NO_SYNC == 1
151     static CO_SYNC_t            COO_SYNC;
152 #endif
153 #if CO_NO_TIME == 1
154     static CO_TIME_t            COO_TIME;
155 #endif
156     static CO_RPDO_t            COO_RPDO[CO_NO_RPDO];
157     static CO_TPDO_t            COO_TPDO[CO_NO_TPDO];
158     static CO_HBconsumer_t      COO_HBcons;
159     static CO_HBconsNode_t      COO_HBcons_monitoredNodes[CO_NO_HB_CONS];
160 #if CO_NO_LSS_SERVER == 1
161     static CO_LSSslave_t        CO0_LSSslave;
162 #endif
163 #if CO_NO_LSS_CLIENT == 1
164     static CO_LSSmaster_t       CO0_LSSmaster;
165 #endif
166 #if CO_NO_SDO_CLIENT != 0
167     static CO_SDOclient_t       COO_SDOclient[CO_NO_SDO_CLIENT];
168 #endif
169 #if CO_NO_TRACE > 0
170     static CO_trace_t           COO_trace[CO_NO_TRACE];
171     static uint32_t             COO_traceTimeBuffers[CO_NO_TRACE][CO_TRACE_BUFFER_SIZE_FIXED];
172     static int32_t              COO_traceValueBuffers[CO_NO_TRACE][CO_TRACE_BUFFER_SIZE_FIXED];
173 #endif
174 #endif
175 
176 /* These declarations here are needed in the case the switches for the project
177     change the visibility in the headers in a way that the compiler doesn't see an declaration anymore */
178 
179 #if CO_NO_LSS_SERVER == 0 /* LSS Server means LSS slave */
180 
181 CO_ReturnError_t CO_new(void);
182 
183 CO_ReturnError_t CO_CANinit(
184         void                   *CANdriverState,
185         uint16_t                bitRate);
186 
187 CO_ReturnError_t CO_LSSinit(
188         uint8_t                 nodeId,
189         uint16_t                bitRate);
190 
191 CO_ReturnError_t CO_CANopenInit(
192         uint8_t                 nodeId);
193 
194 #else /* CO_NO_LSS_SERVER == 0 */
195 
196 CO_ReturnError_t CO_init(
197         void                   *CANdriverState,
198         uint8_t                 nodeId,
199         uint16_t                bitRate);
200 
201 #endif /* CO_NO_LSS_SERVER == 0 */
202 
203 
204 /* Helper function for NMT master *********************************************/
205 #if CO_NO_NMT_MASTER == 1
206     CO_CANtx_t *NMTM_txBuff = 0;
207 
CO_sendNMTcommand(CO_t * co,uint8_t command,uint8_t nodeID)208     CO_ReturnError_t CO_sendNMTcommand(CO_t *co, uint8_t command, uint8_t nodeID){
209         if(NMTM_txBuff == 0){
210             /* error, CO_CANtxBufferInit() was not called for this buffer. */
211             return CO_ERROR_TX_UNCONFIGURED; /* -11 */
212         }
213         NMTM_txBuff->data[0] = command;
214         NMTM_txBuff->data[1] = nodeID;
215 
216         CO_ReturnError_t error = CO_ERROR_NO;
217 
218         /* Apply NMT command also to this node, if set so. */
219         if(nodeID == 0 || nodeID == co->NMT->nodeId){
220             switch(command){
221                 case CO_NMT_ENTER_OPERATIONAL:
222                     if((*co->NMT->emPr->errorRegister) == 0) {
223                         co->NMT->operatingState = CO_NMT_OPERATIONAL;
224                     }
225                     break;
226                 case CO_NMT_ENTER_STOPPED:
227                     co->NMT->operatingState = CO_NMT_STOPPED;
228                     break;
229                 case CO_NMT_ENTER_PRE_OPERATIONAL:
230                     co->NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
231                     break;
232                 case CO_NMT_RESET_NODE:
233                     co->NMT->resetCommand = CO_RESET_APP;
234                     break;
235                 case CO_NMT_RESET_COMMUNICATION:
236                     co->NMT->resetCommand = CO_RESET_COMM;
237                     break;
238                 default:
239                     error = CO_ERROR_ILLEGAL_ARGUMENT;
240                     break;
241 
242             }
243         }
244 
245         if(error == CO_ERROR_NO)
246             return CO_CANsend(co->CANmodule[0], NMTM_txBuff); /* 0 = success */
247         else
248         {
249             return error;
250         }
251 
252     }
253 #endif
254 
255 
256 #if CO_NO_TRACE > 0
257 static uint32_t CO_traceBufferSize[CO_NO_TRACE];
258 #endif
259 
260 /******************************************************************************/
CO_new(void)261 CO_ReturnError_t CO_new(void)
262 {
263     int16_t i;
264 #ifndef CO_USE_GLOBALS
265     uint16_t errCnt;
266 #endif
267 
268     /* Verify parameters from CO_OD */
269     if(   sizeof(OD_TPDOCommunicationParameter_t) != sizeof(CO_TPDOCommPar_t)
270        || sizeof(OD_TPDOMappingParameter_t) != sizeof(CO_TPDOMapPar_t)
271        || sizeof(OD_RPDOCommunicationParameter_t) != sizeof(CO_RPDOCommPar_t)
272        || sizeof(OD_RPDOMappingParameter_t) != sizeof(CO_RPDOMapPar_t))
273     {
274         return CO_ERROR_PARAMETERS;
275     }
276 
277     #if CO_NO_SDO_CLIENT != 0
278     if(sizeof(OD_SDOClientParameter_t) != sizeof(CO_SDOclientPar_t)){
279         return CO_ERROR_PARAMETERS;
280     }
281     #endif
282 
283 
284     /* Initialize CANopen object */
285 #ifdef CO_USE_GLOBALS
286     CO = &COO;
287 
288     CO_memset((uint8_t*)CO, 0, sizeof(CO_t));
289     CO->CANmodule[0]                    = &COO_CANmodule;
290     CO_CANmodule_rxArray0               = &COO_CANmodule_rxArray0[0];
291     CO_CANmodule_txArray0               = &COO_CANmodule_txArray0[0];
292     for(i=0; i<CO_NO_SDO_SERVER; i++)
293         CO->SDO[i]                      = &COO_SDO[i];
294     CO_SDO_ODExtensions                 = &COO_SDO_ODExtensions[0];
295     CO->em                              = &COO_EM;
296     CO->emPr                            = &COO_EMpr;
297     CO->NMT                             = &COO_NMT;
298   #if CO_NO_SYNC == 1
299     CO->SYNC                            = &COO_SYNC;
300   #endif
301   #if CO_NO_TIME == 1
302     CO->TIME                            = &COO_TIME;
303   #endif
304     for(i=0; i<CO_NO_RPDO; i++)
305         CO->RPDO[i]                     = &COO_RPDO[i];
306     for(i=0; i<CO_NO_TPDO; i++)
307         CO->TPDO[i]                     = &COO_TPDO[i];
308     CO->HBcons                          = &COO_HBcons;
309     CO_HBcons_monitoredNodes            = &COO_HBcons_monitoredNodes[0];
310   #if CO_NO_LSS_SERVER == 1
311     CO->LSSslave                        = &CO0_LSSslave;
312   #endif
313   #if CO_NO_LSS_CLIENT == 1
314     CO->LSSmaster                       = &CO0_LSSmaster;
315   #endif
316   #if CO_NO_SDO_CLIENT != 0
317     for(i=0; i<CO_NO_SDO_CLIENT; i++) {
318       CO->SDOclient[i]                  = &COO_SDOclient[i];
319     }
320   #endif
321   #if CO_NO_TRACE > 0
322     for(i=0; i<CO_NO_TRACE; i++) {
323         CO->trace[i]                    = &COO_trace[i];
324         CO_traceTimeBuffers[i]          = &COO_traceTimeBuffers[i][0];
325         CO_traceValueBuffers[i]         = &COO_traceValueBuffers[i][0];
326         CO_traceBufferSize[i]           = CO_TRACE_BUFFER_SIZE_FIXED;
327     }
328   #endif
329 #else
330     if(CO == NULL){    /* Use malloc only once */
331         CO = &COO;
332         CO->CANmodule[0]                    = (CO_CANmodule_t *)    calloc(1, sizeof(CO_CANmodule_t));
333         CO_CANmodule_rxArray0               = (CO_CANrx_t *)        calloc(CO_RXCAN_NO_MSGS, sizeof(CO_CANrx_t));
334         CO_CANmodule_txArray0               = (CO_CANtx_t *)        calloc(CO_TXCAN_NO_MSGS, sizeof(CO_CANtx_t));
335         for(i=0; i<CO_NO_SDO_SERVER; i++){
336             CO->SDO[i]                      = (CO_SDO_t *)          calloc(1, sizeof(CO_SDO_t));
337         }
338         CO_SDO_ODExtensions                 = (CO_OD_extension_t*)  calloc(CO_OD_NoOfElements, sizeof(CO_OD_extension_t));
339         CO->em                              = (CO_EM_t *)           calloc(1, sizeof(CO_EM_t));
340         CO->emPr                            = (CO_EMpr_t *)         calloc(1, sizeof(CO_EMpr_t));
341         CO->NMT                             = (CO_NMT_t *)          calloc(1, sizeof(CO_NMT_t));
342       #if CO_NO_SYNC == 1
343         CO->SYNC                            = (CO_SYNC_t *)         calloc(1, sizeof(CO_SYNC_t));
344       #endif
345       #if CO_NO_TIME == 1
346         CO->TIME                            = (CO_TIME_t *)         calloc(1, sizeof(CO_TIME_t));
347       #endif
348         for(i=0; i<CO_NO_RPDO; i++){
349             CO->RPDO[i]                     = (CO_RPDO_t *)         calloc(1, sizeof(CO_RPDO_t));
350         }
351         for(i=0; i<CO_NO_TPDO; i++){
352             CO->TPDO[i]                     = (CO_TPDO_t *)         calloc(1, sizeof(CO_TPDO_t));
353         }
354         CO->HBcons                          = (CO_HBconsumer_t *)   calloc(1, sizeof(CO_HBconsumer_t));
355         CO_HBcons_monitoredNodes            = (CO_HBconsNode_t *)   calloc(CO_NO_HB_CONS, sizeof(CO_HBconsNode_t));
356       #if CO_NO_LSS_SERVER == 1
357         CO->LSSslave                        = (CO_LSSslave_t *)     calloc(1, sizeof(CO_LSSslave_t));
358       #endif
359       #if CO_NO_LSS_CLIENT == 1
360         CO->LSSmaster                       = (CO_LSSmaster_t *)    calloc(1, sizeof(CO_LSSmaster_t));
361       #endif
362       #if CO_NO_SDO_CLIENT != 0
363         for(i=0; i<CO_NO_SDO_CLIENT; i++){
364             CO->SDOclient[i]                = (CO_SDOclient_t *)    calloc(1, sizeof(CO_SDOclient_t));
365         }
366       #endif
367       #if CO_NO_TRACE > 0
368         for(i=0; i<CO_NO_TRACE; i++) {
369             CO->trace[i]                    = (CO_trace_t *)        calloc(1, sizeof(CO_trace_t));
370             CO_traceTimeBuffers[i]          = (uint32_t *)          calloc(OD_traceConfig[i].size, sizeof(uint32_t));
371             CO_traceValueBuffers[i]         = (int32_t *)           calloc(OD_traceConfig[i].size, sizeof(int32_t));
372             if(CO_traceTimeBuffers[i] != NULL && CO_traceValueBuffers[i] != NULL) {
373                 CO_traceBufferSize[i] = OD_traceConfig[i].size;
374             } else {
375                 CO_traceBufferSize[i] = 0;
376             }
377         }
378       #endif
379     }
380 
381     CO_memoryUsed = sizeof(CO_CANmodule_t)
382                   + sizeof(CO_CANrx_t) * CO_RXCAN_NO_MSGS
383                   + sizeof(CO_CANtx_t) * CO_TXCAN_NO_MSGS
384                   + sizeof(CO_SDO_t) * CO_NO_SDO_SERVER
385                   + sizeof(CO_OD_extension_t) * CO_OD_NoOfElements
386                   + sizeof(CO_EM_t)
387                   + sizeof(CO_EMpr_t)
388                   + sizeof(CO_NMT_t)
389   #if CO_NO_SYNC == 1
390                   + sizeof(CO_SYNC_t)
391   #endif
392   #if CO_NO_TIME == 1
393                   + sizeof(CO_TIME_t)
394   #endif
395                   + sizeof(CO_RPDO_t) * CO_NO_RPDO
396                   + sizeof(CO_TPDO_t) * CO_NO_TPDO
397                   + sizeof(CO_HBconsumer_t)
398                   + sizeof(CO_HBconsNode_t) * CO_NO_HB_CONS
399   #if CO_NO_LSS_SERVER == 1
400                   + sizeof(CO_LSSslave_t)
401   #endif
402   #if CO_NO_LSS_CLIENT == 1
403                   + sizeof(CO_LSSmaster_t)
404   #endif
405   #if CO_NO_SDO_CLIENT != 0
406                   + sizeof(CO_SDOclient_t) * CO_NO_SDO_CLIENT
407   #endif
408                   + 0;
409   #if CO_NO_TRACE > 0
410     CO_memoryUsed += sizeof(CO_trace_t) * CO_NO_TRACE;
411     for(i=0; i<CO_NO_TRACE; i++) {
412         CO_memoryUsed += CO_traceBufferSize[i] * 8;
413     }
414   #endif
415 
416     errCnt = 0;
417     if(CO->CANmodule[0]                 == NULL) errCnt++;
418     if(CO_CANmodule_rxArray0            == NULL) errCnt++;
419     if(CO_CANmodule_txArray0            == NULL) errCnt++;
420     for(i=0; i<CO_NO_SDO_SERVER; i++){
421         if(CO->SDO[i]                   == NULL) errCnt++;
422     }
423     if(CO_SDO_ODExtensions              == NULL) errCnt++;
424     if(CO->em                           == NULL) errCnt++;
425     if(CO->emPr                         == NULL) errCnt++;
426     if(CO->NMT                          == NULL) errCnt++;
427   #if CO_NO_SYNC == 1
428     if(CO->SYNC                         == NULL) errCnt++;
429   #endif
430   #if CO_NO_TIME == 1
431     if(CO->TIME                     	== NULL) errCnt++;
432   #endif
433     for(i=0; i<CO_NO_RPDO; i++){
434         if(CO->RPDO[i]                  == NULL) errCnt++;
435     }
436     for(i=0; i<CO_NO_TPDO; i++){
437         if(CO->TPDO[i]                  == NULL) errCnt++;
438     }
439     if(CO->HBcons                       == NULL) errCnt++;
440     if(CO_HBcons_monitoredNodes         == NULL) errCnt++;
441   #if CO_NO_LSS_SERVER == 1
442     if(CO->LSSslave                     == NULL) errCnt++;
443   #endif
444   #if CO_NO_LSS_CLIENT == 1
445     if(CO->LSSmaster                    == NULL) errCnt++;
446   #endif
447   #if CO_NO_SDO_CLIENT != 0
448     for(i=0; i<CO_NO_SDO_CLIENT; i++){
449         if(CO->SDOclient[i]             == NULL) errCnt++;
450     }
451   #endif
452   #if CO_NO_TRACE > 0
453     for(i=0; i<CO_NO_TRACE; i++) {
454         if(CO->trace[i]                 == NULL) errCnt++;
455     }
456   #endif
457 
458     if(errCnt != 0) return CO_ERROR_OUT_OF_MEMORY;
459 #endif
460     return CO_ERROR_NO;
461 }
462 
463 
464 /******************************************************************************/
CO_CANinit(void * CANdriverState,uint16_t bitRate)465 CO_ReturnError_t CO_CANinit(
466         void                   *CANdriverState,
467         uint16_t                bitRate)
468 {
469     CO_ReturnError_t err;
470 
471     CO->CANmodule[0]->CANnormal = false;
472     CO_CANsetConfigurationMode(CANdriverState);
473 
474     err = CO_CANmodule_init(
475             CO->CANmodule[0],
476             CANdriverState,
477             CO_CANmodule_rxArray0,
478             CO_RXCAN_NO_MSGS,
479             CO_CANmodule_txArray0,
480             CO_TXCAN_NO_MSGS,
481             bitRate);
482 
483     return err;
484 }
485 
486 
487 /******************************************************************************/
488 #if CO_NO_LSS_SERVER == 1
CO_LSSinit(uint8_t nodeId,uint16_t bitRate)489 CO_ReturnError_t CO_LSSinit(
490         uint8_t                 nodeId,
491         uint16_t                bitRate)
492 {
493     CO_LSS_address_t lssAddress;
494     CO_ReturnError_t err;
495 
496     lssAddress.identity.productCode = OD_identity.productCode;
497     lssAddress.identity.revisionNumber = OD_identity.revisionNumber;
498     lssAddress.identity.serialNumber = OD_identity.serialNumber;
499     lssAddress.identity.vendorID = OD_identity.vendorID;
500     err = CO_LSSslave_init(
501             CO->LSSslave,
502             lssAddress,
503             bitRate,
504             nodeId,
505             CO->CANmodule[0],
506             CO_RXCAN_LSS,
507             CO_CAN_ID_LSS_SRV,
508             CO->CANmodule[0],
509             CO_TXCAN_LSS,
510             CO_CAN_ID_LSS_CLI);
511 
512     return err;
513 }
514 #endif /* CO_NO_LSS_SERVER == 1 */
515 
516 
517 /******************************************************************************/
CO_CANopenInit(uint8_t nodeId)518 CO_ReturnError_t CO_CANopenInit(
519         uint8_t                 nodeId)
520 {
521     int16_t i;
522     CO_ReturnError_t err;
523 
524     /* Verify CANopen Node-ID */
525     if(nodeId<1 || nodeId>127) {
526         return CO_ERROR_PARAMETERS;
527     }
528 
529     for (i=0; i<CO_NO_SDO_SERVER; i++)
530     {
531         uint32_t COB_IDClientToServer;
532         uint32_t COB_IDServerToClient;
533         if(i==0){
534             /*Default SDO server must be located at first index*/
535             COB_IDClientToServer = CO_CAN_ID_RSDO + nodeId;
536             COB_IDServerToClient = CO_CAN_ID_TSDO + nodeId;
537         }else{
538             COB_IDClientToServer = OD_SDOServerParameter[i].COB_IDClientToServer;
539             COB_IDServerToClient = OD_SDOServerParameter[i].COB_IDServerToClient;
540         }
541 
542         err = CO_SDO_init(
543                 CO->SDO[i],
544                 COB_IDClientToServer,
545                 COB_IDServerToClient,
546                 OD_H1200_SDO_SERVER_PARAM+i,
547                 i==0 ? 0 : CO->SDO[0],
548                &CO_OD[0],
549                 CO_OD_NoOfElements,
550                 CO_SDO_ODExtensions,
551                 nodeId,
552                 CO->CANmodule[0],
553                 CO_RXCAN_SDO_SRV+i,
554                 CO->CANmodule[0],
555                 CO_TXCAN_SDO_SRV+i);
556     }
557 
558     if(err){return err;}
559 
560 
561     err = CO_EM_init(
562             CO->em,
563             CO->emPr,
564             CO->SDO[0],
565            &OD_errorStatusBits[0],
566             ODL_errorStatusBits_stringLength,
567            &OD_errorRegister,
568            &OD_preDefinedErrorField[0],
569             ODL_preDefinedErrorField_arrayLength,
570             CO->CANmodule[0],
571             CO_RXCAN_EMERG,
572             CO->CANmodule[0],
573             CO_TXCAN_EMERG,
574             (uint16_t)CO_CAN_ID_EMERGENCY + nodeId);
575 
576     if(err){return err;}
577 
578 
579     err = CO_NMT_init(
580             CO->NMT,
581             CO->emPr,
582             nodeId,
583             500,
584             CO->CANmodule[0],
585             CO_RXCAN_NMT,
586             CO_CAN_ID_NMT_SERVICE,
587             CO->CANmodule[0],
588             CO_TXCAN_HB,
589             CO_CAN_ID_HEARTBEAT + nodeId);
590 
591     if(err){return err;}
592 
593 
594 #if CO_NO_NMT_MASTER == 1
595     NMTM_txBuff = CO_CANtxBufferInit(/* return pointer to 8-byte CAN data buffer, which should be populated */
596             CO->CANmodule[0], /* pointer to CAN module used for sending this message */
597             CO_TXCAN_NMT,     /* index of specific buffer inside CAN module */
598             0x0000,           /* CAN identifier */
599             0,                /* rtr */
600             2,                /* number of data bytes */
601             0);               /* synchronous message flag bit */
602 #endif
603 #if CO_NO_LSS_CLIENT == 1
604     err = CO_LSSmaster_init(
605             CO->LSSmaster,
606             CO_LSSmaster_DEFAULT_TIMEOUT,
607             CO->CANmodule[0],
608             CO_RXCAN_LSS,
609             CO_CAN_ID_LSS_CLI,
610             CO->CANmodule[0],
611             CO_TXCAN_LSS,
612             CO_CAN_ID_LSS_SRV);
613 
614     if(err){return err;}
615 
616 #endif
617 
618 #if CO_NO_SYNC == 1
619     err = CO_SYNC_init(
620             CO->SYNC,
621             CO->em,
622             CO->SDO[0],
623            &CO->NMT->operatingState,
624             OD_COB_ID_SYNCMessage,
625             OD_communicationCyclePeriod,
626             OD_synchronousCounterOverflowValue,
627             CO->CANmodule[0],
628             CO_RXCAN_SYNC,
629             CO->CANmodule[0],
630             CO_TXCAN_SYNC);
631 
632     if(err){return err;}
633 #endif
634 
635 #if CO_NO_TIME == 1
636     err = CO_TIME_init(
637             CO->TIME,
638             CO->em,
639             CO->SDO[0],
640             &CO->NMT->operatingState,
641             OD_COB_ID_TIME,
642             0,
643             CO->CANmodule[0],
644             CO_RXCAN_TIME,
645             CO->CANmodule[0],
646             CO_TXCAN_TIME);
647 
648     if(err){return err;}
649 #endif
650 
651     for(i=0; i<CO_NO_RPDO; i++){
652         CO_CANmodule_t *CANdevRx = CO->CANmodule[0];
653         uint16_t CANdevRxIdx = CO_RXCAN_RPDO + i;
654 
655         err = CO_RPDO_init(
656                 CO->RPDO[i],
657                 CO->em,
658                 CO->SDO[0],
659                 CO->SYNC,
660                &CO->NMT->operatingState,
661                 nodeId,
662                 ((i<4) ? (CO_CAN_ID_RPDO_1+i*0x100) : 0),
663                 0,
664                 (CO_RPDOCommPar_t*) &OD_RPDOCommunicationParameter[i],
665                 (CO_RPDOMapPar_t*) &OD_RPDOMappingParameter[i],
666                 OD_H1400_RXPDO_1_PARAM+i,
667                 OD_H1600_RXPDO_1_MAPPING+i,
668                 CANdevRx,
669                 CANdevRxIdx);
670 
671         if(err){return err;}
672     }
673 
674     for(i=0; i<CO_NO_TPDO; i++){
675         err = CO_TPDO_init(
676                 CO->TPDO[i],
677                 CO->em,
678                 CO->SDO[0],
679                 CO->SYNC,
680                &CO->NMT->operatingState,
681                 nodeId,
682                 ((i<4) ? (CO_CAN_ID_TPDO_1+i*0x100) : 0),
683                 0,
684                 (CO_TPDOCommPar_t*) &OD_TPDOCommunicationParameter[i],
685                 (CO_TPDOMapPar_t*) &OD_TPDOMappingParameter[i],
686                 OD_H1800_TXPDO_1_PARAM+i,
687                 OD_H1A00_TXPDO_1_MAPPING+i,
688                 CO->CANmodule[0],
689                 CO_TXCAN_TPDO+i);
690 
691         if(err){return err;}
692     }
693 
694 
695     err = CO_HBconsumer_init(
696             CO->HBcons,
697             CO->em,
698             CO->SDO[0],
699            &OD_consumerHeartbeatTime[0],
700             CO_HBcons_monitoredNodes,
701             CO_NO_HB_CONS,
702             CO->CANmodule[0],
703             CO_RXCAN_CONS_HB);
704 
705     if(err){return err;}
706 
707 
708 #if CO_NO_SDO_CLIENT != 0
709 
710     for(i=0; i<CO_NO_SDO_CLIENT; i++){
711 
712         err = CO_SDOclient_init(
713                 CO->SDOclient[i],
714                 CO->SDO[0],
715                 (CO_SDOclientPar_t*) &OD_SDOClientParameter[i],
716                 CO->CANmodule[0],
717                 CO_RXCAN_SDO_CLI+i,
718                 CO->CANmodule[0],
719                 CO_TXCAN_SDO_CLI+i);
720 
721         if(err){return err;}
722 
723     }
724 #endif
725 
726 
727 #if CO_NO_TRACE > 0
728     for(i=0; i<CO_NO_TRACE; i++) {
729         CO_trace_init(
730             CO->trace[i],
731             CO->SDO[0],
732             OD_traceConfig[i].axisNo,
733             CO_traceTimeBuffers[i],
734             CO_traceValueBuffers[i],
735             CO_traceBufferSize[i],
736             &OD_traceConfig[i].map,
737             &OD_traceConfig[i].format,
738             &OD_traceConfig[i].trigger,
739             &OD_traceConfig[i].threshold,
740             &OD_trace[i].value,
741             &OD_trace[i].min,
742             &OD_trace[i].max,
743             &OD_trace[i].triggerTime,
744             OD_INDEX_TRACE_CONFIG + i,
745             OD_INDEX_TRACE + i);
746     }
747 #endif
748 
749     return CO_ERROR_NO;
750 }
751 
752 
753 /******************************************************************************/
CO_init(void * CANdriverState,uint8_t nodeId,uint16_t bitRate)754 CO_ReturnError_t CO_init(
755         void                   *CANdriverState,
756         uint8_t                 nodeId,
757         uint16_t                bitRate)
758 {
759     CO_ReturnError_t err;
760 
761     err = CO_new();
762     if (err) {
763         return err;
764     }
765 
766     err = CO_CANinit(CANdriverState, bitRate);
767     if (err) {
768         CO_delete(CANdriverState);
769         return err;
770     }
771 
772     err = CO_CANopenInit(nodeId);
773     if (err) {
774         CO_delete(CANdriverState);
775         return err;
776     }
777 
778     return CO_ERROR_NO;
779 }
780 
781 
782 /******************************************************************************/
CO_delete(void * CANdriverState)783 void CO_delete(void *CANdriverState){
784 #ifndef CO_USE_GLOBALS
785     int16_t i;
786 #endif
787 
788     CO_CANsetConfigurationMode(CANdriverState);
789     CO_CANmodule_disable(CO->CANmodule[0]);
790 
791 #ifndef CO_USE_GLOBALS
792   #if CO_NO_TRACE > 0
793       for(i=0; i<CO_NO_TRACE; i++) {
794           free(CO->trace[i]);
795           free(CO_traceTimeBuffers[i]);
796           free(CO_traceValueBuffers[i]);
797       }
798   #endif
799   #if CO_NO_SDO_CLIENT != 0
800       for(i=0; i<CO_NO_SDO_CLIENT; i++) {
801           free(CO->SDOclient[i]);
802       }
803   #endif
804   #if CO_NO_LSS_SERVER == 1
805     free(CO->LSSslave);
806   #endif
807   #if CO_NO_LSS_CLIENT == 1
808     free(CO->LSSmaster);
809   #endif
810     free(CO_HBcons_monitoredNodes);
811     free(CO->HBcons);
812     for(i=0; i<CO_NO_RPDO; i++){
813         free(CO->RPDO[i]);
814     }
815     for(i=0; i<CO_NO_TPDO; i++){
816         free(CO->TPDO[i]);
817     }
818   #if CO_NO_SYNC == 1
819     free(CO->SYNC);
820   #endif
821   #if CO_NO_TIME == 1
822     free(CO->TIME);
823   #endif
824     free(CO->NMT);
825     free(CO->emPr);
826     free(CO->em);
827     free(CO_SDO_ODExtensions);
828     for(i=0; i<CO_NO_SDO_SERVER; i++){
829         free(CO->SDO[i]);
830     }
831     free(CO_CANmodule_txArray0);
832     free(CO_CANmodule_rxArray0);
833     free(CO->CANmodule[0]);
834     CO = NULL;
835 #endif
836 }
837 
838 
839 /******************************************************************************/
CO_process(CO_t * co,uint16_t timeDifference_ms,uint16_t * timerNext_ms)840 CO_NMT_reset_cmd_t CO_process(
841         CO_t                   *co,
842         uint16_t                timeDifference_ms,
843         uint16_t               *timerNext_ms)
844 {
845     uint8_t i;
846     bool_t NMTisPreOrOperational = false;
847     CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
848 #ifdef CO_USE_LEDS
849     static uint16_t ms50 = 0;
850 #endif /* CO_USE_LEDS */
851 
852     if(co->NMT->operatingState == CO_NMT_PRE_OPERATIONAL || co->NMT->operatingState == CO_NMT_OPERATIONAL)
853         NMTisPreOrOperational = true;
854 
855 #ifdef CO_USE_LEDS
856     ms50 += timeDifference_ms;
857     if(ms50 >= 50){
858         ms50 -= 50;
859         CO_NMT_blinkingProcess50ms(co->NMT);
860     }
861 #endif /* CO_USE_LEDS */
862 
863     for(i=0; i<CO_NO_SDO_SERVER; i++){
864         CO_SDO_process(
865                 co->SDO[i],
866                 NMTisPreOrOperational,
867                 timeDifference_ms,
868                 1000,
869                 timerNext_ms);
870     }
871 
872     CO_EM_process(
873             co->emPr,
874             NMTisPreOrOperational,
875             timeDifference_ms * 10,
876             OD_inhibitTimeEMCY,
877             timerNext_ms);
878 
879 
880     reset = CO_NMT_process(
881             co->NMT,
882             timeDifference_ms,
883             OD_producerHeartbeatTime,
884             OD_NMTStartup,
885             OD_errorRegister,
886             OD_errorBehavior,
887             timerNext_ms);
888 
889 
890     CO_HBconsumer_process(
891             co->HBcons,
892             NMTisPreOrOperational,
893             timeDifference_ms);
894 
895 #if CO_NO_TIME == 1
896     CO_TIME_process(
897             co->TIME,
898             timeDifference_ms);
899 #endif
900 
901     return reset;
902 }
903 
904 
905 /******************************************************************************/
906 #if CO_NO_SYNC == 1
CO_process_SYNC(CO_t * co,uint32_t timeDifference_us)907 bool_t CO_process_SYNC(
908         CO_t                   *co,
909         uint32_t                timeDifference_us)
910 {
911     bool_t syncWas = false;
912 
913     switch(CO_SYNC_process(co->SYNC, timeDifference_us, OD_synchronousWindowLength)){
914         case 1:     //immediately after the SYNC message
915             syncWas = true;
916             break;
917         case 2:     //outside SYNC window
918             CO_CANclearPendingSyncPDOs(co->CANmodule[0]);
919             break;
920     }
921 
922     return syncWas;
923 }
924 #endif
925 
926 /******************************************************************************/
CO_process_RPDO(CO_t * co,bool_t syncWas)927 void CO_process_RPDO(
928         CO_t                   *co,
929         bool_t                  syncWas)
930 {
931     int16_t i;
932 
933     for(i=0; i<CO_NO_RPDO; i++){
934         CO_RPDO_process(co->RPDO[i], syncWas);
935     }
936 }
937 
938 
939 /******************************************************************************/
CO_process_TPDO(CO_t * co,bool_t syncWas,uint32_t timeDifference_us)940 void CO_process_TPDO(
941         CO_t                   *co,
942         bool_t                  syncWas,
943         uint32_t                timeDifference_us)
944 {
945     int16_t i;
946 
947     /* Verify PDO Change Of State and process PDOs */
948     for(i=0; i<CO_NO_TPDO; i++){
949         if(!co->TPDO[i]->sendRequest)
950             co->TPDO[i]->sendRequest = CO_TPDOisCOS(co->TPDO[i]);
951         CO_TPDO_process(co->TPDO[i], syncWas, timeDifference_us);
952     }
953 }
954