420 lines
13 KiB
C
420 lines
13 KiB
C
/** \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 = 200;
|
|
/* 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 */
|
|
osal_thread_create(&thread1, 128000, &ecatcheck, NULL);
|
|
|
|
/* start cyclic part */
|
|
teststarter(argv[1]);
|
|
}
|
|
else
|
|
{
|
|
printf("Usage: eoe_test ifname1\nifname = eth0 for example\n");
|
|
}
|
|
|
|
printf("End program\n");
|
|
return (0);
|
|
}
|