#include "ethercatcoe.h"
#include "ethercatfoe.h"
#include "ethercatsoe.h"
+#include "ethercateoe.h"
#include "ethercatconfig.h"
#include "ethercatprint.h"
--- /dev/null
+/*
+ * 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 <stdio.h>
+#include <string.h>
+#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;
+}
--- /dev/null
+/*
+ * 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 <ethercattype.h>
+
+/** 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
#include <string.h>
#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 */
&ec_PDOdesc[0], // .PDOdesc =
&ec_SM, // .eepSM =
&ec_FMMU, // .eepFMMU =
- NULL // .FOEhook()
+ NULL, // .FOEhook()
+ NULL // .EOEhook()
};
#endif
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? */
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 */
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;
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 */
/** 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. */
--- /dev/null
+/** \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 <stdio.h>
+#include <string.h>
+#include <inttypes.h>
+
+#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);
+}
projects / 0xmirror / SOEM.git / commitdiff