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