From 3cc1e9739eda053b89fae281d5d881830cd9a179 Mon Sep 17 00:00:00 2001 From: Nick Tsiogkas Date: Thu, 13 Jul 2017 16:40:13 +0200 Subject: [PATCH] Integrate SOEM with RTEMS --- CMakeLists.txt | 9 +- cmake/Modules/Platform/rtems.cmake | 22 + osal/rtems/osal.c | 144 ++++++ osal/rtems/osal_defs.h | 29 ++ oshw/linux/nicdrv.c | 2 +- oshw/rtems/nicdrv.c | 687 +++++++++++++++++++++++++++++ oshw/rtems/nicdrv.h | 120 +++++ oshw/rtems/oshw.c | 123 ++++++ oshw/rtems/oshw.h | 31 ++ soem/ethercatbase.c | 2 +- 10 files changed, 1166 insertions(+), 3 deletions(-) create mode 100644 cmake/Modules/Platform/rtems.cmake create mode 100644 osal/rtems/osal.c create mode 100644 osal/rtems/osal_defs.h create mode 100644 oshw/rtems/nicdrv.c create mode 100644 oshw/rtems/nicdrv.h create mode 100644 oshw/rtems/oshw.c create mode 100644 oshw/rtems/oshw.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b54bd56..55f0f15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ if (CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) endif() set(SOEM_INCLUDE_INSTALL_DIR include/soem) +set(SOEM_LIB_INSTALL_DIR lib) if(WIN32) set(OS "win32") @@ -32,6 +33,10 @@ elseif(${CMAKE_SYSTEM_NAME} MATCHES "rt-kernel") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused-function") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-format") set(OS_LIBS "-Wl,--start-group -l${BSP} -l${ARCH} -lkern -ldev -lsio -lblock -lfs -lusb -llwip -leth -li2c -lrtc -lcan -lnand -lspi -lnor -lpwm -ladc -ltrace -lc -lm -Wl,--end-group") +elseif(${CMAKE_SYSTEM_NAME} MATCHES "rtems") + message("Building for RTEMS") + set(OS "rtems") + set(SOEM_LIB_INSTALL_DIR ${LIB_DIR}) endif() message("OS is ${OS}") @@ -56,7 +61,9 @@ add_library(soem STATIC ${OSHW_EXTRA_SOURCES}) target_link_libraries(soem ${OS_LIBS}) -install(TARGETS soem DESTINATION lib) +message("LIB_DIR: ${SOEM_LIB_INSTALL_DIR}") + +install(TARGETS soem DESTINATION ${SOEM_LIB_INSTALL_DIR}) install(FILES ${SOEM_HEADERS} ${OSAL_HEADERS} diff --git a/cmake/Modules/Platform/rtems.cmake b/cmake/Modules/Platform/rtems.cmake new file mode 100644 index 0000000..97b2293 --- /dev/null +++ b/cmake/Modules/Platform/rtems.cmake @@ -0,0 +1,22 @@ +message("rtems.cmake") + +set(ARCH ${HOST}) +set(BSP ${RTEMS_BSP}) + +set(CMAKE_C_COMPILER_FORCED true) +set(CMAKE_CXX_COMPILER_FORCED true) +set(CMAKE_C_COMPILER ${RTEMS_TOOLS_PATH}/bin/${ARCH}-gcc) +set(CMAKE_CXX_COMPILER ${RTEMS_TOOLS_PATH}/bin/${ARCH}-g++) + +set(SOEM_INCLUDE_INSTALL_DIR ${INCLUDE_DIR}/soem) +set(SOEM_LIB_INSTALL_DIR ${LIB_DIR}) + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${HOST_C_FLAGS}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${HOST_CXX_FLAGS}") + +if(NOT ${HOST_LIBS} STREQUAL "") + set(OS_LIBS "rtemscpu bsd ${HOST_LIBS}") +else() + set(OS_LIBS "-lrtemscpu -lbsd") +endif() + diff --git a/osal/rtems/osal.c b/osal/rtems/osal.c new file mode 100644 index 0000000..8ead2e8 --- /dev/null +++ b/osal/rtems/osal.c @@ -0,0 +1,144 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include + +#define USECS_PER_SEC 1000000 + +int osal_usleep (uint32 usec) +{ + struct timespec ts; + ts.tv_sec = usec / USECS_PER_SEC; + ts.tv_nsec = (usec % USECS_PER_SEC) * 1000; + /* usleep is depricated, use nanosleep instead */ + return nanosleep(&ts, NULL); +} + +int osal_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + struct timespec ts; + int return_value; + (void)tz; /* Not used */ + + /* Use clock_gettime to prevent possible live-lock. + * Gettimeofday uses CLOCK_REALTIME that can get NTP timeadjust. + * If this function preempts timeadjust and it uses vpage it live-locks. + * Also when using XENOMAI, only clock_gettime is RT safe */ + return_value = clock_gettime(CLOCK_MONOTONIC, &ts); + tv->tv_sec = ts.tv_sec; + tv->tv_usec = ts.tv_nsec / 1000; + return return_value; +} + +ec_timet osal_current_time(void) +{ + struct timeval current_time; + ec_timet return_value; + + osal_gettimeofday(¤t_time, 0); + return_value.sec = current_time.tv_sec; + return_value.usec = current_time.tv_usec; + return return_value; +} + +void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff) +{ + if (end->usec < start->usec) { + diff->sec = end->sec - start->sec - 1; + diff->usec = end->usec + 1000000 - start->usec; + } + else { + diff->sec = end->sec - start->sec; + diff->usec = end->usec - start->usec; + } +} + +void osal_timer_start(osal_timert * self, uint32 timeout_usec) +{ + struct timeval start_time; + struct timeval timeout; + struct timeval stop_time; + + osal_gettimeofday(&start_time, 0); + timeout.tv_sec = timeout_usec / USECS_PER_SEC; + timeout.tv_usec = timeout_usec % USECS_PER_SEC; + timeradd(&start_time, &timeout, &stop_time); + + self->stop_time.sec = stop_time.tv_sec; + self->stop_time.usec = stop_time.tv_usec; +} + +boolean osal_timer_is_expired (osal_timert * self) +{ + struct timeval current_time; + struct timeval stop_time; + int is_not_yet_expired; + + osal_gettimeofday(¤t_time, 0); + stop_time.tv_sec = self->stop_time.sec; + stop_time.tv_usec = self->stop_time.usec; + is_not_yet_expired = timercmp(¤t_time, &stop_time, <); + + return is_not_yet_expired == FALSE; +} + +void *osal_malloc(size_t size) +{ + return malloc(size); +} + +void osal_free(void *ptr) +{ + free(ptr); +} + +int osal_thread_create(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + if(ret < 0) + { + return 0; + } + return 1; +} + +int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param) +{ + int ret; + pthread_attr_t attr; + struct sched_param schparam; + pthread_t *threadp; + + threadp = thandle; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(threadp, &attr, func, param); + pthread_attr_destroy(&attr); + if(ret < 0) + { + return 0; + } + memset(&schparam, 0, sizeof(schparam)); + schparam.sched_priority = 40; + ret = pthread_setschedparam(*threadp, SCHED_FIFO, &schparam); + if(ret < 0) + { + return 0; + } + + return 1; +} diff --git a/osal/rtems/osal_defs.h b/osal/rtems/osal_defs.h new file mode 100644 index 0000000..7140754 --- /dev/null +++ b/osal/rtems/osal_defs.h @@ -0,0 +1,29 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#ifndef _osal_defs_ +#define _osal_defs_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef PACKED +#define PACKED_BEGIN +#define PACKED __attribute__((__packed__)) +#define PACKED_END +#endif + +#include +#define OSAL_THREAD_HANDLE pthread_t * +#define OSAL_THREAD_FUNC void +#define OSAL_THREAD_FUNC_RT void + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/oshw/linux/nicdrv.c b/oshw/linux/nicdrv.c index 45cae60..8138485 100644 --- a/oshw/linux/nicdrv.c +++ b/oshw/linux/nicdrv.c @@ -158,7 +158,7 @@ int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) r = ioctl(*psock, SIOCGIFFLAGS, &ifr); /* set flags of NIC interface, here promiscuous and broadcast */ ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; - r = ioctl(*psock, SIOCGIFFLAGS, &ifr); + r = ioctl(*psock, SIOCSIFFLAGS, &ifr); /* bind socket to protocol, in this case RAW EtherCAT */ sll.sll_family = AF_PACKET; sll.sll_ifindex = ifindex; diff --git a/oshw/rtems/nicdrv.c b/oshw/rtems/nicdrv.c new file mode 100644 index 0000000..810b847 --- /dev/null +++ b/oshw/rtems/nicdrv.c @@ -0,0 +1,687 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +/** \file + * \brief + * EtherCAT RAW socket driver. + * + * Low level interface functions to send and receive EtherCAT packets. + * EtherCAT has the property that packets are only send by the master, + * and the send packets always return in the receive buffer. + * There can be multiple packets "on the wire" before they return. + * To combine the received packets with the original send packets a buffer + * system is installed. The identifier is put in the index item of the + * EtherCAT header. The index is stored and compared when a frame is received. + * If there is a match the packet can be combined with the transmit packet + * and returned to the higher level function. + * + * The socket layer can exhibit a reversal in the packet order (rare). + * If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer + * will reorder the packets automatically. + * + * The "redundant" option will configure two sockets and two NIC interfaces. + * Slaves are connected to both interfaces, one on the IN port and one on the + * OUT port. Packets are send via both interfaces. Any one of the connections + * (also an interconnect) can be removed and the slaves are still serviced with + * packets. The software layer will detect the possible failure modes and + * compensate. If needed the packets from interface A are resent through interface B. + * This layer if fully transparent for the higher layers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "oshw.h" +#include "osal.h" + +/** Redundancy modes */ +enum +{ + /** No redundancy, single NIC mode */ + ECT_RED_NONE, + /** Double redundant NIC connecetion */ + ECT_RED_DOUBLE +}; + + +/** Primary source MAC address used for EtherCAT. + * This address is not the MAC address used from the NIC. + * EtherCAT does not care about MAC addressing, but it is used here to + * differentiate the route the packet traverses through the EtherCAT + * segment. This is needed to find out the packet flow in redundant + * configurations. */ +const uint16 priMAC[3] = { 0x0201, 0x0101, 0x0101 }; +/** Secondary source MAC address used for EtherCAT. */ +const uint16 secMAC[3] = { 0x0604, 0x0404, 0x0404 }; + +/** second MAC word is used for identification */ +#define RX_PRIM priMAC[1] +/** second MAC word is used for identification */ +#define RX_SEC secMAC[1] + +static void ecx_clear_rxbufstat(int *rxbufstat) +{ + int i; + for(i = 0; i < EC_MAXBUF; i++) + { + rxbufstat[i] = EC_BUF_EMPTY; + } +} + +/** Basic setup to connect NIC to socket. + * @param[in] port = port context struct + * @param[in] ifname = Name of NIC device, f.e. "eth0" + * @param[in] secondary = if >0 then use secondary stack instead of primary + * @return >0 if succeeded + */ +int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary) +{ + int i; + int r, rval; + struct timeval timeout; + struct ifreq ifr; + int *bpf; + // const uint8_t bpffnamelen = 12; + char fname[13] = {0}; + const int maxbpffile = 1000; + uint true_val = 1; + + rval = 0; + if (secondary) + { + /* secondary port struct available? */ + if (port->redport) + { + /* when using secondary socket it is automatically a redundant setup */ + bpf = &(port->redport->sockhandle); + *bpf = -1; + port->redstate = ECT_RED_DOUBLE; + port->redport->stack.sock = &(port->redport->sockhandle); + port->redport->stack.txbuf = &(port->txbuf); + port->redport->stack.txbuflength = &(port->txbuflength); + port->redport->stack.tempbuf = &(port->redport->tempinbuf); + port->redport->stack.rxbuf = &(port->redport->rxbuf); + port->redport->stack.rxbufstat = &(port->redport->rxbufstat); + port->redport->stack.rxsa = &(port->redport->rxsa); + ecx_clear_rxbufstat(&(port->redport->rxbufstat[0])); + } + else + { + /* fail */ + return 0; + } + } + else + { + pthread_mutex_init(&(port->getindex_mutex), NULL); + pthread_mutex_init(&(port->tx_mutex) , NULL); + pthread_mutex_init(&(port->rx_mutex) , NULL); + port->sockhandle = -1; + port->lastidx = 0; + port->redstate = ECT_RED_NONE; + port->stack.sock = &(port->sockhandle); + port->stack.txbuf = &(port->txbuf); + port->stack.txbuflength = &(port->txbuflength); + port->stack.tempbuf = &(port->tempinbuf); + port->stack.rxbuf = &(port->rxbuf); + port->stack.rxbufstat = &(port->rxbufstat); + port->stack.rxsa = &(port->rxsa); + ecx_clear_rxbufstat(&(port->rxbufstat[0])); + bpf = &(port->sockhandle); + } + + /* Open a bpf file descriptor */ + *bpf = -1; + for(i = 0; *bpf == -1 && i < maxbpffile; ++i) + { + sprintf(fname, "/dev/bpf%i", i); + *bpf = open(fname, O_RDWR); + } + + if(*bpf == -1) + { + /* Failed to open bpf */ + return 0; + } + + /* Need to hand the same buffer size as bpf expects, + force bpf to use our buffer size! */ + uint buffer_len = sizeof(ec_bufT); + if (ioctl(*bpf, BIOCSBLEN, &buffer_len) == -1) { + perror("BIOCGBLEN"); + } + assert(buffer_len >= 1518); + + /* connect bpf to NIC by name */ + strcpy(ifr.ifr_name, ifname); + assert(ioctl(*bpf, BIOCSETIF, &ifr) == 0); + + /* Set required bpf options */ + + /* Activate immediate mode */ + if (ioctl(*bpf, BIOCIMMEDIATE, &true_val) == -1) { + perror("BIOCIMMEDIATE"); + } + + /* Set interface in promiscuous mode */ + if (ioctl(*bpf, BIOCPROMISC, &true_val) == -1) { + perror("BIOCPROMISC"); + } + + /* Allow to have custom source address */ + if (ioctl(*bpf, BIOCSHDRCMPLT, &true_val) == -1) { + perror("BIOCSHDRCMPLT"); + } + + /* Listen only to incomming messages */ + uint direction = BPF_D_IN; + if (ioctl(*bpf, BIOCSDIRECTION, &direction) == -1) { + perror("BIOCSDIRECTION"); + } + + /* Set read timeout */ + timeout.tv_sec = 0; + timeout.tv_usec = 1; + if (ioctl(*bpf, BIOCSRTIMEOUT, &timeout) == -1) { + perror("BIOCSRTIMEOUT"); + } + + /* setup ethernet headers in tx buffers so we don't have to repeat it */ + for (i = 0; i < EC_MAXBUF; i++) + { + ec_setupheader(&(port->txbuf[i])); + port->rxbufstat[i] = EC_BUF_EMPTY; + } + ec_setupheader(&(port->txbuf2)); + if (r == 0) rval = 1; + + return rval; +} + +/** Close sockets used + * @param[in] port = port context struct + * @return 0 + */ +int ecx_closenic(ecx_portt *port) +{ + if (port->sockhandle >= 0) + close(port->sockhandle); + if ((port->redport) && (port->redport->sockhandle >= 0)) + close(port->redport->sockhandle); + + return 0; +} + +/** Fill buffer with ethernet header structure. + * Destination MAC is always broadcast. + * Ethertype is always ETH_P_ECAT. + * @param[out] p = buffer + */ +void ec_setupheader(void *p) +{ + ec_etherheadert *bp; + bp = p; + bp->da0 = htons(0xffff); + bp->da1 = htons(0xffff); + bp->da2 = htons(0xffff); + bp->sa0 = htons(priMAC[0]); + bp->sa1 = htons(priMAC[1]); + bp->sa2 = htons(priMAC[2]); + bp->etype = htons(ETH_P_ECAT); +} + +/** Get new frame identifier index and allocate corresponding rx buffer. + * @param[in] port = port context struct + * @return new index. + */ +int ecx_getindex(ecx_portt *port) +{ + int idx; + int cnt; + + pthread_mutex_lock( &(port->getindex_mutex) ); + + idx = port->lastidx + 1; + /* index can't be larger than buffer array */ + if (idx >= EC_MAXBUF) + { + idx = 0; + } + cnt = 0; + /* try to find unused index */ + while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF)) + { + idx++; + cnt++; + if (idx >= EC_MAXBUF) + { + idx = 0; + } + } + port->rxbufstat[idx] = EC_BUF_ALLOC; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = EC_BUF_ALLOC; + port->lastidx = idx; + + pthread_mutex_unlock( &(port->getindex_mutex) ); + + return idx; +} + +/** Set rx buffer status. + * @param[in] port = port context struct + * @param[in] idx = index in buffer array + * @param[in] bufstat = status to set + */ +void ecx_setbufstat(ecx_portt *port, int idx, int bufstat) +{ + port->rxbufstat[idx] = bufstat; + if (port->redstate != ECT_RED_NONE) + port->redport->rxbufstat[idx] = bufstat; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @param[in] stacknumber = 0=Primary 1=Secondary stack + * @return socket send result + */ +int ecx_outframe(ecx_portt *port, int idx, int stacknumber) +{ + int lp, rval; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = (*stack->txbuflength)[idx]; + //rval = send(*stack->sock, (*stack->txbuf)[idx], lp, 0); + rval = write (*stack->sock,(*stack->txbuf)[idx], lp); + (*stack->rxbufstat)[idx] = EC_BUF_TX; + + return rval; +} + +/** Transmit buffer over socket (non blocking). + * @param[in] port = port context struct + * @param[in] idx = index in tx buffer array + * @return socket send result + */ +int ecx_outframe_red(ecx_portt *port, int idx) +{ + ec_comt *datagramP; + ec_etherheadert *ehp; + int rval; + + ehp = (ec_etherheadert *)&(port->txbuf[idx]); + /* rewrite MAC source address 1 to primary */ + ehp->sa1 = htons(priMAC[1]); + /* transmit over primary socket*/ + rval = ecx_outframe(port, idx, 0); + if (port->redstate != ECT_RED_NONE) + { + pthread_mutex_lock( &(port->tx_mutex) ); + ehp = (ec_etherheadert *)&(port->txbuf2); + /* use dummy frame for secondary socket transmit (BRD) */ + datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]); + /* write index to frame */ + datagramP->index = idx; + /* rewrite MAC source address 1 to secondary */ + ehp->sa1 = htons(secMAC[1]); + /* transmit over secondary socket */ + //send(port->redport->sockhandle, &(port->txbuf2), port->txbuflength2 , 0); + write(port->redport->sockhandle, &(port->txbuf2), port->txbuflength2); + pthread_mutex_unlock( &(port->tx_mutex) ); + port->redport->rxbufstat[idx] = EC_BUF_TX; + } + + return rval; +} + +/** Non blocking read of socket. Put frame in temporary buffer. + * @param[in] port = port context struct + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return >0 if frame is available and read + */ +static int ecx_recvpkt(ecx_portt *port, int stacknumber) +{ + int lp, bytesrx; + ec_stackT *stack; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + lp = sizeof(port->tempinbuf); + //bytesrx = recv(*stack->sock, (*stack->tempbuf), lp, 0); + bytesrx = read(*stack->sock, (*stack->tempbuf), lp); + port->tempinbufs = bytesrx; + + return (bytesrx > 0); +} + +/** Non blocking receive frame function. Uses RX buffer and index to combine + * read frame with transmitted frame. To compensate for received frames that + * are out-of-order all frames are stored in their respective indexed buffer. + * If a frame was placed in the buffer previously, the function retreives it + * from that buffer index without calling ec_recvpkt. If the requested index + * is not already in the buffer it calls ec_recvpkt to fetch it. There are + * three options now, 1 no frame read, so exit. 2 frame read but other + * than requested index, store in buffer and exit. 3 frame read with matching + * index, store in buffer, set completed flag in buffer status and exit. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] stacknumber = 0=primary 1=secondary stack + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME or EC_OTHERFRAME. + */ +int ecx_inframe(ecx_portt *port, int idx, int stacknumber) +{ + uint16 l; + int rval; + int idxf; + ec_etherheadert *ehp; + ec_comt *ecp; + ec_stackT *stack; + ec_bufT *rxbuf; + + if (!stacknumber) + { + stack = &(port->stack); + } + else + { + stack = &(port->redport->stack); + } + rval = EC_NOFRAME; + rxbuf = &(*stack->rxbuf)[idx]; + /* check if requested index is already in buffer ? */ + 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; + } + else + { + pthread_mutex_lock(&(port->rx_mutex)); + /* non blocking call to retrieve frame from socket */ + if (ecx_recvpkt(port, stacknumber)) + { + /* The data read from /dev/bpf includes an extra header, skip it. */ + struct bpf_hdr *bpfh = (struct bpf_hdr *)(stack->tempbuf); + rval = EC_OTHERFRAME; + ehp =(ec_etherheadert*)(*stack->tempbuf + bpfh->bh_hdrlen); + /* check if it is an EtherCAT frame */ + if (ehp->etype == htons(ETH_P_ECAT)) + { + /* The EtherCAT header follows the ethernet frame header. */ + ecp =(ec_comt*)(&ehp[1]); + l = etohs(ecp->elength) & 0x0fff; + idxf = ecp->index; + /* found index equals reqested index ? */ + if (idxf == idx) + { + /* yes, put it in the buffer array (strip headers) */ + memcpy(rxbuf, &(ehp[1]), port->tempinbufs - ((uint32_t)ecp - (uint32_t)*stack->tempbuf)); + /* return WKC */ + rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8)); + /* mark as completed */ + (*stack->rxbufstat)[idx] = EC_BUF_COMPLETE; + /* store MAC source word 1 for redundant routing info */ + (*stack->rxsa)[idx] = ntohs(ehp->sa1); + } + else + { + /* check if index exist and someone is waiting for it */ + if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX) + { + rxbuf = &(*stack->rxbuf)[idxf]; + /* put it in the buffer array (strip ethernet header) */ + memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE); + /* mark as received */ + (*stack->rxbufstat)[idxf] = EC_BUF_RCVD; + (*stack->rxsa)[idxf] = ntohs(ehp->sa1); + } + else + { + /* strange things happend */ + } + } + } + } + pthread_mutex_unlock( &(port->rx_mutex) ); + + } + + /* WKC if mathing frame found */ + return rval; +} + +/** Blocking redundant receive frame function. If redundant mode is not active then + * it skips the secondary stack and redundancy functions. In redundant mode it waits + * for both (primary and secondary) frames to come in. The result goes in an decision + * tree that decides, depending on the route of the packet and its possible missing arrival, + * how to reroute the original packet to get the data in an other try. + * + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timer = absolute timeout time + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +static int ecx_waitinframe_red(ecx_portt *port, int idx, osal_timert *timer) +{ + osal_timert timer2; + int wkc = EC_NOFRAME; + int wkc2 = EC_NOFRAME; + int primrx, secrx; + + /* if not in redundant mode then always assume secondary is OK */ + if (port->redstate == ECT_RED_NONE) + wkc2 = 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) + { + /* 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)); + /* only do redundant functions when in redundant mode */ + if (port->redstate != ECT_RED_NONE) + { + /* primrx if the reveived MAC source on primary socket */ + primrx = 0; + if (wkc > EC_NOFRAME) primrx = port->rxsa[idx]; + /* secrx if the reveived MAC source on psecondary socket */ + secrx = 0; + if (wkc2 > EC_NOFRAME) secrx = port->redport->rxsa[idx]; + + /* primary socket got secondary frame and secondary socket got primary frame */ + /* normal situation in redundant mode */ + if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) ) + { + /* copy secondary buffer to primary */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + /* primary socket got nothing or primary frame, and secondary socket got secondary frame */ + /* we need to resend TX packet */ + if ( ((primrx == 0) && (secrx == RX_SEC)) || + ((primrx == RX_PRIM) && (secrx == RX_SEC)) ) + { + /* If both primary and secondary have partial connection retransmit the primary received + * frame over the secondary socket. The result from the secondary received frame is a combined + * frame that traversed all slaves in standard order. */ + if ( (primrx == RX_PRIM) && (secrx == RX_SEC) ) + { + /* copy primary rx to tx buffer */ + memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + } + osal_timer_start (&timer2, EC_TIMEOUTRET); + /* resend secondary tx */ + ecx_outframe(port, idx, 1); + do + { + /* retrieve frame */ + wkc2 = ecx_inframe(port, idx, 1); + } while ((wkc2 <= EC_NOFRAME) && !osal_timer_is_expired(&timer2)); + if (wkc2 > EC_NOFRAME) + { + /* copy secondary result to primary rx buffer */ + memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE); + wkc = wkc2; + } + } + } + + /* return WKC or EC_NOFRAME */ + return wkc; +} + +/** Blocking receive frame function. Calls ec_waitinframe_red(). + * @param[in] port = port context struct + * @param[in] idx = requested index of frame + * @param[in] timeout = timeout in us + * @return Workcounter if a frame is found with corresponding index, otherwise + * EC_NOFRAME. + */ +int ecx_waitinframe(ecx_portt *port, int idx, int timeout) +{ + int wkc; + osal_timert timer; + + osal_timer_start (&timer, timeout); + wkc = ecx_waitinframe_red(port, idx, &timer); + /* if nothing received, clear buffer index status so it can be used again */ + if (wkc <= EC_NOFRAME) + { + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + } + + return wkc; +} + +/** Blocking send and recieve frame function. Used for non processdata frames. + * A datagram is build into a frame and transmitted via this function. It waits + * for an answer and returns the workcounter. The function retries if time is + * left and the result is WKC=0 or no frame received. + * + * The function calls ec_outframe_red() and ec_waitinframe_red(). + * + * @param[in] port = port context struct + * @param[in] idx = index of frame + * @param[in] timeout = timeout in us + * @return Workcounter or EC_NOFRAME + */ +int ecx_srconfirm(ecx_portt *port, int idx, int timeout) +{ + int wkc = EC_NOFRAME; + osal_timert timer1, timer2; + + osal_timer_start (&timer1, timeout); + do + { + /* tx frame on primary and if in redundant mode a dummy on secondary */ + ecx_outframe_red(port, idx); + if (timeout < EC_TIMEOUTRET) + { + osal_timer_start (&timer2, timeout); + } + else + { + /* normally use partial timout for rx */ + osal_timer_start (&timer2, EC_TIMEOUTRET); + } + /* get frame from primary or if in redundant mode possibly from secondary */ + wkc = ecx_waitinframe_red(port, idx, &timer2); + /* wait for answer with WKC>=0 or otherwise retry until timeout */ + } while ((wkc <= EC_NOFRAME) && !osal_timer_is_expired (&timer1)); + /* if nothing received, clear buffer index status so it can be used again */ + if (wkc <= EC_NOFRAME) + { + ecx_setbufstat(port, idx, EC_BUF_EMPTY); + } + + return wkc; +} + +#ifdef EC_VER1 +int ec_setupnic(const char *ifname, int secondary) +{ + return ecx_setupnic(&ecx_port, ifname, secondary); +} + +int ec_closenic(void) +{ + return ecx_closenic(&ecx_port); +} + +int ec_getindex(void) +{ + return ecx_getindex(&ecx_port); +} + +void ec_setbufstat(int idx, int bufstat) +{ + ecx_setbufstat(&ecx_port, idx, bufstat); +} + +int ec_outframe(int idx, int stacknumber) +{ + return ecx_outframe(&ecx_port, idx, stacknumber); +} + +int ec_outframe_red(int idx) +{ + return ecx_outframe_red(&ecx_port, idx); +} + +int ec_inframe(int idx, int stacknumber) +{ + return ecx_inframe(&ecx_port, idx, stacknumber); +} + +int ec_waitinframe(int idx, int timeout) +{ + return ecx_waitinframe(&ecx_port, idx, timeout); +} + +int ec_srconfirm(int idx, int timeout) +{ + return ecx_srconfirm(&ecx_port, idx, timeout); +} +#endif diff --git a/oshw/rtems/nicdrv.h b/oshw/rtems/nicdrv.h new file mode 100644 index 0000000..4dcfcfd --- /dev/null +++ b/oshw/rtems/nicdrv.h @@ -0,0 +1,120 @@ +/* + * 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 nicdrv.c + */ + +#ifndef _nicdrvh_ +#define _nicdrvh_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/** pointer structure to Tx and Rx stacks */ +typedef struct +{ + /** socket connection used */ + int *sock; + /** tx buffer */ + ec_bufT (*txbuf)[EC_MAXBUF]; + /** tx buffer lengths */ + int (*txbuflength)[EC_MAXBUF]; + /** temporary receive buffer */ + ec_bufT *tempbuf; + /** rx buffers */ + ec_bufT (*rxbuf)[EC_MAXBUF]; + /** rx buffer status fields */ + int (*rxbufstat)[EC_MAXBUF]; + /** received MAC source address (middle word) */ + int (*rxsa)[EC_MAXBUF]; +} ec_stackT; + +/** pointer structure to buffers for redundant port */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; +} ecx_redportt; + +/** pointer structure to buffers, vars and mutexes for port instantiation */ +typedef struct +{ + ec_stackT stack; + int sockhandle; + /** rx buffers */ + ec_bufT rxbuf[EC_MAXBUF]; + /** rx buffer status */ + int rxbufstat[EC_MAXBUF]; + /** rx MAC source address */ + int rxsa[EC_MAXBUF]; + /** temporary rx buffer */ + ec_bufT tempinbuf; + /** temporary rx buffer status */ + int tempinbufs; + /** transmit buffers */ + ec_bufT txbuf[EC_MAXBUF]; + /** transmit buffer lenghts */ + int txbuflength[EC_MAXBUF]; + /** temporary tx buffer */ + ec_bufT txbuf2; + /** temporary tx buffer length */ + int txbuflength2; + /** last used frame index */ + int lastidx; + /** current redundancy state */ + int redstate; + /** pointer to redundancy port and buffers */ + ecx_redportt *redport; + pthread_mutex_t getindex_mutex; + pthread_mutex_t tx_mutex; + pthread_mutex_t rx_mutex; +} ecx_portt; + +extern const uint16 priMAC[3]; +extern const uint16 secMAC[3]; + +#ifdef EC_VER1 +extern ecx_portt ecx_port; +extern ecx_redportt ecx_redport; + +int ec_setupnic(const char * ifname, int secondary); +int ec_closenic(void); +void ec_setbufstat(int idx, int bufstat); +int ec_getindex(void); +int ec_outframe(int idx, int sock); +int ec_outframe_red(int idx); +int ec_waitinframe(int idx, int timeout); +int ec_srconfirm(int idx,int timeout); +#endif + +void ec_setupheader(void *p); +int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); +int ecx_closenic(ecx_portt *port); +void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); +int ecx_getindex(ecx_portt *port); +int ecx_outframe(ecx_portt *port, int idx, int sock); +int ecx_outframe_red(ecx_portt *port, int idx); +int ecx_waitinframe(ecx_portt *port, int idx, int timeout); +int ecx_srconfirm(ecx_portt *port, int idx,int timeout); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/oshw/rtems/oshw.c b/oshw/rtems/oshw.c new file mode 100644 index 0000000..4003490 --- /dev/null +++ b/oshw/rtems/oshw.c @@ -0,0 +1,123 @@ +/* + * Licensed under the GNU General Public License version 2 with exceptions. See + * LICENSE file in the project root for full license information + */ + +#include +#include +#include +#include +#include +#include +#include +#include "oshw.h" + +/** + * Host to Network byte order (i.e. to big endian). + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_htons(uint16 host) +{ + uint16 network = htons (host); + return network; +} + +/** + * Network (i.e. big endian) to Host byte order. + * + * Note that Ethercat uses little endian byte order, except for the Ethernet + * header which is big endian as usual. + */ +uint16 oshw_ntohs(uint16 network) +{ + uint16 host = ntohs (network); + return host; +} + +/** Create list over available network adapters. + * @return First element in linked list of adapters + */ +ec_adaptert * oshw_find_adapters(void) +{ + int i; + int string_len; + struct if_nameindex *ids; + ec_adaptert * adapter; + ec_adaptert * prev_adapter; + ec_adaptert * ret_adapter = NULL; + + + /* Iterate all devices and create a local copy holding the name and + * decsription. + */ + + ids = if_nameindex (); + for(i = 0; ids[i].if_index != 0; i++) + { + adapter = (ec_adaptert *)malloc(sizeof(ec_adaptert)); + /* If we got more than one adapter save link list pointer to previous + * adapter. + * Else save as pointer to return. + */ + if (i) + { + prev_adapter->next = adapter; + } + else + { + ret_adapter = adapter; + } + + /* fetch description and name, in linux we use the same on both */ + adapter->next = NULL; + + if (ids[i].if_name) + { + string_len = strlen(ids[i].if_name); + if (string_len > (EC_MAXLEN_ADAPTERNAME - 1)) + { + string_len = EC_MAXLEN_ADAPTERNAME - 1; + } + strncpy(adapter->name, ids[i].if_name,string_len); + adapter->name[string_len] = '\0'; + strncpy(adapter->desc, ids[i].if_name,string_len); + adapter->desc[string_len] = '\0'; + } + else + { + adapter->name[0] = '\0'; + adapter->desc[0] = '\0'; + } + + prev_adapter = adapter; + } + + if_freenameindex (ids); + + return ret_adapter; +} + +/** Free memory allocated memory used by adapter collection. + * @param[in] adapter = First element in linked list of adapters + * EC_NOFRAME. + */ +void oshw_free_adapters(ec_adaptert * adapter) +{ + ec_adaptert * next_adapter; + /* Iterate the linked list and free all elemnts holding + * adapter information + */ + if(adapter) + { + next_adapter = adapter->next; + free (adapter); + while (next_adapter) + { + adapter = next_adapter; + next_adapter = adapter->next; + free (adapter); + } + } +} diff --git a/oshw/rtems/oshw.h b/oshw/rtems/oshw.h new file mode 100644 index 0000000..78d131b --- /dev/null +++ b/oshw/rtems/oshw.h @@ -0,0 +1,31 @@ +/* + * 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 ethercatbase.c + */ + +#ifndef _oshw_ +#define _oshw_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatmain.h" + +uint16 oshw_htons(uint16 hostshort); +uint16 oshw_ntohs(uint16 networkshort); +ec_adaptert * oshw_find_adapters(void); +void oshw_free_adapters(ec_adaptert * adapter); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/soem/ethercatbase.c b/soem/ethercatbase.c index 1178a92..2621b34 100644 --- a/soem/ethercatbase.c +++ b/soem/ethercatbase.c @@ -634,4 +634,4 @@ int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtim { return ecx_LRWDC(&ecx_port, LogAdr, length, data, DCrs, DCtime, timeout); } -#endif \ No newline at end of file +#endif