2 This file is part of CanFestival, a library implementing CanOpen Stack.
4 Copyright (C): Edouard TISSERANT and Francis DUPIN
6 See COPYING file for copyrights details.
8 This library is free software; you can redistribute it and/or
9 modify it under the terms of the GNU Lesser General Public
10 License as published by the Free Software Foundation; either
11 version 2.1 of the License, or (at your option) any later version.
13 This library is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 Lesser General Public License for more details.
18 You should have received a copy of the GNU Lesser General Public
19 License along with this library; if not, write to the Free Software
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 //#define DEBUG_WAR_CONSOLE_ON
24 //#define DEBUG_ERR_CONSOLE_ON
29 /***************************************************************************/
30 // SDO (un)packing macros
32 /** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO
34 #define getSDOcs(byte) (byte >> 5)
36 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits
38 #define getSDOn2(byte) ((byte >> 2) & 3)
40 /** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits
42 #define getSDOn3(byte) ((byte >> 1) & 7)
44 /** Returns the transfer type from the first byte of the SDO
46 #define getSDOe(byte) ((byte >> 1) & 1)
48 /** Returns the size indicator from the first byte of the SDO
50 #define getSDOs(byte) (byte & 1)
52 /** Returns the indicator of end transmission from the first byte of the SDO
54 #define getSDOc(byte) (byte & 1)
56 /** Returns the toggle from the first byte of the SDO
58 #define getSDOt(byte) ((byte >> 4) & 1)
60 /** Returns the index from the bytes 1 and 2 of the SDO
62 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1))
64 /** Returns the subIndex from the byte 3 of the SDO
66 #define getSDOsubIndex(byte3) (byte3)
68 /***************************************************************************
71 void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
73 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
74 MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId);
75 MSG_WAR(0x2A02, " index : ", d->transfers[id].index);
76 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex);
77 // Reset timer handler
78 d->transfers[id].timer = TIMER_NONE;
79 // Call the user function to inform of the problem.
80 (*d->SDOtimeoutError)(id);
81 // Sending a SDO abort
82 sendSDOabort(d, d->transfers[id].whoami,
83 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
88 #define StopSDO_TIMER(id) \
89 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
90 d->transfers[id].timer = DelAlarm(d->transfers[id].timer);
92 #define StartSDO_TIMER(id) \
93 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
94 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);
96 #define RestartSDO_TIMER(id) \
97 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
98 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }
100 /***************************************************************************/
101 /** Reset all sdo buffers
103 void resetSDO (CO_Data* d)
107 /* transfer structure initialization */
108 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++)
112 /***************************************************************************/
113 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
115 const indextable *ptrTable;
119 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
120 size = d->transfers[line].count;
121 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
122 (void *) d->transfers[line].data, &size, 1);
123 if (errorCode != OD_SUCCESSFUL)
125 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
130 /***************************************************************************/
131 UNS32 objdictToSDOline (CO_Data* d, UNS8 line)
139 MSG_WAR(0x3A05, "Reading at index : ", d->transfers[line].index);
140 MSG_WAR(0x3A06, "Reading at subIndex : ", d->transfers[line].subIndex);
142 errorCode = getODentry(d, d->transfers[line].index,
143 d->transfers[line].subIndex,
144 (void *)d->transfers[line].data,
145 &size, &dataType, 0);
147 if (errorCode != OD_SUCCESSFUL)
150 d->transfers[line].count = size;
151 d->transfers[line].offset = 0;
155 /***************************************************************************/
156 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) {
160 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
161 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
164 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {
165 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);
168 offset = d->transfers[line].offset;
169 for (i = 0 ; i < nbBytes ; i++)
170 * (data + i) = d->transfers[line].data[offset + i];
171 d->transfers[line].offset = d->transfers[line].offset + nbBytes;
176 /***************************************************************************/
177 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data)
182 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) {
183 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
186 offset = d->transfers[line].offset;
187 for (i = 0 ; i < nbBytes ; i++)
188 d->transfers[line].data[offset + i] = * (data + i);
189 d->transfers[line].offset = d->transfers[line].offset + nbBytes;
193 /***************************************************************************/
194 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index,
195 UNS8 subIndex, UNS32 abortCode)
199 err = getSDOlineOnUse( d, nodeId, whoami, &line );
200 if (!err) // If a line on use have been found.
201 MSG_WAR(0x3A20, "FailedSDO : line found : ", line);
202 if ((! err) && (whoami == SDO_SERVER)) {
203 resetSDOline( d, line );
204 MSG_WAR(0x3A21, "FailedSDO : line released : ", line);
206 if ((! err) && (whoami == SDO_CLIENT)) {
208 d->transfers[line].state = SDO_ABORTED_INTERNAL;
210 MSG_WAR(0x3A22, "Sending SDO abort ", 0);
211 err = sendSDOabort(d, whoami, index, subIndex, abortCode);
213 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);
219 /***************************************************************************/
220 void resetSDOline ( CO_Data* d, UNS8 line )
223 MSG_WAR(0x3A25, "reset SDO line nb : ", line);
224 initSDOline(d, line, 0, 0, 0, SDO_RESET);
225 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++)
226 d->transfers[line].data[i] = 0;
229 /***************************************************************************/
230 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state)
232 MSG_WAR(0x3A25, "init SDO line nb : ", line);
233 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){
238 d->transfers[line].nodeId = nodeId;
239 d->transfers[line].index = index;
240 d->transfers[line].subIndex = subIndex;
241 d->transfers[line].state = state;
242 d->transfers[line].toggle = 0;
243 d->transfers[line].count = 0;
244 d->transfers[line].offset = 0;
245 d->transfers[line].dataType = 0;
249 /***************************************************************************/
250 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line )
255 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
256 if ( d->transfers[i].state == SDO_RESET ) {
258 d->transfers[i].whoami = whoami;
262 MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);
266 /***************************************************************************/
267 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line)
272 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){
273 if ( (d->transfers[i].state != SDO_RESET) &&
274 (d->transfers[i].nodeId == nodeId) &&
275 (d->transfers[i].whoami == whoami) ) {
283 /***************************************************************************/
284 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami)
288 err = getSDOlineOnUse(d, nodeId, whoami, &line);
290 MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId);
293 resetSDOline(d, line);
297 /***************************************************************************/
298 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes)
300 if (d->transfers[line].count == 0) // if received initiate SDO protocol with e=0 and s=0
303 * nbBytes = d->transfers[line].count - d->transfers[line].offset;
307 /***************************************************************************/
308 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes)
310 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) {
311 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes);
314 d->transfers[line].count = nbBytes;
319 /***************************************************************************/
320 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo)
327 UNS32 * pwCobId = NULL;
328 UNS8 * pwNodeId = NULL;
333 MSG_WAR(0x3A38, "sendSDO",0);
334 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) {
335 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);
339 /*get the server->client cobid*/
340 if ( whoami == SDO_SERVER ) {/*case server. Easy because today only one server SDO is authorized in CanFestival*/
341 offset = d->firstIndex->SDO_SVR;
343 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0);
346 pwCobId = d->objdict[offset].pSubindex[2].pObject;
347 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId);
349 else { /*case client*/
350 /* Get the client->server cobid.*/
352 offset = d->firstIndex->SDO_CLT;
353 lastIndex = d->lastIndex->SDO_CLT;
355 MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0);
358 /* First, have to find at the index where is defined the communication with the server node */
359 while (offset <= lastIndex){
360 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum);
361 if (d->objdict[offset].bSubCount <= 3) {
362 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum);
365 pwNodeId = d->objdict[offset].pSubindex[3].pObject;
366 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId);
367 if(*pwNodeId == sdo.nodeId) {
375 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId);
378 /* Second, read the cobid client->server */
379 pwCobId = d->objdict[offset].pSubindex[1].pObject;
381 /* message copy for sending */
382 m.cob_id.w = *pwCobId;
383 m.rtr = NOT_A_REQUEST;
384 //the length of SDO must be 8
386 for (i = 0 ; i < 8 ; i++) {
387 m.data[i] = sdo.body.data[i];
389 return (*d->canSend)(&m);
392 /***************************************************************************/
393 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode)
397 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);
398 sdo.nodeId = *d->bDeviceNodeId;
399 sdo.body.data[0] = 0x80;
401 sdo.body.data[1] = index & 0xFF; // LSB
402 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
404 sdo.body.data[3] = subIndex;
406 sdo.body.data[4] = (UNS8)(abortCode & 0xFF);
407 sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF);
408 sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF);
409 sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF);
410 ret = sendSDO(d, whoami, sdo);
415 /***************************************************************************/
416 UNS8 proceedSDO (CO_Data* d, Message *m)
420 UNS8 nbBytes; // received or to be transmited.
421 UNS8 nodeId = 0; // The node from which the SDO is received
422 UNS8 *pNodeId = NULL;
423 UNS8 whoami = SDO_UNKNOWN; // SDO_SERVER or SDO_CLIENT.
424 UNS32 errorCode; // while reading or writing in the local object dictionary.
425 s_SDO sdo; // SDO to transmit
430 UNS32 * pCobId = NULL;
438 MSG_WAR(0x3A60, "proceedSDO ", 0);
439 whoami = SDO_UNKNOWN;
440 // Looking for the cobId in the object dictionary.
442 offset = d->firstIndex->SDO_SVR;
443 lastIndex = d->lastIndex->SDO_SVR;
445 if(offset) while (offset <= lastIndex) {
446 if (d->objdict[offset].bSubCount <= 1) {
447 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j);
450 pCobId = d->objdict[offset].pSubindex[1].pObject;
451 if ( *pCobId == (*m).cob_id.w ) {
453 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);
454 // In case of server, the node id of the client may be unknown. So we put the index minus offset
455 // 0x1200 where the cobid received is defined.
462 if (whoami == SDO_UNKNOWN) {
464 offset = d->firstIndex->SDO_CLT;
465 lastIndex = d->lastIndex->SDO_CLT;
467 if(offset) while (offset <= lastIndex) {
468 if (d->objdict[offset].bSubCount <= 3) {
469 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j);
472 // a) Looking for the cobid received.
473 pCobId = d->objdict[offset].pSubindex[2].pObject;
474 if (*pCobId == (*m).cob_id.w ) {
475 // b) cobid found, so reading the node id of the server.
476 pNodeId = d->objdict[offset].pSubindex[3].pObject;
479 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j);
480 MSG_WAR(0x3A65, " Server nodeId : ", nodeId);
487 if (whoami == SDO_UNKNOWN) {
488 return 0xFF;// This SDO was not for us !
491 // Test if the size of the SDO is ok
492 if ( (*m).len != 8) {
493 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id.w);
494 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR);
498 if (whoami == SDO_CLIENT) {
499 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId);
502 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w);
505 // Testing the command specifier
506 // Allowed : cs = 0, 1, 2, 3, 4. (= all except those for block tranfert).
507 // cs = other : Not allowed -> abort.
508 switch (getSDOcs(m->data[0])) {
512 if (whoami == SDO_SERVER) {
513 // Receiving a download segment data.
514 // A SDO transfert should have been yet initiated.
515 err = getSDOlineOnUse( d, nodeId, whoami, &line );
517 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
519 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ",
521 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
525 RestartSDO_TIMER(line)
526 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId);
527 index = d->transfers[line].index;
528 subIndex = d->transfers[line].subIndex;
530 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
531 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0]));
532 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
535 // Nb of data to be downloaded
536 nbBytes = 7 - getSDOn3(m->data[0]);
537 // Store the data in the transfert structure.
538 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
540 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
543 // Sending the SDO response, CS = 1
544 sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender).
545 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4);
546 for (i = 1 ; i < 8 ; i++)
547 sdo.body.data[i] = 0;
548 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId);
549 sendSDO(d, whoami, sdo);
550 // Inverting the toggle for the next segment.
551 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
552 // If it was the last segment,
553 if (getSDOc(m->data[0])) {
554 // Transfering line data to object dictionary.
555 // The code does not use the "d" of initiate frame. So it is safe if e=s=0
556 errorCode = SDOlineToObjdict(d, line);
558 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0);
559 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
562 // Release of the line
563 resetSDOline(d, line);
564 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId);
569 // It is a request for a previous upload segment. We should find a line opened for this.
570 err = getSDOlineOnUse( d, nodeId, whoami, &line);
572 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
574 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
575 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
579 RestartSDO_TIMER(line)
580 index = d->transfers[line].index;
581 subIndex = d->transfers[line].subIndex;
582 // test of the toggle;
583 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
584 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
585 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
588 // nb of data to be uploaded
589 nbBytes = 7 - getSDOn3(m->data[0]);
590 // Storing the data in the line structure.
591 err = SDOtoLine(d, line, nbBytes, (*m).data + 1);
593 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
596 // Inverting the toggle for the next segment.
597 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
598 // If it was the last segment,
599 if ( getSDOc(m->data[0])) {
600 // Put in state finished
601 // The code is safe for the case e=s=0 in initiate frame.
603 d->transfers[line].state = SDO_FINISHED;
604 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);
606 else { // more segments to receive
607 // Sending the request for the next segment.
609 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4);
610 for (i = 1 ; i < 8 ; i++)
611 sdo.body.data[i] = 0;
612 sendSDO(d, whoami, sdo);
613 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);
620 // Receive of an initiate download
621 if (whoami == SDO_SERVER) {
622 index = getSDOindex(m->data[1],m->data[2]);
623 subIndex = getSDOsubIndex(m->data[3]);
624 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ",
626 MSG_WAR(0x3A80, "Writing at index : ", index);
627 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);
629 // Search if a SDO transfert have been yet initiated
630 err = getSDOlineOnUse( d, nodeId, whoami, &line );
632 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);
633 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
636 // No line on use. Great !
637 // Try to open a new line.
638 err = getSDOfreeLine( d, whoami, &line );
640 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
641 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
644 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
646 if (getSDOe(m->data[0])) { // If SDO expedited
647 // nb of data to be downloaded
648 nbBytes = 4 - getSDOn2(m->data[0]);
649 // Storing the data in the line structure.
650 d->transfers[line].count = nbBytes;
651 err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
654 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
658 // SDO expedited -> transfert finished. Data can be stored in the dictionary.
659 // The line will be reseted when it is downloading in the dictionary.
660 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId);
661 // Transfering line data to object dictionary.
662 errorCode = SDOlineToObjdict(d, line);
664 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0);
665 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
668 // Release of the line.
669 resetSDOline(d, line);
671 else {// So, if it is not an expedited transfert
672 if (getSDOs(m->data[0])) {
673 // TODO : if e and s = 0, not reading m->data[4] but put nbBytes = 0
674 nbBytes = m->data[4]; // Transfert limited to 255 bytes.
675 err = setSDOlineRestBytes(d, nodeId, nbBytes);
677 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
682 //Sending a SDO, cs=3
683 sdo.nodeId = *d->bDeviceNodeId; // The node id of the server, (here it is the sender).
684 sdo.body.data[0] = 3 << 5;
685 sdo.body.data[1] = index & 0xFF; // LSB
686 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
687 sdo.body.data[3] = subIndex;
688 for (i = 4 ; i < 8 ; i++)
689 sdo.body.data[i] = 0;
690 sendSDO(d, whoami, sdo);
691 } // end if I am SERVER
694 // It is a response for a previous download segment. We should find a line opened for this.
695 err = getSDOlineOnUse( d, nodeId, whoami, &line);
697 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
699 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);
700 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
704 RestartSDO_TIMER(line)
705 index = d->transfers[line].index;
706 subIndex = d->transfers[line].subIndex;
707 // test of the toggle;
708 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
709 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId);
710 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
714 // End transmission or downloading next segment. We need to know if it will be the last one.
715 getSDOlineRestBytes(d, line, &nbBytes);
717 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId);
719 d->transfers[line].state = SDO_FINISHED;
722 // At least one transfer to send.
724 // several segments to download.
725 // code to send the next segment. (cs = 0; c = 0)
726 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
727 sdo.nodeId = nodeId; // The server node Id;
728 sdo.body.data[0] = (d->transfers[line].toggle << 4);
729 err = lineToSDO(d, line, 7, sdo.body.data + 1);
731 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
737 // code to send the last segment. (cs = 0; c = 1)
738 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
739 sdo.nodeId = nodeId; // The server node Id;
740 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
741 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
743 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
746 for (i = nbBytes + 1 ; i < 8 ; i++)
747 sdo.body.data[i] = 0;
749 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);
750 sendSDO(d, whoami, sdo);
751 } // end if I am a CLIENT
756 // Receive of an initiate upload.
757 if (whoami == SDO_SERVER) {
758 index = getSDOindex(m->data[1],m->data[2]);
759 subIndex = getSDOsubIndex(m->data[3]);
760 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ",
762 MSG_WAR(0x3A90, "Reading at index : ", index);
763 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);
764 // Search if a SDO transfert have been yet initiated
765 err = getSDOlineOnUse( d, nodeId, whoami, &line );
767 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);
768 MSG_WAR(0x3A93, "nodeId = ", nodeId);
769 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
772 // No line on use. Great !
773 // Try to open a new line.
774 err = getSDOfreeLine( d, whoami, &line );
776 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);
777 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);
780 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
781 // Transfer data from dictionary to the line structure.
782 errorCode = objdictToSDOline(d, line);
785 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ",
787 failedSDO(d, nodeId, whoami, index, subIndex, errorCode);
790 // Preparing the response.
791 getSDOlineRestBytes(d, line, &nbBytes); // Nb bytes to transfer ?
792 sdo.nodeId = nodeId; // The server node Id;
794 // normal transfert. (segmented).
795 // code to send the initiate upload response. (cs = 2)
796 sdo.body.data[0] = (2 << 5) | 1;
797 sdo.body.data[1] = index & 0xFF; // LSB
798 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
799 sdo.body.data[3] = subIndex;
800 sdo.body.data[4] = nbBytes; // Limitation of canfestival2 : Max tranfert is 256 bytes.
801 // It takes too much memory to upgrate to 2^32 because the size of data is also coded
802 // in the object dictionary, at every index and subindex.
803 for (i = 5 ; i < 8 ; i++)
804 sdo.body.data[i] = 0;
805 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);
806 sendSDO(d, whoami, sdo);
809 // Expedited upload. (cs = 2 ; e = 1)
810 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3;
811 sdo.body.data[1] = index & 0xFF; // LSB
812 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
813 sdo.body.data[3] = subIndex;
814 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4);
816 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
819 for (i = 4 + nbBytes ; i < 8 ; i++)
820 sdo.body.data[i] = 0;
821 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ",
823 sendSDO(d, whoami, sdo);
825 resetSDOline(d, line);
827 } // end if I am SERVER
830 // It is the response for the previous initiate upload request.
831 // We should find a line opened for this.
832 err = getSDOlineOnUse( d, nodeId, whoami, &line);
834 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
836 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId);
837 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
841 RestartSDO_TIMER(line)
842 index = d->transfers[line].index;
843 subIndex = d->transfers[line].subIndex;
845 if (getSDOe(m->data[0])) { // If SDO expedited
846 // nb of data to be uploaded
847 nbBytes = 4 - getSDOn2(m->data[0]);
848 // Storing the data in the line structure.
849 err = SDOtoLine(d, line, nbBytes, (*m).data + 4);
851 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
854 // SDO expedited -> transfert finished. data are available via getReadResultNetworkDict().
855 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);
857 d->transfers[line].count = nbBytes;
858 d->transfers[line].state = SDO_FINISHED;
861 else { // So, if it is not an expedited transfert
862 // Storing the nb of data to receive.
863 if (getSDOs(m->data[0])) {
864 nbBytes = m->data[4]; // Remember the limitation to 255 bytes to transfert
865 err = setSDOlineRestBytes(d, line, nbBytes);
867 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
871 // Requesting next segment. (cs = 3)
873 sdo.body.data[0] = 3 << 5;
874 for (i = 1 ; i < 8 ; i++)
875 sdo.body.data[i] = 0;
876 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId);
877 sendSDO(d, whoami, sdo);
884 if (whoami == SDO_SERVER) {
885 // Receiving a upload segment.
886 // A SDO transfert should have been yet initiated.
887 err = getSDOlineOnUse( d, nodeId, whoami, &line );
889 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;
891 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ",
893 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
897 RestartSDO_TIMER(line)
898 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId);
899 index = d->transfers[line].index;
900 subIndex = d->transfers[line].subIndex;
902 if (d->transfers[line].toggle != getSDOt(m->data[0])) {
903 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0]));
904 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);
907 // Uploading next segment. We need to know if it will be the last one.
908 getSDOlineRestBytes(d, line, &nbBytes);
910 // The segment to transfer is not the last one.
911 // code to send the next segment. (cs = 0; c = 0)
912 sdo.nodeId = nodeId; // The server node Id;
913 sdo.body.data[0] = (d->transfers[line].toggle << 4);
914 err = lineToSDO(d, line, 7, sdo.body.data + 1);
916 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
919 // Inverting the toggle for the next tranfert.
920 d->transfers[line].toggle = ! d->transfers[line].toggle & 1;
921 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId);
922 sendSDO(d, whoami, sdo);
926 // code to send the last segment. (cs = 0; c = 1)
927 sdo.nodeId = nodeId; // The server node Id;
928 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
929 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
931 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
934 for (i = nbBytes + 1 ; i < 8 ; i++)
935 sdo.body.data[i] = 0;
936 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId);
937 sendSDO(d, whoami, sdo);
939 resetSDOline(d, line);
944 // It is the response for the previous initiate download request.
945 // We should find a line opened for this.
946 err = getSDOlineOnUse( d, nodeId, whoami, &line);
948 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;
950 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId);
951 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);
955 RestartSDO_TIMER(line)
956 index = d->transfers[line].index;
957 subIndex = d->transfers[line].subIndex;
958 // End transmission or requesting next segment.
959 getSDOlineRestBytes(d, line, &nbBytes);
961 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId);
963 d->transfers[line].state = SDO_FINISHED;
967 // more than one request to send
968 // code to send the next segment. (cs = 0; c = 0)
969 sdo.nodeId = nodeId; // The server node Id;
970 sdo.body.data[0] = (d->transfers[line].toggle << 4);
971 err = lineToSDO(d, line, 7, sdo.body.data + 1);
973 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
979 // code to send the last segment. (cs = 0; c = 1)
980 sdo.nodeId = nodeId; // The server node Id;
981 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1;
982 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1);
984 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR);
987 for (i = nbBytes + 1 ; i < 8 ; i++)
988 sdo.body.data[i] = 0;
990 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);
991 sendSDO(d, whoami, sdo);
993 } // end if I am a CLIENT
997 abortCode = (*m).data[3] |
1001 // Received SDO abort.
1002 // Looking for the line concerned.
1003 if (whoami == SDO_SERVER) {
1004 err = getSDOlineOnUse( d, nodeId, whoami, &line );
1006 resetSDOline( d, line );
1007 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);
1010 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);
1011 // Tips : The end user has no way to know that the server node has received an abort SDO.
1012 // Its is ok, I think.
1014 else { // If I am CLIENT
1015 err = getSDOlineOnUse( d, nodeId, whoami, &line );
1017 // The line *must* be released by the core program.
1019 d->transfers[line].state = SDO_ABORTED_RCV;
1020 MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);
1023 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);
1027 // Error : Unknown cs
1028 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0]));
1035 /*******************************************************************)******/
1036 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
1037 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data)
1042 s_SDO sdo; // SDO to transmit
1048 UNS8 *pNodeIdServer;
1052 MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);
1053 MSG_WAR(0x3AC1, " At index : ", index);
1054 MSG_WAR(0x3AC2, " subIndex : ", subIndex);
1055 MSG_WAR(0x3AC3, " nb bytes : ", count);
1057 // Verify that there is no SDO communication yet.
1058 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1060 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);
1063 // Taking the line ...
1064 err = getSDOfreeLine( d, SDO_CLIENT, &line );
1066 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1069 // Check which SDO to use to communicate with the node
1070 offset = d->firstIndex->SDO_CLT;
1071 lastIndex = d->lastIndex->SDO_CLT;
1073 MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);
1077 while (offset <= lastIndex) {
1078 if (d->objdict[offset].bSubCount <= 3) {
1079 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i);
1082 // looking for the nodeId server
1083 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
1084 nodeIdServer = *pNodeIdServer;
1085 MSG_WAR(0x1AD2, "index : ", 0x1280 + i);
1086 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);
1088 if(nodeIdServer == nodeId) {
1096 MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId);
1099 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i);
1100 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);
1101 d->transfers[line].count = count;
1102 d->transfers[line].dataType = dataType;
1104 // Copy data to transfers structure.
1105 for (j = 0 ; j < count ; j++) {
1106 # ifdef CANOPEN_BIG_ENDIAN
1108 d->transfers[line].data[count - 1 - j] = ((char *)data)[j];
1109 else // String of bytes.
1110 d->transfers[line].data[j] = ((char *)data)[j];
1112 d->transfers[line].data[j] = ((char *)data)[j];
1115 // Send the SDO to the server. Initiate download, cs=1.
1116 sdo.nodeId = nodeId;
1117 if (count <= 4) { // Expedited transfert
1118 sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3;
1119 for (i = 4 ; i < 8 ; i++)
1120 sdo.body.data[i] = d->transfers[line].data[i - 4];
1121 d->transfers[line].offset = count;
1123 else { // Normal transfert
1124 sdo.body.data[0] = (1 << 5) | 1;
1125 sdo.body.data[4] = count; // nb of byte to transmit. Max = 255. (canfestival2 limitation).
1126 for (i = 5 ; i < 8 ; i++)
1127 sdo.body.data[i] = 0;
1129 sdo.body.data[1] = index & 0xFF; // LSB
1130 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
1131 sdo.body.data[3] = subIndex;
1133 err = sendSDO(d, SDO_CLIENT, sdo);
1135 MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);
1137 resetSDOline(d, line);
1145 /***************************************************************************/
1146 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType)
1154 s_SDO sdo; // SDO to transmit
1155 UNS8 *pNodeIdServer;
1160 MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);
1161 MSG_WAR(0x3AD6, " At index : ", index);
1162 MSG_WAR(0x3AD7, " subIndex : ", subIndex);
1165 // Verify that there is no SDO communication yet.
1166 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1168 MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);
1171 // Taking the line ...
1172 err = getSDOfreeLine( d, SDO_CLIENT, &line );
1174 MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);
1178 MSG_WAR(0x3AE0, "Transmission on line : ", line);
1180 // Check which SDO to use to communicate with the node
1181 offset = d->firstIndex->SDO_CLT;
1182 lastIndex = d->lastIndex->SDO_CLT;
1184 MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0);
1188 while (offset <= lastIndex) {
1189 if (d->objdict[offset].bSubCount <= 3) {
1190 MSG_ERR(0x1AE2, "Subindex 3 not found at index ", 0x1280 + i);
1193 // looking for the nodeId server
1194 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject;
1195 nodeIdServer = *pNodeIdServer;
1197 if(nodeIdServer == nodeId) {
1205 MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId);
1208 MSG_WAR(0x3AE4," SDO client defined at index : ", 0x1280 + i);
1209 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS);
1210 getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1211 sdo.nodeId = nodeId;
1212 // Send the SDO to the server. Initiate upload, cs=2.
1213 d->transfers[line].dataType = dataType;
1214 sdo.body.data[0] = (2 << 5);
1215 sdo.body.data[1] = index & 0xFF; // LSB
1216 sdo.body.data[2] = (index >> 8) & 0xFF; // MSB
1217 sdo.body.data[3] = subIndex;
1218 for (i = 4 ; i < 8 ; i++)
1219 sdo.body.data[i] = 0;
1220 err = sendSDO(d, SDO_CLIENT, sdo);
1222 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);
1224 resetSDOline(d, line);
1230 /***************************************************************************/
1232 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size,
1240 // Looking for the line tranfert.
1241 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1243 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);
1244 return SDO_ABORTED_INTERNAL;
1246 if (d->transfers[line].state != SDO_FINISHED)
1247 return d->transfers[line].state;
1249 // Transfert is finished. Put the value in the data.
1250 * size = d->transfers[line].count;
1251 for ( i = 0 ; i < *size ; i++) {
1252 # ifdef CANOPEN_BIG_ENDIAN
1253 if (d->transfers[line].dataType != visible_string)
1254 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i];
1255 else // String of bytes.
1256 ( (char *) data)[i] = d->transfers[line].data[i];
1258 ( (char *) data)[i] = d->transfers[line].data[i];
1261 return SDO_FINISHED;
1264 /***************************************************************************/
1266 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode)
1272 // Looking for the line tranfert.
1273 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line);
1275 MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);
1276 return SDO_ABORTED_INTERNAL;
1278 * abortCode = d->transfers[line].abortCode;
1279 return d->transfers[line].state;