AB&T
/
SOEM
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: EasyCAT_LAB_simple EasyCAT_LAB_very_simple EasyCAT_LAB
Embed:
(wiki syntax)
Show/hide line numbers
ethercatcoe.c
Go to the documentation of this file.
00001 /* 00002 * Licensed under the GNU General Public License version 2 with exceptions. See 00003 * LICENSE file in the project root for full license information 00004 */ 00005 00006 /** \file 00007 * \brief 00008 * CAN over EtherCAT (CoE) module. 00009 * 00010 * SDO read / write and SDO service functions 00011 */ 00012 00013 #include <stdio.h> 00014 #include <string.h> 00015 #include "osal.h" 00016 #include "oshw.h" 00017 #include "ethercattype.h" 00018 #include "ethercatbase.h" 00019 #include "ethercatmain.h" 00020 #include "ethercatcoe.h" 00021 00022 /** SDO structure, not to be confused with EcSDOserviceT */ 00023 PACKED_BEGIN 00024 typedef struct PACKED 00025 { 00026 ec_mbxheadert MbxHeader; 00027 uint16 CANOpen; 00028 uint8 Command; 00029 uint16 Index; 00030 uint8 SubIndex; 00031 union 00032 { 00033 uint8 bdata[0x200]; /* variants for easy data access */ 00034 uint16 wdata[0x100]; 00035 uint32 ldata[0x80]; 00036 }; 00037 } ec_SDOt; 00038 PACKED_END 00039 00040 /** SDO service structure */ 00041 PACKED_BEGIN 00042 typedef struct PACKED 00043 { 00044 ec_mbxheadert MbxHeader; 00045 uint16 CANOpen; 00046 uint8 Opcode; 00047 uint8 Reserved; 00048 uint16 Fragments; 00049 union 00050 { 00051 uint8 bdata[0x200]; /* variants for easy data access */ 00052 uint16 wdata[0x100]; 00053 uint32 ldata[0x80]; 00054 }; 00055 } ec_SDOservicet; 00056 PACKED_END 00057 00058 /** Report SDO error. 00059 * 00060 * @param[in] context = context struct 00061 * @param[in] Slave = Slave number 00062 * @param[in] Index = Index that generated error 00063 * @param[in] SubIdx = Subindex that generated error 00064 * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list 00065 */ 00066 void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) 00067 { 00068 ec_errort Ec; 00069 00070 memset(&Ec, 0, sizeof(Ec)); 00071 Ec.Time = osal_current_time(); 00072 Ec.Slave = Slave; 00073 Ec.Index = Index; 00074 Ec.SubIdx = SubIdx; 00075 *(context->ecaterror) = TRUE; 00076 Ec.Etype = EC_ERR_TYPE_SDO_ERROR; 00077 Ec.AbortCode = AbortCode; 00078 ecx_pusherror(context, &Ec); 00079 } 00080 00081 /** Report SDO info error 00082 * 00083 * @param[in] context = context struct 00084 * @param[in] Slave = Slave number 00085 * @param[in] Index = Index that generated error 00086 * @param[in] SubIdx = Subindex that generated error 00087 * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list 00088 */ 00089 static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) 00090 { 00091 ec_errort Ec; 00092 00093 memset(&Ec, 0, sizeof(Ec)); 00094 Ec.Slave = Slave; 00095 Ec.Index = Index; 00096 Ec.SubIdx = SubIdx; 00097 *(context->ecaterror) = TRUE; 00098 Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR; 00099 Ec.AbortCode = AbortCode; 00100 ecx_pusherror(context, &Ec); 00101 } 00102 00103 /** CoE SDO read, blocking. Single subindex or Complete Access. 00104 * 00105 * Only a "normal" upload request is issued. If the requested parameter is <= 4bytes 00106 * then a "expedited" response is returned, otherwise a "normal" response. If a "normal" 00107 * response is larger than the mailbox size then the response is segmented. The function 00108 * will combine all segments and copy them to the parameter buffer. 00109 * 00110 * @param[in] context = context struct 00111 * @param[in] slave = Slave number 00112 * @param[in] index = Index to read 00113 * @param[in] subindex = Subindex to read, must be 0 or 1 if CA is used. 00114 * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes read. 00115 * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SDO. 00116 * @param[out] p = Pointer to parameter buffer 00117 * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM 00118 * @return Workcounter from last slave response 00119 */ 00120 int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex, 00121 boolean CA, int *psize, void *p, int timeout) 00122 { 00123 ec_SDOt *SDOp, *aSDOp; 00124 uint16 bytesize, Framedatasize; 00125 int wkc; 00126 int32 SDOlen; 00127 uint8 *bp; 00128 uint8 *hp; 00129 ec_mbxbuft MbxIn, MbxOut; 00130 uint8 cnt, toggle; 00131 boolean NotLast; 00132 00133 ec_clearmbx(&MbxIn); 00134 /* Empty slave out mailbox if something is in. Timeout set to 0 */ 00135 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); 00136 ec_clearmbx(&MbxOut); 00137 aSDOp = (ec_SDOt *)&MbxIn; 00138 SDOp = (ec_SDOt *)&MbxOut; 00139 SDOp->MbxHeader.length = htoes(0x000a); 00140 SDOp->MbxHeader.address = htoes(0x0000); 00141 SDOp->MbxHeader.priority = 0x00; 00142 /* get new mailbox count value, used as session handle */ 00143 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 00144 context->slavelist[slave].mbx_cnt = cnt; 00145 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 00146 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ 00147 if (CA) 00148 { 00149 SDOp->Command = ECT_SDO_UP_REQ_CA; /* upload request complete access */ 00150 } 00151 else 00152 { 00153 SDOp->Command = ECT_SDO_UP_REQ; /* upload request normal */ 00154 } 00155 SDOp->Index = htoes(index); 00156 if (CA && (subindex > 1)) 00157 { 00158 subindex = 1; 00159 } 00160 SDOp->SubIndex = subindex; 00161 SDOp->ldata[0] = 0; 00162 /* send CoE request to slave */ 00163 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 00164 if (wkc > 0) /* succeeded to place mailbox in slave ? */ 00165 { 00166 /* clean mailboxbuffer */ 00167 ec_clearmbx(&MbxIn); 00168 /* read slave response */ 00169 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); 00170 if (wkc > 0) /* succeeded to read slave response ? */ 00171 { 00172 /* slave response should be CoE, SDO response and the correct index */ 00173 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 00174 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && 00175 (aSDOp->Index == SDOp->Index)) 00176 { 00177 if ((aSDOp->Command & 0x02) > 0) 00178 { 00179 /* expedited frame response */ 00180 bytesize = 4 - ((aSDOp->Command >> 2) & 0x03); 00181 if (*psize >= bytesize) /* parameter buffer big enough ? */ 00182 { 00183 /* copy parameter in parameter buffer */ 00184 memcpy(p, &aSDOp->ldata[0], bytesize); 00185 /* return the real parameter size */ 00186 *psize = bytesize; 00187 } 00188 else 00189 { 00190 wkc = 0; 00191 ecx_packeterror(context, slave, index, subindex, 3); /* data container too small for type */ 00192 } 00193 } 00194 else 00195 { /* normal frame response */ 00196 SDOlen = etohl(aSDOp->ldata[0]); 00197 /* Does parameter fit in parameter buffer ? */ 00198 if (SDOlen <= *psize) 00199 { 00200 bp = p; 00201 hp = p; 00202 /* calculate mailbox transfer size */ 00203 Framedatasize = (etohs(aSDOp->MbxHeader.length) - 10); 00204 if (Framedatasize < SDOlen) /* transfer in segments? */ 00205 { 00206 /* copy parameter data in parameter buffer */ 00207 memcpy(hp, &aSDOp->ldata[1], Framedatasize); 00208 /* increment buffer pointer */ 00209 hp += Framedatasize; 00210 *psize = Framedatasize; 00211 NotLast = TRUE; 00212 toggle= 0x00; 00213 while (NotLast) /* segmented transfer */ 00214 { 00215 SDOp = (ec_SDOt *)&MbxOut; 00216 SDOp->MbxHeader.length = htoes(0x000a); 00217 SDOp->MbxHeader.address = htoes(0x0000); 00218 SDOp->MbxHeader.priority = 0x00; 00219 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 00220 context->slavelist[slave].mbx_cnt = cnt; 00221 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 00222 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ 00223 SDOp->Command = ECT_SDO_SEG_UP_REQ + toggle; /* segment upload request */ 00224 SDOp->Index = htoes(index); 00225 SDOp->SubIndex = subindex; 00226 SDOp->ldata[0] = 0; 00227 /* send segmented upload request to slave */ 00228 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 00229 /* is mailbox transferred to slave ? */ 00230 if (wkc > 0) 00231 { 00232 ec_clearmbx(&MbxIn); 00233 /* read slave response */ 00234 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); 00235 /* has slave responded ? */ 00236 if (wkc > 0) 00237 { 00238 /* slave response should be CoE, SDO response */ 00239 if ((((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 00240 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && 00241 ((aSDOp->Command & 0xe0) == 0x00))) 00242 { 00243 /* calculate mailbox transfer size */ 00244 Framedatasize = etohs(aSDOp->MbxHeader.length) - 3; 00245 if ((aSDOp->Command & 0x01) > 0) 00246 { /* last segment */ 00247 NotLast = FALSE; 00248 if (Framedatasize == 7) 00249 /* subtract unused bytes from frame */ 00250 Framedatasize = Framedatasize - ((aSDOp->Command & 0x0e) >> 1); 00251 /* copy to parameter buffer */ 00252 memcpy(hp, &(aSDOp->Index), Framedatasize); 00253 } 00254 else /* segments follow */ 00255 { 00256 /* copy to parameter buffer */ 00257 memcpy(hp, &(aSDOp->Index), Framedatasize); 00258 /* increment buffer pointer */ 00259 hp += Framedatasize; 00260 } 00261 /* update parameter size */ 00262 *psize += Framedatasize; 00263 } 00264 /* unexpected frame returned from slave */ 00265 else 00266 { 00267 NotLast = FALSE; 00268 if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ 00269 ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0])); 00270 else 00271 ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */ 00272 wkc = 0; 00273 } 00274 } 00275 } 00276 toggle = toggle ^ 0x10; /* toggle bit for segment request */ 00277 } 00278 } 00279 /* non segmented transfer */ 00280 else 00281 { 00282 /* copy to parameter buffer */ 00283 memcpy(bp, &aSDOp->ldata[1], SDOlen); 00284 *psize = SDOlen; 00285 } 00286 } 00287 /* parameter buffer too small */ 00288 else 00289 { 00290 wkc = 0; 00291 ecx_packeterror(context, slave, index, subindex, 3); /* data container too small for type */ 00292 } 00293 } 00294 } 00295 /* other slave response */ 00296 else 00297 { 00298 if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ 00299 { 00300 ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0])); 00301 } 00302 else 00303 { 00304 ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */ 00305 } 00306 wkc = 0; 00307 } 00308 } 00309 } 00310 return wkc; 00311 } 00312 00313 /** CoE SDO write, blocking. Single subindex or Complete Access. 00314 * 00315 * A "normal" download request is issued, unless we have 00316 * small data, then a "expedited" transfer is used. If the parameter is larger than 00317 * the mailbox size then the download is segmented. The function will split the 00318 * parameter data in segments and send them to the slave one by one. 00319 * 00320 * @param[in] context = context struct 00321 * @param[in] Slave = Slave number 00322 * @param[in] Index = Index to write 00323 * @param[in] SubIndex = Subindex to write, must be 0 or 1 if CA is used. 00324 * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes written. 00325 * @param[in] psize = Size in bytes of parameter buffer. 00326 * @param[out] p = Pointer to parameter buffer 00327 * @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM 00328 * @return Workcounter from last slave response 00329 */ 00330 int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex, 00331 boolean CA, int psize, void *p, int Timeout) 00332 { 00333 ec_SDOt *SDOp, *aSDOp; 00334 int wkc, maxdata; 00335 ec_mbxbuft MbxIn, MbxOut; 00336 uint8 cnt, toggle; 00337 uint16 framedatasize; 00338 boolean NotLast; 00339 uint8 *hp; 00340 00341 ec_clearmbx(&MbxIn); 00342 /* Empty slave out mailbox if something is in. Timeout set to 0 */ 00343 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0); 00344 ec_clearmbx(&MbxOut); 00345 aSDOp = (ec_SDOt *)&MbxIn; 00346 SDOp = (ec_SDOt *)&MbxOut; 00347 maxdata = context->slavelist[Slave].mbx_l - 0x10; /* data section=mailbox size - 6 mbx - 2 CoE - 8 sdo req */ 00348 /* if small data use expedited transfer */ 00349 if ((psize <= 4) && !CA) 00350 { 00351 SDOp->MbxHeader.length = htoes(0x000a); 00352 SDOp->MbxHeader.address = htoes(0x0000); 00353 SDOp->MbxHeader.priority = 0x00; 00354 /* get new mailbox counter, used for session handle */ 00355 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); 00356 context->slavelist[Slave].mbx_cnt = cnt; 00357 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 00358 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */ 00359 SDOp->Command = ECT_SDO_DOWN_EXP | (((4 - psize) << 2) & 0x0c); /* expedited SDO download transfer */ 00360 SDOp->Index = htoes(Index); 00361 SDOp->SubIndex = SubIndex; 00362 hp = p; 00363 /* copy parameter data to mailbox */ 00364 memcpy(&SDOp->ldata[0], hp, psize); 00365 /* send mailbox SDO download request to slave */ 00366 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 00367 if (wkc > 0) 00368 { 00369 ec_clearmbx(&MbxIn); 00370 /* read slave response */ 00371 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); 00372 if (wkc > 0) 00373 { 00374 /* response should be CoE, SDO response, correct index and subindex */ 00375 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 00376 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && 00377 (aSDOp->Index == SDOp->Index) && 00378 (aSDOp->SubIndex == SDOp->SubIndex)) 00379 { 00380 /* all OK */ 00381 } 00382 /* unexpected response from slave */ 00383 else 00384 { 00385 if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ 00386 { 00387 ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); 00388 } 00389 else 00390 { 00391 ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ 00392 } 00393 wkc = 0; 00394 } 00395 } 00396 } 00397 } 00398 else 00399 { 00400 framedatasize = psize; 00401 NotLast = FALSE; 00402 if (framedatasize > maxdata) 00403 { 00404 framedatasize = maxdata; /* segmented transfer needed */ 00405 NotLast = TRUE; 00406 } 00407 SDOp->MbxHeader.length = htoes(0x0a + framedatasize); 00408 SDOp->MbxHeader.address = htoes(0x0000); 00409 SDOp->MbxHeader.priority = 0x00; 00410 /* get new mailbox counter, used for session handle */ 00411 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); 00412 context->slavelist[Slave].mbx_cnt = cnt; 00413 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 00414 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */ 00415 if (CA) 00416 { 00417 SDOp->Command = ECT_SDO_DOWN_INIT_CA; /* Complete Access, normal SDO init download transfer */ 00418 } 00419 else 00420 { 00421 SDOp->Command = ECT_SDO_DOWN_INIT; /* normal SDO init download transfer */ 00422 } 00423 SDOp->Index = htoes(Index); 00424 SDOp->SubIndex = SubIndex; 00425 if (CA && (SubIndex > 1)) 00426 { 00427 SDOp->SubIndex = 1; 00428 } 00429 SDOp->ldata[0] = htoel(psize); 00430 hp = p; 00431 /* copy parameter data to mailbox */ 00432 memcpy(&SDOp->ldata[1], hp, framedatasize); 00433 hp += framedatasize; 00434 psize -= framedatasize; 00435 /* send mailbox SDO download request to slave */ 00436 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 00437 if (wkc > 0) 00438 { 00439 ec_clearmbx(&MbxIn); 00440 /* read slave response */ 00441 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); 00442 if (wkc > 0) 00443 { 00444 /* response should be CoE, SDO response, correct index and subindex */ 00445 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 00446 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && 00447 (aSDOp->Index == SDOp->Index) && 00448 (aSDOp->SubIndex == SDOp->SubIndex)) 00449 { 00450 /* all ok */ 00451 maxdata += 7; 00452 toggle = 0; 00453 /* repeat while segments left */ 00454 while (NotLast) 00455 { 00456 SDOp = (ec_SDOt *)&MbxOut; 00457 framedatasize = psize; 00458 NotLast = FALSE; 00459 SDOp->Command = 0x01; /* last segment */ 00460 if (framedatasize > maxdata) 00461 { 00462 framedatasize = maxdata; /* more segments needed */ 00463 NotLast = TRUE; 00464 SDOp->Command = 0x00; /* segments follow */ 00465 } 00466 if (!NotLast && (framedatasize < 7)) 00467 { 00468 SDOp->MbxHeader.length = htoes(0x0a); /* minimum size */ 00469 SDOp->Command = 0x01 + ((7 - framedatasize) << 1); /* last segment reduced octets */ 00470 } 00471 else 00472 { 00473 SDOp->MbxHeader.length = htoes(framedatasize + 3); /* data + 2 CoE + 1 SDO */ 00474 } 00475 SDOp->MbxHeader.address = htoes(0x0000); 00476 SDOp->MbxHeader.priority = 0x00; 00477 /* get new mailbox counter value */ 00478 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); 00479 context->slavelist[Slave].mbx_cnt = cnt; 00480 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 00481 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ 00482 SDOp->Command = SDOp->Command + toggle; /* add toggle bit to command byte */ 00483 /* copy parameter data to mailbox */ 00484 memcpy(&SDOp->Index, hp, framedatasize); 00485 /* update parameter buffer pointer */ 00486 hp += framedatasize; 00487 psize -= framedatasize; 00488 /* send SDO download request */ 00489 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 00490 if (wkc > 0) 00491 { 00492 ec_clearmbx(&MbxIn); 00493 /* read slave response */ 00494 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); 00495 if (wkc > 0) 00496 { 00497 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 00498 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && 00499 ((aSDOp->Command & 0xe0) == 0x20)) 00500 { 00501 /* all OK, nothing to do */ 00502 } 00503 else 00504 { 00505 if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ 00506 { 00507 ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); 00508 } 00509 else 00510 { 00511 ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ 00512 } 00513 wkc = 0; 00514 NotLast = FALSE; 00515 } 00516 } 00517 } 00518 toggle = toggle ^ 0x10; /* toggle bit for segment request */ 00519 } 00520 } 00521 /* unexpected response from slave */ 00522 else 00523 { 00524 if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ 00525 { 00526 ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); 00527 } 00528 else 00529 { 00530 ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ 00531 } 00532 wkc = 0; 00533 } 00534 } 00535 } 00536 } 00537 00538 return wkc; 00539 } 00540 00541 /** CoE RxPDO write, blocking. 00542 * 00543 * A RxPDO download request is issued. 00544 * 00545 * @param[in] context = context struct 00546 * @param[in] Slave = Slave number 00547 * @param[in] RxPDOnumber = Related RxPDO number 00548 * @param[in] psize = Size in bytes of PDO buffer. 00549 * @param[out] p = Pointer to PDO buffer 00550 * @return Workcounter from last slave response 00551 */ 00552 int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, void *p) 00553 { 00554 ec_SDOt *SDOp; 00555 int wkc, maxdata; 00556 ec_mbxbuft MbxIn, MbxOut; 00557 uint8 cnt; 00558 uint16 framedatasize; 00559 00560 ec_clearmbx(&MbxIn); 00561 /* Empty slave out mailbox if something is in. Timeout set to 0 */ 00562 wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0); 00563 ec_clearmbx(&MbxOut); 00564 SDOp = (ec_SDOt *)&MbxOut; 00565 maxdata = context->slavelist[Slave].mbx_l - 0x08; /* data section=mailbox size - 6 mbx - 2 CoE */ 00566 framedatasize = psize; 00567 if (framedatasize > maxdata) 00568 { 00569 framedatasize = maxdata; /* limit transfer */ 00570 } 00571 SDOp->MbxHeader.length = htoes(0x02 + framedatasize); 00572 SDOp->MbxHeader.address = htoes(0x0000); 00573 SDOp->MbxHeader.priority = 0x00; 00574 /* get new mailbox counter, used for session handle */ 00575 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); 00576 context->slavelist[Slave].mbx_cnt = cnt; 00577 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 00578 SDOp->CANOpen = htoes((RxPDOnumber & 0x01ff) + (ECT_COES_RXPDO << 12)); /* number 9bits service upper 4 bits */ 00579 /* copy PDO data to mailbox */ 00580 memcpy(&SDOp->Command, p, framedatasize); 00581 /* send mailbox RxPDO request to slave */ 00582 wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 00583 00584 return wkc; 00585 } 00586 00587 /** CoE TxPDO read remote request, blocking. 00588 * 00589 * A RxPDO download request is issued. 00590 * 00591 * @param[in] context = context struct 00592 * @param[in] slave = Slave number 00593 * @param[in] TxPDOnumber = Related TxPDO number 00594 * @param[in,out] psize = Size in bytes of PDO buffer, returns bytes read from PDO. 00595 * @param[out] p = Pointer to PDO buffer 00596 * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM 00597 * @return Workcounter from last slave response 00598 */ 00599 int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout) 00600 { 00601 ec_SDOt *SDOp, *aSDOp; 00602 int wkc; 00603 ec_mbxbuft MbxIn, MbxOut; 00604 uint8 cnt; 00605 uint16 framedatasize; 00606 00607 ec_clearmbx(&MbxIn); 00608 /* Empty slave out mailbox if something is in. Timeout set to 0 */ 00609 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); 00610 ec_clearmbx(&MbxOut); 00611 aSDOp = (ec_SDOt *)&MbxIn; 00612 SDOp = (ec_SDOt *)&MbxOut; 00613 SDOp->MbxHeader.length = htoes(0x02); 00614 SDOp->MbxHeader.address = htoes(0x0000); 00615 SDOp->MbxHeader.priority = 0x00; 00616 /* get new mailbox counter, used for session handle */ 00617 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 00618 context->slavelist[slave].mbx_cnt = cnt; 00619 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 00620 SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12)); /* number 9bits service upper 4 bits */ 00621 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 00622 if (wkc > 0) 00623 { 00624 /* clean mailboxbuffer */ 00625 ec_clearmbx(&MbxIn); 00626 /* read slave response */ 00627 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); 00628 if (wkc > 0) /* succeeded to read slave response ? */ 00629 { 00630 /* slave response should be CoE, TxPDO */ 00631 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 00632 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_TXPDO)) 00633 { 00634 /* TxPDO response */ 00635 framedatasize = (aSDOp->MbxHeader.length - 2); 00636 if (*psize >= framedatasize) /* parameter buffer big enough ? */ 00637 { 00638 /* copy parameter in parameter buffer */ 00639 memcpy(p, &aSDOp->Command, framedatasize); 00640 /* return the real parameter size */ 00641 *psize = framedatasize; 00642 } 00643 /* parameter buffer too small */ 00644 else 00645 { 00646 wkc = 0; 00647 ecx_packeterror(context, slave, 0, 0, 3); /* data container too small for type */ 00648 } 00649 } 00650 /* other slave response */ 00651 else 00652 { 00653 if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ 00654 { 00655 ecx_SDOerror(context, slave, 0, 0, etohl(aSDOp->ldata[0])); 00656 } 00657 else 00658 { 00659 ecx_packeterror(context, slave, 0, 0, 1); /* Unexpected frame returned */ 00660 } 00661 wkc = 0; 00662 } 00663 } 00664 } 00665 00666 return wkc; 00667 } 00668 00669 /** Read PDO assign structure 00670 * @param[in] context = context struct 00671 * @param[in] Slave = Slave number 00672 * @param[in] PDOassign = PDO assign object 00673 * @return total bitlength of PDO assign 00674 */ 00675 int ecx_readPDOassign(ecx_contextt *context, uint16 Slave, uint16 PDOassign) 00676 { 00677 uint16 idxloop, nidx, subidxloop, rdat, idx, subidx; 00678 uint8 subcnt; 00679 int wkc, bsize = 0, rdl; 00680 int32 rdat2; 00681 00682 rdl = sizeof(rdat); rdat = 0; 00683 /* read PDO assign subindex 0 ( = number of PDO's) */ 00684 wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); 00685 rdat = etohs(rdat); 00686 /* positive result from slave ? */ 00687 if ((wkc > 0) && (rdat > 0)) 00688 { 00689 /* number of available sub indexes */ 00690 nidx = rdat; 00691 bsize = 0; 00692 /* read all PDO's */ 00693 for (idxloop = 1; idxloop <= nidx; idxloop++) 00694 { 00695 rdl = sizeof(rdat); rdat = 0; 00696 /* read PDO assign */ 00697 wkc = ecx_SDOread(context, Slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); 00698 /* result is index of PDO */ 00699 idx = etohs(rdat); 00700 if (idx > 0) 00701 { 00702 rdl = sizeof(subcnt); subcnt = 0; 00703 /* read number of subindexes of PDO */ 00704 wkc = ecx_SDOread(context, Slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM); 00705 subidx = subcnt; 00706 /* for each subindex */ 00707 for (subidxloop = 1; subidxloop <= subidx; subidxloop++) 00708 { 00709 rdl = sizeof(rdat2); rdat2 = 0; 00710 /* read SDO that is mapped in PDO */ 00711 wkc = ecx_SDOread(context, Slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM); 00712 rdat2 = etohl(rdat2); 00713 /* extract bitlength of SDO */ 00714 if (LO_BYTE(rdat2) < 0xff) 00715 { 00716 bsize += LO_BYTE(rdat2); 00717 } 00718 else 00719 { 00720 rdl = sizeof(rdat); rdat = htoes(0xff); 00721 /* read Object Entry in Object database */ 00722 // wkc = ec_readOEsingle(idx, (uint8)SubCount, pODlist, pOElist); 00723 bsize += etohs(rdat); 00724 } 00725 } 00726 } 00727 } 00728 } 00729 /* return total found bitlength (PDO) */ 00730 return bsize; 00731 } 00732 00733 /** Read PDO assign structure in Complete Access mode 00734 * @param[in] context = context struct 00735 * @param[in] Slave = Slave number 00736 * @param[in] Thread_n = Calling thread index 00737 * @param[in] PDOassign = PDO assign object 00738 * @return total bitlength of PDO assign 00739 */ 00740 int ecx_readPDOassignCA(ecx_contextt *context, uint16 Slave, int Thread_n, 00741 uint16 PDOassign) 00742 { 00743 uint16 idxloop, nidx, subidxloop, idx, subidx; 00744 int wkc, bsize = 0, rdl; 00745 00746 /* find maximum size of PDOassign buffer */ 00747 rdl = sizeof(ec_PDOassignt); 00748 context->PDOassign[Thread_n].n=0; 00749 /* read rxPDOassign in CA mode, all subindexes are read in one struct */ 00750 wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, TRUE, &rdl, 00751 &(context->PDOassign[Thread_n]), EC_TIMEOUTRXM); 00752 /* positive result from slave ? */ 00753 if ((wkc > 0) && (context->PDOassign[Thread_n].n > 0)) 00754 { 00755 nidx = context->PDOassign[Thread_n].n; 00756 bsize = 0; 00757 /* for each PDO do */ 00758 for (idxloop = 1; idxloop <= nidx; idxloop++) 00759 { 00760 /* get index from PDOassign struct */ 00761 idx = etohs(context->PDOassign[Thread_n].index[idxloop - 1]); 00762 if (idx > 0) 00763 { 00764 rdl = sizeof(ec_PDOdesct); context->PDOdesc[Thread_n].n = 0; 00765 /* read SDO's that are mapped in PDO, CA mode */ 00766 wkc = ecx_SDOread(context, Slave,idx, 0x00, TRUE, &rdl, 00767 &(context->PDOdesc[Thread_n]), EC_TIMEOUTRXM); 00768 subidx = context->PDOdesc[Thread_n].n; 00769 /* extract all bitlengths of SDO's */ 00770 for (subidxloop = 1; subidxloop <= subidx; subidxloop++) 00771 { 00772 bsize += LO_BYTE(etohl(context->PDOdesc[Thread_n].PDO[subidxloop -1])); 00773 } 00774 } 00775 } 00776 } 00777 00778 /* return total found bitlength (PDO) */ 00779 return bsize; 00780 } 00781 00782 /** CoE read PDO mapping. 00783 * 00784 * CANopen has standard indexes defined for PDO mapping. This function 00785 * tries to read them and collect a full input and output mapping size 00786 * of designated slave. 00787 * 00788 * Principal structure in slave:\n 00789 * 1C00:00 is number of SM defined\n 00790 * 1C00:01 SM0 type -> 1C10\n 00791 * 1C00:02 SM1 type -> 1C11\n 00792 * 1C00:03 SM2 type -> 1C12\n 00793 * 1C00:04 SM3 type -> 1C13\n 00794 * Type 0 = unused, 1 = mailbox in, 2 = mailbox out, 00795 * 3 = outputs (RxPDO), 4 = inputs (TxPDO). 00796 * 00797 * 1C12:00 is number of PDO's defined for SM2\n 00798 * 1C12:01 PDO assign SDO #1 -> f.e. 1A00\n 00799 * 1C12:02 PDO assign SDO #2 -> f.e. 1A04\ 00800 * 00801 * 1A00:00 is number of object defined for this PDO\n 00802 * 1A00:01 object mapping #1, f.e. 60100710 (SDO 6010 SI 07 bitlength 0x10) 00803 * 00804 * @param[in] context = context struct 00805 * @param[in] Slave = Slave number 00806 * @param[out] Osize = Size in bits of output mapping (rxPDO) found 00807 * @param[out] Isize = Size in bits of input mapping (txPDO) found 00808 * @return >0 if mapping successful. 00809 */ 00810 int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize) 00811 { 00812 int wkc, rdl; 00813 int retVal = 0; 00814 uint8 nSM, iSM, tSM; 00815 int Tsize; 00816 uint8 SMt_bug_add; 00817 00818 *Isize = 0; 00819 *Osize = 0; 00820 SMt_bug_add = 0; 00821 rdl = sizeof(nSM); nSM = 0; 00822 /* read SyncManager Communication Type object count */ 00823 wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM); 00824 /* positive result from slave ? */ 00825 if ((wkc > 0) && (nSM > 2)) 00826 { 00827 /* limit to maximum number of SM defined, if true the slave can't be configured */ 00828 if (nSM > EC_MAXSM) 00829 nSM = EC_MAXSM; 00830 /* iterate for every SM type defined */ 00831 for (iSM = 2 ; iSM < nSM ; iSM++) 00832 { 00833 rdl = sizeof(tSM); tSM = 0; 00834 /* read SyncManager Communication Type */ 00835 wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM); 00836 if (wkc > 0) 00837 { 00838 // start slave bug prevention code, remove if possible 00839 if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! 00840 { 00841 SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 00842 } 00843 if(tSM) 00844 { 00845 tSM += SMt_bug_add; // only add if SMt > 0 00846 } 00847 if((iSM == 2) && (tSM == 0)) // SM2 has type 0, this is a bug in the slave! 00848 { 00849 tSM = 3; 00850 } 00851 if((iSM == 3) && (tSM == 0)) // SM3 has type 0, this is a bug in the slave! 00852 { 00853 tSM = 4; 00854 } 00855 // end slave bug prevention code 00856 00857 context->slavelist[Slave].SMtype[iSM] = tSM; 00858 /* check if SM is unused -> clear enable flag */ 00859 if (tSM == 0) 00860 { 00861 context->slavelist[Slave].SM[iSM].SMflags = 00862 htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK); 00863 } 00864 if ((tSM == 3) || (tSM == 4)) 00865 { 00866 /* read the assign PDO */ 00867 Tsize = ecx_readPDOassign(context, Slave, ECT_SDO_PDOASSIGN + iSM ); 00868 /* if a mapping is found */ 00869 if (Tsize) 00870 { 00871 context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8); 00872 if (tSM == 3) 00873 { 00874 /* we are doing outputs */ 00875 *Osize += Tsize; 00876 } 00877 else 00878 { 00879 /* we are doing inputs */ 00880 *Isize += Tsize; 00881 } 00882 } 00883 } 00884 } 00885 } 00886 } 00887 00888 /* found some I/O bits ? */ 00889 if ((*Isize > 0) || (*Osize > 0)) 00890 { 00891 retVal = 1; 00892 } 00893 00894 return retVal; 00895 } 00896 00897 /** CoE read PDO mapping in Complete Access mode (CA). 00898 * 00899 * CANopen has standard indexes defined for PDO mapping. This function 00900 * tries to read them and collect a full input and output mapping size 00901 * of designated slave. Slave has to support CA, otherwise use ec_readPDOmap(). 00902 * 00903 * @param[in] context = context struct 00904 * @param[in] Slave = Slave number 00905 * @param[in] Thread_n = Calling thread index 00906 * @param[out] Osize = Size in bits of output mapping (rxPDO) found 00907 * @param[out] Isize = Size in bits of input mapping (txPDO) found 00908 * @return >0 if mapping successful. 00909 */ 00910 int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int Thread_n, int *Osize, int *Isize) 00911 { 00912 int wkc, rdl; 00913 int retVal = 0; 00914 uint8 nSM, iSM, tSM; 00915 int Tsize; 00916 uint8 SMt_bug_add; 00917 00918 *Isize = 0; 00919 *Osize = 0; 00920 SMt_bug_add = 0; 00921 rdl = sizeof(ec_SMcommtypet); 00922 context->SMcommtype[Thread_n].n = 0; 00923 /* read SyncManager Communication Type object count Complete Access*/ 00924 wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, TRUE, &rdl, 00925 &(context->SMcommtype[Thread_n]), EC_TIMEOUTRXM); 00926 /* positive result from slave ? */ 00927 if ((wkc > 0) && (context->SMcommtype[Thread_n].n > 2)) 00928 { 00929 nSM = context->SMcommtype[Thread_n].n; 00930 /* limit to maximum number of SM defined, if true the slave can't be configured */ 00931 if (nSM > EC_MAXSM) 00932 { 00933 nSM = EC_MAXSM; 00934 ecx_packeterror(context, Slave, 0, 0, 10); /* #SM larger than EC_MAXSM */ 00935 } 00936 /* iterate for every SM type defined */ 00937 for (iSM = 2 ; iSM < nSM ; iSM++) 00938 { 00939 tSM = context->SMcommtype[Thread_n].SMtype[iSM]; 00940 00941 // start slave bug prevention code, remove if possible 00942 if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! 00943 { 00944 SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 00945 } 00946 if(tSM) 00947 { 00948 tSM += SMt_bug_add; // only add if SMt > 0 00949 } 00950 // end slave bug prevention code 00951 00952 context->slavelist[Slave].SMtype[iSM] = tSM; 00953 /* check if SM is unused -> clear enable flag */ 00954 if (tSM == 0) 00955 { 00956 context->slavelist[Slave].SM[iSM].SMflags = 00957 htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK); 00958 } 00959 if ((tSM == 3) || (tSM == 4)) 00960 { 00961 /* read the assign PDO */ 00962 Tsize = ecx_readPDOassignCA(context, Slave, Thread_n, 00963 ECT_SDO_PDOASSIGN + iSM ); 00964 /* if a mapping is found */ 00965 if (Tsize) 00966 { 00967 context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8); 00968 if (tSM == 3) 00969 { 00970 /* we are doing outputs */ 00971 *Osize += Tsize; 00972 } 00973 else 00974 { 00975 /* we are doing inputs */ 00976 *Isize += Tsize; 00977 } 00978 } 00979 } 00980 } 00981 } 00982 00983 /* found some I/O bits ? */ 00984 if ((*Isize > 0) || (*Osize > 0)) 00985 { 00986 retVal = 1; 00987 } 00988 return retVal; 00989 } 00990 00991 /** CoE read Object Description List. 00992 * 00993 * @param[in] context = context struct 00994 * @param[in] Slave = Slave number. 00995 * @param[out] pODlist = resulting Object Description list. 00996 * @return Workcounter of slave response. 00997 */ 00998 int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist) 00999 { 01000 ec_SDOservicet *SDOp, *aSDOp; 01001 ec_mbxbuft MbxIn, MbxOut; 01002 int wkc; 01003 uint16 x, n, i, sp, offset; 01004 boolean stop; 01005 uint8 cnt; 01006 boolean First; 01007 01008 pODlist->Slave = Slave; 01009 pODlist->Entries = 0; 01010 ec_clearmbx(&MbxIn); 01011 /* clear pending out mailbox in slave if available. Timeout is set to 0 */ 01012 wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); 01013 ec_clearmbx(&MbxOut); 01014 aSDOp = (ec_SDOservicet*)&MbxIn; 01015 SDOp = (ec_SDOservicet*)&MbxOut; 01016 SDOp->MbxHeader.length = htoes(0x0008); 01017 SDOp->MbxHeader.address = htoes(0x0000); 01018 SDOp->MbxHeader.priority = 0x00; 01019 /* Get new mailbox counter value */ 01020 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); 01021 context->slavelist[Slave].mbx_cnt = cnt; 01022 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 01023 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ 01024 SDOp->Opcode = ECT_GET_ODLIST_REQ; /* get object description list request */ 01025 SDOp->Reserved = 0; 01026 SDOp->Fragments = 0; /* fragments left */ 01027 SDOp->wdata[0] = htoes(0x01); /* all objects */ 01028 /* send get object description list request to slave */ 01029 wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); 01030 /* mailbox placed in slave ? */ 01031 if (wkc > 0) 01032 { 01033 x = 0; 01034 sp = 0; 01035 First = TRUE; 01036 offset = 1; /* offset to skip info header in first frame, otherwise set to 0 */ 01037 do 01038 { 01039 stop = TRUE; /* assume this is last iteration */ 01040 ec_clearmbx(&MbxIn); 01041 /* read slave response */ 01042 wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); 01043 /* got response ? */ 01044 if (wkc > 0) 01045 { 01046 /* response should be CoE and "get object description list response" */ 01047 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 01048 ((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES)) 01049 { 01050 if (First) 01051 { 01052 /* extract number of indexes from mailbox data size */ 01053 n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2; 01054 } 01055 else 01056 { 01057 /* extract number of indexes from mailbox data size */ 01058 n = (etohs(aSDOp->MbxHeader.length) - 6) / 2; 01059 } 01060 /* check if indexes fit in buffer structure */ 01061 if ((sp + n) > EC_MAXODLIST) 01062 { 01063 n = EC_MAXODLIST + 1 - sp; 01064 ecx_SDOinfoerror(context, Slave, 0, 0, 0xf000000); /* Too many entries for master buffer */ 01065 stop = TRUE; 01066 } 01067 /* trim to maximum number of ODlist entries defined */ 01068 if ((pODlist->Entries + n) > EC_MAXODLIST) 01069 { 01070 n = EC_MAXODLIST - pODlist->Entries; 01071 } 01072 pODlist->Entries += n; 01073 /* extract indexes one by one */ 01074 for (i = 0; i < n; i++) 01075 { 01076 pODlist->Index[sp + i] = etohs(aSDOp->wdata[i + offset]); 01077 } 01078 sp += n; 01079 /* check if more fragments will follow */ 01080 if (aSDOp->Fragments > 0) 01081 { 01082 stop = FALSE; 01083 } 01084 First = FALSE; 01085 offset = 0; 01086 } 01087 /* got unexpected response from slave */ 01088 else 01089 { 01090 if ((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR) /* SDO info error received */ 01091 { 01092 ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0])); 01093 stop = TRUE; 01094 } 01095 else 01096 { 01097 ecx_packeterror(context, Slave, 0, 0, 1); /* Unexpected frame returned */ 01098 } 01099 wkc = 0; 01100 x += 20; 01101 } 01102 } 01103 x++; 01104 } 01105 while ((x <= 128) && !stop); 01106 } 01107 return wkc; 01108 } 01109 01110 /** CoE read Object Description. Adds textual description to object indexes. 01111 * 01112 * @param[in] context = context struct 01113 * @param[in] Item = Item number in ODlist. 01114 * @param[in,out] pODlist = referencing Object Description list. 01115 * @return Workcounter of slave response. 01116 */ 01117 int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist) 01118 { 01119 ec_SDOservicet *SDOp, *aSDOp; 01120 int wkc; 01121 uint16 n, Slave; 01122 ec_mbxbuft MbxIn, MbxOut; 01123 uint8 cnt; 01124 01125 Slave = pODlist->Slave; 01126 pODlist->DataType[Item] = 0; 01127 pODlist->ObjectCode[Item] = 0; 01128 pODlist->MaxSub[Item] = 0; 01129 pODlist->Name[Item][0] = 0; 01130 ec_clearmbx(&MbxIn); 01131 /* clear pending out mailbox in slave if available. Timeout is set to 0 */ 01132 wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); 01133 ec_clearmbx(&MbxOut); 01134 aSDOp = (ec_SDOservicet*)&MbxIn; 01135 SDOp = (ec_SDOservicet*)&MbxOut; 01136 SDOp->MbxHeader.length = htoes(0x0008); 01137 SDOp->MbxHeader.address = htoes(0x0000); 01138 SDOp->MbxHeader.priority = 0x00; 01139 /* Get new mailbox counter value */ 01140 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); 01141 context->slavelist[Slave].mbx_cnt = cnt; 01142 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 01143 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ 01144 SDOp->Opcode = ECT_GET_OD_REQ; /* get object description request */ 01145 SDOp->Reserved = 0; 01146 SDOp->Fragments = 0; /* fragments left */ 01147 SDOp->wdata[0] = htoes(pODlist->Index[Item]); /* Data of Index */ 01148 /* send get object description request to slave */ 01149 wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); 01150 /* mailbox placed in slave ? */ 01151 if (wkc > 0) 01152 { 01153 ec_clearmbx(&MbxIn); 01154 /* read slave response */ 01155 wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); 01156 /* got response ? */ 01157 if (wkc > 0) 01158 { 01159 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 01160 ((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES)) 01161 { 01162 n = (etohs(aSDOp->MbxHeader.length) - 12); /* length of string(name of object) */ 01163 if (n > EC_MAXNAME) 01164 { 01165 n = EC_MAXNAME; /* max chars */ 01166 } 01167 pODlist->DataType[Item] = etohs(aSDOp->wdata[1]); 01168 pODlist->ObjectCode[Item] = aSDOp->bdata[5]; 01169 pODlist->MaxSub[Item] = aSDOp->bdata[4]; 01170 01171 strncpy(pODlist->Name[Item] , (char *)&aSDOp->bdata[6], n); 01172 pODlist->Name[Item][n] = 0x00; /* String terminator */ 01173 } 01174 /* got unexpected response from slave */ 01175 else 01176 { 01177 if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ 01178 { 01179 ecx_SDOinfoerror(context, Slave,pODlist->Index[Item], 0, etohl(aSDOp->ldata[0])); 01180 } 01181 else 01182 { 01183 ecx_packeterror(context, Slave,pODlist->Index[Item], 0, 1); /* Unexpected frame returned */ 01184 } 01185 wkc = 0; 01186 } 01187 } 01188 } 01189 01190 return wkc; 01191 } 01192 01193 /** CoE read SDO service object entry, single subindex. 01194 * Used in ec_readOE(). 01195 * 01196 * @param[in] context = context struct 01197 * @param[in] Item = Item in ODlist. 01198 * @param[in] SubI = Subindex of item in ODlist. 01199 * @param[in] pODlist = Object description list for reference. 01200 * @param[out] pOElist = resulting object entry structure. 01201 * @return Workcounter of slave response. 01202 */ 01203 int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist) 01204 { 01205 ec_SDOservicet *SDOp, *aSDOp; 01206 int wkc; 01207 uint16 Index, Slave; 01208 int16 n; 01209 ec_mbxbuft MbxIn, MbxOut; 01210 uint8 cnt; 01211 01212 wkc = 0; 01213 Slave = pODlist->Slave; 01214 Index = pODlist->Index[Item]; 01215 ec_clearmbx(&MbxIn); 01216 /* clear pending out mailbox in slave if available. Timeout is set to 0 */ 01217 wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); 01218 ec_clearmbx(&MbxOut); 01219 aSDOp = (ec_SDOservicet*)&MbxIn; 01220 SDOp = (ec_SDOservicet*)&MbxOut; 01221 SDOp->MbxHeader.length = htoes(0x000a); 01222 SDOp->MbxHeader.address = htoes(0x0000); 01223 SDOp->MbxHeader.priority = 0x00; 01224 /* Get new mailbox counter value */ 01225 cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); 01226 context->slavelist[Slave].mbx_cnt = cnt; 01227 SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ 01228 SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ 01229 SDOp->Opcode = ECT_GET_OE_REQ; /* get object entry description request */ 01230 SDOp->Reserved = 0; 01231 SDOp->Fragments = 0; /* fragments left */ 01232 SDOp->wdata[0] = htoes(Index); /* Index */ 01233 SDOp->bdata[2] = SubI; /* SubIndex */ 01234 SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */ 01235 /* send get object entry description request to slave */ 01236 wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); 01237 /* mailbox placed in slave ? */ 01238 if (wkc > 0) 01239 { 01240 ec_clearmbx(&MbxIn); 01241 /* read slave response */ 01242 wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); 01243 /* got response ? */ 01244 if (wkc > 0) 01245 { 01246 if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 01247 ((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES)) 01248 { 01249 pOElist->Entries++; 01250 n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */ 01251 if (n > EC_MAXNAME) 01252 { 01253 n = EC_MAXNAME; /* max string length */ 01254 } 01255 if (n < 0 ) 01256 { 01257 n = 0; 01258 } 01259 pOElist->ValueInfo[SubI] = aSDOp->bdata[3]; 01260 pOElist->DataType[SubI] = etohs(aSDOp->wdata[2]); 01261 pOElist->BitLength[SubI] = etohs(aSDOp->wdata[3]); 01262 pOElist->ObjAccess[SubI] = etohs(aSDOp->wdata[4]); 01263 01264 strncpy(pOElist->Name[SubI] , (char *)&aSDOp->wdata[5], n); 01265 pOElist->Name[SubI][n] = 0x00; /* string terminator */ 01266 } 01267 /* got unexpected response from slave */ 01268 else 01269 { 01270 if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ 01271 { 01272 ecx_SDOinfoerror(context, Slave, Index, SubI, etohl(aSDOp->ldata[0])); 01273 } 01274 else 01275 { 01276 ecx_packeterror(context, Slave, Index, SubI, 1); /* Unexpected frame returned */ 01277 } 01278 wkc = 0; 01279 } 01280 } 01281 } 01282 01283 return wkc; 01284 } 01285 01286 /** CoE read SDO service object entry. 01287 * 01288 * @param[in] context = context struct 01289 * @param[in] Item = Item in ODlist. 01290 * @param[in] pODlist = Object description list for reference. 01291 * @param[out] pOElist = resulting object entry structure. 01292 * @return Workcounter of slave response. 01293 */ 01294 int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist) 01295 { 01296 uint16 SubCount; 01297 int wkc; 01298 uint8 SubI; 01299 01300 wkc = 0; 01301 pOElist->Entries = 0; 01302 SubI = pODlist->MaxSub[Item]; 01303 /* for each entry found in ODlist */ 01304 for (SubCount = 0; SubCount <= SubI; SubCount++) 01305 { 01306 /* read subindex of entry */ 01307 wkc = ecx_readOEsingle(context, Item, (uint8)SubCount, pODlist, pOElist); 01308 } 01309 01310 return wkc; 01311 } 01312 01313 #ifdef EC_VER1 01314 /** Report SDO error. 01315 * 01316 * @param[in] Slave = Slave number 01317 * @param[in] Index = Index that generated error 01318 * @param[in] SubIdx = Subindex that generated error 01319 * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list 01320 * @see ecx_SDOerror 01321 */ 01322 void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) 01323 { 01324 ecx_SDOerror(&ecx_context, Slave, Index, SubIdx, AbortCode); 01325 } 01326 01327 /** CoE SDO read, blocking. Single subindex or Complete Access. 01328 * 01329 * Only a "normal" upload request is issued. If the requested parameter is <= 4bytes 01330 * then a "expedited" response is returned, otherwise a "normal" response. If a "normal" 01331 * response is larger than the mailbox size then the response is segmented. The function 01332 * will combine all segments and copy them to the parameter buffer. 01333 * 01334 * @param[in] slave = Slave number 01335 * @param[in] index = Index to read 01336 * @param[in] subindex = Subindex to read, must be 0 or 1 if CA is used. 01337 * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes read. 01338 * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SDO. 01339 * @param[out] p = Pointer to parameter buffer 01340 * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM 01341 * @return Workcounter from last slave response 01342 * @see ecx_SDOread 01343 */ 01344 int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, 01345 boolean CA, int *psize, void *p, int timeout) 01346 { 01347 return ecx_SDOread(&ecx_context, slave, index, subindex, CA, psize, p, timeout); 01348 } 01349 01350 /** CoE SDO write, blocking. Single subindex or Complete Access. 01351 * 01352 * A "normal" download request is issued, unless we have 01353 * small data, then a "expedited" transfer is used. If the parameter is larger than 01354 * the mailbox size then the download is segmented. The function will split the 01355 * parameter data in segments and send them to the slave one by one. 01356 * 01357 * @param[in] Slave = Slave number 01358 * @param[in] Index = Index to write 01359 * @param[in] SubIndex = Subindex to write, must be 0 or 1 if CA is used. 01360 * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes written. 01361 * @param[in] psize = Size in bytes of parameter buffer. 01362 * @param[out] p = Pointer to parameter buffer 01363 * @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM 01364 * @return Workcounter from last slave response 01365 * @see ecx_SDOwrite 01366 */ 01367 int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, 01368 boolean CA, int psize, void *p, int Timeout) 01369 { 01370 return ecx_SDOwrite(&ecx_context, Slave, Index, SubIndex, CA, psize, p, Timeout); 01371 } 01372 01373 /** CoE RxPDO write, blocking. 01374 * 01375 * A RxPDO download request is issued. 01376 * 01377 * @param[in] Slave = Slave number 01378 * @param[in] RxPDOnumber = Related RxPDO number 01379 * @param[in] psize = Size in bytes of PDO buffer. 01380 * @param[out] p = Pointer to PDO buffer 01381 * @return Workcounter from last slave response 01382 * @see ecx_RxPDO 01383 */ 01384 int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber, int psize, void *p) 01385 { 01386 return ecx_RxPDO(&ecx_context, Slave, RxPDOnumber, psize, p); 01387 } 01388 01389 /** CoE TxPDO read remote request, blocking. 01390 * 01391 * A RxPDO download request is issued. 01392 * 01393 * @param[in] slave = Slave number 01394 * @param[in] TxPDOnumber = Related TxPDO number 01395 * @param[in,out] psize = Size in bytes of PDO buffer, returns bytes read from PDO. 01396 * @param[out] p = Pointer to PDO buffer 01397 * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM 01398 * @return Workcounter from last slave response 01399 * @see ecx_TxPDO 01400 */ 01401 int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout) 01402 { 01403 return ecx_TxPDO(&ecx_context, slave, TxPDOnumber, psize, p, timeout); 01404 } 01405 01406 /** Read PDO assign structure 01407 * @param[in] Slave = Slave number 01408 * @param[in] PDOassign = PDO assign object 01409 * @return total bitlength of PDO assign 01410 */ 01411 int ec_readPDOassign(uint16 Slave, uint16 PDOassign) 01412 { 01413 return ecx_readPDOassign(&ecx_context, Slave, PDOassign); 01414 } 01415 01416 /** Read PDO assign structure in Complete Access mode 01417 * @param[in] Slave = Slave number 01418 * @param[in] PDOassign = PDO assign object 01419 * @param[in] Thread_n = Calling thread index 01420 * @return total bitlength of PDO assign 01421 * @see ecx_readPDOmap 01422 */ 01423 int ec_readPDOassignCA(uint16 Slave, uint16 PDOassign, int Thread_n) 01424 { 01425 return ecx_readPDOassignCA(&ecx_context, Slave, Thread_n, PDOassign); 01426 } 01427 01428 /** CoE read PDO mapping. 01429 * 01430 * CANopen has standard indexes defined for PDO mapping. This function 01431 * tries to read them and collect a full input and output mapping size 01432 * of designated slave. 01433 * 01434 * For details, see #ecx_readPDOmap 01435 * 01436 * @param[in] Slave = Slave number 01437 * @param[out] Osize = Size in bits of output mapping (rxPDO) found 01438 * @param[out] Isize = Size in bits of input mapping (txPDO) found 01439 * @return >0 if mapping succesful. 01440 */ 01441 int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize) 01442 { 01443 return ecx_readPDOmap(&ecx_context, Slave, Osize, Isize); 01444 } 01445 01446 /** CoE read PDO mapping in Complete Access mode (CA). 01447 * 01448 * CANopen has standard indexes defined for PDO mapping. This function 01449 * tries to read them and collect a full input and output mapping size 01450 * of designated slave. Slave has to support CA, otherwise use ec_readPDOmap(). 01451 * 01452 * @param[in] Slave = Slave number 01453 * @param[in] Thread_n = Calling thread index 01454 * @param[out] Osize = Size in bits of output mapping (rxPDO) found 01455 * @param[out] Isize = Size in bits of input mapping (txPDO) found 01456 * @return >0 if mapping succesful. 01457 * @see ecx_readPDOmap ec_readPDOmapCA 01458 */ 01459 int ec_readPDOmapCA(uint16 Slave, int Thread_n, int *Osize, int *Isize) 01460 { 01461 return ecx_readPDOmapCA(&ecx_context, Slave, Thread_n, Osize, Isize); 01462 } 01463 01464 /** CoE read Object Description List. 01465 * 01466 * @param[in] Slave = Slave number. 01467 * @param[out] pODlist = resulting Object Description list. 01468 * @return Workcounter of slave response. 01469 * @see ecx_readODlist 01470 */ 01471 int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist) 01472 { 01473 return ecx_readODlist(&ecx_context, Slave, pODlist); 01474 } 01475 01476 /** CoE read Object Description. Adds textual description to object indexes. 01477 * 01478 * @param[in] Item = Item number in ODlist. 01479 * @param[in,out] pODlist = referencing Object Description list. 01480 * @return Workcounter of slave response. 01481 * @see ecx_readODdescription 01482 */ 01483 int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist) 01484 { 01485 return ecx_readODdescription(&ecx_context, Item, pODlist); 01486 } 01487 01488 int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist) 01489 { 01490 return ecx_readOEsingle(&ecx_context, Item, SubI, pODlist, pOElist); 01491 } 01492 01493 /** CoE read SDO service object entry. 01494 * 01495 * @param[in] Item = Item in ODlist. 01496 * @param[in] pODlist = Object description list for reference. 01497 * @param[out] pOElist = resulting object entry structure. 01498 * @return Workcounter of slave response. 01499 * @see ecx_readOE 01500 */ 01501 int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist) 01502 { 01503 return ecx_readOE(&ecx_context, Item, pODlist, pOElist); 01504 } 01505 #endif
Generated on Tue Jul 12 2022 18:21:13 by
1.7.2