diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..c37ac74 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,17 @@ +# Copyright (c) 2021-2022 HPMicro +# SPDX-License-Identifier: BSD-3-Clause + +sdk_inc(src) +sdk_inc(src/port) +sdk_src(src/ethercatbase.c) +sdk_src(src/ethercatcoe.c) +sdk_src(src/ethercatconfig.c) +sdk_src(src/ethercatdc.c) +sdk_src(src/ethercatfoe.c) +sdk_src(src/ethercatmain.c) +sdk_src(src/ethercatprint.c) +sdk_src(src/ethercatsoe.c) + +sdk_src(src/port/nicdrv.c) +sdk_src(src/port/osal.c) +sdk_src(src/port/hpm_enet_port.c) \ No newline at end of file diff --git a/src/ethercatbase.c b/src/ethercatbase.c new file mode 100644 index 0000000..aea7754 --- /dev/null +++ b/src/ethercatbase.c @@ -0,0 +1,673 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatbase.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \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 +#include +#include "oshw.h" +#include "osal.h" +#include "ethercattype.h" +#include "ethercatbase.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, usefull to retrieve data after RX. + */ +int 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 = 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 excecutes + * @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; +} + +#ifdef EC_VER1 +int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data) +{ + return ecx_setupdatagram (&ecx_port, frame, com, idx, ADP, ADO, length, data); +} + +int ec_adddatagram (void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data) +{ + return ecx_adddatagram (&ecx_port, frame, com, idx, more, ADP, ADO, length, data); +} + +int ec_BWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_BWR (&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_BRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_BRD(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_APRD(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_ARMW(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_FRMW(&ecx_port, ADP, ADO, length, data, timeout); +} + +uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout) +{ + uint16 w; + + w = 0; + ec_APRD(ADP, ADO, sizeof(w), &w, timeout); + + return w; +} + +int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_FPRD(&ecx_port, ADP, ADO, length, data, timeout); +} + +uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int timeout) +{ + uint16 w; + + w = 0; + ec_FPRD(ADP, ADO, sizeof(w), &w, timeout); + return w; +} + +int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_APWR(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout) +{ + return ec_APWR(ADP, ADO, sizeof(data), &data, timeout); +} + +int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout) +{ + return ecx_FPWR(&ecx_port, ADP, ADO, length, data, timeout); +} + +int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout) +{ + return ec_FPWR(ADP, ADO, sizeof(data), &data, timeout); +} + +int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout) +{ + return ecx_LRW(&ecx_port, LogAdr, length, data, timeout); +} + +int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout) +{ + return ecx_LRD(&ecx_port, LogAdr, length, data, timeout); +} + +int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout) +{ + return ecx_LWR(&ecx_port, LogAdr, length, data, timeout); +} + +int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout) +{ + return ecx_LRWDC(&ecx_port, LogAdr, length, data, DCrs, DCtime, timeout); +} +#endif \ No newline at end of file diff --git a/src/ethercatbase.h b/src/ethercatbase.h new file mode 100644 index 0000000..5f24160 --- /dev/null +++ b/src/ethercatbase.h @@ -0,0 +1,99 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatbase.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatbase.c + */ + +#ifndef _ethercatbase_ +#define _ethercatbase_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); +int ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); +int ecx_BWR(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ecx_BRD(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); +int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); +int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); +int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); +int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); +int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); + +#ifdef EC_VER1 +int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); +int ec_adddatagram(void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); +int ec_BWR(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ec_BRD(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); +int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout); +int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int timeout); +int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); +int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); +int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout); +int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout); +int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout); +int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/ethercatcoe.c b/src/ethercatcoe.c new file mode 100644 index 0000000..9dfdbe3 --- /dev/null +++ b/src/ethercatcoe.c @@ -0,0 +1,1413 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatcoe.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * CAN over EtherCAT (CoE) module. + * + * SDO read / write and SDO service functions + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatcoe.h" + +/** SDO structure, not to be confused with EcSDOserviceT */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 CANOpen; + uint8 Command; + uint16 Index; + uint8 SubIndex; + union + { + uint8 bdata[0x200]; /* variants for easy data access */ + uint16 wdata[0x100]; + uint32 ldata[0x80]; + }; +} ec_SDOt; +PACKED_END + +/** SDO service structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 CANOpen; + uint8 Opcode; + uint8 Reserved; + uint16 Fragments; + union + { + uint8 bdata[0x200]; /* variants for easy data access */ + uint16 wdata[0x100]; + uint32 ldata[0x80]; + }; +} ec_SDOservicet; +PACKED_END + +/** Report SDO error. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index that generated error + * @param[in] SubIdx = Subindex that generated error + * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list + */ +void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = Index; + Ec.SubIdx = SubIdx; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_SDO_ERROR; + Ec.AbortCode = AbortCode; + ecx_pusherror(context, &Ec); +} + +/** Report SDO info error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index that generated error + * @param[in] SubIdx = Subindex that generated error + * @param[in] AbortCode = Abortcode, see EtherCAT documentation for list + */ +static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Slave = Slave; + Ec.Index = Index; + Ec.SubIdx = SubIdx; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR; + Ec.AbortCode = AbortCode; + ecx_pusherror(context, &Ec); +} + +/** CoE SDO read, blocking. Single subindex or Complete Access. + * + * Only a "normal" upload request is issued. If the requested parameter is <= 4bytes + * then a "expedited" response is returned, otherwise a "normal" response. If a "normal" + * response is larger than the mailbox size then the response is segmented. The function + * will combine all segments and copy them to the parameter buffer. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] index = Index to read + * @param[in] subindex = Subindex to read, must be 0 or 1 if CA is used. + * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes read. + * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SDO. + * @param[out] p = Pointer to parameter buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout) +{ + ec_SDOt *SDOp, *aSDOp; + uint16 bytesize, Framedatasize; + int wkc; + int32 SDOlen; + uint8 *bp; + uint8 *hp; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt, toggle; + boolean NotLast; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOt *)&MbxIn; + SDOp = (ec_SDOt *)&MbxOut; + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ + if (CA) + { + SDOp->Command = ECT_SDO_UP_REQ_CA; /* upload request complete access */ + } + else + { + SDOp->Command = ECT_SDO_UP_REQ; /* upload request normal */ + } + SDOp->Index = htoes(index); + if (CA && (subindex > 1)) + { + subindex = 1; + } + SDOp->SubIndex = subindex; + SDOp->ldata[0] = 0; + /* send CoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be CoE, SDO response and the correct index */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + (aSDOp->Index == SDOp->Index)) + { + if ((aSDOp->Command & 0x02) > 0) + { + /* expedited frame response */ + bytesize = 4 - ((aSDOp->Command >> 2) & 0x03); + if (*psize >= bytesize) /* parameter buffer big enough ? */ + { + /* copy parameter in parameter buffer */ + memcpy(p, &aSDOp->ldata[0], bytesize); + /* return the real parameter size */ + *psize = bytesize; + } + else + { + wkc = 0; + ecx_packeterror(context, slave, index, subindex, 3); /* data container too small for type */ + } + } + else + { /* normal frame response */ + SDOlen = etohl(aSDOp->ldata[0]); + /* Does parameter fit in parameter buffer ? */ + if (SDOlen <= *psize) + { + bp = p; + hp = p; + /* calculate mailbox transfer size */ + Framedatasize = (etohs(aSDOp->MbxHeader.length) - 10); + if (Framedatasize < SDOlen) /* transfer in segments? */ + { + /* copy parameter data in parameter buffer */ + memcpy(hp, &aSDOp->ldata[1], Framedatasize); + /* increment buffer pointer */ + hp += Framedatasize; + *psize = Framedatasize; + NotLast = TRUE; + toggle= 0x00; + while (NotLast) /* segmented transfer */ + { + SDOp = (ec_SDOt *)&MbxOut; + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ + SDOp->Command = ECT_SDO_SEG_UP_REQ + toggle; /* segment upload request */ + SDOp->Index = htoes(index); + SDOp->SubIndex = subindex; + SDOp->ldata[0] = 0; + /* send segmented upload request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + /* is mailbox transfered to slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + /* has slave responded ? */ + if (wkc > 0) + { + /* slave response should be CoE, SDO response */ + if ((((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + ((aSDOp->Command & 0xe0) == 0x00))) + { + /* calculate mailbox transfer size */ + Framedatasize = etohs(aSDOp->MbxHeader.length) - 3; + if ((aSDOp->Command & 0x01) > 0) + { /* last segment */ + NotLast = FALSE; + if (Framedatasize == 7) + /* substract unused bytes from frame */ + Framedatasize = Framedatasize - ((aSDOp->Command & 0x0e) >> 1); + /* copy to parameter buffer */ + memcpy(hp, &(aSDOp->Index), Framedatasize); + } + else /* segments follow */ + { + /* copy to parameter buffer */ + memcpy(hp, &(aSDOp->Index), Framedatasize); + /* increment buffer pointer */ + hp += Framedatasize; + } + /* update parametersize */ + *psize += Framedatasize; + } + /* unexpected frame returned from slave */ + else + { + NotLast = FALSE; + if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ + ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0])); + else + ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */ + wkc = 0; + } + } + } + toggle = toggle ^ 0x10; /* toggle bit for segment request */ + } + } + /* non segmented transfer */ + else + { + /* copy to parameter buffer */ + memcpy(bp, &aSDOp->ldata[1], SDOlen); + *psize = SDOlen; + } + } + /* parameter buffer too small */ + else + { + wkc = 0; + ecx_packeterror(context, slave, index, subindex, 3); /* data container too small for type */ + } + } + } + /* other slave response */ + else + { + if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + return wkc; +} + +/** CoE SDO write, blocking. Single subindex or Complete Access. + * + * A "normal" download request is issued, unless we have + * small data, then a "expedited" transfer is used. If the parameter is larger than + * the mailbox size then the download is segmented. The function will split the + * parameter data in segments and send them to the slave one by one. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index to write + * @param[in] SubIndex = Subindex to write, must be 0 or 1 if CA is used. + * @param[in] CA = FALSE = single subindex. TRUE = Complete Access, all subindexes written. + * @param[in] psize = Size in bytes of parameter buffer. + * @param[out] p = Pointer to parameter buffer + * @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, void *p, int Timeout) +{ + ec_SDOt *SDOp, *aSDOp; + int wkc, maxdata; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt, toggle; + uint16 framedatasize; + boolean NotLast; + uint8 *hp; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOt *)&MbxIn; + SDOp = (ec_SDOt *)&MbxOut; + maxdata = context->slavelist[Slave].mbx_l - 0x10; /* data section=mailbox size - 6 mbx - 2 CoE - 8 sdo req */ + /* if small data use expedited transfer */ + if ((psize <= 4) && !CA) + { + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */ + SDOp->Command = ECT_SDO_DOWN_EXP | (((4 - psize) << 2) & 0x0c); /* expedited SDO download transfer */ + SDOp->Index = htoes(Index); + SDOp->SubIndex = SubIndex; + hp = p; + /* copy parameter data to mailbox */ + memcpy(&SDOp->ldata[0], hp, psize); + /* send mailbox SDO download request to slave */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); + if (wkc > 0) + { + /* response should be CoE, SDO response, correct index and subindex */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + (aSDOp->Index == SDOp->Index) && + (aSDOp->SubIndex == SDOp->SubIndex)) + { + /* all OK */ + } + /* unexpected response from slave */ + else + { + if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + } + else + { + framedatasize = psize; + NotLast = FALSE; + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* segmented transfer needed */ + NotLast = TRUE; + } + SDOp->MbxHeader.length = htoes(0x0a + framedatasize); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */ + if (CA) + { + SDOp->Command = ECT_SDO_DOWN_INIT_CA; /* Complete Access, normal SDO init download transfer */ + } + else + { + SDOp->Command = ECT_SDO_DOWN_INIT; /* normal SDO init download transfer */ + } + SDOp->Index = htoes(Index); + SDOp->SubIndex = SubIndex; + if (CA && (SubIndex > 1)) + { + SDOp->SubIndex = 1; + } + SDOp->ldata[0] = htoel(psize); + hp = p; + /* copy parameter data to mailbox */ + memcpy(&SDOp->ldata[1], hp, framedatasize); + hp += framedatasize; + psize -= framedatasize; + /* send mailbox SDO download request to slave */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); + if (wkc > 0) + { + /* response should be CoE, SDO response, correct index and subindex */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + (aSDOp->Index == SDOp->Index) && + (aSDOp->SubIndex == SDOp->SubIndex)) + { + /* all ok */ + maxdata += 7; + toggle = 0; + /* repeat while segments left */ + while (NotLast) + { + SDOp = (ec_SDOt *)&MbxOut; + framedatasize = psize; + NotLast = FALSE; + SDOp->Command = 0x01; /* last segment */ + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* more segments needed */ + NotLast = TRUE; + SDOp->Command = 0x00; /* segments follow */ + } + if (!NotLast && (framedatasize < 7)) + { + SDOp->MbxHeader.length = htoes(0x0a); /* minimum size */ + SDOp->Command = 0x01 + ((7 - framedatasize) << 1); /* last segment reduced octets */ + } + else + { + SDOp->MbxHeader.length = htoes(framedatasize + 3); /* data + 2 CoE + 1 SDO */ + } + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */ + SDOp->Command = SDOp->Command + toggle; /* add toggle bit to command byte */ + /* copy parameter data to mailbox */ + memcpy(&SDOp->Index, hp, framedatasize); + /* update parameter buffer pointer */ + hp += framedatasize; + psize -= framedatasize; + /* send SDO download request */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout); + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && + ((aSDOp->Command & 0xe0) == 0x20)) + { + /* all OK, nothing to do */ + } + else + { + if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ + } + wkc = 0; + NotLast = FALSE; + } + } + } + toggle = toggle ^ 0x10; /* toggle bit for segment request */ + } + } + /* unexpected response from slave */ + else + { + if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + } + + return wkc; +} + +/** CoE RxPDO write, blocking. + * + * A RxPDO download request is issued. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] RxPDOnumber = Related RxPDO number + * @param[in] psize = Size in bytes of PDO buffer. + * @param[out] p = Pointer to PDO buffer + * @return Workcounter from last slave response + */ +int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, void *p) +{ + ec_SDOt *SDOp; + int wkc, maxdata; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + uint16 framedatasize; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + SDOp = (ec_SDOt *)&MbxOut; + maxdata = context->slavelist[Slave].mbx_l - 0x08; /* data section=mailbox size - 6 mbx - 2 CoE */ + framedatasize = psize; + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* limit transfer */ + } + SDOp->MbxHeader.length = htoes(0x02 + framedatasize); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes((RxPDOnumber & 0x01ff) + (ECT_COES_RXPDO << 12)); /* number 9bits service upper 4 bits */ + /* copy PDO data to mailbox */ + memcpy(&SDOp->Command, p, framedatasize); + /* send mailbox RxPDO request to slave */ + wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + + return wkc; +} + +/** CoE TxPDO read remote request, blocking. + * + * A RxPDO download request is issued. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] TxPDOnumber = Related TxPDO number + * @param[in,out] psize = Size in bytes of PDO buffer, returns bytes read from PDO. + * @param[out] p = Pointer to PDO buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout) +{ + ec_SDOt *SDOp, *aSDOp; + int wkc; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + uint16 framedatasize; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOt *)&MbxIn; + SDOp = (ec_SDOt *)&MbxOut; + SDOp->MbxHeader.length = htoes(0x02); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12)); /* number 9bits service upper 4 bits */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be CoE, TxPDO */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_TXPDO)) + { + /* TxPDO response */ + framedatasize = (aSDOp->MbxHeader.length - 2); + if (*psize >= framedatasize) /* parameter buffer big enough ? */ + { + /* copy parameter in parameter buffer */ + memcpy(p, &aSDOp->Command, framedatasize); + /* return the real parameter size */ + *psize = framedatasize; + } + /* parameter buffer too small */ + else + { + wkc = 0; + ecx_packeterror(context, slave, 0, 0, 3); /* data container too small for type */ + } + } + /* other slave response */ + else + { + if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */ + { + ecx_SDOerror(context, slave, 0, 0, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, slave, 0, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} + +/** Read PDO assign structure + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] PDOassign = PDO assign object + * @return total bitlength of PDO assign + */ +int ecx_readPDOassign(ecx_contextt *context, uint16 Slave, uint16 PDOassign) +{ + uint16 idxloop, nidx, subidxloop, rdat, idx, subidx; + uint8 subcnt; + int wkc, bsize = 0, rdl; + int32 rdat2; + + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign subindex 0 ( = number of PDO's) */ + wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + rdat = etohs(rdat); + /* positive result from slave ? */ + if ((wkc > 0) && (rdat > 0)) + { + /* number of available sub indexes */ + nidx = rdat; + bsize = 0; + /* read all PDO's */ + for (idxloop = 1; idxloop <= nidx; idxloop++) + { + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign */ + wkc = ecx_SDOread(context, Slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + /* result is index of PDO */ + idx = etohl(rdat); + if (idx > 0) + { + rdl = sizeof(subcnt); subcnt = 0; + /* read number of subindexes of PDO */ + wkc = ecx_SDOread(context, Slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM); + subidx = subcnt; + /* for each subindex */ + for (subidxloop = 1; subidxloop <= subidx; subidxloop++) + { + rdl = sizeof(rdat2); rdat2 = 0; + /* read SDO that is mapped in PDO */ + wkc = ecx_SDOread(context, Slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM); + rdat2 = etohl(rdat2); + /* extract bitlength of SDO */ + if (LO_BYTE(rdat2) < 0xff) + { + bsize += LO_BYTE(rdat2); + } + else + { + rdl = sizeof(rdat); rdat = htoes(0xff); + /* read Object Entry in Object database */ +// wkc = ec_readOEsingle(idx, (uint8)SubCount, pODlist, pOElist); + bsize += etohs(rdat); + } + } + } + } + } + /* return total found bitlength (PDO) */ + return bsize; +} + +/** Read PDO assign structure in Complete Access mode + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] PDOassign = PDO assign object + * @return total bitlength of PDO assign + */ +int ecx_readPDOassignCA(ecx_contextt *context, uint16 Slave, uint16 PDOassign) +{ + uint16 idxloop, nidx, subidxloop, idx, subidx; + int wkc, bsize = 0, rdl; + + /* find maximum size of PDOassign buffer */ + rdl = sizeof(ec_PDOassignt); + context->PDOassign->n=0; + /* read rxPDOassign in CA mode, all subindexes are read in one struct */ + wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, TRUE, &rdl, context->PDOassign, EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (context->PDOassign->n > 0)) + { + nidx = context->PDOassign->n; + bsize = 0; + /* for each PDO do */ + for (idxloop = 1; idxloop <= nidx; idxloop++) + { + /* get index from PDOassign struct */ + idx = etohs(context->PDOassign->index[idxloop - 1]); + if (idx > 0) + { + rdl = sizeof(ec_PDOdesct); context->PDOdesc->n = 0; + /* read SDO's that are mapped in PDO, CA mode */ + wkc = ecx_SDOread(context, Slave,idx, 0x00, TRUE, &rdl, context->PDOdesc, EC_TIMEOUTRXM); + subidx = context->PDOdesc->n; + /* extract all bitlengths of SDO's */ + for (subidxloop = 1; subidxloop <= subidx; subidxloop++) + { + bsize += LO_BYTE(etohl(context->PDOdesc->PDO[subidxloop -1])); + } + } + } + } + + /* return total found bitlength (PDO) */ + return bsize; +} + +/** CoE read PDO mapping. + * + * CANopen has standard indexes defined for PDO mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. + * + * Principal structure in slave:\n + * 1C00:00 is number of SM defined\n + * 1C00:01 SM0 type -> 1C10\n + * 1C00:02 SM1 type -> 1C11\n + * 1C00:03 SM2 type -> 1C12\n + * 1C00:04 SM3 type -> 1C13\n + * Type 0 = unused, 1 = mailbox in, 2 = mailbox out, + * 3 = outputs (RxPDO), 4 = inputs (TxPDO). + * + * 1C12:00 is number of PDO's defined for SM2\n + * 1C12:01 PDO assign SDO #1 -> f.e. 1A00\n + * 1C12:02 PDO assign SDO #2 -> f.e. 1A04\ + * + * 1A00:00 is number of object defined for this PDO\n + * 1A00:01 object mapping #1, f.e. 60100710 (SDO 6010 SI 07 bitlength 0x10) + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[out] Osize = Size in bits of output mapping (rxPDO) found + * @param[out] Isize = Size in bits of input mapping (txPDO) found + * @return >0 if mapping succesful. + */ +int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize) +{ + int wkc, rdl; + int retVal = 0; + uint8 nSM, iSM, tSM; + int Tsize; + uint8 SMt_bug_add; + + *Isize = 0; + *Osize = 0; + SMt_bug_add = 0; + rdl = sizeof(nSM); nSM = 0; + /* read SyncManager Communication Type object count */ + wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (nSM > 2)) + { + /* make nSM equal to number of defined SM */ + nSM--; + /* limit to maximum number of SM defined, if true the slave can't be configured */ + if (nSM > EC_MAXSM) + nSM = EC_MAXSM; + /* iterate for every SM type defined */ + for (iSM = 2 ; iSM <= nSM ; iSM++) + { + rdl = sizeof(tSM); tSM = 0; + /* read SyncManager Communication Type */ + wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM); + if (wkc > 0) + { +// start slave bug prevention code, remove if possible + if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! + { + SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 + } + if(tSM) + { + tSM += SMt_bug_add; // only add if SMt > 0 + } + if((iSM == 2) && (tSM == 0)) // SM2 has type 0, this is a bug in the slave! + { + tSM = 3; + } + if((iSM == 3) && (tSM == 0)) // SM3 has type 0, this is a bug in the slave! + { + tSM = 4; + } +// end slave bug prevention code + + context->slavelist[Slave].SMtype[iSM] = tSM; + /* check if SM is unused -> clear enable flag */ + if (tSM == 0) + { + context->slavelist[Slave].SM[iSM].SMflags = + htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK); + } + if ((tSM == 3) || (tSM == 4)) + { + /* read the assign PDO */ + Tsize = ecx_readPDOassign(context, Slave, ECT_SDO_PDOASSIGN + iSM ); + /* if a mapping is found */ + if (Tsize) + { + context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8); + if (tSM == 3) + { + /* we are doing outputs */ + *Osize += Tsize; + } + else + { + /* we are doing inputs */ + *Isize += Tsize; + } + } + } + } + } + } + + /* found some I/O bits ? */ + if ((*Isize > 0) || (*Osize > 0)) + { + retVal = 1; + } + + return retVal; +} + +/** CoE read PDO mapping in Complete Access mode (CA). + * + * CANopen has standard indexes defined for PDO mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. Slave has to support CA, otherwise use ec_readPDOmap(). + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[out] Osize = Size in bits of output mapping (rxPDO) found + * @param[out] Isize = Size in bits of input mapping (txPDO) found + * @return >0 if mapping succesful. + */ +int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize) +{ + int wkc, rdl; + int retVal = 0; + uint8 nSM, iSM, tSM; + int Tsize; + uint8 SMt_bug_add; + + *Isize = 0; + *Osize = 0; + SMt_bug_add = 0; + rdl = sizeof(ec_SMcommtypet); + context->SMcommtype->n = 0; + /* read SyncManager Communication Type object count Complete Access*/ + wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, TRUE, &rdl, context->SMcommtype, EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (context->SMcommtype->n > 2)) + { + /* make nSM equal to number of defined SM */ + nSM = context->SMcommtype->n - 1; + /* limit to maximum number of SM defined, if true the slave can't be configured */ + if (nSM > EC_MAXSM) + { + nSM = EC_MAXSM; + ecx_packeterror(context, Slave, 0, 0, 10); /* #SM larger than EC_MAXSM */ + } + /* iterate for every SM type defined */ + for (iSM = 2 ; iSM <= nSM ; iSM++) + { + tSM = context->SMcommtype->SMtype[iSM]; + +// start slave bug prevention code, remove if possible + if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! + { + SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 + } + if(tSM) + { + tSM += SMt_bug_add; // only add if SMt > 0 + } +// end slave bug prevention code + + context->slavelist[Slave].SMtype[iSM] = tSM; + /* check if SM is unused -> clear enable flag */ + if (tSM == 0) + { + context->slavelist[Slave].SM[iSM].SMflags = + htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK); + } + if ((tSM == 3) || (tSM == 4)) + { + /* read the assign PDO */ + Tsize = ecx_readPDOassignCA(context, Slave, ECT_SDO_PDOASSIGN + iSM ); + /* if a mapping is found */ + if (Tsize) + { + context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8); + if (tSM == 3) + { + /* we are doing outputs */ + *Osize += Tsize; + } + else + { + /* we are doing inputs */ + *Isize += Tsize; + } + } + } + } + } + + /* found some I/O bits ? */ + if ((*Isize > 0) || (*Osize > 0)) + { + retVal = 1; + } + return retVal; +} + +/** CoE read Object Description List. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number. + * @param[out] pODlist = resulting Object Description list. + * @return Workcounter of slave response. + */ +int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist) +{ + ec_SDOservicet *SDOp, *aSDOp; + ec_mbxbuft MbxIn, MbxOut; + int wkc; + uint16 x, n, i, sp, offset; + boolean stop; + uint8 cnt; + boolean First; + + pODlist->Slave = Slave; + pODlist->Entries = 0; + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x0008); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_ODLIST_REQ; /* get object description list request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(0x01); /* all objects */ + /* send get object description list request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + x = 0; + sp = 0; + First = TRUE; + offset = 1; /* offset to skip info header in first frame, otherwise set to 0 */ + do + { + stop = TRUE; /* assume this is last iteration */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + /* response should be CoE and "get object description list response" */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES)) + { + if (First) + { + /* extract number of indexes from mailbox data size */ + n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2; + } + else + { + /* extract number of indexes from mailbox data size */ + n = (etohs(aSDOp->MbxHeader.length) - 6) / 2; + } + /* check if indexes fit in buffer structure */ + if ((sp + n) > EC_MAXODLIST) + { + n = EC_MAXODLIST + 1 - sp; + ecx_SDOinfoerror(context, Slave, 0, 0, 0xf000000); /* Too many entries for master buffer */ + stop = TRUE; + } + /* trim to maximum number of ODlist entries defined */ + if ((pODlist->Entries + n) > EC_MAXODLIST) + { + n = EC_MAXODLIST - pODlist->Entries; + } + pODlist->Entries += n; + /* extract indexes one by one */ + for (i = 0; i < n; i++) + { + pODlist->Index[sp + i] = etohs(aSDOp->wdata[i + offset]); + } + sp += n; + /* check if more fragments will follow */ + if (aSDOp->Fragments > 0) + { + stop = FALSE; + } + First = FALSE; + offset = 0; + } + /* got unexpected response from slave */ + else + { + if ((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0])); + stop = TRUE; + } + else + { + ecx_packeterror(context, Slave, 0, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + x += 20; + } + } + x++; + } + while ((x <= 128) && !stop); + } + return wkc; +} + +/** CoE read Object Description. Adds textual description to object indexes. + * + * @param[in] context = context struct + * @param[in] Item = Item number in ODlist. + * @param[in,out] pODlist = referencing Object Description list. + * @return Workcounter of slave response. + */ +int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist) +{ + ec_SDOservicet *SDOp, *aSDOp; + int wkc; + uint16 n, Slave; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + + Slave = pODlist->Slave; + pODlist->DataType[Item] = 0; + pODlist->ObjectCode[Item] = 0; + pODlist->MaxSub[Item] = 0; + pODlist->Name[Item][0] = 0; + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x0008); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_OD_REQ; /* get object description request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(pODlist->Index[Item]); /* Data of Index */ + /* send get object description request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES)) + { + n = (etohs(aSDOp->MbxHeader.length) - 12); /* length of string(name of object) */ + if (n > EC_MAXNAME) + { + n = EC_MAXNAME; /* max chars */ + } + pODlist->DataType[Item] = etohs(aSDOp->wdata[1]); + pODlist->ObjectCode[Item] = aSDOp->bdata[5]; + pODlist->MaxSub[Item] = aSDOp->bdata[4]; + + strncpy(pODlist->Name[Item] , (char *)&aSDOp->bdata[6], n); + pODlist->Name[Item][n] = 0x00; /* String terminator */ + } + /* got unexpected response from slave */ + else + { + if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave,pODlist->Index[Item], 0, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave,pODlist->Index[Item], 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} + +/** CoE read SDO service object entry, single subindex. + * Used in ec_readOE(). + * + * @param[in] context = context struct + * @param[in] Item = Item in ODlist. + * @param[in] SubI = Subindex of item in ODlist. + * @param[in] pODlist = Object description list for reference. + * @param[out] pOElist = resulting object entry structure. + * @return Workcounter of slave response. + */ +int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + ec_SDOservicet *SDOp, *aSDOp; + uint16 wkc, Index, Slave; + int16 n; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + + wkc = 0; + Slave = pODlist->Slave; + Index = pODlist->Index[Item]; + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_OE_REQ; /* get object entry description request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(Index); /* Index */ + SDOp->bdata[2] = SubI; /* SubIndex */ + SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */ + /* send get object entry description request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES)) + { + pOElist->Entries++; + n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */ + if (n > EC_MAXNAME) + { + n = EC_MAXNAME; /* max string length */ + } + if (n < 0 ) + { + n = 0; + } + pOElist->ValueInfo[SubI] = aSDOp->bdata[3]; + pOElist->DataType[SubI] = etohs(aSDOp->wdata[2]); + pOElist->BitLength[SubI] = etohs(aSDOp->wdata[3]); + pOElist->ObjAccess[SubI] = etohs(aSDOp->wdata[4]); + + strncpy(pOElist->Name[SubI] , (char *)&aSDOp->wdata[5], n); + pOElist->Name[SubI][n] = 0x00; /* string terminator */ + } + /* got unexpected response from slave */ + else + { + if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave, Index, SubI, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, Index, SubI, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} + +/** CoE read SDO service object entry. + * + * @param[in] context = context struct + * @param[in] Item = Item in ODlist. + * @param[in] pODlist = Object description list for reference. + * @param[out] pOElist = resulting object entry structure. + * @return Workcounter of slave response. + */ +int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + uint16 SubCount; + int wkc; + uint8 SubI; + + wkc = 0; + pOElist->Entries = 0; + SubI = pODlist->MaxSub[Item]; + /* for each entry found in ODlist */ + for (SubCount = 0; SubCount <= SubI; SubCount++) + { + /* read subindex of entry */ + wkc = ecx_readOEsingle(context, Item, (uint8)SubCount, pODlist, pOElist); + } + + return wkc; +} + +#ifdef EC_VER1 +void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) +{ + ecx_SDOerror(&ecx_context, Slave, Index, SubIdx, AbortCode); +} + +int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout) +{ + return ecx_SDOread(&ecx_context, slave, index, subindex, CA, psize, p, timeout); +} + +int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, void *p, int Timeout) +{ + return ecx_SDOwrite(&ecx_context, Slave, Index, SubIndex, CA, psize, p, Timeout); +} + +int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber, int psize, void *p) +{ + return ecx_RxPDO(&ecx_context, Slave, RxPDOnumber, psize, p); +} + +int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout) +{ + return ecx_TxPDO(&ecx_context, slave, TxPDOnumber, psize, p, timeout); +} + +/** Read PDO assign structure */ +int ec_readPDOassign(uint16 Slave, uint16 PDOassign) +{ + return ecx_readPDOassign(&ecx_context, Slave, PDOassign); +} + +/** Read PDO assign structure in Complete Access mode */ +int ec_readPDOassignCA(uint16 Slave, uint16 PDOassign) +{ + return ecx_readPDOassignCA(&ecx_context, Slave, PDOassign); +} + +int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize) +{ + return ecx_readPDOmap(&ecx_context, Slave, Osize, Isize); +} + +int ec_readPDOmapCA(uint16 Slave, int *Osize, int *Isize) +{ + return ecx_readPDOmapCA(&ecx_context, Slave, Osize, Isize); +} + +int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist) +{ + return ecx_readODlist(&ecx_context, Slave, pODlist); +} + +int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist) +{ + return ecx_readODdescription(&ecx_context, Item, pODlist); +} + +int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + return ecx_readOEsingle(&ecx_context, Item, SubI, pODlist, pOElist); +} + +int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist) +{ + return ecx_readOE(&ecx_context, Item, pODlist, pOElist); +} +#endif diff --git a/src/ethercatcoe.h b/src/ethercatcoe.h new file mode 100644 index 0000000..15d01b3 --- /dev/null +++ b/src/ethercatcoe.h @@ -0,0 +1,131 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatcoe.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatcoe.c + */ + +#ifndef _ethercatcoe_ +#define _ethercatcoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** max entries in Object Description list */ +#define EC_MAXODLIST 1024 + +/** max entries in Object Entry list */ +#define EC_MAXOELIST 256 + +/* Storage for object description list */ +typedef struct +{ + /** slave number */ + uint16 Slave; + /** number of entries in list */ + uint16 Entries; + /** array of indexes */ + uint16 Index[EC_MAXODLIST]; + /** array of datatypes, see EtherCAT specification */ + uint16 DataType[EC_MAXODLIST]; + /** array of object codes, see EtherCAT specification */ + uint8 ObjectCode[EC_MAXODLIST]; + /** number of subindexes for each index */ + uint8 MaxSub[EC_MAXODLIST]; + /** textual description of each index */ + char Name[EC_MAXODLIST][EC_MAXNAME+1]; +} ec_ODlistt; + +/* storage for object list entry information */ +typedef struct +{ + /** number of entries in list */ + uint16 Entries; + /** array of value infos, see EtherCAT specification */ + uint8 ValueInfo[EC_MAXOELIST]; + /** array of value infos, see EtherCAT specification */ + uint16 DataType[EC_MAXOELIST]; + /** array of bit lengths, see EtherCAT specification */ + uint16 BitLength[EC_MAXOELIST]; + /** array of object access bits, see EtherCAT specification */ + uint16 ObjAccess[EC_MAXOELIST]; + /** textual description of each index */ + char Name[EC_MAXOELIST][EC_MAXNAME+1]; +} ec_OElistt; + +#ifdef EC_VER1 +void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); +int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout); +int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, void *p, int Timeout); +int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber , int psize, void *p); +int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); +int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize); +int ec_readPDOmapCA(uint16 Slave, int *Osize, int *Isize); +int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist); +int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist); +int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); +int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); +#endif + +void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); +int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex, + boolean CA, int *psize, void *p, int timeout); +int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex, + boolean CA, int psize, void *p, int Timeout); +int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber , int psize, void *p); +int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); +int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize); +int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize); +int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist); +int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist); +int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); +int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/ethercatconfig.c b/src/ethercatconfig.c new file mode 100644 index 0000000..89895ca --- /dev/null +++ b/src/ethercatconfig.c @@ -0,0 +1,1430 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatconfig.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Configuration module for EtherCAT master. + * + * After successful initialisation with ec_init() or ec_init_redundant() + * the slaves can be auto configured with this module. + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatcoe.h" +#include "ethercatsoe.h" +#include "ethercatconfig.h" + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +/* define maximum number of concurrent threads in mapping */ +#define MAX_MAPT 8 + +typedef struct +{ + int running; + ecx_contextt *context; + uint16 slave; +} ecx_mapt_t; + +ecx_mapt_t ecx_mapt[MAX_MAPT]; +OSAL_THREAD_HANDLE ecx_threadh[MAX_MAPT]; + +#ifdef EC_VER1 +/** Slave configuration structure */ +typedef const struct +{ + /** Manufacturer code of slave */ + uint32 man; + /** ID of slave */ + uint32 id; + /** Readable name */ + char name[EC_MAXNAME + 1]; + /** Data type */ + uint8 Dtype; + /** Input bits */ + uint16 Ibits; + /** Output bits */ + uint16 Obits; + /** SyncManager 2 address */ + uint16 SM2a; + /** SyncManager 2 flags */ + uint32 SM2f; + /** SyncManager 3 address */ + uint16 SM3a; + /** SyncManager 3 flags */ + uint32 SM3f; + /** FMMU 0 activation */ + uint8 FM0ac; + /** FMMU 1 activation */ + uint8 FM1ac; +} ec_configlist_t; + +#include "ethercatconfiglist.h" +#endif + +/** standard SM0 flags configuration for mailbox slaves */ +#define EC_DEFAULTMBXSM0 0x00010026 +/** standard SM1 flags configuration for mailbox slaves */ +#define EC_DEFAULTMBXSM1 0x00010022 +/** standard SM0 flags configuration for digital output slaves */ +#define EC_DEFAULTDOSM0 0x00010044 + +#ifdef EC_VER1 +/** Find slave in standard configuration list ec_configlist[] + * + * @param[in] man = manufacturer + * @param[in] id = ID + * @return index in ec_configlist[] when found, otherwise 0 + */ +int ec_findconfig( uint32 man, uint32 id) +{ + int i = 0; + + do + { + i++; + } while ( (ec_configlist[i].man != EC_CONFIGEND) && + ((ec_configlist[i].man != man) || (ec_configlist[i].id != id)) ); + if (ec_configlist[i].man == EC_CONFIGEND) + { + i = 0; + } + return i; +} +#endif + +void ecx_init_context(ecx_contextt *context) +{ + int lp; + *(context->slavecount) = 0; + /* clean ec_slave array */ + memset(context->slavelist, 0x00, sizeof(ec_slavet) * context->maxslave); + memset(context->grouplist, 0x00, sizeof(ec_groupt) * context->maxgroup); + /* clear slave eeprom cache, does not actually read any eeprom */ + ecx_siigetbyte(context, 0, EC_MAXEEPBUF); + for(lp = 0; lp < context->maxgroup; lp++) + { + context->grouplist[lp].logstartaddr = lp << 16; /* default start address per group entry */ + } +} + +int ecx_detect_slaves(ecx_contextt *context) +{ + uint8 b; + uint16 w; + int wkc; + + /* make special pre-init register writes to enable MAC[1] local administered bit * + * setting for old netX100 slaves */ + b = 0x00; + ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET3); /* Ignore Alias register */ + b = EC_STATE_INIT | EC_STATE_ACK; + ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */ + /* netX100 should now be happy */ + ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */ + wkc = ecx_BRD(context->port, 0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */ + if (wkc > 0) + { + *(context->slavecount) = wkc; + } + return wkc; +} + +static void ecx_set_slaves_to_default(ecx_contextt *context) +{ + uint8 b; + uint16 w; + uint8 zbuf[64]; + memset(&zbuf, 0x00, sizeof(zbuf)); + b = 0x00; + ecx_BWR(context->port, 0x0000, ECT_REG_DLPORT , sizeof(b) , &b, EC_TIMEOUTRET3); /* deact loop manual */ + w = htoes(0x0004); + ecx_BWR(context->port, 0x0000, ECT_REG_IRQMASK , sizeof(w) , &w, EC_TIMEOUTRET3); /* set IRQ mask */ + ecx_BWR(context->port, 0x0000, ECT_REG_RXERR , 8 , &zbuf, EC_TIMEOUTRET3); /* reset CRC counters */ + ecx_BWR(context->port, 0x0000, ECT_REG_FMMU0 , 16 * 3 , &zbuf, EC_TIMEOUTRET3); /* reset FMMU's */ + ecx_BWR(context->port, 0x0000, ECT_REG_SM0 , 8 * 4 , &zbuf, EC_TIMEOUTRET3); /* reset SyncM */ + ecx_BWR(context->port, 0x0000, ECT_REG_DCSYSTIME , 4 , &zbuf, EC_TIMEOUTRET3); /* reset system time+ofs */ + w = htoes(0x1000); + ecx_BWR(context->port, 0x0000, ECT_REG_DCSPEEDCNT , sizeof(w) , &w, EC_TIMEOUTRET3); /* DC speedstart */ + w = htoes(0x0c00); + ecx_BWR(context->port, 0x0000, ECT_REG_DCTIMEFILT , sizeof(w) , &w, EC_TIMEOUTRET3); /* DC filt expr */ + b = 0x00; + ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS , sizeof(b) , &b, EC_TIMEOUTRET3); /* Ignore Alias register */ + b = EC_STATE_INIT | EC_STATE_ACK; + ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL , sizeof(b) , &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */ + b = 2; + ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG , sizeof(b) , &b, EC_TIMEOUTRET3); /* force Eeprom from PDI */ + b = 0; + ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG , sizeof(b) , &b, EC_TIMEOUTRET3); /* set Eeprom to master */ +} + +#ifdef EC_VER1 +static int ecx_config_from_table(ecx_contextt *context, uint16 slave) +{ + int cindex; + ec_slavet *csl; + + csl = &(context->slavelist[slave]); + cindex = ec_findconfig( csl->eep_man, csl->eep_id ); + csl->configindex= cindex; + /* slave found in configuration table ? */ + if (cindex) + { + csl->Dtype = ec_configlist[cindex].Dtype; + strcpy(csl->name ,ec_configlist[cindex].name); + csl->Ibits = ec_configlist[cindex].Ibits; + csl->Obits = ec_configlist[cindex].Obits; + if (csl->Obits) + { + csl->FMMU0func = 1; + } + if (csl->Ibits) + { + csl->FMMU1func = 2; + } + csl->FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac; + csl->FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac; + csl->SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a); + csl->SM[2].SMflags = htoel(ec_configlist[cindex].SM2f); + /* simple (no mailbox) output slave found ? */ + if (csl->Obits && !csl->SM[2].StartAddr) + { + csl->SM[0].StartAddr = htoes(0x0f00); + csl->SM[0].SMlength = htoes((csl->Obits + 7) / 8); + csl->SM[0].SMflags = htoel(EC_DEFAULTDOSM0); + csl->FMMU[0].FMMUactive = 1; + csl->FMMU[0].FMMUtype = 2; + csl->SMtype[0] = 3; + } + /* complex output slave */ + else + { + csl->SM[2].SMlength = htoes((csl->Obits + 7) / 8); + csl->SMtype[2] = 3; + } + csl->SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a); + csl->SM[3].SMflags = htoel(ec_configlist[cindex].SM3f); + /* simple (no mailbox) input slave found ? */ + if (csl->Ibits && !csl->SM[3].StartAddr) + { + csl->SM[1].StartAddr = htoes(0x1000); + csl->SM[1].SMlength = htoes((csl->Ibits + 7) / 8); + csl->SM[1].SMflags = htoel(0x00000000); + csl->FMMU[1].FMMUactive = 1; + csl->FMMU[1].FMMUtype = 1; + csl->SMtype[1] = 4; + } + /* complex input slave */ + else + { + csl->SM[3].SMlength = htoes((csl->Ibits + 7) / 8); + csl->SMtype[3] = 4; + } + } + return cindex; +} +#else +static int ecx_config_from_table(ecx_contextt *context, uint16 slave) +{ + return 0; +} +#endif + +/* If slave has SII and same slave ID done before, use previous data. + * This is safe because SII is constant for same slave ID. + */ +static int ecx_lookup_prev_sii(ecx_contextt *context, uint16 slave) +{ + int i, nSM; + if ((slave > 1) && (*(context->slavecount) > 0)) + { + i = 1; + while(((context->slavelist[i].eep_man != context->slavelist[slave].eep_man) || + (context->slavelist[i].eep_id != context->slavelist[slave].eep_id ) || + (context->slavelist[i].eep_rev != context->slavelist[slave].eep_rev)) && + (i < slave)) + { + i++; + } + if(i < slave) + { + context->slavelist[slave].CoEdetails = context->slavelist[i].CoEdetails; + context->slavelist[slave].FoEdetails = context->slavelist[i].FoEdetails; + context->slavelist[slave].EoEdetails = context->slavelist[i].EoEdetails; + context->slavelist[slave].SoEdetails = context->slavelist[i].SoEdetails; + if(context->slavelist[i].blockLRW > 0) + { + context->slavelist[slave].blockLRW = 1; + context->slavelist[0].blockLRW++; + } + context->slavelist[slave].Ebuscurrent = context->slavelist[i].Ebuscurrent; + context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent; + memcpy(context->slavelist[slave].name, context->slavelist[i].name, EC_MAXNAME + 1); + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + context->slavelist[slave].SM[nSM].StartAddr = context->slavelist[i].SM[nSM].StartAddr; + context->slavelist[slave].SM[nSM].SMlength = context->slavelist[i].SM[nSM].SMlength; + context->slavelist[slave].SM[nSM].SMflags = context->slavelist[i].SM[nSM].SMflags; + } + context->slavelist[slave].FMMU0func = context->slavelist[i].FMMU0func; + context->slavelist[slave].FMMU1func = context->slavelist[i].FMMU1func; + context->slavelist[slave].FMMU2func = context->slavelist[i].FMMU2func; + context->slavelist[slave].FMMU3func = context->slavelist[i].FMMU3func; + EC_PRINT("Copy SII slave %d from %d.\n", slave, i); + return 1; + } + } + return 0; +} + +/** Enumerate and init all slaves. + * + * @param[in] context = context struct + * @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise + * @return Workcounter of slave discover datagram = number of slaves found + */ +int ecx_config_init(ecx_contextt *context, uint8 usetable) +{ + uint16 slave, ADPh, configadr, ssigen; + uint16 topology, estat; + int16 topoc, slavec, aliasadr; + uint8 b,h; + uint8 SMc; + uint32 eedat; + int wkc, cindex, nSM; + + EC_PRINT("ec_config_init %d\n",usetable); + ecx_init_context(context); + wkc = ecx_detect_slaves(context); + if (wkc > 0) + { + ecx_set_slaves_to_default(context); + for (slave = 1; slave <= *(context->slavecount); slave++) + { + ADPh = (uint16)(1 - slave); + context->slavelist[slave].Itype = + etohs(ecx_APRDw(context->port, ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET3)); /* read interface type of slave */ + /* a node offset is used to improve readibility of network frames */ + /* this has no impact on the number of addressable slaves (auto wrap around) */ + ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(slave + EC_NODEOFFSET) , EC_TIMEOUTRET3); /* set node address of slave */ + if (slave == 1) + { + b = 1; /* kill non ecat frames for first slave */ + } + else + { + b = 0; /* pass all frames for following slaves */ + } + ecx_APWRw(context->port, ADPh, ECT_REG_DLCTL, htoes(b), EC_TIMEOUTRET3); /* set non ecat frame behaviour */ + configadr = etohs(ecx_APRDw(context->port, ADPh, ECT_REG_STADR, EC_TIMEOUTRET3)); + context->slavelist[slave].configadr = configadr; + ecx_FPRD(context->port, configadr, ECT_REG_ALIAS, sizeof(aliasadr), &aliasadr, EC_TIMEOUTRET3); + context->slavelist[slave].aliasadr = etohs(aliasadr); + ecx_FPRD(context->port, configadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET3); + estat = etohs(estat); + if (estat & EC_ESTAT_R64) /* check if slave can read 8 byte chunks */ + { + context->slavelist[slave].eep_8byte = 1; + } + ecx_readeeprom1(context, slave, ECT_SII_MANUF); /* Manuf */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + context->slavelist[slave].eep_man = + etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* Manuf */ + ecx_readeeprom1(context, slave, ECT_SII_ID); /* ID */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + context->slavelist[slave].eep_id = + etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* ID */ + ecx_readeeprom1(context, slave, ECT_SII_REV); /* revision */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + context->slavelist[slave].eep_rev = + etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* revision */ + ecx_readeeprom1(context, slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */ + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + eedat = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* write mailbox address and mailboxsize */ + context->slavelist[slave].mbx_wo = (uint16)LO_WORD(eedat); + context->slavelist[slave].mbx_l = (uint16)HI_WORD(eedat); + if (context->slavelist[slave].mbx_l > 0) + { + ecx_readeeprom1(context, slave, ECT_SII_TXMBXADR); /* read mailbox offset */ + } + } + for (slave = 1; slave <= *(context->slavecount); slave++) + { + if (context->slavelist[slave].mbx_l > 0) + { + eedat = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* read mailbox offset */ + context->slavelist[slave].mbx_ro = (uint16)LO_WORD(eedat); /* read mailbox offset */ + context->slavelist[slave].mbx_rl = (uint16)HI_WORD(eedat); /*read mailbox length */ + if (context->slavelist[slave].mbx_rl == 0) + { + context->slavelist[slave].mbx_rl = context->slavelist[slave].mbx_l; + } + ecx_readeeprom1(context, slave, ECT_SII_MBXPROTO); + } + configadr = context->slavelist[slave].configadr; + if ((etohs(ecx_FPRDw(context->port, configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET3)) & 0x04) > 0) /* Support DC? */ + { + context->slavelist[slave].hasdc = TRUE; + } + else + { + context->slavelist[slave].hasdc = FALSE; + } + topology = etohs(ecx_FPRDw(context->port, configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET3)); /* extract topology from DL status */ + h = 0; + b = 0; + if ((topology & 0x0300) == 0x0200) /* port0 open and communication established */ + { + h++; + b |= 0x01; + } + if ((topology & 0x0c00) == 0x0800) /* port1 open and communication established */ + { + h++; + b |= 0x02; + } + if ((topology & 0x3000) == 0x2000) /* port2 open and communication established */ + { + h++; + b |= 0x04; + } + if ((topology & 0xc000) == 0x8000) /* port3 open and communication established */ + { + h++; + b |= 0x08; + } + /* ptype = Physical type*/ + context->slavelist[slave].ptype = + LO_BYTE(etohs(ecx_FPRDw(context->port, configadr, ECT_REG_PORTDES, EC_TIMEOUTRET3))); + context->slavelist[slave].topology = h; + context->slavelist[slave].activeports = b; + /* 0=no links, not possible */ + /* 1=1 link , end of line */ + /* 2=2 links , one before and one after */ + /* 3=3 links , split point */ + /* 4=4 links , cross point */ + /* search for parent */ + context->slavelist[slave].parent = 0; /* parent is master */ + if (slave > 1) + { + topoc = 0; + slavec = slave - 1; + do + { + topology = context->slavelist[slavec].topology; + if (topology == 1) + { + topoc--; /* endpoint found */ + } + if (topology == 3) + { + topoc++; /* split found */ + } + if (topology == 4) + { + topoc += 2; /* cross found */ + } + if (((topoc >= 0) && (topology > 1)) || + (slavec == 1)) /* parent found */ + { + context->slavelist[slave].parent = slavec; + slavec = 1; + } + slavec--; + } + while (slavec > 0); + } + (void)ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE); //* check state change Init */ + + /* set default mailbox configuration if slave has mailbox */ + if (context->slavelist[slave].mbx_l>0) + { + context->slavelist[slave].SMtype[0] = 1; + context->slavelist[slave].SMtype[1] = 2; + context->slavelist[slave].SMtype[2] = 3; + context->slavelist[slave].SMtype[3] = 4; + context->slavelist[slave].SM[0].StartAddr = htoes(context->slavelist[slave].mbx_wo); + context->slavelist[slave].SM[0].SMlength = htoes(context->slavelist[slave].mbx_l); + context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0); + context->slavelist[slave].SM[1].StartAddr = htoes(context->slavelist[slave].mbx_ro); + context->slavelist[slave].SM[1].SMlength = htoes(context->slavelist[slave].mbx_rl); + context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1); + context->slavelist[slave].mbx_proto = + ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); + } + cindex = 0; + /* use configuration table ? */ + if (usetable == 1) + { + cindex = ecx_config_from_table(context, slave); + } + /* slave not in configuration table, find out via SII */ + if (!cindex && !ecx_lookup_prev_sii(context, slave)) + { + ssigen = ecx_siifind(context, slave, ECT_SII_GENERAL); + /* SII general section */ + if (ssigen) + { + context->slavelist[slave].CoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x07); + context->slavelist[slave].FoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x08); + context->slavelist[slave].EoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x09); + context->slavelist[slave].SoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x0a); + if((ecx_siigetbyte(context, slave, ssigen + 0x0d) & 0x02) > 0) + { + context->slavelist[slave].blockLRW = 1; + context->slavelist[0].blockLRW++; + } + context->slavelist[slave].Ebuscurrent = ecx_siigetbyte(context, slave, ssigen + 0x0e); + context->slavelist[slave].Ebuscurrent += ecx_siigetbyte(context, slave, ssigen + 0x0f) << 8; + context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent; + } + /* SII strings section */ + if (ecx_siifind(context, slave, ECT_SII_STRING) > 0) + { + ecx_siistring(context, context->slavelist[slave].name, slave, 1); + } + /* no name for slave found, use constructed name */ + else + { + sprintf(context->slavelist[slave].name, "? M:%8.8x I:%8.8x", + (unsigned int)context->slavelist[slave].eep_man, + (unsigned int)context->slavelist[slave].eep_id); + } + /* SII SM section */ + nSM = ecx_siiSM(context, slave, context->eepSM); + if (nSM>0) + { + context->slavelist[slave].SM[0].StartAddr = htoes(context->eepSM->PhStart); + context->slavelist[slave].SM[0].SMlength = htoes(context->eepSM->Plength); + context->slavelist[slave].SM[0].SMflags = + htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16)); + SMc = 1; + while ((SMc < EC_MAXSM) && ecx_siiSMnext(context, slave, context->eepSM, SMc)) + { + context->slavelist[slave].SM[SMc].StartAddr = htoes(context->eepSM->PhStart); + context->slavelist[slave].SM[SMc].SMlength = htoes(context->eepSM->Plength); + context->slavelist[slave].SM[SMc].SMflags = + htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16)); + SMc++; + } + } + /* SII FMMU section */ + if (ecx_siiFMMU(context, slave, context->eepFMMU)) + { + if (context->eepFMMU->FMMU0 !=0xff) + { + context->slavelist[slave].FMMU0func = context->eepFMMU->FMMU0; + } + if (context->eepFMMU->FMMU1 !=0xff) + { + context->slavelist[slave].FMMU1func = context->eepFMMU->FMMU1; + } + if (context->eepFMMU->FMMU2 !=0xff) + { + context->slavelist[slave].FMMU2func = context->eepFMMU->FMMU2; + } + if (context->eepFMMU->FMMU3 !=0xff) + { + context->slavelist[slave].FMMU3func = context->eepFMMU->FMMU3; + } + } + } + + if (context->slavelist[slave].mbx_l > 0) + { + if (context->slavelist[slave].SM[0].StartAddr == 0x0000) /* should never happen */ + { + EC_PRINT("Slave %d has no proper mailbox in configuration, try default.\n", slave); + context->slavelist[slave].SM[0].StartAddr = htoes(0x1000); + context->slavelist[slave].SM[0].SMlength = htoes(0x0080); + context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0); + context->slavelist[slave].SMtype[0] = 1; + } + if (context->slavelist[slave].SM[1].StartAddr == 0x0000) /* should never happen */ + { + EC_PRINT("Slave %d has no proper mailbox out configuration, try default.\n", slave); + context->slavelist[slave].SM[1].StartAddr = htoes(0x1080); + context->slavelist[slave].SM[1].SMlength = htoes(0x0080); + context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1); + context->slavelist[slave].SMtype[1] = 2; + } + /* program SM0 mailbox in and SM1 mailbox out for slave */ + /* writing both SM in one datagram will solve timing issue in old NETX */ + ecx_FPWR(context->port, configadr, ECT_REG_SM0, sizeof(ec_smt) * 2, + &(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3); + } + /* some slaves need eeprom available to PDI in init->preop transition */ + ecx_eeprom2pdi(context, slave); + /* request pre_op for slave */ + ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP | EC_STATE_ACK) , EC_TIMEOUTRET3); /* set preop status */ + } + } + return wkc; +} + +/* If slave has SII mapping and same slave ID done before, use previous mapping. + * This is safe because SII mapping is constant for same slave ID. + */ +static int ecx_lookup_mapping(ecx_contextt *context, uint16 slave, int *Osize, int *Isize) +{ + int i, nSM; + if ((slave > 1) && (*(context->slavecount) > 0)) + { + i = 1; + while(((context->slavelist[i].eep_man != context->slavelist[slave].eep_man) || + (context->slavelist[i].eep_id != context->slavelist[slave].eep_id ) || + (context->slavelist[i].eep_rev != context->slavelist[slave].eep_rev)) && + (i < slave)) + { + i++; + } + if(i < slave) + { + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + context->slavelist[slave].SM[nSM].SMlength = context->slavelist[i].SM[nSM].SMlength; + context->slavelist[slave].SMtype[nSM] = context->slavelist[i].SMtype[nSM]; + } + *Osize = context->slavelist[i].Obits; + *Isize = context->slavelist[i].Ibits; + context->slavelist[slave].Obits = *Osize; + context->slavelist[slave].Ibits = *Isize; + EC_PRINT("Copy mapping slave %d from %d.\n", slave, i); + return 1; + } + } + return 0; +} + +static int ecx_map_coe_soe(ecx_contextt *context, uint16 slave) +{ + int Isize, Osize; + int rval; + + ecx_statecheck(context, slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); /* check state change pre-op */ + + EC_PRINT(" >Slave %d, configadr %x, state %2.2x\n", + slave, context->slavelist[slave].configadr, context->slavelist[slave].state); + + /* execute special slave configuration hook Pre-Op to Safe-OP */ + if(context->slavelist[slave].PO2SOconfig) /* only if registered */ + { + context->slavelist[slave].PO2SOconfig(slave); + } + /* if slave not found in configlist find IO mapping in slave self */ + if (!context->slavelist[slave].configindex) + { + Isize = 0; + Osize = 0; + if (context->slavelist[slave].mbx_proto & ECT_MBXPROT_COE) /* has CoE */ + { + rval = 0; + if (context->slavelist[slave].CoEdetails & ECT_COEDET_SDOCA) /* has Complete Access */ + { + /* read PDO mapping via CoE and use Complete Access */ + rval = ecx_readPDOmapCA(context, slave, &Osize, &Isize); + } + if (!rval) /* CA not available or not succeeded */ + { + /* read PDO mapping via CoE */ + rval = ecx_readPDOmap(context, slave, &Osize, &Isize); + } + EC_PRINT(" CoE Osize:%d Isize:%d\n", Osize, Isize); + } + if ((!Isize && !Osize) && (context->slavelist[slave].mbx_proto & ECT_MBXPROT_SOE)) /* has SoE */ + { + /* read AT / MDT mapping via SoE */ + rval = ecx_readIDNmap(context, slave, &Osize, &Isize); + context->slavelist[slave].SM[2].SMlength = htoes((Osize + 7) / 8); + context->slavelist[slave].SM[3].SMlength = htoes((Isize + 7) / 8); + EC_PRINT(" SoE Osize:%d Isize:%d\n", Osize, Isize); + } + context->slavelist[slave].Obits = Osize; + context->slavelist[slave].Ibits = Isize; + } + + return 1; +} + +static int ecx_map_sii(ecx_contextt *context, uint16 slave) +{ + int Isize, Osize; + int nSM; + ec_eepromPDOt eepPDO; + + Osize = context->slavelist[slave].Obits; + Isize = context->slavelist[slave].Ibits; + + if (!Isize && !Osize) /* find PDO in previous slave with same ID */ + { + (void)ecx_lookup_mapping(context, slave, &Osize, &Isize); + } + if (!Isize && !Osize) /* find PDO mapping by SII */ + { + memset(&eepPDO, 0, sizeof(eepPDO)); + Isize = (int)ecx_siiPDO(context, slave, &eepPDO, 0); + EC_PRINT(" SII Isize:%d\n", Isize); + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + if (eepPDO.SMbitsize[nSM] > 0) + { + context->slavelist[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8); + context->slavelist[slave].SMtype[nSM] = 4; + EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]); + } + } + Osize = (int)ecx_siiPDO(context, slave, &eepPDO, 1); + EC_PRINT(" SII Osize:%d\n", Osize); + for( nSM=0 ; nSM < EC_MAXSM ; nSM++ ) + { + if (eepPDO.SMbitsize[nSM] > 0) + { + context->slavelist[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8); + context->slavelist[slave].SMtype[nSM] = 3; + EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]); + } + } + } + context->slavelist[slave].Obits = Osize; + context->slavelist[slave].Ibits = Isize; + EC_PRINT(" ISIZE:%d %d OSIZE:%d\n", + context->slavelist[slave].Ibits, Isize,context->slavelist[slave].Obits); + + return 1; +} + +static int ecx_map_sm(ecx_contextt *context, uint16 slave) +{ + uint16 configadr; + int nSM; + + configadr = context->slavelist[slave].configadr; + + EC_PRINT(" SM programming\n"); + if (!context->slavelist[slave].mbx_l && context->slavelist[slave].SM[0].StartAddr) + { + ecx_FPWR(context->port, configadr, ECT_REG_SM0, + sizeof(ec_smt), &(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3); + EC_PRINT(" SM0 Type:%d StartAddr:%4.4x Flags:%8.8x\n", + context->slavelist[slave].SMtype[0], + context->slavelist[slave].SM[0].StartAddr, + context->slavelist[slave].SM[0].SMflags); + } + if (!context->slavelist[slave].mbx_l && context->slavelist[slave].SM[1].StartAddr) + { + ecx_FPWR(context->port, configadr, ECT_REG_SM1, + sizeof(ec_smt), &context->slavelist[slave].SM[1], EC_TIMEOUTRET3); + EC_PRINT(" SM1 Type:%d StartAddr:%4.4x Flags:%8.8x\n", + context->slavelist[slave].SMtype[1], + context->slavelist[slave].SM[1].StartAddr, + context->slavelist[slave].SM[1].SMflags); + } + /* program SM2 to SMx */ + for( nSM = 2 ; nSM < EC_MAXSM ; nSM++ ) + { + if (context->slavelist[slave].SM[nSM].StartAddr) + { + /* check if SM length is zero -> clear enable flag */ + if( context->slavelist[slave].SM[nSM].SMlength == 0) + { + context->slavelist[slave].SM[nSM].SMflags = + htoel( etohl(context->slavelist[slave].SM[nSM].SMflags) & EC_SMENABLEMASK); + } + ecx_FPWR(context->port, configadr, ECT_REG_SM0 + (nSM * sizeof(ec_smt)), + sizeof(ec_smt), &context->slavelist[slave].SM[nSM], EC_TIMEOUTRET3); + EC_PRINT(" SM%d Type:%d StartAddr:%4.4x Flags:%8.8x\n", nSM, + context->slavelist[slave].SMtype[nSM], + context->slavelist[slave].SM[nSM].StartAddr, + context->slavelist[slave].SM[nSM].SMflags); + } + } + if (context->slavelist[slave].Ibits > 7) + { + context->slavelist[slave].Ibytes = (context->slavelist[slave].Ibits + 7) / 8; + } + if (context->slavelist[slave].Obits > 7) + { + context->slavelist[slave].Obytes = (context->slavelist[slave].Obits + 7) / 8; + } + + return 1; +} + +OSAL_THREAD_FUNC ecx_mapper_thread(void *param) +{ + ecx_mapt_t *maptp; + maptp = param; + ecx_map_coe_soe(maptp->context, maptp->slave); + maptp->running = 0; +} + +static int ecx_find_mapt(void) +{ + int p; + p = 0; + while((p < MAX_MAPT) && ecx_mapt[p].running) + { + p++; + } + if(p < MAX_MAPT) + { + return p; + } + else + { + return -1; + } +} + +static int ecx_get_threadcount(void) +{ + int thrc, thrn; + thrc = 0; + for(thrn = 0 ; thrn < MAX_MAPT ; thrn++) + { + thrc += ecx_mapt[thrn].running; + } + return thrc; +} + +/** Map all PDOs in one group of slaves to IOmap. + * + * @param[in] context = context struct + * @param[out] pIOmap = pointer to IOmap + * @param[in] group = group to map, 0 = all groups + * @return IOmap size + */ +int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group) +{ + uint16 slave, configadr; + int BitCount, ByteCount, FMMUsize, FMMUdone; + uint16 SMlength, EndAddr; + uint8 BitPos; + uint8 SMc, FMMUc; + uint32 LogAddr = 0; + uint32 oLogAddr = 0; + uint32 diff; + uint16 currentsegment = 0; + uint32 segmentsize = 0; + int thrn, thrc; + + if ((*(context->slavecount) > 0) && (group < context->maxgroup)) + { + EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group); + LogAddr = context->grouplist[group].logstartaddr; + oLogAddr = LogAddr; + BitPos = 0; + context->grouplist[group].nsegments = 0; + context->grouplist[group].outputsWKC = 0; + context->grouplist[group].inputsWKC = 0; + + for(thrn = 0 ; thrn < MAX_MAPT ; thrn++) + { + ecx_mapt[thrn].running = 0; + } + /* find CoE and SoE mapping of slaves in multiple threads */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + if (!group || (group == context->slavelist[slave].group)) + { + if(MAX_MAPT <= 1) + { + /* serialised version */ + ecx_map_coe_soe(context, slave); + } + else + { + /* multi-threaded version */ + while((thrn = ecx_find_mapt()) < 0) + { + osal_usleep(1000); + } + ecx_mapt[thrn].context = context; + ecx_mapt[thrn].slave = slave; + ecx_mapt[thrn].running = 1; + osal_thread_create(&(ecx_threadh[thrn]), 128000, + &ecx_mapper_thread, &(ecx_mapt[thrn])); + } + } + } + /* wait for all threads to finish */ + do + { + thrc = ecx_get_threadcount(); + if(thrc) + { + osal_usleep(1000); + } + } while(thrc); + /* find SII mapping of slave and program SM */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + if (!group || (group == context->slavelist[slave].group)) + { + ecx_map_sii(context, slave); + ecx_map_sm(context, slave); + } + } + + /* do input mapping of slave and program FMMUs */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + configadr = context->slavelist[slave].configadr; + + if (!group || (group == context->slavelist[slave].group)) + { + FMMUc = context->slavelist[slave].FMMUunused; + SMc = 0; + BitCount = 0; + ByteCount = 0; + EndAddr = 0; + FMMUsize = 0; + FMMUdone = 0; + /* create output mapping */ + if (context->slavelist[slave].Obits) + { + EC_PRINT(" OUTPUT MAPPING\n"); + /* search for SM that contribute to the output mapping */ + while ( (SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Obits + 7) / 8))) + { + EC_PRINT(" FMMU %d\n", FMMUc); + while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) SMc++; + EC_PRINT(" SM%d\n", SMc); + context->slavelist[slave].FMMU[FMMUc].PhysStart = + context->slavelist[slave].SM[SMc].StartAddr; + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + while ( (BitCount < context->slavelist[slave].Obits) && (SMc < (EC_MAXSM - 1)) ) /* more SM for output */ + { + SMc++; + while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) SMc++; + /* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */ + if ( etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr ) + { + break; + } + EC_PRINT(" SM%d\n", SMc); + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + } + + /* bit oriented slave */ + if (!context->slavelist[slave].Obytes) + { + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos; + BitPos += context->slavelist[slave].Obits - 1; + if (BitPos > 7) + { + LogAddr++; + BitPos -= 8; + } + FMMUsize = LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1; + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos; + BitPos ++; + if (BitPos > 7) + { + LogAddr++; + BitPos -= 8; + } + } + /* byte oriented slave */ + else + { + if (BitPos) + { + LogAddr++; + BitPos = 0; + } + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos; + BitPos = 7; + FMMUsize = ByteCount; + if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Obytes) + { + FMMUsize = context->slavelist[slave].Obytes - FMMUdone; + } + LogAddr += FMMUsize; + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos; + BitPos = 0; + } + FMMUdone += FMMUsize; + context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0; + context->slavelist[slave].FMMU[FMMUc].FMMUtype = 2; + context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1; + /* program FMMU for output */ + ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc), + sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3); + context->grouplist[group].outputsWKC++; + if (!context->slavelist[slave].outputs) + { + context->slavelist[slave].outputs = + (uint8 *)(pIOmap) + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart); + context->slavelist[slave].Ostartbit = + context->slavelist[slave].FMMU[FMMUc].LogStartbit; + EC_PRINT(" slave %d Outputs %p startbit %d\n", + slave, + context->slavelist[slave].outputs, + context->slavelist[slave].Ostartbit); + } + FMMUc++; + } + context->slavelist[slave].FMMUunused = FMMUc; + diff = LogAddr - oLogAddr; + oLogAddr = LogAddr; + if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM)) + { + context->grouplist[group].IOsegment[currentsegment] = segmentsize; + if (currentsegment < (EC_MAXIOSEGMENTS - 1)) + { + currentsegment++; + segmentsize = diff; + } + } + else + { + segmentsize += diff; + } + } + } + } + if (BitPos) + { + LogAddr++; + oLogAddr = LogAddr; + BitPos = 0; + if ((segmentsize + 1) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM)) + { + context->grouplist[group].IOsegment[currentsegment] = segmentsize; + if (currentsegment < (EC_MAXIOSEGMENTS - 1)) + { + currentsegment++; + segmentsize = 1; + } + } + else + { + segmentsize += 1; + } + } + context->grouplist[group].outputs = pIOmap; + context->grouplist[group].Obytes = LogAddr; + context->grouplist[group].nsegments = currentsegment + 1; + context->grouplist[group].Isegment = currentsegment; + context->grouplist[group].Ioffset = segmentsize; + if (!group) + { + context->slavelist[0].outputs = pIOmap; + context->slavelist[0].Obytes = LogAddr; /* store output bytes in master record */ + } + + /* do input mapping of slave and program FMMUs */ + for (slave = 1; slave <= *(context->slavecount); slave++) + { + configadr = context->slavelist[slave].configadr; + if (!group || (group == context->slavelist[slave].group)) + { + FMMUc = context->slavelist[slave].FMMUunused; + if (context->slavelist[slave].Obits) /* find free FMMU */ + { + while ( context->slavelist[slave].FMMU[FMMUc].LogStart ) FMMUc++; + } + SMc = 0; + BitCount = 0; + ByteCount = 0; + EndAddr = 0; + FMMUsize = 0; + FMMUdone = 0; + /* create input mapping */ + if (context->slavelist[slave].Ibits) + { + EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave); + /* search for SM that contribute to the input mapping */ + while ( (SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Ibits + 7) / 8))) + { + EC_PRINT(" FMMU %d\n", FMMUc); + while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) SMc++; + EC_PRINT(" SM%d\n", SMc); + context->slavelist[slave].FMMU[FMMUc].PhysStart = + context->slavelist[slave].SM[SMc].StartAddr; + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + while ( (BitCount < context->slavelist[slave].Ibits) && (SMc < (EC_MAXSM - 1)) ) /* more SM for input */ + { + SMc++; + while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) SMc++; + /* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */ + if ( etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr ) + { + break; + } + EC_PRINT(" SM%d\n", SMc); + SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength); + ByteCount += SMlength; + BitCount += SMlength * 8; + EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength; + } + + /* bit oriented slave */ + if (!context->slavelist[slave].Ibytes) + { + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos; + BitPos += context->slavelist[slave].Ibits - 1; + if (BitPos > 7) + { + LogAddr++; + BitPos -= 8; + } + FMMUsize = LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1; + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos; + BitPos ++; + if (BitPos > 7) + { + LogAddr++; + BitPos -= 8; + } + } + /* byte oriented slave */ + else + { + if (BitPos) + { + LogAddr++; + BitPos = 0; + } + context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr); + context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos; + BitPos = 7; + FMMUsize = ByteCount; + if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Ibytes) + { + FMMUsize = context->slavelist[slave].Ibytes - FMMUdone; + } + LogAddr += FMMUsize; + context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize); + context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos; + BitPos = 0; + } + FMMUdone += FMMUsize; + if (context->slavelist[slave].FMMU[FMMUc].LogLength) + { + context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0; + context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1; + context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1; + /* program FMMU for input */ + ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc), + sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3); + /* add one for an input FMMU */ + context->grouplist[group].inputsWKC++; + } + if (!context->slavelist[slave].inputs) + { + context->slavelist[slave].inputs = + (uint8 *)(pIOmap) + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart); + context->slavelist[slave].Istartbit = + context->slavelist[slave].FMMU[FMMUc].LogStartbit; + EC_PRINT(" Inputs %p startbit %d\n", + context->slavelist[slave].inputs, + context->slavelist[slave].Istartbit); + } + FMMUc++; + } + context->slavelist[slave].FMMUunused = FMMUc; + diff = LogAddr - oLogAddr; + oLogAddr = LogAddr; + if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM)) + { + context->grouplist[group].IOsegment[currentsegment] = segmentsize; + if (currentsegment < (EC_MAXIOSEGMENTS - 1)) + { + currentsegment++; + segmentsize = diff; + } + } + else + { + segmentsize += diff; + } + } + + ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */ + ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP) , EC_TIMEOUTRET3); /* set safeop status */ + + if (context->slavelist[slave].blockLRW) + { + context->grouplist[group].blockLRW++; + } + context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent; + } + } + if (BitPos) + { + LogAddr++; + oLogAddr = LogAddr; + BitPos = 0; + if ((segmentsize + 1) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM)) + { + context->grouplist[group].IOsegment[currentsegment] = segmentsize; + if (currentsegment < (EC_MAXIOSEGMENTS - 1)) + { + currentsegment++; + segmentsize = 1; + } + } + else + { + segmentsize += 1; + } + } + context->grouplist[group].IOsegment[currentsegment] = segmentsize; + context->grouplist[group].nsegments = currentsegment + 1; + context->grouplist[group].inputs = (uint8 *)(pIOmap) + context->grouplist[group].Obytes; + context->grouplist[group].Ibytes = LogAddr - context->grouplist[group].Obytes; + if (!group) + { + context->slavelist[0].inputs = (uint8 *)(pIOmap) + context->slavelist[0].Obytes; + context->slavelist[0].Ibytes = LogAddr - context->slavelist[0].Obytes; /* store input bytes in master record */ + } + + EC_PRINT("IOmapSize %d\n", LogAddr - context->grouplist[group].logstartaddr); + + return (LogAddr - context->grouplist[group].logstartaddr); + } + + return 0; +} + +/** Recover slave. + * + * @param[in] context = context struct + * @param[in] slave = slave to recover + * @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3 + * @return >0 if successful + */ +int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout) +{ + int rval; + uint16 ADPh, configadr, readadr, wkc; + + rval = 0; + configadr = context->slavelist[slave].configadr; + ADPh = (uint16)(1 - slave); + /* check if we found another slave than the requested */ + readadr = 0xfffe; + wkc = ecx_APRD(context->port, ADPh, ECT_REG_STADR, sizeof(readadr), &readadr, timeout); + /* correct slave found, finished */ + if(readadr == configadr) + { + return 1; + } + /* only try if no config address*/ + if( (wkc > 0) && (readadr == 0)) + { + /* clear possible slaves at EC_TEMPNODE */ + ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , 0); + /* set temporary node address of slave */ + if(ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(EC_TEMPNODE) , timeout) <= 0) + { + ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , 0); + return 0; /* slave fails to respond */ + } + + context->slavelist[slave].configadr = EC_TEMPNODE; /* temporary config address */ + ecx_eeprom2master(context, slave); /* set Eeprom control to master */ + + /* check if slave is the same as configured before */ + if ((ecx_FPRDw(context->port, EC_TEMPNODE, ECT_REG_ALIAS, timeout) == + context->slavelist[slave].aliasadr) && + (ecx_readeeprom(context, slave, ECT_SII_ID, EC_TIMEOUTEEP) == + context->slavelist[slave].eep_id) && + (ecx_readeeprom(context, slave, ECT_SII_MANUF, EC_TIMEOUTEEP) == + context->slavelist[slave].eep_man) && + (ecx_readeeprom(context, slave, ECT_SII_REV, EC_TIMEOUTEEP) == + context->slavelist[slave].eep_rev)) + { + rval = ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(configadr) , timeout); + context->slavelist[slave].configadr = configadr; + } + else + { + /* slave is not the expected one, remove config address*/ + ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , timeout); + context->slavelist[slave].configadr = configadr; + } + } + + return rval; +} + +/** Reconfigure slave. + * + * @param[in] context = context struct + * @param[in] slave = slave to reconfigure + * @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3 + * @return Slave state + */ +int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout) +{ + int state, nSM, FMMUc; + uint16 configadr; + + configadr = context->slavelist[slave].configadr; + if (ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_INIT) , timeout) <= 0) + { + return 0; + } + state = 0; + ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */ + /* check state change init */ + state = ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE); + if(state == EC_STATE_INIT) + { + /* program all enabled SM */ + for( nSM = 0 ; nSM < EC_MAXSM ; nSM++ ) + { + if (context->slavelist[slave].SM[nSM].StartAddr) + { + ecx_FPWR(context->port, configadr, ECT_REG_SM0 + (nSM * sizeof(ec_smt)), + sizeof(ec_smt), &context->slavelist[slave].SM[nSM], timeout); + } + } + ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP) , timeout); + state = ecx_statecheck(context, slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); /* check state change pre-op */ + if( state == EC_STATE_PRE_OP) + { + /* execute special slave configuration hook Pre-Op to Safe-OP */ + if(context->slavelist[slave].PO2SOconfig) /* only if registered */ + { + context->slavelist[slave].PO2SOconfig(slave); + } + ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP) , timeout); /* set safeop status */ + state = ecx_statecheck(context, slave, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); /* check state change safe-op */ + /* program configured FMMU */ + for( FMMUc = 0 ; FMMUc < context->slavelist[slave].FMMUunused ; FMMUc++ ) + { + ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc), + sizeof(ec_fmmut), &context->slavelist[slave].FMMU[FMMUc], timeout); + } + } + } + + return state; +} + +#ifdef EC_VER1 +int ec_config_init(uint8 usetable) +{ + return ecx_config_init(&ecx_context, usetable); +} + +int ec_config_map_group(void *pIOmap, uint8 group) +{ + return ecx_config_map_group(&ecx_context, pIOmap, group); +} + +/** Map all PDOs from slaves to IOmap. + * + * @param[out] pIOmap = pointer to IOmap + * @return IOmap size + */ +int ec_config_map(void *pIOmap) +{ + return ec_config_map_group(pIOmap, 0); +} + +/** Enumerate / map and init all slaves. + * + * @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise + * @param[out] pIOmap = pointer to IOmap + * @return Workcounter of slave discover datagram = number of slaves found + */ +int ec_config(uint8 usetable, void *pIOmap) +{ + int wkc; + wkc = ec_config_init(usetable); + if (wkc) + { + ec_config_map(pIOmap); + } + return wkc; +} + +int ec_recover_slave(uint16 slave, int timeout) +{ + return ecx_recover_slave(&ecx_context, slave, timeout); +} + +int ec_reconfig_slave(uint16 slave, int timeout) +{ + return ecx_reconfig_slave(&ecx_context, slave, timeout); +} +#endif diff --git a/src/ethercatconfig.h b/src/ethercatconfig.h new file mode 100644 index 0000000..28a24db --- /dev/null +++ b/src/ethercatconfig.h @@ -0,0 +1,76 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatconfig.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatconfig.c + */ + +#ifndef _ethercatconfig_ +#define _ethercatconfig_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define EC_NODEOFFSET 0x1000 +#define EC_TEMPNODE 0xffff + +#ifdef EC_VER1 +int ec_config_init(uint8 usetable); +int ec_config_map(void *pIOmap); +int ec_config_map_group(void *pIOmap, uint8 group); +int ec_config(uint8 usetable, void *pIOmap); +int ec_recover_slave(uint16 slave, int timeout); +int ec_reconfig_slave(uint16 slave, int timeout); +#endif + +int ecx_config_init(ecx_contextt *context, uint8 usetable); +int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group); +int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout); +int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/ethercatconfiglist.h b/src/ethercatconfiglist.h new file mode 100644 index 0000000..a15b7f1 --- /dev/null +++ b/src/ethercatconfiglist.h @@ -0,0 +1,76 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatconfiglist.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * DEPRICATED Configuration list of known EtherCAT slave devices. + * + * If a slave is found in this list it is configured according to the parameters + * in the list. Otherwise the configuration info is read directly from the slave + * EEPROM (SII or Slave Information Interface). + */ + +#ifndef _ethercatconfiglist_ +#define _ethercatconfiglist_ +//#include "LCT Eca.h" + +/* + explanation of dev: + 1: static device with no IO mapping ie EK1100 + 2: input device no mailbox ie simple IO device + 3: output device no mailbox + 4: input device with mailbox configuration + 5: output device with mailbox configuration + 6: input/output device no mailbox + 7: input.output device with mailbox configuration +*/ +#define EC_CONFIGEND 0xffffffff + +ec_configlist_t ec_configlist[] = { + {/*Man=*/0x00000002,/*ID=*/0x13ed3052,/*Name=*/"EL5101" ,/*dtype=*/7,/*Ibits=*/40,/*Obits=*/24,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, + {/*Man=*/0x00681168,/*ID=*/0x00464445,/*Name=*/"Kinco FD",/*dtype=*/7,/*Ibits=*/104,/*Obits=*/88,/*SM2a*/0x1800,/*SM2f*/0x00010064,/*SM3a*/0x1c00,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, + {/*Man=*/0x0000034E,/*ID=*/0x00000000,/*Name=*/"XMC_ESC",/*dtype=*/7,/*Ibits=*/72,/*Obits=*/72,/*SM2a*/0x1100,/*SM2f*/0x00010064,/*SM3a*/0x1400,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, + {/*Man=*/0x0048504D,/*ID=*/0x00000900,/*Name=*/"hpm fpga 3port",/*dtype=*/7,/*Ibits=*/72,/*Obits=*/72,/*SM2a*/0x1100,/*SM2f*/0x00010064,/*SM3a*/0x1800,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, +// {/*Man=*/0x00004321,/*ID=*/0x00008100,/*Name=*/"DM3E-556",/*dtype=*/7,/*Ibits=*/104,/*Obits=*/56,/*SM2a*/0x1200,/*SM2f*/0x00010064,/*SM3a*/0x1400,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, + {/*Man=*/EC_CONFIGEND,/*ID=*/0x00000000,/*Name=*/"" ,/*dtype=*/0,/*Ibits=*/ 0,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0} +}; + +#endif diff --git a/src/ethercatdc.c b/src/ethercatdc.c new file mode 100644 index 0000000..948a88d --- /dev/null +++ b/src/ethercatdc.c @@ -0,0 +1,472 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatdc.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Distributed Clock EtherCAT functions. + * + */ +#include "oshw.h" +#include "osal.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatdc.h" + +#define PORTM0 0x01 +#define PORTM1 0x02 +#define PORTM2 0x04 +#define PORTM3 0x08 + +/** 1st sync pulse delay in ns here 100ms */ +#define SyncDelay ((int32)100000000) + +/** + * Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset. + * + * @param[in] context = context struct + * @param [in] slave Slave number. + * @param [in] act TRUE = active, FALSE = deactivated + * @param [in] CyclTime Cycltime in ns. + * @param [in] CyclShift CyclShift in ns. + */ +void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift) +{ + uint8 h, RA; + uint16 slaveh; + int64 t, t1; + int32 tc; + + slaveh = context->slavelist[slave].configadr; + RA = 0; + + /* stop cyclic operation, ready for next trigger */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); + if (act) + { + RA = 1 + 2; /* act cyclic operation and sync0, sync1 deactivated */ + } + h = 0; + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */ + t1 = 0; + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */ + t1 = etohll(t1); + + /* Calculate first trigger time, always a whole multiple of CyclTime rounded up + plus the shifttime (can be negative) + This insures best sychronisation between slaves, slaves with the same CyclTime + will sync at the same moment (you can use CyclShift to shift the sync) */ + if (CyclTime > 0) + { + t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift; + } + else + { + t = t1 + SyncDelay + CyclShift; + /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */ + } + t = htoell(t); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */ + tc = htoel(CyclTime); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */ +} + +/** + * Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset. + * + * @param[in] context = context struct + * @param [in] slave Slave number. + * @param [in] act TRUE = active, FALSE = deactivated + * @param [in] CyclTime0 Cycltime SYNC0 in ns. + * @param [in] CyclTime1 Cycltime SYNC1 in ns. This time is a delta time in relation to + the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time + as SYNC0. + * @param [in] CyclShift CyclShift in ns. + */ +void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift) +{ + uint8 h, RA; + uint16 slaveh; + int64 t, t1; + int32 tc; + uint32 TrueCyclTime; + + /* Sync1 can be used as a multiple of Sync0, use true cycle time */ + TrueCyclTime = ((CyclTime1 / CyclTime0) + 1) * CyclTime0; + + slaveh = context->slavelist[slave].configadr; + RA = 0; + + /* stop cyclic operation, ready for next trigger */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); + if (act) + { + RA = 1 + 2 + 4; /* act cyclic operation and sync0 + sync1 */ + } + h = 0; + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */ + t1 = 0; + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */ + t1 = etohll(t1); + + /* Calculate first trigger time, always a whole multiple of TrueCyclTime rounded up + plus the shifttime (can be negative) + This insures best sychronisation between slaves, slaves with the same CyclTime + will sync at the same moment (you can use CyclShift to shift the sync) */ + if (CyclTime0 > 0) + { + t = ((t1 + SyncDelay) / TrueCyclTime) * TrueCyclTime + TrueCyclTime + CyclShift; + } + else + { + t = t1 + SyncDelay + CyclShift; + /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */ + } + t = htoell(t); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */ + tc = htoel(CyclTime0); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */ + tc = htoel(CyclTime1); + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */ +} + +/* latched port time of slave */ +static int32 ecx_porttime(ecx_contextt *context, uint16 slave, uint8 port) +{ + int32 ts; + switch (port) + { + case 0: + ts = context->slavelist[slave].DCrtA; + break; + case 1: + ts = context->slavelist[slave].DCrtB; + break; + case 2: + ts = context->slavelist[slave].DCrtC; + break; + case 3: + ts = context->slavelist[slave].DCrtD; + break; + default: + ts = 0; + break; + } + return ts; +} + +/* calculate previous active port of a slave */ +static uint8 ecx_prevport(ecx_contextt *context, uint16 slave, uint8 port) +{ + uint8 pport = port; + uint8 aport = context->slavelist[slave].activeports; + switch(port) + { + case 0: + if(aport & PORTM2) + pport = 2; + else if (aport & PORTM1) + pport = 1; + else if (aport & PORTM3) + pport = 3; + break; + case 1: + if(aport & PORTM3) + pport = 3; + else if (aport & PORTM0) + pport = 0; + else if (aport & PORTM2) + pport = 2; + break; + case 2: + if(aport & PORTM1) + pport = 1; + else if (aport & PORTM3) + pport = 3; + else if (aport & PORTM0) + pport = 0; + break; + case 3: + if(aport & PORTM0) + pport = 0; + else if (aport & PORTM2) + pport = 2; + else if (aport & PORTM1) + pport = 1; + break; + } + return pport; +} + +/* search unconsumed ports in parent, consume and return first open port */ +static uint8 ecx_parentport(ecx_contextt *context, uint16 parent) +{ + uint8 parentport = 0; + uint8 b; + /* search order is important, here 3 - 1 - 2 - 0 */ + b = context->slavelist[parent].consumedports; + if (b & PORTM3) + { + parentport = 3; + b &= (uint8)~PORTM3; + } + else if (b & PORTM1) + { + parentport = 1; + b &= (uint8)~PORTM1; + } + else if (b & PORTM2) + { + parentport = 2; + b &= (uint8)~PORTM2; + } + else if (b & PORTM0) + { + parentport = 0; + b &= (uint8)~PORTM0; + } + context->slavelist[parent].consumedports = b; + return parentport; +} + +/** + * Locate DC slaves, measure propagation delays. + * + * @param[in] context = context struct + * @return boolean if slaves are found with DC + */ +boolean ecx_configdc(ecx_contextt *context) +{ + uint16 i, slaveh, parent, child; + uint16 parenthold = 0; + uint16 prevDCslave = 0; + int32 ht, dt1, dt2, dt3; + int64 hrt; + uint8 entryport; + int8 nlist; + int8 plist[4]; + int32 tlist[4]; + ec_timet mastertime; + uint64 mastertime64; + + context->slavelist[0].hasdc = FALSE; + context->grouplist[0].hasdc = FALSE; + ht = 0; + mastertime = osal_current_time(); + ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); /* latch DCrecvTimeA of all slaves */ + mastertime64 = (((uint64)mastertime.sec * 1000000) + (uint64)mastertime.usec) * 1000; + for (i = 1; i <= *(context->slavecount); i++) + { + context->slavelist[i].consumedports = context->slavelist[i].activeports; + if (context->slavelist[i].hasdc) + { + if (!context->slavelist[0].hasdc) + { + context->slavelist[0].hasdc = TRUE; + context->slavelist[0].DCnext = i; + context->slavelist[i].DCprevious = 0; + context->grouplist[0].hasdc = TRUE; + context->grouplist[0].DCnext = i; + } + else + { + context->slavelist[prevDCslave].DCnext = i; + context->slavelist[i].DCprevious = prevDCslave; + } + /* this branch has DC slave so remove parenthold */ + parenthold = 0; + prevDCslave = i; + slaveh = context->slavelist[i].configadr; + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtA = etohl(ht); + /* 64bit latched DCrecvTimeA of each specific slave */ + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET); + /* use it as offset in order to set local time around 0 + mastertime */ + hrt = htoell(-etohll(hrt) + mastertime64); + /* save it in the offset register */ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET); + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtB = etohl(ht); + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtC = etohl(ht); + (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET); + context->slavelist[i].DCrtD = etohl(ht); + + /* make list of active ports and their time stamps */ + nlist = 0; + if (context->slavelist[i].activeports & PORTM0) + { + plist[nlist] = 0; + tlist[nlist] = context->slavelist[i].DCrtA; + nlist++; + } + if (context->slavelist[i].activeports & PORTM3) + { + plist[nlist] = 3; + tlist[nlist] = context->slavelist[i].DCrtD; + nlist++; + } + if (context->slavelist[i].activeports & PORTM1) + { + plist[nlist] = 1; + tlist[nlist] = context->slavelist[i].DCrtB; + nlist++; + } + if (context->slavelist[i].activeports & PORTM2) + { + plist[nlist] = 2; + tlist[nlist] = context->slavelist[i].DCrtC; + nlist++; + } + /* entryport is port with the lowest timestamp */ + entryport = 0; + if((nlist > 1) && (tlist[1] < tlist[entryport])) + { + entryport = 1; + } + if((nlist > 2) && (tlist[2] < tlist[entryport])) + { + entryport = 2; + } + if((nlist > 3) && (tlist[3] < tlist[entryport])) + { + entryport = 3; + } + entryport = plist[entryport]; + context->slavelist[i].entryport = entryport; + /* consume entryport from activeports */ + context->slavelist[i].consumedports &= (uint8)~(1 << entryport); + + /* finding DC parent of current */ + parent = i; + do + { + child = parent; + parent = context->slavelist[parent].parent; + } + while (!((parent == 0) || (context->slavelist[parent].hasdc))); + /* only calculate propagation delay if slave is not the first */ + if (parent > 0) + { + /* find port on parent this slave is connected to */ + context->slavelist[i].parentport = ecx_parentport(context, parent); + if (context->slavelist[parent].topology == 1) + { + context->slavelist[i].parentport = context->slavelist[parent].entryport; + } + + dt1 = 0; + dt2 = 0; + /* delta time of (parentport - 1) - parentport */ + /* note: order of ports is 0 - 3 - 1 -2 */ + /* non active ports are skipped */ + dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) - + ecx_porttime(context, parent, + ecx_prevport(context, parent, context->slavelist[i].parentport)); + /* current slave has children */ + /* those childrens delays need to be substacted */ + if (context->slavelist[i].topology > 1) + { + dt1 = ecx_porttime(context, i, + ecx_prevport(context, i, context->slavelist[i].entryport)) - + ecx_porttime(context, i, context->slavelist[i].entryport); + } + /* we are only interrested in positive diference */ + if (dt1 > dt3) dt1 = -dt1; + /* current slave is not the first child of parent */ + /* previous childs delays need to be added */ + if ((child - parent) > 1) + { + dt2 = ecx_porttime(context, parent, + ecx_prevport(context, parent, context->slavelist[i].parentport)) - + ecx_porttime(context, parent, context->slavelist[parent].entryport); + } + if (dt2 < 0) dt2 = -dt2; + + /* calculate current slave delay from delta times */ + /* assumption : forward delay equals return delay */ + context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 + + context->slavelist[parent].pdelay; + ht = htoel(context->slavelist[i].pdelay); + /* write propagation delay*/ + (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET); + } + } + else + { + context->slavelist[i].DCrtA = 0; + context->slavelist[i].DCrtB = 0; + context->slavelist[i].DCrtC = 0; + context->slavelist[i].DCrtD = 0; + parent = context->slavelist[i].parent; + /* if non DC slave found on first position on branch hold root parent */ + if ( (parent > 0) && (context->slavelist[parent].topology > 2)) + parenthold = parent; + /* if branch has no DC slaves consume port on root parent */ + if ( parenthold && (context->slavelist[i].topology == 1)) + { + ecx_parentport(context, parenthold); + parenthold = 0; + } + } + } + + return context->slavelist[0].hasdc; +} + +#ifdef EC_VER1 +void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift) +{ + ecx_dcsync0(&ecx_context, slave, act, CyclTime, CyclShift); +} + +void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift) +{ + ecx_dcsync01(&ecx_context, slave, act, CyclTime0, CyclTime1, CyclShift); +} + +boolean ec_configdc(void) +{ + return ecx_configdc(&ecx_context); +} +#endif diff --git a/src/ethercatdc.h b/src/ethercatdc.h new file mode 100644 index 0000000..08e90af --- /dev/null +++ b/src/ethercatdc.h @@ -0,0 +1,69 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatdc.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatdc.c + */ + +#ifndef _EC_ECATDC_H +#define _EC_ECATDC_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifdef EC_VER1 +boolean ec_configdc(); +void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift); +void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift); +#endif + +boolean ecx_configdc(ecx_contextt *context); +void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift); +void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift); + +#ifdef __cplusplus +} +#endif + +#endif /* _EC_ECATDC_H */ diff --git a/src/ethercatfoe.c b/src/ethercatfoe.c new file mode 100644 index 0000000..a1e9e25 --- /dev/null +++ b/src/ethercatfoe.c @@ -0,0 +1,401 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatfoe.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + + * 14-06-2010 : fixed bug in FOEread() by Torsten Bitterlich + */ + +/** \file + * \brief + * File over EtherCAT (FoE) module. + * + * SDO read / write and SDO service functions + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatfoe.h" + +#define EC_MAXFOEDATA 512 + +/** FOE structure. + * Used for Read, Write, Data, Ack and Error mailbox packets. + */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint8 OpCode; + uint8 Reserved; + union + { + uint32 Password; + uint32 PacketNumber; + uint32 ErrorCode; + }; + union + { + char FileName[EC_MAXFOEDATA]; + uint8 Data[EC_MAXFOEDATA]; + char ErrorText[EC_MAXFOEDATA]; + }; +} ec_FOEt; +PACKED_END + +/** FoE progress hook. + * + * @param[in] context = context struct + * @param[in] hook = Pointer to hook function. + * @return 1 + */ +int ecx_FOEdefinehook(ecx_contextt *context, void *hook) +{ + context->FOEhook = hook; + return 1; +} + +/** FoE read, blocking. + * + * @param[in] context = context struct + * @param[in] slave = Slave number. + * @param[in] filename = Filename of file to read. + * @param[in] password = password. + * @param[in,out] psize = Size in bytes of file buffer, returns bytes read from file. + * @param[out] p = Pointer to file buffer + * @param[in] timeout = Timeout per mailbox cycle in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout) +{ + ec_FOEt *FOEp, *aFOEp; + int wkc; + int32 dataread = 0; + int32 buffersize, packetnumber, prevpacket = 0; + uint16 fnsize, maxdata, segmentdata; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean worktodo; + + buffersize = *psize; + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aFOEp = (ec_FOEt *)&MbxIn; + FOEp = (ec_FOEt *)&MbxOut; + fnsize = strlen(filename); + maxdata = context->slavelist[slave].mbx_l - 12; + if (fnsize > maxdata) + { + fnsize = maxdata; + } + FOEp->MbxHeader.length = htoes(0x0006 + fnsize); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ + FOEp->OpCode = ECT_FOE_READ; + FOEp->Password = htoel(password); + /* copy filename in mailbox */ + memcpy(&FOEp->FileName[0], filename, fnsize); + /* send FoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + do + { + worktodo = FALSE; + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be FoE */ + if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE) + { + if(aFOEp->OpCode == ECT_FOE_DATA) + { + segmentdata = etohs(aFOEp->MbxHeader.length) - 0x0006; + packetnumber = etohl(aFOEp->PacketNumber); + if ((packetnumber == ++prevpacket) && (dataread + segmentdata <= buffersize)) + { + memcpy(p, &aFOEp->Data[0], segmentdata); + dataread += segmentdata; + p = (uint8 *)p + segmentdata; + if (segmentdata == maxdata) + { + worktodo = TRUE; + } + FOEp->MbxHeader.length = htoes(0x0006); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ + FOEp->OpCode = ECT_FOE_ACK; + FOEp->PacketNumber = htoel(packetnumber); + /* send FoE ack to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc <= 0) + { + worktodo = FALSE; + } + if (context->FOEhook) + { + context->FOEhook(slave, packetnumber, dataread); + } + } + else + { + /* FoE error */ + wkc = -EC_ERR_TYPE_FOE_BUF2SMALL; + } + } + else + { + if(aFOEp->OpCode == ECT_FOE_ERROR) + { + /* FoE error */ + wkc = -EC_ERR_TYPE_FOE_ERROR; + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + *psize = dataread; + } + } while (worktodo); + } + + return wkc; +} + +/** FoE write, blocking. + * + * @param[in] context = context struct + * @param[in] slave = Slave number. + * @param[in] filename = Filename of file to write. + * @param[in] password = password. + * @param[in] psize = Size in bytes of file buffer. + * @param[out] p = Pointer to file buffer + * @param[in] timeout = Timeout per mailbox cycle in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout) +{ + ec_FOEt *FOEp, *aFOEp; + int wkc; + int32 packetnumber, sendpacket = 0; + uint16 fnsize, maxdata; + int segmentdata; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean worktodo, dofinalzero; + int tsize; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aFOEp = (ec_FOEt *)&MbxIn; + FOEp = (ec_FOEt *)&MbxOut; + dofinalzero = FALSE; + fnsize = strlen(filename); + maxdata = context->slavelist[slave].mbx_l - 12; + if (fnsize > maxdata) + { + fnsize = maxdata; + } + FOEp->MbxHeader.length = htoes(0x0006 + fnsize); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ + FOEp->OpCode = ECT_FOE_WRITE; + FOEp->Password = htoel(password); + /* copy filename in mailbox */ + memcpy(&FOEp->FileName[0], filename, fnsize); + /* send FoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + do + { + worktodo = FALSE; + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be FoE */ + if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE) + { + switch (aFOEp->OpCode) + { + case ECT_FOE_ACK: + { + packetnumber = etohl(aFOEp->PacketNumber); + if (packetnumber == sendpacket) + { + if (context->FOEhook) + { + context->FOEhook(slave, packetnumber, psize); + } + tsize = psize; + if (tsize > maxdata) + { + tsize = maxdata; + } + if(tsize || dofinalzero) + { + worktodo = TRUE; + dofinalzero = FALSE; + segmentdata = tsize; + psize -= segmentdata; + /* if last packet was full size, add a zero size packet as final */ + /* EOF is defined as packetsize < full packetsize */ + if (!psize && (segmentdata == maxdata)) + { + dofinalzero = TRUE; + } + FOEp->MbxHeader.length = htoes(0x0006 + segmentdata); + FOEp->MbxHeader.address = htoes(0x0000); + FOEp->MbxHeader.priority = 0x00; + /* get new mailbox count value */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ + FOEp->OpCode = ECT_FOE_DATA; + sendpacket++; + FOEp->PacketNumber = htoel(sendpacket); + memcpy(&FOEp->Data[0], p, segmentdata); + p = (uint8 *)p + segmentdata; + /* send FoE data to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc <= 0) + { + worktodo = FALSE; + } + } + } + else + { + /* FoE error */ + wkc = -EC_ERR_TYPE_FOE_PACKETNUMBER; + } + break; + } + case ECT_FOE_BUSY: + { + /* resend if data has been send before */ + /* otherwise ignore */ + if (sendpacket) + { + if (!psize) + { + dofinalzero = TRUE; + } + psize += segmentdata; + p = (uint8 *)p - segmentdata; + --sendpacket; + } + break; + } + case ECT_FOE_ERROR: + { + /* FoE error */ + wkc = -EC_ERR_TYPE_FOE_ERROR; + break; + } + default: + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + break; + } + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } while (worktodo); + } + + return wkc; +} + +#ifdef EC_VER1 +int ec_FOEdefinehook(void *hook) +{ + return ecx_FOEdefinehook(&ecx_context, hook); +} + +int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout) +{ + return ecx_FOEread(&ecx_context, slave, filename, password, psize, p, timeout); +} + +int ec_FOEwrite(uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout) +{ + return ecx_FOEwrite(&ecx_context, slave, filename, password, psize, p, timeout); +} +#endif diff --git a/src/ethercatfoe.h b/src/ethercatfoe.h new file mode 100644 index 0000000..f18bf19 --- /dev/null +++ b/src/ethercatfoe.h @@ -0,0 +1,69 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatfoe.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatfoe.c + */ + +#ifndef _ethercatfoe_ +#define _ethercatfoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifdef EC_VER1 +int ec_FOEdefinehook(void *hook); +int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); +int ec_FOEwrite(uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); +#endif + +int ecx_FOEdefinehook(ecx_contextt *context, void *hook); +int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); +int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/ethercatmain.c b/src/ethercatmain.c new file mode 100644 index 0000000..2424b54 --- /dev/null +++ b/src/ethercatmain.c @@ -0,0 +1,2059 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatmain.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** + * \file + * \brief + * Main EtherCAT functions. + * + * Initialisation, state set and read, mailbox primitives, EEPROM primitives, + * SII reading and processdata exchange. + * + * Defines ec_slave[]. All slave information is put in this structure. + * Needed for most user interaction with slaves. + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" + + +/** delay in us for eeprom ready loop */ +#define EC_LOCALDELAY 200 + +/** record for ethercat eeprom communications */ +PACKED_BEGIN +typedef struct PACKED +{ + uint16 comm; + uint16 addr; + uint16 d2; +} ec_eepromt; +PACKED_END + +/** mailbox error structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 Type; + uint16 Detail; +} ec_mbxerrort; +PACKED_END + +/** emergency request structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 CANOpen; + uint16 ErrorCode; + uint8 ErrorReg; + uint8 bData; + uint16 w1,w2; +} ec_emcyt; +PACKED_END + +#ifdef EC_VER1 +/** Main slave data array. + * Each slave found on the network gets its own record. + * ec_slave[0] is reserved for the master. Structure gets filled + * in by the configuration function ec_config(). + */ +ec_slavet ec_slave[EC_MAXSLAVE]; +/** number of slaves found on the network */ +int ec_slavecount; +/** slave group structure */ +ec_groupt ec_group[EC_MAXGROUP]; + +/** cache for EEPROM read functions */ +static uint8 ec_esibuf[EC_MAXEEPBUF]; +/** bitmap for filled cache buffer bytes */ +static uint32 ec_esimap[EC_MAXEEPBITMAP]; +/** current slave for EEPROM cache buffer */ +static ec_eringt ec_elist; +static ec_idxstackT ec_idxstack; + +/** SyncManager Communication Type struct to store data of one slave */ +static ec_SMcommtypet ec_SMcommtype; +/** PDO assign struct to store data of one slave */ +static ec_PDOassignt ec_PDOassign; +/** PDO description struct to store data of one slave */ +static ec_PDOdesct ec_PDOdesc; + +/** buffer for EEPROM SM data */ +static ec_eepromSMt ec_SM; +/** buffer for EEPROM FMMU data */ +static ec_eepromFMMUt ec_FMMU; +/** Global variable TRUE if error available in error stack */ +boolean EcatError = FALSE; + +int64 ec_DCtime; + +ecx_portt ecx_port; +ecx_redportt ecx_redport; + +ecx_contextt ecx_context = { + &ecx_port, // .port = + &ec_slave[0], // .slavelist = + &ec_slavecount, // .slavecount = + EC_MAXSLAVE, // .maxslave = + &ec_group[0], // .grouplist = + EC_MAXGROUP, // .maxgroup = + &ec_esibuf[0], // .esibuf = + &ec_esimap[0], // .esimap = + 0, // .esislave = + &ec_elist, // .elist = + &ec_idxstack, // .idxstack = + &EcatError, // .ecaterror = + 0, // .DCtO = + 0, // .DCl = + &ec_DCtime, // .DCtime = + &ec_SMcommtype, // .SMcommtype = + &ec_PDOassign, // .PDOassign = + &ec_PDOdesc, // .PDOdesc = + &ec_SM, // .eepSM = + &ec_FMMU, // .eepFMMU = + NULL // .FOEhook() +}; +#endif + +/** Create list over available network adapters. + * + * @return First element in list over available network adapters. + */ +ec_adaptert * ec_find_adapters (void) +{ + ec_adaptert * ret_adapter; + + ret_adapter = oshw_find_adapters (); + + return ret_adapter; +} + +/** Free dynamically allocated list over available network adapters. + * + * @param[in] adapter = Struct holding adapter name, description and pointer to next. + */ +void ec_free_adapters (ec_adaptert * adapter) +{ + oshw_free_adapters (adapter); +} + +/** Pushes an error on the error list. + * + * @param[in] context = context struct + * @param[in] Ec pointer describing the error. + */ +void ecx_pusherror(ecx_contextt *context, const ec_errort *Ec) +{ + context->elist->Error[context->elist->head] = *Ec; + context->elist->Error[context->elist->head].Signal = TRUE; + context->elist->head++; + if (context->elist->head > EC_MAXELIST) + { + context->elist->head = 0; + } + if (context->elist->head == context->elist->tail) + { + context->elist->tail++; + } + if (context->elist->tail > EC_MAXELIST) + { + context->elist->tail = 0; + } + *(context->ecaterror) = TRUE; +} + +/** Pops an error from the list. + * + * @param[in] context = context struct + * @param[out] Ec = Struct describing the error. + * @return TRUE if an error was popped. + */ +boolean ecx_poperror(ecx_contextt *context, ec_errort *Ec) +{ + boolean notEmpty = (context->elist->head != context->elist->tail); + + *Ec = context->elist->Error[context->elist->tail]; + context->elist->Error[context->elist->tail].Signal = FALSE; + if (notEmpty) + { + context->elist->tail++; + if (context->elist->tail > EC_MAXELIST) + { + context->elist->tail = 0; + } + } + else + { + *(context->ecaterror) = FALSE; + } + return notEmpty; +} + +/** Check if error list has entries. + * + * @param[in] context = context struct + * @return TRUE if error list contains entries. + */ +boolean ecx_iserror(ecx_contextt *context) +{ + return (context->elist->head != context->elist->tail); +} + +/** Report packet error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Index = Index that generated error + * @param[in] SubIdx = Subindex that generated error + * @param[in] ErrorCode = Error code + */ +void ecx_packeterror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = Index; + Ec.SubIdx = SubIdx; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_PACKET_ERROR; + Ec.ErrorCode = ErrorCode; + ecx_pusherror(context, &Ec); +} + +/** Report Mailbox Error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] Detail = Following EtherCAT specification + */ +static void ecx_mbxerror(ecx_contextt *context, uint16 Slave,uint16 Detail) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = 0; + Ec.SubIdx = 0; + Ec.Etype = EC_ERR_TYPE_MBX_ERROR; + Ec.ErrorCode = Detail; + ecx_pusherror(context, &Ec); +} + +/** Report Mailbox Emergency Error + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] ErrorCode = Following EtherCAT specification + * @param[in] ErrorReg + * @param[in] b1 + * @param[in] w1 + * @param[in] w2 + */ +static void ecx_mbxemergencyerror(ecx_contextt *context, uint16 Slave,uint16 ErrorCode,uint16 ErrorReg, + uint8 b1, uint16 w1, uint16 w2) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = 0; + Ec.SubIdx = 0; + Ec.Etype = EC_ERR_TYPE_EMERGENCY; + Ec.ErrorCode = ErrorCode; + Ec.ErrorReg = (uint8)ErrorReg; + Ec.b1 = b1; + Ec.w1 = w1; + Ec.w2 = w2; + ecx_pusherror(context, &Ec); +} + +/** Initialise lib in single NIC mode + * @param[in] context = context struct + * @param[in] ifname = Dev name, f.e. "eth0" + * @return >0 if OK + */ +int ecx_init(ecx_contextt *context, char * ifname) +{ + return ecx_setupnic(context->port, ifname, FALSE); +} + +/** Initialise lib in redundant NIC mode + * @param[in] context = context struct + * @param[in] redport = pointer to redport, redundant port data + * @param[in] ifname = Primary Dev name, f.e. "eth0" + * @param[in] if2name = Secondary Dev name, f.e. "eth1" + * @return >0 if OK + */ +int ecx_init_redundant(ecx_contextt *context, ecx_redportt *redport, char *ifname, char *if2name) +{ + int rval, zbuf; + ec_etherheadert *ehp; + + context->port->redport = redport; + ecx_setupnic(context->port, ifname, FALSE); + rval = ecx_setupnic(context->port, if2name, TRUE); + /* prepare "dummy" BRD tx frame for redundant operation */ + ehp = (ec_etherheadert *)&(context->port->txbuf2); + ehp->sa1 = oshw_htons(secMAC[0]); + zbuf = 0; + ecx_setupdatagram(context->port, &(context->port->txbuf2), EC_CMD_BRD, 0, 0x0000, 0x0000, 2, &zbuf); + context->port->txbuflength2 = ETH_HEADERSIZE + EC_HEADERSIZE + EC_WKCSIZE + 2; + + return rval; +} + +/** Close lib. + * @param[in] context = context struct + */ +void ecx_close(ecx_contextt *context) +{ + ecx_closenic(context->port); +}; + +/** Read one byte from slave EEPROM via cache. + * If the cache location is empty then a read request is made to the slave. + * Depending on the slave capabillities the request is 4 or 8 bytes. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[in] address = eeprom address in bytes (slave uses words) + * @return requested byte, if not available then 0xff + */ +uint8 ecx_siigetbyte(ecx_contextt *context, uint16 slave, uint16 address) +{ + uint16 configadr, eadr; + uint64 edat; + uint16 mapw, mapb; + int lp,cnt; + uint8 retval; + + retval = 0xff; + if (slave != context->esislave) /* not the same slave? */ + { + memset(context->esimap, 0x00, EC_MAXEEPBITMAP * sizeof(uint32)); /* clear esibuf cache map */ + context->esislave = slave; + } + if (address < EC_MAXEEPBUF) + { + mapw = address >> 5; + mapb = address - (mapw << 5); + if (context->esimap[mapw] & (uint32)(1 << mapb)) + { + /* byte is already in buffer */ + retval = context->esibuf[address]; + } + else + { + /* byte is not in buffer, put it there */ + configadr = context->slavelist[slave].configadr; + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + eadr = address >> 1; + edat = ecx_readeepromFP (context, configadr, eadr, EC_TIMEOUTEEP); + /* 8 byte response */ + if (context->slavelist[slave].eep_8byte) + { + put_unaligned64(edat, &(context->esibuf[eadr << 1])); + cnt = 8; + } + /* 4 byte response */ + else + { + put_unaligned32(edat, &(context->esibuf[eadr << 1])); + cnt = 4; + } + /* find bitmap location */ + mapw = eadr >> 4; + mapb = (eadr << 1) - (mapw << 5); + for(lp = 0 ; lp < cnt ; lp++) + { + /* set bitmap for each byte that is read */ + context->esimap[mapw] |= (1 << mapb); + mapb++; + if (mapb > 31) + { + mapb = 0; + mapw++; + } + } + retval = context->esibuf[address]; + } + } + + return retval; +} + +/** Find SII section header in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[in] cat = section category + * @return byte address of section at section length entry, if not available then 0 + */ +int16 ecx_siifind(ecx_contextt *context, uint16 slave, uint16 cat) +{ + int16 a; + uint16 p; + uint8 eectl = context->slavelist[slave].eep_pdi; + + a = ECT_SII_START << 1; + /* read first SII section category */ + p = ecx_siigetbyte(context, slave, a++); + p += (ecx_siigetbyte(context, slave, a++) << 8); + /* traverse SII while category is not found and not EOF */ + while ((p != cat) && (p != 0xffff)) + { + /* read section length */ + p = ecx_siigetbyte(context, slave, a++); + p += (ecx_siigetbyte(context, slave, a++) << 8); + /* locate next section category */ + a += p << 1; + /* read section category */ + p = ecx_siigetbyte(context, slave, a++); + p += (ecx_siigetbyte(context, slave, a++) << 8); + } + if (p != cat) + { + a = 0; + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return a; +} + +/** Get string from SII string section in slave EEPROM. + * @param[in] context = context struct + * @param[out] str = requested string, 0x00 if not found + * @param[in] slave = slave number + * @param[in] Sn = string number + */ +void ecx_siistring(ecx_contextt *context, char *str, uint16 slave, uint16 Sn) +{ + uint16 a,i,j,l,n,ba; + char *ptr; + uint8 eectl = context->slavelist[slave].eep_pdi; + + ptr = str; + a = ecx_siifind (context, slave, ECT_SII_STRING); /* find string section */ + if (a > 0) + { + ba = a + 2; /* skip SII section header */ + n = ecx_siigetbyte(context, slave, ba++); /* read number of strings in section */ + if (Sn <= n) /* is req string available? */ + { + for (i = 1; i <= Sn; i++) /* walk through strings */ + { + l = ecx_siigetbyte(context, slave, ba++); /* length of this string */ + if (i < Sn) + { + ba += l; + } + else + { + ptr = str; + for (j = 1; j <= l; j++) /* copy one string */ + { + if(j <= EC_MAXNAME) + { + *ptr = (char)ecx_siigetbyte(context, slave, ba++); + ptr++; + } + else + { + ba++; + } + } + } + } + *ptr = 0; /* add zero terminator */ + } + else + { + ptr = str; + *ptr = 0; /* empty string */ + } + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } +} + +/** Get FMMU data from SII FMMU section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] FMMU = FMMU struct from SII, max. 4 FMMU's + * @return number of FMMU's defined in section + */ +uint16 ecx_siiFMMU(ecx_contextt *context, uint16 slave, ec_eepromFMMUt* FMMU) +{ + uint16 a; + uint8 eectl = context->slavelist[slave].eep_pdi; + + FMMU->nFMMU = 0; + FMMU->FMMU0 = 0; + FMMU->FMMU1 = 0; + FMMU->FMMU2 = 0; + FMMU->FMMU3 = 0; + FMMU->Startpos = ecx_siifind(context, slave, ECT_SII_FMMU); + + if (FMMU->Startpos > 0) + { + a = FMMU->Startpos; + FMMU->nFMMU = ecx_siigetbyte(context, slave, a++); + FMMU->nFMMU += (ecx_siigetbyte(context, slave, a++) << 8); + FMMU->nFMMU *= 2; + FMMU->FMMU0 = ecx_siigetbyte(context, slave, a++); + FMMU->FMMU1 = ecx_siigetbyte(context, slave, a++); + if (FMMU->nFMMU > 2) + { + FMMU->FMMU2 = ecx_siigetbyte(context, slave, a++); + FMMU->FMMU3 = ecx_siigetbyte(context, slave, a++); + } + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return FMMU->nFMMU; +} + +/** Get SM data from SII SM section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] SM = first SM struct from SII + * @return number of SM's defined in section + */ +uint16 ecx_siiSM(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM) +{ + uint16 a,w; + uint8 eectl = context->slavelist[slave].eep_pdi; + + SM->nSM = 0; + SM->Startpos = ecx_siifind(context, slave, ECT_SII_SM); + if (SM->Startpos > 0) + { + a = SM->Startpos; + w = ecx_siigetbyte(context, slave, a++); + w += (ecx_siigetbyte(context, slave, a++) << 8); + SM->nSM = (w / 4); + SM->PhStart = ecx_siigetbyte(context, slave, a++); + SM->PhStart += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Plength = ecx_siigetbyte(context, slave, a++); + SM->Plength += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Creg = ecx_siigetbyte(context, slave, a++); + SM->Sreg = ecx_siigetbyte(context, slave, a++); + SM->Activate = ecx_siigetbyte(context, slave, a++); + SM->PDIctrl = ecx_siigetbyte(context, slave, a++); + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return SM->nSM; +} + +/** Get next SM data from SII SM section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] SM = first SM struct from SII + * @param[in] n = SM number + * @return >0 if OK + */ +uint16 ecx_siiSMnext(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM, uint16 n) +{ + uint16 a; + uint16 retVal = 0; + uint8 eectl = context->slavelist[slave].eep_pdi; + + if (n < SM->nSM) + { + a = SM->Startpos + 2 + (n * 8); + SM->PhStart = ecx_siigetbyte(context, slave, a++); + SM->PhStart += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Plength = ecx_siigetbyte(context, slave, a++); + SM->Plength += (ecx_siigetbyte(context, slave, a++) << 8); + SM->Creg = ecx_siigetbyte(context, slave, a++); + SM->Sreg = ecx_siigetbyte(context, slave, a++); + SM->Activate = ecx_siigetbyte(context, slave, a++); + SM->PDIctrl = ecx_siigetbyte(context, slave, a++); + retVal = 1; + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return retVal; +} + +/** Get PDO data from SII PDO section in slave EEPROM. + * @param[in] context = context struct + * @param[in] slave = slave number + * @param[out] PDO = PDO struct from SII + * @param[in] t = 0=RXPDO 1=TXPDO + * @return mapping size in bits of PDO + */ +int ecx_siiPDO(ecx_contextt *context, uint16 slave, ec_eepromPDOt* PDO, uint8 t) +{ + uint16 a , w, c, e, er, Size; + uint8 eectl = context->slavelist[slave].eep_pdi; + + Size = 0; + PDO->nPDO = 0; + PDO->Length = 0; + PDO->Index[1] = 0; + for (c = 0 ; c < EC_MAXSM ; c++) PDO->SMbitsize[c] = 0; + if (t > 1) + t = 1; + PDO->Startpos = ecx_siifind(context, slave, ECT_SII_PDO + t); + if (PDO->Startpos > 0) + { + a = PDO->Startpos; + w = ecx_siigetbyte(context, slave, a++); + w += (ecx_siigetbyte(context, slave, a++) << 8); + PDO->Length = w; + c = 1; + /* traverse through all PDOs */ + do + { + PDO->nPDO++; + PDO->Index[PDO->nPDO] = ecx_siigetbyte(context, slave, a++); + PDO->Index[PDO->nPDO] += (ecx_siigetbyte(context, slave, a++) << 8); + PDO->BitSize[PDO->nPDO] = 0; + c++; + e = ecx_siigetbyte(context, slave, a++); + PDO->SyncM[PDO->nPDO] = ecx_siigetbyte(context, slave, a++); + a += 4; + c += 2; + if (PDO->SyncM[PDO->nPDO] < EC_MAXSM) /* active and in range SM? */ + { + /* read all entries defined in PDO */ + for (er = 1; er <= e; er++) + { + c += 4; + a += 5; + PDO->BitSize[PDO->nPDO] += ecx_siigetbyte(context, slave, a++); + a += 2; + } + PDO->SMbitsize[ PDO->SyncM[PDO->nPDO] ] += PDO->BitSize[PDO->nPDO]; + Size += PDO->BitSize[PDO->nPDO]; + c++; + } + else /* PDO deactivated because SM is 0xff or > EC_MAXSM */ + { + c += 4 * e; + a += 8 * e; + c++; + } + if (PDO->nPDO >= (EC_MAXEEPDO - 1)) + { + c = PDO->Length; /* limit number of PDO entries in buffer */ + } + } + while (c < PDO->Length); + } + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } + + return (Size); +} + +#define MAX_FPRD_MULTI 64 + +int ecx_FPRD_multi(ecx_contextt *context, int n, uint16 *configlst, ec_alstatust *slstatlst, int timeout) +{ + int wkc; + uint8 idx; + ecx_portt *port; + int sldatapos[MAX_FPRD_MULTI]; + int slcnt; + + port = context->port; + idx = ecx_getindex(port); + slcnt = 0; + ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, + *(configlst + slcnt), ECT_REG_ALSTAT, sizeof(ec_alstatust), slstatlst + slcnt); + sldatapos[slcnt] = EC_HEADERSIZE; + while(++slcnt < (n - 1)) + { + sldatapos[slcnt] = ecx_adddatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, TRUE, + *(configlst + slcnt), ECT_REG_ALSTAT, sizeof(ec_alstatust), slstatlst + slcnt); + } + if(slcnt < n) + { + sldatapos[slcnt] = ecx_adddatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, FALSE, + *(configlst + slcnt), ECT_REG_ALSTAT, sizeof(ec_alstatust), slstatlst + slcnt); + } + wkc = ecx_srconfirm(port, idx, timeout); + if (wkc >= 0) + { + for(slcnt = 0 ; slcnt < n ; slcnt++) + { + memcpy(slstatlst + slcnt, &(port->rxbuf[idx][sldatapos[slcnt]]), sizeof(ec_alstatust)); + } + } + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + return wkc; +} + +/** Read all slave states in ec_slave. + * @param[in] context = context struct + * @return lowest state found + */ +int ecx_readstate(ecx_contextt *context) +{ + uint16 slave, fslave, lslave, configadr, lowest, rval; + ec_alstatust sl[MAX_FPRD_MULTI]; + uint16 slca[MAX_FPRD_MULTI]; + + lowest = 0xff; + context->slavelist[0].ALstatuscode = 0; + fslave = 1; + do + { + lslave = *(context->slavecount); + if ((lslave - fslave) >= MAX_FPRD_MULTI) + { + lslave = fslave + MAX_FPRD_MULTI - 1; + } + for (slave = fslave; slave <= lslave; slave++) + { + const ec_alstatust zero = {0, 0, 0}; + + configadr = context->slavelist[slave].configadr; + slca[slave - fslave] = configadr; + sl[slave - fslave] = zero; + } + ecx_FPRD_multi(context, (lslave - fslave) + 1, &(slca[0]), &(sl[0]), EC_TIMEOUTRET3); + for (slave = fslave; slave <= lslave; slave++) + { + configadr = context->slavelist[slave].configadr; + rval = etohs(sl[slave - fslave].alstatus); + context->slavelist[slave].ALstatuscode = etohs(sl[slave - fslave].alstatuscode); + if (rval < lowest) + { + lowest = rval; + } + context->slavelist[slave].state = rval; + context->slavelist[0].ALstatuscode |= context->slavelist[slave].ALstatuscode; + } + fslave = lslave + 1; + } while(lslave < *(context->slavecount)); + context->slavelist[0].state = lowest; + + return lowest; +} + +/** Write slave state, if slave = 0 then write to all slaves. + * The function does not check if the actual state is changed. + * @param[in] context = context struct + * @param[in] slave = Slave number, 0 = master + * @return 0 + */ +int ecx_writestate(ecx_contextt *context, uint16 slave) +{ + uint16 configadr, slstate; + + if (slave == 0) + { + slstate = htoes(context->slavelist[slave].state); + ecx_BWR(context->port, 0, ECT_REG_ALCTL, sizeof(slstate), &slstate, EC_TIMEOUTRET3); /* write slave status */ + } + else + { + configadr = context->slavelist[slave].configadr; + + ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(context->slavelist[slave].state), EC_TIMEOUTRET3); /* write slave status */ + } + return 0; +} + +/** Check actual slave state. + * This is a blocking function. + * @param[in] context = context struct + * @param[in] slave = Slave number, 0 = all slaves + * @param[in] reqstate = Requested state + * @param[in] timeout = Timout value in us + * @return Requested state, or found state after timeout. + */ +uint16 ecx_statecheck(ecx_contextt *context, uint16 slave, uint16 reqstate, int timeout) +{ + uint16 configadr, state, rval; + ec_alstatust slstat; + osal_timert timer; + + if ( slave > *(context->slavecount) ) + { + return 0; + } + osal_timer_start(&timer, timeout); + configadr = context->slavelist[slave].configadr; + do + { + if (slave < 1) + { + rval = 0; + ecx_BRD(context->port, 0, ECT_REG_ALSTAT, sizeof(rval), &rval , EC_TIMEOUTRET); + rval = etohs(rval); + } + else + { + slstat.alstatus = 0; + slstat.alstatuscode = 0; + ecx_FPRD(context->port, configadr, ECT_REG_ALSTAT, sizeof(slstat), &slstat, EC_TIMEOUTRET); + rval = etohs(slstat.alstatus); + context->slavelist[slave].ALstatuscode = etohs(slstat.alstatuscode); + } + state = rval & 0x000f; /* read slave status */ + if (state != reqstate) + { + osal_usleep(1000); + } + } + while ((state != reqstate) && (osal_timer_is_expired(&timer) == FALSE)); + context->slavelist[slave].state = rval; + + return state; +} + +/** Get index of next mailbox counter value. + * Used for Mailbox Link Layer. + * @param[in] cnt = Mailbox counter value [0..7] + * @return next mailbox counter value + */ +uint8 ec_nextmbxcnt(uint8 cnt) +{ + cnt++; + if (cnt > 7) + { + cnt = 1; /* wrap around to 1, not 0 */ + } + + return cnt; +} + +/** Clear mailbox buffer. + * @param[out] Mbx = Mailbox buffer to clear + */ +void ec_clearmbx(ec_mbxbuft *Mbx) +{ + memset(Mbx, 0x00, EC_MAXMBX); +} + +/** Check if IN mailbox of slave is empty. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] timeout = Timeout in us + * @return >0 is success + */ +int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout) +{ + uint16 configadr; + uint8 SMstat; + int wkc; + osal_timert timer; + + osal_timer_start(&timer, timeout); + configadr = context->slavelist[slave].configadr; + do + { + SMstat = 0; + wkc = ecx_FPRD(context->port, configadr, ECT_REG_SM0STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + if (((SMstat & 0x08) != 0) && (timeout > EC_LOCALDELAY)) + { + osal_usleep(EC_LOCALDELAY); + } + } + while (((wkc <= 0) || ((SMstat & 0x08) != 0)) && (osal_timer_is_expired(&timer) == FALSE)); + + if ((wkc > 0) && ((SMstat & 0x08) == 0)) + { + return 1; + } + + return 0; +} + +/** Write IN mailbox to slave. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] mbx = Mailbox data + * @param[in] timeout = Timeout in us + * @return Work counter (>0 is success) + */ +int ecx_mbxsend(ecx_contextt *context, uint16 slave,ec_mbxbuft *mbx, int timeout) +{ + uint16 mbxwo,mbxl,configadr; + int wkc; + + wkc = 0; + configadr = context->slavelist[slave].configadr; + mbxl = context->slavelist[slave].mbx_l; + if ((mbxl > 0) && (mbxl <= EC_MAXMBX)) + { + if (ecx_mbxempty(context, slave, timeout)) + { + mbxwo = context->slavelist[slave].mbx_wo; + /* write slave in mailbox */ + wkc = ecx_FPWR(context->port, configadr, mbxwo, mbxl, mbx, EC_TIMEOUTRET3); + } + else + { + wkc = 0; + } + } + + return wkc; +} + +/** Read OUT mailbox from slave. + * Supports Mailbox Link Layer with repeat requests. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] mbx = Mailbox data + * @param[in] timeout = Timeout in us + * @return Work counter (>0 is success) + */ +int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout) +{ + uint16 mbxro,mbxl,configadr; + int wkc=0; + int wkc2; + uint16 SMstat; + uint8 SMcontr; + ec_mbxheadert *mbxh; + ec_emcyt *EMp; + ec_mbxerrort *MBXEp; + + configadr = context->slavelist[slave].configadr; + mbxl = context->slavelist[slave].mbx_rl; + if ((mbxl > 0) && (mbxl <= EC_MAXMBX)) + { + osal_timert timer; + + osal_timer_start(&timer, timeout); + wkc = 0; + do /* wait for read mailbox available */ + { + SMstat = 0; + wkc = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY)) + { + osal_usleep(EC_LOCALDELAY); + } + } + while (((wkc <= 0) || ((SMstat & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE)); + + if ((wkc > 0) && ((SMstat & 0x08) > 0)) /* read mailbox available ? */ + { + mbxro = context->slavelist[slave].mbx_ro; + mbxh = (ec_mbxheadert *)mbx; + do + { + wkc = ecx_FPRD(context->port, configadr, mbxro, mbxl, mbx, EC_TIMEOUTRET); /* get mailbox */ + if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == 0x00)) /* Mailbox error response? */ + { + MBXEp = (ec_mbxerrort *)mbx; + ecx_mbxerror(context, slave, etohs(MBXEp->Detail)); + wkc = 0; /* prevent emergency to cascade up, it is already handled. */ + } + else if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == 0x03)) /* CoE response? */ + { + EMp = (ec_emcyt *)mbx; + if ((etohs(EMp->CANOpen) >> 12) == 0x01) /* Emergency request? */ + { + ecx_mbxemergencyerror(context, slave, etohs(EMp->ErrorCode), EMp->ErrorReg, + EMp->bData, etohs(EMp->w1), etohs(EMp->w2)); + wkc = 0; /* prevent emergency to cascade up, it is already handled. */ + } + } + else + { + if (wkc <= 0) /* read mailbox lost */ + { + SMstat ^= 0x0200; /* toggle repeat request */ + SMstat = htoes(SMstat); + wkc2 = ecx_FPWR(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + do /* wait for toggle ack */ + { + wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1CONTR, sizeof(SMcontr), &SMcontr, EC_TIMEOUTRET); + } while (((wkc2 <= 0) || ((SMcontr & 0x02) != (HI_BYTE(SMstat) & 0x02))) && (osal_timer_is_expired(&timer) == FALSE)); + do /* wait for read mailbox available */ + { + wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET); + SMstat = etohs(SMstat); + if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY)) + { + osal_usleep(EC_LOCALDELAY); + } + } while (((wkc2 <= 0) || ((SMstat & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE)); + } + } + } while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE)); /* if WKC<=0 repeat */ + } + else /* no read mailbox available */ + { + wkc = 0; + } + } + + return wkc; +} + +/** Dump complete EEPROM data from slave in buffer. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] esibuf = EEPROM data buffer, make sure it is big enough. + */ +void ecx_esidump(ecx_contextt *context, uint16 slave, uint8 *esibuf) +{ + int address, incr; + uint16 configadr; + uint64 *p64; + uint16 *p16; + uint64 edat; + uint8 eectl = context->slavelist[slave].eep_pdi; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + address = ECT_SII_START; + p16=(uint16*)esibuf; + if (context->slavelist[slave].eep_8byte) + { + incr = 4; + } + else + { + incr = 2; + } + do + { + edat = ecx_readeepromFP(context, configadr, address, EC_TIMEOUTEEP); + p64 = (uint64*)p16; + *p64 = edat; + p16 += incr; + address += incr; + } while ((address <= (EC_MAXEEPBUF >> 1)) && ((uint32)edat != 0xffffffff)); + + if (eectl) + { + ecx_eeprom2pdi(context, slave); /* if eeprom control was previously pdi then restore */ + } +} + +/** Read EEPROM from slave bypassing cache. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 32bit + */ +uint32 ecx_readeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, int timeout) +{ + uint16 configadr; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + + return ((uint32)ecx_readeepromFP(context, configadr, eeproma, timeout)); +} + +/** Write EEPROM to slave bypassing cache. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + */ +int ecx_writeeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, uint16 data, int timeout) +{ + uint16 configadr; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + return (ecx_writeeepromFP(context, configadr, eeproma, data, timeout)); +} + +/** Set eeprom control to master. Only if set to PDI. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @return >0 if OK + */ +int ecx_eeprom2master(ecx_contextt *context, uint16 slave) +{ + int wkc = 1, cnt = 0; + uint16 configadr; + uint8 eepctl; + + if ( context->slavelist[slave].eep_pdi ) + { + configadr = context->slavelist[slave].configadr; + eepctl = 2; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* force Eeprom from PDI */ + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + eepctl = 0; + cnt = 0; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to master */ + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + context->slavelist[slave].eep_pdi = 0; + } + + return wkc; +} + +/** Set eeprom control to PDI. Only if set to master. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @return >0 if OK + */ +int ecx_eeprom2pdi(ecx_contextt *context, uint16 slave) +{ + int wkc = 1, cnt = 0; + uint16 configadr; + uint8 eepctl; + + if ( !context->slavelist[slave].eep_pdi ) + { + configadr = context->slavelist[slave].configadr; + eepctl = 1; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl , EC_TIMEOUTRET); /* set Eeprom to PDI */ + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + context->slavelist[slave].eep_pdi = 1; + } + + return wkc; +} + +uint16 ecx_eeprom_waitnotbusyAP(ecx_contextt *context, uint16 aiadr,uint16 *estat, int timeout) +{ + int wkc, cnt = 0, retval = 0; + osal_timert timer; + + osal_timer_start(&timer, timeout); + do + { + if (cnt++) + { + osal_usleep(EC_LOCALDELAY); + } + *estat = 0; + wkc=ecx_APRD(context->port, aiadr, ECT_REG_EEPSTAT, sizeof(*estat), estat, EC_TIMEOUTRET); + *estat = etohs(*estat); + } + while (((wkc <= 0) || ((*estat & EC_ESTAT_BUSY) > 0)) && (osal_timer_is_expired(&timer) == FALSE)); /* wait for eeprom ready */ + if ((*estat & EC_ESTAT_BUSY) == 0) + { + retval = 1; + } + + return retval; +} + +/** Read EEPROM from slave bypassing cache. APRD method. + * @param[in] context = context struct + * @param[in] aiadr = auto increment address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 64bit or 32bit + */ +uint64 ecx_readeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, int timeout) +{ + uint16 estat; + uint32 edat32; + uint64 edat64; + ec_eepromt ed; + int wkc, cnt, nackcnt = 0; + + edat64 = 0; + edat32 = 0; + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + + do + { + ed.comm = htoes(EC_ECMD_READ); + ed.addr = htoes(eeproma); + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + if (estat & EC_ESTAT_R64) + { + cnt = 0; + do + { + wkc = ecx_APRD(context->port, aiadr, ECT_REG_EEPDAT, sizeof(edat64), &edat64, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } + else + { + cnt = 0; + do + { + wkc = ecx_APRD(context->port, aiadr, ECT_REG_EEPDAT, sizeof(edat32), &edat32, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + edat64=(uint64)edat32; + } + } + } + } + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return edat64; +} + +/** Write EEPROM to slave bypassing cache. APWR method. + * @param[in] context = context struct + * @param[in] aiadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + */ +int ecx_writeeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, uint16 data, int timeout) +{ + uint16 estat; + ec_eepromt ed; + int wkc, rval = 0, cnt = 0, nackcnt = 0; + + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + do + { + cnt = 0; + do + { + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPDAT, sizeof(data), &data, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + + ed.comm = EC_ECMD_WRITE; + ed.addr = eeproma; + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc = ecx_APWR(context->port, aiadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY * 2); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyAP(context, aiadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + rval = 1; + } + } + } + + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return rval; +} + +uint16 ecx_eeprom_waitnotbusyFP(ecx_contextt *context, uint16 configadr,uint16 *estat, int timeout) +{ + int wkc, cnt = 0, retval = 0; + osal_timert timer; + + osal_timer_start(&timer, timeout); + do + { + if (cnt++) + { + osal_usleep(EC_LOCALDELAY); + } + *estat = 0; + wkc=ecx_FPRD(context->port, configadr, ECT_REG_EEPSTAT, sizeof(*estat), estat, EC_TIMEOUTRET); + *estat = etohs(*estat); + } + while (((wkc <= 0) || ((*estat & EC_ESTAT_BUSY) > 0)) && (osal_timer_is_expired(&timer) == FALSE)); /* wait for eeprom ready */ + if ((*estat & EC_ESTAT_BUSY) == 0) + { + retval = 1; + } + + return retval; +} + +/** Read EEPROM from slave bypassing cache. FPRD method. + * @param[in] context = context struct + * @param[in] configadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] timeout = Timeout in us. + * @return EEPROM data 64bit or 32bit + */ +uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, int timeout) +{ + uint16 estat; + uint32 edat32; + uint64 edat64; + ec_eepromt ed; + int wkc, cnt, nackcnt = 0; + + edat64 = 0; + edat32 = 0; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc=ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + + do + { + ed.comm = htoes(EC_ECMD_READ); + ed.addr = htoes(eeproma); + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc=ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + if (estat & EC_ESTAT_R64) + { + cnt = 0; + do + { + wkc=ecx_FPRD(context->port, configadr, ECT_REG_EEPDAT, sizeof(edat64), &edat64, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } + else + { + cnt = 0; + do + { + wkc=ecx_FPRD(context->port, configadr, ECT_REG_EEPDAT, sizeof(edat32), &edat32, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + edat64=(uint64)edat32; + } + } + } + } + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return edat64; +} + +/** Write EEPROM to slave bypassing cache. FPWR method. + * @param[in] context = context struct + * @param[in] configadr = configured address of slave + * @param[in] eeproma = (WORD) Address in the EEPROM + * @param[in] data = 16bit data + * @param[in] timeout = Timeout in us. + * @return >0 if OK + */ +int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout) +{ + uint16 estat; + ec_eepromt ed; + int wkc, rval = 0, cnt = 0, nackcnt = 0; + + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + do + { + cnt = 0; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPDAT, sizeof(data), &data, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + ed.comm = EC_ECMD_WRITE; + ed.addr = eeproma; + ed.d2 = 0x0000; + cnt = 0; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + if (wkc) + { + osal_usleep(EC_LOCALDELAY * 2); + estat = 0x0000; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + if (estat & EC_ESTAT_NACK) + { + nackcnt++; + osal_usleep(EC_LOCALDELAY * 5); + } + else + { + nackcnt = 0; + rval = 1; + } + } + } + } + while ((nackcnt > 0) && (nackcnt < 3)); + } + + return rval; +} + +/** Read EEPROM from slave bypassing cache. + * Parallel read step 1, make request to slave. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] eeproma = (WORD) Address in the EEPROM + */ +void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma) +{ + uint16 configadr, estat; + ec_eepromt ed; + int wkc, cnt = 0; + + ecx_eeprom2master(context, slave); /* set eeprom control to master */ + configadr = context->slavelist[slave].configadr; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, EC_TIMEOUTEEP)) + { + if (estat & EC_ESTAT_EMASK) /* error bits are set */ + { + estat = htoes(EC_ECMD_NOP); /* clear error bits */ + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET3); + } + ed.comm = htoes(EC_ECMD_READ); + ed.addr = htoes(eeproma); + ed.d2 = 0x0000; + do + { + wkc = ecx_FPWR(context->port, configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } +} + +/** Read EEPROM from slave bypassing cache. + * Parallel read step 2, actual read from slave. + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] timeout = Timeout in us. + * @return EEPROM data 32bit + */ +uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout) +{ + uint16 estat, configadr; + uint32 edat; + int wkc, cnt = 0; + + configadr = context->slavelist[slave].configadr; + edat = 0; + estat = 0x0000; + if (ecx_eeprom_waitnotbusyFP(context, configadr, &estat, timeout)) + { + do + { + wkc = ecx_FPRD(context->port, configadr, ECT_REG_EEPDAT, sizeof(edat), &edat, EC_TIMEOUTRET); + } + while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES)); + } + + return edat; +} + +/** Push index of segmented LRD/LWR/LRW combination. + * @param[in] context = context struct + * @param[in] idx = Used datagram index. + * @param[in] data = Pointer to process data segment. + * @param[in] length = Length of data segment in bytes. + */ +static void ecx_pushindex(ecx_contextt *context, uint8 idx, void *data, uint16 length) +{ + if(context->idxstack->pushed < EC_MAXBUF) + { + context->idxstack->idx[context->idxstack->pushed] = idx; + context->idxstack->data[context->idxstack->pushed] = data; + context->idxstack->length[context->idxstack->pushed] = length; + context->idxstack->pushed++; + } +} + +/** Pull index of segmented LRD/LWR/LRW combination. + * @param[in] context = context struct + * @return Stack location, -1 if stack is empty. + */ +static int ecx_pullindex(ecx_contextt *context) +{ + int rval = -1; + if(context->idxstack->pulled < context->idxstack->pushed) + { + rval = context->idxstack->pulled; + context->idxstack->pulled++; + } + + return rval; +} + +/** Transmit processdata to slaves. + * Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW). + * Both the input and output processdata are transmitted. + * The outputs with the actual data, the inputs have a placeholder. + * The inputs are gathered with the receive processdata function. + * In contrast to the base LRW function this function is non-blocking. + * If the processdata does not fit in one datagram, multiple are used. + * In order to recombine the slave response, a stack is used. + * @param[in] context = context struct + * @param[in] group = group number + * @return >0 if processdata is transmitted. + */ +int ecx_send_processdata_group(ecx_contextt *context, uint8 group) +{ + uint32 LogAdr; + uint16 w1, w2; + int length, sublength; + uint8 idx; + int wkc; + uint8* data; + boolean first=FALSE; + uint16 currentsegment = 0; + + wkc = 0; + if(context->grouplist[group].hasdc) + { + first = TRUE; + } + length = context->grouplist[group].Obytes + context->grouplist[group].Ibytes; + LogAdr = context->grouplist[group].logstartaddr; + if (length) + { + if(!group) + { + context->idxstack->pushed = 0; + context->idxstack->pulled = 0; + } + wkc = 1; + /* LRW blocked by one or more slaves ? */ + if (context->grouplist[group].blockLRW) + { + /* if inputs available generate LRD */ + if(context->grouplist[group].Ibytes) + { + currentsegment = context->grouplist[group].Isegment; + data = context->grouplist[group].inputs; + length = context->grouplist[group].Ibytes; + LogAdr += context->grouplist[group].Obytes; + /* segment transfer if needed */ + do + { + if(currentsegment == context->grouplist[group].Isegment) + { + sublength = context->grouplist[group].IOsegment[currentsegment++] - context->grouplist[group].Ioffset; + } + else + { + sublength = context->grouplist[group].IOsegment[currentsegment++]; + } + /* get new index */ + idx = ecx_getindex(context->port); + w1 = LO_WORD(LogAdr); + w2 = HI_WORD(LogAdr); + ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRD, idx, w1, w2, sublength, data); + if(first) + { + context->DCl = sublength; + /* FPRMW in second datagram */ + context->DCtO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, + context->slavelist[context->grouplist[group].DCnext].configadr, + ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); + first = FALSE; + } + /* send frame */ + ecx_outframe_red(context->port, idx); + /* push index and data pointer on stack */ + ecx_pushindex(context, idx, data, sublength); + length -= sublength; + LogAdr += sublength; + data += sublength; + } while (length && (currentsegment < context->grouplist[group].nsegments)); + } + /* if outputs available generate LWR */ + if(context->grouplist[group].Obytes) + { + data = context->grouplist[group].outputs; + length = context->grouplist[group].Obytes; + LogAdr = context->grouplist[group].logstartaddr; + currentsegment = 0; + /* segment transfer if needed */ + do + { + sublength = context->grouplist[group].IOsegment[currentsegment++]; + if((length - sublength) < 0) + { + sublength = length; + } + /* get new index */ + idx = ecx_getindex(context->port); + w1 = LO_WORD(LogAdr); + w2 = HI_WORD(LogAdr); + ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LWR, idx, w1, w2, sublength, data); + if(first) + { + context->DCl = sublength; + /* FPRMW in second datagram */ + context->DCtO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, + context->slavelist[context->grouplist[group].DCnext].configadr, + ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); + first = FALSE; + } + /* send frame */ + ecx_outframe_red(context->port, idx); + /* push index and data pointer on stack */ + ecx_pushindex(context, idx, data, sublength); + length -= sublength; + LogAdr += sublength; + data += sublength; + } while (length && (currentsegment < context->grouplist[group].nsegments)); + } + } + /* LRW can be used */ + else + { + if (context->grouplist[group].Obytes) + { + data = context->grouplist[group].outputs; + } + else + { + data = context->grouplist[group].inputs; + } + /* segment transfer if needed */ + do + { + sublength = context->grouplist[group].IOsegment[currentsegment++]; + /* get new index */ + idx = ecx_getindex(context->port); + w1 = LO_WORD(LogAdr); + w2 = HI_WORD(LogAdr); + ecx_setupdatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_LRW, idx, w1, w2, sublength, data); + if(first) + { + context->DCl = sublength; + /* FPRMW in second datagram */ + context->DCtO = ecx_adddatagram(context->port, &(context->port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, + context->slavelist[context->grouplist[group].DCnext].configadr, + ECT_REG_DCSYSTIME, sizeof(int64), context->DCtime); + first = FALSE; + } + /* send frame */ + ecx_outframe_red(context->port, idx); + /* push index and data pointer on stack */ + ecx_pushindex(context, idx, data, sublength); + length -= sublength; + LogAdr += sublength; + data += sublength; + } while (length && (currentsegment < context->grouplist[group].nsegments)); + } + } + + return wkc; +} + +/** Receive processdata from slaves. + * Second part from ec_send_processdata(). + * Received datagrams are recombined with the processdata with help from the stack. + * If a datagram contains input processdata it copies it to the processdata structure. + * @param[in] context = context struct + * @param[in] group = group number + * @param[in] timeout = Timeout in us. + * @return Work counter. + */ +int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout) +{ + int pos, idx; + int wkc = 0, wkc2; + uint16 le_wkc = 0; + int64 le_DCtime; + boolean first = FALSE; + + if(context->grouplist[group].hasdc) + { + first = TRUE; + } + /* get first index */ + pos = ecx_pullindex(context); + /* read the same number of frames as send */ + while (pos >= 0) + { + idx = context->idxstack->idx[pos]; + wkc2 = ecx_waitinframe(context->port, context->idxstack->idx[pos], timeout); + /* check if there is input data in frame */ + if (wkc2 > EC_NOFRAME) + { + if((context->port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD) || (context->port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRW)) + { + if(first) + { + memcpy(context->idxstack->data[pos], &(context->port->rxbuf[idx][EC_HEADERSIZE]), context->DCl); + memcpy(&le_wkc, &(context->port->rxbuf[idx][EC_HEADERSIZE + context->DCl]), EC_WKCSIZE); + wkc = etohs(le_wkc); + memcpy(&le_DCtime, &(context->port->rxbuf[idx][context->DCtO]), sizeof(le_DCtime)); + *(context->DCtime) = etohll(le_DCtime); + first = FALSE; + } + else + { + /* copy input data back to process data buffer */ + memcpy(context->idxstack->data[pos], &(context->port->rxbuf[idx][EC_HEADERSIZE]), context->idxstack->length[pos]); + wkc += wkc2; + } + } + else if(context->port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LWR) + { + if(first) + { + memcpy(&le_wkc, &(context->port->rxbuf[idx][EC_HEADERSIZE + context->DCl]), EC_WKCSIZE); + /* output WKC counts 2 times when using LRW, emulate the same for LWR */ + wkc = etohs(le_wkc) * 2; + memcpy(&le_DCtime, &(context->port->rxbuf[idx][context->DCtO]), sizeof(le_DCtime)); + *(context->DCtime) = etohll(le_DCtime); + first = FALSE; + } + else + { + /* output WKC counts 2 times when using LRW, emulate the same for LWR */ + wkc += wkc2 * 2; + } + } + } + /* release buffer */ + ecx_setbufstat(context->port, idx, EC_BUF_EMPTY); + /* get next index */ + pos = ecx_pullindex(context); + } + + return wkc; +} + + +int ecx_send_processdata(ecx_contextt *context) +{ + return ecx_send_processdata_group(context, 0); +} + +int ecx_receive_processdata(ecx_contextt *context, int timeout) +{ + return ecx_receive_processdata_group(context, 0, timeout); +} + +#ifdef EC_VER1 +void ec_pusherror(const ec_errort *Ec) +{ + ecx_pusherror(&ecx_context, Ec); +} + +boolean ec_poperror(ec_errort *Ec) +{ + return ecx_poperror(&ecx_context, Ec); +} + +boolean ec_iserror(void) +{ + return ecx_iserror(&ecx_context); +} + +void ec_packeterror(uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode) +{ + ecx_packeterror(&ecx_context, Slave, Index, SubIdx, ErrorCode); +} + +int ec_init(char * ifname) +{ + return ecx_init(&ecx_context, ifname); +} + +int ec_init_redundant(char *ifname, char *if2name) +{ + return ecx_init_redundant (&ecx_context, &ecx_redport, ifname, if2name); +} + +void ec_close(void) +{ + ecx_close(&ecx_context); +}; + +uint8 ec_siigetbyte(uint16 slave, uint16 address) +{ + return ecx_siigetbyte (&ecx_context, slave, address); +} + +int16 ec_siifind(uint16 slave, uint16 cat) +{ + return ecx_siifind (&ecx_context, slave, cat); +} + +void ec_siistring(char *str, uint16 slave, uint16 Sn) +{ + ecx_siistring(&ecx_context, str, slave, Sn); +} + +uint16 ec_siiFMMU(uint16 slave, ec_eepromFMMUt* FMMU) +{ + return ecx_siiFMMU (&ecx_context, slave, FMMU); +} + +uint16 ec_siiSM(uint16 slave, ec_eepromSMt* SM) +{ + return ecx_siiSM (&ecx_context, slave, SM); +} + +uint16 ec_siiSMnext(uint16 slave, ec_eepromSMt* SM, uint16 n) +{ + return ecx_siiSMnext (&ecx_context, slave, SM, n); +} + +int ec_siiPDO(uint16 slave, ec_eepromPDOt* PDO, uint8 t) +{ + return ecx_siiPDO (&ecx_context, slave, PDO, t); +} + +int ec_readstate(void) +{ + return ecx_readstate (&ecx_context); +} + +int ec_writestate(uint16 slave) +{ + return ecx_writestate(&ecx_context, slave); +} + +uint16 ec_statecheck(uint16 slave, uint16 reqstate, int timeout) +{ + return ecx_statecheck (&ecx_context, slave, reqstate, timeout); +} + +int ec_mbxempty(uint16 slave, int timeout) +{ + return ecx_mbxempty (&ecx_context, slave, timeout); +} + +int ec_mbxsend(uint16 slave,ec_mbxbuft *mbx, int timeout) +{ + return ecx_mbxsend (&ecx_context, slave, mbx, timeout); +} + +int ec_mbxreceive(uint16 slave, ec_mbxbuft *mbx, int timeout) +{ + return ecx_mbxreceive (&ecx_context, slave, mbx, timeout); +} + +void ec_esidump(uint16 slave, uint8 *esibuf) +{ + ecx_esidump (&ecx_context, slave, esibuf); +} + +uint32 ec_readeeprom(uint16 slave, uint16 eeproma, int timeout) +{ + return ecx_readeeprom (&ecx_context, slave, eeproma, timeout); +} + +int ec_writeeeprom(uint16 slave, uint16 eeproma, uint16 data, int timeout) +{ + return ecx_writeeeprom (&ecx_context, slave, eeproma, data, timeout); +} + +int ec_eeprom2master(uint16 slave) +{ + return ecx_eeprom2master(&ecx_context, slave); +} + +int ec_eeprom2pdi(uint16 slave) +{ + return ecx_eeprom2pdi(&ecx_context, slave); +} + +uint16 ec_eeprom_waitnotbusyAP(uint16 aiadr,uint16 *estat, int timeout) +{ + return ecx_eeprom_waitnotbusyAP (&ecx_context, aiadr, estat, timeout); +} + +uint64 ec_readeepromAP(uint16 aiadr, uint16 eeproma, int timeout) +{ + return ecx_readeepromAP (&ecx_context, aiadr, eeproma, timeout); +} + +int ec_writeeepromAP(uint16 aiadr, uint16 eeproma, uint16 data, int timeout) +{ + return ecx_writeeepromAP (&ecx_context, aiadr, eeproma, data, timeout); +} + +uint16 ec_eeprom_waitnotbusyFP(uint16 configadr,uint16 *estat, int timeout) +{ + return ecx_eeprom_waitnotbusyFP (&ecx_context, configadr, estat, timeout); +} + +uint64 ec_readeepromFP(uint16 configadr, uint16 eeproma, int timeout) +{ + return ecx_readeepromFP (&ecx_context, configadr, eeproma, timeout); +} + +int ec_writeeepromFP(uint16 configadr, uint16 eeproma, uint16 data, int timeout) +{ + return ecx_writeeepromFP (&ecx_context, configadr, eeproma, data, timeout); +} + +void ec_readeeprom1(uint16 slave, uint16 eeproma) +{ + ecx_readeeprom1 (&ecx_context, slave, eeproma); +} + +uint32 ec_readeeprom2(uint16 slave, int timeout) +{ + return ecx_readeeprom2 (&ecx_context, slave, timeout); +} + +int ec_send_processdata_group(uint8 group) +{ + return ecx_send_processdata_group (&ecx_context, group); +} + +int ec_receive_processdata_group(uint8 group, int timeout) +{ + return ecx_receive_processdata_group (&ecx_context, group, timeout); +} + +int ec_send_processdata(void) +{ + return ec_send_processdata_group(0); +} + +int ec_receive_processdata(int timeout) +{ + return ec_receive_processdata_group(0, timeout); +} +#endif diff --git a/src/ethercatmain.h b/src/ethercatmain.h new file mode 100644 index 0000000..087d37e --- /dev/null +++ b/src/ethercatmain.h @@ -0,0 +1,553 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatmain.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatmain.c + */ + +#ifndef _ethercatmain_ +#define _ethercatmain_ + + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** max. etries in EtherCAT error list */ +#define EC_MAXELIST 64 +/** max. length of readable name in slavelist and Object Description List */ +#define EC_MAXNAME 40 +/** max. number of slaves in array */ +#define EC_MAXSLAVE 200 +/** max. number of groups */ +#define EC_MAXGROUP 2 +/** max. number of IO segments per group */ +#define EC_MAXIOSEGMENTS 64 +/** max. mailbox size */ +#define EC_MAXMBX 1486 +/** max. eeprom PDO entries */ +#define EC_MAXEEPDO 0x200 +/** max. SM used */ +#define EC_MAXSM 8 +/** max. FMMU used */ +#define EC_MAXFMMU 4 +/** max. Adapter */ +#define EC_MAXLEN_ADAPTERNAME 128 + +typedef struct ec_adapter ec_adaptert; +struct ec_adapter +{ + char name[EC_MAXLEN_ADAPTERNAME]; + char desc[EC_MAXLEN_ADAPTERNAME]; + ec_adaptert *next; +}; + +/** record for FMMU */ +PACKED_BEGIN +typedef struct PACKED +{ + uint32 LogStart; + uint16 LogLength; + uint8 LogStartbit; + uint8 LogEndbit; + uint16 PhysStart; + uint8 PhysStartBit; + uint8 FMMUtype; + uint8 FMMUactive; + uint8 unused1; + uint16 unused2; +} ec_fmmut; +PACKED_END + +/** record for sync manager */ +PACKED_BEGIN +typedef struct PACKED +{ + uint16 StartAddr; + uint16 SMlength; + uint32 SMflags; +} ec_smt; +PACKED_END + +PACKED_BEGIN +typedef struct PACKED +{ + uint16 State; + uint16 Unused; + uint16 ALstatuscode; +} ec_state_status; +PACKED_END + +#define ECT_MBXPROT_AOE 0x0001 +#define ECT_MBXPROT_EOE 0x0002 +#define ECT_MBXPROT_COE 0x0004 +#define ECT_MBXPROT_FOE 0x0008 +#define ECT_MBXPROT_SOE 0x0010 +#define ECT_MBXPROT_VOE 0x0020 + +#define ECT_COEDET_SDO 0x01 +#define ECT_COEDET_SDOINFO 0x02 +#define ECT_COEDET_PDOASSIGN 0x04 +#define ECT_COEDET_PDOCONFIG 0x08 +#define ECT_COEDET_UPLOAD 0x10 +#define ECT_COEDET_SDOCA 0x20 + +#define EC_SMENABLEMASK 0xfffeffff + +/** for list of ethercat slaves detected */ +typedef struct +{ + /** state of slave */ + uint16 state; + /** AL status code */ + uint16 ALstatuscode; + /** Configured address */ + uint16 configadr; + /** Alias address */ + uint16 aliasadr; + /** Manufacturer from EEprom */ + uint32 eep_man; + /** ID from EEprom */ + uint32 eep_id; + /** revision from EEprom */ + uint32 eep_rev; + /** Interface type */ + uint16 Itype; + /** Device type */ + uint16 Dtype; + /** output bits */ + uint16 Obits; + /** output bytes, if Obits < 8 then Obytes = 0 */ + uint32 Obytes; + /** output pointer in IOmap buffer */ + uint8 *outputs; + /** startbit in first output byte */ + uint8 Ostartbit; + /** input bits */ + uint16 Ibits; + /** input bytes, if Ibits < 8 then Ibytes = 0 */ + uint32 Ibytes; + /** input pointer in IOmap buffer */ + uint8 *inputs; + /** startbit in first input byte */ + uint8 Istartbit; + /** SM structure */ + ec_smt SM[EC_MAXSM]; + /** SM type 0=unused 1=MbxWr 2=MbxRd 3=Outputs 4=Inputs */ + uint8 SMtype[EC_MAXSM]; + /** FMMU structure */ + ec_fmmut FMMU[EC_MAXFMMU]; + /** FMMU0 function */ + uint8 FMMU0func; + /** FMMU1 function */ + uint8 FMMU1func; + /** FMMU2 function */ + uint8 FMMU2func; + /** FMMU3 function */ + uint8 FMMU3func; + /** length of write mailbox in bytes, if no mailbox then 0 */ + uint16 mbx_l; + /** mailbox write offset */ + uint16 mbx_wo; + /** length of read mailbox in bytes */ + uint16 mbx_rl; + /** mailbox read offset */ + uint16 mbx_ro; + /** mailbox supported protocols */ + uint16 mbx_proto; + /** Counter value of mailbox link layer protocol 1..7 */ + uint8 mbx_cnt; + /** has DC capabillity */ + boolean hasdc; + /** Physical type; Ebus, EtherNet combinations */ + uint8 ptype; + /** topology: 1 to 3 links */ + uint8 topology; + /** active ports bitmap : ....3210 , set if respective port is active **/ + uint8 activeports; + /** consumed ports bitmap : ....3210, used for internal delay measurement **/ + uint8 consumedports; + /** slave number for parent, 0=master */ + uint16 parent; + /** port number on parent this slave is connected to **/ + uint8 parentport; + /** port number on this slave the parent is connected to **/ + uint8 entryport; + /** DC receivetimes on port A */ + int32 DCrtA; + /** DC receivetimes on port B */ + int32 DCrtB; + /** DC receivetimes on port C */ + int32 DCrtC; + /** DC receivetimes on port D */ + int32 DCrtD; + /** propagation delay */ + int32 pdelay; + /** next DC slave */ + uint16 DCnext; + /** previous DC slave */ + uint16 DCprevious; + /** DC cyle time in ns */ + int32 DCcycle; + /** DC shift from clock modulus boundary */ + int32 DCshift; + /** DC sync activation, 0=off, 1=on */ + uint8 DCactive; + /** link to config table */ + uint16 configindex; + /** link to SII config */ + uint16 SIIindex; + /** 1 = 8 bytes per read, 0 = 4 bytes per read */ + uint8 eep_8byte; + /** 0 = eeprom to master , 1 = eeprom to PDI */ + uint8 eep_pdi; + /** CoE details */ + uint8 CoEdetails; + /** FoE details */ + uint8 FoEdetails; + /** EoE details */ + uint8 EoEdetails; + /** SoE details */ + uint8 SoEdetails; + /** E-bus current */ + int16 Ebuscurrent; + /** if >0 block use of LRW in processdata */ + uint8 blockLRW; + /** group */ + uint8 group; + /** first unused FMMU */ + uint8 FMMUunused; + /** TRUE is slave is not responding at all */ + boolean islost; + /** registered configuration function PO->SO */ + int (*PO2SOconfig)(uint16 slave); + /** readable name */ + char name[EC_MAXNAME + 1]; +} ec_slavet; + +/** for list of ethercat slave groups */ +typedef struct +{ + /** logical start address for this group */ + uint32 logstartaddr; + /** output bytes, if Obits < 8 then Obytes = 0 */ + uint32 Obytes; + /** output pointer in IOmap buffer */ + uint8 *outputs; + /** input bytes, if Ibits < 8 then Ibytes = 0 */ + uint32 Ibytes; + /** input pointer in IOmap buffer */ + uint8 *inputs; + /** has DC capabillity */ + boolean hasdc; + /** next DC slave */ + uint16 DCnext; + /** E-bus current */ + int16 Ebuscurrent; + /** if >0 block use of LRW in processdata */ + uint8 blockLRW; + /** IO segegments used */ + uint16 nsegments; + /** 1st input segment */ + uint16 Isegment; + /** Offset in input segment */ + uint16 Ioffset; + /** Expected workcounter outputs */ + uint16 outputsWKC; + /** Expected workcounter inputs */ + uint16 inputsWKC; + /** check slave states */ + boolean docheckstate; + /** IO segmentation list. Datagrams must not break SM in two. */ + uint32 IOsegment[EC_MAXIOSEGMENTS]; +} ec_groupt; + +/** SII FMMU structure */ +typedef struct +{ + uint16 Startpos; + uint8 nFMMU; + uint8 FMMU0; + uint8 FMMU1; + uint8 FMMU2; + uint8 FMMU3; +} ec_eepromFMMUt; + +/** SII SM structure */ +typedef struct +{ + uint16 Startpos; + uint8 nSM; + uint16 PhStart; + uint16 Plength; + uint8 Creg; + uint8 Sreg; /* dont care */ + uint8 Activate; + uint8 PDIctrl; /* dont care */ +} ec_eepromSMt; + +/** record to store rxPDO and txPDO table from eeprom */ +typedef struct +{ + uint16 Startpos; + uint16 Length; + uint16 nPDO; + uint16 Index[EC_MAXEEPDO]; + uint16 SyncM[EC_MAXEEPDO]; + uint16 BitSize[EC_MAXEEPDO]; + uint16 SMbitsize[EC_MAXSM]; +} ec_eepromPDOt; + +/** mailbox buffer array */ +typedef uint8 ec_mbxbuft[EC_MAXMBX + 1]; + +/** standard ethercat mailbox header */ +PACKED_BEGIN +typedef struct PACKED +{ + uint16 length; + uint16 address; + uint8 priority; + uint8 mbxtype; +} ec_mbxheadert; +PACKED_END + +/** ALstatus and ALstatus code */ +PACKED_BEGIN +typedef struct PACKED +{ + uint16 alstatus; + uint16 unused; + uint16 alstatuscode; +} ec_alstatust; +PACKED_END + +/** stack structure to store segmented LRD/LWR/LRW constructs */ +typedef struct +{ + uint8 pushed; + uint8 pulled; + uint8 idx[EC_MAXBUF]; + void *data[EC_MAXBUF]; + uint16 length[EC_MAXBUF]; +} ec_idxstackT; + +/** ringbuf for error storage */ +typedef struct +{ + int16 head; + int16 tail; + ec_errort Error[EC_MAXELIST + 1]; +} ec_eringt; + +/** SyncManager Communication Type structure for CA */ +PACKED_BEGIN +typedef struct PACKED +{ + uint8 n; + uint8 nu1; + uint8 SMtype[EC_MAXSM]; +} ec_SMcommtypet; +PACKED_END + +/** SDO assign structure for CA */ +PACKED_BEGIN +typedef struct PACKED +{ + uint8 n; + uint8 nu1; + uint16 index[256]; +} ec_PDOassignt; +PACKED_END + +/** SDO description structure for CA */ +PACKED_BEGIN +typedef struct PACKED +{ + uint8 n; + uint8 nu1; + uint32 PDO[256]; +} ec_PDOdesct; +PACKED_END + +/** Context structure , referenced by all ecx functions*/ +typedef struct +{ + /** port reference, may include red_port */ + ecx_portt *port; + /** slavelist reference */ + ec_slavet *slavelist; + /** number of slaves found in configuration */ + int *slavecount; + /** maximum number of slaves allowed in slavelist */ + int maxslave; + /** grouplist reference */ + ec_groupt *grouplist; + /** maximum number of groups allowed in grouplist */ + int maxgroup; + /** internal, reference to eeprom cache buffer */ + uint8 *esibuf; + /** internal, reference to eeprom cache map */ + uint32 *esimap; + /** internal, current slave for eeprom cache */ + uint16 esislave; + /** internal, reference to error list */ + ec_eringt *elist; + /** internal, reference to processdata stack buffer info */ + ec_idxstackT *idxstack; + /** reference to ecaterror state */ + boolean *ecaterror; + /** internal, position of DC datagram in process data packet */ + uint16 DCtO; + /** internal, length of DC datagram */ + uint16 DCl; + /** reference to last DC time from slaves */ + int64 *DCtime; + /** internal, SM buffer */ + ec_SMcommtypet *SMcommtype; + /** internal, PDO assign list */ + ec_PDOassignt *PDOassign; + /** internal, PDO description list */ + ec_PDOdesct *PDOdesc; + /** internal, SM list from eeprom */ + ec_eepromSMt *eepSM; + /** internal, FMMU list from eeprom */ + ec_eepromFMMUt *eepFMMU; + /** registered FoE hook */ + int (*FOEhook)(uint16 slave, int packetnumber, int datasize); +} ecx_contextt; + +#ifdef EC_VER1 +/** global struct to hold default master context */ +extern ecx_contextt ecx_context; +/** main slave data structure array */ +extern ec_slavet ec_slave[EC_MAXSLAVE]; +/** number of slaves found by configuration function */ +extern int ec_slavecount; +/** slave group structure */ +extern ec_groupt ec_group[EC_MAXGROUP]; +extern boolean EcatError; +extern int64 ec_DCtime; + +void ec_pusherror(const ec_errort *Ec); +boolean ec_poperror(ec_errort *Ec); +boolean ec_iserror(void); +void ec_packeterror(uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode); +int ec_init(char * ifname); +int ec_init_redundant(char *ifname, char *if2name); +void ec_close(void); +uint8 ec_siigetbyte(uint16 slave, uint16 address); +int16 ec_siifind(uint16 slave, uint16 cat); +void ec_siistring(char *str, uint16 slave, uint16 Sn); +uint16 ec_siiFMMU(uint16 slave, ec_eepromFMMUt* FMMU); +uint16 ec_siiSM(uint16 slave, ec_eepromSMt* SM); +uint16 ec_siiSMnext(uint16 slave, ec_eepromSMt* SM, uint16 n); +int ec_siiPDO(uint16 slave, ec_eepromPDOt* PDO, uint8 t); +int ec_readstate(void); +int ec_writestate(uint16 slave); +uint16 ec_statecheck(uint16 slave, uint16 reqstate, int timeout); +int ec_mbxempty(uint16 slave, int timeout); +int ec_mbxsend(uint16 slave,ec_mbxbuft *mbx, int timeout); +int ec_mbxreceive(uint16 slave, ec_mbxbuft *mbx, int timeout); +void ec_esidump(uint16 slave, uint8 *esibuf); +uint32 ec_readeeprom(uint16 slave, uint16 eeproma, int timeout); +int ec_writeeeprom(uint16 slave, uint16 eeproma, uint16 data, int timeout); +int ec_eeprom2master(uint16 slave); +int ec_eeprom2pdi(uint16 slave); +uint64 ec_readeepromAP(uint16 aiadr, uint16 eeproma, int timeout); +int ec_writeeepromAP(uint16 aiadr, uint16 eeproma, uint16 data, int timeout); +uint64 ec_readeepromFP(uint16 configadr, uint16 eeproma, int timeout); +int ec_writeeepromFP(uint16 configadr, uint16 eeproma, uint16 data, int timeout); +void ec_readeeprom1(uint16 slave, uint16 eeproma); +uint32 ec_readeeprom2(uint16 slave, int timeout); +int ec_send_processdata_group(uint8 group); +int ec_receive_processdata_group(uint8 group, int timeout); +int ec_send_processdata(void); +int ec_receive_processdata(int timeout); +#endif + +ec_adaptert * ec_find_adapters(void); +void ec_free_adapters(ec_adaptert * adapter); +uint8 ec_nextmbxcnt(uint8 cnt); +void ec_clearmbx(ec_mbxbuft *Mbx); +void ecx_pusherror(ecx_contextt *context, const ec_errort *Ec); +boolean ecx_poperror(ecx_contextt *context, ec_errort *Ec); +boolean ecx_iserror(ecx_contextt *context); +void ecx_packeterror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode); +int ecx_init(ecx_contextt *context, char * ifname); +int ecx_init_redundant(ecx_contextt *context, ecx_redportt *redport, char *ifname, char *if2name); +void ecx_close(ecx_contextt *context); +uint8 ecx_siigetbyte(ecx_contextt *context, uint16 slave, uint16 address); +int16 ecx_siifind(ecx_contextt *context, uint16 slave, uint16 cat); +void ecx_siistring(ecx_contextt *context, char *str, uint16 slave, uint16 Sn); +uint16 ecx_siiFMMU(ecx_contextt *context, uint16 slave, ec_eepromFMMUt* FMMU); +uint16 ecx_siiSM(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM); +uint16 ecx_siiSMnext(ecx_contextt *context, uint16 slave, ec_eepromSMt* SM, uint16 n); +int ecx_siiPDO(ecx_contextt *context, uint16 slave, ec_eepromPDOt* PDO, uint8 t); +int ecx_readstate(ecx_contextt *context); +int ecx_writestate(ecx_contextt *context, uint16 slave); +uint16 ecx_statecheck(ecx_contextt *context, uint16 slave, uint16 reqstate, int timeout); +int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout); +int ecx_mbxsend(ecx_contextt *context, uint16 slave,ec_mbxbuft *mbx, int timeout); +int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout); +void ecx_esidump(ecx_contextt *context, uint16 slave, uint8 *esibuf); +uint32 ecx_readeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, int timeout); +int ecx_writeeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, uint16 data, int timeout); +int ecx_eeprom2master(ecx_contextt *context, uint16 slave); +int ecx_eeprom2pdi(ecx_contextt *context, uint16 slave); +uint64 ecx_readeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, int timeout); +int ecx_writeeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, uint16 data, int timeout); +uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, int timeout); +int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout); +void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma); +uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout); +int ecx_send_processdata_group(ecx_contextt *context, uint8 group); +int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout); +int ecx_send_processdata(ecx_contextt *context); +int ecx_receive_processdata(ecx_contextt *context, int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/ethercatprint.c b/src/ethercatprint.c new file mode 100644 index 0000000..c8261b9 --- /dev/null +++ b/src/ethercatprint.c @@ -0,0 +1,400 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatprint.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Module to convert EtherCAT errors to readable messages. + * + * SDO abort messages and AL status codes are used to relay slave errors to + * the user application. This module converts the binary codes to readble text. + * For the defined error codes see the EtherCAT protocol documentation. + */ + +#include +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatmain.h" + +#define EC_MAXERRORNAME 127 + +/** SDO error list type definition */ +typedef struct +{ + /** Error code returned from SDO */ + uint32 errorcode; + /** Readable error description */ + char errordescription[EC_MAXERRORNAME + 1]; +} ec_sdoerrorlist_t; + +/** AL status code list type definition */ +typedef struct +{ + /** AL status code */ + uint16 ALstatuscode; + /** Readable description */ + char ALstatuscodedescription[EC_MAXERRORNAME + 1]; +} ec_ALstatuscodelist_t; + +/** SoE error list type definition */ +typedef struct +{ + /** SoE error code */ + uint16 errorcode; + /** Readable description */ + char errordescription[EC_MAXERRORNAME + 1]; +} ec_soeerrorlist_t; + +/** MBX error list type definition */ +typedef struct +{ + /** MBX error code */ + uint16 errorcode; + /** Readable description */ + char errordescription[EC_MAXERRORNAME + 1]; +} ec_mbxerrorlist_t; + +char estring[EC_MAXERRORNAME]; + +/** SDO error list definition */ +const ec_sdoerrorlist_t ec_sdoerrorlist[] = { + {0x00000000, "No error" }, + {0x05030000, "Toggle bit not changed" }, + {0x05040000, "SDO protocol timeout" }, + {0x05040001, "Client/Server command specifier not valid or unknown" }, + {0x05040005, "Out of memory" }, + {0x06010000, "Unsupported access to an object" }, + {0x06010001, "Attempt to read to a write only object" }, + {0x06010002, "Attempt to write to a read only object" }, + {0x06010003, "Subindex can not be written, SI0 must be 0 for write access" }, + {0x06010004, "SDO Complete access not supported for variable length objects" }, + {0x06010005, "Object length exceeds mailbox size" }, + {0x06010006, "Object mapped to RxPDO, SDO download blocked" }, + {0x06020000, "The object does not exist in the object directory" }, + {0x06040041, "The object can not be mapped into the PDO" }, + {0x06040042, "The number and length of the objects to be mapped would exceed the PDO length" }, + {0x06040043, "General parameter incompatibility reason" }, + {0x06040047, "General internal incompatibility in the device" }, + {0x06060000, "Access failed due to a hardware error" }, + {0x06070010, "Data type does not match, length of service parameter does not match" }, + {0x06070012, "Data type does not match, length of service parameter too high" }, + {0x06070013, "Data type does not match, length of service parameter too low" }, + {0x06090011, "Subindex does not exist" }, + {0x06090030, "Value range of parameter exceeded (only for write access)" }, + {0x06090031, "Value of parameter written too high" }, + {0x06090032, "Value of parameter written too low" }, + {0x06090036, "Maximum value is less than minimum value" }, + {0x08000000, "General error" }, + {0x08000020, "Data cannot be transferred or stored to the application" }, + {0x08000021, "Data cannot be transferred or stored to the application because of local control" }, + {0x08000022, "Data cannot be transferred or stored to the application because of the present device state" }, + {0x08000023, "Object dictionary dynamic generation fails or no object dictionary is present" }, + {0xffffffff, "Unknown" } +}; + +/** AL status code list definition */ +const ec_ALstatuscodelist_t ec_ALstatuscodelist[] = { + {0x0000 , "No error" }, + {0x0001 , "Unspecified error" }, + {0x0002 , "No memory" }, + {0x0011 , "Invalid requested state change" }, + {0x0012 , "Unknown requested state" }, + {0x0013 , "Bootstrap not supported" }, + {0x0014 , "No valid firmware" }, + {0x0015 , "Invalid mailbox configuration" }, + {0x0016 , "Invalid mailbox configuration" }, + {0x0017 , "Invalid sync manager configuration" }, + {0x0018 , "No valid inputs available" }, + {0x0019 , "No valid outputs" }, + {0x001A , "Synchronization error" }, + {0x001B , "Sync manager watchdog" }, + {0x001C , "Invalid sync Manager types" }, + {0x001D , "Invalid output configuration" }, + {0x001E , "Invalid input configuration" }, + {0x001F , "Invalid watchdog configuration" }, + {0x0020 , "Slave needs cold start" }, + {0x0021 , "Slave needs INIT" }, + {0x0022 , "Slave needs PREOP" }, + {0x0023 , "Slave needs SAFEOP" }, + {0x0024 , "Invalid input mapping" }, + {0x0025 , "Invalid output mapping" }, + {0x0026 , "Inconsistent settings" }, + {0x0027 , "Freerun not supported" }, + {0x0028 , "Synchronisation not supported" }, + {0x0029 , "Freerun needs 3buffer mode" }, + {0x002A , "Background watchdog" }, + {0x002B , "No valid Inputs and Outputs" }, + {0x002C , "Fatal sync error" }, + {0x002D , "No sync error" }, // was "Invalid Output FMMU Configuration" + {0x002E , "Invalid input FMMU configuration" }, + {0x0030 , "Invalid DC SYNC configuration" }, + {0x0031 , "Invalid DC latch configuration" }, + {0x0032 , "PLL error" }, + {0x0033 , "DC sync IO error" }, + {0x0034 , "DC sync timeout error" }, + {0x0035 , "DC invalid sync cycle time" }, + {0x0035 , "DC invalid sync0 cycle time" }, + {0x0035 , "DC invalid sync1 cycle time" }, + {0x0042 , "MBX_EOE" }, + {0x0043 , "MBX_COE" }, + {0x0044 , "MBX_FOE" }, + {0x0045 , "MBX_SOE" }, + {0x004F , "MBX_VOE" }, + {0x0050 , "EEPROM no access" }, + {0x0051 , "EEPROM error" }, + {0x0060 , "Slave restarted locally" }, + {0x0061 , "Device identification value updated" }, + {0x00f0 , "Application controller available" }, + {0xffff , "Unknown" } +}; + +/** SoE error list definition */ +const ec_soeerrorlist_t ec_soeerrorlist[] = { + {0x0000, "No error" }, + {0x1001, "No IDN" }, + {0x1009, "Invalid access to element 1" }, + {0x2001, "No Name" }, + {0x2002, "Name transmission too short" }, + {0x2003, "Name transmission too long" }, + {0x2004, "Name cannot be changed (read only)" }, + {0x2005, "Name is write-protected at this time" }, + {0x3002, "Attribute transmission too short" }, + {0x3003, "Attribute transmission too long" }, + {0x3004, "Attribute cannot be changed (read only)" }, + {0x3005, "Attribute is write-protected at this time" }, + {0x4001, "No units" }, + {0x4002, "Unit transmission too short" }, + {0x4003, "Unit transmission too long" }, + {0x4004, "Unit cannot be changed (read only)" }, + {0x4005, "Unit is write-protected at this time" }, + {0x5001, "No minimum input value" }, + {0x5002, "Minimum input value transmission too short" }, + {0x5003, "Minimum input value transmission too long" }, + {0x5004, "Minimum input value cannot be changed (read only)" }, + {0x5005, "Minimum input value is write-protected at this time" }, + {0x6001, "No maximum input value" }, + {0x6002, "Maximum input value transmission too short" }, + {0x6003, "Maximum input value transmission too long" }, + {0x6004, "Maximum input value cannot be changed (read only)" }, + {0x6005, "Maximum input value is write-protected at this time" }, + {0x7002, "Operation data transmission too short" }, + {0x7003, "Operation data transmission too long" }, + {0x7004, "Operation data cannot be changed (read only)" }, + {0x7005, "Operation data is write-protected at this time (state)" }, + {0x7006, "Operation data is smaller than the minimum input value" }, + {0x7007, "Operation data is smaller than the maximum input value" }, + {0x7008, "Invalid operation data:Configured IDN will not be supported" }, + {0x7009, "Operation data write protected by a password" }, + {0x700A, "Operation data is write protected, it is configured cyclically" }, + {0x700B, "Invalid indirect addressing: (e.g., data container, list handling)" }, + {0x700C, "Operation data is write protected, due to other settings" }, + {0x700D, "Reserved" }, + {0x7010, "Procedure command already active" }, + {0x7011, "Procedure command not interruptible" }, + {0x7012, "Procedure command at this time not executable (state)" }, + {0x7013, "Procedure command not executable (invalid or false parameters)" }, + {0x7014, "No data state" }, + {0x8001, "No default value" }, + {0x8002, "Default value transmission too long" }, + {0x8004, "Default value cannot be changed, read only" }, + {0x800A, "Invalid drive number" }, + {0x800A, "General error" }, + {0x800A, "No element addressed" }, + {0xffff, "Unknown" } +}; + +/** MBX error list definition */ +const ec_mbxerrorlist_t ec_mbxerrorlist[] = { + {0x0000, "No error" }, + {0x0001, "Syntax of 6 octet Mailbox Header is wrong" }, + {0x0002, "The mailbox protocol is not supported" }, + {0x0003, "Channel Field contains wrong value"}, + {0x0004, "The service is no supported"}, + {0x0005, "Invalid mailbox header"}, + {0x0006, "Length of received mailbox data is too short"}, + {0x0007, "No more memory in slave"}, + {0x0008, "The lenght of data is inconsistent"}, + {0xffff, "Unknown"} +}; + +/** Look up text string that belongs to SDO error code. + * + * @param[in] sdoerrorcode = SDO error code as defined in EtherCAT protocol + * @return readable string + */ +const char* ec_sdoerror2string( uint32 sdoerrorcode) +{ + int i = 0; + + while ( (ec_sdoerrorlist[i].errorcode != 0xffffffffUL) && + (ec_sdoerrorlist[i].errorcode != sdoerrorcode) ) + { + i++; + } + + return ec_sdoerrorlist[i].errordescription; +} + +/** Look up text string that belongs to AL status code. + * + * @param[in] ALstatuscode = AL status code as defined in EtherCAT protocol + * @return readable string + */ +char* ec_ALstatuscode2string( uint16 ALstatuscode) +{ + int i = 0; + + while ( (ec_ALstatuscodelist[i].ALstatuscode != 0xffff) && + (ec_ALstatuscodelist[i].ALstatuscode != ALstatuscode) ) + { + i++; + } + + return (char *) ec_ALstatuscodelist[i].ALstatuscodedescription; +} + +/** Look up text string that belongs to SoE error code. + * + * @param[in] errorcode = SoE error code as defined in EtherCAT protocol + * @return readable string + */ +char* ec_soeerror2string( uint16 errorcode) +{ + int i = 0; + + while ( (ec_soeerrorlist[i].errorcode != 0xffff) && + (ec_soeerrorlist[i].errorcode != errorcode) ) + { + i++; + } + + return (char *) ec_soeerrorlist[i].errordescription; +} + +/** Look up text string that belongs to MBX error code. + * + * @param[in] errorcode = MBX error code as defined in EtherCAT protocol + * @return readable string + */ +char* ec_mbxerror2string( uint16 errorcode) +{ + int i = 0; + + while ( (ec_mbxerrorlist[i].errorcode != 0xffff) && + (ec_mbxerrorlist[i].errorcode != errorcode) ) + { + i++; + } + + return (char *) ec_mbxerrorlist[i].errordescription; +} + +/** Look up error in ec_errorlist and convert to text string. + * + * @param[in] context = context struct + * @return readable string + */ +char* ecx_elist2string(ecx_contextt *context) +{ + ec_errort Ec; + char timestr[20]; + + if (ecx_poperror(context, &Ec)) + { + sprintf(timestr, "Time:%12.3f", Ec.Time.sec + (Ec.Time.usec / 1000000.0) ); + switch (Ec.Etype) + { + case EC_ERR_TYPE_SDO_ERROR: + { + sprintf(estring, "%s SDO slave:%d index:%4.4x.%2.2x error:%8.8x %s\n", + timestr, Ec.Slave, Ec.Index, Ec.SubIdx, (unsigned)Ec.AbortCode, ec_sdoerror2string(Ec.AbortCode)); + break; + } + case EC_ERR_TYPE_EMERGENCY: + { + sprintf(estring, "%s EMERGENCY slave:%d error:%4.4x\n", + timestr, Ec.Slave, Ec.ErrorCode); + break; + } + case EC_ERR_TYPE_PACKET_ERROR: + { + sprintf(estring, "%s PACKET slave:%d index:%4.4x.%2.2x error:%d\n", + timestr, Ec.Slave, Ec.Index, Ec.SubIdx, Ec.ErrorCode); + break; + } + case EC_ERR_TYPE_SDOINFO_ERROR: + { + sprintf(estring, "%s SDO slave:%d index:%4.4x.%2.2x error:%8.8x %s\n", + timestr, Ec.Slave, Ec.Index, Ec.SubIdx, (unsigned)Ec.AbortCode, ec_sdoerror2string(Ec.AbortCode)); + break; + } + case EC_ERR_TYPE_SOE_ERROR: + { + sprintf(estring, "%s SoE slave:%d IDN:%4.4x error:%4.4x %s\n", + timestr, Ec.Slave, Ec.Index, (unsigned)Ec.AbortCode, ec_soeerror2string(Ec.ErrorCode)); + break; + } + case EC_ERR_TYPE_MBX_ERROR: + { + sprintf(estring, "%s MBX slave:%d error:%4.4x %s\n", + timestr, Ec.Slave, Ec.ErrorCode, ec_mbxerror2string(Ec.ErrorCode)); + break; + } + default: + { + sprintf(estring, "%s error:%8.8x\n", + timestr, (unsigned)Ec.AbortCode); + break; + } + } + return (char*) estring; + } + else + { + return ""; + } +} + +#ifdef EC_VER1 +char* ec_elist2string(void) +{ + return ecx_elist2string(&ecx_context); +} +#endif diff --git a/src/ethercatprint.h b/src/ethercatprint.h new file mode 100644 index 0000000..72f4fdd --- /dev/null +++ b/src/ethercatprint.h @@ -0,0 +1,68 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatprint.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatprint.c + */ + +#ifndef _ethercatprint_ +#define _ethercatprint_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +char* ec_sdoerror2string( uint16 sdoerrorcode); +char* ec_ALstatuscode2string( uint16 ALstatuscode); +char* ec_soeerror2string( uint16 errorcode); +char* ecx_elist2string(ecx_contextt *context); + +#ifdef EC_VER1 +char* ec_elist2string(void); +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/ethercatsoe.c b/src/ethercatsoe.c new file mode 100644 index 0000000..f4fb18e --- /dev/null +++ b/src/ethercatsoe.c @@ -0,0 +1,425 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatsoe.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Servo over EtherCAT (SoE) Module. + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercattype.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatsoe.h" + +#define EC_SOE_MAX_DRIVES 8 + +/** SoE (Servo over EtherCAT) mailbox structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint8 opCode :3; + uint8 incomplete :1; + uint8 error :1; + uint8 driveNo :3; + uint8 elementflags; + union + { + uint16 idn; + uint16 fragmentsleft; + }; +} ec_SoEt; +PACKED_END + +/** Report SoE error. + * + * @param[in] context = context struct + * @param[in] Slave = Slave number + * @param[in] idn = IDN that generated error + * @param[in] Error = Error code, see EtherCAT documentation for list + */ +void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Time = osal_current_time(); + Ec.Slave = Slave; + Ec.Index = idn; + Ec.SubIdx = 0; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_SOE_ERROR; + Ec.ErrorCode = Error; + ecx_pusherror(context, &Ec); +} + +/** SoE read, blocking. + * + * The IDN object of the selected slave and DriveNo is read. If a response + * is larger than the mailbox size then the response is segmented. The function + * will combine all segments and copy them to the parameter buffer. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] driveNo = Drive number in slave + * @param[in] elementflags = Flags to select what properties of IDN are to be transfered. + * @param[in] idn = IDN. + * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SoE. + * @param[out] p = Pointer to parameter buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout) +{ + ec_SoEt *SoEp, *aSoEp; + uint16 totalsize, framedatasize; + int wkc; + uint8 *bp; + uint8 *mp; + uint16 *errorcode; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean NotLast; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSoEp = (ec_SoEt *)&MbxIn; + SoEp = (ec_SoEt *)&MbxOut; + SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert)); + SoEp->MbxHeader.address = htoes(0x0000); + SoEp->MbxHeader.priority = 0x00; + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */ + SoEp->opCode = ECT_SOE_READREQ; + SoEp->incomplete = 0; + SoEp->error = 0; + SoEp->driveNo = driveNo; + SoEp->elementflags = elementflags; + SoEp->idn = htoes(idn); + totalsize = 0; + bp = p; + mp = (uint8 *)&MbxIn + sizeof(ec_SoEt); + NotLast = TRUE; + /* send SoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + while (NotLast) + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + /* slave response should be SoE, ReadRes */ + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_READRES) && + (aSoEp->error == 0) && + (aSoEp->driveNo == driveNo) && + (aSoEp->elementflags == elementflags)) + { + framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert); + totalsize += framedatasize; + /* Does parameter fit in parameter buffer ? */ + if (totalsize <= *psize) + { + /* copy parameter data in parameter buffer */ + memcpy(bp, mp, framedatasize); + /* increment buffer pointer */ + bp += framedatasize; + } + else + { + framedatasize -= totalsize - *psize; + totalsize = *psize; + /* copy parameter data in parameter buffer */ + if (framedatasize > 0) memcpy(bp, mp, framedatasize); + } + + if (!aSoEp->incomplete) + { + NotLast = FALSE; + *psize = totalsize; + } + } + /* other slave response */ + else + { + NotLast = FALSE; + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_READRES) && + (aSoEp->error == 1)) + { + mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16)); + errorcode = (uint16 *)mp; + ecx_SoEerror(context, slave, idn, *errorcode); + } + else + { + ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + else + { + NotLast = FALSE; + ecx_packeterror(context, slave, idn, 0, 4); /* no response */ + } + } + } + return wkc; +} + +/** SoE write, blocking. + * + * The IDN object of the selected slave and DriveNo is written. If a response + * is larger than the mailbox size then the response is segmented. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[in] driveNo = Drive number in slave + * @param[in] elementflags = Flags to select what properties of IDN are to be transfered. + * @param[in] idn = IDN. + * @param[in] psize = Size in bytes of parameter buffer. + * @param[out] p = Pointer to parameter buffer + * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM + * @return Workcounter from last slave response + */ +int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout) +{ + ec_SoEt *SoEp, *aSoEp; + uint16 framedatasize, maxdata; + int wkc; + uint8 *mp; + uint8 *hp; + uint16 *errorcode; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + boolean NotLast; + + ec_clearmbx(&MbxIn); + /* Empty slave out mailbox if something is in. Timeout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aSoEp = (ec_SoEt *)&MbxIn; + SoEp = (ec_SoEt *)&MbxOut; + SoEp->MbxHeader.address = htoes(0x0000); + SoEp->MbxHeader.priority = 0x00; + SoEp->opCode = ECT_SOE_WRITEREQ; + SoEp->error = 0; + SoEp->driveNo = driveNo; + SoEp->elementflags = elementflags; + hp = p; + mp = (uint8 *)&MbxOut + sizeof(ec_SoEt); + maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt); + NotLast = TRUE; + while (NotLast) + { + framedatasize = psize; + NotLast = FALSE; + SoEp->idn = htoes(idn); + SoEp->incomplete = 0; + if (framedatasize > maxdata) + { + framedatasize = maxdata; /* segmented transfer needed */ + NotLast = TRUE; + SoEp->incomplete = 1; + SoEp->fragmentsleft = psize / maxdata; + } + SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize); + /* get new mailbox counter, used for session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */ + /* copy parameter data to mailbox */ + memcpy(mp, hp, framedatasize); + hp += framedatasize; + psize -= framedatasize; + /* send SoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if (wkc > 0) /* succeeded to place mailbox in slave ? */ + { + if (!NotLast || !ecx_mbxempty(context, slave, timeout)) + { + /* clean mailboxbuffer */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + if (wkc > 0) /* succeeded to read slave response ? */ + { + NotLast = FALSE; + /* slave response should be SoE, WriteRes */ + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_WRITERES) && + (aSoEp->error == 0) && + (aSoEp->driveNo == driveNo) && + (aSoEp->elementflags == elementflags)) + { + /* SoE write succeeded */ + } + /* other slave response */ + else + { + if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && + (aSoEp->opCode == ECT_SOE_READRES) && + (aSoEp->error == 1)) + { + mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16)); + errorcode = (uint16 *)mp; + ecx_SoEerror(context, slave, idn, *errorcode); + } + else + { + ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + else + { + ecx_packeterror(context, slave, idn, 0, 4); /* no response */ + } + } + } + } + return wkc; +} + +/** SoE read AT and MTD mapping. + * + * SoE has standard indexes defined for mapping. This function + * tries to read them and collect a full input and output mapping size + * of designated slave. + * + * @param[in] context = context struct + * @param[in] slave = Slave number + * @param[out] Osize = Size in bits of output mapping (MTD) found + * @param[out] Isize = Size in bits of input mapping (AT) found + * @return >0 if mapping succesful. + */ +int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize) +{ + int retVal = 0; + int wkc; + int psize; + int driveNr; + uint16 entries, itemcount; + ec_SoEmappingt SoEmapping; + ec_SoEattributet SoEattribute; + + *Isize = 0; + *Osize = 0; + for(driveNr = 0; driveNr < EC_SOE_MAX_DRIVES; driveNr++) + { + psize = sizeof(SoEmapping); + /* read output mapping via SoE */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM); + if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING)) + { + /* command word (uint16) is always mapped but not in list */ + *Osize = 16; + for (itemcount = 0 ; itemcount < entries ; itemcount++) + { + psize = sizeof(SoEattribute); + /* read attribute of each IDN in mapping list */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM); + if ((wkc > 0) && (!SoEattribute.list)) + { + /* length : 0 = 8bit, 1 = 16bit .... */ + *Osize += (int)8 << SoEattribute.length; + } + } + } + psize = sizeof(SoEmapping); + /* read input mapping via SoE */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM); + if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING)) + { + /* status word (uint16) is always mapped but not in list */ + *Isize = 16; + for (itemcount = 0 ; itemcount < entries ; itemcount++) + { + psize = sizeof(SoEattribute); + /* read attribute of each IDN in mapping list */ + wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM); + if ((wkc > 0) && (!SoEattribute.list)) + { + /* length : 0 = 8bit, 1 = 16bit .... */ + *Isize += (int)8 << SoEattribute.length; + } + } + } + } + + /* found some I/O bits ? */ + if ((*Isize > 0) || (*Osize > 0)) + { + retVal = 1; + } + return retVal; +} + +#ifdef EC_VER1 +int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout) +{ + return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout); +} + +int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout) +{ + return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout); +} + +int ec_readIDNmap(uint16 slave, int *Osize, int *Isize) +{ + return ecx_readIDNmap(&ecx_context, slave, Osize, Isize); +} +#endif diff --git a/src/ethercatsoe.h b/src/ethercatsoe.h new file mode 100644 index 0000000..9224c11 --- /dev/null +++ b/src/ethercatsoe.h @@ -0,0 +1,166 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercatsoe.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for ethercatsoe.c + */ + +#ifndef _ethercatsoe_ +#define _ethercatsoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define EC_SOE_DATASTATE_B 0x01 +#define EC_SOE_NAME_B 0x02 +#define EC_SOE_ATTRIBUTE_B 0x04 +#define EC_SOE_UNIT_B 0x08 +#define EC_SOE_MIN_B 0x10 +#define EC_SOE_MAX_B 0x20 +#define EC_SOE_VALUE_B 0x40 +#define EC_SOE_DEFAULT_B 0x80 + +#define EC_SOE_MAXNAME 60 +#define EC_SOE_MAXMAPPING 64 + +#define EC_IDN_MDTCONFIG 24 +#define EC_IDN_ATCONFIG 16 + +/** SoE name structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** current length in bytes of list */ + uint16 currentlength; + /** maximum length in bytes of list */ + uint16 maxlength; + char name[EC_SOE_MAXNAME]; +} ec_SoEnamet; +PACKED_END + +/** SoE list structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** current length in bytes of list */ + uint16 currentlength; + /** maximum length in bytes of list */ + uint16 maxlength; + union + { + uint8 byte[8]; + uint16 word[4]; + uint32 dword[2]; + uint64 lword[1]; + }; +} ec_SoElistt; +PACKED_END + +/** SoE IDN mapping structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** current length in bytes of list */ + uint16 currentlength; + /** maximum length in bytes of list */ + uint16 maxlength; + uint16 idn[EC_SOE_MAXMAPPING]; +} ec_SoEmappingt; +PACKED_END + +#define EC_SOE_LENGTH_1 0x00 +#define EC_SOE_LENGTH_2 0x01 +#define EC_SOE_LENGTH_4 0x02 +#define EC_SOE_LENGTH_8 0x03 +#define EC_SOE_TYPE_BINARY 0x00 +#define EC_SOE_TYPE_UINT 0x01 +#define EC_SOE_TYPE_INT 0x02 +#define EC_SOE_TYPE_HEX 0x03 +#define EC_SOE_TYPE_STRING 0x04 +#define EC_SOE_TYPE_IDN 0x05 +#define EC_SOE_TYPE_FLOAT 0x06 +#define EC_SOE_TYPE_PARAMETER 0x07 + +/** SoE attribute structure */ +PACKED_BEGIN +typedef struct PACKED +{ + /** evaluation factor for display purposes */ + uint32 evafactor :16; + /** length of IDN element(s) */ + uint32 length :2; + /** IDN is list */ + uint32 list :1; + /** IDN is command */ + uint32 command :1; + /** datatype */ + uint32 datatype :3; + uint32 reserved1 :1; + /** decimals to display if float datatype */ + uint32 decimals :4; + /** write protected in pre-op */ + uint32 wppreop :1; + /** write protected in safe-op */ + uint32 wpsafeop :1; + /** write protected in op */ + uint32 wpop :1; + uint32 reserved2 :1; +} ec_SoEattributet; +PACKED_END + +#ifdef EC_VER1 +int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout); +int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout); +int ec_readIDNmap(uint16 slave, int *Osize, int *Isize); +#endif + +int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout); +int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout); +int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/ethercattype.h b/src/ethercattype.h new file mode 100644 index 0000000..987a96c --- /dev/null +++ b/src/ethercattype.h @@ -0,0 +1,592 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : ethercattype.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * General typedefs and defines for EtherCAT. + * + * Defines that could need optimalisation for specific applications + * are the EC_TIMEOUTxxx. Assumptions for the standard settings are a + * standard linux PC or laptop and a wired connection to maximal 100 slaves. + * For use with wireless connections or lots of slaves the timouts need + * increasing. For fast systems running Xenomai and RT-net or alike the + * timeouts need to be shorter. + */ + +#ifndef _EC_TYPE_H +#define _EC_TYPE_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** Define Little or Big endian target */ +#define EC_LITTLE_ENDIAN + +/** define EC_VER1 if version 1 default context and functions are needed + * comment if application uses only ecx_ functions and own context */ +#define EC_VER1 + +#include + +/** return value general error */ +#define EC_ERROR -3 +/** return value no frame returned */ +#define EC_NOFRAME -1 +/** return value unknown frame received */ +#define EC_OTHERFRAME -2 +/** maximum EtherCAT frame length in bytes */ +#define EC_MAXECATFRAME 1518 +/** maximum EtherCAT LRW frame length in bytes */ +/* MTU - Ethernet header - length - datagram header - WCK - FCS */ +#define EC_MAXLRWDATA (EC_MAXECATFRAME - 14 - 2 - 10 - 2 - 4) +/** size of DC datagram used in first LRW frame */ +#define EC_FIRSTDCDATAGRAM 20 +/** standard frame buffer size in bytes */ +#define EC_BUFSIZE EC_MAXECATFRAME +/** datagram type EtherCAT */ +#define EC_ECATTYPE 0x1000 +/** number of frame buffers per channel (tx, rx1 rx2) */ +#define EC_MAXBUF 16 +/** timeout value in us for tx frame to return to rx */ +#define EC_TIMEOUTRET 2000 +/** timeout value in us for safe data transfer, max. triple retry */ +#define EC_TIMEOUTRET3 (EC_TIMEOUTRET * 3) +/** timeout value in us for return "safe" variant (f.e. wireless) */ +#define EC_TIMEOUTSAFE 20000 +/** timeout value in us for EEPROM access */ +#define EC_TIMEOUTEEP 20000 +/** timeout value in us for tx mailbox cycle */ +#define EC_TIMEOUTTXM 20000 +/** timeout value in us for rx mailbox cycle */ +#define EC_TIMEOUTRXM 700000 +/** timeout value in us for check statechange */ +#define EC_TIMEOUTSTATE 2000000 +/** size of EEPROM bitmap cache */ +#define EC_MAXEEPBITMAP 128 +/** size of EEPROM cache buffer */ +#define EC_MAXEEPBUF EC_MAXEEPBITMAP << 5 +/** default number of retries if wkc <= 0 */ +#define EC_DEFAULTRETRIES 3 + +/** definition for frame buffers */ +typedef uint8 ec_bufT[EC_BUFSIZE]; + +/** ethernet header definition */ +PACKED_BEGIN +typedef struct PACKED +{ + /** destination MAC */ + uint16 da0,da1,da2; + /** source MAC */ + uint16 sa0,sa1,sa2; + /** ethernet type */ + uint16 etype; +} ec_etherheadert; +PACKED_END + +/** ethernet header size */ +#define ETH_HEADERSIZE sizeof(ec_etherheadert) + +/** EtherCAT datagram header definition */ +PACKED_BEGIN +typedef struct PACKED +{ + /** length of EtherCAT datagram */ + uint16 elength; + /** EtherCAT command, see ec_cmdtype */ + uint8 command; + /** index, used in SOEM for Tx to Rx recombination */ + uint8 index; + /** ADP */ + uint16 ADP; + /** ADO */ + uint16 ADO; + /** length of data portion in datagram */ + uint16 dlength; + /** interrupt, currently unused */ + uint16 irpt; +} ec_comt; +PACKED_END + +/** EtherCAT header size */ +#define EC_HEADERSIZE sizeof(ec_comt) +/** size of ec_comt.elength item in EtherCAT header */ +#define EC_ELENGTHSIZE sizeof(uint16) +/** offset position of command in EtherCAT header */ +#define EC_CMDOFFSET EC_ELENGTHSIZE +/** size of workcounter item in EtherCAT datagram */ +#define EC_WKCSIZE sizeof(uint16) +/** definition of datagram follows bit in ec_comt.dlength */ +#define EC_DATAGRAMFOLLOWS (1 << 15) + +/** Possible error codes returned. */ +typedef enum +{ + /** No error */ + EC_ERR_OK = 0, + /** Library already initialized. */ + EC_ERR_ALREADY_INITIALIZED, + /** Library not initialized. */ + EC_ERR_NOT_INITIALIZED, + /** Timeout occured during execution of the function. */ + EC_ERR_TIMEOUT, + /** No slaves were found. */ + EC_ERR_NO_SLAVES, + /** Function failed. */ + EC_ERR_NOK +} ec_err; + +/** Possible EtherCAT slave states */ +typedef enum +{ + /** Init state*/ + EC_STATE_INIT = 0x01, + /** Pre-operational. */ + EC_STATE_PRE_OP = 0x02, + /** Boot state*/ + EC_STATE_BOOT = 0x03, + /** Safe-operational. */ + EC_STATE_SAFE_OP = 0x04, + /** Operational */ + EC_STATE_OPERATIONAL = 0x08, + /** Error or ACK error */ + EC_STATE_ACK = 0x10, + EC_STATE_ERROR = 0x10 +} ec_state; + +/** Possible buffer states */ +typedef enum +{ + /** Empty */ + EC_BUF_EMPTY = 0x00, + /** Allocated, but not filled */ + EC_BUF_ALLOC = 0x01, + /** Transmitted */ + EC_BUF_TX = 0x02, + /** Received, but not consumed */ + EC_BUF_RCVD = 0x03, + /** Cycle completed */ + EC_BUF_COMPLETE = 0x04 +} ec_bufstate; + +/** Ethercat data types */ +typedef enum +{ + ECT_BOOLEAN = 0x0001, + ECT_INTEGER8 = 0x0002, + ECT_INTEGER16 = 0x0003, + ECT_INTEGER32 = 0x0004, + ECT_UNSIGNED8 = 0x0005, + ECT_UNSIGNED16 = 0x0006, + ECT_UNSIGNED32 = 0x0007, + ECT_REAL32 = 0x0008, + ECT_VISIBLE_STRING = 0x0009, + ECT_OCTET_STRING = 0x000A, + ECT_UNICODE_STRING = 0x000B, + ECT_TIME_OF_DAY = 0x000C, + ECT_TIME_DIFFERENCE = 0x000D, + ECT_DOMAIN = 0x000F, + ECT_INTEGER24 = 0x0010, + ECT_REAL64 = 0x0011, + ECT_INTEGER64 = 0x0015, + ECT_UNSIGNED24 = 0x0016, + ECT_UNSIGNED64 = 0x001B, + ECT_BIT1 = 0x0030, + ECT_BIT2 = 0x0031, + ECT_BIT3 = 0x0032, + ECT_BIT4 = 0x0033, + ECT_BIT5 = 0x0034, + ECT_BIT6 = 0x0035, + ECT_BIT7 = 0x0036, + ECT_BIT8 = 0x0037 +} ec_datatype; + +/** Ethercat command types */ +typedef enum +{ + /** No operation */ + EC_CMD_NOP = 0x00, + /** Auto Increment Read */ + EC_CMD_APRD, + /** Auto Increment Write */ + EC_CMD_APWR, + /** Auto Increment Read Write */ + EC_CMD_APRW, + /** Configured Address Read */ + EC_CMD_FPRD, + /** Configured Address Write */ + EC_CMD_FPWR, + /** Configured Address Read Write */ + EC_CMD_FPRW, + /** Broadcast Read */ + EC_CMD_BRD, + /** Broaddcast Write */ + EC_CMD_BWR, + /** Broadcast Read Write */ + EC_CMD_BRW, + /** Logical Memory Read */ + EC_CMD_LRD, + /** Logical Memory Write */ + EC_CMD_LWR, + /** Logical Memory Read Write */ + EC_CMD_LRW, + /** Auto Increment Read Mulitple Write */ + EC_CMD_ARMW, + /** Configured Read Mulitple Write */ + EC_CMD_FRMW + /** Reserved */ +} ec_cmdtype; + +/** Ethercat EEprom command types */ +typedef enum +{ + /** No operation */ + EC_ECMD_NOP = 0x0000, + /** Read */ + EC_ECMD_READ = 0x0100, + /** Write */ + EC_ECMD_WRITE = 0x0201, + /** Reload */ + EC_ECMD_RELOAD = 0x0300 +} ec_ecmdtype; + +/** EEprom state machine read size */ +#define EC_ESTAT_R64 0x0040 +/** EEprom state machine busy flag */ +#define EC_ESTAT_BUSY 0x8000 +/** EEprom state machine error flag mask */ +#define EC_ESTAT_EMASK 0x7800 +/** EEprom state machine error acknowledge */ +#define EC_ESTAT_NACK 0x2000 + +/* Ethercat SSI (Slave Information Interface) */ + +/** Start address SII sections in Eeprom */ +#define ECT_SII_START 0x0040 + +enum +{ + /** SII category strings */ + ECT_SII_STRING = 10, + /** SII category general */ + ECT_SII_GENERAL = 30, + /** SII category FMMU */ + ECT_SII_FMMU = 40, + /** SII category SM */ + ECT_SII_SM = 41, + /** SII category PDO */ + ECT_SII_PDO = 50 +}; + +/** Item offsets in SII general section */ +enum +{ + ECT_SII_MANUF = 0x0008, + ECT_SII_ID = 0x000a, + ECT_SII_REV = 0x000c, + ECT_SII_BOOTRXMBX = 0x0014, + ECT_SII_BOOTTXMBX = 0x0016, + ECT_SII_MBXSIZE = 0x0019, + ECT_SII_TXMBXADR = 0x001a, + ECT_SII_RXMBXADR = 0x0018, + ECT_SII_MBXPROTO = 0x001c +}; + +/** Mailbox types definitions */ +enum +{ + /** Error mailbox type */ + ECT_MBXT_ERR = 0x00, + /** ADS over EtherCAT mailbox type */ + ECT_MBXT_AOE, + /** Ethernet over EtherCAT mailbox type */ + ECT_MBXT_EOE, + /** CANopen over EtherCAT mailbox type */ + ECT_MBXT_COE, + /** File over EtherCAT mailbox type */ + ECT_MBXT_FOE, + /** Servo over EtherCAT mailbox type */ + ECT_MBXT_SOE, + /** Vendor over EtherCAT mailbox type */ + ECT_MBXT_VOE = 0x0f +}; + +/** CoE mailbox types */ +enum +{ + ECT_COES_EMERGENCY = 0x01, + ECT_COES_SDOREQ, + ECT_COES_SDORES, + ECT_COES_TXPDO, + ECT_COES_RXPDO, + ECT_COES_TXPDO_RR, + ECT_COES_RXPDO_RR, + ECT_COES_SDOINFO +}; + +/** CoE SDO commands */ +enum +{ + ECT_SDO_DOWN_INIT = 0x21, + ECT_SDO_DOWN_EXP = 0x23, + ECT_SDO_DOWN_INIT_CA = 0x31, + ECT_SDO_UP_REQ = 0x40, + ECT_SDO_UP_REQ_CA = 0x50, + ECT_SDO_SEG_UP_REQ = 0x60, + ECT_SDO_ABORT = 0x80 +}; + +/** CoE Object Description commands */ +enum +{ + ECT_GET_ODLIST_REQ = 0x01, + ECT_GET_ODLIST_RES = 0x02, + ECT_GET_OD_REQ = 0x03, + ECT_GET_OD_RES = 0x04, + ECT_GET_OE_REQ = 0x05, + ECT_GET_OE_RES = 0x06, + ECT_SDOINFO_ERROR = 0x07 +}; + +/** FoE opcodes */ +enum +{ + ECT_FOE_READ = 0x01, + ECT_FOE_WRITE, + ECT_FOE_DATA, + ECT_FOE_ACK, + ECT_FOE_ERROR, + ECT_FOE_BUSY +}; + +/** SoE opcodes */ +enum +{ + ECT_SOE_READREQ = 0x01, + ECT_SOE_READRES, + ECT_SOE_WRITEREQ, + ECT_SOE_WRITERES, + ECT_SOE_NOTIFICATION, + ECT_SOE_EMERGENCY +}; + +/** Ethercat registers */ +enum +{ + ECT_REG_TYPE = 0x0000, + ECT_REG_PORTDES = 0x0007, + ECT_REG_ESCSUP = 0x0008, + ECT_REG_STADR = 0x0010, + ECT_REG_ALIAS = 0x0012, + ECT_REG_DLCTL = 0x0100, + ECT_REG_DLPORT = 0x0101, + ECT_REG_DLALIAS = 0x0103, + ECT_REG_DLSTAT = 0x0110, + ECT_REG_ALCTL = 0x0120, + ECT_REG_ALSTAT = 0x0130, + ECT_REG_ALSTATCODE = 0x0134, + ECT_REG_PDICTL = 0x0140, + ECT_REG_IRQMASK = 0x0200, + ECT_REG_RXERR = 0x0300, + ECT_REG_FRXERR = 0x0308, + ECT_REG_EPUECNT = 0x030C, + ECT_REG_PECNT = 0x030D, + ECT_REG_PECODE = 0x030E, + ECT_REG_LLCNT = 0x0310, + ECT_REG_WDCNT = 0x0442, + ECT_REG_EEPCFG = 0x0500, + ECT_REG_EEPCTL = 0x0502, + ECT_REG_EEPSTAT = 0x0502, + ECT_REG_EEPADR = 0x0504, + ECT_REG_EEPDAT = 0x0508, + ECT_REG_FMMU0 = 0x0600, + ECT_REG_FMMU1 = ECT_REG_FMMU0 + 0x10, + ECT_REG_FMMU2 = ECT_REG_FMMU1 + 0x10, + ECT_REG_FMMU3 = ECT_REG_FMMU2 + 0x10, + ECT_REG_SM0 = 0x0800, + ECT_REG_SM1 = ECT_REG_SM0 + 0x08, + ECT_REG_SM2 = ECT_REG_SM1 + 0x08, + ECT_REG_SM3 = ECT_REG_SM2 + 0x08, + ECT_REG_SM0STAT = ECT_REG_SM0 + 0x05, + ECT_REG_SM1STAT = ECT_REG_SM1 + 0x05, + ECT_REG_SM1ACT = ECT_REG_SM1 + 0x06, + ECT_REG_SM1CONTR = ECT_REG_SM1 + 0x07, + ECT_REG_DCTIME0 = 0x0900, + ECT_REG_DCTIME1 = 0x0904, + ECT_REG_DCTIME2 = 0x0908, + ECT_REG_DCTIME3 = 0x090C, + ECT_REG_DCSYSTIME = 0x0910, + ECT_REG_DCSOF = 0x0918, + ECT_REG_DCSYSOFFSET = 0x0920, + ECT_REG_DCSYSDELAY = 0x0928, + ECT_REG_DCSYSDIFF = 0x092C, + ECT_REG_DCSPEEDCNT = 0x0930, + ECT_REG_DCTIMEFILT = 0x0934, + ECT_REG_DCCUC = 0x0980, + ECT_REG_DCSYNCACT = 0x0981, + ECT_REG_DCSTART0 = 0x0990, + ECT_REG_DCCYCLE0 = 0x09A0, + ECT_REG_DCCYCLE1 = 0x09A4 +}; + +/** standard SDO Sync Manager Communication Type */ +#define ECT_SDO_SMCOMMTYPE 0x1c00 +/** standard SDO PDO assignment */ +#define ECT_SDO_PDOASSIGN 0x1c10 +/** standard SDO RxPDO assignment */ +#define ECT_SDO_RXPDOASSIGN 0x1c12 +/** standard SDO TxPDO assignment */ +#define ECT_SDO_TXPDOASSIGN 0x1c13 + +/** Ethercat packet type */ +#define ETH_P_ECAT 0x88A4 + +/** Error types */ +typedef enum +{ + EC_ERR_TYPE_SDO_ERROR = 0, + EC_ERR_TYPE_EMERGENCY = 1, + EC_ERR_TYPE_PACKET_ERROR = 3, + EC_ERR_TYPE_SDOINFO_ERROR = 4, + EC_ERR_TYPE_FOE_ERROR = 5, + EC_ERR_TYPE_FOE_BUF2SMALL = 6, + EC_ERR_TYPE_FOE_PACKETNUMBER = 7, + EC_ERR_TYPE_SOE_ERROR = 8, + EC_ERR_TYPE_MBX_ERROR = 9 +} ec_err_type; + +/** Struct to retrieve errors. */ +typedef struct +{ + /** Time at which the error was generated. */ + ec_timet Time; + /** Signal bit, error set but not read */ + boolean Signal; + /** Slave number that generated the error */ + uint16 Slave; + /** CoE SDO index that generated the error */ + uint16 Index; + /** CoE SDO subindex that generated the error */ + uint8 SubIdx; + /** Type of error */ + ec_err_type Etype; + union + { + /** General abortcode */ + int32 AbortCode; + /** Specific error for Emergency mailbox */ + struct + { + uint16 ErrorCode; + uint8 ErrorReg; + uint8 b1; + uint16 w1; + uint16 w2; + }; + }; +} ec_errort; + +/** Helper macros */ +/** Macro to make a word from 2 bytes */ +#define MK_WORD(msb, lsb) ((((uint16)(msb))<<8) | (lsb)) +/** Macro to get hi byte of a word */ +#define HI_BYTE(w) ((w) >> 8) +/** Macro to get low byte of a word */ +#define LO_BYTE(w) ((w) & 0x00ff) +/** Macro to swap hi and low byte of a word */ +#define SWAP(w) ((((w)& 0xff00) >> 8) | (((w) & 0x00ff) << 8)) +/** Macro to get hi word of a dword */ +#define LO_WORD(l) ((l) & 0xffff) +/** Macro to get hi word of a dword */ +#define HI_WORD(l) ((l) >> 16) + +#define get_unaligned(ptr) \ + ({ __typeof__(*(ptr)) __tmp; memcpy(&__tmp, (ptr), sizeof(*(ptr))); __tmp; }) + +#define put_unaligned32(val, ptr) \ + (memcpy((ptr), &(val), 4)) + +#define put_unaligned64(val, ptr) \ + (memcpy((ptr), &(val), 8)) + +#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) + + #define htoes(A) (A) + #define htoel(A) (A) + #define htoell(A) (A) + #define etohs(A) (A) + #define etohl(A) (A) + #define etohll(A) (A) + +#elif !defined(EC_LITTLE_ENDIAN) && defined(EC_BIG_ENDIAN) + + #define htoes(A) ((((uint16)(A) & 0xff00) >> 8) | \ + (((uint16)(A) & 0x00ff) << 8)) + #define htoel(A) ((((uint32)(A) & 0xff000000) >> 24) | \ + (((uint32)(A) & 0x00ff0000) >> 8) | \ + (((uint32)(A) & 0x0000ff00) << 8) | \ + (((uint32)(A) & 0x000000ff) << 24)) + #define htoell(A) ((((uint64)(A) & (uint64)0xff00000000000000ULL) >> 56) | \ + (((uint64)(A) & (uint64)0x00ff000000000000ULL) >> 40) | \ + (((uint64)(A) & (uint64)0x0000ff0000000000ULL) >> 24) | \ + (((uint64)(A) & (uint64)0x000000ff00000000ULL) >> 8) | \ + (((uint64)(A) & (uint64)0x00000000ff000000ULL) << 8) | \ + (((uint64)(A) & (uint64)0x0000000000ff0000ULL) << 24) | \ + (((uint64)(A) & (uint64)0x000000000000ff00ULL) << 40) | \ + (((uint64)(A) & (uint64)0x00000000000000ffULL) << 56)) + + #define etohs htoes + #define etohl htoel + #define etohll htoell + +#else + + #error "Must define one of EC_BIG_ENDIAN or EC_LITTLE_ENDIAN" + +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* _EC_TYPE_H */ diff --git a/src/port/ethercat.h b/src/port/ethercat.h new file mode 100644 index 0000000..ac6fe26 --- /dev/null +++ b/src/port/ethercat.h @@ -0,0 +1,15 @@ +#ifndef ETHERCAT_H +#define ETHERCAT_H + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatcoe.h" +#include "ethercatfoe.h" +#include "ethercatconfig.h" +#include "ethercatprint.h" +#include "ethercatdc.h" +#include "hpm_enet_port.h" + +#endif \ No newline at end of file diff --git a/src/port/hpm_enet_port.c b/src/port/hpm_enet_port.c new file mode 100644 index 0000000..434bf53 --- /dev/null +++ b/src/port/hpm_enet_port.c @@ -0,0 +1,339 @@ +#include "hpm_enet_port.h" +#include "hpm_clock_drv.h" +#include "hpm_enet_drv.h" +#include "hpm_enet_phy_common.h" +#include "hpm_otp_drv.h" +#include "board.h" + +ATTR_PLACE_AT_NONCACHEABLE_WITH_ALIGNMENT(ENET_SOC_DESC_ADDR_ALIGNMENT) +__RW enet_rx_desc_t dma_rx_desc_tab[ENET_RX_BUFF_COUNT]; /* Ethernet Rx DMA Descriptor */ + +ATTR_PLACE_AT_NONCACHEABLE_WITH_ALIGNMENT(ENET_SOC_DESC_ADDR_ALIGNMENT) +__RW enet_tx_desc_t dma_tx_desc_tab[ENET_TX_BUFF_COUNT]; /* Ethernet Tx DMA Descriptor */ + +ATTR_PLACE_AT_NONCACHEABLE_WITH_ALIGNMENT(ENET_SOC_BUFF_ADDR_ALIGNMENT) +__RW uint8_t rx_buff[ENET_RX_BUFF_COUNT][ENET_RX_BUFF_SIZE]; /* Ethernet Receive Buffer */ + +ATTR_PLACE_AT_NONCACHEABLE_WITH_ALIGNMENT(ENET_SOC_BUFF_ADDR_ALIGNMENT) +__RW uint8_t tx_buff[ENET_TX_BUFF_COUNT][ENET_TX_BUFF_SIZE]; /* Ethernet Transmit Buffer */ + +enet_desc_t desc; +uint8_t mac[ENET_MAC]; + +ATTR_WEAK void enet_get_mac_address(uint8_t *mac) +{ + bool invalid = true; + + uint32_t uuid[(ENET_MAC + (ENET_MAC - 1)) / sizeof(uint32_t)]; + + for (int i = 0; i < ARRAY_SIZE(uuid); i++) + { + uuid[i] = otp_read_from_shadow(OTP_SOC_UUID_IDX + i); + if (uuid[i] != 0xFFFFFFFFUL && uuid[i] != 0) + { + invalid = false; + } + } + + if (invalid == true) + { + memcpy(mac, &uuid, ENET_MAC); + } + else + { + mac[0] = MAC_ADDR0; + mac[1] = MAC_ADDR1; + mac[2] = MAC_ADDR2; + mac[3] = MAC_ADDR3; + mac[4] = MAC_ADDR4; + mac[5] = MAC_ADDR5; + } +} + +/*---------------------------------------------------------------------* + * Initialization + *---------------------------------------------------------------------*/ +static hpm_stat_t enet_init(ENET_Type *ptr) +{ + enet_int_config_t int_config = {.int_enable = 0, .int_mask = 0}; + enet_mac_config_t enet_config; +#if defined(RGMII) && RGMII +#if defined(__USE_DP83867) && __USE_DP83867 + dp83867_config_t phy_config; +#else + rtl8211_config_t phy_config; +#endif +#else +#if defined(__USE_DP83848) && __USE_DP83848 + dp83848_config_t phy_config; +#else + rtl8201_config_t phy_config; +#endif +#endif + + /* Initialize td, rd and the corresponding buffers */ + memset((uint8_t *)dma_tx_desc_tab, 0x00, sizeof(dma_tx_desc_tab)); + memset((uint8_t *)dma_rx_desc_tab, 0x00, sizeof(dma_rx_desc_tab)); + memset((uint8_t *)rx_buff, 0x00, sizeof(rx_buff)); + memset((uint8_t *)tx_buff, 0x00, sizeof(tx_buff)); + + desc.tx_desc_list_head = (enet_tx_desc_t *)core_local_mem_to_sys_address(BOARD_RUNNING_CORE, (uint32_t)dma_tx_desc_tab); + desc.rx_desc_list_head = (enet_rx_desc_t *)core_local_mem_to_sys_address(BOARD_RUNNING_CORE, (uint32_t)dma_rx_desc_tab); + + desc.tx_buff_cfg.buffer = core_local_mem_to_sys_address(BOARD_RUNNING_CORE, (uint32_t)tx_buff); + desc.tx_buff_cfg.count = ENET_TX_BUFF_COUNT; + desc.tx_buff_cfg.size = ENET_TX_BUFF_SIZE; + + desc.rx_buff_cfg.buffer = core_local_mem_to_sys_address(BOARD_RUNNING_CORE, (uint32_t)rx_buff); + desc.rx_buff_cfg.count = ENET_RX_BUFF_COUNT; + desc.rx_buff_cfg.size = ENET_RX_BUFF_SIZE; + + /* Get MAC address */ + enet_get_mac_address(mac); + + /* Set mac0 address */ + enet_config.mac_addr_high[0] = mac[5] << 8 | mac[4]; + enet_config.mac_addr_low[0] = mac[3] << 24 | mac[2] << 16 | mac[1] << 8 | mac[0]; + enet_config.valid_max_count = 1; + + /* Set DMA PBL */ + enet_config.dma_pbl = board_get_enet_dma_pbl(ENET); + + /* Initialize enet controller */ + enet_controller_init(ptr, ENET_INF_TYPE, &desc, &enet_config, &int_config); + +/* Initialize phy */ +#if defined(RGMII) && RGMII +#if defined(__USE_DP83867) && __USE_DP83867 + dp83867_reset(ptr); + dp83867_basic_mode_default_config(ptr, &phy_config); + if (dp83867_basic_mode_init(ptr, &phy_config) == true) + { +#else + rtl8211_reset(ptr); + rtl8211_basic_mode_default_config(ptr, &phy_config); + if (rtl8211_basic_mode_init(ptr, &phy_config) == true) + { +#endif +#else +#if defined(__USE_DP83848) && __USE_DP83848 + dp83848_reset(ptr); + dp83848_basic_mode_default_config(ptr, &phy_config); + if (dp83848_basic_mode_init(ptr, &phy_config) == true) + { +#else + rtl8201_reset(ptr); + rtl8201_basic_mode_default_config(ptr, &phy_config); + if (rtl8201_basic_mode_init(ptr, &phy_config) == true) + { +#endif +#endif + printf("Enet phy init passes !\n"); + return status_success; + } + else + { + printf("Enet phy init fails !\n"); + return status_fail; + } +} + +void ethcat_init(void) +{ + /* Initialize GPIOs */ + board_init_enet_pins(ENET); + + /* Reset an enet PHY */ + board_reset_enet_phy(ENET); +#if defined(RGMII) && RGMII + /* Set RGMII clock delay */ + board_init_enet_rgmii_clock_delay(ENET); +#else + /* Set RMII reference clock */ + board_init_enet_rmii_reference_clock(ENET, BOARD_ENET_RMII_INT_REF_CLK); + printf("Reference Clock: %s\n", BOARD_ENET_RMII_INT_REF_CLK ? "Internal Clock" : "External Clock"); +#endif + + /* Initialize MAC and DMA */ + if (enet_init(ENET) == 0) + { + } + else + { + printf("Enet initialization fails !!!\n"); + while (1) + { + } + } +} + +int bfin_emac_init(void) +{ + ethcat_init(); + return 0; +} + +int bfin_emac_send(void *packet, int length) +{ + uint8_t *buffer; + __IO enet_tx_desc_t *dma_tx_desc; + uint16_t frame_length = 0; + uint32_t buffer_offset = 0; + uint32_t bytes_left_to_copy = 0; + uint32_t payload_offset = 0; + enet_tx_desc_t *tx_desc_list_cur = desc.tx_desc_list_cur; + + dma_tx_desc = tx_desc_list_cur; + buffer = (uint8_t *)(dma_tx_desc->tdes2_bm.buffer1); + buffer_offset = 0; + + bytes_left_to_copy = length; + payload_offset = 0; + if (dma_tx_desc->tdes0_bm.own != 0) + { + return -1; + } + /* Check if the length of data to copy is bigger than Tx buffer size*/ + while ((bytes_left_to_copy + buffer_offset) > ENET_TX_BUFF_SIZE) + { + /* Copy data to Tx buffer*/ + memcpy((uint8_t *)((uint8_t *)buffer + buffer_offset), + (uint8_t *)((uint8_t *)packet + payload_offset), + ENET_TX_BUFF_SIZE - buffer_offset); + + /* Point to next descriptor */ + dma_tx_desc = (enet_tx_desc_t *)(dma_tx_desc->tdes3_bm.next_desc); + + /* Check if the buffer is available */ + if (dma_tx_desc->tdes0_bm.own != 0) + { + return -1; + } + + buffer = (uint8_t *)(dma_tx_desc->tdes2_bm.buffer1); + + bytes_left_to_copy = bytes_left_to_copy - (ENET_TX_BUFF_SIZE - buffer_offset); + payload_offset = payload_offset + (ENET_TX_BUFF_SIZE - buffer_offset); + frame_length = frame_length + (ENET_TX_BUFF_SIZE - buffer_offset); + buffer_offset = 0; + } + + /* pass payload to buffer */ + desc.tx_desc_list_cur->tdes2_bm.buffer1 = core_local_mem_to_sys_address(BOARD_RUNNING_CORE, (uint32_t)packet); + buffer_offset = buffer_offset + bytes_left_to_copy; + frame_length = frame_length + bytes_left_to_copy; + + /* Prepare transmit descriptors to give to DMA*/ + frame_length += 4; + enet_prepare_transmission_descriptors(ENET, &desc.tx_desc_list_cur, frame_length, desc.tx_buff_cfg.size); + return 0; +} + +int bfin_emac_recv(uint8_t *packet, size_t size) +{ + uint32_t sta = 0; + uint32_t len; + uint8_t *buffer; + + enet_frame_t frame = {0, 0, 0}; + enet_rx_desc_t *dma_rx_desc; + uint32_t buffer_offset = 0; + uint32_t payload_offset = 0; + uint32_t bytes_left_to_copy = 0; + uint32_t i = 0; + /* Check and get a received frame */ +#if 0 + frame = enet_get_received_frame_interrupt(&desc.rx_desc_list_cur, &desc.rx_frame_info, ENET_RX_BUFF_COUNT); +#else + sta = enet_check_received_frame(&desc.rx_desc_list_cur, &desc.rx_frame_info); + if (1 == sta) + { + frame = enet_get_received_frame(&desc.rx_desc_list_cur, &desc.rx_frame_info); + /* Obtain the size of the packet and put it into the "len" variable. */ + len = frame.length; + buffer = (uint8_t *)frame.buffer; + if (len <= 0) + return -1; + dma_rx_desc = frame.rx_desc; + buffer_offset = 0; + + bytes_left_to_copy = size; + payload_offset = 0; + /* Check if the length of bytes to copy in current pbuf is bigger than Rx buffer size*/ + while ((bytes_left_to_copy + buffer_offset) > ENET_RX_BUFF_SIZE) + { + /* Copy data to pbuf*/ + memcpy((uint8_t *)((uint8_t *)packet + payload_offset), (uint8_t *)((uint8_t *)buffer + buffer_offset), (ENET_RX_BUFF_SIZE - buffer_offset)); + + /* Point to next descriptor */ + dma_rx_desc = (enet_rx_desc_t *)(dma_rx_desc->rdes3_bm.next_desc); + buffer = (uint8_t *)(dma_rx_desc->rdes2_bm.buffer1); + + bytes_left_to_copy = bytes_left_to_copy - (ENET_RX_BUFF_SIZE - buffer_offset); + payload_offset = payload_offset + (ENET_RX_BUFF_SIZE - buffer_offset); + buffer_offset = 0; + } + /* pass the buffer to pbuf */ + // packet = buffer; + buffer_offset = buffer_offset + bytes_left_to_copy; + memcpy((uint8_t *)((uint8_t *)packet), (uint8_t *)((uint8_t *)buffer), buffer_offset); + /* Release descriptors to DMA */ + dma_rx_desc = frame.rx_desc; + + /* Set Own bit in Rx descriptors: gives the buffers back to DMA */ + for (i = 0; i < desc.rx_frame_info.seg_count; i++) + { + dma_rx_desc->rdes0_bm.own = 1; + dma_rx_desc = (enet_rx_desc_t *)(dma_rx_desc->rdes3_bm.next_desc); + } + + /* Clear Segment_Count */ + desc.rx_frame_info.seg_count = 0; + return buffer_offset; + } +#endif + return -1; +} + +static enet_phy_status_t last_status; + +void bfin_polling_link_status(void) +{ + enet_phy_status_t status = {0}; + + enet_line_speed_t line_speed[] = {enet_line_speed_10mbps, enet_line_speed_100mbps, enet_line_speed_1000mbps}; + char *speed_str[] = {"10Mbps", "100Mbps", "1000Mbps"}; + char *duplex_str[] = {"Half duplex", "Full duplex"}; + +#if defined(RGMII) && RGMII + #if defined(__USE_DP83867) && __USE_DP83867 + dp83867_get_phy_status(ENET, &status); + #else + rtl8211_get_phy_status(ENET, &status); + #endif +#else + #if defined(__USE_DP83848) && __USE_DP83848 + dp83848_get_phy_status(ENET, &status); + #else + rtl8201_get_phy_status(ENET, &status); + #endif +#endif + + if (memcmp(&last_status, &status, sizeof(enet_phy_status_t)) != 0) { + memcpy(&last_status, &status, sizeof(enet_phy_status_t)); + if (status.enet_phy_link) { + printf("Link Status: Up\n"); + printf("Link Speed: %s\n", speed_str[status.enet_phy_speed]); + printf("Link Duplex: %s\n", duplex_str[status.enet_phy_duplex]); + enet_set_line_speed(ENET, line_speed[status.enet_phy_speed]); + enet_set_duplex_mode(ENET, status.enet_phy_duplex); + } else { + printf("Link Status: Down\n"); + } + } +} + +bool bfin_get_link_status(void) +{ + return last_status.enet_phy_link; +} diff --git a/src/port/hpm_enet_port.h b/src/port/hpm_enet_port.h new file mode 100644 index 0000000..4fcfc47 --- /dev/null +++ b/src/port/hpm_enet_port.h @@ -0,0 +1,40 @@ +#ifndef HPM_ENET_PORT_H +#define HPM_ENET_PORT_H + +#include "board.h" + +#if defined(RGMII) && RGMII +#define ENET_INF_TYPE enet_inf_rgmii +#define ENET BOARD_ENET_RGMII +#else +#define ENET_INF_TYPE enet_inf_rmii +#define ENET BOARD_ENET_RMII +#endif + +#define ENET_TX_BUFF_COUNT (50U) +#define ENET_RX_BUFF_COUNT (60U) +#define ENET_RX_BUFF_SIZE ENET_MAX_FRAME_SIZE +#define ENET_TX_BUFF_SIZE ENET_MAX_FRAME_SIZE + +/* MAC ADDRESS */ +//#define MAC_ADDR0 0x98 +//#define MAC_ADDR1 0x2C +//#define MAC_ADDR2 0xBC +//#define MAC_ADDR3 0xB1 +//#define MAC_ADDR4 0x9F +//#define MAC_ADDR5 0x17 + +#define MAC_ADDR0 0x00 +#define MAC_ADDR1 0x80 +#define MAC_ADDR2 0xE1 +#define MAC_ADDR3 0x00 +#define MAC_ADDR4 0x00 +#define MAC_ADDR5 0x00 + +int bfin_emac_init(void); +int bfin_emac_send (void *packet, int length); +int bfin_emac_recv (uint8_t * packet, size_t size); +void bfin_polling_link_status(void); +bool bfin_get_link_status(void); + +#endif diff --git a/src/port/nicdrv.c b/src/port/nicdrv.c new file mode 100644 index 0000000..2570deb --- /dev/null +++ b/src/port/nicdrv.c @@ -0,0 +1,632 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : nicdrv.c + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets allways return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is recieved. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resend through interface B. + * This layer is fully transparent for the higher layers. + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "hpm_enet_port.h" + +#ifndef MAX +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connecetion */ + ECT_RED_DOUBLE +}; + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to fund out the packet flow in redundant + * confihurations. */ +const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary) +{ + int i; + + if (secondary) + { + /* secondary port stuct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.tempbuf = &(port->redport->tempinbuf); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + return 0; + } + } + else + { + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + } + + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + + return 1; +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is allways broadcast. + * Ethertype is allways ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = (ec_etherheadert *)p; + bp->da0 = htons(0xffff); + bp->da1 = htons(0xffff); + bp->da2 = htons(0xffff); + bp->sa0 = htons(priMAC[0]); + bp->sa1 = htons(priMAC[1]); + bp->sa2 = htons(priMAC[2]); + bp->etype = htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +int ecx_getindex(ecx_portt *port) +{ + int idx; + int cnt; + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + { + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + } + port->lastidx = idx; + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, int idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + { + port->redport->rxbufstat[idx] = bufstat; + } +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, int idx, int stacknumber) +{ + int lp, rval; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + (*stack->rxbufstat)[idx] = EC_BUF_TX; + rval = bfin_emac_send((*stack->txbuf)[idx], lp); + + return rval; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, int idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = htons(secMAC[1]); + /* transmit over secondary socket */ + //send(sockhandle2, &ec_txbuf2, ec_txbuflength2 , 0); + // OBS! redundant not ACTIVE for BFIN, just added to compile + bfin_emac_send(&(port->txbuf2), port->txbuflength2); + //ecx_outframe(port, idx, 1); + port->redport->rxbufstat[idx] = EC_BUF_TX; + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; +if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = sizeof(port->tempinbuf); + bytesrx = bfin_emac_recv((*stack->tempbuf), lp); + port->tempinbufs = bytesrx; + + return (bytesrx > 0); +} +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retreives it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, int idx, int stacknumber) +{ + uint16 l; + uint16 temp; + int rval; + int idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + if ((idx < EC_MAXBUF) && ((*stack->rxbufstat)[idx] == EC_BUF_RCVD)) + { + l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + } + else + { + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(stack->tempbuf); + /* check if it is an EtherCAT frame */ + if (ehp->etype == htons(ETH_P_ECAT)) + { + ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals reqested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE); + /* return WKC */ + l=(*stack->txbuflength)[idx] - ETH_HEADERSIZE; + rval = ((*rxbuf)[l-2] + ((uint16)((*rxbuf)[l-1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = etohs(ehp->sa1); + } + else + { + /* check if index exist? */ + if (idxf < EC_MAXBUF) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = etohs(ehp->sa1); + (*stack->rxbufstat)[idxf] = EC_BUF_EMPTY; + } + else + { + /* strange things happend */ + } + } + } + } + } + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] tvs = timeout + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, int idx, osal_timert *timer) +{ + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + { + wkc2 = 0; + } + + do + { + /* only read frame if not already in */ + if (wkc <= EC_NOFRAME) + { + wkc = ecx_inframe(port, idx, 0); + } + /* only try secondary if in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* only read frame if not already in */ + if (wkc2 <= EC_NOFRAME) + wkc2 = ecx_inframe(port, idx, 1); + } + /* wait for both frames to arrive or timeout */ + }while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && (osal_timer_is_expired(timer) == FALSE)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the reveived MAC source on primary socket */ + primrx = 0xffff; + if (wkc > EC_NOFRAME) + { + primrx = port->rxsa[idx]; + } + /* secrx if the reveived MAC source on psecondary socket */ + secrx = 0xffff; + if (wkc2 > EC_NOFRAME) + { + secrx = port->redport->rxsa[idx]; + } + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + + if ( ((primrx == 0xffff) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + osal_timert read_timer; + + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + + osal_timer_start(&read_timer, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && (osal_timer_is_expired(&read_timer) == FALSE)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, int idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + /* if nothing received, clear buffer index status so it can be used again */ + if (wkc <= EC_NOFRAME) + { + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + } + + return wkc; +} + +/** Blocking send and recieve frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, int idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer; + + osal_timer_start(&timer, timeout); + do + { + osal_timert read_timer; + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + osal_timer_start(&read_timer, MIN(timeout, EC_TIMEOUTRET)); + wkc = ecx_waitinframe_red(port, idx, &read_timer); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && (osal_timer_is_expired(&timer) == FALSE)); + /* if nothing received, clear buffer index status so it can be used again */ + if (wkc <= EC_NOFRAME) + { + ec_setbufstat(idx, EC_BUF_EMPTY); + } + return wkc; +} + + +#ifdef EC_VER1 +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +int ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(int idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(int idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(int idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(int idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(int idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(int idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} +#endif diff --git a/src/port/nicdrv.h b/src/port/nicdrv.h new file mode 100644 index 0000000..ed19110 --- /dev/null +++ b/src/port/nicdrv.h @@ -0,0 +1,150 @@ +/* + * Simple Open EtherCAT Master Library + * + * File : nicdrv.h + * Version : 1.3.1 + * Date : 11-03-2015 + * Copyright (C) 2005-2015 Speciaal Machinefabriek Ketels v.o.f. + * Copyright (C) 2005-2015 Arthur Ketels + * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven + * Copyright (C) 2014-2015 rt-labs AB , Sweden + * + * SOEM is free software; you can redistribute it and/or modify it under + * the terms of the GNU General Public License version 2 as published by the Free + * Software Foundation. + * + * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * As a special exception, if other files instantiate templates or use macros + * or inline functions from this file, or you compile this file and link it + * with other works to produce a work based on this file, this file does not + * by itself cause the resulting work to be covered by the GNU General Public + * License. However the source code for this file must still be made available + * in accordance with section (3) of the GNU General Public License. + * + * This exception does not invalidate any other reasons why a work based on + * this file might be covered by the GNU General Public License. + * + * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual + * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for + * the sole purpose of creating, using and/or selling or otherwise distributing + * an EtherCAT network master provided that an EtherCAT Master License is obtained + * from Beckhoff Automation GmbH. + * + * In case you did not receive a copy of the EtherCAT Master License along with + * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany + * (www.beckhoff.com). + */ + +/** \file + * \brief + * Headerfile for nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define HAVE_REMOTE +#include "hpm_common.h" + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt ; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lenghts */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + int lastidx; + /** current redundancy state */ + int redstate; + ecx_redportt *redport; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; +#ifdef EC_VER1 +extern ATTR_PLACE_AT_WITH_ALIGNMENT(".fast_ram", 4) ecx_portt ecx_port; +extern ATTR_PLACE_AT_WITH_ALIGNMENT(".fast_ram", 4) ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(int idx, int bufstat); +int ec_getindex(void); +int ec_outframe(int idx, int stacknumber); +int ec_outframe_red(int idx); +int ec_waitinframe(int idx, int timeout); +int ec_srconfirm(int idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); +int ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, int idx, int stacknumber); +int ecx_outframe_red(ecx_portt *port, int idx); +int ecx_waitinframe(ecx_portt *port, int idx, int timeout); +int ecx_srconfirm(ecx_portt *port, int idx,int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/port/osal.c b/src/port/osal.c new file mode 100644 index 0000000..2634b34 --- /dev/null +++ b/src/port/osal.c @@ -0,0 +1,121 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include "hpm_clock_drv.h" +#include "hpm_csr_drv.h" + +#define timercmp(a, b, CMP) \ + (((a)->tv_sec == (b)->tv_sec) ? \ + ((a)->tv_usec CMP(b)->tv_usec) : \ + ((a)->tv_sec CMP(b)->tv_sec)) +#define timeradd(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \ + if ((result)->tv_usec >= 1000000) { \ + ++(result)->tv_sec; \ + (result)->tv_usec -= 1000000; \ + } \ + } while (0) +#define timersub(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ + if ((result)->tv_usec < 0) { \ + --(result)->tv_sec; \ + (result)->tv_usec += 1000000; \ + } \ + } while (0) + +#define CFG_TICKS_PER_SECOND 1000000 +#define USECS_PER_SEC 1000000 +#define USECS_PER_TICK (USECS_PER_SEC / CFG_TICKS_PER_SECOND) + +int64_t board_get_us() +{ + return (hpm_csr_get_core_mcycle() / (clock_get_frequency(clock_cpu0) / 1000000)); +} + +int osal_gettimeofday(struct timeval *tp) +{ + uint64_t tick = board_get_us(); + uint64_t ticks_left; + + tp->tv_sec = tick / CFG_TICKS_PER_SECOND; + + ticks_left = tick % CFG_TICKS_PER_SECOND; + tp->tv_usec = ticks_left * USECS_PER_TICK; + + return 0; +} + +int osal_usleep(uint32 usec) +{ + board_delay_us(usec); + return 1; +} + +ec_timet osal_current_time(void) +{ + struct timeval current_time; + ec_timet return_value; + + osal_gettimeofday(¤t_time); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_timer_start(osal_timert *self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_gettimeofday(&start_time); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd(&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired(osal_timert *self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_gettimeofday(¤t_time); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp(¤t_time, &stop_time, <); + + return is_not_yet_expired == false; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + void (*thread_func)(void *) = func; + + thread_func(param); + return 1; +} \ No newline at end of file diff --git a/src/port/osal.h b/src/port/osal.h new file mode 100644 index 0000000..6715f37 --- /dev/null +++ b/src/port/osal.h @@ -0,0 +1,78 @@ +/****************************************************************************** + * * *** *** + * *** *** *** + * *** **** ********** *** ***** *** **** ***** + * ********* ********** *** ********* ************ ********* + * **** *** *** *** *** **** *** + * *** *** ****** *** *********** *** **** ***** + * *** *** ****** *** ************* *** **** ***** + * *** **** **** *** *** *** **** *** + * *** ******* ***** ************** ************* ********* + * *** ***** *** ******* ** ** ****** ***** + * t h e r e a l t i m e t a r g e t e x p e r t s + * + * http://www.rt-labs.com + * Copyright (C) 2009. rt-labs AB, Sweden. All rights reserved. + *------------------------------------------------------------------------------ + * $Id: osal.h 473 2013-04-08 11:43:02Z rtlaka $ + *------------------------------------------------------------------------------ + */ + +#ifndef _osal_ +#define _osal_ + +#include + +/* General types */ +typedef uint8_t boolean; +#define TRUE 1 +#define FALSE 0 +typedef int8_t int8; +typedef int16_t int16; +typedef int32_t int32; +typedef uint8_t uint8; +typedef uint16_t uint16; +typedef uint32_t uint32; +typedef int64_t int64; +typedef uint64_t uint64; +typedef float float32; +typedef double float64; + +// define if debug printf is needed +//#define EC_DEBUG + +#ifdef EC_DEBUG +#define EC_PRINT printf +#else +#define EC_PRINT(...) do {} while (0) +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +#define OSAL_THREAD_HANDLE void * +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +typedef struct +{ + uint32 sec; /*< Seconds elapsed since the Epoch (Jan 1, 1970) */ + int32 usec; /*< Microseconds elapsed since last second boundary */ +} ec_timet; + +typedef struct osal_timer +{ + ec_timet stop_time; + uint64_t start_time; +} osal_timert; + +void osal_timer_start(osal_timert * self, uint32 timeout_us); +boolean osal_timer_is_expired(osal_timert * self); +int osal_usleep(uint32 usec); +ec_timet osal_current_time(void); +int osal_thread_create(void *thandle, int stacksize, void *func, void *param); + +#endif diff --git a/src/port/oshw.c b/src/port/oshw.c new file mode 100644 index 0000000..98a26e7 --- /dev/null +++ b/src/port/oshw.c @@ -0,0 +1,53 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include "oshw.h" +#include +#include + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons(const uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs(const uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/* Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters(void) +{ + ec_adaptert * ret_adapter = NULL; + + /* TODO if needed */ + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert * adapter) +{ + /* TODO if needed */ +} diff --git a/src/port/oshw.h b/src/port/oshw.h new file mode 100644 index 0000000..134a5c7 --- /dev/null +++ b/src/port/oshw.h @@ -0,0 +1,40 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Headerfile for oshw.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +#define htons(A) ((((uint16)(A) & 0xff00) >> 8) | \ + (((uint16)(A) & 0x00ff) << 8)) +#define htonl(A) ((((uint32)(A) & 0xff000000) >> 24) | \ + (((uint32)(A) & 0x00ff0000) >> 8) | \ + (((uint32)(A) & 0x0000ff00) << 8) | \ + (((uint32)(A) & 0x000000ff) << 24)) + +uint16 oshw_htons(uint16 host); +uint16 oshw_ntohs(uint16 network); + +ec_adaptert * oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif