Impelmented EoE filter function in mailbox receive (#244)
This commit is contained in:
nakarlsson
GitHub
parent
4427684cc5
commit
86a2584e47
|
|
@ -19,6 +19,7 @@
|
|||
#include "ethercatcoe.h"
|
||||
#include "ethercatfoe.h"
|
||||
#include "ethercatsoe.h"
|
||||
#include "ethercateoe.h"
|
||||
#include "ethercatconfig.h"
|
||||
#include "ethercatprint.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <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;
|
||||
}
|
||||
|
|
@ -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 <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
|
||||
|
|
@ -19,9 +19,7 @@
|
|||
#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 */
|
||||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -459,7 +459,8 @@ typedef enum
|
|||
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_FOE_FILE_NOTFOUND = 10,
|
||||
EC_ERR_TYPE_EOE_INVALID_RX_DATA = 11
|
||||
} ec_err_type;
|
||||
|
||||
/** Struct to retrieve errors. */
|
||||
|
|
|
|||
|
|
@ -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 <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);
|
||||
}
|
||||
Loading…
Reference in New Issue