From 86a2584e474372611b7e292df33e5e0d15896725 Mon Sep 17 00:00:00 2001 From: nakarlsson Date: Thu, 31 Jan 2019 14:57:16 +0100 Subject: [PATCH] Impelmented EoE filter function in mailbox receive (#244) --- soem/ethercat.h | 1 + soem/ethercateoe.c | 628 +++++++++++++++++++++++++++++++++ soem/ethercateoe.h | 206 +++++++++++ soem/ethercatmain.c | 28 +- soem/ethercatmain.h | 7 +- soem/ethercattype.h | 21 +- test/linux/eoe_test/eoe_test.c | 420 ++++++++++++++++++++++ 7 files changed, 1294 insertions(+), 17 deletions(-) create mode 100644 soem/ethercateoe.c create mode 100644 soem/ethercateoe.h create mode 100644 test/linux/eoe_test/eoe_test.c diff --git a/soem/ethercat.h b/soem/ethercat.h index ee92358..d87d748 100644 --- a/soem/ethercat.h +++ b/soem/ethercat.h @@ -19,6 +19,7 @@ #include "ethercatcoe.h" #include "ethercatfoe.h" #include "ethercatsoe.h" +#include "ethercateoe.h" #include "ethercatconfig.h" #include "ethercatprint.h" diff --git a/soem/ethercateoe.c b/soem/ethercateoe.c new file mode 100644 index 0000000..043dcbb --- /dev/null +++ b/soem/ethercateoe.c @@ -0,0 +1,628 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * Ethernet over EtherCAT (EoE) module. + * + * Set / Get IP functions + * Blocking send/receive Ethernet Frame + * Read incoming EoE fragment to Ethernet Frame + */ + +#include +#include +#include "osal.h" +#include "oshw.h" +#include "ethercat.h" + + /** EoE utility function to convert uint32 to eoe ip bytes. + * @param[in] ip = ip in uint32 + * @param[out] byte_ip = eoe ip 4th octet, 3ed octet, 2nd octet, 1st octet + */ +static void EOE_ip_uint32_to_byte(eoe_ip4_addr_t * ip, uint8_t * byte_ip) +{ + byte_ip[3] = eoe_ip4_addr1(ip); /* 1st octet */ + byte_ip[2] = eoe_ip4_addr2(ip); /* 2nd octet */ + byte_ip[1] = eoe_ip4_addr3(ip); /* 3ed octet */ + byte_ip[0] = eoe_ip4_addr4(ip); /* 4th octet */ +} + +/** EoE utility function to convert eoe ip bytes to uint32. +* @param[in] byte_ip = eoe ip 4th octet, 3ed octet, 2nd octet, 1st octet +* @param[out] ip = ip in uint32 +*/ +static void EOE_ip_byte_to_uint32(uint8_t * byte_ip, eoe_ip4_addr_t * ip) +{ + EOE_IP4_ADDR_TO_U32(ip, + byte_ip[3], /* 1st octet */ + byte_ip[2], /* 2nd octet */ + byte_ip[1], /* 3ed octet */ + byte_ip[0]); /* 4th octet */ +} + +/** EoE fragment data handler hook. Should not block. +* +* @param[in] context = context struct +* @param[in] hook = Pointer to hook function. +* @return 1 +*/ +int ecx_EOEdefinehook(ecx_contextt *context, void *hook) +{ + context->EOEhook = hook; + return 1; +} + +/** EoE EOE set IP, blocking. Waits for response from the slave. +* +* @param[in] context = Context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[in] ipparam = IP parameter data to be sent +* @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave response or returned result code +*/ +int ecx_EOEsetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout) +{ + ec_EOEt *EOEp, *aEOEp; + ec_mbxbuft MbxIn, MbxOut; + uint16 frameinfo1, result; + uint8 cnt, data_offset; + uint8 flags = 0; + int wkc; + + 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); + aEOEp = (ec_EOEt *)&MbxIn; + EOEp = (ec_EOEt *)&MbxOut; + EOEp->mbxheader.address = htoes(0x0000); + EOEp->mbxheader.priority = 0x00; + data_offset = EOE_PARAM_OFFSET; + + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + + EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */ + + EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_INIT_REQ) | + EOE_HDR_FRAME_PORT_SET(port) | + EOE_HDR_LAST_FRAGMENT); + EOEp->frameinfo2 = 0; + + /* The EoE frame will include "empty" IP/DNS entries, makes wireshark happy. + * Specification say they are optional, TwinCAT include empty entries. + */ + if (ipparam->mac_set) + { + flags |= EOE_PARAM_MAC_INCLUDE; + memcpy(&EOEp->data[data_offset], ipparam->mac.addr, EOE_ETHADDR_LENGTH); + } + data_offset += EOE_ETHADDR_LENGTH; + if (ipparam->ip_set) + { + flags |= EOE_PARAM_IP_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->ip, &EOEp->data[data_offset]); + } + data_offset += 4; + if (ipparam->subnet_set) + { + flags |= EOE_PARAM_SUBNET_IP_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->subnet, &EOEp->data[data_offset]); + } + data_offset += 4; + if (ipparam->default_gateway_set) + { + flags |= EOE_PARAM_DEFAULT_GATEWAY_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->default_gateway, &EOEp->data[data_offset]); + } + data_offset += 4; + if (ipparam->dns_ip_set) + { + flags |= EOE_PARAM_DNS_IP_INCLUDE; + EOE_ip_uint32_to_byte(&ipparam->dns_ip, &EOEp->data[data_offset]); + } + data_offset += 4; + if (ipparam->dns_name_set) + { + flags |= EOE_PARAM_DNS_NAME_INCLUDE; + memcpy(&EOEp->data[data_offset], (void *)ipparam->dns_name, EOE_DNS_NAME_LENGTH); + } + data_offset += EOE_DNS_NAME_LENGTH; + + EOEp->mbxheader.length = htoes(EOE_PARAM_OFFSET + data_offset); + EOEp->data[0] = flags; + + /* send EoE 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 FoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + frameinfo1 = etohs(aEOEp->frameinfo1); + result = etohs(aEOEp->result); + if ((EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_INIT_RESP) || + (result != EOE_RESULT_SUCCESS)) + { + wkc = -result; + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } + return wkc; +} + +/** EoE EOE get IP, blocking. Waits for response from the slave. +* +* @param[in] context = Context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[out] ipparam = IP parameter data retrived from slave +* @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave response or returned result code +*/ +int ecx_EOEgetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout) +{ + ec_EOEt *EOEp, *aEOEp; + ec_mbxbuft MbxIn, MbxOut; + uint16 frameinfo1, eoedatasize; + uint8 cnt, data_offset; + uint8 flags = 0; + int wkc; + + /* Empty slave out mailbox if something is in. Timout set to 0 */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); + ec_clearmbx(&MbxOut); + aEOEp = (ec_EOEt *)&MbxIn; + EOEp = (ec_EOEt *)&MbxOut; + EOEp->mbxheader.address = htoes(0x0000); + EOEp->mbxheader.priority = 0x00; + data_offset = EOE_PARAM_OFFSET; + + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + + EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */ + + EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_GET_IP_PARAM_REQ) | + EOE_HDR_FRAME_PORT_SET(port) | + EOE_HDR_LAST_FRAGMENT); + EOEp->frameinfo2 = 0; + + EOEp->mbxheader.length = htoes(0x0004); + EOEp->data[0] = flags; + + /* send EoE 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 FoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + frameinfo1 = etohs(aEOEp->frameinfo1); + eoedatasize = etohs(aEOEp->mbxheader.length) - 0x0004; + if (EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_GET_IP_PARAM_RESP) + { + wkc = -EOE_RESULT_UNSUPPORTED_FRAME_TYPE; + } + else + { + /* The EoE frame will include "empty" IP/DNS entries, makes + * wireshark happy. Specification say they are optional, TwinCAT + * include empty entries. + */ + flags = aEOEp->data[0]; + if (flags & EOE_PARAM_MAC_INCLUDE) + { + memcpy(ipparam->mac.addr, + &aEOEp->data[data_offset], + EOE_ETHADDR_LENGTH); + ipparam->mac_set = 1; + } + data_offset += EOE_ETHADDR_LENGTH; + if (flags & EOE_PARAM_IP_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->ip); + ipparam->ip_set = 1; + } + data_offset += 4; + if (flags & EOE_PARAM_SUBNET_IP_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->subnet); + ipparam->subnet_set = 1; + } + data_offset += 4; + if (flags & EOE_PARAM_DEFAULT_GATEWAY_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->default_gateway); + ipparam->default_gateway_set = 1; + } + data_offset += 4; + if (flags & EOE_PARAM_DNS_IP_INCLUDE) + { + EOE_ip_byte_to_uint32(&aEOEp->data[data_offset], + &ipparam->dns_ip); + ipparam->dns_ip_set = 1; + } + data_offset += 4; + if (flags & EOE_PARAM_DNS_NAME_INCLUDE) + { + uint16_t dns_len; + if ((eoedatasize - data_offset) < EOE_DNS_NAME_LENGTH) + { + dns_len = (eoedatasize - data_offset); + } + else + { + dns_len = EOE_DNS_NAME_LENGTH; + } + /* Assume ZERO terminated string */ + memcpy(ipparam->dns_name, &aEOEp->data[data_offset], dns_len); + ipparam->dns_name_set = 1; + } + data_offset += EOE_DNS_NAME_LENGTH; + /* Something os not correct, flag the error */ + if(data_offset > eoedatasize) + { + wkc = -EC_ERR_TYPE_MBX_ERROR; + } + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + } + return wkc; +} + +/** EoE ethernet buffer write, blocking. +* +* If the buffer is larger than the mailbox size then the buffer is sent in +* several fragments. The function will split the buf data in fragments and +* send them to the slave one by one. +* +* @param[in] context = context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[in] psize = Size in bytes of parameter buffer. +* @param[in] p = Pointer to parameter buffer +* @param[in] Timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave transmission +*/ +int ecx_EOEsend(ecx_contextt *context, uint16 slave, uint8 port, int psize, void *p, int timeout) +{ + ec_EOEt *EOEp; + ec_mbxbuft MbxOut; + uint16 frameinfo1, frameinfo2; + uint16 txframesize, txframeoffset; + uint8 cnt, txfragmentno; + boolean NotLast; + int wkc, maxdata; + const uint8 * buf = p; + static uint8_t txframeno = 0; + + ec_clearmbx(&MbxOut); + EOEp = (ec_EOEt *)&MbxOut; + EOEp->mbxheader.address = htoes(0x0000); + EOEp->mbxheader.priority = 0x00; + /* data section=mailbox size - 6 mbx - 4 EoEh */ + maxdata = context->slavelist[slave].mbx_l - 0x0A; + txframesize = psize; + txfragmentno = 0; + txframeoffset = 0; + NotLast = TRUE; + + do + { + txframesize = psize - txframeoffset; + if (txframesize > maxdata) + { + /* Adjust to even 32-octect blocks */ + txframesize = ((maxdata >> 5) << 5); + } + + if (txframesize == (psize - txframeoffset)) + { + frameinfo1 = (EOE_HDR_LAST_FRAGMENT_SET(1) | EOE_HDR_FRAME_PORT_SET(port)); + NotLast = FALSE; + } + else + { + frameinfo1 = EOE_HDR_FRAME_PORT_SET(port); + } + + frameinfo2 = EOE_HDR_FRAG_NO_SET(txfragmentno); + if (txfragmentno > 0) + { + frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET((txframeoffset >> 5))); + } + else + { + frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET(((psize + 31) >> 5))); + txframeno++; + } + frameinfo2 = frameinfo2 | EOE_HDR_FRAME_NO_SET(txframeno); + + /* get new mailbox count value, used as session handle */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + + EOEp->mbxheader.length = htoes(4 + txframesize); /* no timestamp */ + EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */ + + EOEp->frameinfo1 = htoes(frameinfo1); + EOEp->frameinfo2 = htoes(frameinfo2); + + memcpy(EOEp->data, &buf[txframeoffset], txframesize); + + /* send EoE request to slave */ + wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); + if ((NotLast == TRUE) && (wkc > 0)) + { + txframeoffset += txframesize; + txfragmentno++; + } + } while ((NotLast == TRUE) && (wkc > 0)); + + return wkc; +} + + +/** EoE ethernet buffer read, blocking. +* +* If the buffer is larger than the mailbox size then the buffer is received +* by several fragments. The function will assamble the fragments into +* a complete Ethernet buffer. +* +* @param[in] context = context struct +* @param[in] slave = Slave number +* @param[in] port = Port number on slave if applicable +* @param[in/out] psize = Size in bytes of parameter buffer. +* @param[in] p = Pointer to parameter buffer +* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM +* @return Workcounter from last slave response or error code +*/ +int ecx_EOErecv(ecx_contextt *context, uint16 slave, uint8 port, int * psize, void *p, int timeout) +{ + ec_EOEt *aEOEp; + ec_mbxbuft MbxIn; + uint16 frameinfo1, frameinfo2, rxframesize, rxframeoffset, eoedatasize; + uint8 rxfragmentno, rxframeno; + boolean NotLast; + int wkc, buffersize; + uint8 * buf = p; + + ec_clearmbx(&MbxIn); + aEOEp = (ec_EOEt *)&MbxIn; + NotLast = TRUE; + buffersize = *psize; + rxfragmentno = 0; + + /* Hang for a while if nothing is in */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + + while ((wkc > 0) && (NotLast == TRUE)) + { + /* slave response should be FoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + + eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004; + frameinfo1 = etohs(aEOEp->frameinfo1); + frameinfo2 = etohs(aEOEp->frameinfo2); + + if (rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2)) + { + if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + } + + if (rxfragmentno == 0) + { + rxframesize = (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5); + rxframeoffset = 0; + rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2); + } + else + { + if (rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2)) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + else if (rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5)) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + /* Exit here*/ + break; + } + } + + if ((rxframeoffset + eoedatasize) <= buffersize) + { + memcpy(&buf[rxframeoffset], aEOEp->data, eoedatasize); + rxframeoffset += eoedatasize; + rxfragmentno++; + } + + if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1)) + { + /* Remove timestamp */ + if (EOE_HDR_TIME_APPEND_GET(frameinfo1)) + { + rxframeoffset -= 4; + } + NotLast = FALSE; + *psize = rxframeoffset; + } + else + { + /* Hang for a while if nothing is in */ + wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + } + return wkc; +} + +/** EoE mailbox fragment read +* +* Will take the data in incoming mailbox buffer and copy to destination +* Ethernet frame buffer at given offset and update current fragment variables +* +* @param[in] MbxIn = Received mailbox containing fragment data +* @param[in/out] rxfragmentno = Fragment number +* @param[in/out] rxframesize = Frame size +* @param[in/out] rxframeoffset = Frame offset +* @param[in/out] rxframeno = Frame number +* @param[in/out] psize = Size in bytes of frame buffer. +* @param[out] p = Pointer to frame buffer +* @return 0= if fragment OK, >0 if last fragment, <0 on error +*/ +int ecx_EOEreadfragment( + ec_mbxbuft * MbxIn, + uint8 * rxfragmentno, + uint16 * rxframesize, + uint16 * rxframeoffset, + uint16 * rxframeno, + int * psize, + void *p) +{ + uint16 frameinfo1, frameinfo2, eoedatasize; + int wkc; + ec_EOEt * aEOEp; + uint8 * buf; + + aEOEp = (ec_EOEt *)MbxIn; + buf = p; + wkc = 0; + + /* slave response should be EoE */ + if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE) + { + eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004; + frameinfo1 = etohs(aEOEp->frameinfo1); + frameinfo2 = etohs(aEOEp->frameinfo2); + + /* Retrive fragment number, is it what we expect? */ + if (*rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2)) + { + /* If expected fragment number is not 0, reset working variables */ + if (*rxfragmentno != 0) + { + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + } + + /* If incoming fragment number is not 0 we can't recover, exit */ + if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0) + { + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + return wkc; + } + } + + /* Is it a new frame?*/ + if (*rxfragmentno == 0) + { + *rxframesize = (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5); + *rxframeoffset = 0; + *rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2); + } + else + { + /* If we're inside a frame, make sure it is the same */ + if (*rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2)) + { + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + return wkc; + } + else if (*rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5)) + { + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA; + return wkc; + } + } + + /* Make sure we're inside expected frame size */ + if (((*rxframeoffset + eoedatasize) <= *rxframesize) && + ((*rxframeoffset + eoedatasize) <= *psize)) + { + memcpy(&buf[*rxframeoffset], aEOEp->data, eoedatasize); + *rxframeoffset += eoedatasize; + *rxfragmentno += 1; + } + + /* Is it the last fragment */ + if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1)) + { + /* Remove timestamp */ + if (EOE_HDR_TIME_APPEND_GET(frameinfo1)) + { + *rxframeoffset -= 4; + } + *psize = *rxframeoffset; + *rxfragmentno = 0; + *rxframesize = 0; + *rxframeoffset = 0; + *rxframeno = 0; + wkc = 1; + } + } + else + { + /* unexpected mailbox received */ + wkc = -EC_ERR_TYPE_PACKET_ERROR; + } + return wkc; +} diff --git a/soem/ethercateoe.h b/soem/ethercateoe.h new file mode 100644 index 0000000..b517398 --- /dev/null +++ b/soem/ethercateoe.h @@ -0,0 +1,206 @@ +/* + * 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 ethercatfoe.c + */ + +#ifndef _ethercateoe_ +#define _ethercateoe_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/** DNS length according to ETG 1000.6 */ +#define EOE_DNS_NAME_LENGTH 32 +/** Ethernet address length not including VLAN */ +#define EOE_ETHADDR_LENGTH 6 + +#define EOE_MAKEU32(a,b,c,d) (((uint32_t)((a) & 0xff) << 24) | \ + ((uint32_t)((b) & 0xff) << 16) | \ + ((uint32_t)((c) & 0xff) << 8) | \ + (uint32_t)((d) & 0xff)) + +#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) + +#define EOE_HTONS(x) ((((x) & 0x00ffUL) << 8) | (((x) & 0xff00UL) >> 8)) +#define EOE_NTOHS(x) EOE_HTONS(x) +#define EOE_HTONL(x) ((((x) & 0x000000ffUL) << 24) | \ + (((x) & 0x0000ff00UL) << 8) | \ + (((x) & 0x00ff0000UL) >> 8) | \ + (((x) & 0xff000000UL) >> 24)) +#define EOE_NTOHL(x) EOE_HTONL(x) +#else +#define EOE_HTONS(x) (x) +#define EOE_NTOHS(x) (x) +#define EOE_HTONL(x) (x) +#define EOE_NTOHL(x) (x) +#endif /* !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) */ + +/** Get one byte from the 4-byte address */ +#define eoe_ip4_addr1(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[0]) +#define eoe_ip4_addr2(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[1]) +#define eoe_ip4_addr3(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[2]) +#define eoe_ip4_addr4(ipaddr) (((const uint8_t*)(&(ipaddr)->addr))[3]) + +/** Set an IP address given by the four byte-parts */ +#define EOE_IP4_ADDR_TO_U32(ipaddr,a,b,c,d) \ + (ipaddr)->addr = EOE_HTONL(EOE_MAKEU32(a,b,c,d)) + +/** Header frame info 1 */ +#define EOE_HDR_FRAME_TYPE_OFFSET 0 +#define EOE_HDR_FRAME_TYPE (0xF << 0) +#define EOE_HDR_FRAME_TYPE_SET(x) (((x) & 0xF) << 0) +#define EOE_HDR_FRAME_TYPE_GET(x) (((x) >> 0) & 0xF) +#define EOE_HDR_FRAME_PORT_OFFSET 4 +#define EOE_HDR_FRAME_PORT (0xF << 4) +#define EOE_HDR_FRAME_PORT_SET(x) (((x) & 0xF) << 4) +#define EOE_HDR_FRAME_PORT_GET(x) (((x) >> 4) & 0xF) +#define EOE_HDR_LAST_FRAGMENT_OFFSET 8 +#define EOE_HDR_LAST_FRAGMENT (0x1 << 8) +#define EOE_HDR_LAST_FRAGMENT_SET(x) (((x) & 0x1) << 8) +#define EOE_HDR_LAST_FRAGMENT_GET(x) (((x) >> 8) & 0x1) +#define EOE_HDR_TIME_APPEND_OFFSET 9 +#define EOE_HDR_TIME_APPEND (0x1 << 9) +#define EOE_HDR_TIME_APPEND_SET(x) (((x) & 0x1) << 9) +#define EOE_HDR_TIME_APPEND_GET(x) (((x) >> 9) & 0x1) +#define EOE_HDR_TIME_REQUEST_OFFSET 10 +#define EOE_HDR_TIME_REQUEST (0x1 << 10) +#define EOE_HDR_TIME_REQUEST_SET(x) (((x) & 0x1) << 10) +#define EOE_HDR_TIME_REQUEST_GET(x) (((x) >> 10) & 0x1) + +/** Header frame info 2 */ +#define EOE_HDR_FRAG_NO_OFFSET 0 +#define EOE_HDR_FRAG_NO (0x3F << 0) +#define EOE_HDR_FRAG_NO_SET(x) (((x) & 0x3F) << 0) +#define EOE_HDR_FRAG_NO_GET(x) (((x) >> 0) & 0x3F) +#define EOE_HDR_FRAME_OFFSET_OFFSET 6 +#define EOE_HDR_FRAME_OFFSET (0x3F << 6) +#define EOE_HDR_FRAME_OFFSET_SET(x) (((x) & 0x3F) << 6) +#define EOE_HDR_FRAME_OFFSET_GET(x) (((x) >> 6) & 0x3F) +#define EOE_HDR_FRAME_NO_OFFSET 12 +#define EOE_HDR_FRAME_NO (0xF << 12) +#define EOE_HDR_FRAME_NO_SET(x) (((x) & 0xF) << 12) +#define EOE_HDR_FRAME_NO_GET(x) (((x) >> 12) & 0xF) + +/** EOE param */ +#define EOE_PARAM_OFFSET 4 +#define EOE_PARAM_MAC_INCLUDE (0x1 << 0) +#define EOE_PARAM_IP_INCLUDE (0x1 << 1) +#define EOE_PARAM_SUBNET_IP_INCLUDE (0x1 << 2) +#define EOE_PARAM_DEFAULT_GATEWAY_INCLUDE (0x1 << 3) +#define EOE_PARAM_DNS_IP_INCLUDE (0x1 << 4) +#define EOE_PARAM_DNS_NAME_INCLUDE (0x1 << 5) + +/** EoE frame types */ +#define EOE_FRAG_DATA 0 +#define EOE_INIT_RESP_TIMESTAMP 1 +#define EOE_INIT_REQ 2 /* Spec SET IP REQ */ +#define EOE_INIT_RESP 3 /* Spec SET IP RESP */ +#define EOE_SET_ADDR_FILTER_REQ 4 +#define EOE_SET_ADDR_FILTER_RESP 5 +#define EOE_GET_IP_PARAM_REQ 6 +#define EOE_GET_IP_PARAM_RESP 7 +#define EOE_GET_ADDR_FILTER_REQ 8 +#define EOE_GET_ADDR_FILTER_RESP 9 + +/** EoE parameter result codes */ +#define EOE_RESULT_SUCCESS 0x0000 +#define EOE_RESULT_UNSPECIFIED_ERROR 0x0001 +#define EOE_RESULT_UNSUPPORTED_FRAME_TYPE 0x0002 +#define EOE_RESULT_NO_IP_SUPPORT 0x0201 +#define EOE_RESULT_NO_DHCP_SUPPORT 0x0202 +#define EOE_RESULT_NO_FILTER_SUPPORT 0x0401 + + +/** EOE ip4 address in network order */ +typedef struct eoe_ip4_addr { + uint32_t addr; +}eoe_ip4_addr_t; + +/** EOE ethernet address */ +PACKED_BEGIN +typedef struct PACKED eoe_ethaddr +{ + uint8_t addr[EOE_ETHADDR_LENGTH]; +} eoe_ethaddr_t; +PACKED_END + +/** EoE IP request structure, storage only, no need to pack */ +typedef struct eoe_param +{ + uint8_t mac_set : 1; + uint8_t ip_set : 1; + uint8_t subnet_set : 1; + uint8_t default_gateway_set : 1; + uint8_t dns_ip_set : 1; + uint8_t dns_name_set : 1; + eoe_ethaddr_t mac; + eoe_ip4_addr_t ip; + eoe_ip4_addr_t subnet; + eoe_ip4_addr_t default_gateway; + eoe_ip4_addr_t dns_ip; + char dns_name[EOE_DNS_NAME_LENGTH]; +} eoe_param_t; + +/** EOE structure. +* Used to interpret EoE mailbox packets. +*/ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert mbxheader; + uint16_t frameinfo1; + union + { + uint16_t frameinfo2; + uint16_t result; + }; + uint8 data[0]; +} ec_EOEt; +PACKED_END + +int ecx_EOEdefinehook(ecx_contextt *context, void *hook); +int ecx_EOEsetIp(ecx_contextt *context, + uint16 slave, + uint8 port, + eoe_param_t * ipparam, + int timeout); +int ecx_EOEgetIp(ecx_contextt *context, + uint16 slave, + uint8 port, + eoe_param_t * ipparam, + int timeout); +int ecx_EOEsend(ecx_contextt *context, + uint16 slave, + uint8 port, + int psize, + void *p, + int timeout); +int ecx_EOErecv(ecx_contextt *context, + uint16 slave, + uint8 port, + int * psize, + void *p, + int timeout); +int ecx_EOEreadfragment( + ec_mbxbuft * MbxIn, + uint8 * rxfragmentno, + uint16 * rxframesize, + uint16 * rxframeoffset, + uint16 * rxframeno, + int * psize, + void *p); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem/ethercatmain.c b/soem/ethercatmain.c index 8bc44e4..4051eb7 100644 --- a/soem/ethercatmain.c +++ b/soem/ethercatmain.c @@ -19,9 +19,7 @@ #include #include "osal.h" #include "oshw.h" -#include "ethercattype.h" -#include "ethercatbase.h" -#include "ethercatmain.h" +#include "ethercat.h" /** delay in us for eeprom ready loop */ @@ -120,7 +118,8 @@ ecx_contextt ecx_context = { &ec_PDOdesc[0], // .PDOdesc = &ec_SM, // .eepSM = &ec_FMMU, // .eepFMMU = - NULL // .FOEhook() + NULL, // .FOEhook() + NULL // .EOEhook() }; #endif @@ -1042,7 +1041,7 @@ int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int tim 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? */ + else if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == ECT_MBXT_COE)) /* CoE response? */ { EMp = (ec_emcyt *)mbx; if ((etohs(EMp->CANOpen) >> 12) == 0x01) /* Emergency request? */ @@ -1052,6 +1051,25 @@ int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int tim wkc = 0; /* prevent emergency to cascade up, it is already handled. */ } } + else if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == ECT_MBXT_EOE)) /* EoE response? */ + { + ec_EOEt * eoembx = (ec_EOEt *)mbx; + uint16 frameinfo1 = etohs(eoembx->frameinfo1); + /* All non fragment data frame types are expected to be handled by + * slave send/receive API if the EoE hook is set + */ + if (EOE_HDR_FRAME_TYPE_GET(frameinfo1) == EOE_FRAG_DATA) + { + if (context->EOEhook) + { + if (context->EOEhook(context, slave, eoembx) > 0) + { + /* Fragment handled by EoE hook */ + wkc = 0; + } + } + } + } else { if (wkc <= 0) /* read mailbox lost */ diff --git a/soem/ethercatmain.h b/soem/ethercatmain.h index b8a3059..098f39d 100644 --- a/soem/ethercatmain.h +++ b/soem/ethercatmain.h @@ -377,7 +377,8 @@ typedef struct PACKED ec_PDOdesc PACKED_END /** Context structure , referenced by all ecx functions*/ -typedef struct ecx_context +typedef struct ecx_context ecx_contextt; +struct ecx_context { /** port reference, may include red_port */ ecx_portt *port; @@ -421,7 +422,9 @@ typedef struct ecx_context ec_eepromFMMUt *eepFMMU; /** registered FoE hook */ int (*FOEhook)(uint16 slave, int packetnumber, int datasize); -} ecx_contextt; + /** registered EoE hook */ + int (*EOEhook)(ecx_contextt * context, uint16 slave, void * eoembx); +}; #ifdef EC_VER1 /** global struct to hold default master context */ diff --git a/soem/ethercattype.h b/soem/ethercattype.h index 12f40bd..a0eb8a6 100644 --- a/soem/ethercattype.h +++ b/soem/ethercattype.h @@ -450,16 +450,17 @@ enum /** 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_FOE_FILE_NOTFOUND = 10 + 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_FOE_FILE_NOTFOUND = 10, + EC_ERR_TYPE_EOE_INVALID_RX_DATA = 11 } ec_err_type; /** Struct to retrieve errors. */ diff --git a/test/linux/eoe_test/eoe_test.c b/test/linux/eoe_test/eoe_test.c new file mode 100644 index 0000000..7a69d8f --- /dev/null +++ b/test/linux/eoe_test/eoe_test.c @@ -0,0 +1,420 @@ +/** \file + * \brief Example code for Simple Open EtherCAT master EoE + * + * This example will run the follwing EoE functions + * SetIP + * GetIP + * + * Loop + * Send fragment data (Layer 2 0x88A4) + * Receive fragment data (Layer 2 0x88A4) + * + * For this to work, a special slave test code is running that + * will bounce the sent 0x88A4 back to receive. + * + * Usage : eoe_test [ifname1] + * ifname is NIC interface, f.e. eth0 + * + * This is a minimal test. + * + * (c)Arthur Ketels 2010 - 2011 + */ + +#include +#include +#include + +#include "ethercat.h" + +#define EC_TIMEOUTMON 500 + +char IOmap[4096]; + +int expectedWKC; +boolean needlf; +volatile int globalwkc; +boolean inOP; +uint8 currentgroup = 0; +OSAL_THREAD_HANDLE thread1; +OSAL_THREAD_HANDLE thread2; +uint8 txbuf[1024]; + +/** Current RX fragment number */ +uint8_t rxfragmentno = 0; +/** Complete RX frame size of current frame */ +uint16_t rxframesize = 0; +/** Current RX data offset in frame */ +uint16_t rxframeoffset = 0; +/** Current RX frame number */ +uint16_t rxframeno = 0; +uint8 rxbuf[1024]; +int size_of_rx = sizeof(rxbuf); + +/** registered EoE hook */ +int eoe_hook(ecx_contextt * context, uint16 slave, void * eoembx) +{ + int wkc; + /* Pass received Mbx data to EoE recevive fragment function that + * that will start/continue fill an Ethernet frame buffer + */ + size_of_rx = sizeof(rxbuf); + wkc = ecx_EOEreadfragment(eoembx, + &rxfragmentno, + &rxframesize, + &rxframeoffset, + &rxframeno, + &size_of_rx, + rxbuf); + + printf("Read frameno %d, fragmentno %d\n", rxframeno, rxfragmentno); + + /* wkc == 1 would mean a frame is complete , last fragment flag have been set and all + * other checks must have past + */ + if (wkc > 0) + { + ec_etherheadert *bp = (ec_etherheadert *)rxbuf; + uint16 type = ntohs(bp->etype); + printf("Frameno %d, type 0x%x complete\n", rxframeno, type); + if (type == ETH_P_ECAT) + { + /* Sanity check that received buffer still is OK */ + if (sizeof(txbuf) != size_of_rx) + { + printf("Size differs, expected %d , received %d\n", sizeof(txbuf), size_of_rx); + } + else + { + printf("Size OK, expected %d , received %d\n", sizeof(txbuf), size_of_rx); + } + /* Check that the TX and RX frames are EQ */ + if (memcmp(rxbuf, txbuf, size_of_rx)) + { + printf("memcmp result != 0\n"); + } + else + { + printf("memcmp result == 0\n"); + } + /* Send a new frame */ + int ixme; + for (ixme = ETH_HEADERSIZE; ixme < sizeof(txbuf); ixme++) + { + txbuf[ixme] = (uint8)rand(); + } + printf("Send a new frame\n"); + ecx_EOEsend(context, 1, 0, sizeof(txbuf), txbuf, EC_TIMEOUTRXM); + } + else + { + printf("Skip type 0x%x\n", type); + } + } + /* No point in returning as unhandled */ + return 1; +} + +OSAL_THREAD_FUNC mailbox_reader(void *lpParam) +{ + ecx_contextt *context = (ecx_contextt *)lpParam; + int wkc; + ec_mbxbuft MbxIn; + ec_mbxheadert * MbxHdr = (ec_mbxheadert *)MbxIn; + + int ixme; + ec_setupheader(&txbuf); + for (ixme = ETH_HEADERSIZE; ixme < sizeof(txbuf); ixme++) + { + txbuf[ixme] = (uint8)rand(); + } + /* Send a made up frame to trigger a fragmented transfer + * Used with a special bound impelmentaion of SOES. Will + * trigger a fragmented transfer back of the same frame. + */ + ecx_EOEsend(context, 1, 0, sizeof(txbuf), txbuf, EC_TIMEOUTRXM); + + for (;;) + { + /* Read mailbox if no other mailbox conversation is ongoing eg. SDOwrite/SDOwrite etc.*/ + wkc = ecx_mbxreceive(context, 1, (ec_mbxbuft *)&MbxIn, 0); + if (wkc > 0) + { + printf("Unhandled mailbox response 0x%x\n", MbxHdr->mbxtype); + } + osal_usleep(1 * 1000 * 1000); + } +} + +void test_eoe(ecx_contextt * context) +{ + /* Set the HOOK */ + ecx_EOEdefinehook(context, eoe_hook); + + eoe_param_t ipsettings, re_ipsettings; + memset(&ipsettings, 0, sizeof(ipsettings)); + memset(&re_ipsettings, 0, sizeof(re_ipsettings)); + + ipsettings.ip_set = 1; + ipsettings.subnet_set = 1; + ipsettings.default_gateway_set = 1; + + EOE_IP4_ADDR_TO_U32(&ipsettings.ip, 192, 168, 9, 200); + EOE_IP4_ADDR_TO_U32(&ipsettings.subnet, 255, 255, 255, 0); + EOE_IP4_ADDR_TO_U32(&ipsettings.default_gateway, 0, 0, 0, 0); + + /* Send a set IP request */ + ecx_EOEsetIp(context, 1, 0, &ipsettings, EC_TIMEOUTRXM); + + /* Send a get IP request, should return the expected IP back */ + ecx_EOEgetIp(context, 1, 0, &re_ipsettings, EC_TIMEOUTRXM); + + /* Trigger an MBX read request, to be replaced by slave Mbx + * full notification via polling of FMMU status process data + */ + printf("recieved IP (%d.%d.%d.%d)\n", + eoe_ip4_addr1(&re_ipsettings.ip), + eoe_ip4_addr2(&re_ipsettings.ip), + eoe_ip4_addr3(&re_ipsettings.ip), + eoe_ip4_addr4(&re_ipsettings.ip)); + printf("recieved subnet (%d.%d.%d.%d)\n", + eoe_ip4_addr1(&re_ipsettings.subnet), + eoe_ip4_addr2(&re_ipsettings.subnet), + eoe_ip4_addr3(&re_ipsettings.subnet), + eoe_ip4_addr4(&re_ipsettings.subnet)); + printf("recieved gateway (%d.%d.%d.%d)\n", + eoe_ip4_addr1(&re_ipsettings.default_gateway), + eoe_ip4_addr2(&re_ipsettings.default_gateway), + eoe_ip4_addr3(&re_ipsettings.default_gateway), + eoe_ip4_addr4(&re_ipsettings.default_gateway)); + + /* Create a asyncronous EoE reader */ + osal_thread_create(&thread2, 128000, &mailbox_reader, &ecx_context); +} + +void teststarter(char *ifname) +{ + int i, oloop, iloop, chk; + needlf = FALSE; + inOP = FALSE; + + printf("Starting eoe test\n"); + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config_init(FALSE) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + ec_config_map(&IOmap); + + ec_configdc(); + + printf("Slaves mapped, state to SAFE_OP.\n"); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); + + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + + printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]); + + printf("Request operational state for all slaves\n"); + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* send one valid process data to make outputs in slaves happy*/ + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + + /* Simple EoE test */ + test_eoe(&ecx_context); + + /* request OP state for all slaves */ + ec_writestate(0); + chk = 40; + /* wait for all slaves to reach OP state */ + do + { + ec_send_processdata(); + ec_receive_processdata(EC_TIMEOUTRET); + ec_statecheck(0, EC_STATE_OPERATIONAL, 50000); + } + while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL)); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + globalwkc = expectedWKC; + inOP = TRUE; + /* cyclic loop */ + for(i = 1; i <= 10000; i++) + { + ec_send_processdata(); + globalwkc = ec_receive_processdata(EC_TIMEOUTRET * 5); +#if PRINT_EOE_INFO_INSTEAD + int j; + if(globalwkc >= expectedWKC) + { + printf("Processdata cycle %4d, WKC %d , O:", i, globalwkc); + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf(" T:%"PRId64"\r",ec_DCtime); + needlf = TRUE; + } +#endif + osal_usleep(1000); + } + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + printf("\nRequest init state for all slaves\n"); + ec_slave[0].state = EC_STATE_INIT; + /* request INIT state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End eoe test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +OSAL_THREAD_FUNC ecatcheck( void *ptr ) +{ + int slave; + (void)ptr; /* Not used */ + + while(1) + { + if( inOP && ((globalwkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (globalwkc < expectedWKC) + { + printf("(wkc < expectedWKC) , globalwkc %d\n", globalwkc); + } + if (ec_group[currentgroup].docheckstate) + { + printf("(ec_group[currentgroup].docheckstate)\n"); + } + + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } +} + +int main(int argc, char *argv[]) +{ + printf("SOEM (Simple Open EtherCAT Master)\nEoE test\n"); + + if (argc > 1) + { + /* create thread to handle slave error handling in OP */ +// pthread_create( &thread1, NULL, (void *) &ecatcheck, (void*) &ctime); + osal_thread_create(&thread1, 128000, &ecatcheck, (void*) &ctime); + + /* start cyclic part */ + teststarter(argv[1]); + } + else + { + printf("Usage: eoe_test ifname1\nifname = eth0 for example\n"); + } + + printf("End program\n"); + return (0); +}