AB&T / SOEM

SOEM EtherCAT Master library for STM Nucleo F767ZI

Dependents:   EasyCAT_LAB_simple EasyCAT_LAB_very_simple EasyCAT_LAB

  • It has been developed for the EasyCAT LAB , a complete educational and experimental EtherCAT® system, composed of one master and two slaves .

Warning

/media/uploads/EasyCAT/easycat_lab.jpg

SOEM/ethercatsoe.c@0:543d6784d4cc, 2019-06-11 (annotated)

Committer:
EasyCAT
Date:
Tue Jun 11 10:29:09 2019 +0000
Revision:
0:543d6784d4cc
SOEM EtherCAT Master Library for STM Nucleo F767ZI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EasyCAT 0:543d6784d4cc 1 /*
EasyCAT 0:543d6784d4cc 2 * Licensed under the GNU General Public License version 2 with exceptions. See
EasyCAT 0:543d6784d4cc 3 * LICENSE file in the project root for full license information
EasyCAT 0:543d6784d4cc 4 */
EasyCAT 0:543d6784d4cc 5
EasyCAT 0:543d6784d4cc 6 /** \file
EasyCAT 0:543d6784d4cc 7 * \brief
EasyCAT 0:543d6784d4cc 8 * Servo over EtherCAT (SoE) Module.
EasyCAT 0:543d6784d4cc 9 */
EasyCAT 0:543d6784d4cc 10
EasyCAT 0:543d6784d4cc 11 #include <stdio.h>
EasyCAT 0:543d6784d4cc 12 #include <string.h>
EasyCAT 0:543d6784d4cc 13 #include "osal.h"
EasyCAT 0:543d6784d4cc 14 #include "oshw.h"
EasyCAT 0:543d6784d4cc 15 #include "ethercattype.h"
EasyCAT 0:543d6784d4cc 16 #include "ethercatbase.h"
EasyCAT 0:543d6784d4cc 17 #include "ethercatmain.h"
EasyCAT 0:543d6784d4cc 18 #include "ethercatsoe.h"
EasyCAT 0:543d6784d4cc 19
EasyCAT 0:543d6784d4cc 20 #define EC_SOE_MAX_DRIVES 8
EasyCAT 0:543d6784d4cc 21
EasyCAT 0:543d6784d4cc 22 /** SoE (Servo over EtherCAT) mailbox structure */
EasyCAT 0:543d6784d4cc 23 PACKED_BEGIN
EasyCAT 0:543d6784d4cc 24 typedef struct PACKED
EasyCAT 0:543d6784d4cc 25 {
EasyCAT 0:543d6784d4cc 26 ec_mbxheadert MbxHeader;
EasyCAT 0:543d6784d4cc 27 uint8 opCode :3;
EasyCAT 0:543d6784d4cc 28 uint8 incomplete :1;
EasyCAT 0:543d6784d4cc 29 uint8 error :1;
EasyCAT 0:543d6784d4cc 30 uint8 driveNo :3;
EasyCAT 0:543d6784d4cc 31 uint8 elementflags;
EasyCAT 0:543d6784d4cc 32 union
EasyCAT 0:543d6784d4cc 33 {
EasyCAT 0:543d6784d4cc 34 uint16 idn;
EasyCAT 0:543d6784d4cc 35 uint16 fragmentsleft;
EasyCAT 0:543d6784d4cc 36 };
EasyCAT 0:543d6784d4cc 37 } ec_SoEt;
EasyCAT 0:543d6784d4cc 38 PACKED_END
EasyCAT 0:543d6784d4cc 39
EasyCAT 0:543d6784d4cc 40 /** Report SoE error.
EasyCAT 0:543d6784d4cc 41 *
EasyCAT 0:543d6784d4cc 42 * @param[in] context = context struct
EasyCAT 0:543d6784d4cc 43 * @param[in] Slave = Slave number
EasyCAT 0:543d6784d4cc 44 * @param[in] idn = IDN that generated error
EasyCAT 0:543d6784d4cc 45 * @param[in] Error = Error code, see EtherCAT documentation for list
EasyCAT 0:543d6784d4cc 46 */
EasyCAT 0:543d6784d4cc 47 void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error)
EasyCAT 0:543d6784d4cc 48 {
EasyCAT 0:543d6784d4cc 49 ec_errort Ec;
EasyCAT 0:543d6784d4cc 50
EasyCAT 0:543d6784d4cc 51 memset(&Ec, 0, sizeof(Ec));
EasyCAT 0:543d6784d4cc 52 Ec.Time = osal_current_time();
EasyCAT 0:543d6784d4cc 53 Ec.Slave = Slave;
EasyCAT 0:543d6784d4cc 54 Ec.Index = idn;
EasyCAT 0:543d6784d4cc 55 Ec.SubIdx = 0;
EasyCAT 0:543d6784d4cc 56 *(context->ecaterror) = TRUE;
EasyCAT 0:543d6784d4cc 57 Ec.Etype = EC_ERR_TYPE_SOE_ERROR;
EasyCAT 0:543d6784d4cc 58 Ec.ErrorCode = Error;
EasyCAT 0:543d6784d4cc 59 ecx_pusherror(context, &Ec);
EasyCAT 0:543d6784d4cc 60 }
EasyCAT 0:543d6784d4cc 61
EasyCAT 0:543d6784d4cc 62 /** SoE read, blocking.
EasyCAT 0:543d6784d4cc 63 *
EasyCAT 0:543d6784d4cc 64 * The IDN object of the selected slave and DriveNo is read. If a response
EasyCAT 0:543d6784d4cc 65 * is larger than the mailbox size then the response is segmented. The function
EasyCAT 0:543d6784d4cc 66 * will combine all segments and copy them to the parameter buffer.
EasyCAT 0:543d6784d4cc 67 *
EasyCAT 0:543d6784d4cc 68 * @param[in] context = context struct
EasyCAT 0:543d6784d4cc 69 * @param[in] slave = Slave number
EasyCAT 0:543d6784d4cc 70 * @param[in] driveNo = Drive number in slave
EasyCAT 0:543d6784d4cc 71 * @param[in] elementflags = Flags to select what properties of IDN are to be transferred.
EasyCAT 0:543d6784d4cc 72 * @param[in] idn = IDN.
EasyCAT 0:543d6784d4cc 73 * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SoE.
EasyCAT 0:543d6784d4cc 74 * @param[out] p = Pointer to parameter buffer
EasyCAT 0:543d6784d4cc 75 * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
EasyCAT 0:543d6784d4cc 76 * @return Workcounter from last slave response
EasyCAT 0:543d6784d4cc 77 */
EasyCAT 0:543d6784d4cc 78 int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
EasyCAT 0:543d6784d4cc 79 {
EasyCAT 0:543d6784d4cc 80 ec_SoEt *SoEp, *aSoEp;
EasyCAT 0:543d6784d4cc 81 uint16 totalsize, framedatasize;
EasyCAT 0:543d6784d4cc 82 int wkc;
EasyCAT 0:543d6784d4cc 83 uint8 *bp;
EasyCAT 0:543d6784d4cc 84 uint8 *mp;
EasyCAT 0:543d6784d4cc 85 uint16 *errorcode;
EasyCAT 0:543d6784d4cc 86 ec_mbxbuft MbxIn, MbxOut;
EasyCAT 0:543d6784d4cc 87 uint8 cnt;
EasyCAT 0:543d6784d4cc 88 boolean NotLast;
EasyCAT 0:543d6784d4cc 89
EasyCAT 0:543d6784d4cc 90 ec_clearmbx(&MbxIn);
EasyCAT 0:543d6784d4cc 91 /* Empty slave out mailbox if something is in. Timeout set to 0 */
EasyCAT 0:543d6784d4cc 92 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
EasyCAT 0:543d6784d4cc 93 ec_clearmbx(&MbxOut);
EasyCAT 0:543d6784d4cc 94 aSoEp = (ec_SoEt *)&MbxIn;
EasyCAT 0:543d6784d4cc 95 SoEp = (ec_SoEt *)&MbxOut;
EasyCAT 0:543d6784d4cc 96 SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
EasyCAT 0:543d6784d4cc 97 SoEp->MbxHeader.address = htoes(0x0000);
EasyCAT 0:543d6784d4cc 98 SoEp->MbxHeader.priority = 0x00;
EasyCAT 0:543d6784d4cc 99 /* get new mailbox count value, used as session handle */
EasyCAT 0:543d6784d4cc 100 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
EasyCAT 0:543d6784d4cc 101 context->slavelist[slave].mbx_cnt = cnt;
EasyCAT 0:543d6784d4cc 102 SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
EasyCAT 0:543d6784d4cc 103 SoEp->opCode = ECT_SOE_READREQ;
EasyCAT 0:543d6784d4cc 104 SoEp->incomplete = 0;
EasyCAT 0:543d6784d4cc 105 SoEp->error = 0;
EasyCAT 0:543d6784d4cc 106 SoEp->driveNo = driveNo;
EasyCAT 0:543d6784d4cc 107 SoEp->elementflags = elementflags;
EasyCAT 0:543d6784d4cc 108 SoEp->idn = htoes(idn);
EasyCAT 0:543d6784d4cc 109 totalsize = 0;
EasyCAT 0:543d6784d4cc 110 bp = p;
EasyCAT 0:543d6784d4cc 111 mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
EasyCAT 0:543d6784d4cc 112 NotLast = TRUE;
EasyCAT 0:543d6784d4cc 113 /* send SoE request to slave */
EasyCAT 0:543d6784d4cc 114 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
EasyCAT 0:543d6784d4cc 115 if (wkc > 0) /* succeeded to place mailbox in slave ? */
EasyCAT 0:543d6784d4cc 116 {
EasyCAT 0:543d6784d4cc 117 while (NotLast)
EasyCAT 0:543d6784d4cc 118 {
EasyCAT 0:543d6784d4cc 119 /* clean mailboxbuffer */
EasyCAT 0:543d6784d4cc 120 ec_clearmbx(&MbxIn);
EasyCAT 0:543d6784d4cc 121 /* read slave response */
EasyCAT 0:543d6784d4cc 122 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
EasyCAT 0:543d6784d4cc 123 if (wkc > 0) /* succeeded to read slave response ? */
EasyCAT 0:543d6784d4cc 124 {
EasyCAT 0:543d6784d4cc 125 /* slave response should be SoE, ReadRes */
EasyCAT 0:543d6784d4cc 126 if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
EasyCAT 0:543d6784d4cc 127 (aSoEp->opCode == ECT_SOE_READRES) &&
EasyCAT 0:543d6784d4cc 128 (aSoEp->error == 0) &&
EasyCAT 0:543d6784d4cc 129 (aSoEp->driveNo == driveNo) &&
EasyCAT 0:543d6784d4cc 130 (aSoEp->elementflags == elementflags))
EasyCAT 0:543d6784d4cc 131 {
EasyCAT 0:543d6784d4cc 132 framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert);
EasyCAT 0:543d6784d4cc 133 totalsize += framedatasize;
EasyCAT 0:543d6784d4cc 134 /* Does parameter fit in parameter buffer ? */
EasyCAT 0:543d6784d4cc 135 if (totalsize <= *psize)
EasyCAT 0:543d6784d4cc 136 {
EasyCAT 0:543d6784d4cc 137 /* copy parameter data in parameter buffer */
EasyCAT 0:543d6784d4cc 138 memcpy(bp, mp, framedatasize);
EasyCAT 0:543d6784d4cc 139 /* increment buffer pointer */
EasyCAT 0:543d6784d4cc 140 bp += framedatasize;
EasyCAT 0:543d6784d4cc 141 }
EasyCAT 0:543d6784d4cc 142 else
EasyCAT 0:543d6784d4cc 143 {
EasyCAT 0:543d6784d4cc 144 framedatasize -= totalsize - *psize;
EasyCAT 0:543d6784d4cc 145 totalsize = *psize;
EasyCAT 0:543d6784d4cc 146 /* copy parameter data in parameter buffer */
EasyCAT 0:543d6784d4cc 147 if (framedatasize > 0) memcpy(bp, mp, framedatasize);
EasyCAT 0:543d6784d4cc 148 }
EasyCAT 0:543d6784d4cc 149
EasyCAT 0:543d6784d4cc 150 if (!aSoEp->incomplete)
EasyCAT 0:543d6784d4cc 151 {
EasyCAT 0:543d6784d4cc 152 NotLast = FALSE;
EasyCAT 0:543d6784d4cc 153 *psize = totalsize;
EasyCAT 0:543d6784d4cc 154 }
EasyCAT 0:543d6784d4cc 155 }
EasyCAT 0:543d6784d4cc 156 /* other slave response */
EasyCAT 0:543d6784d4cc 157 else
EasyCAT 0:543d6784d4cc 158 {
EasyCAT 0:543d6784d4cc 159 NotLast = FALSE;
EasyCAT 0:543d6784d4cc 160 if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
EasyCAT 0:543d6784d4cc 161 (aSoEp->opCode == ECT_SOE_READRES) &&
EasyCAT 0:543d6784d4cc 162 (aSoEp->error == 1))
EasyCAT 0:543d6784d4cc 163 {
EasyCAT 0:543d6784d4cc 164 mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
EasyCAT 0:543d6784d4cc 165 errorcode = (uint16 *)mp;
EasyCAT 0:543d6784d4cc 166 ecx_SoEerror(context, slave, idn, *errorcode);
EasyCAT 0:543d6784d4cc 167 }
EasyCAT 0:543d6784d4cc 168 else
EasyCAT 0:543d6784d4cc 169 {
EasyCAT 0:543d6784d4cc 170 ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
EasyCAT 0:543d6784d4cc 171 }
EasyCAT 0:543d6784d4cc 172 wkc = 0;
EasyCAT 0:543d6784d4cc 173 }
EasyCAT 0:543d6784d4cc 174 }
EasyCAT 0:543d6784d4cc 175 else
EasyCAT 0:543d6784d4cc 176 {
EasyCAT 0:543d6784d4cc 177 NotLast = FALSE;
EasyCAT 0:543d6784d4cc 178 ecx_packeterror(context, slave, idn, 0, 4); /* no response */
EasyCAT 0:543d6784d4cc 179 }
EasyCAT 0:543d6784d4cc 180 }
EasyCAT 0:543d6784d4cc 181 }
EasyCAT 0:543d6784d4cc 182 return wkc;
EasyCAT 0:543d6784d4cc 183 }
EasyCAT 0:543d6784d4cc 184
EasyCAT 0:543d6784d4cc 185 /** SoE write, blocking.
EasyCAT 0:543d6784d4cc 186 *
EasyCAT 0:543d6784d4cc 187 * The IDN object of the selected slave and DriveNo is written. If a response
EasyCAT 0:543d6784d4cc 188 * is larger than the mailbox size then the response is segmented.
EasyCAT 0:543d6784d4cc 189 *
EasyCAT 0:543d6784d4cc 190 * @param[in] context = context struct
EasyCAT 0:543d6784d4cc 191 * @param[in] slave = Slave number
EasyCAT 0:543d6784d4cc 192 * @param[in] driveNo = Drive number in slave
EasyCAT 0:543d6784d4cc 193 * @param[in] elementflags = Flags to select what properties of IDN are to be transferred.
EasyCAT 0:543d6784d4cc 194 * @param[in] idn = IDN.
EasyCAT 0:543d6784d4cc 195 * @param[in] psize = Size in bytes of parameter buffer.
EasyCAT 0:543d6784d4cc 196 * @param[out] p = Pointer to parameter buffer
EasyCAT 0:543d6784d4cc 197 * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
EasyCAT 0:543d6784d4cc 198 * @return Workcounter from last slave response
EasyCAT 0:543d6784d4cc 199 */
EasyCAT 0:543d6784d4cc 200 int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
EasyCAT 0:543d6784d4cc 201 {
EasyCAT 0:543d6784d4cc 202 ec_SoEt *SoEp, *aSoEp;
EasyCAT 0:543d6784d4cc 203 uint16 framedatasize, maxdata;
EasyCAT 0:543d6784d4cc 204 int wkc;
EasyCAT 0:543d6784d4cc 205 uint8 *mp;
EasyCAT 0:543d6784d4cc 206 uint8 *hp;
EasyCAT 0:543d6784d4cc 207 uint16 *errorcode;
EasyCAT 0:543d6784d4cc 208 ec_mbxbuft MbxIn, MbxOut;
EasyCAT 0:543d6784d4cc 209 uint8 cnt;
EasyCAT 0:543d6784d4cc 210 boolean NotLast;
EasyCAT 0:543d6784d4cc 211
EasyCAT 0:543d6784d4cc 212 ec_clearmbx(&MbxIn);
EasyCAT 0:543d6784d4cc 213 /* Empty slave out mailbox if something is in. Timeout set to 0 */
EasyCAT 0:543d6784d4cc 214 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
EasyCAT 0:543d6784d4cc 215 ec_clearmbx(&MbxOut);
EasyCAT 0:543d6784d4cc 216 aSoEp = (ec_SoEt *)&MbxIn;
EasyCAT 0:543d6784d4cc 217 SoEp = (ec_SoEt *)&MbxOut;
EasyCAT 0:543d6784d4cc 218 SoEp->MbxHeader.address = htoes(0x0000);
EasyCAT 0:543d6784d4cc 219 SoEp->MbxHeader.priority = 0x00;
EasyCAT 0:543d6784d4cc 220 SoEp->opCode = ECT_SOE_WRITEREQ;
EasyCAT 0:543d6784d4cc 221 SoEp->error = 0;
EasyCAT 0:543d6784d4cc 222 SoEp->driveNo = driveNo;
EasyCAT 0:543d6784d4cc 223 SoEp->elementflags = elementflags;
EasyCAT 0:543d6784d4cc 224 hp = p;
EasyCAT 0:543d6784d4cc 225 mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
EasyCAT 0:543d6784d4cc 226 maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt);
EasyCAT 0:543d6784d4cc 227 NotLast = TRUE;
EasyCAT 0:543d6784d4cc 228 while (NotLast)
EasyCAT 0:543d6784d4cc 229 {
EasyCAT 0:543d6784d4cc 230 framedatasize = psize;
EasyCAT 0:543d6784d4cc 231 NotLast = FALSE;
EasyCAT 0:543d6784d4cc 232 SoEp->idn = htoes(idn);
EasyCAT 0:543d6784d4cc 233 SoEp->incomplete = 0;
EasyCAT 0:543d6784d4cc 234 if (framedatasize > maxdata)
EasyCAT 0:543d6784d4cc 235 {
EasyCAT 0:543d6784d4cc 236 framedatasize = maxdata; /* segmented transfer needed */
EasyCAT 0:543d6784d4cc 237 NotLast = TRUE;
EasyCAT 0:543d6784d4cc 238 SoEp->incomplete = 1;
EasyCAT 0:543d6784d4cc 239 SoEp->fragmentsleft = psize / maxdata;
EasyCAT 0:543d6784d4cc 240 }
EasyCAT 0:543d6784d4cc 241 SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize);
EasyCAT 0:543d6784d4cc 242 /* get new mailbox counter, used for session handle */
EasyCAT 0:543d6784d4cc 243 cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
EasyCAT 0:543d6784d4cc 244 context->slavelist[slave].mbx_cnt = cnt;
EasyCAT 0:543d6784d4cc 245 SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
EasyCAT 0:543d6784d4cc 246 /* copy parameter data to mailbox */
EasyCAT 0:543d6784d4cc 247 memcpy(mp, hp, framedatasize);
EasyCAT 0:543d6784d4cc 248 hp += framedatasize;
EasyCAT 0:543d6784d4cc 249 psize -= framedatasize;
EasyCAT 0:543d6784d4cc 250 /* send SoE request to slave */
EasyCAT 0:543d6784d4cc 251 wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
EasyCAT 0:543d6784d4cc 252 if (wkc > 0) /* succeeded to place mailbox in slave ? */
EasyCAT 0:543d6784d4cc 253 {
EasyCAT 0:543d6784d4cc 254 if (!NotLast || !ecx_mbxempty(context, slave, timeout))
EasyCAT 0:543d6784d4cc 255 {
EasyCAT 0:543d6784d4cc 256 /* clean mailboxbuffer */
EasyCAT 0:543d6784d4cc 257 ec_clearmbx(&MbxIn);
EasyCAT 0:543d6784d4cc 258 /* read slave response */
EasyCAT 0:543d6784d4cc 259 wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
EasyCAT 0:543d6784d4cc 260 if (wkc > 0) /* succeeded to read slave response ? */
EasyCAT 0:543d6784d4cc 261 {
EasyCAT 0:543d6784d4cc 262 NotLast = FALSE;
EasyCAT 0:543d6784d4cc 263 /* slave response should be SoE, WriteRes */
EasyCAT 0:543d6784d4cc 264 if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
EasyCAT 0:543d6784d4cc 265 (aSoEp->opCode == ECT_SOE_WRITERES) &&
EasyCAT 0:543d6784d4cc 266 (aSoEp->error == 0) &&
EasyCAT 0:543d6784d4cc 267 (aSoEp->driveNo == driveNo) &&
EasyCAT 0:543d6784d4cc 268 (aSoEp->elementflags == elementflags))
EasyCAT 0:543d6784d4cc 269 {
EasyCAT 0:543d6784d4cc 270 /* SoE write succeeded */
EasyCAT 0:543d6784d4cc 271 }
EasyCAT 0:543d6784d4cc 272 /* other slave response */
EasyCAT 0:543d6784d4cc 273 else
EasyCAT 0:543d6784d4cc 274 {
EasyCAT 0:543d6784d4cc 275 if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
EasyCAT 0:543d6784d4cc 276 (aSoEp->opCode == ECT_SOE_READRES) &&
EasyCAT 0:543d6784d4cc 277 (aSoEp->error == 1))
EasyCAT 0:543d6784d4cc 278 {
EasyCAT 0:543d6784d4cc 279 mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
EasyCAT 0:543d6784d4cc 280 errorcode = (uint16 *)mp;
EasyCAT 0:543d6784d4cc 281 ecx_SoEerror(context, slave, idn, *errorcode);
EasyCAT 0:543d6784d4cc 282 }
EasyCAT 0:543d6784d4cc 283 else
EasyCAT 0:543d6784d4cc 284 {
EasyCAT 0:543d6784d4cc 285 ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
EasyCAT 0:543d6784d4cc 286 }
EasyCAT 0:543d6784d4cc 287 wkc = 0;
EasyCAT 0:543d6784d4cc 288 }
EasyCAT 0:543d6784d4cc 289 }
EasyCAT 0:543d6784d4cc 290 else
EasyCAT 0:543d6784d4cc 291 {
EasyCAT 0:543d6784d4cc 292 ecx_packeterror(context, slave, idn, 0, 4); /* no response */
EasyCAT 0:543d6784d4cc 293 }
EasyCAT 0:543d6784d4cc 294 }
EasyCAT 0:543d6784d4cc 295 }
EasyCAT 0:543d6784d4cc 296 }
EasyCAT 0:543d6784d4cc 297 return wkc;
EasyCAT 0:543d6784d4cc 298 }
EasyCAT 0:543d6784d4cc 299
EasyCAT 0:543d6784d4cc 300 /** SoE read AT and MTD mapping.
EasyCAT 0:543d6784d4cc 301 *
EasyCAT 0:543d6784d4cc 302 * SoE has standard indexes defined for mapping. This function
EasyCAT 0:543d6784d4cc 303 * tries to read them and collect a full input and output mapping size
EasyCAT 0:543d6784d4cc 304 * of designated slave.
EasyCAT 0:543d6784d4cc 305 *
EasyCAT 0:543d6784d4cc 306 * @param[in] context = context struct
EasyCAT 0:543d6784d4cc 307 * @param[in] slave = Slave number
EasyCAT 0:543d6784d4cc 308 * @param[out] Osize = Size in bits of output mapping (MTD) found
EasyCAT 0:543d6784d4cc 309 * @param[out] Isize = Size in bits of input mapping (AT) found
EasyCAT 0:543d6784d4cc 310 * @return >0 if mapping successful.
EasyCAT 0:543d6784d4cc 311 */
EasyCAT 0:543d6784d4cc 312 int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize)
EasyCAT 0:543d6784d4cc 313 {
EasyCAT 0:543d6784d4cc 314 int retVal = 0;
EasyCAT 0:543d6784d4cc 315 int wkc;
EasyCAT 0:543d6784d4cc 316 int psize;
EasyCAT 0:543d6784d4cc 317 int driveNr;
EasyCAT 0:543d6784d4cc 318 uint16 entries, itemcount;
EasyCAT 0:543d6784d4cc 319 ec_SoEmappingt SoEmapping;
EasyCAT 0:543d6784d4cc 320 ec_SoEattributet SoEattribute;
EasyCAT 0:543d6784d4cc 321
EasyCAT 0:543d6784d4cc 322 *Isize = 0;
EasyCAT 0:543d6784d4cc 323 *Osize = 0;
EasyCAT 0:543d6784d4cc 324 for(driveNr = 0; driveNr < EC_SOE_MAX_DRIVES; driveNr++)
EasyCAT 0:543d6784d4cc 325 {
EasyCAT 0:543d6784d4cc 326 psize = sizeof(SoEmapping);
EasyCAT 0:543d6784d4cc 327 /* read output mapping via SoE */
EasyCAT 0:543d6784d4cc 328 wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
EasyCAT 0:543d6784d4cc 329 if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
EasyCAT 0:543d6784d4cc 330 {
EasyCAT 0:543d6784d4cc 331 /* command word (uint16) is always mapped but not in list */
EasyCAT 0:543d6784d4cc 332 *Osize = 16;
EasyCAT 0:543d6784d4cc 333 for (itemcount = 0 ; itemcount < entries ; itemcount++)
EasyCAT 0:543d6784d4cc 334 {
EasyCAT 0:543d6784d4cc 335 psize = sizeof(SoEattribute);
EasyCAT 0:543d6784d4cc 336 /* read attribute of each IDN in mapping list */
EasyCAT 0:543d6784d4cc 337 wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
EasyCAT 0:543d6784d4cc 338 if ((wkc > 0) && (!SoEattribute.list))
EasyCAT 0:543d6784d4cc 339 {
EasyCAT 0:543d6784d4cc 340 /* length : 0 = 8bit, 1 = 16bit .... */
EasyCAT 0:543d6784d4cc 341 *Osize += (int)8 << SoEattribute.length;
EasyCAT 0:543d6784d4cc 342 }
EasyCAT 0:543d6784d4cc 343 }
EasyCAT 0:543d6784d4cc 344 }
EasyCAT 0:543d6784d4cc 345 psize = sizeof(SoEmapping);
EasyCAT 0:543d6784d4cc 346 /* read input mapping via SoE */
EasyCAT 0:543d6784d4cc 347 wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
EasyCAT 0:543d6784d4cc 348 if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
EasyCAT 0:543d6784d4cc 349 {
EasyCAT 0:543d6784d4cc 350 /* status word (uint16) is always mapped but not in list */
EasyCAT 0:543d6784d4cc 351 *Isize = 16;
EasyCAT 0:543d6784d4cc 352 for (itemcount = 0 ; itemcount < entries ; itemcount++)
EasyCAT 0:543d6784d4cc 353 {
EasyCAT 0:543d6784d4cc 354 psize = sizeof(SoEattribute);
EasyCAT 0:543d6784d4cc 355 /* read attribute of each IDN in mapping list */
EasyCAT 0:543d6784d4cc 356 wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
EasyCAT 0:543d6784d4cc 357 if ((wkc > 0) && (!SoEattribute.list))
EasyCAT 0:543d6784d4cc 358 {
EasyCAT 0:543d6784d4cc 359 /* length : 0 = 8bit, 1 = 16bit .... */
EasyCAT 0:543d6784d4cc 360 *Isize += (int)8 << SoEattribute.length;
EasyCAT 0:543d6784d4cc 361 }
EasyCAT 0:543d6784d4cc 362 }
EasyCAT 0:543d6784d4cc 363 }
EasyCAT 0:543d6784d4cc 364 }
EasyCAT 0:543d6784d4cc 365
EasyCAT 0:543d6784d4cc 366 /* found some I/O bits ? */
EasyCAT 0:543d6784d4cc 367 if ((*Isize > 0) || (*Osize > 0))
EasyCAT 0:543d6784d4cc 368 {
EasyCAT 0:543d6784d4cc 369 retVal = 1;
EasyCAT 0:543d6784d4cc 370 }
EasyCAT 0:543d6784d4cc 371 return retVal;
EasyCAT 0:543d6784d4cc 372 }
EasyCAT 0:543d6784d4cc 373
EasyCAT 0:543d6784d4cc 374 #ifdef EC_VER1
EasyCAT 0:543d6784d4cc 375 int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
EasyCAT 0:543d6784d4cc 376 {
EasyCAT 0:543d6784d4cc 377 return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
EasyCAT 0:543d6784d4cc 378 }
EasyCAT 0:543d6784d4cc 379
EasyCAT 0:543d6784d4cc 380 int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
EasyCAT 0:543d6784d4cc 381 {
EasyCAT 0:543d6784d4cc 382 return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
EasyCAT 0:543d6784d4cc 383 }
EasyCAT 0:543d6784d4cc 384
EasyCAT 0:543d6784d4cc 385 int ec_readIDNmap(uint16 slave, int *Osize, int *Isize)
EasyCAT 0:543d6784d4cc 386 {
EasyCAT 0:543d6784d4cc 387 return ecx_readIDNmap(&ecx_context, slave, Osize, Isize);
EasyCAT 0:543d6784d4cc 388 }
EasyCAT 0:543d6784d4cc 389 #endif