Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23840] pkg/trunk/stacks/pr2
|
From: <wg...@us...> - 2009-09-04 18:36:05
|
Revision: 23840
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23840&view=rev
Author: wgdking
Date: 2009-09-04 18:35:50 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
EML posix driver now supports split txandrx calls, and uses seperate recv thread.
pr2_etherCAT now runs non-realtime thread to collect out-of-band diagnositcs data from slave devices.
pr2_etherCAT pubishes more diagnostics data for master and slave devices.
Modified Paths:
--------------
pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/CMakeLists.txt
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/ethercat_device.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/wg014.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/wg0x.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ek1122.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg014.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg0x.cpp
Added Paths:
-----------
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/ethercat_com.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_com.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_device.cpp
Modified: pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp 2009-09-04 18:27:37 UTC (rev 23839)
+++ pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp 2009-09-04 18:35:50 UTC (rev 23840)
@@ -41,6 +41,7 @@
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
+#include <pthread.h>
#include <diagnostic_updater/DiagnosticStatusWrapper.h>
#include <pr2_mechanism_control/mechanism_control.h>
@@ -144,6 +145,20 @@
return double(n.tv_nsec) / NSEC_PER_SEC + n.tv_sec;
}
+
+void *diagnosticLoop(void *args)
+{
+ EthercatHardware *ec((EthercatHardware *) args);
+ struct timespec tick;
+ clock_gettime(CLOCK_MONOTONIC, &tick);
+ while (!g_quit) {
+ ec->collectDiagnostics();
+ tick.tv_sec += 1;
+ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &tick, NULL);
+ }
+ return NULL;
+}
+
void *controlLoop(void *)
{
ros::NodeHandle node;
@@ -188,6 +203,15 @@
// Publish one-time before entering real-time to pre-allocate message vectors
publishDiagnostics(publisher);
+ //Start Non-realtime diagonostic thread
+ int rv;
+ static pthread_t diagnosticThread;
+ if ((rv = pthread_create(&diagnosticThread, NULL, diagnosticLoop, &ec)) != 0)
+ {
+ ROS_FATAL("Unable to create control thread: rv = %d\n", rv);
+ ROS_BREAK();
+ }
+
// Set to realtime scheduler for this thread
struct sched_param thread_param;
int policy = SCHED_FIFO;
@@ -242,6 +266,8 @@
}
ec.update(false, true);
+ //pthread_join(diagnosticThread, 0);
+
publisher.stop();
ros::shutdown();
Modified: pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch
===================================================================
--- pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch 2009-09-04 18:27:37 UTC (rev 23839)
+++ pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch 2009-09-04 18:35:50 UTC (rev 23840)
@@ -52,15 +52,306 @@
===================================================================
--- include/ethercat/arch-RTnet/ethercat/ethercat_log.h (revision 36)
+++ include/ethercat/arch-RTnet/ethercat/ethercat_log.h (working copy)
-@@ -42,7 +42,6 @@
+@@ -42,7 +42,7 @@
#define EC_LOG_FATAL 0
// Redefine this function if not using eCos...
-#define log(level, args...) \
-printf(args)
-+#define ec_log(level, args...)
++#define ec_log(level, args...) \
++ if (level <= EC_LOG_FATAL) fprintf(stderr, args)
#endif // __ethercat_log_h__
+Index: include/ethercat/arch-RTnet/ethercat/netif.h
+===================================================================
+--- include/ethercat/arch-RTnet/ethercat/netif.h (revision 36)
++++ include/ethercat/arch-RTnet/ethercat/netif.h (working copy)
+@@ -39,6 +39,7 @@
+ struct EtherCAT_Frame;
+ struct netif;
+
++#include <stdint.h>
+ #include <stdlib.h>
+
+ #define ETHERCAT_DEVICE_NAME_MAX_LENGTH 256
+@@ -50,20 +51,90 @@
+ #define externC extern "C"
+ #else
+
+-typedef int bool;
+-#define false 0
+-#define true 1
+-
+ #define externC
+ #endif
+
++
++typedef char BOOL;
++#define FALSE 0
++#define TRUE 1
++
++
+ externC void if_attach(struct netif * netif);
+ externC int framedump(const struct EtherCAT_Frame * frame, unsigned char * buffer, size_t bufferlength);
+ externC int framebuild(struct EtherCAT_Frame * frame, const unsigned char * buffer);
+
+
++// Number of outstanding packets to keep track of
++#define PKT_LIST_SIZE 128
+
++// Number of buffers to hold recieved packets
++#define BUF_LIST_SIZE 16
+
++// Should be < number of RX buffers
++#define MAX_UNCLAIMED_PACKETS (BUF_LIST_SIZE-1)
++
++
++// buffer to hold recieved packets
++struct pkt_buf {
++ // True if the buffer is being used
++ BOOL is_free;
++ // buffer for to store ethernet message
++ // more than enough to hold any type of message
++ unsigned char data[2000];
++};
++
++// record of packet that has been sent but.
++// 1. has not received reply
++// .... OR ....
++// 2. has not been claimed by higher level software
++struct outstanding_pkt {
++ // whether this record is holding an outstanding packet or not
++ BOOL is_free;
++
++ // pointer to received packet buffer
++ // If this is NULL if the packet has not been received
++ struct pkt_buf *buf;
++
++ // The source MAC address is used to keep track of
++ // which packet belongs where
++ u_int8_t ether_shost[MAC_ADDRESS_SIZE];
++
++ // EtherCAT_Frame that was use to generate the sent packet...
++ // The exact same frame must be used to pickup the packet
++ struct EtherCAT_Frame * frame;
++
++ // To allow synchronization between input thread and rx function
++ //pthread_mutex_t rx_mut;
++ //pthread_mutexattr_t rx_attr;
++ pthread_cond_t rx_cond;
++
++ // Temember when packet was sent
++ struct timespec tx_time;
++};
++
++struct netif_counters
++{
++ uint64_t sent;
++ uint64_t received;
++ uint64_t collected;
++ uint64_t dropped;
++ uint64_t tx_error;
++ uint64_t tx_net_down;
++ uint64_t tx_full;
++ uint64_t rx_runt_pkt;
++ uint64_t rx_not_ecat;
++ uint64_t rx_other_eml;
++ uint64_t rx_bad_index;
++ uint64_t rx_bad_seqnum;
++ uint64_t rx_dup_seqnum;
++ uint64_t rx_dup_pkt;
++ uint64_t rx_bad_order;
++ uint64_t rx_late_pkt; // Count of all late packets
++ uint64_t rx_late_pkt_rtt_us; // Round trip time (in microseconds) of last late packet arrival
++ uint64_t rx_late_pkt_rtt_us_sum; // Sum of rtt's of all late packets (for calculating average)
++};
++
+ /// Generic ethercat interface towards lower level drivers.
+ /** It should be readily re-implemented for different OSes such as
+ RTAI, linux, ... etc. (For the ease of porting the interface
+@@ -75,12 +146,68 @@
+ packages/io/eth/current/src/ethercatmaster/eth_drv.c
+ and mapped in eth_drv_init()
+ */
+- bool (* txandrx)(struct EtherCAT_Frame * frame, struct netif * netif);
++ BOOL (* txandrx)(struct EtherCAT_Frame * frame, struct netif * netif);
+
++ /// Transmit and receive an EtherCAT frame - only attempt to send
++ /// frame once
++ BOOL (* txandrx_once)(struct EtherCAT_Frame * frame, struct netif * netif);
++
++
++ /// Transmit frame
++ /** Negative return value is an error code, positive and zero is a
++ handle
++ */
++ int (* tx)(struct EtherCAT_Frame * frame, struct netif * netif);
++
++
++ /// Recieve frame
++ /** May block, returns true if correct frame is recieved
++ */
++ BOOL (* rx)(struct EtherCAT_Frame * frame, struct netif * netif, int handle);
++
++ /// Recieve frame
++ /** Will not block, returns true if correct frame is recieved
++ */
++ BOOL (* rx_nowait)(struct EtherCAT_Frame * frame, struct netif * netif, int handle);
++
+ /// The MAC address
+ unsigned char hwaddr[MAC_ADDRESS_SIZE];
+ /// Filedescriptor of the socket
+ int socket_private;
++
++ /// Counters for certain types of events (packet drops, invalid packets, etc)
++ struct netif_counters counters;
++
++ /// Sequence number to put on next sent packet
++ unsigned tx_seqnum;
++
++ /// Secuence number of more recently recieved packet
++ unsigned rx_seqnum;
++
++ /// Outstanding pkt slot to use from next tx
++ unsigned next_pkt_index;
++
++ /// List of outstanding packets
++ /// (packets that have been tx'ed but not rx'ed)
++ struct outstanding_pkt pkt_list[PKT_LIST_SIZE];
++
++ // Number of tx'd packets that have not been rx'd yet
++ unsigned unclaimed_packets;
++
++ /// List of buffers used for packet reception
++ struct pkt_buf buf_list[BUF_LIST_SIZE];
++
++ // For thread safety: txandrx() can be called from multiple threads...
++ pthread_mutex_t txandrx_mut;
++ pthread_mutexattr_t txandrx_attr;
++
++ // For input thread -- if it is used
++ pthread_t input_thread;
++ volatile BOOL stop;
++ volatile BOOL is_stopped;
++
++ // Timeout for recieve in microseconds.
++ int timeout_us;
+ };
+
+ #endif // __netif_h__
+Index: include/dll/ethercat_dll.h
+===================================================================
+--- include/dll/ethercat_dll.h (revision 36)
++++ include/dll/ethercat_dll.h (working copy)
+@@ -53,11 +53,26 @@
+ */
+ void attach(struct netif * netif);
+
+- /// transmit and receive EtherCAT frame (blocking call!)
++ /// transmit and receive EtherCAT frame (blocking call!)
+ /** @param a_frame ethercat frame to be sent
++ * NOTE that txandrx will retry sending lost frames.
+ */
+ bool txandrx(EtherCAT_Frame * a_frame);
+
++ /// transmit an EtherCAT frame (non-blocking call)
++ /** @param a_frame ethercat frame to be sent
++ * @return positive or zero handle on success, negative value for error
++ * Successfull tx MUST be followed by rx call.
++ */
++ int tx(EtherCAT_Frame * a_frame);
++
++ /// receive an EtherCAT frame previously sent by tx() (blocking call!)
++ /** @param a_frame ethercat frame to be sent
++ * @param a_handle handle previously returned by tx()
++ * @return false for error/dropped packet
++ */
++ bool rx(EtherCAT_Frame * a_frame, int a_handle);
++
+ /// Destructor
+ /** @todo some kind of smart pointer concept necessary for all these
+ singletons. For EtherCAT master on ecos not really an issue...
+Index: include/dll/ethercat_device_addressed_telegram.h
+===================================================================
+--- include/dll/ethercat_device_addressed_telegram.h (revision 36)
++++ include/dll/ethercat_device_addressed_telegram.h (working copy)
+@@ -134,6 +134,29 @@
+ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
+ };
+
++/// Auto Increment Physical Read and Write Telegram (APRW)
++class APRW_Telegram : public Device_Addressing_Telegram {
++ public:
++ /// Constructor
++ /** @param a_idx index
++ @param a_adp auto increment address
++ @param a_ado physical memory address
++ @param a_wkc working counter
++ @param a_datalen data_length
++ @param a_data data to be read and written
++ */
++ APRW_Telegram(EC_USINT a_idx, EC_UINT a_adp,
++ EC_UINT a_ado,EC_UINT a_wkc,
++ EC_UINT a_datalen,
++ const unsigned char * a_data);
++
++ virtual ~APRW_Telegram();
++
++ protected:
++ virtual unsigned char * dump_command_field(unsigned char * a_buffer) const;
++ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
++};
++
+ /// Broadcast Write Telegram (BWR)
+ class BWR_Telegram : public Device_Addressing_Telegram {
+ public:
+@@ -203,7 +226,32 @@
+ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
+ };
+
++/// Node Addressed Physical Read Write Telegram (NPRW)
++/** @note older versions of the spec and the ethereal plugin use FPRW
++ for this type
++*/
++class NPRW_Telegram : public Device_Addressing_Telegram {
++ public:
++ /// Constructor
++ /** @param a_idx index
++ @param a_adp slave address
++ @param a_ado physical memory address
++ @param a_wkc working counter
++ @param a_datalen data_length
++ @param a_data data to be written
++ */
++ NPRW_Telegram(EC_USINT a_idx, EC_UINT a_adp,
++ EC_UINT a_ado,EC_UINT a_wkc,
++ EC_UINT a_datalen,
++ const unsigned char * a_data);
++
++ virtual ~NPRW_Telegram();
+
++ protected:
++ virtual unsigned char * dump_command_field(unsigned char * a_buffer) const;
++ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
++};
++
+ /// Node Addressed Physical Read Telegram (NPRD)
+ /** @note older versions of the spec and the ethereal plugin use FPWR
+ for this type
+Index: include/dll/ethercat_telegram.h
+===================================================================
+--- include/dll/ethercat_telegram.h (revision 36)
++++ include/dll/ethercat_telegram.h (working copy)
+@@ -78,6 +78,9 @@
+ ETHERCAT_TELEGRAM_HEADER_SIZE +
+ ETHERCAT_TELEGRAM_WKC_SIZE; }
+
++ /// attach telegram to this one - ordering is somewhat arbitrary
++ void attach(EC_Telegram *a_telegram);
++
+ /// Pointer to next telegram
+ EC_Telegram * next;
+ /// Pointer to previous telegram
Index: include/dll/ethercat_slave_memory.h
===================================================================
--- include/dll/ethercat_slave_memory.h (revision 36)
@@ -137,6 +428,30 @@
msg_priority = 0x03;
}
}
+Index: include/al/ethercat_process_data.h
+===================================================================
+--- include/al/ethercat_process_data.h (revision 36)
++++ include/al/ethercat_process_data.h (working copy)
+@@ -88,10 +88,15 @@
+ /// See note in class definition.
+ unsigned int m_is_running;
+
+- /// Telegram to be sent
+- LRW_Telegram m_lrw_telegram;
+- /// EtherCAT frame to be sent
+- EC_Ethernet_Frame m_lrw_frame;
++ /// Process data can be divided over MAX_CHUCKS packets
++ /// of upto CHUNK_SIZE bytes
++ static const unsigned MAX_CHUNKS=4;
++ static const unsigned CHUNK_SIZE=1486;
++
++ /// Telegram(s) to be sent
++ LRW_Telegram *m_lrw_telegram[MAX_CHUNKS];
++ /// EtherCAT frame(s) to be sent
++ EC_Ethernet_Frame *m_lrw_frame[MAX_CHUNKS];
+ };
+
+ #endif
Index: include/al/ethercat_slave_handler.h
===================================================================
--- include/al/ethercat_slave_handler.h (revision 36)
@@ -176,7 +491,15 @@
===================================================================
--- src/dll/ethercat_telegram.cxx (revision 36)
+++ src/dll/ethercat_telegram.cxx (working copy)
-@@ -144,7 +144,7 @@
+@@ -33,6 +33,7 @@
+
+ #include "dll/ethercat_telegram.h"
+ #include "ethercat/ethercat_log.h"
++#include <assert.h>
+
+ // Number of bits of the Reserved and NEXT field
+ static const EC_UINT ETHERCAT_LEN_NUM_BITS = 11;
+@@ -144,7 +145,7 @@
if (index == m_idx)
return true;
else{
@@ -185,7 +508,7 @@
return false;
}
}
-@@ -168,14 +168,14 @@
+@@ -168,14 +169,14 @@
nextbit = lennext & NEXT_BIT;
if ( ((nextbit == NEXT_BIT) && (next == NULL)) ||
((nextbit == 0x0000) && (next != NULL)) ){
@@ -202,6 +525,25 @@
return false;
}
return true;
+@@ -185,3 +186,18 @@
+ // irq field is currently unused
+ const EC_UINT EC_Telegram::m_irq = 0x0000;
+
++
++/** Attach a single a_telegram after this telegram in chain
++ */
++void EC_Telegram::attach(EC_Telegram *a_telegram)
++{
++ assert(this != a_telegram);
++ assert(a_telegram->next == NULL);
++ assert(a_telegram->previous == NULL);
++ a_telegram->next = this->next;
++ a_telegram->previous = this;
++ if (this->next != NULL) {
++ this->next->previous = a_telegram;
++ }
++ this->next = a_telegram;
++}
Index: src/dll/ethercat_slave_memory.cxx
===================================================================
--- src/dll/ethercat_slave_memory.cxx (revision 36)
@@ -266,7 +608,7 @@
===================================================================
--- src/dll/ethercat_dll.cxx (revision 36)
+++ src/dll/ethercat_dll.cxx (working copy)
-@@ -68,7 +68,7 @@
+@@ -68,10 +68,26 @@
{
bool succeed = m_if->txandrx(a_frame, m_if);
if (!succeed)
@@ -275,6 +617,105 @@
return succeed;
}
++int EtherCAT_DataLinkLayer::tx(EtherCAT_Frame * a_frame)
++{
++ int handle = m_if->tx(a_frame, m_if);
++ if (handle < 0)
++ ec_log(EC_LOG_INFO, "DLL::tx Error\n");
++ return handle;
++}
++
++bool EtherCAT_DataLinkLayer::rx(EtherCAT_Frame * a_frame, int a_handle)
++{
++ bool succeed = m_if->rx(a_frame, m_if, a_handle);
++ if (!succeed)
++ ec_log(EC_LOG_INFO, "DLL::rx Error\n");
++ return succeed;
++}
++
+ void EtherCAT_DataLinkLayer::attach(struct netif * netif)
+ {
+ m_if = netif;
+Index: src/dll/ethercat_device_addressed_telegram.cxx
+===================================================================
+--- src/dll/ethercat_device_addressed_telegram.cxx (revision 36)
++++ src/dll/ethercat_device_addressed_telegram.cxx (working copy)
+@@ -42,7 +42,9 @@
+ static const EC_USINT NPRD = 0x04; // Node addressed Physical read
+ static const EC_USINT APWR = 0x02; // Autoincrement Physical write
+ static const EC_USINT NPWR = 0x05; // Node addressed Physical write
+-static const EC_USINT ARMW = 0x0d; // Autoincrement Physical Read
++static const EC_USINT APRW = 0x03; // Autoincrement Physical read_write
++static const EC_USINT NPRW = 0x06; // Node addressed Physical read-write
++static const EC_USINT ARMW = 0x0d; // Autoincrement Physical write
+ // Multiple write
+ static const EC_USINT BRD = 0x07; // Broadcast Read
+ static const EC_USINT BWR = 0x08; // Broadcast Write
+@@ -137,6 +139,30 @@
+ }
+
+ // --------------------------------------------------
++// Auto Increment Physical Read Write Telegram
++// --------------------------------------------------
++APRW_Telegram::APRW_Telegram(EC_USINT a_idx, EC_UINT a_adp, EC_UINT a_ado, EC_UINT a_wkc,
++ EC_UINT a_datalen, const unsigned char * a_data)
++ : DA_TG(a_idx, a_adp, a_ado, a_wkc, a_datalen, a_data)
++{
++}
++
++APRW_Telegram::~APRW_Telegram(){}
++
++unsigned char *
++APRW_Telegram::dump_command_field(unsigned char * a_buffer) const
++{
++ a_buffer = host2nw(a_buffer, APRW);
++ return a_buffer;
++}
++
++const unsigned char * APRW_Telegram::build_command_field(const unsigned char * a_buffer)
++{
++ assert(a_buffer[0] == APRW);
++ return ++a_buffer;
++}
++
++// --------------------------------------------------
+ // Broadcast Write Telegram
+ // --------------------------------------------------
+ BWR_Telegram::BWR_Telegram(EC_USINT a_idx, EC_UINT a_ado,
+@@ -236,7 +262,33 @@
+ return ++a_buffer;
+ }
+
++
+ // --------------------------------------------------
++// Node Addressed Physical Read Write Telegram
++// --------------------------------------------------
++NPRW_Telegram::NPRW_Telegram(EC_USINT a_idx, EC_UINT a_adp, EC_UINT a_ado, EC_UINT a_wkc,
++ EC_UINT a_datalen, const unsigned char * a_data)
++ : DA_TG(a_idx, a_adp, a_ado, a_wkc, a_datalen, a_data)
++{
++}
++
++NPRW_Telegram::~NPRW_Telegram(){}
++
++unsigned char *
++NPRW_Telegram::dump_command_field(unsigned char * a_buffer) const
++{
++ a_buffer = host2nw(a_buffer, NPRW);
++ return a_buffer;
++}
++
++const unsigned char * NPRW_Telegram::build_command_field(const unsigned char * a_buffer)
++{
++ assert(a_buffer[0] == NPRW);
++ return ++a_buffer;
++}
++
++
++// --------------------------------------------------
+ // Auto Increment Physical Read Multiple Write Telegram
+ // --------------------------------------------------
+ ARMW_Telegram::ARMW_Telegram(EC_USINT a_idx, EC_UINT a_adp, EC_UINT a_ado, EC_UINT a_wkc,
Index: src/al/ethercat_slave_conf.cxx
===================================================================
--- src/al/ethercat_slave_conf.cxx (revision 36)
@@ -418,7 +859,39 @@
===================================================================
--- src/al/ethercat_process_data.cxx (revision 36)
+++ src/al/ethercat_process_data.cxx (working copy)
-@@ -74,7 +74,7 @@
+@@ -51,16 +51,27 @@
+ // al_instance cannot be initiated right now, since this results in a
+ // circular instantiation al_instance calls router_instance etc.
+ EtherCAT_PD_Buffer::EtherCAT_PD_Buffer()
+- : m_is_running(0),
+- m_lrw_telegram(0x00,0x00010000,0x00,0,NULL),
+- m_lrw_frame(&m_lrw_telegram)
++ : m_is_running(0)
+ {
++ for (unsigned i=0;i<MAX_CHUNKS;++i) {
++ m_lrw_telegram[i] = new LRW_Telegram(0x00,0x00010000,0x00,0,NULL);
++ m_lrw_frame[i] = new EC_Ethernet_Frame(m_lrw_telegram[i]);
++ }
++
+ // get pointer to DLL and logic
+ m_dll_instance = EtherCAT_DataLinkLayer::instance();
+ m_logic_instance = EC_Logic::instance();
+ }
+
+-EtherCAT_PD_Buffer::~EtherCAT_PD_Buffer(){}
++EtherCAT_PD_Buffer::~EtherCAT_PD_Buffer()
++{
++ for (unsigned i=0;i<MAX_CHUNKS;++i) {
++ delete m_lrw_telegram[i];
++ m_lrw_telegram[i] = NULL;
++ delete m_lrw_frame[i];
++ m_lrw_frame[i];
++ }
++}
+
+ void
+ EtherCAT_PD_Buffer::start()
+@@ -74,27 +85,70 @@
if (m_is_running > 0)
--m_is_running;
else
@@ -427,7 +900,24 @@
}
bool
-@@ -85,13 +85,25 @@
+ EtherCAT_PD_Buffer::txandrx(size_t datalen, unsigned char * data)
+ {
++ //define MAX_CHUNKS 4
++ //define CHUNK_SIZE 1486
++
++ if (datalen>(CHUNK_SIZE*MAX_CHUNKS)) {
++ ec_log(EC_LOG_ERROR, "PD_Buffer: Too much data (%zd) to send in %d chunks of %d bytes\n",
++ datalen, MAX_CHUNKS, CHUNK_SIZE);
++ return false;
++ }
++
++ int handles[MAX_CHUNKS];
++ for (unsigned i=0;i<MAX_CHUNKS;++i)
++ handles[i]=-1;
++
++ bool success = true;
+ if ( m_is_running != 0)
+ // In case only starting when all slaves in the appropriate state
// this becomes something like
// if ( EtherCAT_PD_Buffer::m_al_instance->get_num_slaves() == m_running)
{
@@ -439,27 +929,48 @@
- log(EC_LOG_ERROR, "PD_Buffer: Error sending PD\n");
- return false;
+ int dst = 0x00010000;
-+#define CHUNK_SIZE 1486
++ unsigned index=0;
+ while (datalen > 0)
+ {
++ assert(index<MAX_CHUNKS);
+ size_t chunk_size = datalen < CHUNK_SIZE ? datalen : CHUNK_SIZE;
-+ m_lrw_telegram.set_idx(m_logic_instance->get_idx());
-+ m_lrw_telegram.set_wkc(m_logic_instance->get_wkc());
-+ m_lrw_telegram.set_datalen(chunk_size);
-+ m_lrw_telegram.set_data(data);
-+ m_lrw_telegram.set_adr(dst);
++ LRW_Telegram* a_lrw_telegram(m_lrw_telegram[index]);
++ a_lrw_telegram->set_idx(m_logic_instance->get_idx());
++ a_lrw_telegram->set_wkc(m_logic_instance->get_wkc());
++ a_lrw_telegram->set_datalen(chunk_size);
++ a_lrw_telegram->set_data(data);
++ a_lrw_telegram->set_adr(dst);
+
-+ if (!(m_dll_instance->txandrx(&m_lrw_frame)))
++ int result = m_dll_instance->tx(m_lrw_frame[index]);
++ if (result < 0)
+ {
+ ec_log(EC_LOG_ERROR, "PD_Buffer: Error sending PD\n");
-+ return false;
++ success = false;
++ break;
+ }
++ handles[index] = result;
+ datalen -= chunk_size;
+ data += chunk_size;
+ dst += chunk_size;
++ ++index;
}
++
++ // Receive every packet that was sent
++ for(unsigned i=0; i<index; ++i) {
++ if (handles[i] != -1) {
++ if (!m_dll_instance->rx(m_lrw_frame[i],handles[i])) {
++ ec_log(EC_LOG_ERROR, "PD_Buffer: Error receiving PD\n");
++ success = false;
++ }
++ }
++ }
}
- return true;
+- return true;
++
++ return success;
+ }
+
+
Index: src/al/ethercat_slave_handler.cxx
===================================================================
--- src/al/ethercat_slave_handler.cxx (revision 36)
@@ -718,18 +1229,128 @@
===================================================================
--- src/arch/posix/ethercat_posix_drv.c (revision 36)
+++ src/arch/posix/ethercat_posix_drv.c (working copy)
-@@ -60,7 +60,7 @@
+@@ -30,7 +30,6 @@
+ // Automation GmbH, Eiserstrasse 5, D-33415 Verl, Germany.
+ //===========================================================================
+
+-
+ /*
+ Contributed by RvdM, October, 5th, 2007.
+ Purpose to use EML over PREEMPT_RT kernel
+@@ -39,7 +38,7 @@
+
+ #include <errno.h>
+ #include <signal.h>
+-#include <pthread.h>
++#include <pthread.h>
+ #include <sys/ioctl.h>
+ #include <sys/types.h>
+ #include <sys/socket.h>
+@@ -49,36 +48,94 @@
+ #include <net/if.h>
+ #include <arpa/inet.h>
+
++#include <assert.h>
+ #include <string.h>
+
+ #include <ethercat/netif.h>
+ #include <ethercat/ethercat_log.h>
+
++#include <time.h>
++
++
+ // Maximum tries to send and receive a message
+ #define MAX_TRIES_TX 10
+ // Timeout for receiving and transmitting messages is 2000 us
// specifying 1000us or lower did not work on plain Linux.
- // recv() returned immediately with EWOULDBLOCK.
- // May/must be different for PREEMPT_RT kernels.
+-// recv() returned immediately with EWOULDBLOCK.
+-// May/must be different for PREEMPT_RT kernels.
-#define TIMEOUT_USEC 2000
++// recv() returned immediately with EWOULDBLOCK.
++// May/must be different for xgPREEMPT_RT kernels.
+#define TIMEOUT_USEC 20000
++// Socket timeout - so input_thread can exit
++#define SOCKET_TIMEOUT_USEC 10000
++
// Maximum times the master may retry to create or close a socket
#define MAX_TRIES_SOCKET 10
-@@ -90,14 +90,24 @@
+ // Ethernet Header is 14 bytes: 2 * MAC ADDRESS + 2 bytes for the type
+ #define HEADER_SIZE 14
+ #define MAX_ETH_DATA 1500
+
++#define NSEC_PER_SEC 1000000000
++
++// Size of buffer that holds message from strerror
++#define ERRBUF_LEN 60
++
++static char* my_strerror(int errnum, char *buf, size_t buflen)
++{
++ assert(buflen>0);
++ assert(buf!=NULL);
++ buf[0] = '\0';
++ if (strerror_r(errnum, buf, buflen) != 0) {
++ snprintf(buf, buflen, "N%d", errnum);
++ }
++ return buf;
++}
++
++static void my_usleep(uint32_t usec)
++{
++ char errbuf[ERRBUF_LEN];
++ assert(usec<1000000);
++ struct timespec req, rem;
++ req.tv_sec = 0;
++ req.tv_nsec = usec*1000;
++ while (nanosleep(&req, &rem)!=0) {
++ int error = errno;
++ ec_log(EC_LOG_ERROR, "%s : Error : %s\n", __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ if (error != EINTR) {
++ break;
++ }
++ req = rem;
++ }
++ return;
++}
++
++// No longer set socket timeout, but insteads set timeout variable used by pthread_cond_timedwait.
++// Produces the simular semantics as setting socket timeout in non-threaded posix driver
++// Timeout value it on microseconds
+ int set_socket_timeout(struct netif* ni, int64_t timeout) {
++ if ((timeout*1000) >= NSEC_PER_SEC)
++ {
++ ec_log(EC_LOG_FATAL, "%s: timeout is too large : %ld\n", __func__, timeout);
++ assert(timeout*1000 < NSEC_PER_SEC);
++ return -1;
++ }
++ ni->timeout_us = timeout;
+ return 0;
+ }
+
+-int init_socket(const char* interface) {
++
++
++static int init_socket(const char* interface) {
+ int sock;
+ int tries = 0;
+ struct sockaddr_ll addr;
+ struct ifreq ifr;
+
++ char errbuf[ERRBUF_LEN];
++
+ while (((sock = socket(PF_PACKET, SOCK_RAW, htons(0x88A4))) < 0) && tries < MAX_TRIES_SOCKET) {
++ int error = errno;
++ fprintf(stderr, "Couldn't open raw socket for interface %s : %s\n",
++ interface, my_strerror(error,errbuf,sizeof(errbuf)));
+ sleep(1);
+ tries++;
+ }
+@@ -88,25 +145,45 @@
+ return -1;
+ }
- printf("Socket created: socket id: %d\n", sock);
+- printf("Socket created: socket id: %d\n", sock);
++ //printf("Socket created: socket id: %d\n", sock);
- int index_ioctl;
+ int rv, index_ioctl;
@@ -740,8 +1361,18 @@
+ close(sock);
+ return -1;
+ }
++ if (!(ifr.ifr_flags & IFF_UP)) {
++ fprintf(stderr,
++ "Interface %s is not UP\n"
++ " try : ifup %s\n"
++ ,interface, interface);
++ return -1;
++ }
+ if (!(ifr.ifr_flags & IFF_RUNNING)) {
-+ fprintf(stderr, "Interface %s is not UP and RUNNING\n", interface);
++ fprintf(stderr,
++ "Interface %s is not RUNNING\n"
++ " is cable plugged in and device powered?\n"
++ ,interface);
+ return -1;
+ }
+
@@ -751,78 +1382,910 @@
return -1;
}
- printf("Got interface: index: %d\n", index_ioctl);
-+ printf("Got interface: index: %d for %s\n", index_ioctl, ifr.ifr_name);
++ //printf("Got interface: index: %d for %s\n", index_ioctl, ifr.ifr_name);
struct timeval tv;
- tv.tv_sec = ( TIMEOUT_USEC ) / 1000000;
-@@ -167,12 +177,12 @@
- // The actual send
- int len_send = send(sock,(unsigned char *)&msg_to_send,msg_len ,0);
- if(len_send < 0)
+- tv.tv_sec = ( TIMEOUT_USEC ) / 1000000;
+- tv.tv_usec = ( TIMEOUT_USEC ) % 1000000;
++ tv.tv_sec = ( SOCKET_TIMEOUT_USEC ) / 1000000;
++ tv.tv_usec = ( SOCKET_TIMEOUT_USEC ) % 1000000;
+ if ( setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (void*)&tv, sizeof(tv) ) != 0) {
+ perror("Aborting: Cannot set timeout");
+ return -1;
+ } else {
+- printf("Packet round-trip timeout has been set to %u usecs.\n", TIMEOUT_USEC);
++ //printf("Input thread recv timeout has been set to %u usecs.\n", SOCKET_TIMEOUT_USEC);
+ }
+
+ addr.sll_family = AF_PACKET;
+@@ -123,8 +200,41 @@
+ }
+
+ int close_socket(struct netif *ni) {
++ assert(ni!=NULL);
++ char errbuf[ERRBUF_LEN];
++
++ // If sock is < 0, things have not been started
++ if (ni->socket_private < 0) {
++ ec_log(EC_LOG_ERROR, "%s: close called twice on netif\n", __func__);
++ return -1;
++ }
++
++ // First try stopping input thread
++ ec_log(EC_LOG_INFO, "%s : Stopping input thread...\n", __func__);
++ ni->stop = TRUE;
++ int tries = 0;
++ for (tries = 0; tries<10; ++tries) {
++ my_usleep(SOCKET_TIMEOUT_USEC);
++ if (ni->is_stopped) {
++ ec_log(EC_LOG_INFO, "%s : Input thread has stopped\n", __func__);
++ break;
++ }
++ }
++
++ // Force input thread to exit
++ if (!ni->is_stopped) {
++ ec_log(EC_LOG_ERROR, "%s: input thread d/n stop, cancelling thread\n", __func__);
++ int error = pthread_cancel(ni->input_thread);
++ if (error!=0) {
++ ec_log(EC_LOG_ERROR, "%s: error cancelling input thread : %s\n", __func__,
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ return -1;
++ }
++ my_usleep(SOCKET_TIMEOUT_USEC);
++ }
++
+ int ret = close(ni->socket_private);
+- int tries = 1;
++ tries = 1;
+ while(ret < 0 && tries < MAX_TRIES_SOCKET) {
+ ret = close(ni->socket_private);
+ tries++;
+@@ -132,6 +242,13 @@
+ }
+ if(ret < 0)
+ perror("Failed to close socket");
++
++ ni->socket_private = -1;
++
++ if (ni->is_stopped) {
++ free(ni);
++ }
++
+ return ret;
+ }
+
+@@ -142,112 +259,769 @@
+ unsigned char data [MAX_ETH_DATA];
+ };
+
+-static bool low_level_output(struct EtherCAT_Frame * frame, struct netif * netif)
++// Send packet, if successfull add handle to list of packet awaiting a response.
++// Returns postive handle to packet list for success.
++// Returns negative value for errors
++// Assumes txandrx_mut is held when this function is called
++static int low_level_output(struct EtherCAT_Frame * frame, struct netif * netif)
+ {
+- bool result = false;
++ struct netif *ni = netif;
++ assert(ni!=NULL);
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ char errbuf[ERRBUF_LEN];
++
++ // Is there a slot to send packet with?
++ if (ni->unclaimed_packets >= MAX_UNCLAIMED_PACKETS) {
++ // Maybe put a condition variable here is the future.
++ ec_log(EC_LOG_FATAL, "%s: too many outstanding packets : %d\n",
++ __func__, ni->unclaimed_packets);
++ ++ni->counters.tx_full;
++ return -1;
++ }
++
++ // Find empty spot in list to store information about the sent packet
++ struct outstanding_pkt* pkt=NULL;
++ unsigned pkt_index;
++ { unsigned xx;
++ for (xx=0; xx<PKT_LIST_SIZE; ++xx) {
++ pkt_index = (ni->next_pkt_index + xx) % PKT_LIST_SIZE;
++ struct outstanding_pkt* i_pkt = &ni->pkt_list[pkt_index];
++ if (i_pkt->is_free) {
++ pkt=i_pkt;
++ break;
++ }
++ }
++ }
++
++ // Couldn't find empty slot
++ if (pkt==NULL) {
++ ec_log(EC_LOG_FATAL, "%s: outstanding packet list is full\n", __func__);
++ ++ni->counters.tx_full;
++ return -1;
++ }
++
++ // higher level protocol error. Attempt to map too much data in one ethernet frame
+ struct eth_msg msg_to_send;
+- int tel;
+- for(tel = 0; tel<MAX_ETH_DATA; tel++)
+- msg_to_send.data[tel] = 0x00;
++ memset(msg_to_send.data, 0x00, MAX_ETH_DATA);
+ int len_dump = framedump(frame, msg_to_send.data, MAX_ETH_DATA);
++ if(len_dump==0) {
++ ec_log(EC_LOG_FATAL, "%s: message buffer overflow\n", __func__);
++ ++ni->counters.tx_error;
++ return -1;
++ }
++
++ // Send packet
+ int msg_len = len_dump + ETH_ALEN + ETH_ALEN + 2;
+- if(len_dump) {
+- int sock = netif->socket_private;
++ int sock = ni->socket_private;
+ // Destination address is broadcast MAC address
+ // FIXME Is this also valid for EtherCAT UDP ethernet frames?
+ memset(msg_to_send.ether_dhost, 0xFF, ETH_ALEN);
+- // Source address
+- for(tel = 0; tel<ETH_ALEN; tel++)
+- msg_to_send.ether_shost[tel] = (netif->hwaddr)[tel];
+- // Type is ethercat
++ // Source address, last 3 bytes are used as markers
++ memcpy(msg_to_send.ether_shost, ni->hwaddr, ETH_ALEN);
++ assert(pkt_index<=0xFF);
++ msg_to_send.ether_shost[3] = pkt_index;
++ ni->tx_seqnum = (ni->tx_seqnum+1) & 0xFFFF;
++ msg_to_send.ether_shost[4] = ((ni->tx_seqnum) >> 8) & 0xFF;
++ msg_to_send.ether_shost[5] = ((ni->tx_seqnum) >> 0) & 0xFF;
++
++ // EtherType is ethercat
+ msg_to_send.ether_type[0] = 0x88;
+ msg_to_send.ether_type[1] = 0xA4;
+
+
+- // The actual send
+- int len_send = send(sock,(unsigned char *)&msg_to_send,msg_len ,0);
+- if(len_send < 0)
- log(EC_LOG_FATAL, "low_level_output(): Cannot Send\n");
-+ ec_log(EC_LOG_FATAL, "low_level_output(): Cannot Send\n");
- else
- result = true;
+- else
+- result = true;
++ // Pad the packet so the data (src + dst + ethertype + payload) is at least 60 bytes
++ // This prevents wireshark from complianing about parsing errors
++ int pkt_len = (msg_len < 60) ? 60 : msg_len;
++
++ // Get send time
++ if (clock_gettime(CLOCK_REALTIME, &pkt->tx_time) != 0) {
++ int error = errno;
++ ec_log(EC_LOG_FATAL, "%s: Could not get send_time : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ ++ni->counters.tx_error;
++ return -1;
++ }
++
++ // Send packet - sending should not block.
++ int len_send = send(sock,(unsigned char *)&msg_to_send, pkt_len, MSG_DONTWAIT);
++ if(len_send < 0) {
++ int error = errno;
++ if (error == ENETDOWN) {
++ ++ni->counters.tx_net_down;
++ if ((ni->counters.tx_net_down & 0xFFF) == 1) {
++ ec_log(EC_LOG_FATAL, "%s: %lld times : %s\n",
++ __func__, (unsigned long long) ni->counters.tx_net_down, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++ } else {
++ ec_log(EC_LOG_FATAL, "%s: Cannot Send : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ ++ni->counters.tx_error;
++ }
++ return -1;
++ }
++ if (len_send != pkt_len) {
++ ec_log(EC_LOG_FATAL, "%s: Incomplete send, sent %d or %d bytes\n",
++ __func__, len_send, pkt_len);
++ ++ni->counters.tx_error;
++ return -1;
}
- else { // higher level protocol error. Attempt to map to much data in one ethernet frame
+- else { // higher level protocol error. Attempt to map to much data in one ethernet frame
- log(EC_LOG_FATAL, "EtherCAT fatal: message buffer overflow\n");
-+ ec_log(EC_LOG_FATAL, "EtherCAT fatal: message buffer overflow\n");
- // Release the message buffer again
- }
+- // Release the message buffer again
+- }
-@@ -189,12 +199,12 @@
- int len_recv = recv(sock,buffer_receive,sizeof(buffer_receive),0);
+- return result;
++ // Put packet in list of outstanding packets
++ ++ni->unclaimed_packets;
++ pkt->is_free = FALSE;
++ pkt->frame = frame;
++ memcpy(pkt->ether_shost, msg_to_send.ether_shost, ETH_ALEN);
++
++ // Generate handle for sent packet
++ int handle = 0xFFFFFF &
++ ((pkt->ether_shost[3]<<16) |
++ (pkt->ether_shost[4]<<8 ) |
++ (pkt->ether_shost[5]<<0 ));
++
++ // Start searching when picking next open packet slot
++ ni->next_pkt_index = (ni->next_pkt_index+1) % PKT_LIST_SIZE;
++
++ ++ni->counters.sent;
++ assert(handle >= 0);
++ return handle;
+ }
+
+
+-static bool low_level_input(struct EtherCAT_Frame * frame, struct netif * netif) {
++// Last two bytes of src MAC address lets us determine seqnum of packet
++static unsigned parse_seqnum(u_int8_t ether_shost[MAC_ADDRESS_SIZE]) {
++ unsigned seqnum =
++ ( ((unsigned) ether_shost[4]) << 8) |
++ ( ((unsigned) ether_shost[5]) << 0) ;
++ return seqnum;
++}
+
+- unsigned char buffer_receive[MAX_ETH_DATA + HEADER_SIZE];
+- struct eth_msg *msg_received = (struct eth_msg *)buffer_receive;
+- //Receive message from socket
+- int sock = netif->socket_private;
+- int len_recv = recv(sock,buffer_receive,sizeof(buffer_receive),0);
++
++enum input_retcode {RECOVERABLE_ERROR=0,UNRECOVERABLE_ERROR=-1,SUCCESS=1};
++
++// Receives packet and stores it in pkt_list
++// Returns 1 for success
++// Returns -1 for unrecoverable errors (socket problem, not enough input buffers)
++// Returns 0 for a timeout (no packet ready), or recoverable error (invalid packet received, bad packet format)
++// Assumes txandrx_mut is held when this function is called
++static enum input_retcode low_level_input(struct netif * ni)
++{
++ assert(ni!=NULL);
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ char errbuf[ERRBUF_LEN];
++
++ // Find empty packet buffer to hold recieved packet
++ struct pkt_buf *buf = NULL;
++ {unsigned buf_index;
++ for (buf_index=0; buf_index<BUF_LIST_SIZE; ++buf_index) {
++ struct pkt_buf *i_buf = &ni->buf_list[buf_index];
++ if (i_buf->is_free) {
++ buf = i_buf;
++ break;
++ }
++ }
++ }
++
++ if (buf==NULL) {
++ // No buffer to hold incoming packet
++ ec_log(EC_LOG_FATAL, "%s : EtherCAT fatal: packet buffer list if full\n", __func__);
++ return UNRECOVERABLE_ERROR;
++ }
++ assert(sizeof(struct eth_msg) < sizeof(buf->data)); //Const assert?
++ struct eth_msg *msg_received = (struct eth_msg *) buf->data;
++
++ // Try to receive message from socket
++ // Don't hold mutex while blocking on recv, also mark buffer as
++ // non-free, so it is not used by a different rx call.
++ int sock = ni->socket_private;
++ buf->is_free = FALSE;
++ pthread_mutex_unlock(&ni->txandrx_mut);
++ int len_recv = recv(sock,msg_received,sizeof(*msg_received),0);
++ pthread_mutex_lock(&ni->txandrx_mut);
++ buf->is_free = TRUE;
++
if(len_recv < 0) {
- //perror("low_level_input: Cannot receive msg: ");
+- //perror("low_level_input: Cannot receive msg: ");
- log(EC_LOG_ERROR, "low_level_input: Cannot receive msg: %d\n",len_recv);
-+ ec_log(EC_LOG_ERROR, "low_level_input: Cannot receive msg: %d\n",len_recv);
- return false;
+- return false;
++ int error = errno;
++ if (error != EAGAIN) { // log error, for anthing other than a timeout
++ ec_log(EC_LOG_ERROR, "%s: Cannot receive msg: %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ if (error == EINTR) {
++ return RECOVERABLE_ERROR;
++ } else {
++ return UNRECOVERABLE_ERROR;
++ }
++ }
++ return RECOVERABLE_ERROR;
}
++ if (len_recv <= sizeof(ETH_ALEN + ETH_ALEN + 2)) {
++ ec_log(EC_LOG_ERROR, "%s: recieved runt packet: %d\n",__func__,len_recv);
++ ++ni->counters.rx_runt_pkt;
++ return RECOVERABLE_ERROR; // we did receive a packet
++ }
++
if ( ((msg_received->ether_type[0]) != 0x88) || (msg_received->ether_type[1]) != 0xA4) {
- log(EC_LOG_ERROR, "low_level_input: No EtherCAT msg!\n");
-+ ec_log(EC_LOG_ERROR, "low_level_input: No EtherCAT msg!\n");
- return false;
+- return false;
++ ec_log(EC_LOG_ERROR, "%s: No EtherCAT msg!\n", __func__);
++ ++ni->counters.rx_not_ecat;
++ return RECOVERABLE_ERROR; // we did receive a packet
++ }
++
++ // Use 2nd and 3rd byte of source max address to identify instance of EML that sent this out
++ // Ignore packet with HW addresses from other EML libraries
++ if ((msg_received->ether_shost[1] != ni->hwaddr[1]) ||
++ (msg_received->ether_shost[2] != ni->hwaddr[2]) )
++ {
++ ++ni->counters.rx_other_eml;
++ if ((ni->counters.rx_other_eml & 0x3FF) == 1) {
++ ec_log(EC_LOG_WARNING,
++ "%s: received %lld packets sent out from another EML instance\n",
++ __func__, (unsigned long long) ni->counters.rx_other_eml);
++ }
++ return RECOVERABLE_ERROR;
++ }
++
++ // 3rd byte of source MAC address tells us where outstanding packet should be in queue
++ unsigned pkt_index = 0xFF & msg_received->ether_shost[3];
++ if (pkt_index >= PKT_LIST_SIZE) {
++ ec_log(EC_LOG_ERROR, "%s: packet doesn't belong in queue, bad index %d", __func__, pkt_index);
++ ++ni->counters.rx_bad_index;
++ return RECOVERABLE_ERROR;
++ }
++ struct outstanding_pkt *pkt= &ni->pkt_list[pkt_index];
++
++ unsigned rx_seqnum = parse_seqnum(msg_received->ether_shost);
++
++ // Print warning if this packet is a repeat or is recieved out-of-order
++ // This shouldn't happen with normal setup... It may be worthwhile
++ // know about this odd situation if it occurs.
++ int16_t diff = (rx_seqnum - ni->rx_seqnum) & 0xFFFF;
++ // diff == 1 : Good: this is the next expected packet..
++ // diff == 0 : Bad: seems like a duplicate packet has been recieved,
++ // diff < 1 : Bad: seems like an old got packets out-of-order, or old packet has been re-received
++ // diff > 1 : Ok: could mean previous <diff> packets have been
++ // lost, and this is first to make it back successfully
++ if (diff == 0) {
++ ec_log(EC_LOG_ERROR, "low_level_input: error, got packet with duplicate seqnum %d\n", rx_seqnum);
++ ++ni->counters.rx_dup_seqnum;
++ // allow following checks to drop packet
++ } else if (diff < 0) {
++ // Newer packet has already been recieved, somehow packets got
++ // reordered. This should not happen on a raw ethernet chain.
++ // Could occur if running ethercat over IP/UDP.
++ // Also, could be caused by buffering in OS.
++ ec_log(EC_LOG_ERROR,
++ "low_level_input: warning : got packet in incorrect order: got %d, expected %d\n",
++ rx_seqnum, (ni->rx_seqnum+1) & 0xFFFF);
++ ++ni->counters.rx_bad_order;
++ // Don't drop packet might still be uncollected --
++ // Allow following to filter it out.
++ }
++
++ // Determine if received seqnum matches seqnum used when sending packet
++ unsigned tx_seqnum = parse_seqnum(pkt->ether_shost);
++ if (rx_seqnum != tx_seqnum) {
++ ec_log(EC_LOG_ERROR,
++ "low_level_input: got packet with invalid seqnum: got %d, expected %d\n"
++ " next packet will be sent with seqnum %d\n"
++ " last received packet had seqnum %d\n"
++ , rx_seqnum, tx_seqnum, ni->tx_seqnum, ni->rx_seqnum);
++ ++ni->counters.rx_bad_seqnum;
++ return RECOVERABLE_ERROR;
++ }
++
++ // Make sure packet has not arrived already
++ if ( (pkt->buf!=NULL) ) {
++ ec_log(EC_LOG_ERROR, "low_level_input: got duplicate packet?\n");
++ ++ni->counters.rx_dup_pkt;
++ return RECOVERABLE_ERROR;
++ }
++
++ // Record most recently received seqnum
++ ni->rx_seqnum = rx_seqnum;
++
++ // Make sure packet has not been collected with rx
++ if ( (pkt->is_free==TRUE) ) {
++ ec_log(EC_LOG_ERROR,
++ "%s: got packet that has already been collected with rx(), increase socket timeout?\n", __func__);
++ ++ni->counters.rx_late_pkt;
++ // Keep track of how long it actually took for the late packet to get back.
++ struct timespec rx_time;
++ if (clock_gettime(CLOCK_REALTIME, &rx_time) != 0) {
++ int error = errno;
++ ec_log(EC_LOG_FATAL, "%s: Could not get recv time for late packet : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ } else {
++ unsigned round_trip_time_us =
++ (rx_time.tv_sec - pkt->tx_time.tv_sec) * 1000000 +
++ (rx_time.tv_nsec - pkt->tx_time.tv_nsec) / 1000;
++ ni->counters.rx_late_pkt_rtt_us = round_trip_time_us;
++ ni->counters.rx_late_pkt_rtt_us_sum += round_trip_time_us;
++ ec_log(EC_LOG_ERROR,
++ "%s: late packet arrived in %u us, timeout set to %u us\n",
++ __func__, round_trip_time_us, ni->timeout_us);
++ }
++ return RECOVERABLE_ERROR;
++ }
++
++ // Packet passes all checks... mark outstanding packet as having
++ // received reply - mark buffer as being used.
++ buf->is_free=FALSE;
++ pkt->buf=buf;
++
++ // Signal anybody waiting on sent packet
++ int error = pthread_cond_broadcast(&pkt->rx_cond);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL, "%s: cond broadcast : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ return UNRECOVERABLE_ERROR;
++ }
++
++ ++ni->counters.received;
++ return SUCCESS;
++}
++
++
++// Thread that continuously polls input and puts it into queue
++void* low_level_input_thread_func(void* data)
++{
++ char errbuf[ERRBUF_LEN];
++
++ // Increase priority of input thread
++ struct sched_param thread_param;
++ int policy = SCHED_FIFO;
++ thread_param.sched_priority = sched_get_priority_max(policy);
++ int error = pthread_setschedparam(pthread_self(), policy, &thread_param);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL, "%s : Setting thread sched : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++
++ struct netif * ni = (struct netif *) data;
++ ec_log(EC_LOG_INFO, "INFO: Starting input thread\n");
++ BOOL stop = FALSE;
++ while (stop==FALSE) {
++ error = pthread_mutex_lock(&ni->txandrx_mut);
++ assert(error==0);
++ stop = ni->stop;
++ if (low_level_input(ni)==UNRECOVERABLE_ERROR) {
++ ec_log(EC_LOG_FATAL, "%s : Unrecoverable error on input thread\n", __func__);
++ // Some unrecoverable error can be recovered with external conditions (ENETDOWN)
++ // However, don't spin on recv if it will immediately return an error code
++ //stop = TRUE;
++ my_usleep(10000);
++ }
++ error = pthread_mutex_unlock(&ni->txandrx_mut);
++ assert(error==0);
++ }
++ ec_log(EC_LOG_INFO, "INFO: Input thread is exiting\n");
++ ni->is_stopped=TRUE;
++ return NULL;
++}
++
++
++static void init_buf(struct pkt_buf *buf) {
++ buf->is_free = TRUE;
++}
++
++static BOOL init_pkt(struct outstanding_pkt *pkt) {
++ pkt->is_free = TRUE;
++ pkt->buf = NULL;
++ pkt->frame = NULL;
++ memset(pkt->ether_shost, 0, sizeof(pkt->ether_shost));
++
++ int error;
++ char errbuf[ERRBUF_LEN];
++
++ error = pthread_cond_init(&pkt->rx_cond,NULL);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"%s : Initializing rx condition var failed : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ return FALSE;
++ }
++
++ return TRUE;
++}
++
++
++// Tries lookup of packet from queue using handle
++// returns NULL if handle is stall/invalid, or pointer to outstanding pkt otherwise;
++// Assumes txandrx mutex is already held
++static struct outstanding_pkt *low_level_lookup(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ assert(frame!=NULL);
++ assert(ni!=NULL);
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ // Make sure handle makes sense, also make sure it is not a negative number
++ if ( ((handle>>24) & 0xFF) != 0) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: called with invalid handle 0x%08X\n", handle);
++ return NULL;
++ }
++
++ // Source MAC address packet should have
++ // First 3 bytes are sent by netif source MAC address
++ // Second 3 byte are encoded in handle.
++ u_int8_t ether_shost[ETH_ALEN];
++ memcpy(ether_shost, ni->hwaddr, ETH_ALEN);
++ ether_shost[3] = (handle>>16) & 0xFF;
++ ether_shost[4] = (handle>>8) & 0xFF;
++ ether_shost[5] = (handle>>0) & 0xFF;
++
++ // 3th byte of source MAC address tells us where outstanding packet should be in queue
++ unsigned pkt_index = 0xFF & ether_shost[3];
++ if (pkt_index >= PKT_LIST_SIZE) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: handle 0x%08X references bad pkt_index\n", handle, pkt_index);
++ return NULL; //error
++ }
++ struct outstanding_pkt *pkt = &ni->pkt_list[pkt_index];
++
++ // Check frame stored for outstanding packet against frame that was passed in
++ if (pkt->frame != frame) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: handle frame d/n match passed in frame \n");
++ return NULL; //error
++ }
++
++ // Check MAC address encoded in handle against mac address packet was sent out with
++ if (memcmp(pkt->ether_shost, ether_shost, ETH_ALEN) != 0) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: tried collecting with stale handle \n");
++ return NULL; //error
++ }
++
++ // Make sure packet was not already collected
++ if (pkt->is_free!=FALSE) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: tried collecting with same handle twice \n");
++ return NULL; //error
++ }
++
++ return pkt;
++}
++
++
++// Drop packet from queue of outstanding packets.
++// Returns FALSE if packet d/n seem to exist
++// Assumes txandrx mutex is already held
++BOOL low_level_release(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ // Lookup pointer to packet using handle
++ struct outstanding_pkt *pkt = low_level_lookup(frame, ni, handle);
++ if (pkt == NULL) {
++ return FALSE;
}
-@@ -202,7 +212,7 @@
- int succes = framebuild(frame,msg_received->data);
- if (succes != 0){
+- // build Ethercat Frame
+- int succes = framebuild(frame,msg_received->data);
+- if (succes != 0){
++ // Mark packet buffer as availible
++ if (pkt->buf!=NULL) {
++ init_buf(pkt->buf);
++ }
++
++ // Mark outstanding pkt as unused
++ pkt->is_free = TRUE;
++ pkt->buf = NULL;
++ pkt->frame = NULL;
++ // Don't clear source MAC address, to allow detection of packets
++ // that are received late, (rx was called before packet got back)
++
++ // One less outstanding packet
++ assert(ni->unclaimed_packets > 0);
++ --ni->unclaimed_packets;
++
++ // Packets are only released when they are not recieved
++ ++ni->counters.dropped;
++
++ return TRUE;
++}
++
++
++enum dequeue_retcode {DEQUEUE_SUCCESS=1,DEQUEUE_ERROR=-1,DEQUEUE_NOT_FOUND=0};
++
++// Tries retrieving packet from queue using handle
++// returns negative values for error, 0 if no packet is in queue, or postive value if packet is found
++// Assumes txandrx mutex is already held
++static int low_level_dequeue(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ // Lookup pointer to packet using handle
++ struct outstanding_pkt *pkt = low_level_lookup(frame, ni, handle);
++ if (pkt == NULL) {
++ return DEQUEUE_ERROR;
++ }
++
++ // OK, handle is good...
++ // has response packet been received ?
++ if (pkt->buf==NULL) {
++ return DEQUEUE_NOT_FOUND; // packet has not been recieved
++ }
++ assert(pkt->buf->is_free == FALSE);
++
++ // Response packet has been received...
++
++ // Get pointer to ethernet packet payout
++ assert(sizeof(struct eth_msg) < sizeof(pkt->buf->data)); //Const assert?
++ struct eth_msg *msg_received = (struct eth_msg *) pkt->buf->data;
++
++ // Mark packet buffer as availible
++ init_buf(pkt->buf);
++
++ // Mare outstanding pkt as unused
++ init_pkt(pkt);
++
++ int success = framebuild(frame,msg_received->data);
++ if (success != 0){
// FIXME decent error handling here
- log(EC_LOG_ERROR, "low_level_input: framebuilding failed!\n");
+- return false;
+- }
+ ec_log(EC_LOG_ERROR, "low_level_input: framebuilding failed!\n");
- return false;
- }
++ return DEQUEUE_ERROR;
++ }
++
++ // One less unclaimed packet
++ assert(ni->unclaimed_packets > 0);
++ --ni->unclaimed_packets;
++
++ ++ni->counters.collected;
++ return DEQUEUE_SUCCESS; // Good
++}
++
++// Transmits a packet.
++// Returns a negative value for errors, or positive integer handle
++// handle and frame* should be used to get response with ec_posix_rx()
++static int ec_posix_tx(struct EtherCAT_Frame * frame, struct netif * ni)
++{
++ assert(ni != NULL);
++
++ int error = pthread_mutex_lock (&ni->txandrx_mut);
++ assert(error==0);
++ int handle = low_level_output(frame,ni);
++ error = pthread_mutex_unlock (&ni->txandrx_mut);
++ assert(error==0);
++ return handle;
++}
++
++
++// Receives a packet that has been sent with ec_posix_tx()
++// Returns TRUE for success, FALSE for errors or timeout.
++static BOOL ec_posix_rx_common(struct EtherCAT_Frame * frame, struct netif * ni, int handle, BOOL mayblock)
++{
++ assert(ni != NULL);
++ assert(frame != NULL);
++ char errbuf[ERRBUF_LEN];
++
++ // Find rx mutex for outstanding packet
++ struct outstanding_pkt *pkt;
++ int error = pthread_mutex_lock(&ni->txandrx_mut);
++ assert(error==0);
++ if (error!=0) {
++ ec_log(EC_LOG_FATAL, "%s: error locking mutex : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ return FALSE;
++ }
++ pkt = low_level_lookup(frame, ni, handle);
-@@ -222,24 +232,24 @@
- return true;
+- return true;
++ // Bad handle, double rx, etc...
++ if (pkt == NULL) {
++ pthread_mutex_unlock(&ni->txandrx_mut);
++ return FALSE;
++ }
++
++ // Try pulling result from queue
++ enum dequeue_retcode result = low_level_dequeue(frame, ni, handle);
++
++ // If this can block, try waiting for condition signal
++ if (mayblock) {
++ // Wait until <timeout_us> after packet was sent
++ struct timespec timeout = pkt->tx_time;
++ timeout.tv_nsec += (ni->timeout_us * 1000);
++ if (timeout.tv_nsec >= NSEC_PER_SEC) {
++ timeout.tv_nsec -= NSEC_PER_SEC;
++ ++timeout.tv_sec;
++ }
++ assert(timeout.tv_nsec < NSEC_PER_SEC);
++ assert(timeout.tv_nsec >= 0);
++
++ // Put loop around pthread_cond_timedwait to handle spurious wakeups
++ while (result == DEQUEUE_NOT_FOUND) {
++ // Wait on recv condition from input thread
++ error = pthread_cond_timedwait(&pkt->rx_cond, &ni->txandrx_mut, &timeout);
++ if (error != 0) {
++ if (error == ETIMEDOUT) {
++ // Timeout
++ } else {
++ ec_log(EC_LOG_FATAL, "%s: error waiting on timed condition : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++ break;
++ } else {
++ result = low_level_dequeue(frame, ni, handle);
++ // dequeue should always return 1 (since we know the packet is there)
++ // ... unless there was a spurious wakeup.
++ if (result == DEQUEUE_NOT_FOUND) {
++ ec_log(EC_LOG_FATAL, "%s: spurious wakeup : dequeue result=%d\n",
++ __func__, result);
++ }
++ }
++ }
++ } //end if mayblock
++
++ // If we didn't get the packet, release it's resources
++ if (result!=DEQUEUE_SUCCESS) {
++ low_level_release(frame, ni, handle);
++ }
++
++ error = pthread_mutex_unlock(&ni->txandrx_mut);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL, "%s: error unlocking mutex : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++ return (result==DEQUEUE_SUCCESS) ? TRUE : FALSE;
+ }
+
+-// For thread safety: txandrx() can be called from multiple threads...
+-static pthread_mutex_t txandrx_mut;
+
+-static bool ec_rtdm_txandrx(struct EtherCAT_Frame * frame, struct netif * netif) {
++// Receives a packet that has been sent with ec_posix_tx()
++// Returns TRUE for success, FALSE for errors or timeout.
++static BOOL ec_posix_rx(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ return ec_posix_rx_common(frame, ni, handle, TRUE);
++}
++
++// Receives a packet that has been sent with ec_posix_tx(), doesn't wait
++// Returns TRUE for success, FALSE for errors or timeout.
++static BOOL ec_posix_rx_nowait(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ return ec_posix_rx_common(frame, ni, handle, FALSE);
++}
++
++
++// Only attempt to send one packet
++static BOOL ec_posix_txandrx_once(struct EtherCAT_Frame * frame, struct netif * ni)
++{
++ assert(ni != NULL);
++
++ int handle = ec_posix_tx(frame, ni);
++ if (handle < 0) {
++ return FALSE;
++ }
++ BOOL result = ec_posix_rx(frame, ni, handle);
++ return result;
++}
++
++
++// Normal txandrx, try sending packet multiple times before giving up.
++static BOOL ec_rtdm_txandrx(struct EtherCAT_Frame * frame, struct netif * netif) {
++ struct netif *ni = netif;
++ assert(ni != NULL);
++ char errbuf[ERRBUF_LEN];
++
+ int tries = 0;
+ while (tries < MAX_TRIES_TX) {
+- pthread_mutex_lock (&txandrx_mut);
+- if (low_level_output(frame,netif)){
+- if (low_level_input(frame,netif)){
+- pthread_mutex_unlock(&txandrx_mut);
+- return true;
++ BOOL success = ec_posix_txandrx_once(frame, ni);
++ if (success) {
++ if (tries > 0) {
++ ec_log(EC_LOG_ERROR, "low_level_txandrx: sending/receiving failed %d times\n", tries);
}
- else{
+- else{
- log(EC_LOG_ERROR, "low_level_txandrx: receiving failed\n");
-+ ec_log(EC_LOG_ERROR, "low_level_txandrx: receiving failed\n");
- pthread_mutex_unlock(&txandrx_mut);
- }
+- pthread_mutex_unlock(&txandrx_mut);
+- }
++ return TRUE;
}
- else{
+- else{
- log(EC_LOG_ERROR, "low_level_txandrx: sending failed\n");
-+ ec_log(EC_LOG_ERROR, "low_level_txandrx: sending failed\n");
- pthread_mutex_unlock(&txandrx_mut);
- }
+- pthread_mutex_unlock(&txandrx_mut);
+- }
++ int error = pthread_mutex_lock(&ni->txandrx_mut);
++ assert(error==0);
++ //++ni->retries;
++ error = pthread_mutex_unlock(&ni->txandrx_mut);
++ assert(error==0);
tries++;
}
- log(EC_LOG_FATAL, "low_level_txandrx: failed: MAX_TRIES_TX: Giving up\n");
-+ ec_log(EC_LOG_FATAL, "low_level_txandrx: failed: MAX_TRIES_TX: Giving up\n");
- return false;
+- return false;
++ ec_log(EC_LOG_FATAL, "low_level_txandrx: failed %d times: Giving up\n", MAX_TRIES_TX);
++ return FALSE;
}
++
struct netif* init_ec(const char * interface) {
++ char errbuf[ERRBUF_LEN];
++
int sock = init_socket(interface);
if(sock < 0) {
- log(EC_LOG_FATAL,"Socket initialisation failed\n");
+ ec_log(EC_LOG_FATAL,"Socket initialisation failed\n");
return 0;
}
++
struct netif* ni = (struct netif*)malloc(sizeof(struct netif));
++ if (ni==NULL) {
++ ec_log(EC_LOG_FATAL,"Allocating netif struct failed\n");
++ return NULL;
++ }
++ int error = pthread_mutexattr_init(&ni->txandrx_attr);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"Initializing txandrx mutex attr failed : %s\n",
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ free(ni);
++ return NULL;
++ }
++
++ error = pthread_mutexattr_settype(&ni->txandrx_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"Setting type of mutex attr failed : %s\n",
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ free(ni);
++ return NULL;
++ }
++
++ error = pthread_mutex_init(&ni->txandrx_mut, &ni->txandrx_attr);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"Initializing txandrx mutex failed : %s\n",
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ free(ni);
++ return NULL;
++ }
++
++ int index;
++ for (ind...
[truncated message content] |
