+++ /dev/null
-name: build
-on: [push, pull_request]
-env:
- BUILD_TYPE: Release
-jobs:
- build:
- runs-on: ${{ matrix.os }}
- strategy:
- matrix:
- os:
- - ubuntu-latest
- - ubuntu-20.04
- - ubuntu-18.04
- - macos-latest
- - windows-latest
- steps:
- - uses: actions/checkout@v2
- with:
- submodules: true
-
- - name: Configure
- shell: bash
- run: |
- cmake -E make_directory $GITHUB_WORKSPACE/build
- cmake -B $GITHUB_WORKSPACE/build -S $GITHUB_WORKSPACE \
- -DCMAKE_BUILD_TYPE=$BUILD_TYPE
-
- - name: Build
- shell: bash
- run: |
- cmake --build $GITHUB_WORKSPACE/build -j4 --target install
/doc/latex
/doc/html
tags
+.vscode
add_subdirectory(test/linux/slaveinfo)
add_subdirectory(test/linux/eepromtool)
add_subdirectory(test/linux/simple_test)
+ add_subdirectory(test/linux/coetest)
endif()
return 1;
}
+
+void *osal_mutex_create(void)
+{
+ pthread_mutexattr_t mutexattr;
+ osal_mutext *mutex;
+ mutex = (osal_mutext *)osal_malloc (sizeof(osal_mutext));
+ if(mutex)
+ {
+ pthread_mutexattr_init(&mutexattr);
+ pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT);
+ pthread_mutex_init(mutex, &mutexattr);
+ }
+ return (void *)mutex;
+}
+
+void osal_mutex_destroy(void *mutex)
+{
+ pthread_mutex_destroy((osal_mutext *)mutex);
+ osal_free(mutex);
+}
+
+void osal_mutex_lock(void *mutex)
+{
+ pthread_mutex_lock((osal_mutext *) mutex);
+}
+
+void osal_mutex_unlock(void *mutex)
+{
+ pthread_mutex_unlock((osal_mutext *) mutex);
+}
#define OSAL_THREAD_FUNC void
#define OSAL_THREAD_FUNC_RT void
+#define osal_mutext pthread_mutex_t
+
#ifdef __cplusplus
}
#endif
#include "osal_defs.h"
#include <stdint.h>
+#include <stddef.h>
/* General types */
#ifndef TRUE
void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff);
int osal_thread_create(void *thandle, int stacksize, void *func, void *param);
int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param);
+void *osal_malloc(size_t size);
+void osal_free(void *ptr);
+void *osal_mutex_create(void);
+void osal_mutex_destroy(void *mutex);
+void osal_mutex_lock(void *mutex);
+void osal_mutex_unlock(void *mutex);
#ifdef __cplusplus
}
#define OSAL_THREAD_FUNC void
#define OSAL_THREAD_FUNC_RT void
+#define osal_mutext CRITICAL_SECTION
+
#ifdef __cplusplus
}
#endif
* This layer if fully transparent for the higher layers.
*/
+#define _GNU_SOURCE
+
#include <sys/types.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <string.h>
#include <netpacket/packet.h>
#include <pthread.h>
+#include <poll.h>
#include "oshw.h"
#include "osal.h"
{
int i;
int r, rval, ifindex;
- struct timeval timeout;
+ // struct timeval timeout;
struct ifreq ifr;
struct sockaddr_ll sll;
int *psock;
if(*psock < 0)
return 0;
+ r = 0;
+/*
timeout.tv_sec = 0;
timeout.tv_usec = 1;
- r = 0;
r |= setsockopt(*psock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
r |= setsockopt(*psock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout));
+*/
i = 1;
r |= setsockopt(*psock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i));
/* connect socket to NIC by name */
else
{
pthread_mutex_lock(&(port->rx_mutex));
+ /* check again if requested index is already in buffer ?
+ * other task might have reveived it befor we grabbed mutex */
+ if ((idx < EC_MAXBUF) && ((*stack->rxbufstat)[idx] == EC_BUF_RCVD))
+ {
+ l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8);
+ /* return WKC */
+ rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8));
+ /* mark as completed */
+ (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE;
+ }
/* non blocking call to retrieve frame from socket */
- if (ecx_recvpkt(port, stacknumber))
+ else if (ecx_recvpkt(port, stacknumber))
{
rval = EC_OTHERFRAME;
ehp =(ec_etherheadert*)(stack->tempbuf);
/* check if it is an EtherCAT frame */
if (ehp->etype == htons(ETH_P_ECAT))
{
+ stack->rxcnt++;
ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]);
l = etohs(ecp->elength) & 0x0fff;
idxf = ecp->index;
/* if not in redundant mode then always assume secondary is OK */
if (port->redstate == ECT_RED_NONE)
wkc2 = 0;
+ /* use ppoll to reduce busy_polling */
+ struct pollfd fds[2];
+ struct pollfd *fdsp;
+ int poll_err = 0;
+ struct timespec timeout_spec = { 0, 0 };
+ timeout_spec.tv_nsec = 50 * 1000;
+ ec_stackT *stack;
+ stack = &(port->stack);
+ fds[0].fd = *stack->sock;
+ fds[0].events = POLLIN;
+ int pollcnt = 1;
+ if(port->redstate != ECT_RED_NONE)
+ {
+ pollcnt = 2;
+ stack = &(port->redport->stack);
+ fds[1].fd = *stack->sock;
+ fds[1].events = POLLIN;
+ }
+ fdsp = &fds[0];
do
{
- /* only read frame if not already in */
- if (wkc <= EC_NOFRAME)
- wkc = ecx_inframe(port, idx, 0);
- /* only try secondary if in redundant mode */
- if (port->redstate != ECT_RED_NONE)
+ poll_err = ppoll(fdsp, pollcnt, &timeout_spec, NULL);
+ if(poll_err >= 0)
{
/* only read frame if not already in */
- if (wkc2 <= EC_NOFRAME)
- wkc2 = ecx_inframe(port, idx, 1);
+ if (wkc <= EC_NOFRAME)
+ wkc = ecx_inframe(port, idx, 0);
+ /* only try secondary if in redundant mode */
+ if (port->redstate != ECT_RED_NONE)
+ {
+ /* only read frame if not already in */
+ if (wkc2 <= EC_NOFRAME)
+ wkc2 = ecx_inframe(port, idx, 1);
+ }
}
/* wait for both frames to arrive or timeout */
} while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && !osal_timer_is_expired(timer));
int (*rxbufstat)[EC_MAXBUF];
/** received MAC source address (middle word) */
int (*rxsa)[EC_MAXBUF];
+ uint64 rxcnt;
} ec_stackT;
/** pointer structure to buffers for redundant port */
{
ec_SDOt *SDOp, *aSDOp;
uint16 bytesize, Framedatasize;
- int wkc;
+ int wkc = 0;
int32 SDOlen;
uint8 *bp;
uint8 *hp;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt, toggle;
boolean NotLast;
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSDOp = (ec_SDOt *)&MbxIn;
- SDOp = (ec_SDOt *)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, 0);
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut) return wkc;
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOt *)MbxOut;
SDOp->MbxHeader.length = htoes(0x000a);
SDOp->MbxHeader.address = htoes(0x0000);
SDOp->MbxHeader.priority = 0x00;
/* get new mailbox count value, used as session handle */
- cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
+ cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
context->slavelist[slave].mbx_cnt = cnt;
SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */
SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */
SDOp->SubIndex = subindex;
SDOp->ldata[0] = 0;
/* send CoE request to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
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 ? */
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
+ do
+ {
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, timeout);
+ } while ((wkc > 0) && !MbxIn);
+ aSDOp = (ec_SDOt *)MbxIn;
+ if ((wkc > 0) && MbxIn) /* succeeded to read slave response ? */
{
/* slave response should be CoE, SDO response and the correct index */
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
toggle= 0x00;
while (NotLast) /* segmented transfer */
{
- SDOp = (ec_SDOt *)&MbxOut;
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut)
+ {
+ NotLast = FALSE;
+ break;
+ }
+ SDOp = (ec_SDOt *)MbxOut;
SDOp->MbxHeader.length = htoes(0x000a);
SDOp->MbxHeader.address = htoes(0x0000);
SDOp->MbxHeader.priority = 0x00;
SDOp->SubIndex = subindex;
SDOp->ldata[0] = 0;
/* send segmented upload request to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
- /* is mailbox transferred to slave ? */
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0)
{
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, timeout);
+ aSDOp = (ec_SDOt *)MbxIn;
/* has slave responded ? */
if (wkc > 0)
{
}
}
}
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ if (MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
boolean CA, int psize, const void *p, int Timeout)
{
ec_SDOt *SDOp, *aSDOp;
- int wkc, maxdata, framedatasize;
- ec_mbxbuft MbxIn, MbxOut;
+ int wkc, maxdata;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt, toggle;
+ int framedatasize;
boolean NotLast;
const uint8 *hp;
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSDOp = (ec_SDOt *)&MbxIn;
- SDOp = (ec_SDOt *)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, 0);
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut) return wkc;
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOt *)MbxOut;
maxdata = context->slavelist[Slave].mbx_l - 0x10; /* data section=mailbox size - 6 mbx - 2 CoE - 8 sdo req */
/* if small data use expedited transfer */
if ((psize <= 4) && !CA)
/* copy parameter data to mailbox */
memcpy(&SDOp->ldata[0], hp, psize);
/* send mailbox SDO download request to slave */
- wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, Slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0)
{
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, Timeout);
if (wkc > 0)
{
+ aSDOp = (ec_SDOt *)MbxIn;
/* response should be CoE, SDO response, correct index and subindex */
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
hp += framedatasize;
psize -= framedatasize;
/* send mailbox SDO download request to slave */
- wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, Slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0)
{
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, Timeout);
if (wkc > 0)
{
+ aSDOp = (ec_SDOt *)MbxIn;
/* response should be CoE, SDO response, correct index and subindex */
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
/* repeat while segments left */
while (NotLast)
{
- SDOp = (ec_SDOt *)&MbxOut;
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut)
+ {
+ NotLast = FALSE;
+ break;
+ }
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOt *)MbxOut;
framedatasize = psize;
NotLast = FALSE;
SDOp->Command = 0x01; /* last segment */
hp += framedatasize;
psize -= framedatasize;
/* send SDO download request */
- wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, Slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0)
{
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, Timeout);
if (wkc > 0)
{
+ aSDOp = (ec_SDOt *)MbxIn;
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
((aSDOp->Command & 0xe0) == 0x20))
}
}
}
-
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ if (MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
{
ec_SDOt *SDOp;
int wkc, maxdata, framedatasize;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
-
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- SDOp = (ec_SDOt *)&MbxOut;
+
+ MbxIn = NULL;
+ MbxOut = NULL;
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, 0);
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut) return wkc;
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOt *)MbxOut;
maxdata = context->slavelist[Slave].mbx_l - 0x08; /* data section=mailbox size - 6 mbx - 2 CoE */
framedatasize = psize;
if (framedatasize > maxdata)
/* copy PDO data to mailbox */
memcpy(&SDOp->Command, p, framedatasize);
/* send mailbox RxPDO request to slave */
- wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
-
+ wkc = ecx_mbxsend(context, Slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
return wkc;
}
{
ec_SDOt *SDOp, *aSDOp;
int wkc;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
uint16 framedatasize;
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSDOp = (ec_SDOt *)&MbxIn;
- SDOp = (ec_SDOt *)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, 0);
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut) return wkc;
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOt *)MbxOut;
SDOp->MbxHeader.length = htoes(0x02);
SDOp->MbxHeader.address = htoes(0x0000);
SDOp->MbxHeader.priority = 0x00;
context->slavelist[slave].mbx_cnt = cnt;
SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */
SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12)); /* number 9bits service upper 4 bits */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0)
{
- /* clean mailboxbuffer */
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
+ aSDOp = (ec_SDOt *)MbxIn;
/* slave response should be CoE, TxPDO */
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_TXPDO))
}
}
}
-
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ if (MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist)
{
ec_SDOservicet *SDOp, *aSDOp;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
int wkc;
uint16 x, n, i, sp, offset;
boolean stop;
pODlist->Slave = Slave;
pODlist->Entries = 0;
- ec_clearmbx(&MbxIn);
- /* clear pending out mailbox in slave if available. Timeout is set to 0 */
- wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSDOp = (ec_SDOservicet*)&MbxIn;
- SDOp = (ec_SDOservicet*)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ /* Empty slave out mailbox if something is in. Timout set to 0 */
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, 0);
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut) return wkc;
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOservicet *)MbxOut;
SDOp->MbxHeader.length = htoes(0x0008);
SDOp->MbxHeader.address = htoes(0x0000);
SDOp->MbxHeader.priority = 0x00;
SDOp->Fragments = 0; /* fragments left */
SDOp->wdata[0] = htoes(0x01); /* all objects */
/* send get object description list request to slave */
- wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, Slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
/* mailbox placed in slave ? */
if (wkc > 0)
{
do
{
stop = TRUE; /* assume this is last iteration */
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, EC_TIMEOUTRXM);
/* got response ? */
if (wkc > 0)
{
+ aSDOp = (ec_SDOservicet*)MbxIn;
/* response should be CoE and "get object description list response" */
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES))
}
while ((x <= 128) && !stop);
}
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ if (MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
ec_SDOservicet *SDOp, *aSDOp;
int wkc;
uint16 n, Slave;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
Slave = pODlist->Slave;
pODlist->ObjectCode[Item] = 0;
pODlist->MaxSub[Item] = 0;
pODlist->Name[Item][0] = 0;
- ec_clearmbx(&MbxIn);
- /* clear pending out mailbox in slave if available. Timeout is set to 0 */
- wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSDOp = (ec_SDOservicet*)&MbxIn;
- SDOp = (ec_SDOservicet*)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ /* Empty slave out mailbox if something is in. Timout set to 0 */
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, 0);
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut) return wkc;
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOservicet *)MbxOut;
SDOp->MbxHeader.length = htoes(0x0008);
SDOp->MbxHeader.address = htoes(0x0000);
SDOp->MbxHeader.priority = 0x00;
SDOp->Fragments = 0; /* fragments left */
SDOp->wdata[0] = htoes(pODlist->Index[Item]); /* Data of Index */
/* send get object description request to slave */
- wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, Slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
/* mailbox placed in slave ? */
if (wkc > 0)
{
- ec_clearmbx(&MbxIn);
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, EC_TIMEOUTRXM);
/* got response ? */
if (wkc > 0)
{
+ aSDOp = (ec_SDOservicet*)MbxIn;
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES))
{
}
}
}
-
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ if (MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
int wkc;
uint16 Index, Slave;
int16 n;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
wkc = 0;
Slave = pODlist->Slave;
Index = pODlist->Index[Item];
- ec_clearmbx(&MbxIn);
- /* clear pending out mailbox in slave if available. Timeout is set to 0 */
- wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSDOp = (ec_SDOservicet*)&MbxIn;
- SDOp = (ec_SDOservicet*)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ /* Empty slave out mailbox if something is in. Timout set to 0 */
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, 0);
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxOut = ecx_getmbx(context);
+ if(!MbxOut) return wkc;
+ ec_clearmbx(MbxOut);
+ SDOp = (ec_SDOservicet *)MbxOut;
SDOp->MbxHeader.length = htoes(0x000a);
SDOp->MbxHeader.address = htoes(0x0000);
SDOp->MbxHeader.priority = 0x00;
SDOp->bdata[2] = SubI; /* SubIndex */
SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */
/* send get object entry description request to slave */
- wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, Slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
/* mailbox placed in slave ? */
if (wkc > 0)
{
- ec_clearmbx(&MbxIn);
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
+ wkc = ecx_mbxreceive2(context, Slave, &MbxIn, EC_TIMEOUTRXM);
/* got response ? */
if (wkc > 0)
{
+ aSDOp = (ec_SDOservicet*)MbxIn;
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES))
{
}
}
}
-
+ if (MbxIn) ecx_dropmbx(context, MbxIn);
+ if (MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
{
/* default start address per group entry */
context->grouplist[lp].logstartaddr = lp << EC_LOGGROUPOFFSET;
+ ecx_initmbxqueue(context, lp);
}
}
{
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* Manuf */
context->slavelist[slave].eep_man = etohl(eedat);
- ecx_readeeprom1(context, slave, ECT_SII_SN); /* serial # */
- }
- for (slave = 1; slave <= *(context->slavecount); slave++)
- {
- eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* serial # */
- context->slavelist[slave].eep_sn = etohl(eedat);
ecx_readeeprom1(context, slave, ECT_SII_ID); /* ID */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* revision */
context->slavelist[slave].eep_rev = etohl(eedat);
+ ecx_readeeprom1(context, slave, ECT_SII_SER); /* serial number */
+ }
+ for (slave = 1; slave <= *(context->slavecount); slave++)
+ {
+ eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* serial number */
+ context->slavelist[slave].eep_ser = etohl(eedat);
ecx_readeeprom1(context, slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
context->slavelist[slave].FMMUunused = FMMUc;
}
+static void ecx_config_create_mbxstatus_mappings(ecx_contextt *context, void *pIOmap,
+ uint8 group, int16 slave, uint32 * LogAddr)
+{
+ uint16 FMMUsize = 1;
+ uint16 configadr;
+ uint8 FMMUc;
+ int position;
+
+ EC_PRINT(" =Slave %d, MBXSTATUS MAPPING\n", slave);
+
+ configadr = context->slavelist[slave].configadr;
+ FMMUc = context->slavelist[slave].FMMUunused;
+ if (context->slavelist[slave].mbx_l && (FMMUc < EC_MAXFMMU)) /* slave with mailbox */
+ {
+ context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
+ context->slavelist[slave].FMMU[FMMUc].LogStartbit = 0;
+ *LogAddr += FMMUsize;
+ context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
+ context->slavelist[slave].FMMU[FMMUc].LogEndbit = 7;
+ context->slavelist[slave].FMMU[FMMUc].PhysStart = ECT_REG_SM1STAT;
+ context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
+ context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1;
+ context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
+ /* program FMMU for input */
+ ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
+ sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
+
+ position = etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
+ context->slavelist[slave].mbxstatus = (uint8 *)(pIOmap) + position;
+ position -= context->grouplist[group].Obytes + context->grouplist[group].Ibytes;
+ context->grouplist[group].mbxstatuslookup[position] = slave;
+ context->grouplist[group].mbxstatuslength++;
+ FMMUc++;
+ /* account for MBXSTATUS wkc increment */
+ if(!context->slavelist[slave].Ibytes)
+ context->grouplist[group].inputsWKC++;
+ }
+ context->slavelist[slave].FMMUunused = FMMUc;
+}
+
static int ecx_main_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group, boolean forceByteAlignment)
{
uint16 slave, configadr;
context->grouplist[group].nsegments = currentsegment + 1;
context->grouplist[group].inputs = (uint8 *)(pIOmap) + context->grouplist[group].Obytes;
context->grouplist[group].Ibytes = LogAddr -
- context->grouplist[group].logstartaddr -
- context->grouplist[group].Obytes;
+ context->grouplist[group].logstartaddr -
+ context->grouplist[group].Obytes;
+
+ /* do mbxstatus mapping of slave and program FMMUs */
+ for (slave = 1; slave <= *(context->slavecount); slave++)
+ {
+ configadr = context->slavelist[slave].configadr;
+ if (!group || (group == context->slavelist[slave].group))
+ {
+ ecx_config_create_mbxstatus_mappings(context, pIOmap, group, slave, &LogAddr);
+ diff = LogAddr - oLogAddr;
+ oLogAddr = LogAddr;
+ if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
+ {
+ context->grouplist[group].IOsegment[currentsegment] = segmentsize;
+ if (currentsegment < (EC_MAXIOSEGMENTS - 1))
+ {
+ currentsegment++;
+ segmentsize = diff;
+ }
+ }
+ else
+ {
+ segmentsize += diff;
+ }
+ ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */
+ ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP) , EC_TIMEOUTRET3); /* set safeop status */
+ if (context->slavelist[slave].blockLRW)
+ {
+ context->grouplist[group].blockLRW++;
+ }
+ context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
+ }
+ }
+
+ context->grouplist[group].IOsegment[currentsegment] = segmentsize;
+ context->grouplist[group].nsegments = currentsegment + 1;
+ context->grouplist[group].mbxstatus = (uint8 *)(pIOmap) + context->grouplist[group].Obytes + context->grouplist[group].Ibytes;
+ context->grouplist[group].mbxstatuslength = LogAddr - context->grouplist[group].Obytes - context->grouplist[group].Ibytes;
if (!group)
{
context->slavelist[0].inputs = (uint8 *)(pIOmap) + context->slavelist[0].Obytes;
context->slavelist[0].Ibytes = LogAddr -
context->grouplist[group].logstartaddr -
context->slavelist[0].Obytes; /* store input bytes in master record */
+ context->slavelist[0].mbxstatus = (uint8 *)(pIOmap) + context->slavelist[0].Obytes + context->slavelist[0].Ibytes;
}
EC_PRINT("IOmapSize %d\n", LogAddr - context->grouplist[group].logstartaddr);
context->grouplist[group].outputs = pIOmap;
context->grouplist[group].inputs = (uint8 *)pIOmap + context->grouplist[group].Obytes;
- /* Move calculated inputs with OBytes offset*/
+ context->grouplist[group].mbxstatus = (uint8 *)pIOmap + context->grouplist[group].Obytes + context->grouplist[group].Ibytes;
+
+ /* Move calculated inputs with OBytes offset*/
for (slave = 1; slave <= *(context->slavecount); slave++)
{
if (!group || (group == context->slavelist[slave].group))
context->slavelist[0].Obytes = soLogAddr - context->grouplist[group].logstartaddr;
context->slavelist[0].inputs = (uint8 *)pIOmap + context->slavelist[0].Obytes;
context->slavelist[0].Ibytes = siLogAddr - context->grouplist[group].logstartaddr;
+ context->slavelist[0].mbxstatus = (uint8 *)pIOmap + context->slavelist[0].Obytes + context->slavelist[0].Ibytes;
}
EC_PRINT("IOmapSize %d\n", context->grouplist[group].Obytes + context->grouplist[group].Ibytes);
sizeof(ec_smt), &context->slavelist[slave].SM[nSM], timeout);
}
}
+ /* small delay to allow slave to process SM changes */
+ osal_usleep(5000);
ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP) , timeout);
state = ecx_statecheck(context, slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); /* check state change pre-op */
if( state == EC_STATE_PRE_OP)
int32 dataread = 0;
int32 buffersize, packetnumber, prevpacket = 0;
uint16 fnsize, maxdata, segmentdata;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
boolean worktodo;
buffersize = *psize;
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aFOEp = (ec_FOEt *)&MbxIn;
- FOEp = (ec_FOEt *)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ /* Empty slave out mailbox if something is in. Timout set to 0 */
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, 0);
+ MbxOut = ecx_getmbx(context);
+ ec_clearmbx(MbxOut);
+ FOEp = (ec_FOEt *)MbxOut;
fnsize = (uint16)strlen(filename);
if (fnsize > EC_MAXFOEDATA)
{
/* copy filename in mailbox */
memcpy(&FOEp->FileName[0], filename, fnsize);
/* send FoE request to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
do
{
worktodo = FALSE;
- /* clean mailboxbuffer */
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
+ aFOEp = (ec_FOEt *)MbxIn;
/* slave response should be FoE */
if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE)
{
{
worktodo = TRUE;
}
+ MbxOut = ecx_getmbx(context);
+ ec_clearmbx(MbxOut);
+ FOEp = (ec_FOEt *)MbxOut;
FOEp->MbxHeader.length = htoes(0x0006);
FOEp->MbxHeader.address = htoes(0x0000);
FOEp->MbxHeader.priority = 0x00;
FOEp->OpCode = ECT_FOE_ACK;
FOEp->PacketNumber = htoel(packetnumber);
/* send FoE ack to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc <= 0)
{
worktodo = FALSE;
}
} while (worktodo);
}
-
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ if(MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
int32 packetnumber, sendpacket = 0;
uint16 fnsize, maxdata;
int segmentdata;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
boolean worktodo, dofinalzero;
int tsize;
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aFOEp = (ec_FOEt *)&MbxIn;
- FOEp = (ec_FOEt *)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ /* Empty slave out mailbox if something is in. Timout set to 0 */
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, 0);
+ MbxOut = ecx_getmbx(context);
+ ec_clearmbx(MbxOut);
+ FOEp = (ec_FOEt *)MbxOut;
dofinalzero = TRUE;
fnsize = (uint16)strlen(filename);
if (fnsize > EC_MAXFOEDATA)
/* copy filename in mailbox */
memcpy(&FOEp->FileName[0], filename, fnsize);
/* send FoE request to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
do
{
worktodo = FALSE;
- /* clean mailboxbuffer */
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
+ aFOEp = (ec_FOEt *)MbxIn;
/* slave response should be FoE */
if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE)
{
{
dofinalzero = TRUE;
}
+ MbxOut = ecx_getmbx(context);
+ ec_clearmbx(MbxOut);
+ FOEp = (ec_FOEt *)MbxOut;
FOEp->MbxHeader.length = htoes((uint16)(0x0006 + segmentdata));
FOEp->MbxHeader.address = htoes(0x0000);
FOEp->MbxHeader.priority = 0x00;
memcpy(&FOEp->Data[0], p, segmentdata);
p = (uint8 *)p + segmentdata;
/* send FoE data to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc <= 0)
{
worktodo = FALSE;
}
} while (worktodo);
}
-
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ if(MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
ecx_portt ecx_port;
ecx_redportt ecx_redport;
+ec_mbxpoolt ec_mbxpool;
+
ecx_contextt ecx_context = {
&ecx_port, // .port =
&ec_slave[0], // .slavelist =
&ec_PDOdesc[0], // .PDOdesc =
&ec_SM, // .eepSM =
&ec_FMMU, // .eepFMMU =
+ &ec_mbxpool, // .mbxpool =
NULL, // .FOEhook()
NULL, // .EOEhook()
0, // .manualstatechange
*/
int ecx_init(ecx_contextt *context, const char * ifname)
{
+ ecx_initmbxpool(context);
return ecx_setupnic(context->port, ifname, FALSE);
}
int rval, zbuf;
ec_etherheadert *ehp;
+ ecx_initmbxpool(context);
context->port->redport = redport;
ecx_setupnic(context->port, ifname, FALSE);
rval = ecx_setupnic(context->port, if2name, TRUE);
*/
void ecx_close(ecx_contextt *context)
{
+ osal_mutex_destroy(context->mbxpool->mbxmutex);
ecx_closenic(context->port);
-};
+}
+
+ec_mbxbuft *ecx_getmbx(ecx_contextt *context)
+{
+ ec_mbxbuft *mbx = NULL;
+ ec_mbxpoolt *mbxpool = context->mbxpool;
+ osal_mutex_lock(mbxpool->mbxmutex);
+ if(mbxpool->listcount > 0)
+ {
+ mbx = (ec_mbxbuft *)&(mbxpool->mbx[mbxpool->mbxemptylist[mbxpool->listtail]]);
+// printf("getmbx item:%d mbx:%p\n\r",mbxpool->mbxemptylist[mbxpool->listtail], mbx);
+ mbxpool->listtail++;
+ if(mbxpool->listtail >= EC_MBXPOOLSIZE) mbxpool->listtail = 0;
+ mbxpool->listcount--;
+ }
+ osal_mutex_unlock(mbxpool->mbxmutex);
+ return mbx;
+}
+
+int ecx_dropmbx(ecx_contextt *context, ec_mbxbuft *mbx)
+{
+ ec_mbxpoolt *mbxpool = context->mbxpool;
+ int item = mbx - &(mbxpool->mbx[0]);
+// printf("dropmbx item:%d mbx:%p\n\r",item, mbx);
+ if((item >= 0) && (item < EC_MBXPOOLSIZE))
+ {
+ osal_mutex_lock(mbxpool->mbxmutex);
+ mbxpool->mbxemptylist[mbxpool->listhead++] = item;
+ if(mbxpool->listhead >= EC_MBXPOOLSIZE) mbxpool->listhead = 0;
+ mbxpool->listcount++;
+ osal_mutex_unlock(mbxpool->mbxmutex);
+ return 1;
+ }
+ return 0;
+}
+
+int ecx_initmbxpool(ecx_contextt *context)
+{
+ int retval = 0;
+ ec_mbxpoolt *mbxpool = context->mbxpool;
+ mbxpool->mbxmutex = (osal_mutext *)osal_mutex_create();
+ for(int item = 0 ; item < EC_MBXPOOLSIZE ; item++)
+ {
+ mbxpool->mbxemptylist[item] = item;
+ }
+ mbxpool->listhead = 0;
+ mbxpool->listtail = 0;
+ mbxpool->listcount = EC_MBXPOOLSIZE;
+ // printf("intmbxpool mbxp:%p mutex:%p\n\r", mbxpool->mbx[0], mbxpool->mbxmutex);
+ return retval;
+}
+
+int ecx_initmbxqueue(ecx_contextt *context, uint16 group)
+{
+ int retval = 0;
+ int cnt;
+ ec_mbxqueuet *mbxqueue = &(context->grouplist[group].mbxtxqueue);
+ mbxqueue->mbxmutex = (osal_mutext *)osal_mutex_create();
+ mbxqueue->listhead = 0;
+ mbxqueue->listtail = 0;
+ mbxqueue->listcount = 0;
+ for(cnt = 0 ; cnt < EC_MBXPOOLSIZE ; cnt++)
+ mbxqueue->mbxticket[cnt] = -1;
+ return retval;
+}
+
+int ecx_mbxaddqueue(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx)
+{
+ int ticketloc;
+ int ticket = -1;
+ uint8 group = context->slavelist[slave].group;
+ ec_mbxqueuet *mbxqueue = &(context->grouplist[group].mbxtxqueue);
+ osal_mutex_lock(mbxqueue->mbxmutex);
+ if((mbxqueue->listcount < EC_MBXPOOLSIZE))
+ {
+ ticketloc = mbxqueue->listhead;
+ while((++ticket < EC_MBXPOOLSIZE) && (mbxqueue->mbxticket[ticket] >= 0)) {};
+ mbxqueue->mbxticket[ticket] = ticketloc;
+ mbxqueue->mbxslave[ticketloc] = slave;
+ mbxqueue->mbx[ticketloc] = mbx;
+ mbxqueue->listhead++;
+ if(mbxqueue->listhead >= EC_MBXPOOLSIZE) mbxqueue->listhead = 0;
+ mbxqueue->mbxremove[ticketloc] = 0;
+ mbxqueue->mbxstate[ticketloc] = EC_MBXQUEUESTATE_REQ;
+ mbxqueue->listcount++;
+ }
+ osal_mutex_unlock(mbxqueue->mbxmutex);
+ return ticket;
+}
+
+int ecx_mbxdonequeue(ecx_contextt *context, uint16 slave, int ticket)
+{
+ int retval = 0;
+ if((ticket >= 0) && (ticket < EC_MBXPOOLSIZE))
+ {
+ uint8 group = context->slavelist[slave].group;
+ ec_mbxqueuet *mbxqueue = &(context->grouplist[group].mbxtxqueue);
+ osal_mutex_lock(mbxqueue->mbxmutex);
+ if(mbxqueue->mbxstate[mbxqueue->mbxticket[ticket]] == EC_MBXQUEUESTATE_DONE)
+ {
+ mbxqueue->mbxremove[mbxqueue->mbxticket[ticket]] = 1;
+ mbxqueue->mbxticket[ticket] = -1;
+ retval = 1;
+ }
+ osal_mutex_unlock(mbxqueue->mbxmutex);
+ }
+ return retval;
+}
+
+int ecx_mbxexpirequeue(ecx_contextt *context, uint16 slave, int ticket)
+{
+ int retval = 0;
+ if((ticket >= 0) && (ticket < EC_MBXPOOLSIZE))
+ {
+ uint8 group = context->slavelist[slave].group;
+ ec_mbxqueuet *mbxqueue = &(context->grouplist[group].mbxtxqueue);
+ osal_mutex_lock(mbxqueue->mbxmutex);
+ if(mbxqueue->mbxstate[mbxqueue->mbxticket[ticket]] > EC_MBXQUEUESTATE_NONE)
+ {
+ mbxqueue->mbxremove[mbxqueue->mbxticket[ticket]] = 1;
+ mbxqueue->mbxticket[ticket] = -1;
+ retval = 1;
+ }
+ osal_mutex_unlock(mbxqueue->mbxmutex);
+ }
+ return retval;
+}
+
+int ecx_mbxrotatequeue(ecx_contextt *context, uint16 group, int ticketloc)
+{
+ int retval = 0;
+ int cnt = 0;
+ int ticket;
+ ec_mbxqueuet *mbxqueue = &(context->grouplist[group].mbxtxqueue);
+ osal_mutex_lock(mbxqueue->mbxmutex);
+ int head = mbxqueue->listhead;
+ int tail = mbxqueue->listtail;
+ if(head != tail)
+ {
+ while((cnt < EC_MBXPOOLSIZE) && (mbxqueue->mbxticket[cnt] != ticketloc)) cnt++;
+ ticket = cnt;
+ if((ticket >= 0) && (ticket < EC_MBXPOOLSIZE))
+ {
+ mbxqueue->mbxticket[ticket] = head;
+ mbxqueue->mbxremove[head] = mbxqueue->mbxremove[tail];
+ mbxqueue->mbxstate[head] = mbxqueue->mbxstate[tail];
+ mbxqueue->mbxslave[head] = mbxqueue->mbxslave[tail];
+ mbxqueue->mbx[head] = mbxqueue->mbx[tail];
+ mbxqueue->listhead++;
+ if(mbxqueue->listhead >= EC_MBXPOOLSIZE) mbxqueue->listhead = 0;
+ mbxqueue->listtail++;
+ if(mbxqueue->listtail >= EC_MBXPOOLSIZE) mbxqueue->listtail = 0;
+ retval = 1;
+ }
+ }
+ else
+ {
+ retval = 1;
+ }
+ osal_mutex_unlock(mbxqueue->mbxmutex);
+ return retval;
+}
+
+int ecx_slavembxcyclic(ecx_contextt *context, uint16 slave)
+{
+ if(context->slavelist[slave].mbxstatus)
+ {
+ context->slavelist[slave].coembxin = EC_MBXINENABLE;
+ context->slavelist[slave].mbxhandlerstate = ECT_MBXH_CYCLIC;
+ return 1;
+ }
+ return 0;
+}
+ec_mbxbuft *ecx_mbxdropqueue(ecx_contextt *context, uint16 group, int ticketloc)
+{
+ ec_mbxbuft *mbx;
+ ec_mbxqueuet *mbxqueue = &(context->grouplist[group].mbxtxqueue);
+// printf("mbxgetqueue item:%d mbx:%p\n\r",item, mbx);
+ osal_mutex_lock(mbxqueue->mbxmutex);
+ mbxqueue->mbxstate[ticketloc] = EC_MBXQUEUESTATE_NONE;
+ mbxqueue->mbxremove[ticketloc] = 0;
+ mbxqueue->mbxslave[ticketloc] = 0;
+ mbx = mbxqueue->mbx[ticketloc];
+ mbxqueue->mbx[ticketloc] = NULL;
+ mbxqueue->listtail++;
+ if(mbxqueue->listtail >= EC_MBXPOOLSIZE) mbxqueue->listtail = 0;
+ mbxqueue->listcount--;
+ osal_mutex_unlock(mbxqueue->mbxmutex);
+ return mbx;
+}
+
/** Read one byte from slave EEPROM via cache.
* If the cache location is empty then a read request is made to the slave.
* Depending on the slave capabilities the request is 4 or 8 bytes.
*/
void ec_clearmbx(ec_mbxbuft *Mbx)
{
+ if(Mbx)
memset(Mbx, 0x00, EC_MAXMBX);
}
+int ecx_clearmbxstatus(ecx_contextt *context, uint8 group)
+{
+ if(context->grouplist[group].mbxstatus && context->grouplist[group].mbxstatuslength)
+ {
+ memset(context->grouplist[group].mbxstatus, 0x00, context->grouplist[group].mbxstatuslength);
+ return 1;
+ }
+ return 0;
+}
+
+int ecx_readmbxstatus(ecx_contextt *context, uint16 slave, uint8 *SMstat)
+{
+ int wkc = 0;
+ if(context->slavelist[slave].mbxhandlerstate == ECT_MBXH_CYCLIC)
+ {
+ *SMstat = *(context->slavelist[slave].mbxstatus);
+ wkc = 1;
+ }
+ else
+ {
+ uint16 configadr = context->slavelist[slave].configadr;
+ wkc = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(uint8), SMstat, EC_TIMEOUTRET);
+ }
+ return wkc;
+}
+
+int ecx_readmbxstatusex(ecx_contextt *context, uint16 slave, uint16 *SMstatex)
+{
+ uint16 hu16;
+ uint16 configadr = context->slavelist[slave].configadr;
+ int wkc = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(hu16), &hu16, EC_TIMEOUTRET);
+ *SMstatex = etohs(hu16);
+ return wkc;
+}
+
/** Check if IN mailbox of slave is empty.
* @param[in] context = context struct
* @param[in] slave = Slave number
return 0;
}
+int ecx_mbxinhandler(ecx_contextt *context, uint8 group, int limit)
+{
+ int cnt, cntoffset, wkc, wkc2, limitcnt;
+ int maxcnt = context->grouplist[group].mbxstatuslength;
+ ec_mbxbuft *mbx;
+ ec_mbxheadert *mbxh;
+ ec_emcyt *EMp;
+ ec_mbxerrort *MBXEp;
+ uint8 SMcontr;
+ uint16 SMstatex;
+
+ limitcnt = 0;
+ int firstmbxpos = context->grouplist[group].lastmbxpos + 1;
+ int maxcntstored = maxcnt;
+ /* iterate over all possible mailbox slaves */
+ for(cnt = 0 ; cnt < maxcnt ; cnt++)
+ {
+ /* start from last stored slave position to allow fair handling under load */
+ cntoffset = firstmbxpos + cnt;
+ if(cntoffset >= maxcntstored) cntoffset -= maxcntstored;
+ context->grouplist[group].lastmbxpos = cntoffset;
+ uint16 slave = context->grouplist[group].mbxstatuslookup[cntoffset];
+ ec_slavet *slaveitem = &context->slavelist[slave];
+ uint16 configadr = slaveitem->configadr;
+ /* cyclic handler enabled for this slave */
+ if(slaveitem->mbxhandlerstate == ECT_MBXH_CYCLIC)
+ {
+ /* handle robust mailbox protocol state machine */
+ if(slaveitem->mbxrmpstate)
+ {
+ if(slaveitem->islost) slaveitem->mbxrmpstate = 0;
+ else
+ {
+ switch(slaveitem->mbxrmpstate)
+ {
+ case 1 :
+ if(ecx_readmbxstatusex(context, slave, &(slaveitem->mbxinstateex)) > 0)
+ {
+ slaveitem->mbxinstateex ^= 0x0200; /* toggle repeat request */
+ slaveitem->mbxrmpstate++;
+ }
+ break;
+ case 2 :
+ SMstatex = htoes(slaveitem->mbxinstateex);
+ if(ecx_FPWR(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstatex), &(slaveitem->mbxinstateex), EC_TIMEOUTRET) > 0)
+ {
+ slaveitem->mbxrmpstate++;
+ }
+ break;
+ case 3 :
+ /* wait for repeat ack */
+ wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1CONTR, sizeof(SMcontr), &SMcontr, EC_TIMEOUTRET);
+ if((wkc2 > 0) && ((SMcontr & 0x02) == (HI_BYTE(SMstatex) & 0x02)))
+ {
+ slaveitem->mbxrmpstate = 0;
+ }
+ break;
+ }
+ /* keep track of work limit */
+ if(++limitcnt >= limit) maxcnt = 0;
+ }
+ }
+ /* mbxin full detected */
+ else if((*(context->grouplist[group].mbxstatus + cntoffset) & 0x08) > 0)
+ {
+ uint16 mbxl = slaveitem->mbx_rl;
+ uint16 mbxro = slaveitem->mbx_ro;
+ if((mbxl > 0) && (mbx = ecx_getmbx(context)))
+ {
+ /* keep track of work limit */
+ if(++limitcnt >= limit) maxcnt = 0;
+ wkc = ecx_FPRD(context->port, configadr, mbxro, mbxl, mbx, EC_TIMEOUTRET); /* get mailbox */
+ if(wkc > 0)
+ {
+ mbxh = (ec_mbxheadert *)mbx;
+ if ((mbxh->mbxtype & 0x0f) == ECT_MBXT_ERR) /* Mailbox error response? */
+ {
+ MBXEp = (ec_mbxerrort *)mbx;
+ ecx_mbxerror(context, slave, etohs(MBXEp->Detail));
+ }
+ else if ((mbxh->mbxtype & 0x0f) == ECT_MBXT_COE) /* CoE response? */
+ {
+ EMp = (ec_emcyt *)mbx;
+ if ((etohs(EMp->CANOpen) >> 12) == 0x01) /* Emergency request? */
+ {
+ ecx_mbxemergencyerror(context, slave, etohs(EMp->ErrorCode), EMp->ErrorReg,
+ EMp->bData, etohs(EMp->w1), etohs(EMp->w2));
+ }
+ else
+ {
+ if(slaveitem->coembxin && (slaveitem->coembxinfull == FALSE))
+ {
+ slaveitem->coembxin = (uint8 *)mbx;
+ mbx = NULL;
+ slaveitem->coembxinfull = TRUE;
+ }
+ else
+ {
+ slaveitem->coembxoverrun++;
+ }
+ }
+ }
+ else if ((mbxh->mbxtype & 0x0f) == ECT_MBXT_SOE) /* SoE response? */
+ {
+ if(slaveitem->soembxin && (slaveitem->soembxinfull == FALSE))
+ {
+ slaveitem->soembxin = (uint8 *)mbx;
+ mbx = NULL;
+ slaveitem->soembxinfull = TRUE;
+ }
+ else
+ {
+ slaveitem->soembxoverrun++;
+ }
+ }
+ else if ((mbxh->mbxtype & 0x0f) == ECT_MBXT_EOE) /* EoE response? */
+ {
+ if(slaveitem->eoembxin && (slaveitem->eoembxinfull == FALSE))
+ {
+ slaveitem->eoembxin = (uint8 *)mbx;
+ mbx = NULL;
+ slaveitem->eoembxinfull = TRUE;
+ }
+ else
+ {
+ slaveitem->eoembxoverrun++;
+ }
+ }
+ else if ((mbxh->mbxtype & 0x0f) == ECT_MBXT_FOE) /* FoE response? */
+ {
+ if(slaveitem->foembxin && (slaveitem->foembxinfull == FALSE))
+ {
+ slaveitem->foembxin = (uint8 *)mbx;
+ mbx = NULL;
+ slaveitem->foembxinfull = TRUE;
+ }
+ else
+ {
+ slaveitem->foembxoverrun++;
+ }
+ }
+ else if ((mbxh->mbxtype & 0x0f) == ECT_MBXT_VOE) /* VoE response? */
+ {
+ if(slaveitem->voembxin && (slaveitem->voembxinfull == FALSE))
+ {
+ slaveitem->voembxin = (uint8 *)mbx;
+ mbx = NULL;
+ slaveitem->voembxinfull = TRUE;
+ }
+ else
+ {
+ slaveitem->voembxoverrun++;
+ }
+ }
+ else if ((mbxh->mbxtype & 0x0f) == ECT_MBXT_AOE) /* AoE response? */
+ {
+ if(slaveitem->aoembxin && (slaveitem->aoembxinfull == FALSE))
+ {
+ slaveitem->aoembxin = (uint8 *)mbx;
+ mbx = NULL;
+ slaveitem->aoembxinfull = TRUE;
+ }
+ else
+ {
+ slaveitem->aoembxoverrun++;
+ }
+ }
+ }
+ else
+ {
+ /* mailbox lost, initiate robust mailbox protocol */
+ slaveitem->mbxrmpstate = 1;
+ }
+ /* release mailbox to pool if still owner */
+ if (mbx)
+ {
+ ecx_dropmbx(context, mbx);
+ }
+ }
+ }
+ }
+ }
+ return limitcnt;
+}
+
+int ecx_mbxouthandler(ecx_contextt *context, uint8 group, int limit)
+{
+ int wkc;
+ int limitcnt = 0;
+ int ticketloc, state;
+ uint16 slave, mbxl, mbxwo, configadr;
+ ec_mbxbuft *mbx;
+ ec_mbxqueuet *mbxqueue = &(context->grouplist[group].mbxtxqueue);
+ int listcount = mbxqueue->listcount;
+ while((limitcnt <= limit) && listcount)
+ {
+ listcount--;
+ ticketloc = mbxqueue->listtail;
+ state = mbxqueue->mbxstate[ticketloc];
+ switch(state)
+ {
+ case EC_MBXQUEUESTATE_REQ :
+ case EC_MBXQUEUESTATE_FAIL:
+ slave = mbxqueue->mbxslave[ticketloc];
+ mbx = mbxqueue->mbx[ticketloc];
+ mbxl = context->slavelist[slave].mbx_l;
+ configadr = context->slavelist[slave].configadr;
+ mbxwo = context->slavelist[slave].mbx_wo;
+ limitcnt++;
+ if(context->slavelist[slave].state >= EC_STATE_PRE_OP)
+ {
+ /* write slave in mailbox 1st try*/
+ wkc = ecx_FPWR(context->port, configadr, mbxwo, mbxl, mbx, EC_TIMEOUTRET);
+ if(wkc > 0)
+ {
+ mbxqueue->mbxstate[ticketloc] = EC_MBXQUEUESTATE_DONE; // mbx tx ok
+ ecx_dropmbx(context, mbx);
+ mbxqueue->mbx[ticketloc] = NULL;
+ }
+ else
+ {
+ if(state != EC_MBXQUEUESTATE_FAIL)
+ mbxqueue->mbxstate[ticketloc] = EC_MBXQUEUESTATE_FAIL; // mbx tx fail, retry
+ }
+ }
+ /* fall through */
+ case EC_MBXQUEUESTATE_DONE: // mbx tx ok
+ ecx_mbxrotatequeue(context, group, ticketloc);
+ break;
+ }
+ if(mbxqueue->mbxremove[ticketloc])
+ {
+ mbx = ecx_mbxdropqueue(context, group, ticketloc);
+ if(mbx) ecx_dropmbx(context, mbx);
+ }
+ }
+ return limitcnt;
+}
+
+int ecx_mbxhandler(ecx_contextt *context, uint8 group, int limit)
+{
+ int limitcnt;
+ limitcnt = ecx_mbxinhandler(context, group, limit);
+ return ecx_mbxouthandler(context, group, (limit - limitcnt));
+ //return limitcnt;
+}
+
/** Write IN mailbox to slave.
+ * Mailbox is fetched from pool by caller, ownership is transferred and dropped back to pool automatically.
* @param[in] context = context struct
* @param[in] slave = Slave number
- * @param[out] mbx = Mailbox data
+ * @param[out] mbx = Pointer to mailbox data
* @param[in] timeout = Timeout in us
* @return Work counter (>0 is success)
*/
int ecx_mbxsend(ecx_contextt *context, uint16 slave,ec_mbxbuft *mbx, int timeout)
{
- uint16 mbxwo,mbxl,configadr;
- int wkc;
+ uint16 mbxwo, mbxl, configadr;
+ int wkc, ticket;
+ osal_timert timer;
+ ec_slavet *slavelist = &(context->slavelist[slave]);
wkc = 0;
- configadr = context->slavelist[slave].configadr;
- mbxl = context->slavelist[slave].mbx_l;
- if ((mbxl > 0) && (mbxl <= EC_MAXMBX))
+ configadr = slavelist->configadr;
+ mbxl = slavelist->mbx_l;
+ if (slavelist->mbxhandlerstate == ECT_MBXH_CYCLIC)
{
- if (ecx_mbxempty(context, slave, timeout))
+ osal_timer_start(&timer, timeout);
+ wkc = 0;
+ if(mbxl > 0)
{
- mbxwo = context->slavelist[slave].mbx_wo;
- /* write slave in mailbox */
- wkc = ecx_FPWR(context->port, configadr, mbxwo, mbxl, mbx, EC_TIMEOUTRET3);
+ do
+ {
+ if((ticket = ecx_mbxaddqueue(context, slave, mbx)) >= 0)
+ {
+ mbx = NULL;
+ do
+ {
+ wkc = ecx_mbxdonequeue(context, slave, ticket);
+ if (!wkc && (timeout > EC_LOCALDELAY))
+ {
+ osal_usleep(EC_LOCALDELAY);
+ }
+ }
+ while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE));
+ if(wkc <= 0)
+ {
+ if(!ecx_mbxexpirequeue(context, slave, ticket))
+ {
+// printf("expirequeue failed\n\r");
+ }
+ }
+ }
+ else if ((timeout > EC_LOCALDELAY))
+ {
+ osal_usleep(EC_LOCALDELAY);
+ }
+ }
+ while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE));
}
- else
+ }
+ else if ((mbxl > 0) && (mbxl <= EC_MAXMBX) && (slavelist->state >= EC_STATE_PRE_OP))
+ {
+ mbxwo = context->slavelist[slave].mbx_wo;
+ /* write slave in mailbox 1st try*/
+ wkc = ecx_FPWR(context->port, configadr, mbxwo, mbxl, mbx, EC_TIMEOUTRET3);
+ /* if failed wait for empty mailbox */
+ if ((wkc <= 0) && ecx_mbxempty(context, slave, timeout))
{
- wkc = 0;
+ /* retry */
+ wkc = ecx_FPWR(context->port, configadr, mbxwo, mbxl, mbx, EC_TIMEOUTRET3);
}
+ if(wkc < 0 ) wkc = 0;
}
-
+ if (mbx) ecx_dropmbx(context, mbx);
return wkc;
}
uint16 mbxro,mbxl,configadr;
int wkc=0;
int wkc2;
- uint16 SMstat;
+ uint8 SMstat;
+ uint16 SMstatex;
uint8 SMcontr;
ec_mbxheadert *mbxh;
ec_emcyt *EMp;
ec_mbxerrort *MBXEp;
+ osal_timert timer;
+ ec_slavet *slavelist = &(context->slavelist[slave]);
- configadr = context->slavelist[slave].configadr;
- mbxl = context->slavelist[slave].mbx_rl;
- if ((mbxl > 0) && (mbxl <= EC_MAXMBX))
+ configadr = slavelist->configadr;
+ mbxl = slavelist->mbx_rl;
+ if (slavelist->mbxhandlerstate == ECT_MBXH_CYCLIC)
+ {
+ osal_timer_start(&timer, timeout);
+ wkc = 0;
+ do
+ {
+ if (slavelist->coembxinfull == TRUE)
+ {
+ memcpy(mbx, slavelist->coembxin, mbxl);
+ ecx_dropmbx(context, (ec_mbxbuft *)slavelist->coembxin);
+ slavelist->coembxin = EC_MBXINENABLE;
+ slavelist->coembxinfull = FALSE;
+ wkc = 1;
+ }
+ else if (slavelist->soembxinfull == TRUE)
+ {
+ memcpy(mbx, slavelist->soembxin, mbxl);
+ slavelist->soembxinfull = FALSE;
+ wkc = 1;
+ }
+ else if (slavelist->foembxinfull == TRUE)
+ {
+ memcpy(mbx, slavelist->foembxin, mbxl);
+ slavelist->foembxinfull = FALSE;
+ wkc = 1;
+ }
+ if (!wkc && (timeout > EC_LOCALDELAY))
+ {
+ osal_usleep(EC_LOCALDELAY);
+ }
+ }
+ while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE));
+ }
+ else if ((mbxl > 0) && (mbxl <= EC_MAXMBX))
{
- osal_timert timer;
-
osal_timer_start(&timer, timeout);
wkc = 0;
do /* wait for read mailbox available */
{
SMstat = 0;
- wkc = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET);
- SMstat = etohs(SMstat);
+ wkc = ecx_readmbxstatus(context, slave, &SMstat);
if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY))
{
osal_usleep(EC_LOCALDELAY);
if ((wkc > 0) && ((SMstat & 0x08) > 0)) /* read mailbox available ? */
{
- mbxro = context->slavelist[slave].mbx_ro;
+ mbxro = slavelist->mbx_ro;
mbxh = (ec_mbxheadert *)mbx;
do
{
{
if (wkc <= 0) /* read mailbox lost */
{
- SMstat ^= 0x0200; /* toggle repeat request */
- SMstat = htoes(SMstat);
- wkc2 = ecx_FPWR(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET);
- SMstat = etohs(SMstat);
+ do /* read extended mailbox status */
+ {
+ wkc2 = ecx_readmbxstatusex(context, slave, &SMstatex);
+ } while ((wkc2 <= 0) && (osal_timer_is_expired(&timer) == FALSE));
+ SMstatex ^= 0x0200; /* toggle repeat request */
+ SMstatex = htoes(SMstatex);
+ wkc2 = ecx_FPWR(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstatex), &SMstatex, EC_TIMEOUTRET);
+ SMstatex = etohs(SMstatex);
+ do /* wait for toggle ack */
+ {
+ wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1CONTR, sizeof(SMcontr), &SMcontr, EC_TIMEOUTRET);
+ } while (((wkc2 <= 0) || ((SMcontr & 0x02) != (HI_BYTE(SMstatex) & 0x02))) && (osal_timer_is_expired(&timer) == FALSE));
+ do /* wait for read mailbox available */
+ {
+ wkc2 = ecx_readmbxstatusex(context, slave, &SMstatex);
+ if (((SMstatex & 0x08) == 0) && (timeout > EC_LOCALDELAY))
+ {
+ osal_usleep(EC_LOCALDELAY);
+ }
+ } while (((wkc2 <= 0) || ((SMstatex & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE));
+ }
+ }
+ } while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE)); /* if WKC<=0 repeat */
+ }
+ else /* no read mailbox available */
+ {
+ wkc = 0;
+ }
+ }
+
+ return wkc;
+}
+
+/** Read OUT mailbox from slave.
+ * Supports Mailbox Link Layer with repeat requests.
+ * Mailbox is fetched from pool, caller is owner after return
+ * and therefore should drop it back to the pool whe finished.
+ * @param[in] context = context struct
+ * @param[in] slave = Slave number
+ * @param[out] mbx = Double pointer to mailbox data
+ * @param[in] timeout = Timeout in us
+ * @return Work counter (>0 is success)
+ */
+int ecx_mbxreceive2(ecx_contextt *context, uint16 slave, ec_mbxbuft **mbx, int timeout)
+{
+ uint16 mbxro,mbxl,configadr;
+ int wkc=0;
+ int wkc2;
+ uint8 SMstat;
+ uint16 SMstatex;
+ uint8 SMcontr;
+ ec_mbxbuft *mbxin;
+ ec_mbxheadert *mbxh;
+ ec_emcyt *EMp;
+ ec_mbxerrort *MBXEp;
+ osal_timert timer;
+ ec_slavet *slavelist = &(context->slavelist[slave]);
+
+ configadr = slavelist->configadr;
+ mbxl = slavelist->mbx_rl;
+ if (slavelist->mbxhandlerstate == ECT_MBXH_CYCLIC)
+ {
+ osal_timer_start(&timer, timeout);
+ wkc = 0;
+ do
+ {
+ if (slavelist->coembxinfull == TRUE)
+ {
+ *mbx = (ec_mbxbuft *)slavelist->coembxin;
+ slavelist->coembxin = EC_MBXINENABLE;
+ slavelist->coembxinfull = FALSE;
+ wkc = 1;
+ }
+ else if (slavelist->soembxinfull == TRUE)
+ {
+ *mbx = (ec_mbxbuft *)slavelist->soembxin;
+ slavelist->soembxin = EC_MBXINENABLE;
+ slavelist->soembxinfull = FALSE;
+ wkc = 1;
+ }
+ else if (slavelist->foembxinfull == TRUE)
+ {
+ *mbx = (ec_mbxbuft *)slavelist->foembxin;
+ slavelist->foembxin = EC_MBXINENABLE;
+ slavelist->foembxinfull = FALSE;
+ wkc = 1;
+ }
+ if (!wkc && (timeout > EC_LOCALDELAY))
+ {
+ osal_usleep(EC_LOCALDELAY);
+ }
+ }
+ while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE));
+ }
+ else if ((mbxl > 0) && (mbxl <= EC_MAXMBX))
+ {
+ osal_timer_start(&timer, timeout);
+ wkc = 0;
+ do /* wait for read mailbox available */
+ {
+ SMstat = 0;
+ wkc = ecx_readmbxstatus(context, slave, &SMstat);
+ if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY))
+ {
+ osal_usleep(EC_LOCALDELAY);
+ }
+ }
+ while (((wkc <= 0) || ((SMstat & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE));
+
+ if ((wkc > 0) && ((SMstat & 0x08) > 0)) /* read mailbox available ? */
+ {
+ mbxro = slavelist->mbx_ro;
+ mbxin = ecx_getmbx(context);
+ mbxh = (ec_mbxheadert *)mbxin;
+ do
+ {
+ wkc = ecx_FPRD(context->port, configadr, mbxro, mbxl, mbxin, EC_TIMEOUTRET); /* get mailbox */
+ if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == 0x00)) /* Mailbox error response? */
+ {
+ MBXEp = (ec_mbxerrort *)mbxin;
+ ecx_mbxerror(context, slave, etohs(MBXEp->Detail));
+ ecx_dropmbx(context, mbxin);
+ mbxin = NULL;
+ wkc = 0; /* prevent emergency to cascade up, it is already handled. */
+ }
+ else if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == 0x03)) /* CoE response? */
+ {
+ EMp = (ec_emcyt *)mbx;
+ if ((etohs(EMp->CANOpen) >> 12) == 0x01) /* Emergency request? */
+ {
+ ecx_mbxemergencyerror(context, slave, etohs(EMp->ErrorCode), EMp->ErrorReg,
+ EMp->bData, etohs(EMp->w1), etohs(EMp->w2));
+ ecx_dropmbx(context, mbxin);
+ mbxin = NULL;
+ wkc = 0; /* prevent emergency to cascade up, it is already handled. */
+ }
+ else
+ {
+ *mbx = mbxin;
+ mbxin = NULL;
+ }
+ }
+ else
+ {
+ if (wkc <= 0) /* read mailbox lost */
+ {
+ do /* read extended mailbox status */
+ {
+ wkc2 = ecx_readmbxstatusex(context, slave, &SMstatex);
+ } while ((wkc2 <= 0) && (osal_timer_is_expired(&timer) == FALSE));
+ SMstatex ^= 0x0200; /* toggle repeat request */
+ SMstatex = htoes(SMstatex);
+ wkc2 = ecx_FPWR(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstatex), &SMstatex, EC_TIMEOUTRET);
+ SMstatex = etohs(SMstatex);
do /* wait for toggle ack */
{
wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1CONTR, sizeof(SMcontr), &SMcontr, EC_TIMEOUTRET);
- } while (((wkc2 <= 0) || ((SMcontr & 0x02) != (HI_BYTE(SMstat) & 0x02))) && (osal_timer_is_expired(&timer) == FALSE));
+ } while (((wkc2 <= 0) || ((SMcontr & 0x02) != (HI_BYTE(SMstatex) & 0x02))) && (osal_timer_is_expired(&timer) == FALSE));
do /* wait for read mailbox available */
{
- wkc2 = ecx_FPRD(context->port, configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET);
- SMstat = etohs(SMstat);
- if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY))
+ wkc2 = ecx_readmbxstatusex(context, slave, &SMstatex);
+ if (((SMstatex & 0x08) == 0) && (timeout > EC_LOCALDELAY))
{
osal_usleep(EC_LOCALDELAY);
}
- } while (((wkc2 <= 0) || ((SMstat & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE));
+ } while (((wkc2 <= 0) || ((SMstatex & 0x08) == 0)) && (osal_timer_is_expired(&timer) == FALSE));
}
}
} while ((wkc <= 0) && (osal_timer_is_expired(&timer) == FALSE)); /* if WKC<=0 repeat */
+ if(mbxin) ecx_dropmbx(context, mbxin);
}
else /* no read mailbox available */
{
first = TRUE;
}
+ ecx_clearmbxstatus(context, group);
+
/* For overlapping IO map use the biggest */
if(use_overlap_io == TRUE)
{
} ec_state_status;
PACKED_END
+/** mailbox buffer array */
+typedef uint8 ec_mbxbuft[EC_MAXMBX + 1];
+
+#define EC_MBXPOOLSIZE 32
+#define EC_MBXINENABLE (uint8 *)1
+
+typedef struct
+{
+ int listhead, listtail, listcount;
+ int mbxemptylist[EC_MBXPOOLSIZE];
+ osal_mutext *mbxmutex;
+ ec_mbxbuft mbx[EC_MBXPOOLSIZE];
+} ec_mbxpoolt;
+
+#define EC_MBXQUEUESTATE_NONE 0
+#define EC_MBXQUEUESTATE_REQ 1
+#define EC_MBXQUEUESTATE_FAIL 2
+#define EC_MBXQUEUESTATE_DONE 3
+
+typedef struct
+{
+ int listhead, listtail, listcount;
+ ec_mbxbuft *mbx[EC_MBXPOOLSIZE];
+ int mbxstate[EC_MBXPOOLSIZE];
+ int mbxremove[EC_MBXPOOLSIZE];
+ int mbxticket[EC_MBXPOOLSIZE];
+ uint16 mbxslave[EC_MBXPOOLSIZE];
+ osal_mutext *mbxmutex;
+} ec_mbxqueuet;
+
#define ECT_MBXPROT_AOE 0x0001
#define ECT_MBXPROT_EOE 0x0002
#define ECT_MBXPROT_COE 0x0004
typedef struct ecx_context ecx_contextt;
+#define ECT_MBXH_NONE 0
+#define ECT_MBXH_CYCLIC 1
+#define ECT_MBXH_LOST 2
+
/** for list of ethercat slaves detected */
typedef struct ec_slave
{
/** revision from EEprom */
uint32 eep_rev;
/** serial number from EEprom */
- uint32 eep_sn;
+ uint32 eep_ser;
/** Interface type */
uint16 Itype;
/** Device type */
uint32 Obytes;
/** output pointer in IOmap buffer */
uint8 *outputs;
+ /** output offset in IOmap buffer */
+ uint32 Ooffset;
/** startbit in first output byte */
uint8 Ostartbit;
/** input bits */
uint32 Ibytes;
/** input pointer in IOmap buffer */
uint8 *inputs;
+ /** input offset in IOmap buffer */
+ uint32 Ioffset;
/** startbit in first input byte */
uint8 Istartbit;
/** SM structure */
uint8 SMtype[EC_MAXSM];
/** FMMU structure */
ec_fmmut FMMU[EC_MAXFMMU];
- /** FMMU0 function */
+ /** FMMU0 function 0=unused 1=outputs 2=inputs 3=SM status*/
uint8 FMMU0func;
/** FMMU1 function */
uint8 FMMU1func;
int (*PO2SOconfig)(uint16 slave);
/** registered configuration function PO->SO */
int (*PO2SOconfigx)(ecx_contextt * context, uint16 slave);
+ /** mailbox handler state, 0 = no handler, 1 = cyclic task mbx handler, 2 = slave lost */
+ int mbxhandlerstate;
+ /** mailbox handler robust mailbox protocol state */
+ int mbxrmpstate;
+ /** mailbox handler RMP extended mbx in state */
+ uint16 mbxinstateex;
+ /** pointer to CoE mailbox in buffer */
+ uint8 *coembxin;
+ /** CoE mailbox in flag, true = mailbox full */
+ boolean coembxinfull;
+ /** CoE mailbox in overrun counter */
+ int coembxoverrun;
+ /** pointer to SoE mailbox in buffer */
+ uint8 *soembxin;
+ /** SoE mailbox in flag, true = mailbox full */
+ boolean soembxinfull;
+ /** SoE mailbox in overrun counter */
+ int soembxoverrun;
+ /** pointer to FoE mailbox in buffer */
+ uint8 *foembxin;
+ /** FoE mailbox in flag, true = mailbox full */
+ boolean foembxinfull;
+ /** FoE mailbox in overrun counter */
+ int foembxoverrun;
+ /** pointer to EoE mailbox in buffer */
+ uint8 *eoembxin;
+ /** EoE mailbox in flag, true = mailbox full */
+ boolean eoembxinfull;
+ /** EoE mailbox in overrun counter */
+ int eoembxoverrun;
+ /** pointer to VoE mailbox in buffer */
+ uint8 *voembxin;
+ /** VoE mailbox in flag, true = mailbox full */
+ boolean voembxinfull;
+ /** VoE mailbox in overrun counter */
+ int voembxoverrun;
+ /** pointer to AoE mailbox in buffer */
+ uint8 *aoembxin;
+ /** AoE mailbox in flag, true = mailbox full */
+ boolean aoembxinfull;
+ /** AoE mailbox in overrun counter */
+ int aoembxoverrun;
+ /** pointer to out mailbox status register buffer */
+ uint8 *mbxstatus;
/** readable name */
char name[EC_MAXNAME + 1];
} ec_slavet;
boolean docheckstate;
/** IO segmentation list. Datagrams must not break SM in two. */
uint32 IOsegment[EC_MAXIOSEGMENTS];
+ /** pointer to out mailbox status register buffer */
+ uint8 *mbxstatus;
+ /** mailbox status register buffer length */
+ int32 mbxstatuslength;
+ /** mailbox status lookup table */
+ uint16 mbxstatuslookup[EC_MAXSLAVE];
+ /** mailbox last handled in mxbhandler */
+ uint16 lastmbxpos;
+ /** mailbox transmit queue struct */
+ ec_mbxqueuet mbxtxqueue;
} ec_groupt;
/** SII FMMU structure */
uint16 SMbitsize[EC_MAXSM];
} ec_eepromPDOt;
-/** mailbox buffer array */
-typedef uint8 ec_mbxbuft[EC_MAXMBX + 1];
-
/** standard ethercat mailbox header */
PACKED_BEGIN
typedef struct PACKED ec_mbxheader
void *data[EC_MAXBUF];
uint16 length[EC_MAXBUF];
uint16 dcoffset[EC_MAXBUF];
+ uint8 type[EC_MAXBUF];
} ec_idxstackT;
/** ringbuf for error storage */
ec_eepromSMt *eepSM;
/** internal, FMMU list from eeprom */
ec_eepromFMMUt *eepFMMU;
+ /** internal, mailbox pool */
+ ec_mbxpoolt *mbxpool;
/** registered FoE hook */
int (*FOEhook)(uint16 slave, int packetnumber, int datasize);
/** registered EoE hook */
int (*EOEhook)(ecx_contextt * context, uint16 slave, void * eoembx);
/** flag to control legacy automatic state change or manual state change */
int manualstatechange;
- /** userdata, promotes application configuration esp. in EC_VER2 with multiple
+ /** userdata, promotes application configuration esp. in EC_VER2 with multiple
* ec_context instances. Note: userdata memory is managed by application, not SOEM */
void *userdata;
};
extern ec_groupt ec_group[EC_MAXGROUP];
extern boolean EcatError;
extern int64 ec_DCtime;
+extern ec_mbxpoolt ec_mbxpool;
void ec_pusherror(const ec_errort *Ec);
boolean ec_poperror(ec_errort *Ec);
int ecx_readstate(ecx_contextt *context);
int ecx_writestate(ecx_contextt *context, uint16 slave);
uint16 ecx_statecheck(ecx_contextt *context, uint16 slave, uint16 reqstate, int timeout);
+int ecx_mbxhandler(ecx_contextt *context, uint8 group, int limit);
int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout);
int ecx_mbxsend(ecx_contextt *context, uint16 slave,ec_mbxbuft *mbx, int timeout);
int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout);
+int ecx_mbxreceive2(ecx_contextt *context, uint16 slave, ec_mbxbuft **mbx, int timeout);
void ecx_esidump(ecx_contextt *context, uint16 slave, uint8 *esibuf);
uint32 ecx_readeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, int timeout);
int ecx_writeeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, uint16 data, int timeout);
int ecx_send_overlap_processdata(ecx_contextt *context);
int ecx_receive_processdata(ecx_contextt *context, int timeout);
int ecx_send_processdata_group(ecx_contextt *context, uint8 group);
+ec_mbxbuft *ecx_getmbx(ecx_contextt *context);
+int ecx_dropmbx(ecx_contextt *context, ec_mbxbuft *mbx);
+int ecx_initmbxpool(ecx_contextt *context);
+int ecx_initmbxqueue(ecx_contextt *context, uint16 group);
+int ecx_slavembxcyclic(ecx_contextt *context, uint16 slave);
#ifdef __cplusplus
}
uint8 *bp;
uint8 *mp;
uint16 *errorcode;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
boolean NotLast;
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSoEp = (ec_SoEt *)&MbxIn;
- SoEp = (ec_SoEt *)&MbxOut;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ /* Empty slave out mailbox if something is in. Timout set to 0 */
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, 0);
+ MbxOut = ecx_getmbx(context);
+ ec_clearmbx(MbxOut);
+ SoEp = (ec_SoEt *)MbxOut;
SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
SoEp->MbxHeader.address = htoes(0x0000);
SoEp->MbxHeader.priority = 0x00;
SoEp->idn = htoes(idn);
totalsize = 0;
bp = p;
- mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
+ mp = (uint8 *)MbxIn + sizeof(ec_SoEt);
NotLast = TRUE;
/* send SoE request to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
while (NotLast)
{
- /* clean mailboxbuffer */
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
+ aSoEp = (ec_SoEt *)MbxIn;
/* slave response should be SoE, ReadRes */
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 1))
{
- mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
+ mp = (uint8 *)MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
errorcode = (uint16 *)mp;
ecx_SoEerror(context, slave, idn, *errorcode);
}
}
}
}
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ if(MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
uint8 *mp;
uint8 *hp;
uint16 *errorcode;
- ec_mbxbuft MbxIn, MbxOut;
+ ec_mbxbuft *MbxIn, *MbxOut;
uint8 cnt;
boolean NotLast;
- ec_clearmbx(&MbxIn);
- /* Empty slave out mailbox if something is in. Timeout set to 0 */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
- ec_clearmbx(&MbxOut);
- aSoEp = (ec_SoEt *)&MbxIn;
- SoEp = (ec_SoEt *)&MbxOut;
- SoEp->MbxHeader.address = htoes(0x0000);
- SoEp->MbxHeader.priority = 0x00;
- SoEp->opCode = ECT_SOE_WRITEREQ;
- SoEp->error = 0;
- SoEp->driveNo = driveNo;
- SoEp->elementflags = elementflags;
+ MbxIn = NULL;
+ MbxOut = NULL;
+ /* Empty slave out mailbox if something is in. Timout set to 0 */
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, 0);
hp = p;
- mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt);
NotLast = TRUE;
while (NotLast)
{
+ MbxOut = ecx_getmbx(context);
+ ec_clearmbx(MbxOut);
+ SoEp = (ec_SoEt *)MbxOut;
+ SoEp->MbxHeader.address = htoes(0x0000);
+ SoEp->MbxHeader.priority = 0x00;
+ SoEp->opCode = ECT_SOE_WRITEREQ;
+ SoEp->error = 0;
+ SoEp->driveNo = driveNo;
+ SoEp->elementflags = elementflags;
+ mp = (uint8 *)MbxOut + sizeof(ec_SoEt);
framedatasize = psize;
NotLast = FALSE;
SoEp->idn = htoes(idn);
hp += framedatasize;
psize -= framedatasize;
/* send SoE request to slave */
- wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
+ wkc = ecx_mbxsend(context, slave, MbxOut, EC_TIMEOUTTXM);
+ MbxOut = NULL;
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
if (!NotLast || !ecx_mbxempty(context, slave, timeout))
{
- /* clean mailboxbuffer */
- ec_clearmbx(&MbxIn);
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ MbxIn = NULL;
/* read slave response */
- wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
+ wkc = ecx_mbxreceive2(context, slave, &MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
+ aSoEp = (ec_SoEt *)MbxIn;
NotLast = FALSE;
/* slave response should be SoE, WriteRes */
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 1))
{
- mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
+ mp = (uint8 *)MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
errorcode = (uint16 *)mp;
ecx_SoEerror(context, slave, idn, *errorcode);
}
}
}
}
+ if(MbxIn) ecx_dropmbx(context, MbxIn);
+ if(MbxOut) ecx_dropmbx(context, MbxOut);
return wkc;
}
ECT_SII_MANUF = 0x0008,
ECT_SII_ID = 0x000a,
ECT_SII_REV = 0x000c,
- ECT_SII_SN = 0x000e,
+ ECT_SII_SER = 0x000e,
ECT_SII_BOOTRXMBX = 0x0014,
ECT_SII_BOOTTXMBX = 0x0016,
ECT_SII_MBXSIZE = 0x0019,
--- /dev/null
+set(SOURCES coetest.c)
+add_executable(coetest ${SOURCES})
+target_link_libraries(coetest soem)
\ No newline at end of file
--- /dev/null
+/** \file
+ * \brief CoE example code for Simple Open EtherCAT master
+ *
+ * Usage : coetest [ifname1]
+ * ifname is NIC interface, f.e. eth0
+ *
+ * (c)Arthur Ketels 2010 - 2024
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <inttypes.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <time.h>
+#include <pthread.h>
+
+#include "ethercat.h"
+
+boolean forceByteAlignment = FALSE;
+#define EC_TIMEOUTMON 500
+
+char IOmap[4096];
+OSAL_THREAD_HANDLE threadrt, thread1;
+int expectedWKC;
+int mappingdone, dorun, inOP, dowkccheck;
+int adapterisbound, conf_io_size, currentgroup;
+int64_t cycletime = 1000000;
+#define NSEC_PER_MSEC 1000000
+#define NSEC_PER_SEC 1000000000
+
+/* add ns to timespec */
+void add_timespec(struct timespec *ts, int64 addtime)
+{
+ int64 sec, nsec;
+
+ nsec = addtime % NSEC_PER_SEC;
+ sec = (addtime - nsec) / NSEC_PER_SEC;
+ ts->tv_sec += sec;
+ ts->tv_nsec += nsec;
+ if ( ts->tv_nsec >= NSEC_PER_SEC )
+ {
+ nsec = ts->tv_nsec % NSEC_PER_SEC;
+ ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
+ ts->tv_nsec = nsec;
+ }
+}
+
+static float pgain = 0.01f;
+static float igain = 0.00002f;
+/* set linux sync point 500us later than DC sync, just as example */
+static int64 syncoffset = 500000;
+int64 timeerror;
+
+/* PI calculation to get linux time synced to DC time */
+void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime)
+{
+ static int64 integral = 0;
+ int64 delta;
+ delta = (reftime - syncoffset) % cycletime;
+ if(delta> (cycletime / 2)) { delta = delta - cycletime; }
+ timeerror = -delta;
+ integral += timeerror;
+ *offsettime = (timeerror * pgain) + (integral * igain);
+}
+
+/* RT EtherCAT thread */
+OSAL_THREAD_FUNC_RT ecatthread(void)
+{
+ struct timespec ts, tleft;
+ int ht, wkc;
+ static int64_t toff = 0;
+
+ dorun = 0;
+ while(!mappingdone) { osal_usleep(100); }
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
+ ts.tv_nsec = ht * 1000000;
+ ecx_send_processdata(&ecx_context);
+ while(1)
+ {
+ /* calculate next cycle start */
+ add_timespec(&ts, cycletime + toff);
+ /* wait to cycle start */
+ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft);
+ if (dorun > 0)
+ {
+ wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);
+ if(wkc != expectedWKC) dowkccheck++; else dowkccheck = 0;
+ if (ec_slave[0].hasdc && (wkc > 0))
+ {
+ /* calulate toff to get linux time and DC synced */
+ ec_sync(ec_DCtime, cycletime, &toff);
+ }
+ ecx_mbxhandler(&ecx_context, 0, 4);
+ ecx_send_processdata(&ecx_context);
+ }
+ }
+}
+
+OSAL_THREAD_FUNC ecatcheck( void)
+{
+ int slave;
+
+ while(1)
+ {
+ if( inOP && ((dowkccheck > 2) || ec_group[currentgroup].docheckstate))
+ {
+ /* 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_INIT)
+ {
+ 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. State %2.2x\n",slave, ec_slave[slave].state);
+ }
+ }
+ }
+ if(!ec_group[currentgroup].docheckstate)
+ printf("OK : all slaves resumed OPERATIONAL.\n");
+ dowkccheck = 0;
+ }
+ osal_usleep(10000);
+ }
+}
+
+void ethercatstartup(char *ifname)
+{
+ printf("EtherCAT Startup\n");
+ int rv = ecx_init(&ecx_context, ifname);
+ if(rv)
+ {
+ adapterisbound = 1;
+ ecx_config_init(&ecx_context, FALSE);
+ if(ec_slavecount > 0)
+ {
+ if(forceByteAlignment)
+ conf_io_size = ecx_config_map_group_aligned(&ecx_context, &IOmap, 0);
+ else
+ conf_io_size = ecx_config_map_group(&ecx_context, &IOmap, 0);
+ expectedWKC = (ecx_context.grouplist[0].outputsWKC * 2) + ecx_context.grouplist[0].inputsWKC;
+ mappingdone = 1;
+ ecx_configdc(&ecx_context);
+ int sdoslave = -1;
+ for(int si = 1 ; si <= *ecx_context.slavecount ; si++ )
+ {
+ ec_slavet *slave = &ecx_context.slavelist[si];
+ printf("Slave %d name:%s man:%8.8x id:%8.8x rev:%d ser:%d\n", si, slave->name, slave->eep_man, slave->eep_id, slave->eep_rev, slave->eep_ser);
+ if(slave->CoEdetails > 0)
+ {
+ ecx_slavembxcyclic(&ecx_context, si);
+ sdoslave = si;
+ printf(" Slave added to cyclic mailbox handler\n");
+ }
+ }
+ dorun = 1;
+ osal_usleep(1000000);
+ ecx_context.slavelist[0].state = EC_STATE_OPERATIONAL;
+ ecx_writestate(&ecx_context, 0);
+ ecx_statecheck(&ecx_context, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
+ inOP = 1;
+ printf("EtherCAT OP\n");
+ uint32_t aval = 0;
+ int avals = sizeof(aval);
+ for(int i = 0 ; i < 100 ; i++)
+ {
+ printf("Cycle %d timeerror %ld\n", i, timeerror);
+ if(sdoslave >0)
+ {
+ aval = 0;
+ int rval = ecx_SDOread(&ecx_context, sdoslave, 0x1018, 0x02, FALSE, &avals, &aval, EC_TIMEOUTRXM);
+ printf(" Slave %d Rval %d Prodcode %8.8x\n", sdoslave, rval , aval);
+ }
+ osal_usleep(100000);
+ }
+ inOP = 0;
+ printf("EtherCAT to safe-OP\n");
+ ecx_context.slavelist[0].state = EC_STATE_SAFE_OP;
+ ecx_writestate(&ecx_context, 0);
+ ecx_statecheck(&ecx_context, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
+ printf("EtherCAT to INIT\n");
+ ecx_context.slavelist[0].state = EC_STATE_INIT;
+ ecx_writestate(&ecx_context, 0);
+ ecx_statecheck(&ecx_context, 0, EC_STATE_INIT, EC_TIMEOUTSTATE);
+ }
+ }
+}
+
+int main(int argc, char *argv[])
+{
+ printf("SOEM (Simple Open EtherCAT Master)\nCoEtest\n");
+
+ if (argc > 1)
+ {
+ /* create thread to handle slave error handling in OP */
+ osal_thread_create_rt(&threadrt, 128000, &ecatthread, NULL);
+ /* create thread to handle slave error handling in OP */
+ osal_thread_create(&thread1, 128000, &ecatcheck, NULL);
+ /* start cyclic part */
+ ethercatstartup(argv[1]);
+ }
+ else
+ {
+ ec_adaptert * adapter = NULL;
+ printf("Usage: coetest ifname1\nifname = eth0 for example\n");
+
+ printf ("\nAvailable adapters:\n");
+ adapter = ec_find_adapters ();
+ while (adapter != NULL)
+ {
+ printf (" - %s (%s)\n", adapter->name, adapter->desc);
+ adapter = adapter->next;
+ }
+ ec_free_adapters(adapter);
+ }
+
+ printf("End program\n");
+ return (0);
+}
{
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
ec_slave[slave].state = EC_STATE_OPERATIONAL;
+ if(ec_slave[slave].mbxhandlerstate == ECT_MBXH_LOST) ec_slave[slave].mbxhandlerstate = ECT_MBXH_CYCLIC;
ec_writestate(slave);
}
else if(ec_slave[slave].state > EC_STATE_NONE)
{
- if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
+ if (ec_reconfig_slave(slave, EC_TIMEOUTMON) >= EC_STATE_PRE_OP)
{
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d reconfigured\n",slave);
if (ec_slave[slave].state == EC_STATE_NONE)
{
ec_slave[slave].islost = TRUE;
+ ec_slave[slave].mbxhandlerstate = ECT_MBXH_LOST;
+ /* zero input data for this slave */
+ if(ec_slave[slave].Ibytes)
+ {
+ memset(ec_slave[slave].inputs, 0x00, ec_slave[slave].Ibytes);
+ printf("zero inputs %p %d\n\r", ec_slave[slave].inputs, ec_slave[slave].Ibytes);
+ }
printf("ERROR : slave %d lost\n",slave);
}
}
}
if (ec_slave[slave].islost)
{
- if(ec_slave[slave].state == EC_STATE_NONE)
+ if(ec_slave[slave].state <= EC_STATE_INIT)
{
if (ec_recover_slave(slave, EC_TIMEOUTMON))
{
printf(" CoE Object Description found, %d entries.\n",ODlist.Entries);
for( i = 0 ; i < ODlist.Entries ; i++)
{
- uint8_t max_sub;
+ uint16_t max_sub;
char name[128] = { 0 };
ec_readODdescription(i, &ODlist);
projects / 0xmirror / SOEM.git / commitdiff