/* * This software is dual-licensed under GPLv3 and a commercial * license. See the file LICENSE.md distributed with this software for * full license information. */ /** \file * \brief * Base EtherCAT functions. * * Setting up a datagram in an ethernet frame. * EtherCAT datagram primitives, broadcast, auto increment, configured and * logical addressed data transfers. All base transfers are blocking, so * wait for the frame to be returned to the master or timeout. If this is * not acceptable build your own datagrams and use the functions from nicdrv.c. */ #include "soem/soem.h" #include #include "oshw.h" #include "osal.h" /** Write data to EtherCAT datagram. * * @param[out] datagramdata data part of datagram * @param[in] com command * @param[in] length length of databuffer * @param[in] data databuffer to be copied into datagram */ static void ecx_writedatagramdata(void *datagramdata, ec_cmdtype com, uint16 length, const void *data) { if (length > 0) { switch (com) { case EC_CMD_NOP: /* Fall-through */ case EC_CMD_APRD: /* Fall-through */ case EC_CMD_FPRD: /* Fall-through */ case EC_CMD_BRD: /* Fall-through */ case EC_CMD_LRD: /* no data to write. initialise data so frame is in a known state */ memset(datagramdata, 0, length); break; default: memcpy(datagramdata, data, length); break; } } } /** Generate and set EtherCAT datagram in a standard ethernet frame. * * @param[in] port port context struct * @param[out] frame framebuffer * @param[in] com command * @param[in] idx index used for TX and RX buffers * @param[in] ADP Address Position * @param[in] ADO Address Offset * @param[in] length length of datagram excluding EtherCAT header * @param[in] data databuffer to be copied in datagram * @return always 0 */ int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data) { ec_comt *datagramP; uint8 *frameP; frameP = frame; /* Ethernet header is preset and fixed in frame buffers EtherCAT header needs to be added after that */ datagramP = (ec_comt *)&frameP[ETH_HEADERSIZE]; datagramP->elength = htoes(EC_ECATTYPE + EC_HEADERSIZE + length); datagramP->command = com; datagramP->index = idx; datagramP->ADP = htoes(ADP); datagramP->ADO = htoes(ADO); datagramP->dlength = htoes(length); ecx_writedatagramdata(&frameP[ETH_HEADERSIZE + EC_HEADERSIZE], com, length, data); /* set WKC to zero */ frameP[ETH_HEADERSIZE + EC_HEADERSIZE + length] = 0x00; frameP[ETH_HEADERSIZE + EC_HEADERSIZE + length + 1] = 0x00; /* set size of frame in buffer array */ port->txbuflength[idx] = ETH_HEADERSIZE + EC_HEADERSIZE + EC_WKCSIZE + length; return 0; } /** Add EtherCAT datagram to a standard ethernet frame with existing datagram(s). * * @param[in] port port context struct * @param[out] frame framebuffer * @param[in] com command * @param[in] idx index used for TX and RX buffers * @param[in] more TRUE if still more datagrams to follow * @param[in] ADP Address Position * @param[in] ADO Address Offset * @param[in] length length of datagram excluding EtherCAT header * @param[in] data databuffer to be copied in datagram * @return Offset to data in rx frame, useful to retrieve data after RX. */ uint16 ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data) { ec_comt *datagramP; uint8 *frameP; uint16 prevlength; frameP = frame; /* copy previous frame size */ prevlength = (uint16)port->txbuflength[idx]; datagramP = (ec_comt *)&frameP[ETH_HEADERSIZE]; /* add new datagram to ethernet frame size */ datagramP->elength = htoes(etohs(datagramP->elength) + EC_HEADERSIZE + length); /* add "datagram follows" flag to previous subframe dlength */ datagramP->dlength = htoes(etohs(datagramP->dlength) | EC_DATAGRAMFOLLOWS); /* set new EtherCAT header position */ datagramP = (ec_comt *)&frameP[prevlength - EC_ELENGTHSIZE]; datagramP->command = com; datagramP->index = idx; datagramP->ADP = htoes(ADP); datagramP->ADO = htoes(ADO); if (more) { /* this is not the last datagram to add */ datagramP->dlength = htoes(length | EC_DATAGRAMFOLLOWS); } else { /* this is the last datagram in the frame */ datagramP->dlength = htoes(length); } ecx_writedatagramdata(&frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE], com, length, data); /* set WKC to zero */ frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length] = 0x00; frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length + 1] = 0x00; /* set size of frame in buffer array */ port->txbuflength[idx] = prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + EC_WKCSIZE + length; /* return offset to data in rx frame 14 bytes smaller than tx frame due to stripping of ethernet header */ return prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE - ETH_HEADERSIZE; } /** BRW "broadcast write" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, normally 0 * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[in] data databuffer to be written to slaves * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_BWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { uint8 idx; int wkc; /* get fresh index */ idx = ecx_getindex(port); /* setup datagram */ ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_BWR, idx, ADP, ADO, length, data); /* send data and wait for answer */ wkc = ecx_srconfirm(port, idx, timeout); /* clear buffer status */ ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** BRD "broadcast read" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, normally 0 * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[out] data databuffer to put slave data in * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_BRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { uint8 idx; int wkc; /* get fresh index */ idx = ecx_getindex(port); /* setup datagram */ ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_BRD, idx, ADP, ADO, length, data); /* send data and wait for answer */ wkc = ecx_srconfirm(port, idx, timeout); if (wkc > 0) { /* copy datagram to data buffer */ memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); } /* clear buffer status */ ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** APRD "auto increment address read" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, each slave ++, slave that has 0 executes * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[out] data databuffer to put slave data in * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { int wkc; uint8 idx; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_APRD, idx, ADP, ADO, length, data); wkc = ecx_srconfirm(port, idx, timeout); if (wkc > 0) { memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); } ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** APRMW "auto increment address read, multiple write" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, each slave ++, slave that has 0 reads, * following slaves write. * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[out] data databuffer to put slave data in * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { int wkc; uint8 idx; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_ARMW, idx, ADP, ADO, length, data); wkc = ecx_srconfirm(port, idx, timeout); if (wkc > 0) { memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); } ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** FPRMW "configured address read, multiple write" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, slave that has address reads, * following slaves write. * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[out] data databuffer to put slave data in * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { int wkc; uint8 idx; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FRMW, idx, ADP, ADO, length, data); wkc = ecx_srconfirm(port, idx, timeout); if (wkc > 0) { memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); } ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** APRDw "auto increment address read" word return primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, each slave ++, slave that has 0 reads. * @param[in] ADO Address Offset, slave memory address * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return word data from slave */ uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout) { uint16 w; w = 0; ecx_APRD(port, ADP, ADO, sizeof(w), &w, timeout); return w; } /** FPRD "configured address read" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, slave that has address reads. * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[out] data databuffer to put slave data in * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { int wkc; uint8 idx; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, ADP, ADO, length, data); wkc = ecx_srconfirm(port, idx, timeout); if (wkc > 0) { memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); } ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** FPRDw "configured address read" word return primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, slave that has address reads. * @param[in] ADO Address Offset, slave memory address * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return word data from slave */ uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout) { uint16 w; w = 0; ecx_FPRD(port, ADP, ADO, sizeof(w), &w, timeout); return w; } /** APWR "auto increment address write" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, each slave ++, slave that has 0 writes. * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[in] data databuffer to write to slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { uint8 idx; int wkc; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_APWR, idx, ADP, ADO, length, data); wkc = ecx_srconfirm(port, idx, timeout); ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** APWRw "auto increment address write" word primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, each slave ++, slave that has 0 writes. * @param[in] ADO Address Offset, slave memory address * @param[in] data word data to write to slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout) { return ecx_APWR(port, ADP, ADO, sizeof(data), &data, timeout); } /** FPWR "configured address write" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, slave that has address writes. * @param[in] ADO Address Offset, slave memory address * @param[in] length length of databuffer * @param[in] data databuffer to write to slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) { int wkc; uint8 idx; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPWR, idx, ADP, ADO, length, data); wkc = ecx_srconfirm(port, idx, timeout); ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** FPWR "configured address write" primitive. Blocking. * * @param[in] port port context struct * @param[in] ADP Address Position, slave that has address writes. * @param[in] ADO Address Offset, slave memory address * @param[in] data word to write to slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout) { return ecx_FPWR(port, ADP, ADO, sizeof(data), &data, timeout); } /** LRW "logical memory read / write" primitive. Blocking. * * @param[in] port port context struct * @param[in] LogAdr Logical memory address * @param[in] length length of databuffer * @param[in,out] data databuffer to write to and read from slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout) { uint8 idx; int wkc; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); wkc = ecx_srconfirm(port, idx, timeout); if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW)) { memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); } ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** LRD "logical memory read" primitive. Blocking. * * @param[in] port port context struct * @param[in] LogAdr Logical memory address * @param[in] length length of bytes to read from slave. * @param[out] data databuffer to read from slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout) { uint8 idx; int wkc; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRD, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); wkc = ecx_srconfirm(port, idx, timeout); if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRD)) { memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); } ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** LWR "logical memory write" primitive. Blocking. * * @param[in] port port context struct * @param[in] LogAdr Logical memory address * @param[in] length length of databuffer * @param[in] data databuffer to write to slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout) { uint8 idx; int wkc; idx = ecx_getindex(port); ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LWR, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); wkc = ecx_srconfirm(port, idx, timeout); ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; } /** LRW "logical memory read / write" primitive plus Clock Distribution. Blocking. * Frame consists of two datagrams, one LRW and one FPRMW. * * @param[in] port port context struct * @param[in] LogAdr Logical memory address * @param[in] length length of databuffer * @param[in,out] data databuffer to write to and read from slave. * @param[in] DCrs Distributed Clock reference slave address. * @param[out] DCtime DC time read from reference slave. * @param[in] timeout timeout in us, standard is EC_TIMEOUTRET * @return Workcounter or EC_NOFRAME */ int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout) { uint16 DCtO; uint8 idx; int wkc; uint64 DCtE; idx = ecx_getindex(port); /* LRW in first datagram */ ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data); /* FPRMW in second datagram */ DCtE = htoell(*DCtime); DCtO = ecx_adddatagram(port, &(port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, DCrs, ECT_REG_DCSYSTIME, sizeof(DCtime), &DCtE); wkc = ecx_srconfirm(port, idx, timeout); if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW)) { memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length); memcpy(&wkc, &(port->rxbuf[idx][EC_HEADERSIZE + length]), EC_WKCSIZE); memcpy(&DCtE, &(port->rxbuf[idx][DCtO]), sizeof(*DCtime)); *DCtime = etohll(DCtE); } ecx_setbufstat(port, idx, EC_BUF_EMPTY); return wkc; }