nico@207: nico@207:
nico@207:00001 /* nico@207: 00002 This file is part of CanFestival, a library implementing CanOpen Stack. nico@207: 00003 nico@207: 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN nico@207: 00005 nico@207: 00006 See COPYING file for copyrights details. nico@207: 00007 nico@207: 00008 This library is free software; you can redistribute it and/or nico@207: 00009 modify it under the terms of the GNU Lesser General Public nico@207: 00010 License as published by the Free Software Foundation; either nico@207: 00011 version 2.1 of the License, or (at your option) any later version. nico@207: 00012 nico@207: 00013 This library is distributed in the hope that it will be useful, nico@207: 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of nico@207: 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@207: 00016 Lesser General Public License for more details. nico@207: 00017 nico@207: 00018 You should have received a copy of the GNU Lesser General Public nico@207: 00019 License along with this library; if not, write to the Free Software nico@207: 00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA nico@207: 00021 */ nico@210: 00032 /* #define DEBUG_WAR_CONSOLE_ON */ nico@210: 00033 /* #define DEBUG_ERR_CONSOLE_ON */ nico@210: 00034 nico@210: 00035 #include "objacces.h" nico@210: 00036 #include "sdo.h" nico@210: 00037 #include "canfestival.h" nico@210: 00038 nico@210: 00039 /* Uncomment if your compiler does not support inline functions */ nico@210: 00040 #define NO_INLINE nico@210: 00041 nico@210: 00042 #ifdef NO_INLINE nico@210: 00043 #define INLINE nico@210: 00044 #else nico@210: 00045 #define INLINE inline nico@210: 00046 #endif nico@210: 00047 nico@210: 00048 /*Internals prototypes*/ nico@207: 00049 nico@210: 00065 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, nico@210: 00066 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize); nico@210: 00067 nico@210: 00080 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, nico@210: 00081 UNS8 dataType, SDOCallback_t Callback); nico@210: 00082 nico@210: 00083 nico@210: 00084 /***************************************************************************/ nico@210: 00085 /* SDO (un)packing macros */ nico@210: 00086 nico@210: 00089 #define getSDOcs(byte) (byte >> 5) nico@210: 00090 nico@210: 00093 #define getSDOn2(byte) ((byte >> 2) & 3) nico@210: 00094 nico@210: 00097 #define getSDOn3(byte) ((byte >> 1) & 7) nico@210: 00098 nico@210: 00101 #define getSDOe(byte) ((byte >> 1) & 1) nico@210: 00102 nico@210: 00105 #define getSDOs(byte) (byte & 1) nico@210: 00106 nico@210: 00109 #define getSDOc(byte) (byte & 1) nico@210: 00110 nico@210: 00113 #define getSDOt(byte) ((byte >> 4) & 1) nico@210: 00114 nico@210: 00117 #define getSDOindex(byte1, byte2) ((byte2 << 8) | (byte1)) nico@210: 00118 nico@210: 00121 #define getSDOsubIndex(byte3) (byte3) nico@210: 00122 nico@210: 00129 void SDOTimeoutAlarm(CO_Data* d, UNS32 id) nico@210: 00130 { nico@210: 00131 MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0); nico@210: 00132 MSG_WAR(0x2A02, "server node : ", d->transfers[id].nodeId); nico@210: 00133 MSG_WAR(0x2A02, " index : ", d->transfers[id].index); nico@210: 00134 MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex); nico@210: 00135 /* Reset timer handler */ nico@210: 00136 d->transfers[id].timer = TIMER_NONE; nico@210: 00137 /*Set aborted state*/ nico@210: 00138 d->transfers[id].state = SDO_ABORTED_INTERNAL; nico@210: 00139 /* Sending a SDO abort */ nico@210: 00140 sendSDOabort(d, d->transfers[id].whoami, nico@210: 00141 d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT); nico@210: 00142 d->transfers[id].abortCode = SDOABT_TIMED_OUT; nico@210: 00143 /* Call the user function to inform of the problem.*/ nico@210: 00144 if(d->transfers[id].Callback) nico@210: 00145 /*If ther is a callback, it is responsible to close SDO transfer (client)*/ nico@210: 00146 (*d->transfers[id].Callback)(d,d->transfers[id].nodeId); nico@210: 00147 else if(d->transfers[id].whoami == SDO_SERVER) nico@210: 00148 /*Else, if server, reset the line*/ nico@210: 00149 resetSDOline(d, (UNS8)id); nico@210: 00150 } nico@210: 00151 nico@210: 00152 #define StopSDO_TIMER(id) \ nico@210: 00153 MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\ nico@210: 00154 d->transfers[id].timer = DelAlarm(d->transfers[id].timer); nico@210: 00155 nico@210: 00156 #define StartSDO_TIMER(id) \ nico@210: 00157 MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\ nico@210: 00158 d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0); nico@210: 00159 nico@210: 00160 #define RestartSDO_TIMER(id) \ nico@210: 00161 MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\ nico@210: 00162 if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) } nico@210: 00163 nico@210: 00169 void resetSDO (CO_Data* d) nico@210: 00170 { nico@210: 00171 UNS8 j; nico@210: 00172 nico@210: 00174 for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; j++) nico@210: 00175 resetSDOline(d, j); nico@210: 00176 } nico@210: 00177 nico@210: 00186 UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line) nico@210: 00187 { nico@210: 00188 UNS8 size; nico@210: 00189 UNS32 errorCode; nico@210: 00190 MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line); nico@210: 00191 size = (UNS8)d->transfers[line].count; nico@210: 00192 errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, nico@210: 00193 (void *) d->transfers[line].data, &size, 1); nico@210: 00194 if (errorCode != OD_SUCCESSFUL) nico@210: 00195 return errorCode; nico@210: 00196 MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line); nico@210: 00197 return 0; nico@210: 00198 nico@210: 00199 } nico@210: 00200 nico@210: 00209 UNS32 objdictToSDOline (CO_Data* d, UNS8 line) nico@210: 00210 { nico@210: 00211 UNS8 size = 0; nico@210: 00212 UNS8 dataType; nico@210: 00213 UNS32 errorCode; nico@210: 00214 nico@210: 00215 MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index); nico@210: 00216 MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex); nico@210: 00217 nico@210: 00218 errorCode = getODentry(d, d->transfers[line].index, nico@210: 00219 d->transfers[line].subIndex, nico@210: 00220 (void *)d->transfers[line].data, nico@210: 00221 &size, &dataType, 0); nico@210: 00222 nico@210: 00223 if (errorCode != OD_SUCCESSFUL) nico@210: 00224 return errorCode; nico@207: 00225 nico@210: 00226 d->transfers[line].count = size; nico@210: 00227 d->transfers[line].offset = 0; nico@210: 00228 #if 0 nico@210: 00229 /*Me laisser a, please ! (FD)*/ nico@210: 00230 { nico@210: 00231 UNS8 i; nico@210: 00232 for (i = 0 ; i < 10 ; i++) { nico@210: 00233 MSG_WAR(i, "data= ", d->transfers[line].data[i]); nico@210: 00234 } nico@210: 00235 } nico@210: 00236 #endif nico@210: 00237 return 0; nico@210: 00238 } nico@210: 00239 nico@210: 00250 UNS8 lineToSDO (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) { nico@210: 00251 UNS8 i; nico@210: 00252 UNS8 offset; nico@210: 00253 nico@210: 00254 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { nico@210: 00255 MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); nico@210: 00256 return 0xFF; nico@210: 00257 } nico@210: 00258 if ((d->transfers[line].offset + nbBytes) > d->transfers[line].count) { nico@210: 00259 MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes); nico@210: 00260 return 0xFF; nico@210: 00261 } nico@210: 00262 offset = (UNS8)d->transfers[line].offset; nico@210: 00263 for (i = 0 ; i < nbBytes ; i++) nico@210: 00264 * (data + i) = d->transfers[line].data[offset + i]; nico@210: 00265 d->transfers[line].offset = d->transfers[line].offset + nbBytes; nico@210: 00266 return 0; nico@210: 00267 } nico@210: 00268 nico@210: 00279 UNS8 SDOtoLine (CO_Data* d, UNS8 line, UNS8 nbBytes, UNS8* data) nico@210: 00280 { nico@210: 00281 UNS8 i; nico@210: 00282 UNS8 offset; nico@210: 00283 nico@210: 00284 if ((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFERT) { nico@210: 00285 MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); nico@210: 00286 return 0xFF; nico@210: 00287 } nico@210: 00288 offset = (UNS8)d->transfers[line].offset; nico@210: 00289 for (i = 0 ; i < nbBytes ; i++) nico@210: 00290 d->transfers[line].data[offset + i] = * (data + i); nico@210: 00291 d->transfers[line].offset = d->transfers[line].offset + nbBytes; nico@210: 00292 return 0; nico@210: 00293 } nico@210: 00294 nico@210: 00307 UNS8 failedSDO (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS16 index, nico@210: 00308 UNS8 subIndex, UNS32 abortCode) nico@210: 00309 { nico@210: 00310 UNS8 err; nico@210: 00311 UNS8 line; nico@210: 00312 err = getSDOlineOnUse( d, nodeId, whoami, &line ); nico@210: 00313 if (!err) /* If a line on use have been found.*/ nico@210: 00314 MSG_WAR(0x3A20, "FailedSDO : line found : ", line); nico@210: 00315 if ((! err) && (whoami == SDO_SERVER)) { nico@210: 00316 resetSDOline( d, line ); nico@210: 00317 MSG_WAR(0x3A21, "FailedSDO : line released : ", line); nico@210: 00318 } nico@210: 00319 if ((! err) && (whoami == SDO_CLIENT)) { nico@210: 00320 StopSDO_TIMER(line); nico@210: 00321 d->transfers[line].state = SDO_ABORTED_INTERNAL; nico@210: 00322 } nico@210: 00323 MSG_WAR(0x3A22, "Sending SDO abort ", 0); nico@210: 00324 err = sendSDOabort(d, whoami, index, subIndex, abortCode); nico@210: 00325 if (err) { nico@210: 00326 MSG_WAR(0x3A23, "Unable to send the SDO abort", 0); nico@210: 00327 return 0xFF; nico@210: 00328 } nico@210: 00329 return 0; nico@210: 00330 } nico@210: 00331 nico@210: 00338 void resetSDOline ( CO_Data* d, UNS8 line ) nico@210: 00339 { nico@210: 00340 UNS8 i; nico@210: 00341 MSG_WAR(0x3A25, "reset SDO line nb : ", line); nico@210: 00342 initSDOline(d, line, 0, 0, 0, SDO_RESET); nico@210: 00343 for (i = 0 ; i < SDO_MAX_LENGTH_TRANSFERT ; i++) nico@210: 00344 d->transfers[line].data[i] = 0; nico@210: 00345 } nico@210: 00346 nico@210: 00359 UNS8 initSDOline (CO_Data* d, UNS8 line, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 state) nico@210: 00360 { nico@210: 00361 MSG_WAR(0x3A25, "init SDO line nb : ", line); nico@210: 00362 if (state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS){ nico@210: 00363 StartSDO_TIMER(line) nico@210: 00364 }else{ nico@210: 00365 StopSDO_TIMER(line) nico@210: 00366 } nico@210: 00367 d->transfers[line].nodeId = nodeId; nico@210: 00368 d->transfers[line].index = index; nico@210: 00369 d->transfers[line].subIndex = subIndex; nico@210: 00370 d->transfers[line].state = state; nico@210: 00371 d->transfers[line].toggle = 0; nico@210: 00372 d->transfers[line].count = 0; nico@210: 00373 d->transfers[line].offset = 0; nico@210: 00374 d->transfers[line].dataType = 0; nico@210: 00375 d->transfers[line].Callback = NULL; nico@210: 00376 return 0; nico@210: 00377 } nico@210: 00378 nico@210: 00388 UNS8 getSDOfreeLine ( CO_Data* d, UNS8 whoami, UNS8 *line ) nico@210: 00389 { nico@210: 00390 nico@210: 00391 UNS8 i; nico@210: 00392 nico@210: 00393 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ nico@210: 00394 if ( d->transfers[i].state == SDO_RESET ) { nico@210: 00395 *line = i; nico@210: 00396 d->transfers[i].whoami = whoami; nico@210: 00397 return 0; nico@210: 00398 } /* end if */ nico@210: 00399 } /* end for */ nico@210: 00400 MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i); nico@210: 00401 return 0xFF; nico@210: 00402 } nico@210: 00403 nico@210: 00414 UNS8 getSDOlineOnUse (CO_Data* d, UNS8 nodeId, UNS8 whoami, UNS8 *line) nico@210: 00415 { nico@210: 00416 nico@210: 00417 UNS8 i; nico@210: 00418 nico@210: 00419 for (i = 0 ; i < SDO_MAX_SIMULTANEOUS_TRANSFERTS ; i++){ nico@210: 00420 if ( (d->transfers[i].state != SDO_RESET) && nico@210: 00421 (d->transfers[i].nodeId == nodeId) && nico@210: 00422 (d->transfers[i].whoami == whoami) ) { nico@210: 00423 *line = i; nico@210: 00424 return 0; nico@210: 00425 } nico@210: 00426 } nico@210: 00427 return 0xFF; nico@210: 00428 } nico@210: 00429 nico@210: 00439 UNS8 closeSDOtransfer (CO_Data* d, UNS8 nodeId, UNS8 whoami) nico@210: 00440 { nico@210: 00441 UNS8 err; nico@210: 00442 UNS8 line; nico@210: 00443 err = getSDOlineOnUse(d, nodeId, whoami, &line); nico@210: 00444 if (err) { nico@210: 00445 MSG_WAR(0x2A30, "No SDO communication to close for node : ", nodeId); nico@210: 00446 return 0xFF; nico@210: 00447 } nico@210: 00448 resetSDOline(d, line); nico@210: 00449 return 0; nico@210: 00450 } nico@210: 00451 nico@210: 00461 UNS8 getSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 * nbBytes) nico@210: 00462 { nico@210: 00463 if (d->transfers[line].count == 0) /* if received initiate SDO protocol with e=0 and s=0 */ nico@210: 00464 * nbBytes = 0; nico@210: 00465 else nico@210: 00466 * nbBytes = (UNS8)d->transfers[line].count - (UNS8)d->transfers[line].offset; nico@210: 00467 return 0; nico@210: 00468 } nico@210: 00469 nico@210: 00479 UNS8 setSDOlineRestBytes (CO_Data* d, UNS8 line, UNS8 nbBytes) nico@210: 00480 { nico@210: 00481 if (nbBytes > SDO_MAX_LENGTH_TRANSFERT) { nico@210: 00482 MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFERT", nbBytes); nico@210: 00483 return 0xFF; nico@210: 00484 } nico@210: 00485 d->transfers[line].count = nbBytes; nico@210: 00486 return 0; nico@210: 00487 } nico@210: 00488 nico@210: 00498 UNS8 sendSDO (CO_Data* d, UNS8 whoami, s_SDO sdo) nico@210: 00499 { nico@210: 00500 UNS16 offset; nico@210: 00501 UNS16 lastIndex; nico@210: 00502 UNS8 found = 0; nico@210: 00503 Message m; nico@210: 00504 UNS8 i; nico@210: 00505 UNS32 * pwCobId = NULL; nico@210: 00506 UNS32 * pwNodeId = NULL; nico@210: 00507 nico@210: 00508 MSG_WAR(0x3A38, "sendSDO",0); nico@210: 00509 if( !((d->nodeState == Operational) || (d->nodeState == Pre_operational ))) { nico@210: 00510 MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState); nico@210: 00511 return 0xFF; nico@210: 00512 } nico@210: 00513 nico@210: 00515 if ( whoami == SDO_SERVER ) { nico@210: 00516 offset = d->firstIndex->SDO_SVR; nico@210: 00517 if (offset == 0) { nico@210: 00518 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); nico@210: 00519 return 0xFF; nico@210: 00520 } nico@210: 00521 pwCobId = d->objdict[offset].pSubindex[2].pObject; nico@210: 00522 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); nico@210: 00523 } nico@210: 00524 else { nico@210: 00526 UNS16 sdoNum = 0; nico@210: 00527 offset = d->firstIndex->SDO_CLT; nico@210: 00528 lastIndex = d->lastIndex->SDO_CLT; nico@210: 00529 if (offset == 0) { nico@210: 00530 MSG_ERR(0x1A42, "SendSDO : No SDO client index found", 0); nico@210: 00531 return 0xFF; nico@210: 00532 } nico@210: 00534 while (offset <= lastIndex){ nico@210: 00535 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum); nico@210: 00536 if (d->objdict[offset].bSubCount <= 3) { nico@210: 00537 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum); nico@210: 00538 return 0xFF; nico@210: 00539 } nico@210: 00540 pwNodeId = d->objdict[offset].pSubindex[3].pObject; nico@210: 00541 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId); nico@210: 00542 if(*pwNodeId == sdo.nodeId) { nico@210: 00543 found = 1; nico@210: 00544 break; nico@210: 00545 } nico@210: 00546 offset ++; nico@210: 00547 sdoNum ++; nico@210: 00548 } nico@210: 00549 if (! found){ nico@210: 00550 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); nico@210: 00551 return 0xFF; nico@210: 00552 } nico@210: 00554 pwCobId = d->objdict[offset].pSubindex[1].pObject; nico@210: 00555 } nico@210: 00557 m.cob_id.w = *pwCobId; nico@210: 00558 m.rtr = NOT_A_REQUEST; nico@210: 00560 m.len = 8; nico@210: 00561 for (i = 0 ; i < 8 ; i++) { nico@210: 00562 m.data[i] = sdo.body.data[i]; nico@210: 00563 } nico@210: 00564 return canSend(d->canHandle,&m); nico@210: 00565 } nico@210: 00566 nico@210: 00578 UNS8 sendSDOabort (CO_Data* d, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode) nico@210: 00579 { nico@210: 00580 s_SDO sdo; nico@210: 00581 UNS8 ret; nico@210: 00582 MSG_WAR(0x2A50,"Sending SDO abort ", abortCode); nico@210: 00583 sdo.nodeId = *d->bDeviceNodeId; nico@210: 00584 sdo.body.data[0] = 0x80; nico@210: 00585 /* Index */ nico@210: 00586 sdo.body.data[1] = index & 0xFF; /* LSB */ nico@210: 00587 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ nico@210: 00588 /* Subindex */ nico@210: 00589 sdo.body.data[3] = subIndex; nico@210: 00590 /* Data */ nico@210: 00591 sdo.body.data[4] = (UNS8)(abortCode & 0xFF); nico@210: 00592 sdo.body.data[5] = (UNS8)((abortCode >> 8) & 0xFF); nico@210: 00593 sdo.body.data[6] = (UNS8)((abortCode >> 16) & 0xFF); nico@210: 00594 sdo.body.data[7] = (UNS8)((abortCode >> 24) & 0xFF); nico@210: 00595 ret = sendSDO(d, whoami, sdo); nico@210: 00596 nico@210: 00597 return ret; nico@210: 00598 } nico@210: 00599 nico@210: 00608 UNS8 proceedSDO (CO_Data* d, Message *m) nico@210: 00609 { nico@210: 00610 UNS8 err; nico@210: 00611 UNS8 line; nico@210: 00612 UNS8 nbBytes; nico@210: 00613 UNS8 nodeId = 0; nico@210: 00614 UNS32 nodeId_32; nico@210: 00615 UNS32 *pNodeId = NULL; nico@210: 00616 UNS8 whoami = SDO_UNKNOWN; nico@210: 00617 UNS32 errorCode; nico@210: 00618 s_SDO sdo; nico@210: 00619 UNS16 index; nico@210: 00620 UNS8 subIndex; nico@210: 00621 UNS32 abortCode; nico@210: 00622 UNS8 i,j; nico@210: 00623 UNS32 * pCobId = NULL; nico@210: 00624 UNS16 offset; nico@210: 00625 UNS16 lastIndex; nico@210: 00626 nico@210: 00627 MSG_WAR(0x3A60, "proceedSDO ", 0); nico@210: 00628 whoami = SDO_UNKNOWN; nico@210: 00631 offset = d->firstIndex->SDO_SVR; nico@210: 00632 lastIndex = d->lastIndex->SDO_SVR; nico@210: 00633 j = 0; nico@210: 00634 if(offset) while (offset <= lastIndex) { nico@210: 00635 if (d->objdict[offset].bSubCount <= 1) { nico@210: 00636 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); nico@210: 00637 return 0xFF; nico@210: 00638 } nico@210: 00639 pCobId = d->objdict[offset].pSubindex[1].pObject; nico@210: 00640 if ( *pCobId == (*m).cob_id.w ) { nico@210: 00641 whoami = SDO_SERVER; nico@210: 00642 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); nico@210: 00645 nodeId = j; nico@210: 00646 break; nico@210: 00647 } nico@210: 00648 j++; nico@210: 00649 offset++; nico@210: 00650 } nico@210: 00651 if (whoami == SDO_UNKNOWN) { nico@210: 00653 offset = d->firstIndex->SDO_CLT; nico@210: 00654 lastIndex = d->lastIndex->SDO_CLT; nico@210: 00655 j = 0; nico@210: 00656 if(offset) while (offset <= lastIndex) { nico@210: 00657 if (d->objdict[offset].bSubCount <= 3) { nico@210: 00658 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); nico@210: 00659 return 0xFF; nico@210: 00660 } nico@210: 00662 pCobId = d->objdict[offset].pSubindex[2].pObject; nico@210: 00663 if (*pCobId == (*m).cob_id.w ) { nico@210: 00665 pNodeId = d->objdict[offset].pSubindex[3].pObject; nico@210: 00666 whoami = SDO_CLIENT; nico@210: 00667 nodeId_32 = *pNodeId; nico@210: 00668 nodeId = (UNS8)nodeId_32; nico@210: 00669 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); nico@210: 00670 MSG_WAR(0x3A65, " Server nodeId : ", nodeId); nico@210: 00671 break; nico@210: 00672 } nico@210: 00673 j++; nico@210: 00674 offset++; nico@210: 00675 } /* end while */ nico@210: 00676 } nico@210: 00677 if (whoami == SDO_UNKNOWN) { nico@210: 00678 return 0xFF; nico@210: 00679 } nico@210: 00680 nico@210: 00682 if ( (*m).len != 8) { nico@210: 00683 MSG_ERR(0x1A67, "Error size SDO. CobId : ", (*m).cob_id.w); nico@210: 00684 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_GENERAL_ERROR); nico@210: 00685 return 0xFF; nico@210: 00686 } nico@210: 00687 nico@210: 00688 if (whoami == SDO_CLIENT) { nico@210: 00689 MSG_WAR(0x3A68, "I am CLIENT. Received SDO from nodeId : ", nodeId); nico@210: 00690 } nico@210: 00691 else { nico@210: 00692 MSG_WAR(0x3A69, "I am SERVER. Received SDO cobId : ", (*m).cob_id.w); nico@210: 00693 } nico@210: 00694 nico@210: 00698 switch (getSDOcs(m->data[0])) { nico@210: 00699 nico@210: 00700 case 0: nico@210: 00702 if (whoami == SDO_SERVER) { nico@210: 00705 err = getSDOlineOnUse( d, nodeId, whoami, &line ); nico@210: 00706 if (!err) nico@210: 00707 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; nico@210: 00708 if (err) { nico@210: 00709 MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", nico@210: 00710 nodeId); nico@210: 00711 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); nico@210: 00712 return 0xFF; nico@210: 00713 } nico@210: 00715 RestartSDO_TIMER(line) nico@210: 00716 MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", nodeId); nico@210: 00717 index = d->transfers[line].index; nico@210: 00718 subIndex = d->transfers[line].subIndex; nico@210: 00720 if (d->transfers[line].toggle != getSDOt(m->data[0])) { nico@210: 00721 MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0])); nico@210: 00722 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); nico@210: 00723 return 0xFF; nico@210: 00724 } nico@210: 00726 nbBytes = 7 - getSDOn3(m->data[0]); nico@210: 00728 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); nico@207: 00729 if (err) { nico@210: 00730 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 00731 return 0xFF; nico@210: 00732 } nico@210: 00734 sdo.nodeId = *d->bDeviceNodeId; nico@210: 00735 sdo.body.data[0] = (1 << 5) | (d->transfers[line].toggle << 4); nico@210: 00736 for (i = 1 ; i < 8 ; i++) nico@210: 00737 sdo.body.data[i] = 0; nico@210: 00738 MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", nodeId); nico@210: 00739 sendSDO(d, whoami, sdo); nico@210: 00741 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; nico@210: 00743 if (getSDOc(m->data[0])) { nico@210: 00746 errorCode = SDOlineToObjdict(d, line); nico@210: 00747 if (errorCode) { nico@210: 00748 MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0); nico@210: 00749 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); nico@210: 00750 return 0xFF; nico@210: 00751 } nico@210: 00753 resetSDOline(d, line); nico@210: 00754 MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", nodeId); nico@210: 00755 } nico@210: 00756 } /* end if SERVER */ nico@210: 00757 else { /* if CLIENT */ nico@210: 00760 err = getSDOlineOnUse( d, nodeId, whoami, &line); nico@210: 00761 if (!err) nico@210: 00762 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; nico@210: 00763 if (err) { nico@210: 00764 MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); nico@210: 00765 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); nico@210: 00766 return 0xFF; nico@210: 00767 } nico@210: 00769 RestartSDO_TIMER(line) nico@210: 00770 index = d->transfers[line].index; nico@210: 00771 subIndex = d->transfers[line].subIndex; nico@210: 00773 if (d->transfers[line].toggle != getSDOt(m->data[0])) { nico@210: 00774 MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId); nico@210: 00775 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); nico@210: 00776 return 0xFF; nico@210: 00777 } nico@210: 00779 nbBytes = 7 - getSDOn3(m->data[0]); nico@210: 00781 err = SDOtoLine(d, line, nbBytes, (*m).data + 1); nico@210: 00782 if (err) { nico@210: 00783 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 00784 return 0xFF; nico@210: 00785 } nico@210: 00787 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; nico@210: 00789 if ( getSDOc(m->data[0])) { nico@210: 00792 StopSDO_TIMER(line) nico@210: 00793 d->transfers[line].state = SDO_FINISHED; nico@210: 00794 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); nico@210: 00795 nico@210: 00796 MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId); nico@210: 00797 } nico@210: 00798 else { nico@210: 00800 sdo.nodeId = nodeId; nico@210: 00801 sdo.body.data[0] = (3 << 5) | (d->transfers[line].toggle << 4); nico@210: 00802 for (i = 1 ; i < 8 ; i++) nico@210: 00803 sdo.body.data[i] = 0; nico@210: 00804 sendSDO(d, whoami, sdo); nico@210: 00805 MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId); nico@210: 00806 } nico@210: 00807 } /* End if CLIENT */ nico@210: 00808 break; nico@210: 00809 nico@210: 00810 case 1: nico@210: 00813 if (whoami == SDO_SERVER) { nico@210: 00814 index = getSDOindex(m->data[1],m->data[2]); nico@210: 00815 subIndex = getSDOsubIndex(m->data[3]); nico@210: 00816 MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", nico@210: 00817 nodeId); nico@210: 00818 MSG_WAR(0x3A80, "Writing at index : ", index); nico@210: 00819 MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex); nico@210: 00820 nico@210: 00822 err = getSDOlineOnUse( d, nodeId, whoami, &line ); nico@210: 00823 if (! err) { nico@210: 00824 MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0); nico@210: 00825 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); nico@210: 00826 return 0xFF; nico@210: 00827 } nico@210: 00830 err = getSDOfreeLine( d, whoami, &line ); nico@210: 00831 if (err) { nico@210: 00832 MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0); nico@210: 00833 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); nico@210: 00834 return 0xFF; nico@210: 00835 } nico@210: 00836 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); nico@210: 00837 nico@210: 00838 if (getSDOe(m->data[0])) { nico@210: 00840 nbBytes = 4 - getSDOn2(m->data[0]); nico@210: 00842 d->transfers[line].count = nbBytes; nico@210: 00843 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); nico@210: 00844 nico@210: 00845 if (err) { nico@210: 00846 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 00847 return 0xFF; nico@210: 00848 } nico@210: 00849 nico@210: 00852 MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfert. Finished.: ", nodeId); nico@210: 00854 errorCode = SDOlineToObjdict(d, line); nico@210: 00855 if (errorCode) { nico@210: 00856 MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0); nico@210: 00857 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); nico@210: 00858 return 0xFF; nico@210: 00859 } nico@210: 00861 resetSDOline(d, line); nico@210: 00862 } nico@210: 00863 else { nico@210: 00864 if (getSDOs(m->data[0])) { nico@210: 00866 nbBytes = m->data[4]; nico@210: 00867 err = setSDOlineRestBytes(d, nodeId, nbBytes); nico@210: 00868 if (err) { nico@210: 00869 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 00870 return 0xFF; nico@210: 00871 } nico@210: 00872 } nico@210: 00873 } nico@210: 00875 sdo.nodeId = *d->bDeviceNodeId; nico@210: 00876 sdo.body.data[0] = 3 << 5; nico@210: 00877 sdo.body.data[1] = index & 0xFF; /* LSB */ nico@210: 00878 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ nico@210: 00879 sdo.body.data[3] = subIndex; nico@210: 00880 for (i = 4 ; i < 8 ; i++) nico@210: 00881 sdo.body.data[i] = 0; nico@210: 00882 sendSDO(d, whoami, sdo); nico@210: 00883 } /* end if I am SERVER */ nico@210: 00884 else { nico@210: 00887 err = getSDOlineOnUse( d, nodeId, whoami, &line); nico@210: 00888 if (!err) nico@210: 00889 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; nico@210: 00890 if (err) { nico@210: 00891 MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId); nico@210: 00892 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); nico@210: 00893 return 0xFF; nico@210: 00894 } nico@210: 00896 RestartSDO_TIMER(line) nico@210: 00897 index = d->transfers[line].index; nico@210: 00898 subIndex = d->transfers[line].subIndex; nico@210: 00900 if (d->transfers[line].toggle != getSDOt(m->data[0])) { nico@210: 00901 MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId); nico@210: 00902 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); nico@210: 00903 return 0xFF; nico@210: 00904 } nico@210: 00905 nico@210: 00907 getSDOlineRestBytes(d, line, &nbBytes); nico@210: 00908 if (nbBytes == 0) { nico@210: 00909 MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId); nico@210: 00910 StopSDO_TIMER(line) nico@210: 00911 d->transfers[line].state = SDO_FINISHED; nico@210: 00912 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); nico@210: 00913 return 0x00; nico@210: 00914 } nico@210: 00916 if (nbBytes > 7) { nico@210: 00919 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; nico@210: 00920 sdo.nodeId = nodeId; /* The server node Id; */ nico@210: 00921 sdo.body.data[0] = (d->transfers[line].toggle << 4); nico@210: 00922 err = lineToSDO(d, line, 7, sdo.body.data + 1); nico@210: 00923 if (err) { nico@210: 00924 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 00925 return 0xFF; nico@210: 00926 } nico@210: 00927 } nico@210: 00928 else { nico@210: 00931 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; nico@210: 00932 sdo.nodeId = nodeId; /* The server node Id; */ nico@210: 00933 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; nico@210: 00934 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); nico@210: 00935 if (err) { nico@210: 00936 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 00937 return 0xFF; nico@210: 00938 } nico@210: 00939 for (i = nbBytes + 1 ; i < 8 ; i++) nico@210: 00940 sdo.body.data[i] = 0; nico@210: 00941 } nico@210: 00942 MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId); nico@210: 00943 sendSDO(d, whoami, sdo); nico@210: 00944 } nico@210: 00945 break; nico@210: 00946 nico@210: 00947 case 2: nico@210: 00950 if (whoami == SDO_SERVER) { nico@210: 00951 index = getSDOindex(m->data[1],m->data[2]); nico@210: 00952 subIndex = getSDOsubIndex(m->data[3]); nico@210: 00953 MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", nico@210: 00954 nodeId); nico@210: 00955 MSG_WAR(0x3A90, "Reading at index : ", index); nico@210: 00956 MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex); nico@210: 00958 err = getSDOlineOnUse( d, nodeId, whoami, &line ); nico@210: 00959 if (! err) { nico@210: 00960 MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line); nico@210: 00961 MSG_WAR(0x3A93, "nodeId = ", nodeId); nico@210: 00962 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); nico@210: 00963 return 0xFF; nico@210: 00964 } nico@210: 00967 err = getSDOfreeLine( d, whoami, &line ); nico@210: 00968 if (err) { nico@210: 00969 MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0); nico@210: 00970 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR); nico@210: 00971 return 0xFF; nico@210: 00972 } nico@210: 00973 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); nico@210: 00975 errorCode = objdictToSDOline(d, line); nico@210: 00976 nico@210: 00977 if (errorCode) { nico@210: 00978 MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", nico@210: 00979 errorCode); nico@210: 00980 failedSDO(d, nodeId, whoami, index, subIndex, errorCode); nico@210: 00981 return 0xFF; nico@210: 00982 } nico@210: 00984 getSDOlineRestBytes(d, line, &nbBytes); nico@210: 00985 sdo.nodeId = nodeId; nico@210: 00986 if (nbBytes > 4) { nico@210: 00989 sdo.body.data[0] = (2 << 5) | 1; nico@210: 00990 sdo.body.data[1] = index & 0xFF; /* LSB */ nico@210: 00991 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ nico@210: 00992 sdo.body.data[3] = subIndex; nico@210: 00993 sdo.body.data[4] = nbBytes; /* Limitation of canfestival2 : Max tranfert is 256 bytes.*/ nico@210: 00996 for (i = 5 ; i < 8 ; i++) nico@210: 00997 sdo.body.data[i] = 0; nico@210: 00998 MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId); nico@210: 00999 sendSDO(d, whoami, sdo); nico@210: 01000 } nico@210: 01001 else { nico@210: 01003 sdo.body.data[0] = (2 << 5) | ((4 - nbBytes) << 2) | 3; nico@210: 01004 sdo.body.data[1] = index & 0xFF; /* LSB */ nico@210: 01005 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ nico@210: 01006 sdo.body.data[3] = subIndex; nico@210: 01007 err = lineToSDO(d, line, nbBytes, sdo.body.data + 4); nico@210: 01008 if (err) { nico@210: 01009 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 01010 return 0xFF; nico@210: 01011 } nico@210: 01012 for (i = 4 + nbBytes ; i < 8 ; i++) nico@210: 01013 sdo.body.data[i] = 0; nico@210: 01014 MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", nico@210: 01015 nodeId); nico@210: 01016 sendSDO(d, whoami, sdo); nico@210: 01018 resetSDOline(d, line); nico@210: 01019 } nico@210: 01020 } nico@210: 01021 else { nico@210: 01025 err = getSDOlineOnUse( d, nodeId, whoami, &line); nico@210: 01026 if (!err) nico@210: 01027 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; nico@210: 01028 if (err) { nico@210: 01029 MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId); nico@210: 01030 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); nico@210: 01031 return 0xFF; nico@210: 01032 } nico@210: 01034 RestartSDO_TIMER(line) nico@210: 01035 index = d->transfers[line].index; nico@210: 01036 subIndex = d->transfers[line].subIndex; nico@210: 01037 nico@210: 01038 if (getSDOe(m->data[0])) { /* If SDO expedited */ nico@210: 01040 nbBytes = 4 - getSDOn2(m->data[0]); nico@210: 01042 err = SDOtoLine(d, line, nbBytes, (*m).data + 4); nico@210: 01043 if (err) { nico@210: 01044 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 01045 return 0xFF; nico@210: 01046 } nico@210: 01048 MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId); nico@210: 01049 StopSDO_TIMER(line) nico@210: 01050 d->transfers[line].count = nbBytes; nico@210: 01051 d->transfers[line].state = SDO_FINISHED; nico@210: 01052 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); nico@210: 01053 return 0; nico@210: 01054 } nico@210: 01055 else { nico@210: 01057 if (getSDOs(m->data[0])) { nico@210: 01058 nbBytes = m->data[4]; nico@210: 01059 err = setSDOlineRestBytes(d, line, nbBytes); nico@210: 01060 if (err) { nico@210: 01061 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 01062 return 0xFF; nico@210: 01063 } nico@210: 01064 } nico@210: 01066 sdo.nodeId = nodeId; nico@210: 01067 sdo.body.data[0] = 3 << 5; nico@210: 01068 for (i = 1 ; i < 8 ; i++) nico@210: 01069 sdo.body.data[i] = 0; nico@210: 01070 MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId); nico@210: 01071 sendSDO(d, whoami, sdo); nico@210: 01072 } nico@210: 01073 } nico@210: 01074 break; nico@210: 01075 nico@210: 01076 case 3: nico@210: 01078 if (whoami == SDO_SERVER) { nico@210: 01081 err = getSDOlineOnUse( d, nodeId, whoami, &line ); nico@210: 01082 if (!err) nico@210: 01083 err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS; nico@210: 01084 if (err) { nico@210: 01085 MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", nico@210: 01086 nodeId); nico@210: 01087 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); nico@210: 01088 return 0xFF; nico@210: 01089 } nico@210: 01091 RestartSDO_TIMER(line) nico@210: 01092 MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", nodeId); nico@210: 01093 index = d->transfers[line].index; nico@210: 01094 subIndex = d->transfers[line].subIndex; nico@210: 01096 if (d->transfers[line].toggle != getSDOt(m->data[0])) { nico@210: 01097 MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0])); nico@210: 01098 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED); nico@210: 01099 return 0xFF; nico@210: 01100 } nico@210: 01102 getSDOlineRestBytes(d, line, &nbBytes); nico@210: 01103 if (nbBytes > 7) { nico@210: 01106 sdo.nodeId = nodeId; /* The server node Id; */ nico@210: 01107 sdo.body.data[0] = (d->transfers[line].toggle << 4); nico@210: 01108 err = lineToSDO(d, line, 7, sdo.body.data + 1); nico@210: 01109 if (err) { nico@210: 01110 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 01111 return 0xFF; nico@210: 01112 } nico@210: 01114 d->transfers[line].toggle = ! d->transfers[line].toggle & 1; nico@210: 01115 MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", nodeId); nico@210: 01116 sendSDO(d, whoami, sdo); nico@210: 01117 } nico@210: 01118 else { nico@210: 01121 sdo.nodeId = nodeId; nico@210: 01122 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; nico@210: 01123 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); nico@210: 01124 if (err) { nico@210: 01125 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 01126 return 0xFF; nico@210: 01127 } nico@210: 01128 for (i = nbBytes + 1 ; i < 8 ; i++) nico@210: 01129 sdo.body.data[i] = 0; nico@210: 01130 MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", nodeId); nico@210: 01131 sendSDO(d, whoami, sdo); nico@210: 01133 resetSDOline(d, line); nico@210: 01134 } nico@210: 01135 } /* end if SERVER*/ nico@210: 01136 else { nico@210: 01140 err = getSDOlineOnUse( d, nodeId, whoami, &line); nico@210: 01141 if (!err) nico@210: 01142 err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS; nico@210: 01143 if (err) { nico@210: 01144 MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId); nico@210: 01145 failedSDO(d, nodeId, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR); nico@210: 01146 return 0xFF; nico@210: 01147 } nico@210: 01149 RestartSDO_TIMER(line) nico@210: 01150 index = d->transfers[line].index; nico@210: 01151 subIndex = d->transfers[line].subIndex; nico@210: 01152 /* End transmission or requesting next segment. */ nico@210: 01153 getSDOlineRestBytes(d, line, &nbBytes); nico@210: 01154 if (nbBytes == 0) { nico@210: 01155 MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId); nico@210: 01156 StopSDO_TIMER(line) nico@210: 01157 d->transfers[line].state = SDO_FINISHED; nico@210: 01158 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); nico@210: 01159 return 0x00; nico@210: 01160 } nico@210: 01161 if (nbBytes > 7) { nico@210: 01164 sdo.nodeId = nodeId; nico@210: 01165 sdo.body.data[0] = (d->transfers[line].toggle << 4); nico@210: 01166 err = lineToSDO(d, line, 7, sdo.body.data + 1); nico@210: 01167 if (err) { nico@210: 01168 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 01169 return 0xFF; nico@210: 01170 } nico@210: 01171 } nico@210: 01172 else { nico@210: 01175 sdo.nodeId = nodeId; nico@210: 01176 sdo.body.data[0] = (d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1; nico@210: 01177 err = lineToSDO(d, line, nbBytes, sdo.body.data + 1); nico@210: 01178 if (err) { nico@210: 01179 failedSDO(d, nodeId, whoami, index, subIndex, SDOABT_GENERAL_ERROR); nico@210: 01180 return 0xFF; nico@210: 01181 } nico@210: 01182 for (i = nbBytes + 1 ; i < 8 ; i++) nico@210: 01183 sdo.body.data[i] = 0; nico@210: 01184 } nico@210: 01185 MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId); nico@210: 01186 sendSDO(d, whoami, sdo); nico@207: 01187 nico@210: 01188 } /* end if I am a CLIENT */ nico@210: 01189 break; nico@210: 01190 nico@210: 01191 case 4: nico@210: 01192 abortCode = (*m).data[3] | nico@210: 01193 ((UNS32)m->data[5] << 8) | nico@210: 01194 ((UNS32)m->data[6] << 16) | nico@210: 01195 ((UNS32)m->data[7] << 24); nico@210: 01198 if (whoami == SDO_SERVER) { nico@210: 01199 err = getSDOlineOnUse( d, nodeId, whoami, &line ); nico@210: 01200 if (!err) { nico@210: 01201 resetSDOline( d, line ); nico@210: 01202 MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode); nico@210: 01203 } nico@210: 01204 else nico@210: 01205 MSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode); nico@210: 01208 } nico@210: 01209 else { nico@210: 01210 err = getSDOlineOnUse( d, nodeId, whoami, &line ); nico@210: 01211 if (!err) { nico@210: 01213 StopSDO_TIMER(line) nico@210: 01214 d->transfers[line].state = SDO_ABORTED_RCV; nico@210: 01215 d->transfers[line].abortCode = abortCode; nico@210: 01216 MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode); nico@210: 01217 if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId); nico@210: 01218 } nico@210: 01219 else nico@210: 01220 MSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode); nico@210: 01221 } nico@210: 01222 break; nico@210: 01223 default: nico@210: 01225 MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", getSDOcs(m->data[0])); nico@210: 01226 return 0xFF; nico@210: 01227 nico@210: 01228 } /* End switch */ nico@210: 01229 return 0; nico@210: 01230 } nico@210: 01231 nico@210: 01247 INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, nico@210: 01248 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize) nico@210: 01249 { nico@210: 01250 UNS8 err; nico@210: 01251 UNS8 SDOfound = 0; nico@210: 01252 UNS8 line; nico@210: 01253 s_SDO sdo; /* SDO to transmit */ nico@210: 01254 UNS8 i, j; nico@210: 01255 UNS16 lastIndex; nico@210: 01256 UNS16 offset; nico@210: 01257 UNS32 *pNodeIdServer; nico@210: 01258 UNS32 nodeIdServer; nico@210: 01259 nico@210: 01260 MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId); nico@210: 01261 MSG_WAR(0x3AC1, " At index : ", index); nico@210: 01262 MSG_WAR(0x3AC2, " subIndex : ", subIndex); nico@210: 01263 MSG_WAR(0x3AC3, " nb bytes : ", count); nico@210: 01264 nico@210: 01266 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); nico@210: 01267 if (!err) { nico@210: 01268 MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId); nico@210: 01269 return 0xFF; nico@210: 01270 } nico@210: 01272 err = getSDOfreeLine( d, SDO_CLIENT, &line ); nico@210: 01273 if (err) { nico@210: 01274 MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); nico@210: 01275 return (0xFF); nico@210: 01276 } nico@210: 01278 offset = d->firstIndex->SDO_CLT; nico@210: 01279 lastIndex = d->lastIndex->SDO_CLT; nico@210: 01280 if (offset == 0) { nico@210: 01281 MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0); nico@210: 01282 return 0xFF; nico@210: 01283 } nico@210: 01284 i = 0; nico@210: 01285 while (offset <= lastIndex) { nico@210: 01286 if (d->objdict[offset].bSubCount <= 3) { nico@210: 01287 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i); nico@210: 01288 return 0xFF; nico@210: 01289 } nico@210: 01291 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; nico@210: 01292 nodeIdServer = *pNodeIdServer; nico@210: 01293 MSG_WAR(0x1AD2, "index : ", 0x1280 + i); nico@210: 01294 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer); nico@210: 01295 nico@210: 01296 if(nodeIdServer == (UNS32)nodeId) { nico@210: 01297 SDOfound = 1; nico@210: 01298 break; nico@210: 01299 } nico@210: 01300 offset++; nico@210: 01301 i++; nico@210: 01302 } /* end while */ nico@210: 01303 if (!SDOfound) { nico@210: 01304 MSG_ERR(0x1AC9, "SDO. Error. No client found to communicate with node : ", nodeId); nico@210: 01305 return 0xFF; nico@210: 01306 } nico@210: 01307 MSG_WAR(0x3AD0," SDO client defined at index : ", 0x1280 + i); nico@210: 01308 initSDOline(d, line, nodeId, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS); nico@210: 01309 d->transfers[line].count = count; nico@210: 01310 d->transfers[line].dataType = dataType; nico@210: 01311 nico@210: 01313 for (j = 0 ; j < count ; j++) { nico@210: 01314 # ifdef CANOPEN_BIG_ENDIAN nico@210: 01315 if (dataType == 0 && endianize) nico@210: 01316 d->transfers[line].data[count - 1 - j] = ((char *)data)[j]; nico@210: 01317 else /* String of bytes. */ nico@210: 01318 d->transfers[line].data[j] = ((char *)data)[j]; nico@210: 01319 # else nico@210: 01320 d->transfers[line].data[j] = ((char *)data)[j]; nico@210: 01321 # endif nico@210: 01322 } nico@210: 01324 sdo.nodeId = nodeId; nico@210: 01325 if (count <= 4) { nico@210: 01326 sdo.body.data[0] = (1 << 5) | ((4 - count) << 2) | 3; nico@210: 01327 for (i = 4 ; i < 8 ; i++) nico@210: 01328 sdo.body.data[i] = d->transfers[line].data[i - 4]; nico@210: 01329 d->transfers[line].offset = count; nico@210: 01330 } nico@210: 01331 else { nico@210: 01332 sdo.body.data[0] = (1 << 5) | 1; nico@210: 01333 sdo.body.data[4] = count; nico@210: 01334 for (i = 5 ; i < 8 ; i++) nico@210: 01335 sdo.body.data[i] = 0; nico@210: 01336 } nico@210: 01337 sdo.body.data[1] = index & 0xFF; /* LSB */ nico@210: 01338 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ nico@210: 01339 sdo.body.data[3] = subIndex; nico@210: 01340 nico@210: 01341 d->transfers[line].Callback = Callback; nico@210: 01342 nico@210: 01343 err = sendSDO(d, SDO_CLIENT, sdo); nico@210: 01344 if (err) { nico@210: 01345 MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId); nico@210: 01347 resetSDOline(d, line); nico@210: 01348 return 0xFF; nico@210: 01349 } nico@210: 01350 nico@210: 01351 nico@210: 01352 return 0; nico@210: 01353 } nico@210: 01354 nico@210: 01368 UNS8 writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, nico@210: 01369 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data) nico@210: 01370 { nico@210: 01371 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, NULL, 1); nico@210: 01372 } nico@210: 01373 nico@210: 01388 UNS8 writeNetworkDictCallBack (CO_Data* d, UNS8 nodeId, UNS16 index, nico@210: 01389 UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback) nico@210: 01390 { nico@210: 01391 return _writeNetworkDict (d, nodeId, index, subIndex, count, dataType, data, Callback, 1); nico@210: 01392 } nico@210: 01393 nico@210: 01406 INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) nico@210: 01407 { nico@210: 01408 UNS8 err; nico@210: 01409 UNS8 SDOfound = 0; nico@210: 01410 UNS8 i; nico@210: 01411 UNS8 line; nico@210: 01412 s_SDO sdo; /* SDO to transmit */ nico@210: 01413 UNS32 *pNodeIdServer; nico@210: 01414 UNS32 nodeIdServer; nico@210: 01415 UNS16 offset; nico@210: 01416 UNS16 lastIndex; nico@210: 01417 MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId); nico@210: 01418 MSG_WAR(0x3AD6, " At index : ", index); nico@210: 01419 MSG_WAR(0x3AD7, " subIndex : ", subIndex); nico@210: 01420 nico@210: 01421 nico@210: 01423 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); nico@210: 01424 if (!err) { nico@210: 01425 MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId); nico@210: 01426 return 0xFF; nico@210: 01427 } nico@210: 01429 err = getSDOfreeLine( d, SDO_CLIENT, &line ); nico@210: 01430 if (err) { nico@210: 01431 MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId); nico@210: 01432 return (0xFF); nico@210: 01433 } nico@210: 01434 else nico@210: 01435 MSG_WAR(0x3AE0, "Transmission on line : ", line); nico@210: 01436 nico@210: 01438 offset = d->firstIndex->SDO_CLT; nico@210: 01439 lastIndex = d->lastIndex->SDO_CLT; nico@210: 01440 if (offset == 0) { nico@210: 01441 MSG_ERR(0x1AE1, "writeNetworkDict : No SDO client index found", 0); nico@210: 01442 return 0xFF; nico@210: 01443 } nico@210: 01444 i = 0; nico@210: 01445 while (offset <= lastIndex) { nico@210: 01446 if (d->objdict[offset].bSubCount <= 3) { nico@210: 01447 MSG_ERR(0x1AE2, "Subindex 3 not found at index ", 0x1280 + i); nico@210: 01448 return 0xFF; nico@210: 01449 } nico@210: 01451 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; nico@210: 01452 nodeIdServer = *pNodeIdServer; nico@210: 01453 nico@210: 01454 if(nodeIdServer == (UNS32)nodeId) { nico@210: 01455 SDOfound = 1; nico@210: 01456 break; nico@210: 01457 } nico@210: 01458 offset++; nico@210: 01459 i++; nico@210: 01460 } /* end while */ nico@210: 01461 if (!SDOfound) { nico@210: 01462 MSG_ERR(0x1AE3, "SDO. Error. No client found to communicate with node : ", nodeId); nico@210: 01463 return 0xFF; nico@210: 01464 } nico@210: 01465 MSG_WAR(0x3AE4," SDO client defined at index : ", 0x1280 + i); nico@210: 01466 initSDOline(d, line, nodeId, index, subIndex, SDO_UPLOAD_IN_PROGRESS); nico@210: 01467 getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); nico@210: 01468 sdo.nodeId = nodeId; nico@210: 01470 d->transfers[line].dataType = dataType; nico@210: 01471 sdo.body.data[0] = (2 << 5); nico@210: 01472 sdo.body.data[1] = index & 0xFF; /* LSB */ nico@210: 01473 sdo.body.data[2] = (index >> 8) & 0xFF; /* MSB */ nico@210: 01474 sdo.body.data[3] = subIndex; nico@210: 01475 for (i = 4 ; i < 8 ; i++) nico@210: 01476 sdo.body.data[i] = 0; nico@210: 01477 d->transfers[line].Callback = Callback; nico@210: 01478 err = sendSDO(d, SDO_CLIENT, sdo); nico@210: 01479 if (err) { nico@210: 01480 MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId); nico@210: 01481 /* release the line */ nico@210: 01482 resetSDOline(d, line); nico@210: 01483 return 0xFF; nico@210: 01484 } nico@210: 01485 return 0; nico@210: 01486 } nico@210: 01487 nico@210: 01499 UNS8 readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType) nico@210: 01500 { nico@210: 01501 return _readNetworkDict (d, nodeId, index, subIndex, dataType, NULL); nico@210: 01502 } nico@210: 01503 nico@210: 01516 UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback) nico@210: 01517 { nico@210: 01518 return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback); nico@210: 01519 } nico@210: 01520 nico@210: 01532 UNS8 getReadResultNetworkDict (CO_Data* d, UNS8 nodeId, void* data, UNS8 *size, nico@210: 01533 UNS32 * abortCode) nico@210: 01534 { nico@210: 01535 UNS8 i; nico@210: 01536 UNS8 err; nico@210: 01537 UNS8 line; nico@210: 01538 * size = 0; nico@210: 01539 nico@210: 01541 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); nico@210: 01542 if (err) { nico@210: 01543 MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId); nico@210: 01544 return SDO_ABORTED_INTERNAL; nico@210: 01545 } nico@210: 01546 if (d->transfers[line].state != SDO_FINISHED) nico@210: 01547 return d->transfers[line].state; nico@210: 01548 nico@210: 01550 * size = (UNS8)d->transfers[line].count; nico@210: 01551 for ( i = 0 ; i < *size ; i++) { nico@210: 01552 # ifdef CANOPEN_BIG_ENDIAN nico@210: 01553 if (d->transfers[line].dataType != visible_string) nico@210: 01554 ( (char *) data)[*size - 1 - i] = d->transfers[line].data[i]; nico@210: 01555 else nico@210: 01556 ( (char *) data)[i] = d->transfers[line].data[i]; nico@210: 01557 # else nico@210: 01558 ( (char *) data)[i] = d->transfers[line].data[i]; nico@210: 01559 # endif nico@210: 01560 } nico@210: 01561 return SDO_FINISHED; nico@210: 01562 } nico@210: 01563 nico@210: 01573 UNS8 getWriteResultNetworkDict (CO_Data* d, UNS8 nodeId, UNS32 * abortCode) nico@210: 01574 { nico@210: 01575 UNS8 line = 0; nico@210: 01576 UNS8 err; nico@210: 01577 nico@210: 01578 * abortCode = 0; nico@210: 01580 err = getSDOlineOnUse(d, nodeId, SDO_CLIENT, &line); nico@210: 01581 if (err) { nico@210: 01582 MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId); nico@210: 01583 return SDO_ABORTED_INTERNAL; nico@210: 01584 } nico@210: 01585 * abortCode = d->transfers[line].abortCode; nico@210: 01586 return d->transfers[line].state; nico@210: 01587 } nico@210: