00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00048 #include <stdio.h>
00049 #include <string.h>
00050 #include "ethercat_soem/osal.h"
00051 #include "ethercat_soem/oshw.h"
00052 #include "ethercat_soem/ethercattype.h"
00053 #include "ethercat_soem/ethercatbase.h"
00054 #include "ethercat_soem/ethercatmain.h"
00055 #include "ethercat_soem/ethercatcoe.h"
00056
00058 PACKED_BEGIN
00059 typedef struct PACKED
00060 {
00061 ec_mbxheadert MbxHeader;
00062 uint16 CANOpen;
00063 uint8 Command;
00064 uint16 Index;
00065 uint8 SubIndex;
00066 union
00067 {
00068 uint8 bdata[0x200];
00069 uint16 wdata[0x100];
00070 uint32 ldata[0x80];
00071 };
00072 } ec_SDOt;
00073 PACKED_END
00074
00076 PACKED_BEGIN
00077 typedef struct PACKED
00078 {
00079 ec_mbxheadert MbxHeader;
00080 uint16 CANOpen;
00081 uint8 Opcode;
00082 uint8 Reserved;
00083 uint16 Fragments;
00084 union
00085 {
00086 uint8 bdata[0x200];
00087 uint16 wdata[0x100];
00088 uint32 ldata[0x80];
00089 };
00090 } ec_SDOservicet;
00091 PACKED_END
00092
00101 void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
00102 {
00103 ec_errort Ec;
00104
00105 Ec.Time = osal_current_time();
00106 Ec.Slave = Slave;
00107 Ec.Index = Index;
00108 Ec.SubIdx = SubIdx;
00109 *(context->ecaterror) = TRUE;
00110 Ec.Etype = EC_ERR_TYPE_SDO_ERROR;
00111 Ec.AbortCode = AbortCode;
00112 ecx_pusherror(context, &Ec);
00113 }
00114
00123 static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
00124 {
00125 ec_errort Ec;
00126
00127 Ec.Slave = Slave;
00128 Ec.Index = Index;
00129 Ec.SubIdx = SubIdx;
00130 *(context->ecaterror) = TRUE;
00131 Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR;
00132 Ec.AbortCode = AbortCode;
00133 ecx_pusherror(context, &Ec);
00134 }
00135
00153 int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex,
00154 boolean CA, int *psize, void *p, int timeout)
00155 {
00156 ec_SDOt *SDOp, *aSDOp;
00157 uint16 bytesize, Framedatasize;
00158 int wkc;
00159 int32 SDOlen;
00160 uint8 *bp;
00161 uint8 *hp;
00162 ec_mbxbuft MbxIn, MbxOut;
00163 uint8 cnt, toggle;
00164 boolean NotLast;
00165
00166 ec_clearmbx(&MbxIn);
00167
00168 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
00169 ec_clearmbx(&MbxOut);
00170 aSDOp = (ec_SDOt *)&MbxIn;
00171 SDOp = (ec_SDOt *)&MbxOut;
00172 SDOp->MbxHeader.length = htoes(0x000a);
00173 SDOp->MbxHeader.address = htoes(0x0000);
00174 SDOp->MbxHeader.priority = 0x00;
00175
00176 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00177 context->slavelist[slave].mbx_cnt = cnt;
00178 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
00179 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12));
00180 if (CA)
00181 {
00182 SDOp->Command = ECT_SDO_UP_REQ_CA;
00183 }
00184 else
00185 {
00186 SDOp->Command = ECT_SDO_UP_REQ;
00187 }
00188 SDOp->Index = htoes(index);
00189 if (CA && (subindex > 1))
00190 {
00191 subindex = 1;
00192 }
00193 SDOp->SubIndex = subindex;
00194 SDOp->ldata[0] = 0;
00195
00196 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00197 if (wkc > 0)
00198 {
00199
00200 ec_clearmbx(&MbxIn);
00201
00202 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00203 if (wkc > 0)
00204 {
00205
00206 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00207 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00208 (aSDOp->Index == SDOp->Index))
00209 {
00210 if ((aSDOp->Command & 0x02) > 0)
00211 {
00212
00213 bytesize = 4 - ((aSDOp->Command >> 2) & 0x03);
00214 if (*psize >= bytesize)
00215 {
00216
00217 memcpy(p, &aSDOp->ldata[0], bytesize);
00218
00219 *psize = bytesize;
00220 }
00221 else
00222 {
00223 wkc = 0;
00224 ecx_packeterror(context, slave, index, subindex, 3);
00225 }
00226 }
00227 else
00228 {
00229 SDOlen = etohl(aSDOp->ldata[0]);
00230
00231 if (SDOlen <= *psize)
00232 {
00233 bp = p;
00234 hp = p;
00235
00236 Framedatasize = (etohs(aSDOp->MbxHeader.length) - 10);
00237 if (Framedatasize < SDOlen)
00238 {
00239
00240 memcpy(hp, &aSDOp->ldata[1], Framedatasize);
00241
00242 hp += Framedatasize;
00243 *psize = Framedatasize;
00244 NotLast = TRUE;
00245 toggle= 0x00;
00246 while (NotLast)
00247 {
00248 SDOp = (ec_SDOt *)&MbxOut;
00249 SDOp->MbxHeader.length = htoes(0x000a);
00250 SDOp->MbxHeader.address = htoes(0x0000);
00251 SDOp->MbxHeader.priority = 0x00;
00252 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00253 context->slavelist[slave].mbx_cnt = cnt;
00254 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
00255 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12));
00256 SDOp->Command = ECT_SDO_SEG_UP_REQ + toggle;
00257 SDOp->Index = htoes(index);
00258 SDOp->SubIndex = subindex;
00259 SDOp->ldata[0] = 0;
00260
00261 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00262
00263 if (wkc > 0)
00264 {
00265 ec_clearmbx(&MbxIn);
00266
00267 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00268
00269 if (wkc > 0)
00270 {
00271
00272 if ((((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00273 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00274 ((aSDOp->Command & 0xe0) == 0x00)))
00275 {
00276
00277 Framedatasize = etohs(aSDOp->MbxHeader.length) - 3;
00278 if ((aSDOp->Command & 0x01) > 0)
00279 {
00280 NotLast = FALSE;
00281 if (Framedatasize == 7)
00282
00283 Framedatasize = Framedatasize - ((aSDOp->Command & 0x0e) >> 1);
00284
00285 memcpy(hp, &(aSDOp->Index), Framedatasize);
00286 }
00287 else
00288 {
00289
00290 memcpy(hp, &(aSDOp->Index), Framedatasize);
00291
00292 hp += Framedatasize;
00293 }
00294
00295 *psize += Framedatasize;
00296 }
00297
00298 else
00299 {
00300 NotLast = FALSE;
00301 if ((aSDOp->Command) == ECT_SDO_ABORT)
00302 ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0]));
00303 else
00304 ecx_packeterror(context, slave, index, subindex, 1);
00305 wkc = 0;
00306 }
00307 }
00308 }
00309 toggle = toggle ^ 0x10;
00310 }
00311 }
00312
00313 else
00314 {
00315
00316 memcpy(bp, &aSDOp->ldata[1], SDOlen);
00317 *psize = SDOlen;
00318 }
00319 }
00320
00321 else
00322 {
00323 wkc = 0;
00324 ecx_packeterror(context, slave, index, subindex, 3);
00325 }
00326 }
00327 }
00328
00329 else
00330 {
00331 if ((aSDOp->Command) == ECT_SDO_ABORT)
00332 {
00333 ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0]));
00334 }
00335 else
00336 {
00337 ecx_packeterror(context, slave, index, subindex, 1);
00338 }
00339 wkc = 0;
00340 }
00341 }
00342 }
00343 return wkc;
00344 }
00345
00363 int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex,
00364 boolean CA, int psize, void *p, int Timeout)
00365 {
00366 ec_SDOt *SDOp, *aSDOp;
00367 int wkc, maxdata;
00368 ec_mbxbuft MbxIn, MbxOut;
00369 uint8 cnt, toggle;
00370 uint16 framedatasize;
00371 boolean NotLast;
00372 uint8 *hp;
00373
00374 ec_clearmbx(&MbxIn);
00375
00376 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
00377 ec_clearmbx(&MbxOut);
00378 aSDOp = (ec_SDOt *)&MbxIn;
00379 SDOp = (ec_SDOt *)&MbxOut;
00380 maxdata = context->slavelist[Slave].mbx_l - 0x10;
00381
00382 if ((psize <= 4) && !CA)
00383 {
00384 SDOp->MbxHeader.length = htoes(0x000a);
00385 SDOp->MbxHeader.address = htoes(0x0000);
00386 SDOp->MbxHeader.priority = 0x00;
00387
00388 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00389 context->slavelist[Slave].mbx_cnt = cnt;
00390 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
00391 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12));
00392 SDOp->Command = ECT_SDO_DOWN_EXP | (((4 - psize) << 2) & 0x0c);
00393 SDOp->Index = htoes(Index);
00394 SDOp->SubIndex = SubIndex;
00395 hp = p;
00396
00397 memcpy(&SDOp->ldata[0], hp, psize);
00398
00399 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00400 if (wkc > 0)
00401 {
00402 ec_clearmbx(&MbxIn);
00403
00404 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
00405 if (wkc > 0)
00406 {
00407
00408 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00409 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00410 (aSDOp->Index == SDOp->Index) &&
00411 (aSDOp->SubIndex == SDOp->SubIndex))
00412 {
00413
00414 }
00415
00416 else
00417 {
00418 if (aSDOp->Command == ECT_SDO_ABORT)
00419 {
00420 ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
00421 }
00422 else
00423 {
00424 ecx_packeterror(context, Slave, Index, SubIndex, 1);
00425 }
00426 wkc = 0;
00427 }
00428 }
00429 }
00430 }
00431 else
00432 {
00433 framedatasize = psize;
00434 NotLast = FALSE;
00435 if (framedatasize > maxdata)
00436 {
00437 framedatasize = maxdata;
00438 NotLast = TRUE;
00439 }
00440 SDOp->MbxHeader.length = htoes(0x0a + framedatasize);
00441 SDOp->MbxHeader.address = htoes(0x0000);
00442 SDOp->MbxHeader.priority = 0x00;
00443
00444 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00445 context->slavelist[Slave].mbx_cnt = cnt;
00446 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
00447 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12));
00448 if (CA)
00449 {
00450 SDOp->Command = ECT_SDO_DOWN_INIT_CA;
00451 }
00452 else
00453 {
00454 SDOp->Command = ECT_SDO_DOWN_INIT;
00455 }
00456 SDOp->Index = htoes(Index);
00457 SDOp->SubIndex = SubIndex;
00458 if (CA && (SubIndex > 1))
00459 {
00460 SDOp->SubIndex = 1;
00461 }
00462 SDOp->ldata[0] = htoel(psize);
00463 hp = p;
00464
00465 memcpy(&SDOp->ldata[1], hp, framedatasize);
00466 hp += framedatasize;
00467 psize -= framedatasize;
00468
00469 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00470 if (wkc > 0)
00471 {
00472 ec_clearmbx(&MbxIn);
00473
00474 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
00475 if (wkc > 0)
00476 {
00477
00478 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00479 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00480 (aSDOp->Index == SDOp->Index) &&
00481 (aSDOp->SubIndex == SDOp->SubIndex))
00482 {
00483
00484 maxdata += 7;
00485 toggle = 0;
00486
00487 while (NotLast)
00488 {
00489 SDOp = (ec_SDOt *)&MbxOut;
00490 framedatasize = psize;
00491 NotLast = FALSE;
00492 SDOp->Command = 0x01;
00493 if (framedatasize > maxdata)
00494 {
00495 framedatasize = maxdata;
00496 NotLast = TRUE;
00497 SDOp->Command = 0x00;
00498 }
00499 if (!NotLast && (framedatasize < 7))
00500 {
00501 SDOp->MbxHeader.length = htoes(0x0a);
00502 SDOp->Command = 0x01 + ((7 - framedatasize) << 1);
00503 }
00504 else
00505 {
00506 SDOp->MbxHeader.length = htoes(framedatasize + 3);
00507 }
00508 SDOp->MbxHeader.address = htoes(0x0000);
00509 SDOp->MbxHeader.priority = 0x00;
00510
00511 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00512 context->slavelist[Slave].mbx_cnt = cnt;
00513 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
00514 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12));
00515 SDOp->Command = SDOp->Command + toggle;
00516
00517 memcpy(&SDOp->Index, hp, framedatasize);
00518
00519 hp += framedatasize;
00520 psize -= framedatasize;
00521
00522 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00523 if (wkc > 0)
00524 {
00525 ec_clearmbx(&MbxIn);
00526
00527 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
00528 if (wkc > 0)
00529 {
00530 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00531 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00532 ((aSDOp->Command & 0xe0) == 0x20))
00533 {
00534
00535 }
00536 else
00537 {
00538 if (aSDOp->Command == ECT_SDO_ABORT)
00539 {
00540 ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
00541 }
00542 else
00543 {
00544 ecx_packeterror(context, Slave, Index, SubIndex, 1);
00545 }
00546 wkc = 0;
00547 NotLast = FALSE;
00548 }
00549 }
00550 }
00551 toggle = toggle ^ 0x10;
00552 }
00553 }
00554
00555 else
00556 {
00557 if (aSDOp->Command == ECT_SDO_ABORT)
00558 {
00559 ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
00560 }
00561 else
00562 {
00563 ecx_packeterror(context, Slave, Index, SubIndex, 1);
00564 }
00565 wkc = 0;
00566 }
00567 }
00568 }
00569 }
00570
00571 return wkc;
00572 }
00573
00585 int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, void *p)
00586 {
00587 ec_SDOt *SDOp;
00588 int wkc, maxdata;
00589 ec_mbxbuft MbxIn, MbxOut;
00590 uint8 cnt;
00591 uint16 framedatasize;
00592
00593 ec_clearmbx(&MbxIn);
00594
00595 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
00596 ec_clearmbx(&MbxOut);
00597 SDOp = (ec_SDOt *)&MbxOut;
00598 maxdata = context->slavelist[Slave].mbx_l - 0x08;
00599 framedatasize = psize;
00600 if (framedatasize > maxdata)
00601 {
00602 framedatasize = maxdata;
00603 }
00604 SDOp->MbxHeader.length = htoes(0x02 + framedatasize);
00605 SDOp->MbxHeader.address = htoes(0x0000);
00606 SDOp->MbxHeader.priority = 0x00;
00607
00608 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00609 context->slavelist[Slave].mbx_cnt = cnt;
00610 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
00611 SDOp->CANOpen = htoes((RxPDOnumber & 0x01ff) + (ECT_COES_RXPDO << 12));
00612
00613 memcpy(&SDOp->Command, p, framedatasize);
00614
00615 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00616
00617 return wkc;
00618 }
00619
00632 int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout)
00633 {
00634 ec_SDOt *SDOp, *aSDOp;
00635 int wkc;
00636 ec_mbxbuft MbxIn, MbxOut;
00637 uint8 cnt;
00638 uint16 framedatasize;
00639
00640 ec_clearmbx(&MbxIn);
00641
00642 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
00643 ec_clearmbx(&MbxOut);
00644 aSDOp = (ec_SDOt *)&MbxIn;
00645 SDOp = (ec_SDOt *)&MbxOut;
00646 SDOp->MbxHeader.length = htoes(0x02);
00647 SDOp->MbxHeader.address = htoes(0x0000);
00648 SDOp->MbxHeader.priority = 0x00;
00649
00650 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00651 context->slavelist[slave].mbx_cnt = cnt;
00652 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
00653 SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12));
00654 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00655 if (wkc > 0)
00656 {
00657
00658 ec_clearmbx(&MbxIn);
00659
00660 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00661 if (wkc > 0)
00662 {
00663
00664 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00665 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_TXPDO))
00666 {
00667
00668 framedatasize = (aSDOp->MbxHeader.length - 2);
00669 if (*psize >= framedatasize)
00670 {
00671
00672 memcpy(p, &aSDOp->Command, framedatasize);
00673
00674 *psize = framedatasize;
00675 }
00676
00677 else
00678 {
00679 wkc = 0;
00680 ecx_packeterror(context, slave, 0, 0, 3);
00681 }
00682 }
00683
00684 else
00685 {
00686 if ((aSDOp->Command) == ECT_SDO_ABORT)
00687 {
00688 ecx_SDOerror(context, slave, 0, 0, etohl(aSDOp->ldata[0]));
00689 }
00690 else
00691 {
00692 ecx_packeterror(context, slave, 0, 0, 1);
00693 }
00694 wkc = 0;
00695 }
00696 }
00697 }
00698
00699 return wkc;
00700 }
00701
00708 int ecx_readPDOassign(ecx_contextt *context, uint16 Slave, uint16 PDOassign)
00709 {
00710 uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
00711 uint8 subcnt;
00712 int wkc, bsize = 0, rdl;
00713 int32 rdat2;
00714
00715 rdl = sizeof(rdat); rdat = 0;
00716
00717 wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
00718 rdat = etohs(rdat);
00719
00720 if ((wkc > 0) && (rdat > 0))
00721 {
00722
00723 nidx = rdat;
00724 bsize = 0;
00725
00726 for (idxloop = 1; idxloop <= nidx; idxloop++)
00727 {
00728 rdl = sizeof(rdat); rdat = 0;
00729
00730 wkc = ecx_SDOread(context, Slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
00731
00732 idx = etohl(rdat);
00733 if (idx > 0)
00734 {
00735 rdl = sizeof(subcnt); subcnt = 0;
00736
00737 wkc = ecx_SDOread(context, Slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM);
00738 subidx = subcnt;
00739
00740 for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
00741 {
00742 rdl = sizeof(rdat2); rdat2 = 0;
00743
00744 wkc = ecx_SDOread(context, Slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM);
00745 rdat2 = etohl(rdat2);
00746
00747 if (LO_BYTE(rdat2) < 0xff)
00748 {
00749 bsize += LO_BYTE(rdat2);
00750 }
00751 else
00752 {
00753 rdl = sizeof(rdat); rdat = htoes(0xff);
00754
00755
00756 bsize += etohs(rdat);
00757 }
00758 }
00759 }
00760 }
00761 }
00762
00763 return bsize;
00764 }
00765
00772 int ecx_readPDOassignCA(ecx_contextt *context, uint16 Slave, uint16 PDOassign)
00773 {
00774 uint16 idxloop, nidx, subidxloop, idx, subidx;
00775 int wkc, bsize = 0, rdl;
00776
00777
00778 rdl = sizeof(ec_PDOassignt);
00779 context->PDOassign->n=0;
00780
00781 wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, TRUE, &rdl, context->PDOassign, EC_TIMEOUTRXM);
00782
00783 if ((wkc > 0) && (context->PDOassign->n > 0))
00784 {
00785 nidx = context->PDOassign->n;
00786 bsize = 0;
00787
00788 for (idxloop = 1; idxloop <= nidx; idxloop++)
00789 {
00790
00791 idx = etohs(context->PDOassign->index[idxloop - 1]);
00792 if (idx > 0)
00793 {
00794 rdl = sizeof(ec_PDOdesct); context->PDOdesc->n = 0;
00795
00796 wkc = ecx_SDOread(context, Slave,idx, 0x00, TRUE, &rdl, context->PDOdesc, EC_TIMEOUTRXM);
00797 subidx = context->PDOdesc->n;
00798
00799 for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
00800 {
00801 bsize += LO_BYTE(etohl(context->PDOdesc->PDO[subidxloop -1]));
00802 }
00803 }
00804 }
00805 }
00806
00807
00808 return bsize;
00809 }
00810
00839 int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize)
00840 {
00841 int wkc, rdl;
00842 int retVal = 0;
00843 uint8 nSM, iSM, tSM;
00844 int Tsize;
00845 uint8 SMt_bug_add;
00846
00847 *Isize = 0;
00848 *Osize = 0;
00849 SMt_bug_add = 0;
00850 rdl = sizeof(nSM); nSM = 0;
00851
00852 wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM);
00853
00854 if ((wkc > 0) && (nSM > 2))
00855 {
00856
00857 nSM--;
00858
00859 if (nSM > EC_MAXSM)
00860 nSM = EC_MAXSM;
00861
00862 for (iSM = 2 ; iSM <= nSM ; iSM++)
00863 {
00864 rdl = sizeof(tSM); tSM = 0;
00865
00866 wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM);
00867 if (wkc > 0)
00868 {
00869
00870 if((iSM == 2) && (tSM == 2))
00871 {
00872 SMt_bug_add = 1;
00873 }
00874 if(tSM)
00875 {
00876 tSM += SMt_bug_add;
00877 }
00878 if((iSM == 2) && (tSM == 0))
00879 {
00880 tSM = 3;
00881 }
00882 if((iSM == 3) && (tSM == 0))
00883 {
00884 tSM = 4;
00885 }
00886
00887
00888 context->slavelist[Slave].SMtype[iSM] = tSM;
00889
00890 if (tSM == 0)
00891 {
00892 context->slavelist[Slave].SM[iSM].SMflags =
00893 htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
00894 }
00895 if ((tSM == 3) || (tSM == 4))
00896 {
00897
00898 Tsize = ecx_readPDOassign(context, Slave, ECT_SDO_PDOASSIGN + iSM );
00899
00900 if (Tsize)
00901 {
00902 context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8);
00903 if (tSM == 3)
00904 {
00905
00906 *Osize += Tsize;
00907 }
00908 else
00909 {
00910
00911 *Isize += Tsize;
00912 }
00913 }
00914 }
00915 }
00916 }
00917 }
00918
00919
00920 if ((*Isize > 0) || (*Osize > 0))
00921 {
00922 retVal = 1;
00923 }
00924
00925 return retVal;
00926 }
00927
00940 int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize)
00941 {
00942 int wkc, rdl;
00943 int retVal = 0;
00944 uint8 nSM, iSM, tSM;
00945 int Tsize;
00946 uint8 SMt_bug_add;
00947
00948 *Isize = 0;
00949 *Osize = 0;
00950 SMt_bug_add = 0;
00951 rdl = sizeof(ec_SMcommtypet);
00952 context->SMcommtype->n = 0;
00953
00954 wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, TRUE, &rdl, context->SMcommtype, EC_TIMEOUTRXM);
00955
00956 if ((wkc > 0) && (context->SMcommtype->n > 2))
00957 {
00958
00959 nSM = context->SMcommtype->n - 1;
00960
00961 if (nSM > EC_MAXSM)
00962 {
00963 nSM = EC_MAXSM;
00964 ecx_packeterror(context, Slave, 0, 0, 10);
00965 }
00966
00967 for (iSM = 2 ; iSM <= nSM ; iSM++)
00968 {
00969 tSM = context->SMcommtype->SMtype[iSM];
00970
00971
00972 if((iSM == 2) && (tSM == 2))
00973 {
00974 SMt_bug_add = 1;
00975 }
00976 if(tSM)
00977 {
00978 tSM += SMt_bug_add;
00979 }
00980
00981
00982 context->slavelist[Slave].SMtype[iSM] = tSM;
00983
00984 if (tSM == 0)
00985 {
00986 context->slavelist[Slave].SM[iSM].SMflags =
00987 htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
00988 }
00989 if ((tSM == 3) || (tSM == 4))
00990 {
00991
00992 Tsize = ecx_readPDOassignCA(context, Slave, ECT_SDO_PDOASSIGN + iSM );
00993
00994 if (Tsize)
00995 {
00996 context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8);
00997 if (tSM == 3)
00998 {
00999
01000 *Osize += Tsize;
01001 }
01002 else
01003 {
01004
01005 *Isize += Tsize;
01006 }
01007 }
01008 }
01009 }
01010 }
01011
01012
01013 if ((*Isize > 0) || (*Osize > 0))
01014 {
01015 retVal = 1;
01016 }
01017 return retVal;
01018 }
01019
01027 int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist)
01028 {
01029 ec_SDOservicet *SDOp, *aSDOp;
01030 ec_mbxbuft MbxIn, MbxOut;
01031 int wkc;
01032 uint16 x, n, i, sp, offset;
01033 boolean stop;
01034 uint8 cnt;
01035 boolean First;
01036
01037 pODlist->Slave = Slave;
01038 pODlist->Entries = 0;
01039 ec_clearmbx(&MbxIn);
01040
01041 wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
01042 ec_clearmbx(&MbxOut);
01043 aSDOp = (ec_SDOservicet*)&MbxIn;
01044 SDOp = (ec_SDOservicet*)&MbxOut;
01045 SDOp->MbxHeader.length = htoes(0x0008);
01046 SDOp->MbxHeader.address = htoes(0x0000);
01047 SDOp->MbxHeader.priority = 0x00;
01048
01049 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
01050 context->slavelist[Slave].mbx_cnt = cnt;
01051 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
01052 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12));
01053 SDOp->Opcode = ECT_GET_ODLIST_REQ;
01054 SDOp->Reserved = 0;
01055 SDOp->Fragments = 0;
01056 SDOp->wdata[0] = htoes(0x01);
01057
01058 wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
01059
01060 if (wkc > 0)
01061 {
01062 x = 0;
01063 sp = 0;
01064 First = TRUE;
01065 offset = 1;
01066 do
01067 {
01068 stop = TRUE;
01069 ec_clearmbx(&MbxIn);
01070
01071 wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
01072
01073 if (wkc > 0)
01074 {
01075
01076 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
01077 ((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES))
01078 {
01079 if (First)
01080 {
01081
01082 n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2;
01083 }
01084 else
01085 {
01086
01087 n = (etohs(aSDOp->MbxHeader.length) - 6) / 2;
01088 }
01089
01090 if ((sp + n) > EC_MAXODLIST)
01091 {
01092 n = EC_MAXODLIST + 1 - sp;
01093 ecx_SDOinfoerror(context, Slave, 0, 0, 0xf000000);
01094 stop = TRUE;
01095 }
01096
01097 if ((pODlist->Entries + n) > EC_MAXODLIST)
01098 {
01099 n = EC_MAXODLIST - pODlist->Entries;
01100 }
01101 pODlist->Entries += n;
01102
01103 for (i = 0; i < n; i++)
01104 {
01105 pODlist->Index[sp + i] = etohs(aSDOp->wdata[i + offset]);
01106 }
01107 sp += n;
01108
01109 if (aSDOp->Fragments > 0)
01110 {
01111 stop = FALSE;
01112 }
01113 First = FALSE;
01114 offset = 0;
01115 }
01116
01117 else
01118 {
01119 if ((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)
01120 {
01121 ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0]));
01122 stop = TRUE;
01123 }
01124 else
01125 {
01126 ecx_packeterror(context, Slave, 0, 0, 1);
01127 }
01128 wkc = 0;
01129 x += 20;
01130 }
01131 }
01132 x++;
01133 }
01134 while ((x <= 128) && !stop);
01135 }
01136 return wkc;
01137 }
01138
01146 int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist)
01147 {
01148 ec_SDOservicet *SDOp, *aSDOp;
01149 int wkc;
01150 uint16 n, Slave;
01151 ec_mbxbuft MbxIn, MbxOut;
01152 uint8 cnt;
01153
01154 Slave = pODlist->Slave;
01155 pODlist->DataType[Item] = 0;
01156 pODlist->ObjectCode[Item] = 0;
01157 pODlist->MaxSub[Item] = 0;
01158 pODlist->Name[Item][0] = 0;
01159 ec_clearmbx(&MbxIn);
01160
01161 wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
01162 ec_clearmbx(&MbxOut);
01163 aSDOp = (ec_SDOservicet*)&MbxIn;
01164 SDOp = (ec_SDOservicet*)&MbxOut;
01165 SDOp->MbxHeader.length = htoes(0x0008);
01166 SDOp->MbxHeader.address = htoes(0x0000);
01167 SDOp->MbxHeader.priority = 0x00;
01168
01169 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
01170 context->slavelist[Slave].mbx_cnt = cnt;
01171 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
01172 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12));
01173 SDOp->Opcode = ECT_GET_OD_REQ;
01174 SDOp->Reserved = 0;
01175 SDOp->Fragments = 0;
01176 SDOp->wdata[0] = htoes(pODlist->Index[Item]);
01177
01178 wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
01179
01180 if (wkc > 0)
01181 {
01182 ec_clearmbx(&MbxIn);
01183
01184 wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
01185
01186 if (wkc > 0)
01187 {
01188 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
01189 ((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES))
01190 {
01191 n = (etohs(aSDOp->MbxHeader.length) - 12);
01192 if (n > EC_MAXNAME)
01193 {
01194 n = EC_MAXNAME;
01195 }
01196 pODlist->DataType[Item] = etohs(aSDOp->wdata[1]);
01197 pODlist->ObjectCode[Item] = aSDOp->bdata[5];
01198 pODlist->MaxSub[Item] = aSDOp->bdata[4];
01199
01200 strncpy(pODlist->Name[Item] , (char *)&aSDOp->bdata[6], n);
01201 pODlist->Name[Item][n] = 0x00;
01202 }
01203
01204 else
01205 {
01206 if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR))
01207 {
01208 ecx_SDOinfoerror(context, Slave,pODlist->Index[Item], 0, etohl(aSDOp->ldata[0]));
01209 }
01210 else
01211 {
01212 ecx_packeterror(context, Slave,pODlist->Index[Item], 0, 1);
01213 }
01214 wkc = 0;
01215 }
01216 }
01217 }
01218
01219 return wkc;
01220 }
01221
01232 int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01233 {
01234 ec_SDOservicet *SDOp, *aSDOp;
01235 uint16 wkc, Index, Slave;
01236 int16 n;
01237 ec_mbxbuft MbxIn, MbxOut;
01238 uint8 cnt;
01239
01240 wkc = 0;
01241 Slave = pODlist->Slave;
01242 Index = pODlist->Index[Item];
01243 ec_clearmbx(&MbxIn);
01244
01245 wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
01246 ec_clearmbx(&MbxOut);
01247 aSDOp = (ec_SDOservicet*)&MbxIn;
01248 SDOp = (ec_SDOservicet*)&MbxOut;
01249 SDOp->MbxHeader.length = htoes(0x000a);
01250 SDOp->MbxHeader.address = htoes(0x0000);
01251 SDOp->MbxHeader.priority = 0x00;
01252
01253 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
01254 context->slavelist[Slave].mbx_cnt = cnt;
01255 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4);
01256 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12));
01257 SDOp->Opcode = ECT_GET_OE_REQ;
01258 SDOp->Reserved = 0;
01259 SDOp->Fragments = 0;
01260 SDOp->wdata[0] = htoes(Index);
01261 SDOp->bdata[2] = SubI;
01262 SDOp->bdata[3] = 1 + 2 + 4;
01263
01264 wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
01265
01266 if (wkc > 0)
01267 {
01268 ec_clearmbx(&MbxIn);
01269
01270 wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
01271
01272 if (wkc > 0)
01273 {
01274 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
01275 ((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES))
01276 {
01277 pOElist->Entries++;
01278 n = (etohs(aSDOp->MbxHeader.length) - 16);
01279 if (n > EC_MAXNAME)
01280 {
01281 n = EC_MAXNAME;
01282 }
01283 if (n < 0 )
01284 {
01285 n = 0;
01286 }
01287 pOElist->ValueInfo[SubI] = aSDOp->bdata[3];
01288 pOElist->DataType[SubI] = etohs(aSDOp->wdata[2]);
01289 pOElist->BitLength[SubI] = etohs(aSDOp->wdata[3]);
01290 pOElist->ObjAccess[SubI] = etohs(aSDOp->wdata[4]);
01291
01292 strncpy(pOElist->Name[SubI] , (char *)&aSDOp->wdata[5], n);
01293 pOElist->Name[SubI][n] = 0x00;
01294 }
01295
01296 else
01297 {
01298 if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR))
01299 {
01300 ecx_SDOinfoerror(context, Slave, Index, SubI, etohl(aSDOp->ldata[0]));
01301 }
01302 else
01303 {
01304 ecx_packeterror(context, Slave, Index, SubI, 1);
01305 }
01306 wkc = 0;
01307 }
01308 }
01309 }
01310
01311 return wkc;
01312 }
01313
01322 int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01323 {
01324 uint16 SubCount;
01325 int wkc;
01326 uint8 SubI;
01327
01328 wkc = 0;
01329 pOElist->Entries = 0;
01330 SubI = pODlist->MaxSub[Item];
01331
01332 for (SubCount = 0; SubCount <= SubI; SubCount++)
01333 {
01334
01335 wkc = ecx_readOEsingle(context, Item, (uint8)SubCount, pODlist, pOElist);
01336 }
01337
01338 return wkc;
01339 }
01340
01341 #ifdef EC_VER1
01342 void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
01343 {
01344 ecx_SDOerror(&ecx_context, Slave, Index, SubIdx, AbortCode);
01345 }
01346
01347 int ec_SDOread(uint16 slave, uint16 index, uint8 subindex,
01348 boolean CA, int *psize, void *p, int timeout)
01349 {
01350 return ecx_SDOread(&ecx_context, slave, index, subindex, CA, psize, p, timeout);
01351 }
01352
01353 int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex,
01354 boolean CA, int psize, void *p, int Timeout)
01355 {
01356 return ecx_SDOwrite(&ecx_context, Slave, Index, SubIndex, CA, psize, p, Timeout);
01357 }
01358
01359 int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber, int psize, void *p)
01360 {
01361 return ecx_RxPDO(&ecx_context, Slave, RxPDOnumber, psize, p);
01362 }
01363
01364 int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout)
01365 {
01366 return ecx_TxPDO(&ecx_context, slave, TxPDOnumber, psize, p, timeout);
01367 }
01368
01370 int ec_readPDOassign(uint16 Slave, uint16 PDOassign)
01371 {
01372 return ecx_readPDOassign(&ecx_context, Slave, PDOassign);
01373 }
01374
01376 int ec_readPDOassignCA(uint16 Slave, uint16 PDOassign)
01377 {
01378 return ecx_readPDOassignCA(&ecx_context, Slave, PDOassign);
01379 }
01380
01381 int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize)
01382 {
01383 return ecx_readPDOmap(&ecx_context, Slave, Osize, Isize);
01384 }
01385
01386 int ec_readPDOmapCA(uint16 Slave, int *Osize, int *Isize)
01387 {
01388 return ecx_readPDOmapCA(&ecx_context, Slave, Osize, Isize);
01389 }
01390
01391 int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist)
01392 {
01393 return ecx_readODlist(&ecx_context, Slave, pODlist);
01394 }
01395
01396 int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist)
01397 {
01398 return ecx_readODdescription(&ecx_context, Item, pODlist);
01399 }
01400
01401 int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01402 {
01403 return ecx_readOEsingle(&ecx_context, Item, SubI, pODlist, pOElist);
01404 }
01405
01406 int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01407 {
01408 return ecx_readOE(&ecx_context, Item, pODlist, pOElist);
01409 }
01410 #endif