70 memset(&Ec, 0,
sizeof(Ec));
93 memset(&Ec, 0,
sizeof(Ec));
121 boolean CA,
int *psize,
void *p,
int timeout)
124 uint16 bytesize, Framedatasize;
139 SDOp->MbxHeader.length =
htoes(0x000a);
140 SDOp->MbxHeader.address =
htoes(0x0000);
141 SDOp->MbxHeader.priority = 0x00;
155 SDOp->Index =
htoes(index);
156 if (CA && (subindex > 1))
160 SDOp->SubIndex = subindex;
173 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
175 (aSDOp->Index == SDOp->Index))
177 if ((aSDOp->Command & 0x02) > 0)
180 bytesize = 4 - ((aSDOp->Command >> 2) & 0x03);
181 if (*psize >= bytesize)
184 memcpy(p, &aSDOp->ldata[0], bytesize);
196 SDOlen =
etohl(aSDOp->ldata[0]);
198 if (SDOlen <= *psize)
203 Framedatasize = (
etohs(aSDOp->MbxHeader.length) - 10);
204 if (Framedatasize < SDOlen)
207 memcpy(hp, &aSDOp->ldata[1], Framedatasize);
210 *psize = Framedatasize;
216 SDOp->MbxHeader.length =
htoes(0x000a);
217 SDOp->MbxHeader.address =
htoes(0x0000);
218 SDOp->MbxHeader.priority = 0x00;
224 SDOp->Index =
htoes(index);
225 SDOp->SubIndex = subindex;
239 if ((((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
241 ((aSDOp->Command & 0xe0) == 0x00)))
244 Framedatasize =
etohs(aSDOp->MbxHeader.length) - 3;
245 if ((aSDOp->Command & 0x01) > 0)
248 if (Framedatasize == 7)
250 Framedatasize = Framedatasize - ((aSDOp->Command & 0x0e) >> 1);
252 memcpy(hp, &(aSDOp->Index), Framedatasize);
257 memcpy(hp, &(aSDOp->Index), Framedatasize);
262 *psize += Framedatasize;
276 toggle = toggle ^ 0x10;
283 memcpy(bp, &aSDOp->ldata[1], SDOlen);
331 boolean CA,
int psize,
void *p,
int Timeout)
349 if ((psize <= 4) && !CA)
351 SDOp->MbxHeader.length =
htoes(0x000a);
352 SDOp->MbxHeader.address =
htoes(0x0000);
353 SDOp->MbxHeader.priority = 0x00;
360 SDOp->Index =
htoes(Index);
361 SDOp->SubIndex = SubIndex;
364 memcpy(&SDOp->ldata[0], hp, psize);
375 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
377 (aSDOp->Index == SDOp->Index) &&
378 (aSDOp->SubIndex == SDOp->SubIndex))
400 framedatasize = psize;
402 if (framedatasize > maxdata)
404 framedatasize = maxdata;
407 SDOp->MbxHeader.length =
htoes(0x0a + framedatasize);
408 SDOp->MbxHeader.address =
htoes(0x0000);
409 SDOp->MbxHeader.priority = 0x00;
423 SDOp->Index =
htoes(Index);
424 SDOp->SubIndex = SubIndex;
425 if (CA && (SubIndex > 1))
429 SDOp->ldata[0] =
htoel(psize);
432 memcpy(&SDOp->ldata[1], hp, framedatasize);
434 psize -= framedatasize;
445 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
447 (aSDOp->Index == SDOp->Index) &&
448 (aSDOp->SubIndex == SDOp->SubIndex))
457 framedatasize = psize;
459 SDOp->Command = 0x01;
460 if (framedatasize > maxdata)
462 framedatasize = maxdata;
464 SDOp->Command = 0x00;
466 if (!NotLast && (framedatasize < 7))
468 SDOp->MbxHeader.length =
htoes(0x0a);
469 SDOp->Command = 0x01 + ((7 - framedatasize) << 1);
473 SDOp->MbxHeader.length =
htoes(framedatasize + 3);
475 SDOp->MbxHeader.address =
htoes(0x0000);
476 SDOp->MbxHeader.priority = 0x00;
482 SDOp->Command = SDOp->Command + toggle;
484 memcpy(&SDOp->Index, hp, framedatasize);
487 psize -= framedatasize;
497 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
499 ((aSDOp->Command & 0xe0) == 0x20))
518 toggle = toggle ^ 0x10;
566 framedatasize = psize;
567 if (framedatasize > maxdata)
569 framedatasize = maxdata;
571 SDOp->MbxHeader.length =
htoes(0x02 + framedatasize);
572 SDOp->MbxHeader.address =
htoes(0x0000);
573 SDOp->MbxHeader.priority = 0x00;
580 memcpy(&SDOp->Command, p, framedatasize);
613 SDOp->MbxHeader.length =
htoes(0x02);
614 SDOp->MbxHeader.address =
htoes(0x0000);
615 SDOp->MbxHeader.priority = 0x00;
631 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
635 framedatasize = (aSDOp->MbxHeader.length - 2);
636 if (*psize >= framedatasize)
639 memcpy(p, &aSDOp->Command, framedatasize);
641 *psize = framedatasize;
677 uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
679 int wkc, bsize = 0, rdl;
682 rdl =
sizeof(rdat); rdat = 0;
687 if ((
wkc > 0) && (rdat > 0))
693 for (idxloop = 1; idxloop <= nidx; idxloop++)
695 rdl =
sizeof(rdat); rdat = 0;
702 rdl =
sizeof(subcnt); subcnt = 0;
707 for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
709 rdl =
sizeof(rdat2); rdat2 = 0;
712 rdat2 =
etohl(rdat2);
720 rdl =
sizeof(rdat); rdat =
htoes(0xff);
723 bsize +=
etohs(rdat);
743 uint16 idxloop, nidx, subidxloop, idx, subidx;
744 int wkc, bsize = 0, rdl;
758 for (idxloop = 1; idxloop <= nidx; idxloop++)
768 subidx = context->
PDOdesc[Thread_n].n;
770 for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
821 rdl =
sizeof(nSM); nSM = 0;
825 if ((
wkc > 0) && (nSM > 2))
831 for (iSM = 2 ; iSM < nSM ; iSM++)
833 rdl =
sizeof(tSM); tSM = 0;
839 if((iSM == 2) && (tSM == 2))
847 if((iSM == 2) && (tSM == 0))
851 if((iSM == 3) && (tSM == 0))
864 if ((tSM == 3) || (tSM == 4))
889 if ((*Isize > 0) || (*Osize > 0))
937 for (iSM = 2 ; iSM < nSM ; iSM++)
939 tSM = context->
SMcommtype[Thread_n].SMtype[iSM];
942 if((iSM == 2) && (tSM == 2))
959 if ((tSM == 3) || (tSM == 4))
984 if ((*Isize > 0) || (*Osize > 0))
1003 uint16 x, n, i, sp, offset;
1008 pODlist->
Slave = Slave;
1016 SDOp->MbxHeader.length =
htoes(0x0008);
1017 SDOp->MbxHeader.address =
htoes(0x0000);
1018 SDOp->MbxHeader.priority = 0x00;
1026 SDOp->Fragments = 0;
1027 SDOp->wdata[0] =
htoes(0x01);
1047 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
1053 n = (
etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2;
1058 n = (
etohs(aSDOp->MbxHeader.length) - 6) / 2;
1074 for (i = 0; i < n; i++)
1076 pODlist->
Index[sp + i] =
etohs(aSDOp->wdata[i + offset]);
1080 if (aSDOp->Fragments > 0)
1105 while ((x <= 128) && !stop);
1125 Slave = pODlist->
Slave;
1128 pODlist->
MaxSub[Item] = 0;
1129 pODlist->
Name[Item][0] = 0;
1136 SDOp->MbxHeader.length =
htoes(0x0008);
1137 SDOp->MbxHeader.address =
htoes(0x0000);
1138 SDOp->MbxHeader.priority = 0x00;
1146 SDOp->Fragments = 0;
1147 SDOp->wdata[0] =
htoes(pODlist->
Index[Item]);
1159 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
1162 n = (
etohs(aSDOp->MbxHeader.length) - 12);
1169 pODlist->
MaxSub[Item] = aSDOp->bdata[4];
1171 strncpy(pODlist->
Name[Item] , (
char *)&aSDOp->bdata[6], n);
1172 pODlist->
Name[Item][n] = 0x00;
1213 Slave = pODlist->
Slave;
1214 Index = pODlist->
Index[Item];
1221 SDOp->MbxHeader.length =
htoes(0x000a);
1222 SDOp->MbxHeader.address =
htoes(0x0000);
1223 SDOp->MbxHeader.priority = 0x00;
1231 SDOp->Fragments = 0;
1232 SDOp->wdata[0] =
htoes(Index);
1233 SDOp->bdata[2] = SubI;
1234 SDOp->bdata[3] = 1 + 2 + 4;
1246 if (((aSDOp->MbxHeader.mbxtype & 0x0f) ==
ECT_MBXT_COE) &&
1250 n = (
etohs(aSDOp->MbxHeader.length) - 16);
1259 pOElist->
ValueInfo[SubI] = aSDOp->bdata[3];
1264 strncpy(pOElist->
Name[SubI] , (
char *)&aSDOp->wdata[5], n);
1265 pOElist->
Name[SubI][n] = 0x00;
1302 SubI = pODlist->
MaxSub[Item];
1304 for (SubCount = 0; SubCount <= SubI; SubCount++)
1345 boolean CA,
int *psize,
void *p,
int timeout)
1368 boolean CA,
int psize,
void *p,
int Timeout)