[SRU][N][PATCH 1/3] UBUNTU: SAUCE: ubuntu/igh-ecat: Import IgH EtherCAT master
Wen-chien Jesse Sung
jesse.sung at canonical.com
Wed Sep 16 11:33:39 UTC 2026
BugLink: https://launchpad.net/bugs/2138621
Upstream repo can be found at
https://gitlab.com/etherlab.org/ethercat
This imports from version 1.6.8 of the IgH EtherCAT Master.
tag: 1.6.8
commit: 2543cc55bd13833ddefb6cd9ec24a064cde68959
Signed-off-by: Wen-chien Jesse Sung <jesse.sung at canonical.com>
---
ubuntu/igh-ecat/config.h | 100 +
ubuntu/igh-ecat/devices/ecdev.h | 71 +
ubuntu/igh-ecat/devices/generic.c | 483 ++
ubuntu/igh-ecat/globals.h | 55 +
ubuntu/igh-ecat/include/Makefile.am | 26 +
ubuntu/igh-ecat/include/ecrt.h | 3153 ++++++++++++
ubuntu/igh-ecat/include/ectty.h | 106 +
ubuntu/igh-ecat/master/Kbuild.in | 112 +
ubuntu/igh-ecat/master/Makefile.am | 76 +
ubuntu/igh-ecat/master/api_usage_notes.md | 51 +
ubuntu/igh-ecat/master/cdev.c | 281 ++
ubuntu/igh-ecat/master/cdev.h | 53 +
ubuntu/igh-ecat/master/coe_emerg_ring.c | 169 +
ubuntu/igh-ecat/master/coe_emerg_ring.h | 70 +
ubuntu/igh-ecat/master/datagram.c | 644 +++
ubuntu/igh-ecat/master/datagram.h | 142 +
ubuntu/igh-ecat/master/datagram_pair.c | 193 +
ubuntu/igh-ecat/master/datagram_pair.h | 61 +
ubuntu/igh-ecat/master/debug.c | 258 +
ubuntu/igh-ecat/master/debug.h | 58 +
ubuntu/igh-ecat/master/device.c | 716 +++
ubuntu/igh-ecat/master/device.h | 150 +
ubuntu/igh-ecat/master/domain.c | 719 +++
ubuntu/igh-ecat/master/domain.h | 82 +
ubuntu/igh-ecat/master/doxygen.c | 82 +
ubuntu/igh-ecat/master/eoe_request.c | 80 +
ubuntu/igh-ecat/master/eoe_request.h | 70 +
ubuntu/igh-ecat/master/ethernet.c | 886 ++++
ubuntu/igh-ecat/master/ethernet.h | 122 +
ubuntu/igh-ecat/master/flag.c | 69 +
ubuntu/igh-ecat/master/flag.h | 51 +
ubuntu/igh-ecat/master/fmmu_config.c | 91 +
ubuntu/igh-ecat/master/fmmu_config.h | 58 +
ubuntu/igh-ecat/master/foe.h | 54 +
ubuntu/igh-ecat/master/foe_request.c | 195 +
ubuntu/igh-ecat/master/foe_request.h | 78 +
ubuntu/igh-ecat/master/fsm_change.c | 615 +++
ubuntu/igh-ecat/master/fsm_change.h | 84 +
ubuntu/igh-ecat/master/fsm_coe.c | 2537 ++++++++++
ubuntu/igh-ecat/master/fsm_coe.h | 74 +
ubuntu/igh-ecat/master/fsm_eoe.c | 509 ++
ubuntu/igh-ecat/master/fsm_eoe.h | 67 +
ubuntu/igh-ecat/master/fsm_foe.c | 915 ++++
ubuntu/igh-ecat/master/fsm_foe.h | 87 +
ubuntu/igh-ecat/master/fsm_master.c | 1472 ++++++
ubuntu/igh-ecat/master/fsm_master.h | 107 +
ubuntu/igh-ecat/master/fsm_pdo.c | 806 +++
ubuntu/igh-ecat/master/fsm_pdo.h | 76 +
ubuntu/igh-ecat/master/fsm_pdo_entry.c | 541 ++
ubuntu/igh-ecat/master/fsm_pdo_entry.h | 75 +
ubuntu/igh-ecat/master/fsm_sii.c | 482 ++
ubuntu/igh-ecat/master/fsm_sii.h | 82 +
ubuntu/igh-ecat/master/fsm_slave.c | 685 +++
ubuntu/igh-ecat/master/fsm_slave.h | 87 +
ubuntu/igh-ecat/master/fsm_slave_config.c | 1864 +++++++
ubuntu/igh-ecat/master/fsm_slave_config.h | 81 +
ubuntu/igh-ecat/master/fsm_slave_scan.c | 1121 +++++
ubuntu/igh-ecat/master/fsm_slave_scan.h | 75 +
ubuntu/igh-ecat/master/fsm_soe.c | 844 ++++
ubuntu/igh-ecat/master/fsm_soe.h | 67 +
ubuntu/igh-ecat/master/globals.h | 314 ++
ubuntu/igh-ecat/master/ioctl.c | 5591 +++++++++++++++++++++
ubuntu/igh-ecat/master/ioctl.h | 879 ++++
ubuntu/igh-ecat/master/mailbox.c | 203 +
ubuntu/igh-ecat/master/mailbox.h | 64 +
ubuntu/igh-ecat/master/master.c | 3350 ++++++++++++
ubuntu/igh-ecat/master/master.h | 386 ++
ubuntu/igh-ecat/master/module.c | 681 +++
ubuntu/igh-ecat/master/pdo.c | 309 ++
ubuntu/igh-ecat/master/pdo.h | 67 +
ubuntu/igh-ecat/master/pdo_entry.c | 124 +
ubuntu/igh-ecat/master/pdo_entry.h | 58 +
ubuntu/igh-ecat/master/pdo_list.c | 338 ++
ubuntu/igh-ecat/master/pdo_list.h | 71 +
ubuntu/igh-ecat/master/reg_request.c | 125 +
ubuntu/igh-ecat/master/reg_request.h | 59 +
ubuntu/igh-ecat/master/rt_locks.h | 75 +
ubuntu/igh-ecat/master/rtdm-ioctl.c | 1 +
ubuntu/igh-ecat/master/rtdm.c | 259 +
ubuntu/igh-ecat/master/rtdm.h | 50 +
ubuntu/igh-ecat/master/rtdm_details.h | 60 +
ubuntu/igh-ecat/master/rtdm_xenomai_v3.c | 193 +
ubuntu/igh-ecat/master/sdo.c | 124 +
ubuntu/igh-ecat/master/sdo.h | 61 +
ubuntu/igh-ecat/master/sdo_entry.c | 69 +
ubuntu/igh-ecat/master/sdo_entry.h | 64 +
ubuntu/igh-ecat/master/sdo_request.c | 256 +
ubuntu/igh-ecat/master/sdo_request.h | 75 +
ubuntu/igh-ecat/master/slave.c | 1010 ++++
ubuntu/igh-ecat/master/slave.h | 266 +
ubuntu/igh-ecat/master/slave_config.c | 1652 ++++++
ubuntu/igh-ecat/master/slave_config.h | 196 +
ubuntu/igh-ecat/master/soe_errors.c | 88 +
ubuntu/igh-ecat/master/soe_request.c | 333 ++
ubuntu/igh-ecat/master/soe_request.h | 76 +
ubuntu/igh-ecat/master/sync.c | 170 +
ubuntu/igh-ecat/master/sync.h | 60 +
ubuntu/igh-ecat/master/sync_config.c | 55 +
ubuntu/igh-ecat/master/sync_config.h | 51 +
ubuntu/igh-ecat/master/voe_handler.c | 553 ++
ubuntu/igh-ecat/master/voe_handler.h | 65 +
101 files changed, 41499 insertions(+)
create mode 100644 ubuntu/igh-ecat/config.h
create mode 100644 ubuntu/igh-ecat/devices/ecdev.h
create mode 100644 ubuntu/igh-ecat/devices/generic.c
create mode 100644 ubuntu/igh-ecat/globals.h
create mode 100644 ubuntu/igh-ecat/include/Makefile.am
create mode 100644 ubuntu/igh-ecat/include/ecrt.h
create mode 100644 ubuntu/igh-ecat/include/ectty.h
create mode 100644 ubuntu/igh-ecat/master/Kbuild.in
create mode 100644 ubuntu/igh-ecat/master/Makefile.am
create mode 100644 ubuntu/igh-ecat/master/api_usage_notes.md
create mode 100644 ubuntu/igh-ecat/master/cdev.c
create mode 100644 ubuntu/igh-ecat/master/cdev.h
create mode 100644 ubuntu/igh-ecat/master/coe_emerg_ring.c
create mode 100644 ubuntu/igh-ecat/master/coe_emerg_ring.h
create mode 100644 ubuntu/igh-ecat/master/datagram.c
create mode 100644 ubuntu/igh-ecat/master/datagram.h
create mode 100644 ubuntu/igh-ecat/master/datagram_pair.c
create mode 100644 ubuntu/igh-ecat/master/datagram_pair.h
create mode 100644 ubuntu/igh-ecat/master/debug.c
create mode 100644 ubuntu/igh-ecat/master/debug.h
create mode 100644 ubuntu/igh-ecat/master/device.c
create mode 100644 ubuntu/igh-ecat/master/device.h
create mode 100644 ubuntu/igh-ecat/master/domain.c
create mode 100644 ubuntu/igh-ecat/master/domain.h
create mode 100644 ubuntu/igh-ecat/master/doxygen.c
create mode 100644 ubuntu/igh-ecat/master/eoe_request.c
create mode 100644 ubuntu/igh-ecat/master/eoe_request.h
create mode 100644 ubuntu/igh-ecat/master/ethernet.c
create mode 100644 ubuntu/igh-ecat/master/ethernet.h
create mode 100644 ubuntu/igh-ecat/master/flag.c
create mode 100644 ubuntu/igh-ecat/master/flag.h
create mode 100644 ubuntu/igh-ecat/master/fmmu_config.c
create mode 100644 ubuntu/igh-ecat/master/fmmu_config.h
create mode 100644 ubuntu/igh-ecat/master/foe.h
create mode 100644 ubuntu/igh-ecat/master/foe_request.c
create mode 100644 ubuntu/igh-ecat/master/foe_request.h
create mode 100644 ubuntu/igh-ecat/master/fsm_change.c
create mode 100644 ubuntu/igh-ecat/master/fsm_change.h
create mode 100644 ubuntu/igh-ecat/master/fsm_coe.c
create mode 100644 ubuntu/igh-ecat/master/fsm_coe.h
create mode 100644 ubuntu/igh-ecat/master/fsm_eoe.c
create mode 100644 ubuntu/igh-ecat/master/fsm_eoe.h
create mode 100644 ubuntu/igh-ecat/master/fsm_foe.c
create mode 100644 ubuntu/igh-ecat/master/fsm_foe.h
create mode 100644 ubuntu/igh-ecat/master/fsm_master.c
create mode 100644 ubuntu/igh-ecat/master/fsm_master.h
create mode 100644 ubuntu/igh-ecat/master/fsm_pdo.c
create mode 100644 ubuntu/igh-ecat/master/fsm_pdo.h
create mode 100644 ubuntu/igh-ecat/master/fsm_pdo_entry.c
create mode 100644 ubuntu/igh-ecat/master/fsm_pdo_entry.h
create mode 100644 ubuntu/igh-ecat/master/fsm_sii.c
create mode 100644 ubuntu/igh-ecat/master/fsm_sii.h
create mode 100644 ubuntu/igh-ecat/master/fsm_slave.c
create mode 100644 ubuntu/igh-ecat/master/fsm_slave.h
create mode 100644 ubuntu/igh-ecat/master/fsm_slave_config.c
create mode 100644 ubuntu/igh-ecat/master/fsm_slave_config.h
create mode 100644 ubuntu/igh-ecat/master/fsm_slave_scan.c
create mode 100644 ubuntu/igh-ecat/master/fsm_slave_scan.h
create mode 100644 ubuntu/igh-ecat/master/fsm_soe.c
create mode 100644 ubuntu/igh-ecat/master/fsm_soe.h
create mode 100644 ubuntu/igh-ecat/master/globals.h
create mode 100644 ubuntu/igh-ecat/master/ioctl.c
create mode 100644 ubuntu/igh-ecat/master/ioctl.h
create mode 100644 ubuntu/igh-ecat/master/mailbox.c
create mode 100644 ubuntu/igh-ecat/master/mailbox.h
create mode 100644 ubuntu/igh-ecat/master/master.c
create mode 100644 ubuntu/igh-ecat/master/master.h
create mode 100644 ubuntu/igh-ecat/master/module.c
create mode 100644 ubuntu/igh-ecat/master/pdo.c
create mode 100644 ubuntu/igh-ecat/master/pdo.h
create mode 100644 ubuntu/igh-ecat/master/pdo_entry.c
create mode 100644 ubuntu/igh-ecat/master/pdo_entry.h
create mode 100644 ubuntu/igh-ecat/master/pdo_list.c
create mode 100644 ubuntu/igh-ecat/master/pdo_list.h
create mode 100644 ubuntu/igh-ecat/master/reg_request.c
create mode 100644 ubuntu/igh-ecat/master/reg_request.h
create mode 100644 ubuntu/igh-ecat/master/rt_locks.h
create mode 120000 ubuntu/igh-ecat/master/rtdm-ioctl.c
create mode 100644 ubuntu/igh-ecat/master/rtdm.c
create mode 100644 ubuntu/igh-ecat/master/rtdm.h
create mode 100644 ubuntu/igh-ecat/master/rtdm_details.h
create mode 100644 ubuntu/igh-ecat/master/rtdm_xenomai_v3.c
create mode 100644 ubuntu/igh-ecat/master/sdo.c
create mode 100644 ubuntu/igh-ecat/master/sdo.h
create mode 100644 ubuntu/igh-ecat/master/sdo_entry.c
create mode 100644 ubuntu/igh-ecat/master/sdo_entry.h
create mode 100644 ubuntu/igh-ecat/master/sdo_request.c
create mode 100644 ubuntu/igh-ecat/master/sdo_request.h
create mode 100644 ubuntu/igh-ecat/master/slave.c
create mode 100644 ubuntu/igh-ecat/master/slave.h
create mode 100644 ubuntu/igh-ecat/master/slave_config.c
create mode 100644 ubuntu/igh-ecat/master/slave_config.h
create mode 100644 ubuntu/igh-ecat/master/soe_errors.c
create mode 100644 ubuntu/igh-ecat/master/soe_request.c
create mode 100644 ubuntu/igh-ecat/master/soe_request.h
create mode 100644 ubuntu/igh-ecat/master/sync.c
create mode 100644 ubuntu/igh-ecat/master/sync.h
create mode 100644 ubuntu/igh-ecat/master/sync_config.c
create mode 100644 ubuntu/igh-ecat/master/sync_config.h
create mode 100644 ubuntu/igh-ecat/master/voe_handler.c
create mode 100644 ubuntu/igh-ecat/master/voe_handler.h
diff --git a/ubuntu/igh-ecat/config.h b/ubuntu/igh-ecat/config.h
new file mode 100644
index 00000000000000..558fe7e327eb33
--- /dev/null
+++ b/ubuntu/igh-ecat/config.h
@@ -0,0 +1,100 @@
+/* config.h. Generated from config.h.in by configure. */
+/* config.h.in. Generated from configure.ac by autoheader. */
+
+/* Debug interfaces enabled */
+/* #undef EC_DEBUG_IF */
+
+/* Debug ring enabled */
+/* #undef EC_DEBUG_RING */
+
+/* EoE support enabled */
+#define EC_EOE 1
+
+/* Use CPU timestamp counter */
+/* #undef EC_HAVE_CYCLES */
+
+/* Use vendor id / product code wildcards */
+/* #undef EC_IDENT_WILDCARDS */
+
+/* Max. number of Ethernet devices per master */
+#define EC_MAX_NUM_DEVICES 1
+
+/* Read alias adresses from register */
+/* #undef EC_REGALIAS */
+
+/* RTDM interface enabled */
+/* #undef EC_RTDM */
+
+/* Use Xenomai3 RTDM flavour */
+/* #undef EC_RTDM_XENOMAI_V3 */
+
+/* Output to syslog in RT context */
+#define EC_RT_SYSLOG 1
+
+/* Assign SII to PDI */
+#define EC_SII_ASSIGN 1
+
+/* Use hrtimer for scheduling */
+/* #undef EC_USE_HRTIMER */
+
+/* Define to 1 if you have the <dlfcn.h> header file. */
+#define HAVE_DLFCN_H 1
+
+/* Define to 1 if you have the <inttypes.h> header file. */
+#define HAVE_INTTYPES_H 1
+
+/* Define to 1 if you have the <stdint.h> header file. */
+#define HAVE_STDINT_H 1
+
+/* Define to 1 if you have the <stdio.h> header file. */
+#define HAVE_STDIO_H 1
+
+/* Define to 1 if you have the <stdlib.h> header file. */
+#define HAVE_STDLIB_H 1
+
+/* Define to 1 if you have the <strings.h> header file. */
+#define HAVE_STRINGS_H 1
+
+/* Define to 1 if you have the <string.h> header file. */
+#define HAVE_STRING_H 1
+
+/* Define to 1 if you have the <sys/stat.h> header file. */
+#define HAVE_SYS_STAT_H 1
+
+/* Define to 1 if you have the <sys/types.h> header file. */
+#define HAVE_SYS_TYPES_H 1
+
+/* Define to 1 if you have the <unistd.h> header file. */
+#define HAVE_UNISTD_H 1
+
+/* Define to the sub-directory where libtool stores uninstalled libraries. */
+#define LT_OBJDIR ".libs/"
+
+/* Name of package */
+#define PACKAGE "ethercat"
+
+/* Define to the address where bug reports for this package should be sent. */
+#define PACKAGE_BUGREPORT "fp at igh.de"
+
+/* Define to the full name of this package. */
+#define PACKAGE_NAME "ethercat"
+
+/* Define to the full name and version of this package. */
+#define PACKAGE_STRING "ethercat 1.6.8"
+
+/* Define to the one symbol short name of this package. */
+#define PACKAGE_TARNAME "ethercat"
+
+/* Define to the home page for this package. */
+#define PACKAGE_URL ""
+
+/* Define to the version of this package. */
+#define PACKAGE_VERSION "1.6.8"
+
+/* Define to 1 if all of the C89 standard headers exist (not just the ones
+ required in a freestanding environment). This macro is provided for
+ backward compatibility; new code need not use it. */
+#define STDC_HEADERS 1
+
+/* Version number of package */
+#define VERSION "1.6.8"
diff --git a/ubuntu/igh-ecat/devices/ecdev.h b/ubuntu/igh-ecat/devices/ecdev.h
new file mode 100644
index 00000000000000..2f2e51d50ef928
--- /dev/null
+++ b/ubuntu/igh-ecat/devices/ecdev.h
@@ -0,0 +1,71 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ *
+ * EtherCAT interface for EtherCAT device drivers.
+ *
+ * \defgroup DeviceInterface EtherCAT Device Interface
+ *
+ * Master interface for EtherCAT-capable network device drivers. Through the
+ * EtherCAT device interface, EtherCAT-capable network device drivers are able
+ * to connect their device(s) to the master, pass received frames and notify
+ * the master about status changes. The master on his part, can send his
+ * frames through connected devices.
+ */
+
+/****************************************************************************/
+
+#ifndef __ECDEV_H__
+#define __ECDEV_H__
+
+#include <linux/netdevice.h>
+
+/****************************************************************************/
+
+struct ec_device;
+typedef struct ec_device ec_device_t; /**< \see ec_device */
+
+/** Device poll function type.
+ */
+typedef void (*ec_pollfunc_t)(struct net_device *);
+
+/*****************************************************************************
+ * Offering/withdrawal functions
+ ****************************************************************************/
+
+ec_device_t *ecdev_offer(struct net_device *net_dev, ec_pollfunc_t poll,
+ struct module *module);
+void ecdev_withdraw(ec_device_t *device);
+
+/*****************************************************************************
+ * Device methods
+ ****************************************************************************/
+
+int ecdev_open(ec_device_t *device);
+void ecdev_close(ec_device_t *device);
+void ecdev_receive(ec_device_t *device, const void *data, size_t size);
+void ecdev_set_link(ec_device_t *device, uint8_t state);
+uint8_t ecdev_get_link(const ec_device_t *device);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/devices/generic.c b/ubuntu/igh-ecat/devices/generic.c
new file mode 100644
index 00000000000000..f6cef9b5028531
--- /dev/null
+++ b/ubuntu/igh-ecat/devices/generic.c
@@ -0,0 +1,483 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT generic Ethernet device module.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/version.h>
+#include <linux/if_arp.h> /* ARPHRD_ETHER */
+#include <linux/etherdevice.h>
+
+#include "../globals.h"
+#include "ecdev.h"
+
+#define PFX "ec_generic: "
+
+#define ETH_P_ETHERCAT 0x88A4
+
+#define EC_GEN_RX_BUF_SIZE 1600
+
+#if defined(CONFIG_SUSE_KERNEL) && LINUX_VERSION_CODE >= KERNEL_VERSION(5, 14, 0)
+#include <linux/suse_version.h>
+#else
+# ifndef SUSE_VERSION
+# define SUSE_VERSION 0
+# endif
+# ifndef SUSE_PATCHLEVEL
+# define SUSE_PATCHLEVEL 0
+# endif
+#endif
+
+/****************************************************************************/
+
+int __init ec_gen_init_module(void);
+void __exit ec_gen_cleanup_module(void);
+void ec_gen_poll(struct net_device *);
+
+/****************************************************************************/
+
+/** \cond */
+
+MODULE_AUTHOR("Florian Pose <fp at igh.de>");
+MODULE_DESCRIPTION("EtherCAT master generic Ethernet device module");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(EC_MASTER_VERSION);
+
+/** \endcond */
+
+struct list_head generic_devices;
+
+typedef struct {
+ struct list_head list;
+ struct net_device *netdev;
+ struct net_device *used_netdev;
+ struct socket *socket;
+ ec_device_t *ecdev;
+ uint8_t *rx_buf;
+} ec_gen_device_t;
+
+typedef struct {
+ struct list_head list;
+ struct net_device *netdev;
+ char name[IFNAMSIZ];
+ int ifindex;
+ uint8_t dev_addr[ETH_ALEN];
+} ec_gen_interface_desc_t;
+
+int ec_gen_device_init(ec_gen_device_t *);
+void ec_gen_device_clear(ec_gen_device_t *);
+int ec_gen_device_create_socket(ec_gen_device_t *, ec_gen_interface_desc_t *);
+int ec_gen_device_offer(ec_gen_device_t *, ec_gen_interface_desc_t *);
+int ec_gen_device_open(ec_gen_device_t *);
+int ec_gen_device_stop(ec_gen_device_t *);
+int ec_gen_device_start_xmit(ec_gen_device_t *, struct sk_buff *);
+void ec_gen_device_poll(ec_gen_device_t *);
+
+int offer_device(ec_gen_interface_desc_t *);
+void clear_devices(void);
+
+/****************************************************************************/
+
+static int ec_gen_netdev_open(struct net_device *dev)
+{
+ ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
+ return ec_gen_device_open(gendev);
+}
+
+/****************************************************************************/
+
+static int ec_gen_netdev_stop(struct net_device *dev)
+{
+ ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
+ return ec_gen_device_stop(gendev);
+}
+
+/****************************************************************************/
+
+static int ec_gen_netdev_start_xmit(
+ struct sk_buff *skb,
+ struct net_device *dev
+ )
+{
+ ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
+ return ec_gen_device_start_xmit(gendev, skb);
+}
+
+/****************************************************************************/
+
+void ec_gen_poll(struct net_device *dev)
+{
+ ec_gen_device_t *gendev = *((ec_gen_device_t **) netdev_priv(dev));
+ ec_gen_device_poll(gendev);
+}
+
+/****************************************************************************/
+
+static const struct net_device_ops ec_gen_netdev_ops = {
+ .ndo_open = ec_gen_netdev_open,
+ .ndo_stop = ec_gen_netdev_stop,
+ .ndo_start_xmit = ec_gen_netdev_start_xmit,
+};
+
+/****************************************************************************/
+
+/** Init generic device.
+ */
+int ec_gen_device_init(
+ ec_gen_device_t *dev
+ )
+{
+ ec_gen_device_t **priv;
+ char null = 0x00;
+
+ dev->ecdev = NULL;
+ dev->socket = NULL;
+ dev->rx_buf = NULL;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0)
+ dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null,
+ NET_NAME_UNKNOWN, ether_setup);
+#else
+ dev->netdev = alloc_netdev(sizeof(ec_gen_device_t *), &null, ether_setup);
+#endif
+ if (!dev->netdev) {
+ return -ENOMEM;
+ }
+
+ dev->netdev->netdev_ops = &ec_gen_netdev_ops;
+
+ priv = netdev_priv(dev->netdev);
+ *priv = dev;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Clear generic device.
+ */
+void ec_gen_device_clear(
+ ec_gen_device_t *dev
+ )
+{
+ if (dev->ecdev) {
+ ecdev_close(dev->ecdev);
+ ecdev_withdraw(dev->ecdev);
+ }
+ if (dev->socket) {
+ sock_release(dev->socket);
+ }
+ free_netdev(dev->netdev);
+
+ if (dev->rx_buf) {
+ kfree(dev->rx_buf);
+ }
+}
+
+/****************************************************************************/
+
+/** Creates a network socket.
+ */
+int ec_gen_device_create_socket(
+ ec_gen_device_t *dev,
+ ec_gen_interface_desc_t *desc
+ )
+{
+ int ret;
+ struct sockaddr_ll sa;
+
+ dev->rx_buf = kmalloc(EC_GEN_RX_BUF_SIZE, GFP_KERNEL);
+ if (!dev->rx_buf) {
+ return -ENOMEM;
+ }
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 2, 0)
+ ret = sock_create_kern(&init_net, PF_PACKET, SOCK_RAW,
+ htons(ETH_P_ETHERCAT), &dev->socket);
+#else
+ ret = sock_create_kern(PF_PACKET, SOCK_RAW, htons(ETH_P_ETHERCAT),
+ &dev->socket);
+#endif
+ if (ret) {
+ printk(KERN_ERR PFX "Failed to create socket (ret = %i).\n", ret);
+ return ret;
+ }
+
+ printk(KERN_ERR PFX "Binding socket to interface %i (%s).\n",
+ desc->ifindex, desc->name);
+
+ memset(&sa, 0x00, sizeof(sa));
+ sa.sll_family = AF_PACKET;
+ sa.sll_protocol = htons(ETH_P_ETHERCAT);
+ sa.sll_ifindex = desc->ifindex;
+ ret = kernel_bind(dev->socket, (struct sockaddr *) &sa, sizeof(sa));
+ if (ret) {
+ printk(KERN_ERR PFX "Failed to bind() socket to interface"
+ " (ret = %i).\n", ret);
+ sock_release(dev->socket);
+ dev->socket = NULL;
+ return ret;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Offer generic device to master.
+ */
+int ec_gen_device_offer(
+ ec_gen_device_t *dev,
+ ec_gen_interface_desc_t *desc
+ )
+{
+ int ret = 0;
+
+ dev->used_netdev = desc->netdev;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 15, 0) || (SUSE_VERSION == 15 && SUSE_PATCHLEVEL >= 5)
+ eth_hw_addr_set(dev->netdev, desc->dev_addr);
+#else
+ memcpy(dev->netdev->dev_addr, desc->dev_addr, ETH_ALEN);
+#endif
+
+ dev->ecdev = ecdev_offer(dev->netdev, ec_gen_poll, THIS_MODULE);
+ if (dev->ecdev) {
+ if (ec_gen_device_create_socket(dev, desc)) {
+ ecdev_withdraw(dev->ecdev);
+ dev->ecdev = NULL;
+ } else if (ecdev_open(dev->ecdev)) {
+ ecdev_withdraw(dev->ecdev);
+ dev->ecdev = NULL;
+ } else {
+ ecdev_set_link(dev->ecdev, netif_carrier_ok(dev->used_netdev)); // FIXME
+ ret = 1;
+ }
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Open the device.
+ */
+int ec_gen_device_open(
+ ec_gen_device_t *dev
+ )
+{
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Stop the device.
+ */
+int ec_gen_device_stop(
+ ec_gen_device_t *dev
+ )
+{
+ return 0;
+}
+
+/****************************************************************************/
+
+int ec_gen_device_start_xmit(
+ ec_gen_device_t *dev,
+ struct sk_buff *skb
+ )
+{
+ struct msghdr msg;
+ struct kvec iov;
+ size_t len = skb->len;
+ int ret;
+
+ ecdev_set_link(dev->ecdev, netif_carrier_ok(dev->used_netdev));
+
+ iov.iov_base = skb->data;
+ iov.iov_len = len;
+ memset(&msg, 0, sizeof(msg));
+
+ ret = kernel_sendmsg(dev->socket, &msg, &iov, 1, len);
+
+ return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY;
+}
+
+/****************************************************************************/
+
+/** Polls the device.
+ */
+void ec_gen_device_poll(
+ ec_gen_device_t *dev
+ )
+{
+ struct msghdr msg;
+ struct kvec iov;
+ int ret, budget = 10; // FIXME
+
+ ecdev_set_link(dev->ecdev, netif_carrier_ok(dev->used_netdev));
+
+ do {
+ iov.iov_base = dev->rx_buf;
+ iov.iov_len = EC_GEN_RX_BUF_SIZE;
+ memset(&msg, 0, sizeof(msg));
+
+ ret = kernel_recvmsg(dev->socket, &msg, &iov, 1, iov.iov_len,
+ MSG_DONTWAIT);
+ if (ret > 0) {
+ ecdev_receive(dev->ecdev, dev->rx_buf, ret);
+ } else if (ret < 0) {
+ break;
+ }
+ budget--;
+ } while (budget);
+}
+
+/****************************************************************************/
+
+/** Offer device.
+ */
+int offer_device(
+ ec_gen_interface_desc_t *desc
+ )
+{
+ ec_gen_device_t *gendev;
+ int ret = 0;
+
+ gendev = kmalloc(sizeof(ec_gen_device_t), GFP_KERNEL);
+ if (!gendev) {
+ return -ENOMEM;
+ }
+
+ ret = ec_gen_device_init(gendev);
+ if (ret) {
+ kfree(gendev);
+ return ret;
+ }
+
+ if (ec_gen_device_offer(gendev, desc)) {
+ list_add_tail(&gendev->list, &generic_devices);
+ } else {
+ ec_gen_device_clear(gendev);
+ kfree(gendev);
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Clear devices.
+ */
+void clear_devices(void)
+{
+ ec_gen_device_t *gendev, *next;
+
+ list_for_each_entry_safe(gendev, next, &generic_devices, list) {
+ list_del(&gendev->list);
+ ec_gen_device_clear(gendev);
+ kfree(gendev);
+ }
+}
+
+/****************************************************************************/
+
+/** Module initialization.
+ *
+ * Initializes \a master_count masters.
+ * \return 0 on success, else < 0
+ */
+int __init ec_gen_init_module(void)
+{
+ int ret = 0;
+ struct list_head descs;
+ struct net_device *netdev;
+ ec_gen_interface_desc_t *desc, *next;
+
+ printk(KERN_INFO PFX "EtherCAT master generic Ethernet device module %s\n",
+ EC_MASTER_VERSION);
+
+ INIT_LIST_HEAD(&generic_devices);
+ INIT_LIST_HEAD(&descs);
+
+ rcu_read_lock();
+ for_each_netdev_rcu(&init_net, netdev) {
+ if (netdev->type != ARPHRD_ETHER)
+ continue;
+ desc = kmalloc(sizeof(ec_gen_interface_desc_t), GFP_ATOMIC);
+ if (!desc) {
+ ret = -ENOMEM;
+ rcu_read_unlock();
+ goto out_err;
+ }
+ strncpy(desc->name, netdev->name, IFNAMSIZ);
+ desc->netdev = netdev;
+ desc->ifindex = netdev->ifindex;
+ memcpy(desc->dev_addr, netdev->dev_addr, ETH_ALEN);
+ list_add_tail(&desc->list, &descs);
+ }
+ rcu_read_unlock();
+
+ list_for_each_entry_safe(desc, next, &descs, list) {
+ ret = offer_device(desc);
+ if (ret) {
+ goto out_err;
+ }
+ kfree(desc);
+ }
+ return ret;
+
+out_err:
+ list_for_each_entry_safe(desc, next, &descs, list) {
+ list_del(&desc->list);
+ kfree(desc);
+ }
+ clear_devices();
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Module cleanup.
+ *
+ * Clears all master instances.
+ */
+void __exit ec_gen_cleanup_module(void)
+{
+ clear_devices();
+ printk(KERN_INFO PFX "Unloading.\n");
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+module_init(ec_gen_init_module);
+module_exit(ec_gen_cleanup_module);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/globals.h b/ubuntu/igh-ecat/globals.h
new file mode 100644
index 00000000000000..d2a470e9ce8c7f
--- /dev/null
+++ b/ubuntu/igh-ecat/globals.h
@@ -0,0 +1,55 @@
+/******************************************************************************
+ *
+ * Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT master.
+ *
+ * The file is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU Lesser General Public License as published by the
+ * Free Software Foundation; version 2.1 of the License.
+ *
+ * This file is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this file. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *****************************************************************************/
+
+/**
+ \file
+ Global definitions and macros.
+*/
+
+/*****************************************************************************/
+
+#ifndef __EC_GLOBALS_H__
+#define __EC_GLOBALS_H__
+
+#include "config.h"
+
+/******************************************************************************
+ * Overall macros
+ *****************************************************************************/
+
+/** Helper macro for EC_STR(), literates a macro argument.
+ *
+ * \param X argument to literate.
+ */
+#define EC_LIT(X) #X
+
+/** Converts a macro argument to a string.
+ *
+ * \param X argument to stringify.
+ */
+#define EC_STR(X) EC_LIT(X)
+
+/** Master version string
+ */
+#define EC_MASTER_VERSION VERSION " " EC_STR(REV)
+
+/*****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/include/Makefile.am b/ubuntu/igh-ecat/include/Makefile.am
new file mode 100644
index 00000000000000..3ae0eeff8bbf7a
--- /dev/null
+++ b/ubuntu/igh-ecat/include/Makefile.am
@@ -0,0 +1,26 @@
+#-----------------------------------------------------------------------------
+#
+# Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+#
+# This file is part of the IgH EtherCAT Master.
+#
+# The IgH EtherCAT Master is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License version 2, as
+# published by the Free Software Foundation.
+#
+# The IgH EtherCAT Master is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+# Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with the IgH EtherCAT Master; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+#
+#-----------------------------------------------------------------------------
+
+include_HEADERS = \
+ ecrt.h \
+ ectty.h
+
+#-----------------------------------------------------------------------------
diff --git a/ubuntu/igh-ecat/include/ecrt.h b/ubuntu/igh-ecat/include/ecrt.h
new file mode 100644
index 00000000000000..3ab462fbc2b59d
--- /dev/null
+++ b/ubuntu/igh-ecat/include/ecrt.h
@@ -0,0 +1,3153 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT master userspace library.
+ *
+ * The IgH EtherCAT master userspace library is free software; you can
+ * redistribute it and/or modify it under the terms of the GNU Lesser General
+ * Public License as published by the Free Software Foundation; version 2.1
+ * of the License.
+ *
+ * The IgH EtherCAT master userspace library is distributed in the hope that
+ * it will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with the IgH EtherCAT master userspace library. If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/** \file
+ *
+ * EtherCAT master application interface.
+ *
+ * \defgroup ApplicationInterface EtherCAT Application Interface
+ *
+ * EtherCAT interface for realtime applications. This interface is designed
+ * for realtime modules that want to use EtherCAT. There are functions to
+ * request a master, to map process data, to communicate with slaves via CoE
+ * and to configure and activate the bus.
+ *
+ *
+ * Changes in version 1.6.0:
+ *
+ * - Added the ecrt_master_scan_progress() method, the
+ * ec_master_scan_progress_t structure and the EC_HAVE_SCAN_PROGRESS
+ * definition to check for its existence.
+ * - Added the EoE configuration methods ecrt_slave_config_eoe_mac_address(),
+ * ecrt_slave_config_eoe_ip_address(), ecrt_slave_config_eoe_subnet_mask(),
+ * ecrt_slave_config_eoe_default_gateway(),
+ * ecrt_slave_config_eoe_dns_address(),
+ * ecrt_slave_config_eoe_hostname() and the EC_HAVE_SET_IP
+ * definition to check for its existence.
+ * - Added ecrt_slave_config_state_timeout() to set the application-layer
+ * state change timeout and EC_HAVE_STATE_TIMEOUT to check for its
+ * existence.
+ *
+ * Changes since version 1.5.2:
+ *
+ * - Added the ecrt_slave_config_flag() method and the EC_HAVE_FLAGS
+ * definition to check for its existence.
+ * - Added SoE IDN requests, including the datatype ec_soe_request_t and the
+ * methods ecrt_slave_config_create_soe_request(),
+ * ecrt_soe_request_object(), ecrt_soe_request_timeout(),
+ * ecrt_soe_request_data(), ecrt_soe_request_data_size(),
+ * ecrt_soe_request_state(), ecrt_soe_request_write() and
+ * ecrt_soe_request_read(). Use the EC_HAVE_SOE_REQUESTS to check, if the
+ * functionality is available.
+ *
+ * Changes in version 1.5.2:
+ *
+ * - Added redundancy_active flag to ec_domain_state_t.
+ * - Added ecrt_master_link_state() method and ec_master_link_state_t to query
+ * the state of a redundant link.
+ * - Added the EC_HAVE_REDUNDANCY define, to check, if the interface contains
+ * redundancy features.
+ * - Added ecrt_sdo_request_index() to change SDO index and subindex after
+ * request creation.
+ * - Added interface for retrieving CoE emergency messages, i. e.
+ * ecrt_slave_config_emerg_size(), ecrt_slave_config_emerg_pop(),
+ * ecrt_slave_config_emerg_clear(), ecrt_slave_config_emerg_overruns() and
+ * the defines EC_HAVE_EMERGENCY and EC_COE_EMERGENCY_MSG_SIZE.
+ * - Added interface for direct EtherCAT register access: Added data type
+ * ec_reg_request_t and methods ecrt_slave_config_create_reg_request(),
+ * ecrt_reg_request_data(), ecrt_reg_request_state(),
+ * ecrt_reg_request_write(), ecrt_reg_request_read() and the feature flag
+ * EC_HAVE_REG_ACCESS.
+ * - Added method to select the reference clock,
+ * ecrt_master_select_reference_clock() and the feature flag
+ * EC_HAVE_SELECT_REF_CLOCK to check, if the method is available.
+ * - Added method to get the reference clock time,
+ * ecrt_master_reference_clock_time() and the feature flag
+ * EC_HAVE_REF_CLOCK_TIME to have the possibility to synchronize the master
+ * clock to the reference clock.
+ * - Changed the data types of the shift times in ecrt_slave_config_dc() to
+ * int32_t to correctly display negative shift times.
+ * - Added ecrt_slave_config_reg_pdo_entry_pos() and the feature flag
+ * EC_HAVE_REG_BY_POS for registering PDO entries with non-unique indices
+ * via their positions in the mapping.
+ *
+ * Changes in version 1.5:
+ *
+ * - Added the distributed clocks feature and the respective method
+ * ecrt_slave_config_dc() to configure a slave for cyclic operation, and
+ * ecrt_master_application_time(), ecrt_master_sync_reference_clock() and
+ * ecrt_master_sync_slave_clocks() for offset and drift compensation. The
+ * EC_TIMEVAL2NANO() macro can be used for epoch time conversion, while the
+ * ecrt_master_sync_monitor_queue() and ecrt_master_sync_monitor_process()
+ * methods can be used to monitor the synchrony.
+ * - Improved the callback mechanism. ecrt_master_callbacks() now takes two
+ * callback functions for sending and receiving datagrams.
+ * ecrt_master_send_ext() is used to execute the sending of non-application
+ * datagrams.
+ * - Added watchdog configuration (method ecrt_slave_config_watchdog(),
+ * #ec_watchdog_mode_t, \a watchdog_mode parameter in ec_sync_info_t and
+ * ecrt_slave_config_sync_manager()).
+ * - Added ecrt_slave_config_complete_sdo() method to download an SDO during
+ * configuration via CompleteAccess.
+ * - Added ecrt_master_deactivate() to remove the master configuration.
+ * - Added ecrt_open_master() and ecrt_master_reserve() separation for
+ * userspace.
+ * - Added master information interface (methods ecrt_master(),
+ * ecrt_master_get_slave(), ecrt_master_get_sync_manager(),
+ * ecrt_master_get_pdo() and ecrt_master_get_pdo_entry()) to get information
+ * about the currently connected slaves and the PDO entries provided.
+ * - Added ecrt_master_sdo_download(), ecrt_master_sdo_download_complete() and
+ * ecrt_master_sdo_upload() methods to let an application transfer SDOs
+ * before activating the master.
+ * - Changed the meaning of the negative return values of
+ * ecrt_slave_config_reg_pdo_entry() and ecrt_slave_config_sdo*().
+ * - Implemented the Vendor-specific over EtherCAT mailbox protocol. See
+ * ecrt_slave_config_create_voe_handler().
+ * - Renamed ec_sdo_request_state_t to #ec_request_state_t, because it is also
+ * used by VoE handlers.
+ * - Removed 'const' from argument of ecrt_sdo_request_state(), because the
+ * userspace library has to modify object internals.
+ * - Added 64-bit data access macros.
+ * - Added ecrt_slave_config_idn() method for storing SoE IDN configurations,
+ * and ecrt_master_read_idn() and ecrt_master_write_idn() to read/write IDNs
+ * ad-hoc via the user-space library.
+ * - Added ecrt_master_reset() to initiate retrying to configure slaves.
+ *
+ * @{
+ */
+
+/****************************************************************************/
+
+#ifndef __ECRT_H__
+#define __ECRT_H__
+
+#ifdef __KERNEL__
+#include <asm/byteorder.h>
+#include <linux/types.h>
+#include <linux/time.h>
+#include <linux/in.h> // struct in_addr
+#else
+#include <stdlib.h> // for size_t
+#include <stdint.h>
+#include <sys/time.h> // for struct timeval
+#include <netinet/in.h> // struct in_addr
+#endif
+
+/*****************************************************************************
+ * Global definitions
+ ****************************************************************************/
+
+/** EtherCAT realtime interface major version number.
+ */
+#define ECRT_VER_MAJOR 1
+
+/** EtherCAT realtime interface minor version number.
+ */
+#define ECRT_VER_MINOR 6
+
+/** EtherCAT realtime interface version word generator.
+ */
+#define ECRT_VERSION(a, b) (((a) << 8) + (b))
+
+/** EtherCAT realtime interface version word.
+ */
+#define ECRT_VERSION_MAGIC ECRT_VERSION(ECRT_VER_MAJOR, ECRT_VER_MINOR)
+
+/*****************************************************************************
+ * Feature flags
+ ****************************************************************************/
+
+/** Defined, if the redundancy features are available.
+ *
+ * I. e. if the \a redundancy_active flag in ec_domain_state_t and the
+ * ecrt_master_link_state() method are available.
+ */
+#define EC_HAVE_REDUNDANCY
+
+/** Defined, if the CoE emergency ring feature is available.
+ *
+ * I. e. if the ecrt_slave_config_emerg_*() methods are available.
+ */
+#define EC_HAVE_EMERGENCY
+
+/** Defined, if the register access interface is available.
+ *
+ * I. e. if the methods ecrt_slave_config_create_reg_request(),
+ * ecrt_reg_request_data(), ecrt_reg_request_state(), ecrt_reg_request_write()
+ * and ecrt_reg_request_read() are available.
+ */
+#define EC_HAVE_REG_ACCESS
+
+/** Defined if the method ecrt_master_select_reference_clock() is available.
+ */
+#define EC_HAVE_SELECT_REF_CLOCK
+
+/** Defined if the method ecrt_master_reference_clock_time() is available.
+ */
+#define EC_HAVE_REF_CLOCK_TIME
+
+/** Defined if the method ecrt_slave_config_reg_pdo_entry_pos() is available.
+ */
+#define EC_HAVE_REG_BY_POS
+
+/** Defined if the method ecrt_master_sync_reference_clock_to() is available.
+ */
+#define EC_HAVE_SYNC_TO
+
+/** Defined if the method ecrt_slave_config_flag() is available.
+ */
+#define EC_HAVE_FLAGS
+
+/** Defined if the methods ecrt_slave_config_create_soe_request(),
+ * ecrt_soe_request_object(), ecrt_soe_request_timeout(),
+ * ecrt_soe_request_data(), ecrt_soe_request_data_size(),
+ * ecrt_soe_request_state(), ecrt_soe_request_write() and
+ * ecrt_soe_request_read() and the datatype ec_soe_request_t are available.
+ */
+#define EC_HAVE_SOE_REQUESTS
+
+/** Defined, if the method ecrt_master_scan_progress() and the
+ * ec_master_scan_progress_t structure are available.
+ */
+#define EC_HAVE_SCAN_PROGRESS
+
+/** Defined, if the methods ecrt_slave_config_eoe_mac_address(),
+ * ecrt_slave_config_eoe_ip_address(), ecrt_slave_config_eoe_subnet_mask(),
+ * ecrt_slave_config_eoe_default_gateway(),
+ * ecrt_slave_config_eoe_dns_address(), ecrt_slave_config_eoe_hostname() are
+ * available.
+ */
+#define EC_HAVE_SET_IP
+
+/** Defined, if the method ecrt_slave_config_state_timeout() is available.
+ */
+#define EC_HAVE_STATE_TIMEOUT
+
+/****************************************************************************/
+
+/** Symbol visibility control macro.
+ */
+#ifndef EC_PUBLIC_API
+# if defined(ethercat_EXPORTS) && !defined(__KERNEL__)
+# define EC_PUBLIC_API __attribute__ ((visibility ("default")))
+# else
+# define EC_PUBLIC_API
+# endif
+#endif
+
+/****************************************************************************/
+
+/** End of list marker.
+ *
+ * This can be used with ecrt_slave_config_pdos().
+ */
+#define EC_END ~0U
+
+/** Maximum number of sync managers per slave.
+ */
+#define EC_MAX_SYNC_MANAGERS 16
+
+/** Maximum string length.
+ *
+ * Used in ec_slave_info_t.
+ */
+#define EC_MAX_STRING_LENGTH 64
+
+/** Maximum number of slave ports. */
+#define EC_MAX_PORTS 4
+
+/** Timeval to nanoseconds conversion.
+ *
+ * This macro converts a Unix epoch time to EtherCAT DC time.
+ *
+ * \see void ecrt_master_application_time()
+ *
+ * \param TV struct timeval containing epoch time.
+ */
+#define EC_TIMEVAL2NANO(TV) \
+ (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_usec * 1000ULL)
+
+/** Size of a CoE emergency message in byte.
+ *
+ * \see ecrt_slave_config_emerg_pop().
+ */
+#define EC_COE_EMERGENCY_MSG_SIZE 8
+
+/*****************************************************************************
+ * Data types
+ ****************************************************************************/
+
+struct ec_master;
+typedef struct ec_master ec_master_t; /**< \see ec_master */
+
+struct ec_slave_config;
+typedef struct ec_slave_config ec_slave_config_t; /**< \see ec_slave_config */
+
+struct ec_domain;
+typedef struct ec_domain ec_domain_t; /**< \see ec_domain */
+
+struct ec_sdo_request;
+typedef struct ec_sdo_request ec_sdo_request_t; /**< \see ec_sdo_request. */
+
+struct ec_soe_request;
+typedef struct ec_soe_request ec_soe_request_t; /**< \see ec_soe_request. */
+
+struct ec_voe_handler;
+typedef struct ec_voe_handler ec_voe_handler_t; /**< \see ec_voe_handler. */
+
+struct ec_reg_request;
+typedef struct ec_reg_request ec_reg_request_t; /**< \see ec_reg_request. */
+
+/****************************************************************************/
+
+/** Master state.
+ *
+ * This is used for the output parameter of ecrt_master_state().
+ *
+ * \see ecrt_master_state().
+ */
+typedef struct {
+ unsigned int slaves_responding; /**< Sum of responding slaves on all
+ Ethernet devices. */
+ unsigned int al_states : 4; /**< Application-layer states of all slaves.
+ The states are coded in the lower 4 bits.
+ If a bit is set, it means that at least one
+ slave in the network is in the corresponding
+ state:
+ - Bit 0: \a INIT
+ - Bit 1: \a PREOP
+ - Bit 2: \a SAFEOP
+ - Bit 3: \a OP */
+ unsigned int link_up : 1; /**< \a true, if at least one Ethernet link is
+ up. */
+} ec_master_state_t;
+
+/****************************************************************************/
+
+/** Redundant link state.
+ *
+ * This is used for the output parameter of ecrt_master_link_state().
+ *
+ * \see ecrt_master_link_state().
+ */
+typedef struct {
+ unsigned int slaves_responding; /**< Sum of responding slaves on the given
+ link. */
+ unsigned int al_states : 4; /**< Application-layer states of the slaves on
+ the given link. The states are coded in the
+ lower 4 bits. If a bit is set, it means
+ that at least one slave in the network is in
+ the corresponding state:
+ - Bit 0: \a INIT
+ - Bit 1: \a PREOP
+ - Bit 2: \a SAFEOP
+ - Bit 3: \a OP */
+ unsigned int link_up : 1; /**< \a true, if the given Ethernet link is up.
+ */
+} ec_master_link_state_t;
+
+/****************************************************************************/
+
+/** Slave configuration state.
+ *
+ * This is used as an output parameter of ecrt_slave_config_state().
+ *
+ * \see ecrt_slave_config_state().
+ */
+typedef struct {
+ unsigned int online : 1; /**< The slave is online. */
+ unsigned int operational : 1; /**< The slave was brought into \a OP state
+ using the specified configuration. */
+ unsigned int al_state : 4; /**< The application-layer state of the slave.
+ - 1: \a INIT
+ - 2: \a PREOP
+ - 4: \a SAFEOP
+ - 8: \a OP
+
+ Note that each state is coded in a different
+ bit! */
+} ec_slave_config_state_t;
+
+/****************************************************************************/
+
+/** Master information.
+ *
+ * This is used as an output parameter of ecrt_master().
+ *
+ * \see ecrt_master().
+ */
+typedef struct {
+ unsigned int slave_count; /**< Number of slaves in the network. */
+ unsigned int link_up : 1; /**< \a true, if the network link is up. */
+ uint8_t scan_busy; /**< \a true, while the master is scanning the network.
+ */
+ uint64_t app_time; /**< Application time. */
+} ec_master_info_t;
+
+/****************************************************************************/
+
+/** Master scan progress information.
+ *
+ * This is used as an output parameter of ecrt_master_scan_progress().
+ *
+ * \see ecrt_master_scan_progress().
+ */
+typedef struct {
+ unsigned int slave_count; /**< Number of slaves detected. */
+ unsigned int scan_index; /**< Index of the slave that is currently
+ scanned. If it is less than the \a
+ slave_count, the network scan is in progress.
+ */
+} ec_master_scan_progress_t;
+
+/****************************************************************************/
+
+/** EtherCAT slave port descriptor.
+ */
+typedef enum {
+ EC_PORT_NOT_IMPLEMENTED, /**< Port is not implemented. */
+ EC_PORT_NOT_CONFIGURED, /**< Port is not configured. */
+ EC_PORT_EBUS, /**< Port is an E-Bus. */
+ EC_PORT_MII /**< Port is a MII. */
+} ec_slave_port_desc_t;
+
+/****************************************************************************/
+
+/** EtherCAT slave port information.
+ */
+typedef struct {
+ uint8_t link_up; /**< Link detected. */
+ uint8_t loop_closed; /**< Loop closed. */
+ uint8_t signal_detected; /**< Detected signal on RX port. */
+} ec_slave_port_link_t;
+
+/****************************************************************************/
+
+/** Slave information.
+ *
+ * This is used as an output parameter of ecrt_master_get_slave().
+ *
+ * \see ecrt_master_get_slave().
+ */
+typedef struct {
+ uint16_t position; /**< Offset of the slave in the ring. */
+ uint32_t vendor_id; /**< Vendor-ID stored on the slave. */
+ uint32_t product_code; /**< Product-Code stored on the slave. */
+ uint32_t revision_number; /**< Revision-Number stored on the slave. */
+ uint32_t serial_number; /**< Serial-Number stored on the slave. */
+ uint16_t alias; /**< The slaves alias if not equal to 0. */
+ int16_t current_on_ebus; /**< Used current in mA. */
+ struct {
+ ec_slave_port_desc_t desc; /**< Physical port type. */
+ ec_slave_port_link_t link; /**< Port link state. */
+ uint32_t receive_time; /**< Receive time on DC transmission delay
+ measurement. */
+ uint16_t next_slave; /**< Ring position of next DC slave on that
+ port. */
+ uint32_t delay_to_next_dc; /**< Delay [ns] to next DC slave. */
+ } ports[EC_MAX_PORTS]; /**< Port information. */
+ uint8_t al_state; /**< Current state of the slave. */
+ uint8_t error_flag; /**< Error flag for that slave. */
+ uint8_t sync_count; /**< Number of sync managers. */
+ uint16_t sdo_count; /**< Number of SDOs. */
+ char name[EC_MAX_STRING_LENGTH]; /**< Name of the slave. */
+} ec_slave_info_t;
+
+/****************************************************************************/
+
+/** Domain working counter interpretation.
+ *
+ * This is used in ec_domain_state_t.
+ */
+typedef enum {
+ EC_WC_ZERO = 0, /**< No registered process data were exchanged. */
+ EC_WC_INCOMPLETE, /**< Some of the registered process data were
+ exchanged. */
+ EC_WC_COMPLETE /**< All registered process data were exchanged. */
+} ec_wc_state_t;
+
+/****************************************************************************/
+
+/** Domain state.
+ *
+ * This is used for the output parameter of ecrt_domain_state().
+ */
+typedef struct {
+ unsigned int working_counter; /**< Value of the last working counter. */
+ ec_wc_state_t wc_state; /**< Working counter interpretation. */
+ unsigned int redundancy_active; /**< Redundant link is in use. */
+} ec_domain_state_t;
+
+/****************************************************************************/
+
+/** Direction type for PDO assignment functions.
+ */
+typedef enum {
+ EC_DIR_INVALID, /**< Invalid direction. Do not use this value. */
+ EC_DIR_OUTPUT, /**< Values written by the master. */
+ EC_DIR_INPUT, /**< Values read by the master. */
+ EC_DIR_COUNT /**< Number of directions. For internal use only. */
+} ec_direction_t;
+
+/****************************************************************************/
+
+/** Watchdog mode for sync manager configuration.
+ *
+ * Used to specify, if a sync manager's watchdog is to be enabled.
+ */
+typedef enum {
+ EC_WD_DEFAULT, /**< Use the default setting of the sync manager. */
+ EC_WD_ENABLE, /**< Enable the watchdog. */
+ EC_WD_DISABLE, /**< Disable the watchdog. */
+} ec_watchdog_mode_t;
+
+/****************************************************************************/
+
+/** PDO entry configuration information.
+ *
+ * This is the data type of the \a entries field in ec_pdo_info_t.
+ *
+ * \see ecrt_slave_config_pdos().
+ */
+typedef struct {
+ uint16_t index; /**< PDO entry index. */
+ uint8_t subindex; /**< PDO entry subindex. */
+ uint8_t bit_length; /**< Size of the PDO entry in bit. */
+} ec_pdo_entry_info_t;
+
+/****************************************************************************/
+
+/** PDO configuration information.
+ *
+ * This is the data type of the \a pdos field in ec_sync_info_t.
+ *
+ * \see ecrt_slave_config_pdos().
+ */
+typedef struct {
+ uint16_t index; /**< PDO index. */
+ unsigned int n_entries; /**< Number of PDO entries in \a entries to map.
+ Zero means, that the default mapping shall be
+ used (this can only be done if the slave is
+ present at configuration time). */
+ ec_pdo_entry_info_t const *entries; /**< Array of PDO entries to map. Can
+ either be \a NULL, or must contain
+ at least \a n_entries values. */
+} ec_pdo_info_t;
+
+/****************************************************************************/
+
+/** Sync manager configuration information.
+ *
+ * This can be use to configure multiple sync managers including the PDO
+ * assignment and PDO mapping. It is used as an input parameter type in
+ * ecrt_slave_config_pdos().
+ */
+typedef struct {
+ uint8_t index; /**< Sync manager index. Must be less
+ than #EC_MAX_SYNC_MANAGERS for a valid sync manager,
+ but can also be \a 0xff to mark the end of the list. */
+ ec_direction_t dir; /**< Sync manager direction. */
+ unsigned int n_pdos; /**< Number of PDOs in \a pdos. */
+ ec_pdo_info_t const *pdos; /**< Array with PDOs to assign. This must
+ contain at least \a n_pdos PDOs. */
+ ec_watchdog_mode_t watchdog_mode; /**< Watchdog mode. */
+} ec_sync_info_t;
+
+/****************************************************************************/
+
+/** List record type for PDO entry mass-registration.
+ *
+ * This type is used for the array parameter of the
+ * ecrt_domain_reg_pdo_entry_list()
+ */
+typedef struct {
+ uint16_t alias; /**< Slave alias address. */
+ uint16_t position; /**< Slave position. */
+ uint32_t vendor_id; /**< Slave vendor ID. */
+ uint32_t product_code; /**< Slave product code. */
+ uint16_t index; /**< PDO entry index. */
+ uint8_t subindex; /**< PDO entry subindex. */
+ unsigned int *offset; /**< Pointer to a variable to store the PDO entry's
+ (byte-)offset in the process data. */
+ unsigned int *bit_position; /**< Pointer to a variable to store a bit
+ position (0-7) within the \a offset. Can be
+ NULL, in which case an error is raised if
+ the PDO entry does not byte-align. */
+} ec_pdo_entry_reg_t;
+
+/****************************************************************************/
+
+/** Request state.
+ *
+ * This is used as return type for ecrt_sdo_request_state() and
+ * ecrt_voe_handler_state().
+ */
+typedef enum {
+ EC_REQUEST_UNUSED, /**< Not requested. */
+ EC_REQUEST_BUSY, /**< Request is being processed. */
+ EC_REQUEST_SUCCESS, /**< Request was processed successfully. */
+ EC_REQUEST_ERROR, /**< Request processing failed. */
+} ec_request_state_t;
+
+/****************************************************************************/
+
+/** Application-layer state.
+ */
+typedef enum {
+ EC_AL_STATE_INIT = 1, /**< Init. */
+ EC_AL_STATE_PREOP = 2, /**< Pre-operational. */
+ EC_AL_STATE_SAFEOP = 4, /**< Safe-operational. */
+ EC_AL_STATE_OP = 8, /**< Operational. */
+} ec_al_state_t;
+
+/*****************************************************************************
+ * Global functions
+ ****************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Returns the version magic of the realtime interface.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Value of ECRT_VERSION_MAGIC() at EtherCAT master compile time.
+ */
+EC_PUBLIC_API unsigned int ecrt_version_magic(void);
+
+/** Requests an EtherCAT master for realtime operation.
+ *
+ * Before an application can access an EtherCAT master, it has to reserve one
+ * for exclusive use.
+ *
+ * In userspace, this is a convenience function for ecrt_open_master() and
+ * ecrt_master_reserve().
+ *
+ * This function has to be the first function an application has to call to
+ * use EtherCAT. The function takes the index of the master as its argument.
+ * The first master has index 0, the n-th master has index n - 1. The number
+ * of masters has to be specified when loading the master module.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return Pointer to the reserved master, otherwise \a NULL.
+ */
+EC_PUBLIC_API ec_master_t *ecrt_request_master(
+ unsigned int master_index /**< Index of the master to request. */
+ );
+
+#ifndef __KERNEL__
+
+/** Opens an EtherCAT master for userspace access.
+ *
+ * This function has to be the first function an application has to call to
+ * use EtherCAT. The function takes the index of the master as its argument.
+ * The first master has index 0, the n-th master has index n - 1. The number
+ * of masters has to be specified when loading the master module.
+ *
+ * For convenience, the function ecrt_request_master() can be used.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return Pointer to the opened master, otherwise \a NULL.
+ */
+EC_PUBLIC_API ec_master_t *ecrt_open_master(
+ unsigned int master_index /**< Index of the master to request. */
+ );
+
+#endif // #ifndef __KERNEL__
+
+/** Releases a requested EtherCAT master.
+ *
+ * After use, a master it has to be released to make it available for other
+ * applications.
+ *
+ * This method frees all created data structures. It should not be called in
+ * realtime context.
+ *
+ * If the master was activated, ecrt_master_deactivate() is called internally.
+ *
+ * \apiusage{master_any,blocking}
+ */
+EC_PUBLIC_API void ecrt_release_master(
+ ec_master_t *master /**< EtherCAT master */
+ );
+
+/*****************************************************************************
+ * Master methods
+ ****************************************************************************/
+
+#ifndef __KERNEL__
+
+/** Reserves an EtherCAT master for realtime operation.
+ *
+ * Before an application can use PDO/domain registration functions or SDO
+ * request functions on the master, it has to reserve one for exclusive use.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return 0 in case of success, else < 0
+ */
+EC_PUBLIC_API int ecrt_master_reserve(
+ ec_master_t *master /**< EtherCAT master */
+ );
+
+#endif // #ifndef __KERNEL__
+
+#ifdef __KERNEL__
+
+/** Sets the locking callbacks.
+ *
+ * For concurrent master access, i. e. if other instances than the application
+ * want to send and receive datagrams on the network, the application has to
+ * provide a callback mechanism. This method takes two function pointers as
+ * its parameters. Asynchronous master access (like EoE processing) is only
+ * possible if the callbacks have been set.
+ *
+ * The task of the send callback (\a send_cb) is to decide, if the network
+ * hardware is currently accessible and whether or not to call the
+ * ecrt_master_send_ext() method.
+ *
+ * The task of the receive callback (\a receive_cb) is to decide, if a call to
+ * ecrt_master_receive() is allowed and to execute it respectively.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \attention This method has to be called before ecrt_master_activate().
+ */
+void ecrt_master_callbacks(
+ ec_master_t *master, /**< EtherCAT master */
+ void (*send_cb)(void *), /**< Datagram sending callback. */
+ void (*receive_cb)(void *), /**< Receive callback. */
+ void *cb_data /**< Arbitrary pointer passed to the callback functions.
+ */
+ );
+
+#endif /* __KERNEL__ */
+
+/** Creates a new process data domain.
+ *
+ * For process data exchange, at least one process data domain is needed.
+ * This method creates a new process data domain and returns a pointer to the
+ * new domain object. This object can be used for registering PDOs and
+ * exchanging them in cyclic operation.
+ *
+ * This method allocates memory and should be called in non-realtime context
+ * before ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return Pointer to the new domain on success, else NULL.
+ */
+EC_PUBLIC_API ec_domain_t *ecrt_master_create_domain(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Obtains a slave configuration.
+ *
+ * Creates a slave configuration object for the given \a alias and \a position
+ * tuple and returns it. If a configuration with the same \a alias and \a
+ * position already exists, it will be re-used. In the latter case, the given
+ * vendor ID and product code are compared to the stored ones. On mismatch, an
+ * error message is raised and the function returns \a NULL.
+ *
+ * Slaves are addressed with the \a alias and \a position parameters.
+ * - If \a alias is zero, \a position is interpreted as the desired slave's
+ * ring position.
+ * - If \a alias is non-zero, it matches a slave with the given alias. In this
+ * case, \a position is interpreted as ring offset, starting from the
+ * aliased slave, so a position of zero means the aliased slave itself and a
+ * positive value matches the n-th slave behind the aliased one.
+ *
+ * If the slave with the given address is found during the configuration,
+ * its vendor ID and product code are matched against the given value. On
+ * mismatch, the slave is not configured and an error message is raised.
+ *
+ * If different slave configurations are pointing to the same slave during
+ * configuration, a warning is raised and only the first configuration is
+ * applied.
+ *
+ * This method allocates memory and should be called in non-realtime context
+ * before ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval >0 Pointer to the slave configuration structure.
+ * \retval NULL in the error case.
+ */
+EC_PUBLIC_API ec_slave_config_t *ecrt_master_slave_config(
+ ec_master_t *master, /**< EtherCAT master */
+ uint16_t alias, /**< Slave alias. */
+ uint16_t position, /**< Slave position. */
+ uint32_t vendor_id, /**< Expected vendor ID. */
+ uint32_t product_code /**< Expected product code. */
+ );
+
+/** Selects the reference clock for distributed clocks.
+ *
+ * If this method is not called for a certain master, or if the slave
+ * configuration pointer is NULL, then the first slave with DC functionality
+ * will provide the reference clock.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_select_reference_clock(
+ ec_master_t *master, /**< EtherCAT master. */
+ ec_slave_config_t *sc /**< Slave config of the slave to use as the
+ * reference slave (or NULL). */
+ );
+
+/** Obtains master information.
+ *
+ * No memory is allocated on the heap in this function.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \attention The pointer to this structure must point to a valid variable.
+ *
+ * \return 0 in case of success, else < 0
+ */
+EC_PUBLIC_API int ecrt_master(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_master_info_t *master_info /**< Structure that will output the
+ information */
+ );
+
+/** Obtains network scan progress information.
+ *
+ * No memory is allocated on the heap in this function.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \attention The pointer to this structure must point to a valid variable.
+ *
+ * \return 0 in case of success, else < 0
+ */
+EC_PUBLIC_API int ecrt_master_scan_progress(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_master_scan_progress_t *progress /**< Structure that will output
+ the progress information. */
+ );
+
+/** Obtains slave information.
+ *
+ * Tries to find the slave with the given ring position. The obtained
+ * information is stored in a structure. No memory is allocated on the heap in
+ * this function.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \attention The pointer to this structure must point to a valid variable.
+ *
+ * \return 0 in case of success, else < 0
+ */
+EC_PUBLIC_API int ecrt_master_get_slave(
+ ec_master_t *master, /**< EtherCAT master */
+ uint16_t slave_position, /**< Slave position. */
+ ec_slave_info_t *slave_info /**< Structure that will output the
+ information */
+ );
+
+#ifndef __KERNEL__
+
+/** Returns the proposed configuration of a slave's sync manager.
+ *
+ * Fills a given ec_sync_info_t structure with the attributes of a sync
+ * manager. The \a pdos field of the return value is left empty. Use
+ * ecrt_master_get_pdo() to get the PDO information.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \return zero on success, else non-zero
+ */
+EC_PUBLIC_API int ecrt_master_get_sync_manager(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint8_t sync_index, /**< Sync manager index. Must be less
+ than #EC_MAX_SYNC_MANAGERS. */
+ ec_sync_info_t *sync /**< Pointer to output structure. */
+ );
+
+/** Returns information about a currently assigned PDO.
+ *
+ * Fills a given ec_pdo_info_t structure with the attributes of a currently
+ * assigned PDO of the given sync manager. The \a entries field of the return
+ * value is left empty. Use ecrt_master_get_pdo_entry() to get the PDO
+ * entry information.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \retval zero on success, else non-zero
+ */
+EC_PUBLIC_API int ecrt_master_get_pdo(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint8_t sync_index, /**< Sync manager index. Must be less
+ than #EC_MAX_SYNC_MANAGERS. */
+ uint16_t pos, /**< Zero-based PDO position. */
+ ec_pdo_info_t *pdo /**< Pointer to output structure. */
+ );
+
+/** Returns information about a currently mapped PDO entry.
+ *
+ * Fills a given ec_pdo_entry_info_t structure with the attributes of a
+ * currently mapped PDO entry of the given PDO.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \retval zero on success, else non-zero
+ */
+EC_PUBLIC_API int ecrt_master_get_pdo_entry(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint8_t sync_index, /**< Sync manager index. Must be less
+ than #EC_MAX_SYNC_MANAGERS. */
+ uint16_t pdo_pos, /**< Zero-based PDO position. */
+ uint16_t entry_pos, /**< Zero-based PDO entry position. */
+ ec_pdo_entry_info_t *entry /**< Pointer to output structure. */
+ );
+
+#endif /* #ifndef __KERNEL__ */
+
+/** Executes an SDO download request to write data to a slave.
+ *
+ * This request is processed by the master state machine. This method blocks,
+ * until the request has been processed and may not be called in realtime
+ * context.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_master_sdo_download(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint16_t index, /**< Index of the SDO. */
+ uint8_t subindex, /**< Subindex of the SDO. */
+ const uint8_t *data, /**< Data buffer to download. */
+ size_t data_size, /**< Size of the data buffer. */
+ uint32_t *abort_code /**< Abort code of the SDO download. */
+ );
+
+/** Executes an SDO download request to write data to a slave via complete
+ * access.
+ *
+ * This request is processed by the master state machine. This method blocks,
+ * until the request has been processed and may not be called in realtime
+ * context.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_master_sdo_download_complete(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint16_t index, /**< Index of the SDO. */
+ const uint8_t *data, /**< Data buffer to download. */
+ size_t data_size, /**< Size of the data buffer. */
+ uint32_t *abort_code /**< Abort code of the SDO download. */
+ );
+
+/** Executes an SDO upload request to read data from a slave.
+ *
+ * This request is processed by the master state machine. This method blocks,
+ * until the request has been processed and may not be called in realtime
+ * context.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_master_sdo_upload(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint16_t index, /**< Index of the SDO. */
+ uint8_t subindex, /**< Subindex of the SDO. */
+ uint8_t *target, /**< Target buffer for the upload. */
+ size_t target_size, /**< Size of the target buffer. */
+ size_t *result_size, /**< Uploaded data size. */
+ uint32_t *abort_code /**< Abort code of the SDO upload. */
+ );
+
+/** Executes an SoE write request.
+ *
+ * Starts writing an IDN and blocks until the request was processed, or an
+ * error occurred.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_master_write_idn(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint8_t drive_no, /**< Drive number. */
+ uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
+ const uint8_t *data, /**< Pointer to data to write. */
+ size_t data_size, /**< Size of data to write. */
+ uint16_t *error_code /**< Pointer to variable, where an SoE error code
+ can be stored. */
+ );
+
+/** Executes an SoE read request.
+ *
+ * Starts reading an IDN and blocks until the request was processed, or an
+ * error occurred.
+ *
+ * \apiusage{master_any,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_master_read_idn(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t slave_position, /**< Slave position. */
+ uint8_t drive_no, /**< Drive number. */
+ uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
+ uint8_t *target, /**< Pointer to memory where the read data can be
+ stored. */
+ size_t target_size, /**< Size of the memory \a target points to. */
+ size_t *result_size, /**< Actual size of the received data. */
+ uint16_t *error_code /**< Pointer to variable, where an SoE error code
+ can be stored. */
+ );
+
+/** Finishes the configuration phase and prepares for cyclic operation.
+ *
+ * This function tells the master that the configuration phase is finished and
+ * the realtime operation will begin. The function allocates internal memory
+ * for the domains and calculates the logical FMMU addresses for domain
+ * members. It tells the master state machine that the configuration is
+ * now to be applied to the network.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \attention After this function has been called, the realtime application is
+ * in charge of cyclically calling ecrt_master_send() and
+ * ecrt_master_receive() to ensure network communication. Before calling this
+ * function, the master thread is responsible for that, so these functions may
+ * not be called! The method itself allocates memory and should not be called
+ * in realtime context.
+ *
+ * \return 0 in case of success, else < 0
+ */
+EC_PUBLIC_API int ecrt_master_activate(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Deactivates the master.
+ *
+ * Removes the master configuration. All objects created by
+ * ecrt_master_create_domain(), ecrt_master_slave_config(), ecrt_domain_data()
+ * ecrt_slave_config_create_sdo_request() and
+ * ecrt_slave_config_create_voe_handler() are freed, so pointers to them
+ * become invalid.
+ *
+ * \apiusage{master_op,blocking}
+ *
+ * This method should not be called in realtime context.
+ * \return 0 on success, otherwise negative error code.
+ * \retval 0 Success.
+ * \retval -EINVAL Master has not been activated before.
+ */
+EC_PUBLIC_API int ecrt_master_deactivate(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Set interval between calls to ecrt_master_send().
+ *
+ * This information helps the master to decide, how much data can be appended
+ * to a frame by the master state machine. When the master is configured with
+ * --enable-hrtimers, this is used to calculate the scheduling of the master
+ * thread.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 on success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_master_set_send_interval(
+ ec_master_t *master, /**< EtherCAT master. */
+ size_t send_interval /**< Send interval in us */
+ );
+
+/** Sends all datagrams in the queue.
+ *
+ * This method takes all datagrams, that have been queued for transmission,
+ * puts them into frames, and passes them to the Ethernet device for sending.
+ *
+ * Has to be called cyclically by the application after ecrt_master_activate()
+ * has returned.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_send(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Fetches received frames from the hardware and processes the datagrams.
+ *
+ * Queries the network device for received frames by calling the interrupt
+ * service routine. Extracts received datagrams and dispatches the results to
+ * the datagram objects in the queue. Received datagrams, and the ones that
+ * timed out, will be marked, and dequeued.
+ *
+ * Has to be called cyclically by the realtime application after
+ * ecrt_master_activate() has returned.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_receive(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+#ifdef __KERNEL__
+/** Sends non-application datagrams.
+ *
+ * This method has to be called in the send callback function passed via
+ * ecrt_master_callbacks() to allow the sending of non-application datagrams.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ * \retval -EAGAIN Lock could not be acquired, try again later.
+ */
+int ecrt_master_send_ext(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+#endif
+
+/** Reads the current master state.
+ *
+ * Stores the master state information in the given \a state structure.
+ *
+ * This method returns a global state. For the link-specific states in a
+ * redundant network topology, use the ecrt_master_link_state() method.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_state(
+ const ec_master_t *master, /**< EtherCAT master. */
+ ec_master_state_t *state /**< Structure to store the information. */
+ );
+
+/** Reads the current state of a redundant link.
+ *
+ * Stores the link state information in the given \a state structure.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_link_state(
+ const ec_master_t *master, /**< EtherCAT master. */
+ unsigned int dev_idx, /**< Index of the device (0 = main device, 1 =
+ first backup device, ...). */
+ ec_master_link_state_t *state /**< Structure to store the information.
+ */
+ );
+
+/** Sets the application time.
+ *
+ * The master has to know the application's time when operating slaves with
+ * distributed clocks. The time is not incremented by the master itself, so
+ * this method has to be called cyclically.
+ *
+ * \attention The time passed to this method is used to calculate the phase of
+ * the slaves' SYNC0/1 interrupts. It should be called constantly at the same
+ * point of the realtime cycle. So it is recommended to call it at the start
+ * of the calculations to avoid deviancies due to changing execution times.
+ * Avoid calling this method before the realtime cycle is established.
+ *
+ * The time is used when setting the slaves' <tt>System Time Offset</tt> and
+ * <tt>Cyclic Operation Start Time</tt> registers and when synchronizing the
+ * DC reference clock to the application time via
+ * ecrt_master_sync_reference_clock().
+ *
+ * The time is defined as nanoseconds from 2000-01-01 00:00. Converting an
+ * epoch time can be done with the EC_TIMEVAL2NANO() macro, but is not
+ * necessary, since the absolute value is not of any interest.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_application_time(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint64_t app_time /**< Application time. */
+ );
+
+/** Queues the DC reference clock drift compensation datagram for sending.
+ *
+ * The reference clock will by synchronized to the application time provided
+ * by the last call off ecrt_master_application_time().
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ * \retval 0 Success.
+ * \retval -ENXIO No reference clock found.
+ */
+EC_PUBLIC_API int ecrt_master_sync_reference_clock(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Queues the DC reference clock drift compensation datagram for sending.
+ *
+ * The reference clock will by synchronized to the time passed in the
+ * sync_time parameter.
+ *
+ * Has to be called by the application after ecrt_master_activate()
+ * has returned.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Zero on success, otherwise negative error code.
+ * \retval 0 Success.
+ * \retval -ENXIO No reference clock found.
+ */
+EC_PUBLIC_API int ecrt_master_sync_reference_clock_to(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint64_t sync_time /**< Sync reference clock to this time. */
+ );
+
+/** Queues the DC clock drift compensation datagram for sending.
+ *
+ * All slave clocks synchronized to the reference clock.
+ *
+ * Has to be called by the application after ecrt_master_activate()
+ * has returned.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ * \retval 0 Success.
+ * \retval -ENXIO No reference clock found.
+ */
+EC_PUBLIC_API int ecrt_master_sync_slave_clocks(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Get the lower 32 bit of the reference clock system time.
+ *
+ * This method can be used to synchronize the master to the reference clock.
+ *
+ * The reference clock system time is queried via the
+ * ecrt_master_sync_slave_clocks() method, that reads the system time of the
+ * reference clock and writes it to the slave clocks (so be sure to call it
+ * cyclically to get valid data).
+ *
+ * \attention The returned time is the system time of the reference clock
+ * minus the transmission delay of the reference clock.
+ *
+ * Calling this method makes only sense in realtime context (after master
+ * activation), when the ecrt_master_sync_slave_clocks() method is called
+ * cyclically.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \retval 0 success, system time was written into \a time.
+ * \retval -ENXIO No reference clock found.
+ * \retval -EIO Slave synchronization datagram was not received.
+ */
+EC_PUBLIC_API int ecrt_master_reference_clock_time(
+ const ec_master_t *master, /**< EtherCAT master. */
+ uint32_t *time /**< Pointer to store the queried system time. */
+ );
+
+/** Queues the DC synchrony monitoring datagram for sending.
+ *
+ * The datagram broadcast-reads all "System time difference" registers (\a
+ * 0x092c) to get an upper estimation of the DC synchrony. The result can be
+ * checked with the ecrt_master_sync_monitor_process() method.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_sync_monitor_queue(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Processes the DC synchrony monitoring datagram.
+ *
+ * If the sync monitoring datagram was sent before with
+ * ecrt_master_sync_monitor_queue(), the result can be queried with this
+ * method.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Upper estimation of the maximum time difference in ns, -1 on error.
+ * \retval (uint32_t)-1 Error.
+ */
+EC_PUBLIC_API uint32_t ecrt_master_sync_monitor_process(
+ const ec_master_t *master /**< EtherCAT master. */
+ );
+
+/** Retry configuring slaves.
+ *
+ * Via this method, the application can tell the master to bring all slaves to
+ * OP state. In general, this is not necessary, because it is automatically
+ * done by the master. But with special slaves, that can be reconfigured by
+ * the vendor during runtime, it can be useful.
+ *
+ * Calling this method only makes sense in realtime context (after
+ * activation), because slaves will not be configured before.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_master_reset(
+ ec_master_t *master /**< EtherCAT master. */
+ );
+
+/*****************************************************************************
+ * Slave configuration methods
+ ****************************************************************************/
+
+/** Configure a sync manager.
+ *
+ * Sets the direction of a sync manager. This overrides the direction bits
+ * from the default control register from SII.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return zero on success, else non-zero
+ */
+EC_PUBLIC_API int ecrt_slave_config_sync_manager(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t sync_index, /**< Sync manager index. Must be less
+ than #EC_MAX_SYNC_MANAGERS. */
+ ec_direction_t direction, /**< Input/Output. */
+ ec_watchdog_mode_t watchdog_mode /** Watchdog mode. */
+ );
+
+/** Configure a slave's watchdog times.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_watchdog(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t watchdog_divider, /**< Number of 40 ns intervals (register
+ 0x0400). Used as a base unit for all
+ slave watchdogs^. If set to zero, the
+ value is not written, so the default is
+ used. */
+ uint16_t watchdog_intervals /**< Number of base intervals for sync
+ manager watchdog (register 0x0420). If
+ set to zero, the value is not written,
+ so the default is used. */
+ );
+
+/** Add a PDO to a sync manager's PDO assignment.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \see ecrt_slave_config_pdos()
+ * \return zero on success, else non-zero
+ */
+EC_PUBLIC_API int ecrt_slave_config_pdo_assign_add(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t sync_index, /**< Sync manager index. Must be less
+ than #EC_MAX_SYNC_MANAGERS. */
+ uint16_t index /**< Index of the PDO to assign. */
+ );
+
+/** Clear a sync manager's PDO assignment.
+ *
+ * This can be called before assigning PDOs via
+ * ecrt_slave_config_pdo_assign_add(), to clear the default assignment of a
+ * sync manager.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \see ecrt_slave_config_pdos()
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_pdo_assign_clear(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t sync_index /**< Sync manager index. Must be less
+ than #EC_MAX_SYNC_MANAGERS. */
+ );
+
+/** Add a PDO entry to the given PDO's mapping.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \see ecrt_slave_config_pdos()
+ * \return zero on success, else non-zero
+ */
+EC_PUBLIC_API int ecrt_slave_config_pdo_mapping_add(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t pdo_index, /**< Index of the PDO. */
+ uint16_t entry_index, /**< Index of the PDO entry to add to the PDO's
+ mapping. */
+ uint8_t entry_subindex, /**< Subindex of the PDO entry to add to the
+ PDO's mapping. */
+ uint8_t entry_bit_length /**< Size of the PDO entry in bit. */
+ );
+
+/** Clear the mapping of a given PDO.
+ *
+ * This can be called before mapping PDO entries via
+ * ecrt_slave_config_pdo_mapping_add(), to clear the default mapping.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \see ecrt_slave_config_pdos()
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_pdo_mapping_clear(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t pdo_index /**< Index of the PDO. */
+ );
+
+/** Specify a complete PDO configuration.
+ *
+ * This function is a convenience wrapper for the functions
+ * ecrt_slave_config_sync_manager(), ecrt_slave_config_pdo_assign_clear(),
+ * ecrt_slave_config_pdo_assign_add(), ecrt_slave_config_pdo_mapping_clear()
+ * and ecrt_slave_config_pdo_mapping_add(), that are better suitable for
+ * automatic code generation.
+ *
+ * The following example shows, how to specify a complete configuration,
+ * including the PDO mappings. With this information, the master is able to
+ * reserve the complete process data, even if the slave is not present at
+ * configuration time:
+ *
+ * \code
+ * ec_pdo_entry_info_t el3162_channel1[] = {
+ * {0x3101, 1, 8}, // status
+ * {0x3101, 2, 16} // value
+ * };
+ *
+ * ec_pdo_entry_info_t el3162_channel2[] = {
+ * {0x3102, 1, 8}, // status
+ * {0x3102, 2, 16} // value
+ * };
+ *
+ * ec_pdo_info_t el3162_pdos[] = {
+ * {0x1A00, 2, el3162_channel1},
+ * {0x1A01, 2, el3162_channel2}
+ * };
+ *
+ * ec_sync_info_t el3162_syncs[] = {
+ * {2, EC_DIR_OUTPUT},
+ * {3, EC_DIR_INPUT, 2, el3162_pdos},
+ * {0xff}
+ * };
+ *
+ * if (ecrt_slave_config_pdos(sc_ana_in, EC_END, el3162_syncs)) {
+ * // handle error
+ * }
+ * \endcode
+ *
+ * The next example shows, how to configure the PDO assignment only. The
+ * entries for each assigned PDO are taken from the PDO's default mapping.
+ * Please note, that PDO entry registration will fail, if the PDO
+ * configuration is left empty and the slave is offline.
+ *
+ * \code
+ * ec_pdo_info_t pdos[] = {
+ * {0x1600}, // Channel 1
+ * {0x1601} // Channel 2
+ * };
+ *
+ * ec_sync_info_t syncs[] = {
+ * {3, EC_DIR_INPUT, 2, pdos},
+ * };
+ *
+ * if (ecrt_slave_config_pdos(slave_config_ana_in, 1, syncs)) {
+ * // handle error
+ * }
+ * \endcode
+ *
+ * Processing of \a syncs will stop, if
+ * - the number of processed items reaches \a n_syncs, or
+ * - the \a index member of an ec_sync_info_t item is 0xff. In this case,
+ * \a n_syncs should set to a number greater than the number of list items;
+ * using EC_END is recommended.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return zero on success, else non-zero
+ */
+EC_PUBLIC_API int ecrt_slave_config_pdos(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int n_syncs, /**< Number of sync manager configurations in
+ \a syncs. */
+ const ec_sync_info_t syncs[] /**< Array of sync manager
+ configurations. */
+ );
+
+/** Registers a PDO entry for process data exchange in a domain.
+ *
+ * Searches the assigned PDOs for the given PDO entry. An error is raised, if
+ * the given entry is not mapped. Otherwise, the corresponding sync manager
+ * and FMMU configurations are provided for slave configuration and the
+ * respective sync manager's assigned PDOs are appended to the given domain,
+ * if not already done. The offset of the requested PDO entry's data inside
+ * the domain's process data is returned. Optionally, the PDO entry bit
+ * position (0-7) can be retrieved via the \a bit_position output parameter.
+ * This pointer may be \a NULL, in this case an error is raised if the PDO
+ * entry does not byte-align.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval >=0 Success: Offset of the PDO entry's process data.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_reg_pdo_entry(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t entry_index, /**< Index of the PDO entry to register. */
+ uint8_t entry_subindex, /**< Subindex of the PDO entry to register. */
+ ec_domain_t *domain, /**< Domain. */
+ unsigned int *bit_position /**< Optional address if bit addressing
+ is desired */
+ );
+
+/** Registers a PDO entry using its position.
+ *
+ * Similar to ecrt_slave_config_reg_pdo_entry(), but not using PDO indices but
+ * offsets in the PDO mapping, because PDO entry indices may not be unique
+ * inside a slave's PDO mapping. An error is raised, if
+ * one of the given positions is out of range.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval >=0 Success: Offset of the PDO entry's process data.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_reg_pdo_entry_pos(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t sync_index, /**< Sync manager index. */
+ unsigned int pdo_pos, /**< Position of the PDO inside the SM. */
+ unsigned int entry_pos, /**< Position of the entry inside the PDO. */
+ ec_domain_t *domain, /**< Domain. */
+ unsigned int *bit_position /**< Optional address if bit addressing
+ is desired */
+ );
+
+/** Configure distributed clocks.
+ *
+ * Sets the AssignActivate word and the cycle and shift times for the sync
+ * signals.
+ *
+ * The AssignActivate word is vendor-specific and can be taken from the XML
+ * device description file (Device -> Dc -> AssignActivate). Set this to zero,
+ * if the slave shall be operated without distributed clocks (default).
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \attention The \a sync1_shift time is ignored.
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_dc(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t assign_activate, /**< AssignActivate word. */
+ uint32_t sync0_cycle, /**< SYNC0 cycle time [ns]. */
+ int32_t sync0_shift, /**< SYNC0 shift time [ns]. */
+ uint32_t sync1_cycle, /**< SYNC1 cycle time [ns]. */
+ int32_t sync1_shift /**< SYNC1 shift time [ns]. */
+ );
+
+/** Add an SDO configuration.
+ *
+ * An SDO configuration is stored in the slave configuration object and is
+ * downloaded to the slave whenever the slave is being configured by the
+ * master. This usually happens once on master activation, but can be repeated
+ * subsequently, for example after the slave's power supply failed.
+ *
+ * \attention The SDOs for PDO assignment (\p 0x1C10 - \p 0x1C2F) and PDO
+ * mapping (\p 0x1600 - \p 0x17FF and \p 0x1A00 - \p 0x1BFF) should not be
+ * configured with this function, because they are part of the slave
+ * configuration done by the master. Please use ecrt_slave_config_pdos() and
+ * friends instead.
+ *
+ * This is the generic function for adding an SDO configuration. Please note
+ * that the this function does not do any endianness correction. If
+ * datatype-specific functions are needed (that automatically correct the
+ * endianness), have a look at ecrt_slave_config_sdo8(),
+ * ecrt_slave_config_sdo16() and ecrt_slave_config_sdo32().
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_sdo(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t index, /**< Index of the SDO to configure. */
+ uint8_t subindex, /**< Subindex of the SDO to configure. */
+ const uint8_t *data, /**< Pointer to the data. */
+ size_t size /**< Size of the \a data. */
+ );
+
+/** Add a configuration value for an 8-bit SDO.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \see ecrt_slave_config_sdo().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_sdo8(
+ ec_slave_config_t *sc, /**< Slave configuration */
+ uint16_t sdo_index, /**< Index of the SDO to configure. */
+ uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */
+ uint8_t value /**< Value to set. */
+ );
+
+/** Add a configuration value for a 16-bit SDO.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \see ecrt_slave_config_sdo().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_sdo16(
+ ec_slave_config_t *sc, /**< Slave configuration */
+ uint16_t sdo_index, /**< Index of the SDO to configure. */
+ uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */
+ uint16_t value /**< Value to set. */
+ );
+
+/** Add a configuration value for a 32-bit SDO.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \see ecrt_slave_config_sdo().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_sdo32(
+ ec_slave_config_t *sc, /**< Slave configuration */
+ uint16_t sdo_index, /**< Index of the SDO to configure. */
+ uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */
+ uint32_t value /**< Value to set. */
+ );
+
+/** Add configuration data for a complete SDO.
+ *
+ * The SDO data are transferred via CompleteAccess. Data for the first
+ * subindex (0) have to be included.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \see ecrt_slave_config_sdo().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_complete_sdo(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t index, /**< Index of the SDO to configure. */
+ const uint8_t *data, /**< Pointer to the data. */
+ size_t size /**< Size of the \a data. */
+ );
+
+/** Set the size of the CoE emergency ring buffer.
+ *
+ * The initial size is zero, so all messages will be dropped. This method can
+ * be called even after master activation, but it will clear the ring buffer!
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return 0 on success, or negative error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_emerg_size(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ size_t elements /**< Number of records of the CoE emergency ring. */
+ );
+
+/** Read and remove one record from the CoE emergency ring buffer.
+ *
+ * A record consists of 8 bytes:
+ *
+ * Byte 0-1: Error code (little endian)
+ * Byte 2: Error register
+ * Byte 3-7: Data
+ *
+ * Calling this method makes only sense in realtime context (after master
+ * activation).
+ *
+ * \return 0 on success (record popped), or negative error code (i. e.
+ * -ENOENT, if ring is empty).
+ *
+ * \apiusage{master_op,any_context}
+ */
+EC_PUBLIC_API int ecrt_slave_config_emerg_pop(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t *target /**< Pointer to target memory (at least
+ EC_COE_EMERGENCY_MSG_SIZE bytes). */
+ );
+
+/** Clears CoE emergency ring buffer and the overrun counter.
+ *
+ * Calling this method makes only sense in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,any_context}
+ *
+ * \return 0 on success, or negative error code.
+ *
+ */
+EC_PUBLIC_API int ecrt_slave_config_emerg_clear(
+ ec_slave_config_t *sc /**< Slave configuration. */
+ );
+
+/** Read the number of CoE emergency overruns.
+ *
+ * The overrun counter will be incremented when a CoE emergency message could
+ * not be stored in the ring buffer and had to be dropped. Call
+ * ecrt_slave_config_emerg_clear() to reset the counter.
+ *
+ * Calling this method makes only sense in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,any_context}
+ *
+ * \return Number of overruns since last clear, or negative error code.
+ *
+ */
+EC_PUBLIC_API int ecrt_slave_config_emerg_overruns(
+ const ec_slave_config_t *sc /**< Slave configuration. */
+ );
+
+/** Create an SDO request to exchange SDOs during realtime operation.
+ *
+ * The created SDO request object is freed automatically when the master is
+ * released.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return New SDO request, or NULL on error.
+ */
+EC_PUBLIC_API ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint16_t index, /**< SDO index. */
+ uint8_t subindex, /**< SDO subindex. */
+ size_t size /**< Data size to reserve. */
+ );
+
+/** Create an SoE request to exchange SoE IDNs during realtime operation.
+ *
+ * The created SoE request object is freed automatically when the master is
+ * released.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return New SoE request, or NULL on error.
+ */
+EC_PUBLIC_API ec_soe_request_t *ecrt_slave_config_create_soe_request(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t drive_no, /**< Drive number. */
+ uint16_t idn, /**< Sercos ID-Number. */
+ size_t size /**< Data size to reserve. */
+ );
+
+/** Create an VoE handler to exchange vendor-specific data during realtime
+ * operation.
+ *
+ * The number of VoE handlers per slave configuration is not limited, but
+ * usually it is enough to create one for sending and one for receiving, if
+ * both can be done simultaneously.
+ *
+ * The created VoE handler object is freed automatically when the master is
+ * released.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return New VoE handler, or NULL on error.
+ */
+EC_PUBLIC_API ec_voe_handler_t *ecrt_slave_config_create_voe_handler(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ size_t size /**< Data size to reserve. */
+ );
+
+/** Create a register request to exchange EtherCAT register contents during
+ * realtime operation.
+ *
+ * This interface should not be used to take over master functionality,
+ * instead it is intended for debugging and monitoring reasons.
+ *
+ * The created register request object is freed automatically when the master
+ * is released.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return New register request, or NULL on error.
+ */
+EC_PUBLIC_API ec_reg_request_t *ecrt_slave_config_create_reg_request(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ size_t size /**< Data size to reserve. */
+ );
+
+/** Outputs the state of the slave configuration.
+ *
+ * Stores the state information in the given \a state structure. The state
+ * information is updated by the master state machine, so it may take a few
+ * cycles, until it changes.
+ *
+ * \attention If the state of process data exchange shall be monitored in
+ * realtime, ecrt_domain_state() should be used.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_state(
+ const ec_slave_config_t *sc, /**< Slave configuration */
+ ec_slave_config_state_t *state /**< State object to write to. */
+ );
+
+/** Add an SoE IDN configuration.
+ *
+ * A configuration for a Sercos-over-EtherCAT IDN is stored in the slave
+ * configuration object and is written to the slave whenever the slave is
+ * being configured by the master. This usually happens once on master
+ * activation, but can be repeated subsequently, for example after the slave's
+ * power supply failed.
+ *
+ * The \a idn parameter can be separated into several sections:
+ * - Bit 15: Standard data (0) or Product data (1)
+ * - Bit 14 - 12: Parameter set (0 - 7)
+ * - Bit 11 - 0: Data block number (0 - 4095)
+ *
+ * Please note that the this function does not do any endianness correction.
+ * Multi-byte data have to be passed in EtherCAT endianness (little-endian).
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_idn(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ uint8_t drive_no, /**< Drive number. */
+ uint16_t idn, /**< SoE IDN. */
+ ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or
+ SAFEOP). */
+ const uint8_t *data, /**< Pointer to the data. */
+ size_t size /**< Size of the \a data. */
+ );
+
+/** Adds a feature flag to a slave configuration.
+ *
+ * Feature flags are a generic way to configure slave-specific behavior.
+ *
+ * Multiple calls with the same slave configuration and key will overwrite the
+ * configuration.
+ *
+ * The following flags may be available:
+ * - AssignToPdi: Zero (default) keeps the slave information interface (SII)
+ * assigned to EtherCAT (except during transition to PREOP). Non-zero
+ * assigns the SII to the slave controller side before going to PREOP and
+ * leaves it there until a write command happens.
+ * - WaitBeforeSAFEOPms: Number of milliseconds to wait before commanding the
+ * transition from PREOP to SAFEOP. This can be used as a workaround for
+ * slaves that need a little time to initialize.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_flag(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ const char *key, /**< Key as null-terminated ASCII string. */
+ int32_t value /**< Value to store. */
+ );
+
+/** Sets the link/MAC address for Ethernet-over-EtherCAT (EoE) operation.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * The MAC address is stored in the slave configuration object and will be
+ * written to the slave during the configuration process.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_eoe_mac_address(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ const unsigned char *mac_address /**< MAC address. */
+ );
+
+/** Sets the IP address for Ethernet-over-EtherCAT (EoE) operation.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * The IP address is stored in the slave configuration object and will be
+ * written to the slave during the configuration process.
+ *
+ * The IP address is passed by-value as a `struct in_addr`. This structure
+ * contains the 32-bit IPv4 address in network byte order (big endian).
+ *
+ * A string-represented IPv4 address can be converted to a `struct in_addr`
+ * for example via the POSIX function `inet_pton()` (see man 3 inet_pton):
+ *
+ * \code{.c}
+ * #include <arpa/inet.h>
+ * struct in_addr addr;
+ * if (inet_aton("192.168.0.1", &addr) == 0) {
+ * fprintf(stderr, "Failed to convert IP address.\n");
+ * return -1;
+ * }
+ * if (ecrt_slave_config_eoe_ip_address(sc, addr)) {
+ * fprintf(stderr, "Failed to set IP address.\n");
+ * return -1;
+ * }
+ * \endcode
+ *
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_eoe_ip_address(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ struct in_addr ip_address /**< IPv4 address. */
+ );
+
+/** Sets the subnet mask for Ethernet-over-EtherCAT (EoE) operation.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * The subnet mask is stored in the slave configuration object and will be
+ * written to the slave during the configuration process.
+ *
+ * The subnet mask is passed by-value as a `struct in_addr`. This structure
+ * contains the 32-bit mask in network byte order (big endian).
+ *
+ * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded masks
+ * to `struct in_addr`.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_eoe_subnet_mask(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ struct in_addr subnet_mask /**< IPv4 subnet mask. */
+ );
+
+/** Sets the gateway address for Ethernet-over-EtherCAT (EoE) operation.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * The gateway address is stored in the slave configuration object and will be
+ * written to the slave during the configuration process.
+ *
+ * The address is passed by-value as a `struct in_addr`. This structure
+ * contains the 32-bit IPv4 address in network byte order (big endian).
+ *
+ * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded IPv4
+ * addresses to `struct in_addr`.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_eoe_default_gateway(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ struct in_addr gateway_address /**< Gateway's IPv4 address. */
+ );
+
+/** Sets the IPv4 address of the DNS server for Ethernet-over-EtherCAT (EoE)
+ * operation.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * The DNS server address is stored in the slave configuration object and will
+ * be written to the slave during the configuration process.
+ *
+ * The address is passed by-value as a `struct in_addr`. This structure
+ * contains the 32-bit IPv4 address in network byte order (big endian).
+ *
+ * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded IPv4
+ * addresses to `struct in_addr`.
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_eoe_dns_address(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ struct in_addr dns_address /**< IPv4 address of the DNS server. */
+ );
+
+/** Sets the host name for Ethernet-over-EtherCAT (EoE) operation.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * The host name is stored in the slave configuration object and will
+ * be written to the slave during the configuration process.
+ *
+ * The maximum size of the host name is 32 bytes (including the zero
+ * terminator).
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_eoe_hostname(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ const char *name /**< Zero-terminated host name. */
+ );
+
+/** Sets the application-layer state transition timeout in ms.
+ *
+ * Change the maximum allowed time for a slave to make an application-layer
+ * state transition for the given state transition (for example from PREOP to
+ * SAFEOP). The default values are defined in ETG.2000.
+ *
+ * A timeout value of zero ms will restore the default value.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+EC_PUBLIC_API int ecrt_slave_config_state_timeout(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ ec_al_state_t from_state, /**< Initial state. */
+ ec_al_state_t to_state, /**< Target state. */
+ unsigned int timeout_ms /**< Timeout in [ms]. */
+ );
+
+/*****************************************************************************
+ * Domain methods
+ ****************************************************************************/
+
+/** Registers a bunch of PDO entries for a domain.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \see ecrt_slave_config_reg_pdo_entry()
+ *
+ * \attention The registration array has to be terminated with an empty
+ * structure, or one with the \a index field set to zero!
+ *
+ * \apiusage{master_idle,blocking}
+ *
+ * \return 0 on success, else non-zero.
+ */
+EC_PUBLIC_API int ecrt_domain_reg_pdo_entry_list(
+ ec_domain_t *domain, /**< Domain. */
+ const ec_pdo_entry_reg_t *pdo_entry_regs /**< Array of PDO
+ registrations. */
+ );
+
+/** Returns the current size of the domain's process data.
+ *
+ * The domain size is calculated after master activation.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Size of the process data image, or a negative error code.
+ */
+EC_PUBLIC_API size_t ecrt_domain_size(
+ const ec_domain_t *domain /**< Domain. */
+ );
+
+#ifdef __KERNEL__
+
+/** Provide external memory to store the domain's process data.
+ *
+ * Call this after all PDO entries have been registered and before activating
+ * the master.
+ *
+ * The size of the allocated memory must be at least ecrt_domain_size(), after
+ * all PDO entries have been registered.
+ *
+ * This method has to be called in non-realtime context before
+ * ecrt_master_activate().
+ *
+ * \apiusage{master_idle,blocking}
+ */
+void ecrt_domain_external_memory(
+ ec_domain_t *domain, /**< Domain. */
+ uint8_t *memory /**< Address of the memory to store the process
+ data in. */
+ );
+
+#endif /* __KERNEL__ */
+
+/** Returns the domain's process data.
+ *
+ * - In kernel context: If external memory was provided with
+ * ecrt_domain_external_memory(), the returned pointer will contain the
+ * address of that memory. Otherwise it will point to the internally allocated
+ * memory. In the latter case, this method may not be called before
+ * ecrt_master_activate().
+ *
+ * - In userspace context: This method has to be called after
+ * ecrt_master_activate() to get the mapped domain process data memory.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Pointer to the process data memory.
+ */
+EC_PUBLIC_API uint8_t *ecrt_domain_data(
+ const ec_domain_t *domain /**< Domain. */
+ );
+
+/** Determines the states of the domain's datagrams.
+ *
+ * Evaluates the working counters of the received datagrams and outputs
+ * statistics, if necessary. This must be called after ecrt_master_receive()
+ * is expected to receive the domain datagrams in order to make
+ * ecrt_domain_state() return the result of the last process data exchange.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_domain_process(
+ ec_domain_t *domain /**< Domain. */
+ );
+
+/** (Re-)queues all domain datagrams in the master's datagram queue.
+ *
+ * Call this function to mark the domain's datagrams for exchanging at the
+ * next call of ecrt_master_send().
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_domain_queue(
+ ec_domain_t *domain /**< Domain. */
+ );
+
+/** Reads the state of a domain.
+ *
+ * Stores the domain state in the given \a state structure.
+ *
+ * Using this method, the process data exchange can be monitored in realtime.
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_domain_state(
+ const ec_domain_t *domain, /**< Domain. */
+ ec_domain_state_t *state /**< Pointer to a state object to store the
+ information. */
+ );
+
+/*****************************************************************************
+ * SDO request methods.
+ ****************************************************************************/
+
+/** Set the SDO index and subindex.
+ *
+ * \attention If the SDO index and/or subindex is changed while
+ * ecrt_sdo_request_state() returns EC_REQUEST_BUSY, this may lead to
+ * unexpected results.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation). To initialize the SDO request, the index and subindex can be
+ * set via ecrt_slave_config_create_sdo_request().
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_sdo_request_index(
+ ec_sdo_request_t *req, /**< SDO request. */
+ uint16_t index, /**< SDO index. */
+ uint8_t subindex /**< SDO subindex. */
+ );
+
+/** Set the timeout for an SDO request.
+ *
+ * If the request cannot be processed in the specified time, if will be marked
+ * as failed.
+ *
+ * The timeout is permanently stored in the request object and is valid until
+ * the next call of this method.
+ *
+ * The timeout should be defined in non-realtime context, but can also be
+ * changed afterwards.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_sdo_request_timeout(
+ ec_sdo_request_t *req, /**< SDO request. */
+ uint32_t timeout /**< Timeout in milliseconds. Zero means no
+ timeout. */
+ );
+
+/** Access to the SDO request's data.
+ *
+ * This function returns a pointer to the request's internal SDO data memory.
+ *
+ * - After a read operation was successful, integer data can be evaluated
+ * using the EC_READ_*() macros as usual. Example:
+ * \code
+ * uint16_t value = EC_READ_U16(ecrt_sdo_request_data(sdo)));
+ * \endcode
+ * - If a write operation shall be triggered, the data have to be written to
+ * the internal memory. Use the EC_WRITE_*() macros, if you are writing
+ * integer data. Be sure, that the data fit into the memory. The memory size
+ * is a parameter of ecrt_slave_config_create_sdo_request().
+ * \code
+ * EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF);
+ * \endcode
+ *
+ * \attention The return value can be invalid during a read operation, because
+ * the internal SDO data memory could be re-allocated if the read SDO data do
+ * not fit inside.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation), but can also be used to initialize data before.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Pointer to the internal SDO data memory.
+ *
+ */
+EC_PUBLIC_API uint8_t *ecrt_sdo_request_data(
+ const ec_sdo_request_t *req /**< SDO request. */
+ );
+
+/** Returns the current SDO data size.
+ *
+ * When the SDO request is created, the data size is set to the size of the
+ * reserved memory. After a read operation the size is set to the size of the
+ * read data. The size is not modified in any other situation.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return SDO data size in bytes.
+ *
+ */
+EC_PUBLIC_API size_t ecrt_sdo_request_data_size(
+ const ec_sdo_request_t *req /**< SDO request. */
+ );
+
+/** Get the current state of the SDO request.
+ *
+ * The user-space implementation fetches incoming data and stores the received
+ * data size in the request object, so the request is not const.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Request state.
+ *
+ */
+EC_PUBLIC_API ec_request_state_t ecrt_sdo_request_state(
+#ifdef __KERNEL__
+ const
+#endif
+ ec_sdo_request_t *req /**< SDO request. */
+ );
+
+/** Schedule an SDO write operation.
+ *
+ * \attention This method may not be called while ecrt_sdo_request_state()
+ * returns EC_REQUEST_BUSY.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ * \retval -EINVAL Invalid input data, e.g. data size == 0.
+ * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_sdo_request()
+ * too small.
+ */
+EC_PUBLIC_API int ecrt_sdo_request_write(
+ ec_sdo_request_t *req /**< SDO request. */
+ );
+
+/** Schedule an SDO read operation.
+ *
+ * \attention This method may not be called while ecrt_sdo_request_state()
+ * returns EC_REQUEST_BUSY.
+ *
+ * \attention After calling this function, the return value of
+ * ecrt_sdo_request_data() must be considered as invalid while
+ * ecrt_sdo_request_state() returns EC_REQUEST_BUSY.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_sdo_request_read(
+ ec_sdo_request_t *req /**< SDO request. */
+ );
+
+/*****************************************************************************
+ * SoE request methods.
+ ****************************************************************************/
+
+/** Set the request's drive and Sercos ID numbers.
+ *
+ * \attention If the drive number and/or IDN is changed while
+ * ecrt_soe_request_state() returns EC_REQUEST_BUSY, this may lead to
+ * unexpected results.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation). To initialize the SoE request, the drive_no and IDN can be
+ * set via ecrt_slave_config_create_soe_request().
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_soe_request_idn(
+ ec_soe_request_t *req, /**< IDN request. */
+ uint8_t drive_no, /**< SDO index. */
+ uint16_t idn /**< SoE IDN. */
+ );
+
+/** Set the timeout for an SoE request.
+ *
+ * If the request cannot be processed in the specified time, if will be marked
+ * as failed.
+ *
+ * The timeout is permanently stored in the request object and is valid until
+ * the next call of this method.
+ *
+ * The timeout should be defined in non-realtime context, but can also be
+ * changed afterwards.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_soe_request_timeout(
+ ec_soe_request_t *req, /**< SoE request. */
+ uint32_t timeout /**< Timeout in milliseconds. Zero means no
+ timeout. */
+ );
+
+/** Access to the SoE request's data.
+ *
+ * This function returns a pointer to the request's internal IDN data memory.
+ *
+ * - After a read operation was successful, integer data can be evaluated
+ * using the EC_READ_*() macros as usual. Example:
+ * \code
+ * uint16_t value = EC_READ_U16(ecrt_soe_request_data(idn_req)));
+ * \endcode
+ * - If a write operation shall be triggered, the data have to be written to
+ * the internal memory. Use the EC_WRITE_*() macros, if you are writing
+ * integer data. Be sure, that the data fit into the memory. The memory size
+ * is a parameter of ecrt_slave_config_create_soe_request().
+ * \code
+ * EC_WRITE_U16(ecrt_soe_request_data(idn_req), 0xFFFF);
+ * \endcode
+ *
+ * \attention The return value can be invalidated during a read operation,
+ * because the internal IDN data memory could be re-allocated if the read IDN
+ * data do not fit inside.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation), but can also be used to initialize data before.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Pointer to the internal IDN data memory.
+ *
+ */
+EC_PUBLIC_API uint8_t *ecrt_soe_request_data(
+ const ec_soe_request_t *req /**< SoE request. */
+ );
+
+/** Returns the current IDN data size.
+ *
+ * When the SoE request is created, the data size is set to the size of the
+ * reserved memory. After a read operation the size is set to the size of the
+ * read data. The size is not modified in any other situation.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return IDN data size in bytes.
+ */
+EC_PUBLIC_API size_t ecrt_soe_request_data_size(
+ const ec_soe_request_t *req /**< SoE request. */
+ );
+
+/** Get the current state of the SoE request.
+ *
+ * \return Request state.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * In the user-space implementation, the method fetches the size of the
+ * incoming data, so the request object is not const.
+ *
+ * \apiusage{master_op,rt_safe}
+ */
+EC_PUBLIC_API ec_request_state_t ecrt_soe_request_state(
+#ifdef __KERNEL__
+ const
+#endif
+ ec_soe_request_t *req /**< SoE request. */
+ );
+
+/** Schedule an SoE IDN write operation.
+ *
+ * \attention This method may not be called while ecrt_soe_request_state()
+ * returns EC_REQUEST_BUSY.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ * \retval -EINVAL Invalid input data, e.g. data size == 0.
+ * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_soe_request()
+ * too small.
+ */
+EC_PUBLIC_API int ecrt_soe_request_write(
+ ec_soe_request_t *req /**< SoE request. */
+ );
+
+/** Schedule an SoE IDN read operation.
+ *
+ * \attention This method may not be called while ecrt_soe_request_state()
+ * returns EC_REQUEST_BUSY.
+ *
+ * \attention After calling this function, the return value of
+ * ecrt_soe_request_data() must be considered as invalid while
+ * ecrt_soe_request_state() returns EC_REQUEST_BUSY.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_soe_request_read(
+ ec_soe_request_t *req /**< SoE request. */
+ );
+
+/*****************************************************************************
+ * VoE handler methods.
+ ****************************************************************************/
+
+/** Sets the VoE header for future send operations.
+ *
+ * A VoE message shall contain a 4-byte vendor ID, followed by a 2-byte vendor
+ * type at as header. These numbers can be set with this function. The values
+ * are valid and will be used for future send operations until the next call
+ * of this method.
+ *
+ * This method is meant to be called in non-realtime context (before master
+ * activation) to initialize the header data, but it is also safe to
+ * change the header later on in realtime context.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_voe_handler_send_header(
+ ec_voe_handler_t *voe, /**< VoE handler. */
+ uint32_t vendor_id, /**< Vendor ID. */
+ uint16_t vendor_type /**< Vendor-specific type. */
+ );
+
+/** Reads the header data of a received VoE message.
+ *
+ * This method can be used to get the received VoE header information after a
+ * read operation has succeeded.
+ *
+ * The header information is stored at the memory given by the pointer
+ * parameters.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_voe_handler_received_header(
+ const ec_voe_handler_t *voe, /**< VoE handler. */
+ uint32_t *vendor_id, /**< Vendor ID. */
+ uint16_t *vendor_type /**< Vendor-specific type. */
+ );
+
+/** Access to the VoE handler's data.
+ *
+ * This function returns a pointer to the VoE handler's internal memory, that
+ * points to the actual VoE data right after the VoE header (see
+ * ecrt_voe_handler_send_header()).
+ *
+ * - After a read operation was successful, the memory contains the received
+ * data. The size of the received data can be determined via
+ * ecrt_voe_handler_data_size().
+ * - Before a write operation is triggered, the data have to be written to the
+ * internal memory. Be sure, that the data fit into the memory. The reserved
+ * memory size is a parameter of ecrt_slave_config_create_voe_handler().
+ *
+ * \attention The returned pointer is not necessarily persistent: After a read
+ * operation, the internal memory may have been reallocated. This can be
+ * avoided by reserving enough memory via the \a size parameter of
+ * ecrt_slave_config_create_voe_handler().
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Pointer to the internal memory.
+ */
+EC_PUBLIC_API uint8_t *ecrt_voe_handler_data(
+ const ec_voe_handler_t *voe /**< VoE handler. */
+ );
+
+/** Returns the current data size.
+ *
+ * The data size is the size of the VoE data without the header (see
+ * ecrt_voe_handler_send_header()).
+ *
+ * When the VoE handler is created, the data size is set to the size of the
+ * reserved memory. At a write operation, the data size is set to the number
+ * of bytes to write. After a read operation the size is set to the size of
+ * the read data. The size is not modified in any other situation.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Data size in bytes.
+ */
+EC_PUBLIC_API size_t ecrt_voe_handler_data_size(
+ const ec_voe_handler_t *voe /**< VoE handler. */
+ );
+
+/** Start a VoE write operation.
+ *
+ * After this function has been called, the ecrt_voe_handler_execute() method
+ * must be called in every realtime cycle as long as it returns
+ * EC_REQUEST_BUSY. No other operation may be started while the handler is
+ * busy.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_voe_handler
+ * too small.
+ */
+EC_PUBLIC_API int ecrt_voe_handler_write(
+ ec_voe_handler_t *voe, /**< VoE handler. */
+ size_t size /**< Number of bytes to write (without the VoE header). */
+ );
+
+/** Start a VoE read operation.
+ *
+ * After this function has been called, the ecrt_voe_handler_execute() method
+ * must be called in every realtime cycle as long as it returns
+ * EC_REQUEST_BUSY. No other operation may be started while the handler is
+ * busy.
+ *
+ * The state machine queries the slave's send mailbox for new data to be send
+ * to the master. If no data appear within the EC_VOE_RESPONSE_TIMEOUT
+ * (defined in master/voe_handler.c), the operation fails.
+ *
+ * On success, the size of the read data can be determined via
+ * ecrt_voe_handler_data_size(), while the VoE header of the received data
+ * can be retrieved with ecrt_voe_handler_received_header().
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_voe_handler_read(
+ ec_voe_handler_t *voe /**< VoE handler. */
+ );
+
+/** Start a VoE read operation without querying the sync manager status.
+ *
+ * After this function has been called, the ecrt_voe_handler_execute() method
+ * must be called in every realtime cycle as long as it returns
+ * EC_REQUEST_BUSY. No other operation may be started while the handler is
+ * busy.
+ *
+ * The state machine queries the slave by sending an empty mailbox. The slave
+ * fills its data to the master in this mailbox. If no data appear within the
+ * EC_VOE_RESPONSE_TIMEOUT (defined in master/voe_handler.c), the operation
+ * fails.
+ *
+ * On success, the size of the read data can be determined via
+ * ecrt_voe_handler_data_size(), while the VoE header of the received data
+ * can be retrieved with ecrt_voe_handler_received_header().
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ */
+EC_PUBLIC_API int ecrt_voe_handler_read_nosync(
+ ec_voe_handler_t *voe /**< VoE handler. */
+ );
+
+/** Execute the handler.
+ *
+ * This method executes the VoE handler. It has to be called in every realtime
+ * cycle as long as it returns EC_REQUEST_BUSY.
+ *
+ * \return Handler state.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ */
+EC_PUBLIC_API ec_request_state_t ecrt_voe_handler_execute(
+ ec_voe_handler_t *voe /**< VoE handler. */
+ );
+
+/*****************************************************************************
+ * Register request methods.
+ ****************************************************************************/
+
+/** Access to the register request's data.
+ *
+ * This function returns a pointer to the request's internal memory.
+ *
+ * - After a read operation was successful, integer data can be evaluated
+ * using the EC_READ_*() macros as usual. Example:
+ * \code
+ * uint16_t value = EC_READ_U16(ecrt_reg_request_data(reg_request)));
+ * \endcode
+ * - If a write operation shall be triggered, the data have to be written to
+ * the internal memory. Use the EC_WRITE_*() macros, if you are writing
+ * integer data. Be sure, that the data fit into the memory. The memory size
+ * is a parameter of ecrt_slave_config_create_reg_request().
+ * \code
+ * EC_WRITE_U16(ecrt_reg_request_data(reg_request), 0xFFFF);
+ * \endcode
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation), but can also be used to initialize data before.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \return Pointer to the internal memory.
+ *
+ */
+EC_PUBLIC_API uint8_t *ecrt_reg_request_data(
+ const ec_reg_request_t *req /**< Register request. */
+ );
+
+/** Get the current state of the register request.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return Request state.
+ *
+ */
+EC_PUBLIC_API ec_request_state_t ecrt_reg_request_state(
+ const ec_reg_request_t *req /**< Register request. */
+ );
+
+/** Schedule an register write operation.
+ *
+ * \attention This method may not be called while ecrt_reg_request_state()
+ * returns EC_REQUEST_BUSY.
+ *
+ * \attention The \a size parameter is truncated to the size given at request
+ * creation.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_reg_request
+ * too small.
+ */
+EC_PUBLIC_API int ecrt_reg_request_write(
+ ec_reg_request_t *req, /**< Register request. */
+ uint16_t address, /**< Register address. */
+ size_t size /**< Size to write. */
+ );
+
+/** Schedule a register read operation.
+ *
+ * \attention This method may not be called while ecrt_reg_request_state()
+ * returns EC_REQUEST_BUSY.
+ *
+ * \attention The \a size parameter is truncated to the size given at request
+ * creation.
+ *
+ * This method is meant to be called in realtime context (after master
+ * activation).
+ *
+ * \apiusage{master_op,rt_safe}
+ *
+ * \return 0 on success, otherwise negative error code.
+ * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_reg_request
+ * too small.
+ */
+EC_PUBLIC_API int ecrt_reg_request_read(
+ ec_reg_request_t *req, /**< Register request. */
+ uint16_t address, /**< Register address. */
+ size_t size /**< Size to write. */
+ );
+
+/*****************************************************************************
+ * Bitwise read/write macros
+ ****************************************************************************/
+
+/** Read a certain bit of an EtherCAT data byte.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param POS bit position
+ */
+#define EC_READ_BIT(DATA, POS) ((*((uint8_t *) (DATA)) >> (POS)) & 0x01)
+
+/** Write a certain bit of an EtherCAT data byte.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param POS bit position
+ * \param VAL new bit value
+ */
+#define EC_WRITE_BIT(DATA, POS, VAL) \
+ do { \
+ if (VAL) *((uint8_t *) (DATA)) |= (1 << (POS)); \
+ else *((uint8_t *) (DATA)) &= ~(1 << (POS)); \
+ } while (0)
+
+/*****************************************************************************
+ * Byte-swapping functions for user space
+ ****************************************************************************/
+
+#ifndef __KERNEL__
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+
+#define le16_to_cpu(x) x
+#define le32_to_cpu(x) x
+#define le64_to_cpu(x) x
+
+#define cpu_to_le16(x) x
+#define cpu_to_le32(x) x
+#define cpu_to_le64(x) x
+
+#elif __BYTE_ORDER == __BIG_ENDIAN
+
+#define swap16(x) \
+ ((uint16_t)( \
+ (((uint16_t)(x) & 0x00ffU) << 8) | \
+ (((uint16_t)(x) & 0xff00U) >> 8) ))
+#define swap32(x) \
+ ((uint32_t)( \
+ (((uint32_t)(x) & 0x000000ffUL) << 24) | \
+ (((uint32_t)(x) & 0x0000ff00UL) << 8) | \
+ (((uint32_t)(x) & 0x00ff0000UL) >> 8) | \
+ (((uint32_t)(x) & 0xff000000UL) >> 24) ))
+#define swap64(x) \
+ ((uint64_t)( \
+ (((uint64_t)(x) & 0x00000000000000ffULL) << 56) | \
+ (((uint64_t)(x) & 0x000000000000ff00ULL) << 40) | \
+ (((uint64_t)(x) & 0x0000000000ff0000ULL) << 24) | \
+ (((uint64_t)(x) & 0x00000000ff000000ULL) << 8) | \
+ (((uint64_t)(x) & 0x000000ff00000000ULL) >> 8) | \
+ (((uint64_t)(x) & 0x0000ff0000000000ULL) >> 24) | \
+ (((uint64_t)(x) & 0x00ff000000000000ULL) >> 40) | \
+ (((uint64_t)(x) & 0xff00000000000000ULL) >> 56) ))
+
+#define le16_to_cpu(x) swap16(x)
+#define le32_to_cpu(x) swap32(x)
+#define le64_to_cpu(x) swap64(x)
+
+#define cpu_to_le16(x) swap16(x)
+#define cpu_to_le32(x) swap32(x)
+#define cpu_to_le64(x) swap64(x)
+
+#endif
+
+#define le16_to_cpup(x) le16_to_cpu(*((uint16_t *)(x)))
+#define le32_to_cpup(x) le32_to_cpu(*((uint32_t *)(x)))
+#define le64_to_cpup(x) le64_to_cpu(*((uint64_t *)(x)))
+
+#endif /* ifndef __KERNEL__ */
+
+/*****************************************************************************
+ * Read macros
+ ****************************************************************************/
+
+/** Read an 8-bit unsigned value from EtherCAT data.
+ *
+ * \return EtherCAT data value
+ */
+#define EC_READ_U8(DATA) \
+ ((uint8_t) *((uint8_t *) (DATA)))
+
+/** Read an 8-bit signed value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_S8(DATA) \
+ ((int8_t) *((uint8_t *) (DATA)))
+
+/** Read a 16-bit unsigned value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_U16(DATA) \
+ ((uint16_t) le16_to_cpup((void *) (DATA)))
+
+/** Read a 16-bit signed value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_S16(DATA) \
+ ((int16_t) le16_to_cpup((void *) (DATA)))
+
+/** Read a 32-bit unsigned value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_U32(DATA) \
+ ((uint32_t) le32_to_cpup((void *) (DATA)))
+
+/** Read a 32-bit signed value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_S32(DATA) \
+ ((int32_t) le32_to_cpup((void *) (DATA)))
+
+/** Read a 64-bit unsigned value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_U64(DATA) \
+ ((uint64_t) le64_to_cpup((void *) (DATA)))
+
+/** Read a 64-bit signed value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_S64(DATA) \
+ ((int64_t) le64_to_cpup((void *) (DATA)))
+
+/*****************************************************************************
+ * Floating-point read functions and macros (userspace only)
+ ****************************************************************************/
+
+#ifndef __KERNEL__
+
+/** Read a 32-bit floating-point value from EtherCAT data.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \param data EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+EC_PUBLIC_API float ecrt_read_real(const void *data);
+
+/** Read a 32-bit floating-point value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_REAL(DATA) ecrt_read_real(DATA)
+
+/** Read a 64-bit floating-point value from EtherCAT data.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \param data EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+EC_PUBLIC_API double ecrt_read_lreal(const void *data);
+
+/** Read a 64-bit floating-point value from EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \return EtherCAT data value
+ */
+#define EC_READ_LREAL(DATA) ecrt_read_lreal(DATA)
+
+#endif // ifndef __KERNEL__
+
+/*****************************************************************************
+ * Write macros
+ ****************************************************************************/
+
+/** Write an 8-bit unsigned value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_U8(DATA, VAL) \
+ do { \
+ *((uint8_t *)(DATA)) = ((uint8_t) (VAL)); \
+ } while (0)
+
+/** Write an 8-bit signed value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_S8(DATA, VAL) EC_WRITE_U8(DATA, VAL)
+
+/** Write a 16-bit unsigned value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_U16(DATA, VAL) \
+ do { \
+ *((uint16_t *) (DATA)) = cpu_to_le16((uint16_t) (VAL)); \
+ } while (0)
+
+/** Write a 16-bit signed value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_S16(DATA, VAL) EC_WRITE_U16(DATA, VAL)
+
+/** Write a 32-bit unsigned value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_U32(DATA, VAL) \
+ do { \
+ *((uint32_t *) (DATA)) = cpu_to_le32((uint32_t) (VAL)); \
+ } while (0)
+
+/** Write a 32-bit signed value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_S32(DATA, VAL) EC_WRITE_U32(DATA, VAL)
+
+/** Write a 64-bit unsigned value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_U64(DATA, VAL) \
+ do { \
+ *((uint64_t *) (DATA)) = cpu_to_le64((uint64_t) (VAL)); \
+ } while (0)
+
+/** Write a 64-bit signed value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_S64(DATA, VAL) EC_WRITE_U64(DATA, VAL)
+
+/*****************************************************************************
+ * Floating-point write functions and macros (userspace only)
+ ****************************************************************************/
+
+#ifndef __KERNEL__
+
+/** Write a 32-bit floating-point value to EtherCAT data.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \param data EtherCAT data pointer
+ * \param value new value
+ */
+EC_PUBLIC_API void ecrt_write_real(void *data, float value);
+
+/** Write a 32-bit floating-point value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_REAL(DATA, VAL) ecrt_write_real(DATA, VAL)
+
+/** Write a 64-bit floating-point value to EtherCAT data.
+ *
+ * \apiusage{master_any,rt_safe}
+ *
+ * \param data EtherCAT data pointer
+ * \param value new value
+ */
+EC_PUBLIC_API void ecrt_write_lreal(void *data, double value);
+
+/** Write a 64-bit floating-point value to EtherCAT data.
+ *
+ * \param DATA EtherCAT data pointer
+ * \param VAL new value
+ */
+#define EC_WRITE_LREAL(DATA, VAL) ecrt_write_lreal(DATA, VAL)
+
+#endif // ifndef __KERNEL__
+
+/****************************************************************************/
+
+#ifdef __cplusplus
+}
+#endif
+
+/****************************************************************************/
+
+/** @} */
+
+#endif
diff --git a/ubuntu/igh-ecat/include/ectty.h b/ubuntu/igh-ecat/include/ectty.h
new file mode 100644
index 00000000000000..3178d3ce9ba9f4
--- /dev/null
+++ b/ubuntu/igh-ecat/include/ectty.h
@@ -0,0 +1,106 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT master userspace library.
+ *
+ * The IgH EtherCAT master userspace library is free software; you can
+ * redistribute it and/or modify it under the terms of the GNU Lesser General
+ * Public License as published by the Free Software Foundation; version 2.1
+ * of the License.
+ *
+ * The IgH EtherCAT master userspace library is distributed in the hope that
+ * it will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with the IgH EtherCAT master userspace library. If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/** \file
+ *
+ * EtherCAT virtual TTY interface.
+ *
+ * \defgroup TTYInterface EtherCAT Virtual TTY Interface
+ *
+ * @{
+ */
+
+/****************************************************************************/
+
+#ifndef __ECTTY_H__
+#define __ECTTY_H__
+
+#include <linux/termios.h>
+
+/*****************************************************************************
+ * Data types
+ ****************************************************************************/
+
+struct ec_tty;
+typedef struct ec_tty ec_tty_t; /**< \see ec_tty */
+
+/** Operations on the virtual TTY interface.
+ */
+typedef struct {
+ int (*cflag_changed)(void *, tcflag_t); /**< Called when the serial
+ * settings shall be changed. The
+ * \a cflag argument contains the
+ * new settings. */
+} ec_tty_operations_t;
+
+/*****************************************************************************
+ * Global functions
+ ****************************************************************************/
+
+/** Create a virtual TTY interface.
+ *
+ * \param ops Set of callbacks.
+ * \param cb_data Arbitrary data, that is passed to any callback.
+ *
+ * \return Pointer to the interface object, otherwise an ERR_PTR value.
+ */
+ec_tty_t *ectty_create(
+ const ec_tty_operations_t *ops,
+ void *cb_data
+ );
+
+/*****************************************************************************
+ * TTY interface methods
+ ****************************************************************************/
+
+/** Releases a virtual TTY interface.
+ */
+void ectty_free(
+ ec_tty_t *tty /**< TTY interface. */
+ );
+
+/** Reads data to send from the TTY interface.
+ *
+ * If there are data to send, they are copied into the \a buffer. At maximum,
+ * \a size bytes are copied. The actual number of bytes copied is returned.
+ *
+ * \return Number of bytes copied.
+ */
+unsigned int ectty_tx_data(
+ ec_tty_t *tty, /**< TTY interface. */
+ uint8_t *buffer, /**< Buffer for data to transmit. */
+ size_t size /**< Available space in \a buffer. */
+ );
+
+/** Pushes received data to the TTY interface.
+ */
+void ectty_rx_data(
+ ec_tty_t *tty, /**< TTY interface. */
+ const uint8_t *buffer, /**< Buffer with received data. */
+ size_t size /**< Number of bytes in \a buffer. */
+ );
+
+/****************************************************************************/
+
+/** @} */
+
+#endif
diff --git a/ubuntu/igh-ecat/master/Kbuild.in b/ubuntu/igh-ecat/master/Kbuild.in
new file mode 100644
index 00000000000000..afba220236e8f3
--- /dev/null
+++ b/ubuntu/igh-ecat/master/Kbuild.in
@@ -0,0 +1,112 @@
+#-----------------------------------------------------------------------------
+#
+# Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+#
+# This file is part of the IgH EtherCAT Master.
+#
+# The IgH EtherCAT Master is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License version 2, as
+# published by the Free Software Foundation.
+#
+# The IgH EtherCAT Master is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+# Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with the IgH EtherCAT Master; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+#
+# ---
+#
+# vi: syntax=make
+#
+#-----------------------------------------------------------------------------
+
+src := @abs_srcdir@
+ccflags-y := -I at abs_top_builddir@
+
+obj-m := ec_master.o
+
+ec_master-objs := \
+ cdev.o \
+ coe_emerg_ring.o \
+ datagram.o \
+ datagram_pair.o \
+ device.o \
+ domain.o \
+ flag.o \
+ fmmu_config.o \
+ foe_request.o \
+ fsm_change.o \
+ fsm_coe.o \
+ fsm_foe.o \
+ fsm_master.o \
+ fsm_pdo.o \
+ fsm_pdo_entry.o \
+ fsm_sii.o \
+ fsm_slave.o \
+ fsm_slave_config.o \
+ fsm_slave_scan.o \
+ fsm_soe.o \
+ ioctl.o \
+ mailbox.o \
+ master.o \
+ module.o \
+ pdo.o \
+ pdo_entry.o \
+ pdo_list.o \
+ reg_request.o \
+ sdo.o \
+ sdo_entry.o \
+ sdo_request.o \
+ slave.o \
+ slave_config.o \
+ soe_errors.o \
+ soe_request.o \
+ sync.o \
+ sync_config.o \
+ voe_handler.o
+
+ifeq (@ENABLE_EOE@,1)
+ec_master-objs += eoe_request.o ethernet.o fsm_eoe.o
+endif
+
+ifeq (@ENABLE_DEBUG_IF@,1)
+ec_master-objs += debug.o
+endif
+
+ifeq (@ENABLE_RTDM@,1)
+
+ifeq (@ENABLE_XENOMAI_V3@, 1)
+ec_master-objs += rtdm_xenomai_v3.o
+else
+ec_master-objs += rtdm.o
+endif
+
+ifeq (@ENABLE_XENOMAI@, 1)
+CFLAGS_rtdm.o := @XENOMAI_RTDM_CFLAGS@
+CFLAGS_rtdm-ioctl.o := @XENOMAI_RTDM_CFLAGS@
+endif
+
+ifeq (@ENABLE_RTAI@, 1)
+CFLAGS_rtdm.o := @RTAI_KERNEL_CFLAGS@
+CFLAGS_rtdm-ioctl.o := @RTAI_KERNEL_CFLAGS@
+endif
+
+ec_master-objs += rtdm-ioctl.o
+CFLAGS_rtdm-ioctl.o += -DEC_IOCTL_RTDM
+
+endif # ENABLE_RTDM
+
+REV := $(shell if test -s $(src)/../revision; then \
+ cat $(src)/../revision; \
+ else \
+ git -C $(src)/.. describe 2>/dev/null || echo "unknown"; \
+ fi)
+
+CFLAGS_module.o := -DREV=$(REV)
+
+KBUILD_CFLAGS += -Wmaybe-uninitialized
+
+#-----------------------------------------------------------------------------
diff --git a/ubuntu/igh-ecat/master/Makefile.am b/ubuntu/igh-ecat/master/Makefile.am
new file mode 100644
index 00000000000000..1a0ef30a70b025
--- /dev/null
+++ b/ubuntu/igh-ecat/master/Makefile.am
@@ -0,0 +1,76 @@
+#-----------------------------------------------------------------------------
+#
+# Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
+#
+# This file is part of the IgH EtherCAT Master.
+#
+# The IgH EtherCAT Master is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License version 2, as
+# published by the Free Software Foundation.
+#
+# The IgH EtherCAT Master is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+# Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with the IgH EtherCAT Master; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+#
+#-----------------------------------------------------------------------------
+
+include $(top_srcdir)/Makefile.kbuild
+
+# using HEADERS to enable tags target
+noinst_HEADERS = \
+ cdev.c cdev.h \
+ coe_emerg_ring.c coe_emerg_ring.h \
+ datagram.c datagram.h \
+ datagram_pair.c datagram_pair.h \
+ debug.c debug.h \
+ device.c device.h \
+ domain.c domain.h \
+ doxygen.c \
+ eoe_request.c eoe_request.h \
+ ethernet.c ethernet.h \
+ flag.c flag.h \
+ fmmu_config.c fmmu_config.h \
+ foe.h \
+ foe_request.c foe_request.h \
+ fsm_change.c fsm_change.h \
+ fsm_coe.c fsm_coe.h \
+ fsm_eoe.c fsm_eoe.h \
+ fsm_foe.c fsm_foe.h \
+ fsm_master.c fsm_master.h \
+ fsm_pdo.c fsm_pdo.h \
+ fsm_pdo_entry.c fsm_pdo_entry.h \
+ fsm_sii.c fsm_sii.h \
+ fsm_slave.c fsm_slave.h \
+ fsm_slave_config.c fsm_slave_config.h \
+ fsm_slave_scan.c fsm_slave_scan.h \
+ fsm_soe.c fsm_soe.h \
+ globals.h \
+ ioctl.c ioctl.h \
+ mailbox.c mailbox.h \
+ master.c master.h \
+ module.c \
+ pdo.c pdo.h \
+ pdo_entry.c pdo_entry.h \
+ pdo_list.c pdo_list.h \
+ reg_request.c reg_request.h \
+ rtdm-ioctl.c \
+ rtdm.c rtdm.h \
+ rtdm_details.h \
+ rtdm_xenomai_v3.c \
+ sdo.c sdo.h \
+ sdo_entry.c sdo_entry.h \
+ sdo_request.c sdo_request.h \
+ slave.c slave.h \
+ slave_config.c slave_config.h \
+ soe_errors.c \
+ soe_request.c soe_request.h \
+ sync.c sync.h \
+ sync_config.c sync_config.h \
+ voe_handler.c voe_handler.h
+
+#-----------------------------------------------------------------------------
diff --git a/ubuntu/igh-ecat/master/api_usage_notes.md b/ubuntu/igh-ecat/master/api_usage_notes.md
new file mode 100644
index 00000000000000..1880866d6772d9
--- /dev/null
+++ b/ubuntu/igh-ecat/master/api_usage_notes.md
@@ -0,0 +1,51 @@
+Notes regaring API Usage {#apiusage}
+========================
+
+There are some restrictions on the
+[Application Interface](@ref ApplicationInterface) with respect to the state of
+the master instance and the calling context, which are explained in the
+following.
+
+## Rules of Thumb
+
+All configuration (`ecrt_slave_config_*()`) has to be done in Linux process
+context. They can be blocking, so take care when holding locks. After
+ecrt_master_activate() ing the master, your application must not alter the
+slave configuration. Instead, update process data using ecrt_domain_queue()
+and ecrt_domain_process() or use the asynchronous interface like
+ecrt_sdo_request_read(). Don't forget to ecrt_master_receive() and
+ecrt_master_send(). These functions can be called from non-process context
+too, like Xenomai/RTAI applications or custom kernel modules.
+
+## Master Phase
+
+The first distinction of cases is whether ecrt_master_activate() has been
+called or not. Before ecrt_master_activate() (or after
+ecrt_master_deactivate()), the master is in idle phase. Sending and receiving
+EtherCAT frames will be done by the master itself, the application (e. g. you)
+can store slave configurations for later use. After ecrt_master_activate(),
+the master switches into operation mode. The application is now in charge of
+steering the communication. Process data can be exchanged under real time
+constraints. Altering the slave configuration is not possible anymore.
+
+| Tag | Description |
+|---------------|---------------------------------------------------------------------------------------|
+| `master_op` | Master must be in operation phase, so after `ecrt_master_activate()` has been called. |
+| `master_idle` | Master must be in idle phase, so before `ecrt_master_activate()` has been called. |
+| `master_any` | Master can be in idle or operation phase. |
+
+## Allowed Context
+
+The second distinction of cases is the calling context, which means how the
+application is run. Most of the functions of the
+[Application Interface](@ref ApplicationInterface) have to acquire locks or
+allocate memory, so they are potentially sleeping. They are tagged as
+`blocking`. Sleeping is not allowed in all contexts, for instance when using
+Xenomai/RTAI or a kernel timer. Only a very limited set of functions can be
+called from any context, marked as `rt_safe`. They do not allocate memory and
+will not block.
+
+| Tag | Description |
+|------------|-----------------------------------------------------------------------------------------------|
+| `rt_safe` | Realtime context (RT userspace, atomic/softirq context in kernel, Xenomai/RTAI RT task) safe. |
+| `blocking` | Linux process context only (userspace or kernel), might block. |
diff --git a/ubuntu/igh-ecat/master/cdev.c b/ubuntu/igh-ecat/master/cdev.c
new file mode 100644
index 00000000000000..eb95bfe94372b3
--- /dev/null
+++ b/ubuntu/igh-ecat/master/cdev.c
@@ -0,0 +1,281 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2020 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT master character device.
+*/
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
+
+#include "cdev.h"
+#include "master.h"
+#include "slave_config.h"
+#include "voe_handler.h"
+#include "ethernet.h"
+#include "ioctl.h"
+
+/** Set to 1 to enable device operations debugging.
+ */
+#define DEBUG 0
+
+/****************************************************************************/
+
+static int eccdev_open(struct inode *, struct file *);
+static int eccdev_release(struct inode *, struct file *);
+static long eccdev_ioctl(struct file *, unsigned int, unsigned long);
+static int eccdev_mmap(struct file *, struct vm_area_struct *);
+
+/** This is the kernel version from which the .fault member of the
+ * vm_operations_struct is usable.
+ */
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 17, 0)
+# define FAULT_RETURN_TYPE int
+#else
+# define FAULT_RETURN_TYPE vm_fault_t
+#endif
+
+static FAULT_RETURN_TYPE eccdev_vma_fault(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0)
+ struct vm_area_struct *,
+#endif
+ struct vm_fault *);
+
+/****************************************************************************/
+
+/** File operation callbacks for the EtherCAT character device.
+ */
+static struct file_operations eccdev_fops = {
+ .owner = THIS_MODULE,
+ .open = eccdev_open,
+ .release = eccdev_release,
+ .unlocked_ioctl = eccdev_ioctl,
+ .mmap = eccdev_mmap
+};
+
+/** Callbacks for a virtual memory area retrieved with ecdevc_mmap().
+ */
+struct vm_operations_struct eccdev_vm_ops = {
+ .fault = eccdev_vma_fault
+};
+
+/****************************************************************************/
+
+/** Private data structure for file handles.
+ */
+typedef struct {
+ ec_cdev_t *cdev; /**< Character device. */
+ ec_ioctl_context_t ctx; /**< Context. */
+} ec_cdev_priv_t;
+
+/****************************************************************************/
+
+/** Constructor.
+ *
+ * \return 0 in case of success, else < 0
+ */
+int ec_cdev_init(
+ ec_cdev_t *cdev, /**< EtherCAT master character device. */
+ ec_master_t *master, /**< Parent master. */
+ dev_t dev_num /**< Device number. */
+ )
+{
+ int ret;
+
+ cdev->master = master;
+
+ cdev_init(&cdev->cdev, &eccdev_fops);
+ cdev->cdev.owner = THIS_MODULE;
+
+ ret = cdev_add(&cdev->cdev,
+ MKDEV(MAJOR(dev_num), master->index), 1);
+ if (ret) {
+ EC_MASTER_ERR(master, "Failed to add character device!\n");
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_cdev_clear(ec_cdev_t *cdev /**< EtherCAT XML device */)
+{
+ cdev_del(&cdev->cdev);
+}
+
+/*****************************************************************************
+ * File operations
+ ****************************************************************************/
+
+/** Called when the cdev is opened.
+ */
+int eccdev_open(struct inode *inode, struct file *filp)
+{
+ ec_cdev_t *cdev = container_of(inode->i_cdev, ec_cdev_t, cdev);
+ ec_cdev_priv_t *priv;
+
+ priv = kmalloc(sizeof(ec_cdev_priv_t), GFP_KERNEL);
+ if (!priv) {
+ EC_MASTER_ERR(cdev->master,
+ "Failed to allocate memory for private data structure.\n");
+ return -ENOMEM;
+ }
+
+ priv->cdev = cdev;
+ priv->ctx.writable = (filp->f_mode & FMODE_WRITE) != 0;
+ priv->ctx.requested = 0;
+ priv->ctx.process_data = NULL;
+ priv->ctx.process_data_size = 0;
+
+ filp->private_data = priv;
+
+#if DEBUG
+ EC_MASTER_DBG(cdev->master, 0, "File opened.\n");
+#endif
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Called when the cdev is closed.
+ */
+int eccdev_release(struct inode *inode, struct file *filp)
+{
+ ec_cdev_priv_t *priv = (ec_cdev_priv_t *) filp->private_data;
+ ec_master_t *master = priv->cdev->master;
+
+ if (priv->ctx.requested) {
+ ecrt_release_master(master);
+ }
+
+ if (priv->ctx.process_data) {
+ vfree(priv->ctx.process_data);
+ }
+
+#if DEBUG
+ EC_MASTER_DBG(master, 0, "File closed.\n");
+#endif
+
+ kfree(priv);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Called when an ioctl() command is issued.
+ */
+long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+ ec_cdev_priv_t *priv = (ec_cdev_priv_t *) filp->private_data;
+
+#if DEBUG
+ EC_MASTER_DBG(priv->cdev->master, 0,
+ "ioctl(filp = 0x%p, cmd = 0x%08x (0x%02x), arg = 0x%lx)\n",
+ filp, cmd, _IOC_NR(cmd), arg);
+#endif
+
+ return ec_ioctl(priv->cdev->master, &priv->ctx, cmd, (void __user *) arg);
+}
+
+/****************************************************************************/
+
+#ifndef VM_DONTDUMP
+/** VM_RESERVED disappeared in 3.7.
+ */
+#define VM_DONTDUMP VM_RESERVED
+#endif
+
+/** Memory-map callback for the EtherCAT character device.
+ *
+ * The actual mapping will be done in the eccdev_vma_nopage() callback of the
+ * virtual memory area.
+ *
+ * \return Always zero (success).
+ */
+int eccdev_mmap(
+ struct file *filp,
+ struct vm_area_struct *vma
+ )
+{
+ ec_cdev_priv_t *priv = (ec_cdev_priv_t *) filp->private_data;
+
+ EC_MASTER_DBG(priv->cdev->master, 1, "mmap()\n");
+
+ vma->vm_ops = &eccdev_vm_ops;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(6, 3, 0)
+ vm_flags_set(vma, VM_DONTDUMP);
+#else
+ vma->vm_flags |= VM_DONTDUMP; /* Pages will not be swapped out */
+#endif
+ vma->vm_private_data = priv;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Page fault callback for a virtual memory area.
+ *
+ * Called at the first access on a virtual-memory area retrieved with
+ * ecdev_mmap().
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static FAULT_RETURN_TYPE eccdev_vma_fault(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0)
+ struct vm_area_struct *vma, /**< Virtual memory area. */
+#endif
+ struct vm_fault *vmf /**< Fault data. */
+ )
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0)
+ struct vm_area_struct *vma = vmf->vma;
+#endif
+ unsigned long offset = vmf->pgoff << PAGE_SHIFT;
+ ec_cdev_priv_t *priv = (ec_cdev_priv_t *) vma->vm_private_data;
+ struct page *page;
+
+ if (offset >= priv->ctx.process_data_size) {
+ return VM_FAULT_SIGBUS;
+ }
+
+ page = vmalloc_to_page(priv->ctx.process_data + offset);
+ if (!page) {
+ return VM_FAULT_SIGBUS;
+ }
+
+ get_page(page);
+ vmf->page = page;
+
+ EC_MASTER_DBG(priv->cdev->master, 1, "Vma fault,"
+ " offset = %lu, page = %p\n", offset, page);
+
+ return 0;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/cdev.h b/ubuntu/igh-ecat/master/cdev.h
new file mode 100644
index 00000000000000..825d360ee0d553
--- /dev/null
+++ b/ubuntu/igh-ecat/master/cdev.h
@@ -0,0 +1,53 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT master character device.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_CDEV_H__
+#define __EC_CDEV_H__
+
+#include <linux/fs.h>
+#include <linux/cdev.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** EtherCAT master character device.
+*/
+typedef struct {
+ ec_master_t *master; /**< Master owning the device. */
+ struct cdev cdev; /**< Character device. */
+} ec_cdev_t;
+
+/****************************************************************************/
+
+int ec_cdev_init(ec_cdev_t *, ec_master_t *, dev_t);
+void ec_cdev_clear(ec_cdev_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/coe_emerg_ring.c b/ubuntu/igh-ecat/master/coe_emerg_ring.c
new file mode 100644
index 00000000000000..b0026c3b808545
--- /dev/null
+++ b/ubuntu/igh-ecat/master/coe_emerg_ring.c
@@ -0,0 +1,169 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * vim: expandtab
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT CoE emergency ring buffer methods.
+ */
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "coe_emerg_ring.h"
+
+/****************************************************************************/
+
+/** Emergency ring buffer constructor.
+ */
+void ec_coe_emerg_ring_init(
+ ec_coe_emerg_ring_t *ring, /**< Emergency ring. */
+ ec_slave_config_t *sc /**< Slave configuration. */
+ )
+{
+ ring->sc = sc;
+ ring->msgs = NULL;
+ ring->size = 0;
+ ring->read_index = 0;
+ ring->write_index = 0;
+ ring->overruns = 0;
+}
+
+/****************************************************************************/
+
+/** Emergency ring buffer destructor.
+ */
+void ec_coe_emerg_ring_clear(
+ ec_coe_emerg_ring_t *ring /**< Emergency ring. */
+ )
+{
+ if (ring->msgs) {
+ kfree(ring->msgs);
+ }
+}
+
+/****************************************************************************/
+
+/** Set the ring size.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_coe_emerg_ring_size(
+ ec_coe_emerg_ring_t *ring, /**< Emergency ring. */
+ size_t size /**< Maximum number of messages in the ring. */
+ )
+{
+ ring->size = 0;
+
+ if (size < 0) {
+ size = 0;
+ }
+
+ ring->read_index = ring->write_index = 0;
+
+ if (ring->msgs) {
+ kfree(ring->msgs);
+ }
+ ring->msgs = NULL;
+
+ if (size == 0) {
+ return 0;
+ }
+
+ ring->msgs = kmalloc(sizeof(ec_coe_emerg_msg_t) * (size + 1), GFP_KERNEL);
+ if (!ring->msgs) {
+ return -ENOMEM;
+ }
+
+ ring->size = size;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Add a new emergency message.
+ */
+void ec_coe_emerg_ring_push(
+ ec_coe_emerg_ring_t *ring, /**< Emergency ring. */
+ const u8 *msg /**< Emergency message. */
+ )
+{
+ if (!ring->size ||
+ (ring->write_index + 1) % (ring->size + 1) == ring->read_index) {
+ ring->overruns++;
+ return;
+ }
+
+ memcpy(ring->msgs[ring->write_index].data, msg,
+ EC_COE_EMERGENCY_MSG_SIZE);
+ ring->write_index = (ring->write_index + 1) % (ring->size + 1);
+}
+
+/****************************************************************************/
+
+/** Remove an emergency message from the ring.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_coe_emerg_ring_pop(
+ ec_coe_emerg_ring_t *ring, /**< Emergency ring. */
+ u8 *msg /**< Memory to store the emergency message. */
+ )
+{
+ if (ring->read_index == ring->write_index) {
+ return -ENOENT;
+ }
+
+ memcpy(msg, ring->msgs[ring->read_index].data, EC_COE_EMERGENCY_MSG_SIZE);
+ ring->read_index = (ring->read_index + 1) % (ring->size + 1);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Clear the ring.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_coe_emerg_ring_clear_ring(
+ ec_coe_emerg_ring_t *ring /**< Emergency ring. */
+ )
+{
+ ring->read_index = ring->write_index;
+ ring->overruns = 0;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Read the number of overruns.
+ *
+ * \return Number of overruns.
+ */
+int ec_coe_emerg_ring_overruns(
+ const ec_coe_emerg_ring_t *ring /**< Emergency ring. */
+ )
+{
+ return ring->overruns;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/coe_emerg_ring.h b/ubuntu/igh-ecat/master/coe_emerg_ring.h
new file mode 100644
index 00000000000000..36ab2c41f43ca0
--- /dev/null
+++ b/ubuntu/igh-ecat/master/coe_emerg_ring.h
@@ -0,0 +1,70 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT CoE emergency ring buffer structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_COE_EMERG_RING_H__
+#define __EC_COE_EMERG_RING_H__
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** EtherCAT CoE emergency message record.
+ */
+typedef struct {
+ u8 data[EC_COE_EMERGENCY_MSG_SIZE]; /**< Message data. */
+} ec_coe_emerg_msg_t;
+
+/****************************************************************************/
+
+/** EtherCAT CoE emergency ring buffer.
+ */
+typedef struct {
+ ec_slave_config_t *sc; /**< Slave configuration owning the ring. */
+
+ ec_coe_emerg_msg_t *msgs; /**< Message ring. */
+ size_t size; /**< Ring size. */
+
+ unsigned int read_index; /**< Read index. */
+ unsigned int write_index; /**< Write index. */
+ unsigned int overruns; /**< Number of overruns since last reset. */
+} ec_coe_emerg_ring_t;
+
+/****************************************************************************/
+
+void ec_coe_emerg_ring_init(ec_coe_emerg_ring_t *, ec_slave_config_t *);
+void ec_coe_emerg_ring_clear(ec_coe_emerg_ring_t *);
+
+int ec_coe_emerg_ring_size(ec_coe_emerg_ring_t *, size_t);
+void ec_coe_emerg_ring_push(ec_coe_emerg_ring_t *, const u8 *);
+int ec_coe_emerg_ring_pop(ec_coe_emerg_ring_t *, u8 *);
+int ec_coe_emerg_ring_clear_ring(ec_coe_emerg_ring_t *);
+int ec_coe_emerg_ring_overruns(const ec_coe_emerg_ring_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/datagram.c b/ubuntu/igh-ecat/master/datagram.c
new file mode 100644
index 00000000000000..c0f3360afd6eff
--- /dev/null
+++ b/ubuntu/igh-ecat/master/datagram.c
@@ -0,0 +1,644 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Methods of an EtherCAT datagram.
+*/
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "datagram.h"
+#include "master.h"
+
+/****************************************************************************/
+
+/** \cond */
+
+#define EC_FUNC_HEADER \
+ ret = ec_datagram_prealloc(datagram, data_size); \
+ if (unlikely(ret)) \
+ return ret; \
+ datagram->index = 0; \
+ datagram->working_counter = 0; \
+ datagram->state = EC_DATAGRAM_INIT;
+
+#define EC_FUNC_FOOTER \
+ datagram->data_size = data_size; \
+ return 0;
+
+/** \endcond */
+
+/****************************************************************************/
+
+/** Array of datagram type strings used in ec_datagram_type_string().
+ *
+ * \attention This is indexed by ec_datagram_type_t.
+ */
+static const char *type_strings[] = {
+ "?",
+ "APRD",
+ "APWR",
+ "APRW",
+ "FPRD",
+ "FPWR",
+ "FPRW",
+ "BRD",
+ "BWR",
+ "BRW",
+ "LRD",
+ "LWR",
+ "LRW",
+ "ARMW",
+ "FRMW"
+};
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */)
+{
+ INIT_LIST_HEAD(&datagram->queue); // mark as unqueued
+ datagram->device_index = EC_DEVICE_MAIN;
+ datagram->type = EC_DATAGRAM_NONE;
+ memset(datagram->address, 0x00, EC_ADDR_LEN);
+ datagram->data = NULL;
+ datagram->data_origin = EC_ORIG_INTERNAL;
+ datagram->mem_size = 0;
+ datagram->data_size = 0;
+ datagram->index = 0x00;
+ datagram->working_counter = 0x0000;
+ datagram->state = EC_DATAGRAM_INIT;
+#ifdef EC_HAVE_CYCLES
+ datagram->cycles_sent = 0;
+#endif
+ datagram->jiffies_sent = 0;
+#ifdef EC_HAVE_CYCLES
+ datagram->cycles_received = 0;
+#endif
+ datagram->jiffies_received = 0;
+ datagram->skip_count = 0;
+ datagram->stats_output_jiffies = 0;
+ memset(datagram->name, 0x00, EC_DATAGRAM_NAME_SIZE);
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_datagram_clear(ec_datagram_t *datagram /**< EtherCAT datagram. */)
+{
+ ec_datagram_unqueue(datagram);
+
+ if (datagram->data_origin == EC_ORIG_INTERNAL && datagram->data) {
+ kfree(datagram->data);
+ datagram->data = NULL;
+ }
+}
+
+/****************************************************************************/
+
+/** Unqueue datagram.
+ */
+void ec_datagram_unqueue(ec_datagram_t *datagram /**< EtherCAT datagram. */)
+{
+ if (!list_empty(&datagram->queue)) {
+ list_del_init(&datagram->queue);
+ }
+}
+
+/****************************************************************************/
+
+/** Allocates internal payload memory.
+ *
+ * If the allocated memory is already larger than requested, nothing ist done.
+ *
+ * \attention If external payload memory has been provided, no range checking
+ * is done!
+ *
+ * \return 0 in case of success, otherwise \a -ENOMEM.
+ */
+int ec_datagram_prealloc(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ size_t size /**< New payload size in bytes. */
+ )
+{
+ if (datagram->data_origin == EC_ORIG_EXTERNAL
+ || size <= datagram->mem_size)
+ return 0;
+
+ if (datagram->data) {
+ kfree(datagram->data);
+ datagram->data = NULL;
+ datagram->mem_size = 0;
+ }
+
+ if (!(datagram->data = kmalloc(size, GFP_KERNEL))) {
+ EC_ERR("Failed to allocate %zu bytes of datagram memory!\n", size);
+ return -ENOMEM;
+ }
+
+ datagram->mem_size = size;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Fills the datagram payload memory with zeros.
+ */
+void ec_datagram_zero(ec_datagram_t *datagram /**< EtherCAT datagram. */)
+{
+ memset(datagram->data, 0x00, datagram->data_size);
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT APRD datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_aprd(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t ring_position, /**< Auto-increment address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to read. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_APRD;
+ EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT APWR datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_apwr(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t ring_position, /**< Auto-increment address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to write. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_APWR;
+ EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT APRW datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_aprw(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t ring_position, /**< Auto-increment address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to write. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_APRW;
+ EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT ARMW datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_armw(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t ring_position, /**< Auto-increment address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to read. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_ARMW;
+ EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT FPRD datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_fprd(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t configured_address, /**< Configured station address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to read. */
+ )
+{
+ int ret;
+
+ if (unlikely(configured_address == 0x0000))
+ EC_WARN("Using configured station address 0x0000!\n");
+
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_FPRD;
+ EC_WRITE_U16(datagram->address, configured_address);
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT FPWR datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_fpwr(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t configured_address, /**< Configured station address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to write. */
+ )
+{
+ int ret;
+
+ if (unlikely(configured_address == 0x0000))
+ EC_WARN("Using configured station address 0x0000!\n");
+
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_FPWR;
+ EC_WRITE_U16(datagram->address, configured_address);
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT FPRW datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_fprw(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t configured_address, /**< Configured station address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to write. */
+ )
+{
+ int ret;
+
+ if (unlikely(configured_address == 0x0000))
+ EC_WARN("Using configured station address 0x0000!\n");
+
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_FPRW;
+ EC_WRITE_U16(datagram->address, configured_address);
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT FRMW datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_frmw(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t configured_address, /**< Configured station address. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to write. */
+ )
+{
+ int ret;
+
+ if (unlikely(configured_address == 0x0000))
+ EC_WARN("Using configured station address 0x0000!\n");
+
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_FRMW;
+ EC_WRITE_U16(datagram->address, configured_address);
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT BRD datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_brd(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to read. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_BRD;
+ EC_WRITE_U16(datagram->address, 0x0000);
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT BWR datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_bwr(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to write. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_BWR;
+ EC_WRITE_U16(datagram->address, 0x0000);
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT BRW datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_brw(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint16_t mem_address, /**< Physical memory address. */
+ size_t data_size /**< Number of bytes to write. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_BRW;
+ EC_WRITE_U16(datagram->address, 0x0000);
+ EC_WRITE_U16(datagram->address + 2, mem_address);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT LRD datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_lrd(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint32_t offset, /**< Logical address. */
+ size_t data_size /**< Number of bytes to read/write. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_LRD;
+ EC_WRITE_U32(datagram->address, offset);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT LWR datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_lwr(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint32_t offset, /**< Logical address. */
+ size_t data_size /**< Number of bytes to read/write. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_LWR;
+ EC_WRITE_U32(datagram->address, offset);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT LRW datagram.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_lrw(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint32_t offset, /**< Logical address. */
+ size_t data_size /**< Number of bytes to read/write. */
+ )
+{
+ int ret;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_LRW;
+ EC_WRITE_U32(datagram->address, offset);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT LRD datagram with external memory.
+ *
+ * \attention It is assumed, that the external memory is at least \a data_size
+ * bytes large.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_lrd_ext(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint32_t offset, /**< Logical address. */
+ size_t data_size, /**< Number of bytes to read/write. */
+ uint8_t *external_memory /**< Pointer to the memory to use. */
+ )
+{
+ int ret;
+ datagram->data = external_memory;
+ datagram->data_origin = EC_ORIG_EXTERNAL;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_LRD;
+ EC_WRITE_U32(datagram->address, offset);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT LWR datagram with external memory.
+ *
+ * \attention It is assumed, that the external memory is at least \a data_size
+ * bytes large.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_lwr_ext(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint32_t offset, /**< Logical address. */
+ size_t data_size, /**< Number of bytes to read/write. */
+ uint8_t *external_memory /**< Pointer to the memory to use. */
+ )
+{
+ int ret;
+ datagram->data = external_memory;
+ datagram->data_origin = EC_ORIG_EXTERNAL;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_LWR;
+ EC_WRITE_U32(datagram->address, offset);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Initializes an EtherCAT LRW datagram with external memory.
+ *
+ * \attention It is assumed, that the external memory is at least \a data_size
+ * bytes large.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_datagram_lrw_ext(
+ ec_datagram_t *datagram, /**< EtherCAT datagram. */
+ uint32_t offset, /**< Logical address. */
+ size_t data_size, /**< Number of bytes to read/write. */
+ uint8_t *external_memory /**< Pointer to the memory to use. */
+ )
+{
+ int ret;
+ datagram->data = external_memory;
+ datagram->data_origin = EC_ORIG_EXTERNAL;
+ EC_FUNC_HEADER;
+ datagram->type = EC_DATAGRAM_LRW;
+ EC_WRITE_U32(datagram->address, offset);
+ EC_FUNC_FOOTER;
+}
+
+/****************************************************************************/
+
+/** Prints the state of a datagram.
+ *
+ * Outputs a text message.
+ */
+void ec_datagram_print_state(
+ const ec_datagram_t *datagram /**< EtherCAT datagram */
+ )
+{
+ printk(KERN_CONT "Datagram ");
+ switch (datagram->state) {
+ case EC_DATAGRAM_INIT:
+ printk(KERN_CONT "initialized");
+ break;
+ case EC_DATAGRAM_QUEUED:
+ printk(KERN_CONT "queued");
+ break;
+ case EC_DATAGRAM_SENT:
+ printk(KERN_CONT "sent");
+ break;
+ case EC_DATAGRAM_RECEIVED:
+ printk(KERN_CONT "received");
+ break;
+ case EC_DATAGRAM_TIMED_OUT:
+ printk(KERN_CONT "timed out");
+ break;
+ case EC_DATAGRAM_ERROR:
+ printk(KERN_CONT "error");
+ break;
+ default:
+ printk(KERN_CONT "???");
+ }
+
+ printk(KERN_CONT ".\n");
+}
+
+/****************************************************************************/
+
+/** Evaluates the working counter of a single-cast datagram.
+ *
+ * Outputs an error message.
+ */
+void ec_datagram_print_wc_error(
+ const ec_datagram_t *datagram /**< EtherCAT datagram */
+ )
+{
+ if (datagram->working_counter == 0) {
+ printk(KERN_CONT "No response.");
+ }
+ else if (datagram->working_counter > 1) {
+ printk(KERN_CONT "%u slaves responded!", datagram->working_counter);
+ }
+ else {
+ printk(KERN_CONT "Success.");
+ }
+ printk(KERN_CONT "\n");
+}
+
+/****************************************************************************/
+
+/** Outputs datagram statistics at most every second.
+ */
+void ec_datagram_output_stats(
+ ec_datagram_t *datagram
+ )
+{
+ if (jiffies - datagram->stats_output_jiffies > HZ) {
+ datagram->stats_output_jiffies = jiffies;
+
+ if (unlikely(datagram->skip_count)) {
+ EC_WARN("Datagram %p (%s) was SKIPPED %u time%s.\n",
+ datagram, datagram->name,
+ datagram->skip_count,
+ datagram->skip_count == 1 ? "" : "s");
+ datagram->skip_count = 0;
+ }
+ }
+}
+
+/****************************************************************************/
+
+/** Returns a string describing the datagram type.
+ *
+ * \return Pointer on a static memory containing the requested string.
+ */
+const char *ec_datagram_type_string(
+ const ec_datagram_t *datagram /**< EtherCAT datagram. */
+ )
+{
+ return type_strings[datagram->type];
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/datagram.h b/ubuntu/igh-ecat/master/datagram.h
new file mode 100644
index 00000000000000..13184928630065
--- /dev/null
+++ b/ubuntu/igh-ecat/master/datagram.h
@@ -0,0 +1,142 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT datagram structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_DATAGRAM_H__
+#define __EC_DATAGRAM_H__
+
+#include <linux/list.h>
+#include <linux/time.h>
+#include <linux/timex.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** EtherCAT datagram type.
+ */
+typedef enum {
+ EC_DATAGRAM_NONE = 0x00, /**< Dummy. */
+ EC_DATAGRAM_APRD = 0x01, /**< Auto Increment Physical Read. */
+ EC_DATAGRAM_APWR = 0x02, /**< Auto Increment Physical Write. */
+ EC_DATAGRAM_APRW = 0x03, /**< Auto Increment Physical ReadWrite. */
+ EC_DATAGRAM_FPRD = 0x04, /**< Configured Address Physical Read. */
+ EC_DATAGRAM_FPWR = 0x05, /**< Configured Address Physical Write. */
+ EC_DATAGRAM_FPRW = 0x06, /**< Configured Address Physical ReadWrite. */
+ EC_DATAGRAM_BRD = 0x07, /**< Broadcast Read. */
+ EC_DATAGRAM_BWR = 0x08, /**< Broadcast Write. */
+ EC_DATAGRAM_BRW = 0x09, /**< Broadcast ReadWrite. */
+ EC_DATAGRAM_LRD = 0x0A, /**< Logical Read. */
+ EC_DATAGRAM_LWR = 0x0B, /**< Logical Write. */
+ EC_DATAGRAM_LRW = 0x0C, /**< Logical ReadWrite. */
+ EC_DATAGRAM_ARMW = 0x0D, /**< Auto Increment Physical Read Multiple
+ Write. */
+ EC_DATAGRAM_FRMW = 0x0E, /**< Configured Address Physical Read Multiple
+ Write. */
+} ec_datagram_type_t;
+
+/****************************************************************************/
+
+/** EtherCAT datagram state.
+ */
+typedef enum {
+ EC_DATAGRAM_INIT, /**< Initial state of a new datagram. */
+ EC_DATAGRAM_QUEUED, /**< Queued for sending. */
+ EC_DATAGRAM_SENT, /**< Sent (still in the queue). */
+ EC_DATAGRAM_RECEIVED, /**< Received (dequeued). */
+ EC_DATAGRAM_TIMED_OUT, /**< Timed out (dequeued). */
+ EC_DATAGRAM_ERROR /**< Error while sending/receiving (dequeued). */
+} ec_datagram_state_t;
+
+/****************************************************************************/
+
+/** EtherCAT datagram.
+ */
+typedef struct {
+ struct list_head queue; /**< Master datagram queue item,
+ protected by user-supplied mutex. */
+ struct list_head ext_queue; /**< External datagram queue item, protected by ext_queue_sem. */
+ struct list_head sent; /**< Master list item for sent datagrams. */
+ ec_device_index_t device_index; /**< Device via which the datagram shall
+ be / was sent. */
+ ec_datagram_type_t type; /**< Datagram type (APRD, BWR, etc.). */
+ uint8_t address[EC_ADDR_LEN]; /**< Recipient address. */
+ uint8_t *data; /**< Datagram payload. */
+ ec_origin_t data_origin; /**< Origin of the \a data memory. */
+ size_t mem_size; /**< Datagram \a data memory size. */
+ size_t data_size; /**< Size of the data in \a data. */
+ uint8_t index; /**< Index (set by master). */
+ uint16_t working_counter; /**< Working counter. */
+ ec_datagram_state_t state; /**< State. */
+#ifdef EC_HAVE_CYCLES
+ cycles_t cycles_sent; /**< Time, when the datagram was sent. */
+#endif
+ unsigned long jiffies_sent; /**< Jiffies, when the datagram was sent. */
+#ifdef EC_HAVE_CYCLES
+ cycles_t cycles_received; /**< Time, when the datagram was received. */
+#endif
+ unsigned long jiffies_received; /**< Jiffies, when the datagram was
+ received. */
+ unsigned int skip_count; /**< Number of requeues when not yet received. */
+ unsigned long stats_output_jiffies; /**< Last statistics output. */
+ char name[EC_DATAGRAM_NAME_SIZE]; /**< Description of the datagram. */
+} ec_datagram_t;
+
+/****************************************************************************/
+
+void ec_datagram_init(ec_datagram_t *);
+void ec_datagram_clear(ec_datagram_t *);
+void ec_datagram_unqueue(ec_datagram_t *);
+int ec_datagram_prealloc(ec_datagram_t *, size_t);
+void ec_datagram_zero(ec_datagram_t *);
+
+int ec_datagram_aprd(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_apwr(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_aprw(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_armw(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_fprd(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_fpwr(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_fprw(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_frmw(ec_datagram_t *, uint16_t, uint16_t, size_t);
+int ec_datagram_brd(ec_datagram_t *, uint16_t, size_t);
+int ec_datagram_bwr(ec_datagram_t *, uint16_t, size_t);
+int ec_datagram_brw(ec_datagram_t *, uint16_t, size_t);
+int ec_datagram_lrd(ec_datagram_t *, uint32_t, size_t);
+int ec_datagram_lwr(ec_datagram_t *, uint32_t, size_t);
+int ec_datagram_lrw(ec_datagram_t *, uint32_t, size_t);
+int ec_datagram_lrd_ext(ec_datagram_t *, uint32_t, size_t, uint8_t *);
+int ec_datagram_lwr_ext(ec_datagram_t *, uint32_t, size_t, uint8_t *);
+int ec_datagram_lrw_ext(ec_datagram_t *, uint32_t, size_t, uint8_t *);
+
+void ec_datagram_print_state(const ec_datagram_t *);
+void ec_datagram_print_wc_error(const ec_datagram_t *);
+void ec_datagram_output_stats(ec_datagram_t *);
+const char *ec_datagram_type_string(const ec_datagram_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/datagram_pair.c b/ubuntu/igh-ecat/master/datagram_pair.c
new file mode 100644
index 00000000000000..3e3d283886589e
--- /dev/null
+++ b/ubuntu/igh-ecat/master/datagram_pair.c
@@ -0,0 +1,193 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT datagram pair methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "master.h"
+#include "datagram_pair.h"
+
+/****************************************************************************/
+
+/** Datagram pair constructor.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_datagram_pair_init(
+ ec_datagram_pair_t *pair, /**< Datagram pair. */
+ ec_domain_t *domain, /**< Parent domain. */
+ uint32_t logical_offset, /**< Logical offset. */
+ uint8_t *data, /**< Data pointer. */
+ size_t data_size, /**< Data size. */
+ const unsigned int used[] /**< input/output use count. */
+ )
+{
+ ec_device_index_t dev_idx;
+ int ret;
+
+ INIT_LIST_HEAD(&pair->list);
+ pair->domain = domain;
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_init(&pair->datagrams[dev_idx]);
+ snprintf(pair->datagrams[dev_idx].name,
+ EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index,
+ logical_offset, ec_device_names[dev_idx != 0]);
+ pair->datagrams[dev_idx].device_index = dev_idx;
+ }
+
+ pair->expected_working_counter = 0U;
+
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ /* backup datagrams have their own memory */
+ ret = ec_datagram_prealloc(&pair->datagrams[dev_idx], data_size);
+ if (ret) {
+ goto out_datagrams;
+ }
+ }
+
+#if EC_MAX_NUM_DEVICES > 1
+ if (!(pair->send_buffer = kmalloc(data_size, GFP_KERNEL))) {
+ EC_MASTER_ERR(domain->master,
+ "Failed to allocate domain send buffer!\n");
+ ret = -ENOMEM;
+ goto out_datagrams;
+ }
+#endif
+
+ /* The ec_datagram_lxx() calls below can not fail, because either the
+ * datagram has external memory or it is preallocated. */
+
+ if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs
+ ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN],
+ logical_offset, data_size, data);
+
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_lrw(&pair->datagrams[dev_idx],
+ logical_offset, data_size);
+ }
+
+ // If LRW is used, output FMMUs increment the working counter by 2,
+ // while input FMMUs increment it by 1.
+ pair->expected_working_counter =
+ used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT];
+ } else if (used[EC_DIR_OUTPUT]) { // outputs only
+ ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN],
+ logical_offset, data_size, data);
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_lwr(&pair->datagrams[dev_idx],
+ logical_offset, data_size);
+ }
+
+ pair->expected_working_counter = used[EC_DIR_OUTPUT];
+ } else { // inputs only (or nothing)
+ ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN],
+ logical_offset, data_size, data);
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_lrd(&pair->datagrams[dev_idx], logical_offset,
+ data_size);
+ }
+
+ pair->expected_working_counter = used[EC_DIR_INPUT];
+ }
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_zero(&pair->datagrams[dev_idx]);
+ }
+
+ return 0;
+
+out_datagrams:
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ ec_datagram_clear(&pair->datagrams[dev_idx]);
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Datagram pair destructor.
+ */
+void ec_datagram_pair_clear(
+ ec_datagram_pair_t *pair /**< Datagram pair. */
+ )
+{
+ unsigned int dev_idx;
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(pair->domain->master);
+ dev_idx++) {
+ ec_datagram_clear(&pair->datagrams[dev_idx]);
+ }
+
+#if EC_MAX_NUM_DEVICES > 1
+ if (pair->send_buffer) {
+ kfree(pair->send_buffer);
+ }
+#endif
+}
+
+/****************************************************************************/
+
+/** Process received data.
+ *
+ * \return Working counter sum over all devices.
+ */
+uint16_t ec_datagram_pair_process(
+ ec_datagram_pair_t *pair, /**< Datagram pair. */
+ uint16_t wc_sum[] /**< Working counter sums. */
+ )
+{
+ unsigned int dev_idx;
+ uint16_t pair_wc = 0;
+
+ for (dev_idx = 0; dev_idx < ec_master_num_devices(pair->domain->master);
+ dev_idx++) {
+ ec_datagram_t *datagram = &pair->datagrams[dev_idx];
+
+#ifdef EC_RT_SYSLOG
+ ec_datagram_output_stats(datagram);
+#endif
+
+ if (datagram->state == EC_DATAGRAM_RECEIVED) {
+ pair_wc += datagram->working_counter;
+ wc_sum[dev_idx] += datagram->working_counter;
+ }
+ }
+
+ return pair_wc;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/datagram_pair.h b/ubuntu/igh-ecat/master/datagram_pair.h
new file mode 100644
index 00000000000000..0294a2162019db
--- /dev/null
+++ b/ubuntu/igh-ecat/master/datagram_pair.h
@@ -0,0 +1,61 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT datagram pair structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_DATAGRAM_PAIR_H__
+#define __EC_DATAGRAM_PAIR_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+#include "datagram.h"
+
+/****************************************************************************/
+
+/** Domain datagram pair.
+ */
+typedef struct {
+ struct list_head list; /**< List header. */
+ ec_domain_t *domain; /**< Parent domain. */
+ ec_datagram_t datagrams[EC_MAX_NUM_DEVICES]; /**< Datagrams. */
+#if EC_MAX_NUM_DEVICES > 1
+ uint8_t *send_buffer;
+#endif
+ unsigned int expected_working_counter; /**< Expectord working conter. */
+} ec_datagram_pair_t;
+
+/****************************************************************************/
+
+int ec_datagram_pair_init(ec_datagram_pair_t *, ec_domain_t *, uint32_t,
+ uint8_t *, size_t, const unsigned int []);
+void ec_datagram_pair_clear(ec_datagram_pair_t *);
+
+uint16_t ec_datagram_pair_process(ec_datagram_pair_t *, uint16_t[]);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/debug.c b/ubuntu/igh-ecat/master/debug.c
new file mode 100644
index 00000000000000..268a6bb7128a8c
--- /dev/null
+++ b/ubuntu/igh-ecat/master/debug.c
@@ -0,0 +1,258 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Ethernet interface for debugging purposes.
+*/
+
+/****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+
+#include "globals.h"
+#include "master.h"
+#include "debug.h"
+
+/****************************************************************************/
+
+// net_device functions
+int ec_dbgdev_open(struct net_device *);
+int ec_dbgdev_stop(struct net_device *);
+int ec_dbgdev_tx(struct sk_buff *, struct net_device *);
+struct net_device_stats *ec_dbgdev_stats(struct net_device *);
+
+/** Device operations for debug interfaces.
+ */
+static const struct net_device_ops ec_dbg_netdev_ops =
+{
+ .ndo_open = ec_dbgdev_open,
+ .ndo_stop = ec_dbgdev_stop,
+ .ndo_start_xmit = ec_dbgdev_tx,
+ .ndo_get_stats = ec_dbgdev_stats,
+};
+
+/****************************************************************************/
+
+/** Debug interface constructor.
+ *
+ * Initializes the debug object, creates a net_device and registeres it.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_debug_init(
+ ec_debug_t *dbg, /**< Debug object. */
+ ec_device_t *device, /**< EtherCAT device. */
+ const char *name /**< Interface name. */
+ )
+{
+ dbg->device = device;
+ dbg->registered = 0;
+ dbg->opened = 0;
+
+ memset(&dbg->stats, 0, sizeof(struct net_device_stats));
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0)
+ dbg->dev = alloc_netdev(sizeof(ec_debug_t *), name, NET_NAME_UNKNOWN, ether_setup);
+#else
+ dbg->dev = alloc_netdev(sizeof(ec_debug_t *), name, ether_setup);
+#endif
+ if (!(dbg->dev))
+ {
+ EC_MASTER_ERR(device->master, "Unable to allocate net_device"
+ " for debug object!\n");
+ return -ENODEV;
+ }
+
+ // initialize net_device
+ dbg->dev->netdev_ops = &ec_dbg_netdev_ops;
+
+ // initialize private data
+ *((ec_debug_t **) netdev_priv(dbg->dev)) = dbg;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Debug interface destructor.
+ *
+ * Unregisters the net_device and frees allocated memory.
+ */
+void ec_debug_clear(
+ ec_debug_t *dbg /**< debug object */
+ )
+{
+ ec_debug_unregister(dbg);
+ free_netdev(dbg->dev);
+}
+
+/****************************************************************************/
+
+/** Register debug interface.
+ */
+void ec_debug_register(
+ ec_debug_t *dbg, /**< debug object */
+ const struct net_device *net_dev /**< 'Real' Ethernet device. */
+ )
+{
+ int result;
+
+ ec_debug_unregister(dbg);
+
+ // use the Ethernet address of the physical device for the debug device
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 15, 0)
+ eth_hw_addr_set(dbg->dev, net_dev->dev_addr);
+#else
+ memcpy(dbg->dev->dev_addr, net_dev->dev_addr, ETH_ALEN);
+#endif
+
+ // connect the net_device to the kernel
+ if ((result = register_netdev(dbg->dev))) {
+ EC_MASTER_WARN(dbg->device->master, "Unable to register net_device:"
+ " error %i\n", result);
+ } else {
+ dbg->registered = 1;
+ }
+}
+
+/****************************************************************************/
+
+/** Unregister debug interface.
+ */
+void ec_debug_unregister(
+ ec_debug_t *dbg /**< debug object */
+ )
+{
+ if (dbg->registered) {
+ dbg->opened = 0;
+ dbg->registered = 0;
+ unregister_netdev(dbg->dev);
+ }
+}
+
+/****************************************************************************/
+
+/** Sends frame data to the interface.
+ */
+void ec_debug_send(
+ ec_debug_t *dbg, /**< debug object */
+ const uint8_t *data, /**< frame data */
+ size_t size /**< size of the frame data */
+ )
+{
+ struct sk_buff *skb;
+
+ if (!dbg->opened)
+ return;
+
+ // allocate socket buffer
+ if (!(skb = dev_alloc_skb(size))) {
+ dbg->stats.rx_dropped++;
+ return;
+ }
+
+ // copy frame contents into socket buffer
+ memcpy(skb_put(skb, size), data, size);
+
+ // update device statistics
+ dbg->stats.rx_packets++;
+ dbg->stats.rx_bytes += size;
+
+ // pass socket buffer to network stack
+ skb->dev = dbg->dev;
+ skb->protocol = eth_type_trans(skb, dbg->dev);
+ skb->ip_summed = CHECKSUM_UNNECESSARY;
+ netif_rx(skb);
+}
+
+/*****************************************************************************
+ * NET_DEVICE functions
+ ****************************************************************************/
+
+/** Opens the virtual network device.
+ *
+ * \return Always zero (success).
+ */
+int ec_dbgdev_open(
+ struct net_device *dev /**< debug net_device */
+ )
+{
+ ec_debug_t *dbg = *((ec_debug_t **) netdev_priv(dev));
+ dbg->opened = 1;
+ EC_MASTER_INFO(dbg->device->master, "Debug interface %s opened.\n",
+ dev->name);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Stops the virtual network device.
+ *
+ * \return Always zero (success).
+ */
+int ec_dbgdev_stop(
+ struct net_device *dev /**< debug net_device */
+ )
+{
+ ec_debug_t *dbg = *((ec_debug_t **) netdev_priv(dev));
+ dbg->opened = 0;
+ EC_MASTER_INFO(dbg->device->master, "Debug interface %s stopped.\n",
+ dev->name);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Transmits data via the virtual network device.
+ *
+ * \return Always zero (success).
+ */
+int ec_dbgdev_tx(
+ struct sk_buff *skb, /**< transmit socket buffer */
+ struct net_device *dev /**< EoE net_device */
+ )
+{
+ ec_debug_t *dbg = *((ec_debug_t **) netdev_priv(dev));
+
+ dev_kfree_skb(skb);
+ dbg->stats.tx_dropped++;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Gets statistics about the virtual network device.
+ *
+ * \return Statistics.
+ */
+struct net_device_stats *ec_dbgdev_stats(
+ struct net_device *dev /**< debug net_device */
+ )
+{
+ ec_debug_t *dbg = *((ec_debug_t **) netdev_priv(dev));
+ return &dbg->stats;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/debug.h b/ubuntu/igh-ecat/master/debug.h
new file mode 100644
index 00000000000000..5d435d4c788f29
--- /dev/null
+++ b/ubuntu/igh-ecat/master/debug.h
@@ -0,0 +1,58 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Network interface for debugging purposes.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_DEBUG_H__
+#define __EC_DEBUG_H__
+
+#include "../devices/ecdev.h"
+
+/****************************************************************************/
+
+/** Debugging network interface.
+ */
+typedef struct
+{
+ ec_device_t *device; /**< Parent device. */
+ struct net_device *dev; /**< net_device for virtual ethernet device */
+ struct net_device_stats stats; /**< device statistics */
+ uint8_t registered; /**< net_device is opened */
+ uint8_t opened; /**< net_device is opened */
+}
+ec_debug_t;
+
+/****************************************************************************/
+
+int ec_debug_init(ec_debug_t *, ec_device_t *, const char *);
+void ec_debug_clear(ec_debug_t *);
+void ec_debug_register(ec_debug_t *, const struct net_device *);
+void ec_debug_unregister(ec_debug_t *);
+void ec_debug_send(ec_debug_t *, const uint8_t *, size_t);
+
+#endif
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/device.c b/ubuntu/igh-ecat/master/device.c
new file mode 100644
index 00000000000000..598d163a972251
--- /dev/null
+++ b/ubuntu/igh-ecat/master/device.c
@@ -0,0 +1,716 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT device methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+
+#include "device.h"
+#include "master.h"
+
+#ifdef EC_DEBUG_RING
+#define timersub(a, b, result) \
+ do { \
+ (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \
+ (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \
+ if ((result)->tv_usec < 0) { \
+ --(result)->tv_sec; \
+ (result)->tv_usec += 1000000; \
+ } \
+ } while (0)
+#endif
+
+/****************************************************************************/
+
+enum {
+ /* genet driver needs extra headroom in skb for status block */
+ EXTRA_HEADROOM = 64,
+};
+
+/** Constructor.
+ *
+ * \return 0 in case of success, else < 0
+ */
+int ec_device_init(
+ ec_device_t *device, /**< EtherCAT device */
+ ec_master_t *master /**< master owning the device */
+ )
+{
+ int ret;
+ unsigned int i;
+ struct ethhdr *eth;
+#ifdef EC_DEBUG_IF
+ char ifname[10];
+ char mb = 'x';
+#endif
+
+ device->master = master;
+ device->dev = NULL;
+ device->poll = NULL;
+ device->module = NULL;
+ device->open = 0;
+ device->link_state = 0;
+ for (i = 0; i < EC_TX_RING_SIZE; i++) {
+ device->tx_skb[i] = NULL;
+ }
+ device->tx_ring_index = 0;
+#ifdef EC_HAVE_CYCLES
+ device->cycles_poll = 0;
+#endif
+#ifdef EC_DEBUG_RING
+ device->timeval_poll.tv_sec = 0;
+ device->timeval_poll.tv_usec = 0;
+#endif
+ device->jiffies_poll = 0;
+
+ ec_device_clear_stats(device);
+
+#ifdef EC_DEBUG_RING
+ for (i = 0; i < EC_DEBUG_RING_SIZE; i++) {
+ ec_debug_frame_t *df = &device->debug_frames[i];
+ df->dir = TX;
+ df->t.tv_sec = 0;
+ df->t.tv_usec = 0;
+ memset(df->data, 0, EC_MAX_DATA_SIZE);
+ df->data_size = 0;
+ }
+#endif
+#ifdef EC_DEBUG_RING
+ device->debug_frame_index = 0;
+ device->debug_frame_count = 0;
+#endif
+
+#ifdef EC_DEBUG_IF
+ if (device == &master->devices[EC_DEVICE_MAIN]) {
+ mb = 'm';
+ }
+ else {
+ mb = 'b';
+ }
+
+ sprintf(ifname, "ecdbg%c%u", mb, master->index);
+
+ ret = ec_debug_init(&device->dbg, device, ifname);
+ if (ret < 0) {
+ EC_MASTER_ERR(master, "Failed to init debug device!\n");
+ goto out_return;
+ }
+#endif
+
+ for (i = 0; i < EC_TX_RING_SIZE; i++) {
+ if (!(device->tx_skb[i] = dev_alloc_skb(ETH_FRAME_LEN + EXTRA_HEADROOM))) {
+ EC_MASTER_ERR(master, "Error allocating device socket buffer!\n");
+ ret = -ENOMEM;
+ goto out_tx_ring;
+ }
+
+ // add Ethernet-II-header
+ skb_reserve(device->tx_skb[i], ETH_HLEN + EXTRA_HEADROOM);
+ eth = (struct ethhdr *) skb_push(device->tx_skb[i], ETH_HLEN);
+ eth->h_proto = htons(0x88A4);
+ memset(eth->h_dest, 0xFF, ETH_ALEN);
+ }
+
+ return 0;
+
+out_tx_ring:
+ for (i = 0; i < EC_TX_RING_SIZE; i++) {
+ if (device->tx_skb[i]) {
+ dev_kfree_skb(device->tx_skb[i]);
+ }
+ }
+#ifdef EC_DEBUG_IF
+ ec_debug_clear(&device->dbg);
+out_return:
+#endif
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_device_clear(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+ unsigned int i;
+
+ if (device->open) {
+ ec_device_close(device);
+ }
+ for (i = 0; i < EC_TX_RING_SIZE; i++)
+ dev_kfree_skb(device->tx_skb[i]);
+#ifdef EC_DEBUG_IF
+ ec_debug_clear(&device->dbg);
+#endif
+}
+
+/****************************************************************************/
+
+/** Associate with net_device.
+ */
+void ec_device_attach(
+ ec_device_t *device, /**< EtherCAT device */
+ struct net_device *net_dev, /**< net_device structure */
+ ec_pollfunc_t poll, /**< pointer to device's poll function */
+ struct module *module /**< the device's module */
+ )
+{
+ unsigned int i;
+ struct ethhdr *eth;
+
+ ec_device_detach(device); // resets fields
+
+ device->dev = net_dev;
+ device->poll = poll;
+ device->module = module;
+
+ for (i = 0; i < EC_TX_RING_SIZE; i++) {
+ device->tx_skb[i]->dev = net_dev;
+ eth = (struct ethhdr *) (device->tx_skb[i]->data);
+ memcpy(eth->h_source, net_dev->dev_addr, ETH_ALEN);
+ }
+
+#ifdef EC_DEBUG_IF
+ ec_debug_register(&device->dbg, net_dev);
+#endif
+}
+
+/****************************************************************************/
+
+/** Disconnect from net_device.
+ */
+void ec_device_detach(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+ unsigned int i;
+
+#ifdef EC_DEBUG_IF
+ ec_debug_unregister(&device->dbg);
+#endif
+
+ device->dev = NULL;
+ device->poll = NULL;
+ device->module = NULL;
+ device->open = 0;
+ device->link_state = 0; // down
+
+ ec_device_clear_stats(device);
+
+ for (i = 0; i < EC_TX_RING_SIZE; i++) {
+ device->tx_skb[i]->dev = NULL;
+ }
+}
+
+/****************************************************************************/
+
+/** Opens the EtherCAT device.
+ *
+ * \return 0 in case of success, else < 0
+ */
+int ec_device_open(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+ int ret;
+
+ if (!device->dev) {
+ EC_MASTER_ERR(device->master, "No net_device to open!\n");
+ return -ENODEV;
+ }
+
+ if (device->open) {
+ EC_MASTER_WARN(device->master, "Device already opened!\n");
+ return 0;
+ }
+
+ device->link_state = 0;
+
+ ec_device_clear_stats(device);
+
+ ret = device->dev->netdev_ops->ndo_open(device->dev);
+ if (!ret)
+ device->open = 1;
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Stops the EtherCAT device.
+ *
+ * \return 0 in case of success, else < 0
+ */
+int ec_device_close(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+ int ret;
+
+ if (!device->dev) {
+ EC_MASTER_ERR(device->master, "No device to close!\n");
+ return -ENODEV;
+ }
+
+ if (!device->open) {
+ EC_MASTER_WARN(device->master, "Device already closed!\n");
+ return 0;
+ }
+
+ ret = device->dev->netdev_ops->ndo_stop(device->dev);
+ if (!ret)
+ device->open = 0;
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Returns a pointer to the device's transmit memory.
+ *
+ * \return pointer to the TX socket buffer
+ */
+uint8_t *ec_device_tx_data(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+ /* cycle through socket buffers, because otherwise there is a race
+ * condition, if multiple frames are sent and the DMA is not scheduled in
+ * between. */
+ device->tx_ring_index++;
+ device->tx_ring_index %= EC_TX_RING_SIZE;
+ return device->tx_skb[device->tx_ring_index]->data + ETH_HLEN;
+}
+
+/****************************************************************************/
+
+/** Sends the content of the transmit socket buffer.
+ *
+ * Cuts the socket buffer content to the (now known) size, and calls the
+ * start_xmit() function of the assigned net_device.
+ */
+void ec_device_send(
+ ec_device_t *device, /**< EtherCAT device */
+ size_t size /**< number of bytes to send */
+ )
+{
+ struct sk_buff *skb = device->tx_skb[device->tx_ring_index];
+
+ // set the right length for the data
+ skb->len = ETH_HLEN + size;
+
+ if (unlikely(device->master->debug_level > 1)) {
+ EC_MASTER_DBG(device->master, 2, "Sending frame:\n");
+ ec_print_data(skb->data, ETH_HLEN + size);
+ }
+
+ // start sending
+ if (device->dev->netdev_ops->ndo_start_xmit(skb, device->dev) ==
+ NETDEV_TX_OK)
+ {
+ device->tx_count++;
+ device->master->device_stats.tx_count++;
+ device->tx_bytes += ETH_HLEN + size;
+ device->master->device_stats.tx_bytes += ETH_HLEN + size;
+#ifdef EC_DEBUG_IF
+ ec_debug_send(&device->dbg, skb->data, ETH_HLEN + size);
+#endif
+#ifdef EC_DEBUG_RING
+ ec_device_debug_ring_append(
+ device, TX, skb->data + ETH_HLEN, size);
+#endif
+ } else {
+ device->tx_errors++;
+ }
+}
+
+/****************************************************************************/
+
+/** Clears the frame statistics.
+ */
+void ec_device_clear_stats(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+ unsigned int i;
+
+ // zero frame statistics
+ device->tx_count = 0;
+ device->last_tx_count = 0;
+ device->rx_count = 0;
+ device->last_rx_count = 0;
+ device->tx_bytes = 0;
+ device->last_tx_bytes = 0;
+ device->rx_bytes = 0;
+ device->last_rx_bytes = 0;
+ device->tx_errors = 0;
+
+ for (i = 0; i < EC_RATE_COUNT; i++) {
+ device->tx_frame_rates[i] = 0;
+ device->rx_frame_rates[i] = 0;
+ device->tx_byte_rates[i] = 0;
+ device->rx_byte_rates[i] = 0;
+ }
+}
+
+/****************************************************************************/
+
+#ifdef EC_DEBUG_RING
+/** Appends frame data to the debug ring.
+ */
+void ec_device_debug_ring_append(
+ ec_device_t *device, /**< EtherCAT device */
+ ec_debug_frame_dir_t dir, /**< direction */
+ const void *data, /**< frame data */
+ size_t size /**< data size */
+ )
+{
+ ec_debug_frame_t *df = &device->debug_frames[device->debug_frame_index];
+
+ df->dir = dir;
+ if (dir == TX) {
+ do_gettimeofday(&df->t);
+ }
+ else {
+ df->t = device->timeval_poll;
+ }
+ memcpy(df->data, data, size);
+ df->data_size = size;
+
+ device->debug_frame_index++;
+ device->debug_frame_index %= EC_DEBUG_RING_SIZE;
+ if (unlikely(device->debug_frame_count < EC_DEBUG_RING_SIZE))
+ device->debug_frame_count++;
+}
+
+/****************************************************************************/
+
+/** Outputs the debug ring.
+ */
+void ec_device_debug_ring_print(
+ const ec_device_t *device /**< EtherCAT device */
+ )
+{
+ int i;
+ unsigned int ring_index;
+ const ec_debug_frame_t *df;
+ struct timeval t0, diff;
+
+ // calculate index of the newest frame in the ring to get its time
+ ring_index = (device->debug_frame_index + EC_DEBUG_RING_SIZE - 1)
+ % EC_DEBUG_RING_SIZE;
+ t0 = device->debug_frames[ring_index].t;
+
+ EC_MASTER_DBG(device->master, 1, "Debug ring %u:\n", ring_index);
+
+ // calculate index of the oldest frame in the ring
+ ring_index = (device->debug_frame_index + EC_DEBUG_RING_SIZE
+ - device->debug_frame_count) % EC_DEBUG_RING_SIZE;
+
+ for (i = 0; i < device->debug_frame_count; i++) {
+ df = &device->debug_frames[ring_index];
+ timersub(&t0, &df->t, &diff);
+
+ EC_MASTER_DBG(device->master, 1, "Frame %u, dt=%u.%06u s, %s:\n",
+ i + 1 - device->debug_frame_count,
+ (unsigned int) diff.tv_sec,
+ (unsigned int) diff.tv_usec,
+ (df->dir == TX) ? "TX" : "RX");
+ ec_print_data(df->data, df->data_size);
+
+ ring_index++;
+ ring_index %= EC_DEBUG_RING_SIZE;
+ }
+}
+#endif
+
+/****************************************************************************/
+
+/** Calls the poll function of the assigned net_device.
+ *
+ * The master itself works without using interrupts. Therefore the processing
+ * of received data and status changes of the network device has to be
+ * done by the master calling the ISR "manually".
+ */
+void ec_device_poll(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+#ifdef EC_HAVE_CYCLES
+ device->cycles_poll = get_cycles();
+#endif
+ device->jiffies_poll = jiffies;
+#ifdef EC_DEBUG_RING
+ do_gettimeofday(&device->timeval_poll);
+#endif
+ device->poll(device->dev);
+}
+
+/****************************************************************************/
+
+/** Update device statistics.
+ */
+void ec_device_update_stats(
+ ec_device_t *device /**< EtherCAT device */
+ )
+{
+ unsigned int i;
+
+ s32 tx_frame_rate = (device->tx_count - device->last_tx_count) * 1000;
+ s32 rx_frame_rate = (device->rx_count - device->last_rx_count) * 1000;
+ s32 tx_byte_rate = (device->tx_bytes - device->last_tx_bytes);
+ s32 rx_byte_rate = (device->rx_bytes - device->last_rx_bytes);
+
+ /* Low-pass filter:
+ * Y_n = y_(n - 1) + T / tau * (x - y_(n - 1)) | T = 1
+ * -> Y_n += (x - y_(n - 1)) / tau
+ */
+ for (i = 0; i < EC_RATE_COUNT; i++) {
+ s32 n = rate_intervals[i];
+ device->tx_frame_rates[i] +=
+ (tx_frame_rate - device->tx_frame_rates[i]) / n;
+ device->rx_frame_rates[i] +=
+ (rx_frame_rate - device->rx_frame_rates[i]) / n;
+ device->tx_byte_rates[i] +=
+ (tx_byte_rate - device->tx_byte_rates[i]) / n;
+ device->rx_byte_rates[i] +=
+ (rx_byte_rate - device->rx_byte_rates[i]) / n;
+ }
+
+ device->last_tx_count = device->tx_count;
+ device->last_rx_count = device->rx_count;
+ device->last_tx_bytes = device->tx_bytes;
+ device->last_rx_bytes = device->rx_bytes;
+}
+
+/*****************************************************************************
+ * Device interface
+ ****************************************************************************/
+
+/** Withdraws an EtherCAT device from the master.
+ *
+ * The device is disconnected from the master and all device ressources
+ * are freed.
+ *
+ * \attention Before calling this function, the ecdev_stop() function has
+ * to be called, to be sure that the master does not use the device
+ * any more.
+ * \ingroup DeviceInterface
+ */
+void ecdev_withdraw(ec_device_t *device /**< EtherCAT device */)
+{
+ ec_master_t *master = device->master;
+ char dev_str[20], mac_str[20];
+
+ ec_mac_print(device->dev->dev_addr, mac_str);
+
+ if (device == &master->devices[EC_DEVICE_MAIN]) {
+ sprintf(dev_str, "main");
+ } else if (device == &master->devices[EC_DEVICE_BACKUP]) {
+ sprintf(dev_str, "backup");
+ } else {
+ EC_MASTER_WARN(master, "%s() called with unknown device %s!\n",
+ __func__, mac_str);
+ sprintf(dev_str, "UNKNOWN");
+ }
+
+ EC_MASTER_INFO(master, "Releasing %s device %s.\n", dev_str, mac_str);
+
+ down(&master->device_sem);
+ ec_device_detach(device);
+ up(&master->device_sem);
+}
+
+/****************************************************************************/
+
+/** Opens the network device and makes the master enter IDLE phase.
+ *
+ * \return 0 on success, else < 0
+ * \ingroup DeviceInterface
+ */
+int ecdev_open(ec_device_t *device /**< EtherCAT device */)
+{
+ int ret;
+ ec_master_t *master = device->master;
+ unsigned int all_open = 1, dev_idx;
+
+ ret = ec_device_open(device);
+ if (ret) {
+ EC_MASTER_ERR(master, "Failed to open device: error %d!\n", ret);
+ return ret;
+ }
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(device->master); dev_idx++) {
+ if (!master->devices[dev_idx].open) {
+ all_open = 0;
+ break;
+ }
+ }
+
+ if (all_open) {
+ ret = ec_master_enter_idle_phase(device->master);
+ if (ret) {
+ EC_MASTER_ERR(device->master, "Failed to enter IDLE phase!\n");
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Makes the master leave IDLE phase and closes the network device.
+ *
+ * \return 0 on success, else < 0
+ * \ingroup DeviceInterface
+ */
+void ecdev_close(ec_device_t *device /**< EtherCAT device */)
+{
+ ec_master_t *master = device->master;
+
+ if (master->phase == EC_IDLE) {
+ ec_master_leave_idle_phase(master);
+ }
+
+ if (ec_device_close(device)) {
+ EC_MASTER_WARN(master, "Failed to close device!\n");
+ }
+}
+
+/****************************************************************************/
+
+/** Accepts a received frame.
+ *
+ * Forwards the received data to the master. The master will analyze the frame
+ * and dispatch the received commands to the sending instances.
+ *
+ * The data have to begin with the Ethernet header (target MAC address).
+ *
+ * \ingroup DeviceInterface
+ */
+void ecdev_receive(
+ ec_device_t *device, /**< EtherCAT device */
+ const void *data, /**< pointer to received data */
+ size_t size /**< number of bytes received */
+ )
+{
+ const void *ec_data = data + ETH_HLEN;
+ size_t ec_size = size - ETH_HLEN;
+
+ if (unlikely(!data)) {
+ EC_MASTER_WARN(device->master, "%s() called with NULL data.\n",
+ __func__);
+ return;
+ }
+
+ device->rx_count++;
+ device->master->device_stats.rx_count++;
+ device->rx_bytes += size;
+ device->master->device_stats.rx_bytes += size;
+
+ if (unlikely(device->master->debug_level > 1)) {
+ EC_MASTER_DBG(device->master, 2, "Received frame:\n");
+ ec_print_data(data, size);
+ }
+
+#ifdef EC_DEBUG_IF
+ ec_debug_send(&device->dbg, data, size);
+#endif
+#ifdef EC_DEBUG_RING
+ ec_device_debug_ring_append(device, RX, ec_data, ec_size);
+#endif
+
+ ec_master_receive_datagrams(device->master, device, ec_data, ec_size);
+}
+
+/****************************************************************************/
+
+/** Sets a new link state.
+ *
+ * If the device notifies the master about the link being down, the master
+ * will not try to send frames using this device.
+ *
+ * \ingroup DeviceInterface
+ */
+void ecdev_set_link(
+ ec_device_t *device, /**< EtherCAT device */
+ uint8_t state /**< new link state */
+ )
+{
+ if (unlikely(!device)) {
+ EC_WARN("ecdev_set_link() called with null device!\n");
+ return;
+ }
+
+ if (likely(state != device->link_state)) {
+ device->link_state = state;
+ EC_MASTER_INFO(device->master,
+ "Link state of %s changed to %s.\n",
+ device->dev->name, (state ? "UP" : "DOWN"));
+ }
+}
+
+/****************************************************************************/
+
+/** Reads the link state.
+ *
+ * \ingroup DeviceInterface
+ *
+ * \return Link state.
+ */
+uint8_t ecdev_get_link(
+ const ec_device_t *device /**< EtherCAT device */
+ )
+{
+ if (unlikely(!device)) {
+ EC_WARN("ecdev_get_link() called with null device!\n");
+ return 0;
+ }
+
+ return device->link_state;
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecdev_withdraw);
+EXPORT_SYMBOL(ecdev_open);
+EXPORT_SYMBOL(ecdev_close);
+EXPORT_SYMBOL(ecdev_receive);
+EXPORT_SYMBOL(ecdev_get_link);
+EXPORT_SYMBOL(ecdev_set_link);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/device.h b/ubuntu/igh-ecat/master/device.h
new file mode 100644
index 00000000000000..7743093130c7bc
--- /dev/null
+++ b/ubuntu/igh-ecat/master/device.h
@@ -0,0 +1,150 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT device structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_DEVICE_H__
+#define __EC_DEVICE_H__
+
+#include <linux/interrupt.h>
+
+#include "../devices/ecdev.h"
+#include "globals.h"
+
+/**
+ * Size of the transmit ring.
+ * This memory ring is used to transmit frames. It is necessary to use
+ * different memory regions, because otherwise the network device DMA could
+ * send the same data twice, if it is called twice.
+ */
+#define EC_TX_RING_SIZE 2
+
+#ifdef EC_DEBUG_IF
+#include "debug.h"
+#endif
+
+#ifdef EC_DEBUG_RING
+#define EC_DEBUG_RING_SIZE 10
+
+typedef enum {
+ TX, RX
+} ec_debug_frame_dir_t;
+
+typedef struct {
+ ec_debug_frame_dir_t dir;
+ struct timeval t;
+ uint8_t data[EC_MAX_DATA_SIZE];
+ unsigned int data_size;
+} ec_debug_frame_t;
+
+#endif
+
+/****************************************************************************/
+
+/**
+ EtherCAT device.
+ An EtherCAT device is a network interface card, that is owned by an
+ EtherCAT master to send and receive EtherCAT frames with.
+*/
+
+struct ec_device
+{
+ ec_master_t *master; /**< EtherCAT master */
+ struct net_device *dev; /**< pointer to the assigned net_device */
+ ec_pollfunc_t poll; /**< pointer to the device's poll function */
+ struct module *module; /**< pointer to the device's owning module */
+ uint8_t open; /**< true, if the net_device has been opened */
+ uint8_t link_state; /**< device link state */
+ struct sk_buff *tx_skb[EC_TX_RING_SIZE]; /**< transmit skb ring */
+ unsigned int tx_ring_index; /**< last ring entry used to transmit */
+#ifdef EC_HAVE_CYCLES
+ cycles_t cycles_poll; /**< cycles of last poll */
+#endif
+#ifdef EC_DEBUG_RING
+ struct timeval timeval_poll;
+#endif
+ unsigned long jiffies_poll; /**< jiffies of last poll */
+
+ // Frame statistics
+ u64 tx_count; /**< Number of frames sent. */
+ u64 last_tx_count; /**< Number of frames sent of last statistics cycle. */
+ u64 rx_count; /**< Number of frames received. */
+ u64 last_rx_count; /**< Number of frames received of last statistics
+ cycle. */
+ u64 tx_bytes; /**< Number of bytes sent. */
+ u64 last_tx_bytes; /**< Number of bytes sent of last statistics cycle. */
+ u64 rx_bytes; /**< Number of bytes received. */
+ u64 last_rx_bytes; /**< Number of bytes received of last statistics cycle.
+ */
+ u64 tx_errors; /**< Number of transmit errors. */
+ s32 tx_frame_rates[EC_RATE_COUNT]; /**< Transmit rates in frames/s for
+ different statistics cycle periods.
+ */
+ s32 rx_frame_rates[EC_RATE_COUNT]; /**< Receive rates in frames/s for
+ different statistics cycle periods.
+ */
+ s32 tx_byte_rates[EC_RATE_COUNT]; /**< Transmit rates in byte/s for
+ different statistics cycle periods. */
+ s32 rx_byte_rates[EC_RATE_COUNT]; /**< Receive rates in byte/s for
+ different statistics cycle periods. */
+
+#ifdef EC_DEBUG_IF
+ ec_debug_t dbg; /**< debug device */
+#endif
+#ifdef EC_DEBUG_RING
+ ec_debug_frame_t debug_frames[EC_DEBUG_RING_SIZE];
+ unsigned int debug_frame_index;
+ unsigned int debug_frame_count;
+#endif
+};
+
+/****************************************************************************/
+
+int ec_device_init(ec_device_t *, ec_master_t *);
+void ec_device_clear(ec_device_t *);
+
+void ec_device_attach(ec_device_t *, struct net_device *, ec_pollfunc_t,
+ struct module *);
+void ec_device_detach(ec_device_t *);
+
+int ec_device_open(ec_device_t *);
+int ec_device_close(ec_device_t *);
+
+void ec_device_poll(ec_device_t *);
+uint8_t *ec_device_tx_data(ec_device_t *);
+void ec_device_send(ec_device_t *, size_t);
+void ec_device_clear_stats(ec_device_t *);
+void ec_device_update_stats(ec_device_t *);
+
+#ifdef EC_DEBUG_RING
+void ec_device_debug_ring_append(ec_device_t *, ec_debug_frame_dir_t,
+ const void *, size_t);
+void ec_device_debug_ring_print(const ec_device_t *);
+#endif
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/domain.c b/ubuntu/igh-ecat/master/domain.c
new file mode 100644
index 00000000000000..d74fd715b0e162
--- /dev/null
+++ b/ubuntu/igh-ecat/master/domain.c
@@ -0,0 +1,719 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT domain methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/module.h>
+
+#include "globals.h"
+#include "master.h"
+#include "slave_config.h"
+
+#include "domain.h"
+#include "datagram_pair.h"
+
+/** Extra debug output for redundancy functions.
+ */
+#define DEBUG_REDUNDANCY 0
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_domain_clear_data(ec_domain_t *);
+int ec_domain_add_datagram_pair(ec_domain_t *, uint32_t, size_t, uint8_t *,
+ const unsigned int []);
+int shall_count(const ec_fmmu_config_t *, const ec_fmmu_config_t *);
+#if EC_MAX_NUM_DEVICES > 1
+int data_changed(uint8_t *, const ec_datagram_t *, size_t, size_t);
+#endif
+
+/****************************************************************************/
+
+/** Domain constructor.
+ */
+void ec_domain_init(
+ ec_domain_t *domain, /**< EtherCAT domain. */
+ ec_master_t *master, /**< Parent master. */
+ unsigned int index /**< Index. */
+ )
+{
+ unsigned int dev_idx;
+
+ domain->master = master;
+ domain->index = index;
+ INIT_LIST_HEAD(&domain->fmmu_configs);
+ domain->data_size = 0;
+ domain->data = NULL;
+ domain->data_origin = EC_ORIG_INTERNAL;
+ domain->logical_base_address = 0x00000000;
+ INIT_LIST_HEAD(&domain->datagram_pairs);
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ domain->working_counter[dev_idx] = 0x0000;
+ }
+ domain->expected_working_counter = 0x0000;
+ domain->working_counter_changes = 0;
+ domain->redundancy_active = 0;
+ domain->notify_jiffies = 0;
+}
+
+/****************************************************************************/
+
+/** Domain destructor.
+ */
+void ec_domain_clear(ec_domain_t *domain /**< EtherCAT domain */)
+{
+ ec_datagram_pair_t *datagram_pair, *next_pair;
+
+ // dequeue and free datagrams
+ list_for_each_entry_safe(datagram_pair, next_pair,
+ &domain->datagram_pairs, list) {
+ ec_datagram_pair_clear(datagram_pair);
+ kfree(datagram_pair);
+ }
+
+ ec_domain_clear_data(domain);
+}
+
+/****************************************************************************/
+
+/** Frees internally allocated memory.
+ */
+void ec_domain_clear_data(
+ ec_domain_t *domain /**< EtherCAT domain. */
+ )
+{
+ if (domain->data_origin == EC_ORIG_INTERNAL && domain->data) {
+ kfree(domain->data);
+ }
+
+ domain->data = NULL;
+ domain->data_origin = EC_ORIG_INTERNAL;
+}
+
+/****************************************************************************/
+
+/** Adds an FMMU configuration to the domain.
+ */
+void ec_domain_add_fmmu_config(
+ ec_domain_t *domain, /**< EtherCAT domain. */
+ ec_fmmu_config_t *fmmu /**< FMMU configuration. */
+ )
+{
+ fmmu->domain = domain;
+
+ domain->data_size += fmmu->data_size;
+ list_add_tail(&fmmu->list, &domain->fmmu_configs);
+
+ EC_MASTER_DBG(domain->master, 1, "Domain %u:"
+ " Added %u bytes, total %zu.\n",
+ domain->index, fmmu->data_size, domain->data_size);
+}
+
+/****************************************************************************/
+
+/** Allocates a domain datagram pair and appends it to the list.
+ *
+ * The datagrams' types and expected working counters are determined by the
+ * number of input and output fmmus that share the datagrams.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_domain_add_datagram_pair(
+ ec_domain_t *domain, /**< EtherCAT domain. */
+ uint32_t logical_offset, /**< Logical offset. */
+ size_t data_size, /**< Size of the data. */
+ uint8_t *data, /**< Process data. */
+ const unsigned int used[] /**< Slave config counter for in/out. */
+ )
+{
+ ec_datagram_pair_t *datagram_pair;
+ int ret;
+
+ if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) {
+ EC_MASTER_ERR(domain->master,
+ "Failed to allocate domain datagram pair!\n");
+ return -ENOMEM;
+ }
+
+ ret = ec_datagram_pair_init(datagram_pair, domain, logical_offset, data,
+ data_size, used);
+ if (ret) {
+ kfree(datagram_pair);
+ return ret;
+ }
+
+ domain->expected_working_counter +=
+ datagram_pair->expected_working_counter;
+
+ EC_MASTER_DBG(domain->master, 1,
+ "Adding datagram pair with expected WC %u.\n",
+ datagram_pair->expected_working_counter);
+
+
+ list_add_tail(&datagram_pair->list, &domain->datagram_pairs);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Domain finish helper function.
+ *
+ * Detects, if a slave configuration has already been taken into account for
+ * a datagram's expected working counter calculation.
+ *
+ * Walks through the list of all FMMU configurations for the current datagram
+ * and ends before the current datagram.
+ *
+ * \return Non-zero if slave connfig was already counted.
+ */
+int shall_count(
+ const ec_fmmu_config_t *cur_fmmu, /**< Current FMMU with direction to
+ search for. */
+ const ec_fmmu_config_t *first_fmmu /**< Datagram's first FMMU. */
+ )
+{
+ for (; first_fmmu != cur_fmmu;
+ first_fmmu = list_entry(first_fmmu->list.next,
+ ec_fmmu_config_t, list)) {
+
+ if (first_fmmu->sc == cur_fmmu->sc
+ && first_fmmu->dir == cur_fmmu->dir) {
+ return 0; // was already counted
+ }
+ }
+
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Finishes a domain.
+ *
+ * This allocates the necessary datagrams and writes the correct logical
+ * addresses to every configured FMMU.
+ *
+ * \todo Check for FMMUs that do not fit into any datagram.
+ *
+ * \retval 0 Success
+ * \retval <0 Error code.
+ */
+int ec_domain_finish(
+ ec_domain_t *domain, /**< EtherCAT domain. */
+ uint32_t base_address /**< Logical base address. */
+ )
+{
+ uint32_t datagram_offset;
+ size_t datagram_size;
+ unsigned int datagram_count;
+ unsigned int datagram_used[EC_DIR_COUNT];
+ ec_fmmu_config_t *fmmu;
+ const ec_fmmu_config_t *datagram_first_fmmu = NULL;
+ const ec_datagram_pair_t *datagram_pair;
+ int ret;
+
+ domain->logical_base_address = base_address;
+
+ if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) {
+ if (!(domain->data =
+ (uint8_t *) kmalloc(domain->data_size, GFP_KERNEL))) {
+ EC_MASTER_ERR(domain->master, "Failed to allocate %zu bytes"
+ " internal memory for domain %u!\n",
+ domain->data_size, domain->index);
+ return -ENOMEM;
+ }
+ }
+
+ // Cycle through all domain FMMUs and
+ // - correct the logical base addresses
+ // - set up the datagrams to carry the process data
+ // - calculate the datagrams' expected working counters
+ datagram_offset = 0;
+ datagram_size = 0;
+ datagram_count = 0;
+ datagram_used[EC_DIR_OUTPUT] = 0;
+ datagram_used[EC_DIR_INPUT] = 0;
+
+ if (!list_empty(&domain->fmmu_configs)) {
+ datagram_first_fmmu =
+ list_entry(domain->fmmu_configs.next, ec_fmmu_config_t, list);
+ }
+
+ list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
+
+ // Correct logical FMMU address
+ fmmu->logical_start_address += base_address;
+
+ // If the current FMMU's data do not fit in the current datagram,
+ // allocate a new one.
+ if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) {
+ ret = ec_domain_add_datagram_pair(domain,
+ domain->logical_base_address + datagram_offset,
+ datagram_size, domain->data + datagram_offset,
+ datagram_used);
+ if (ret < 0)
+ return ret;
+
+ datagram_offset += datagram_size;
+ datagram_size = 0;
+ datagram_count++;
+ datagram_used[EC_DIR_OUTPUT] = 0;
+ datagram_used[EC_DIR_INPUT] = 0;
+ datagram_first_fmmu = fmmu;
+ }
+
+ // Increment Input/Output counter to determine datagram types
+ // and calculate expected working counters
+ if (shall_count(fmmu, datagram_first_fmmu)) {
+ datagram_used[fmmu->dir]++;
+ }
+
+ datagram_size += fmmu->data_size;
+ }
+
+ /* Allocate last datagram pair, if data are left (this is also the case if
+ * the process data fit into a single datagram) */
+ if (datagram_size) {
+ ret = ec_domain_add_datagram_pair(domain,
+ domain->logical_base_address + datagram_offset,
+ datagram_size, domain->data + datagram_offset,
+ datagram_used);
+ if (ret < 0)
+ return ret;
+ datagram_count++;
+ }
+
+ EC_MASTER_INFO(domain->master, "Domain%u: Logical address 0x%08x,"
+ " %zu byte, expected working counter %u.\n", domain->index,
+ domain->logical_base_address, domain->data_size,
+ domain->expected_working_counter);
+
+ list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) {
+ const ec_datagram_t *datagram =
+ &datagram_pair->datagrams[EC_DEVICE_MAIN];
+ EC_MASTER_INFO(domain->master, " Datagram %s: Logical offset 0x%08x,"
+ " %zu byte, type %s.\n", datagram->name,
+ EC_READ_U32(datagram->address), datagram->data_size,
+ ec_datagram_type_string(datagram));
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get the number of FMMU configurations of the domain.
+ */
+unsigned int ec_domain_fmmu_count(const ec_domain_t *domain)
+{
+ const ec_fmmu_config_t *fmmu;
+ unsigned int num = 0;
+
+ list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
+ num++;
+ }
+
+ return num;
+}
+
+/****************************************************************************/
+
+/** Get a certain FMMU configuration via its position in the list.
+ *
+ * \return FMMU at position \a pos, or NULL.
+ */
+const ec_fmmu_config_t *ec_domain_find_fmmu(
+ const ec_domain_t *domain, /**< EtherCAT domain. */
+ unsigned int pos /**< List position. */
+ )
+{
+ const ec_fmmu_config_t *fmmu;
+
+ list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
+ if (pos--)
+ continue;
+ return fmmu;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+#if EC_MAX_NUM_DEVICES > 1
+
+/** Process received data.
+ */
+int data_changed(
+ uint8_t *send_buffer,
+ const ec_datagram_t *datagram,
+ size_t offset,
+ size_t size
+ )
+{
+ uint8_t *sent = send_buffer + offset;
+ uint8_t *recv = datagram->data + offset;
+ size_t i;
+
+ for (i = 0; i < size; i++) {
+ if (recv[i] != sent[i]) {
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+#endif
+
+/*****************************************************************************
+ * Application interface
+ ****************************************************************************/
+
+int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,
+ const ec_pdo_entry_reg_t *regs)
+{
+ const ec_pdo_entry_reg_t *reg;
+ ec_slave_config_t *sc;
+ int ret;
+
+ EC_MASTER_DBG(domain->master, 1, "ecrt_domain_reg_pdo_entry_list("
+ "domain = 0x%p, regs = 0x%p)\n", domain, regs);
+
+ for (reg = regs; reg->index; reg++) {
+ sc = ecrt_master_slave_config_err(domain->master, reg->alias,
+ reg->position, reg->vendor_id, reg->product_code);
+ if (IS_ERR(sc))
+ return PTR_ERR(sc);
+
+ ret = ecrt_slave_config_reg_pdo_entry(sc, reg->index,
+ reg->subindex, domain, reg->bit_position);
+ if (ret < 0)
+ return ret;
+
+ *reg->offset = ret;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+size_t ecrt_domain_size(const ec_domain_t *domain)
+{
+ return domain->data_size;
+}
+
+/****************************************************************************/
+
+void ecrt_domain_external_memory(ec_domain_t *domain, uint8_t *mem)
+{
+ EC_MASTER_DBG(domain->master, 1, "ecrt_domain_external_memory("
+ "domain = 0x%p, mem = 0x%p)\n", domain, mem);
+
+ down(&domain->master->master_sem);
+
+ ec_domain_clear_data(domain);
+
+ domain->data = mem;
+ domain->data_origin = EC_ORIG_EXTERNAL;
+
+ up(&domain->master->master_sem);
+}
+
+/****************************************************************************/
+
+uint8_t *ecrt_domain_data(const ec_domain_t *domain)
+{
+ return domain->data;
+}
+
+/****************************************************************************/
+
+int ecrt_domain_process(ec_domain_t *domain)
+{
+ uint16_t wc_sum[EC_MAX_NUM_DEVICES] = {}, wc_total;
+ ec_datagram_pair_t *pair;
+#if EC_MAX_NUM_DEVICES > 1
+ uint16_t datagram_pair_wc, redundant_wc;
+ unsigned int datagram_offset;
+ ec_fmmu_config_t *fmmu = list_first_entry(&domain->fmmu_configs,
+ ec_fmmu_config_t, list);
+ unsigned int redundancy;
+#endif
+ unsigned int dev_idx;
+#ifdef EC_RT_SYSLOG
+ unsigned int wc_change;
+#endif
+
+#if DEBUG_REDUNDANCY
+ EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index);
+#endif
+
+ list_for_each_entry(pair, &domain->datagram_pairs, list) {
+#if EC_MAX_NUM_DEVICES > 1
+ datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum);
+#else
+ ec_datagram_pair_process(pair, wc_sum);
+#endif
+
+#if EC_MAX_NUM_DEVICES > 1
+ if (ec_master_num_devices(domain->master) > 1) {
+ ec_datagram_t *main_datagram = &pair->datagrams[EC_DEVICE_MAIN];
+ uint32_t logical_datagram_address =
+ EC_READ_U32(main_datagram->address);
+ size_t datagram_size = main_datagram->data_size;
+
+#if DEBUG_REDUNDANCY
+ EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n",
+ main_datagram->name, logical_datagram_address);
+#endif
+
+ /* Redundancy: Go through FMMU configs to detect data changes. */
+ list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) {
+ ec_datagram_t *backup_datagram =
+ &pair->datagrams[EC_DEVICE_BACKUP];
+
+ if (fmmu->dir != EC_DIR_INPUT) {
+ continue;
+ }
+
+ if (fmmu->logical_start_address >=
+ logical_datagram_address + datagram_size) {
+ // fmmu data contained in next datagram pair
+ break;
+ }
+
+ datagram_offset =
+ fmmu->logical_start_address - logical_datagram_address;
+
+#if DEBUG_REDUNDANCY
+ EC_MASTER_DBG(domain->master, 1,
+ "input fmmu log=%u size=%u offset=%u\n",
+ fmmu->logical_start_address, fmmu->data_size,
+ datagram_offset);
+ if (domain->master->debug_level > 0) {
+ ec_print_data(pair->send_buffer + datagram_offset,
+ fmmu->data_size);
+ ec_print_data(main_datagram->data + datagram_offset,
+ fmmu->data_size);
+ ec_print_data(backup_datagram->data + datagram_offset,
+ fmmu->data_size);
+ }
+#endif
+
+ if (data_changed(pair->send_buffer, main_datagram,
+ datagram_offset, fmmu->data_size)) {
+ /* data changed on main link: no copying necessary. */
+#if DEBUG_REDUNDANCY
+ EC_MASTER_DBG(domain->master, 1, "main changed\n");
+#endif
+ } else if (data_changed(pair->send_buffer, backup_datagram,
+ datagram_offset, fmmu->data_size)) {
+ /* data changed on backup link: copy to main memory. */
+#if DEBUG_REDUNDANCY
+ EC_MASTER_DBG(domain->master, 1, "backup changed\n");
+#endif
+ memcpy(main_datagram->data + datagram_offset,
+ backup_datagram->data + datagram_offset,
+ fmmu->data_size);
+ } else if (datagram_pair_wc ==
+ pair->expected_working_counter) {
+ /* no change, but WC complete: use main data. */
+#if DEBUG_REDUNDANCY
+ EC_MASTER_DBG(domain->master, 1,
+ "no change but complete\n");
+#endif
+ } else {
+ /* no change and WC incomplete: mark WC as zero to avoid
+ * data.dependent WC flickering. */
+ datagram_pair_wc = 0;
+#if DEBUG_REDUNDANCY
+ EC_MASTER_DBG(domain->master, 1,
+ "no change and incomplete\n");
+#endif
+ }
+ }
+ }
+#endif // EC_MAX_NUM_DEVICES > 1
+ }
+
+#if EC_MAX_NUM_DEVICES > 1
+ redundant_wc = 0;
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ redundant_wc += wc_sum[dev_idx];
+ }
+
+ redundancy = redundant_wc > 0;
+ if (redundancy != domain->redundancy_active) {
+#ifdef EC_RT_SYSLOG
+ if (redundancy) {
+ EC_MASTER_WARN(domain->master,
+ "Domain %u: Redundant link in use!\n",
+ domain->index);
+ } else {
+ EC_MASTER_INFO(domain->master,
+ "Domain %u: Redundant link unused again.\n",
+ domain->index);
+ }
+#endif
+ domain->redundancy_active = redundancy;
+ }
+#else
+ domain->redundancy_active = 0;
+#endif
+
+#ifdef EC_RT_SYSLOG
+ wc_change = 0;
+#endif
+ wc_total = 0;
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ if (wc_sum[dev_idx] != domain->working_counter[dev_idx]) {
+#ifdef EC_RT_SYSLOG
+ wc_change = 1;
+#endif
+ domain->working_counter[dev_idx] = wc_sum[dev_idx];
+ }
+ wc_total += wc_sum[dev_idx];
+ }
+
+#ifdef EC_RT_SYSLOG
+ if (wc_change) {
+ domain->working_counter_changes++;
+ }
+
+ if (domain->working_counter_changes &&
+ jiffies - domain->notify_jiffies > HZ) {
+ domain->notify_jiffies = jiffies;
+ if (domain->working_counter_changes == 1) {
+ EC_MASTER_INFO(domain->master, "Domain %u: Working counter"
+ " changed to %u/%u", domain->index,
+ wc_total, domain->expected_working_counter);
+ } else {
+ EC_MASTER_INFO(domain->master, "Domain %u: %u working counter"
+ " changes - now %u/%u", domain->index,
+ domain->working_counter_changes,
+ wc_total, domain->expected_working_counter);
+ }
+#if EC_MAX_NUM_DEVICES > 1
+ if (ec_master_num_devices(domain->master) > 1) {
+ printk(KERN_CONT " (");
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master);
+ dev_idx++) {
+ printk(KERN_CONT "%u", domain->working_counter[dev_idx]);
+ if (dev_idx + 1 < ec_master_num_devices(domain->master)) {
+ printk(KERN_CONT "+");
+ }
+ }
+ printk(KERN_CONT ")");
+ }
+#endif
+ printk(KERN_CONT ".\n");
+
+ domain->working_counter_changes = 0;
+ }
+#endif
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_domain_queue(ec_domain_t *domain)
+{
+ ec_datagram_pair_t *datagram_pair;
+ ec_device_index_t dev_idx;
+
+ list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) {
+
+#if EC_MAX_NUM_DEVICES > 1
+ /* copy main data to send buffer */
+ memcpy(datagram_pair->send_buffer,
+ datagram_pair->datagrams[EC_DEVICE_MAIN].data,
+ datagram_pair->datagrams[EC_DEVICE_MAIN].data_size);
+#endif
+ ec_master_queue_datagram(domain->master,
+ &datagram_pair->datagrams[EC_DEVICE_MAIN]);
+
+ /* copy main data to backup datagram */
+ for (dev_idx = EC_DEVICE_BACKUP;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ memcpy(datagram_pair->datagrams[dev_idx].data,
+ datagram_pair->datagrams[EC_DEVICE_MAIN].data,
+ datagram_pair->datagrams[EC_DEVICE_MAIN].data_size);
+ ec_master_queue_datagram(domain->master,
+ &datagram_pair->datagrams[dev_idx]);
+ }
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state)
+{
+ unsigned int dev_idx;
+ uint16_t wc = 0;
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ wc += domain->working_counter[dev_idx];
+ }
+
+ state->working_counter = wc;
+
+ if (wc) {
+ if (wc == domain->expected_working_counter) {
+ state->wc_state = EC_WC_COMPLETE;
+ } else {
+ state->wc_state = EC_WC_INCOMPLETE;
+ }
+ } else {
+ state->wc_state = EC_WC_ZERO;
+ }
+
+ state->redundancy_active = domain->redundancy_active;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecrt_domain_reg_pdo_entry_list);
+EXPORT_SYMBOL(ecrt_domain_size);
+EXPORT_SYMBOL(ecrt_domain_external_memory);
+EXPORT_SYMBOL(ecrt_domain_data);
+EXPORT_SYMBOL(ecrt_domain_process);
+EXPORT_SYMBOL(ecrt_domain_queue);
+EXPORT_SYMBOL(ecrt_domain_state);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/domain.h b/ubuntu/igh-ecat/master/domain.h
new file mode 100644
index 00000000000000..5408628b3f09a8
--- /dev/null
+++ b/ubuntu/igh-ecat/master/domain.h
@@ -0,0 +1,82 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT domain structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_DOMAIN_H__
+#define __EC_DOMAIN_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+#include "datagram.h"
+#include "master.h"
+#include "fmmu_config.h"
+
+/****************************************************************************/
+
+/** EtherCAT domain.
+ *
+ * Handles the process data and the therefore needed datagrams of a certain
+ * group of slaves.
+ */
+struct ec_domain
+{
+ struct list_head list; /**< List item. */
+ ec_master_t *master; /**< EtherCAT master owning the domain. */
+ unsigned int index; /**< Index (just a number). */
+
+ struct list_head fmmu_configs; /**< FMMU configurations contained. */
+ size_t data_size; /**< Size of the process data. */
+ uint8_t *data; /**< Memory for the process data. */
+ ec_origin_t data_origin; /**< Origin of the \a data memory. */
+ uint32_t logical_base_address; /**< Logical offset address of the
+ process data. */
+ struct list_head datagram_pairs; /**< Datagrams pairs (main/backup) for
+ process data exchange. */
+ uint16_t working_counter[EC_MAX_NUM_DEVICES]; /**< Last working counter
+ values. */
+ uint16_t expected_working_counter; /**< Expected working counter. */
+ unsigned int working_counter_changes; /**< Working counter changes
+ since last notification. */
+ unsigned int redundancy_active; /**< Non-zero, if redundancy is in use. */
+ unsigned long notify_jiffies; /**< Time of last notification. */
+};
+
+/****************************************************************************/
+
+void ec_domain_init(ec_domain_t *, ec_master_t *, unsigned int);
+void ec_domain_clear(ec_domain_t *);
+
+void ec_domain_add_fmmu_config(ec_domain_t *, ec_fmmu_config_t *);
+int ec_domain_finish(ec_domain_t *, uint32_t);
+
+unsigned int ec_domain_fmmu_count(const ec_domain_t *);
+const ec_fmmu_config_t *ec_domain_find_fmmu(const ec_domain_t *, unsigned int);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/doxygen.c b/ubuntu/igh-ecat/master/doxygen.c
new file mode 100644
index 00000000000000..a036af2c8d5499
--- /dev/null
+++ b/ubuntu/igh-ecat/master/doxygen.c
@@ -0,0 +1,82 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+// This file only contains the doxygen mainpage.
+
+/** \file
+ * Doxygen mainpage only.
+ */
+
+/****************************************************************************/
+
+/**
+ \mainpage
+
+ \section sec_general General information
+
+ This HTML contains the complete code documentation.
+
+ The API documentations are in the <a href="modules.html">modules
+ section</a>.
+
+ A list of all native network card drivers can be found
+ <a href="devicedrivers.html">here</a>.
+
+ A <a href="libfakeethercat.html">second userspace library</a> can be used for a dry-run mode
+ or simulating Process Data.
+
+ For information how to build and install, see the INSTALL file in the source
+ root.
+
+ \section sec_contact Contact
+
+ \verbatim
+ Florian Pose <fp at igh.de>
+ Ingenieurgemeinschaft IgH
+ Nordsternstraße 66
+ D-45329 Essen
+ http://igh.de
+ \endverbatim
+
+ \section sec_license License
+
+ \verbatim
+ Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+
+ This file is part of the IgH EtherCAT Master.
+
+ The IgH EtherCAT Master is free software; you can redistribute it and/or
+ modify it under the terms of the GNU General Public License version 2, as
+ published by the Free Software Foundation.
+
+ The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with the IgH EtherCAT Master; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+
+ \endverbatim
+*/
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/eoe_request.c b/ubuntu/igh-ecat/master/eoe_request.c
new file mode 100644
index 00000000000000..f336cf340f411a
--- /dev/null
+++ b/ubuntu/igh-ecat/master/eoe_request.c
@@ -0,0 +1,80 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * Ethernet-over-EtherCAT request functions.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+
+#include "eoe_request.h"
+
+/****************************************************************************/
+
+/** EoE request constructor.
+ */
+void ec_eoe_request_init(
+ ec_eoe_request_t *req /**< EoE request. */
+ )
+{
+ INIT_LIST_HEAD(&req->list);
+ req->state = EC_INT_REQUEST_INIT;
+ req->jiffies_sent = 0U;
+
+ req->mac_address_included = 0;
+ req->ip_address_included = 0;
+ req->subnet_mask_included = 0;
+ req->gateway_included = 0;
+ req->dns_included = 0;
+ req->name_included = 0;
+
+ memset(req->mac_address, 0x00, ETH_ALEN);
+ req->ip_address.s_addr = 0;
+ req->subnet_mask.s_addr = 0;
+ req->gateway.s_addr = 0;
+ req->dns.s_addr = 0;
+ req->name[0] = 0x00;
+
+ req->result = 0x0000;
+}
+
+/****************************************************************************/
+
+/** Checks if EoE request has something to set.
+ */
+int ec_eoe_request_valid(
+ const ec_eoe_request_t *req /**< EoE request. */
+ )
+{
+ return
+ req->mac_address_included ||
+ req->ip_address_included ||
+ req->subnet_mask_included ||
+ req->gateway_included ||
+ req->dns_included ||
+ req->name_included;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/eoe_request.h b/ubuntu/igh-ecat/master/eoe_request.h
new file mode 100644
index 00000000000000..6751f1f99b0973
--- /dev/null
+++ b/ubuntu/igh-ecat/master/eoe_request.h
@@ -0,0 +1,70 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2014 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT EoE request structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_EOE_REQUEST_H__
+#define __EC_EOE_REQUEST_H__
+
+#include <linux/list.h>
+#include <linux/etherdevice.h> // ETH_ALEN
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** Ethernet-over-EtherCAT set IP parameter request.
+ */
+typedef struct {
+ struct list_head list; /**< List item. */
+ ec_internal_request_state_t state; /**< Request state. */
+ unsigned long jiffies_sent; /**< Jiffies, when the request was sent. */
+
+ uint8_t mac_address_included;
+ uint8_t ip_address_included;
+ uint8_t subnet_mask_included;
+ uint8_t gateway_included;
+ uint8_t dns_included;
+ uint8_t name_included;
+
+ unsigned char mac_address[ETH_ALEN];
+ struct in_addr ip_address;
+ struct in_addr subnet_mask;
+ struct in_addr gateway;
+ struct in_addr dns;
+ char name[EC_MAX_HOSTNAME_SIZE];
+
+ uint16_t result;
+} ec_eoe_request_t;
+
+/****************************************************************************/
+
+void ec_eoe_request_init(ec_eoe_request_t *);
+int ec_eoe_request_valid(const ec_eoe_request_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/ethernet.c b/ubuntu/igh-ecat/master/ethernet.c
new file mode 100644
index 00000000000000..55cff3119cd42c
--- /dev/null
+++ b/ubuntu/igh-ecat/master/ethernet.c
@@ -0,0 +1,886 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Ethernet over EtherCAT (EoE).
+*/
+
+/****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/lockdep.h>
+#include <linux/skbuff.h>
+
+#include "globals.h"
+#include "master.h"
+#include "slave.h"
+#include "mailbox.h"
+#include "ethernet.h"
+
+#if defined(CONFIG_SUSE_KERNEL) && LINUX_VERSION_CODE >= KERNEL_VERSION(5, 14, 0)
+#include <linux/suse_version.h>
+#else
+# ifndef SUSE_VERSION
+# define SUSE_VERSION 0
+# endif
+# ifndef SUSE_PATCHLEVEL
+# define SUSE_PATCHLEVEL 0
+# endif
+#endif
+
+/****************************************************************************/
+
+/** Defines the debug level of EoE processing.
+ *
+ * 0 = No debug messages.
+ * 1 = Output warnings.
+ * 2 = Output actions.
+ * 3 = Output actions and frame data.
+ */
+#define EOE_DEBUG_LEVEL 1
+
+/** Size of the EoE tx queue.
+ */
+#define EC_EOE_TX_QUEUE_SIZE 100
+
+/** Number of tries.
+ */
+#define EC_EOE_TRIES 100
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_eoe_flush(ec_eoe_t *);
+int ec_eoe_send(ec_eoe_t *);
+
+/****************************************************************************/
+
+// state functions
+void ec_eoe_state_rx_start(ec_eoe_t *);
+void ec_eoe_state_rx_check(ec_eoe_t *);
+void ec_eoe_state_rx_fetch(ec_eoe_t *);
+void ec_eoe_state_tx_start(ec_eoe_t *);
+void ec_eoe_state_tx_sent(ec_eoe_t *);
+
+// net_device functions
+int ec_eoedev_open(struct net_device *);
+int ec_eoedev_stop(struct net_device *);
+int ec_eoedev_tx(struct sk_buff *, struct net_device *);
+struct net_device_stats *ec_eoedev_stats(struct net_device *);
+
+/****************************************************************************/
+
+/** Device operations for EoE interfaces.
+ */
+static const struct net_device_ops ec_eoedev_ops = {
+ .ndo_open = ec_eoedev_open,
+ .ndo_stop = ec_eoedev_stop,
+ .ndo_start_xmit = ec_eoedev_tx,
+ .ndo_get_stats = ec_eoedev_stats,
+};
+
+/****************************************************************************/
+
+/** EoE constructor.
+ *
+ * Initializes the EoE handler, creates a net_device and registers it.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_eoe_init(
+ ec_eoe_t *eoe, /**< EoE handler */
+ ec_slave_t *slave /**< EtherCAT slave */
+ )
+{
+ ec_eoe_t **priv;
+ int ret = 0;
+ char name[EC_DATAGRAM_NAME_SIZE];
+ u8 mac_addr[ETH_ALEN] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55};
+
+ eoe->slave = slave;
+
+ ec_datagram_init(&eoe->datagram);
+ eoe->queue_datagram = 0;
+ eoe->state = ec_eoe_state_rx_start;
+ eoe->opened = 0;
+ eoe->rx_skb = NULL;
+ eoe->rx_expected_fragment = 0;
+ INIT_LIST_HEAD(&eoe->tx_queue);
+ eoe->tx_frame = NULL;
+ eoe->tx_queue_active = 0;
+ eoe->tx_queue_size = EC_EOE_TX_QUEUE_SIZE;
+ eoe->tx_queued_frames = 0;
+
+ eoe->tx_frame_number = 0xFF;
+ memset(&eoe->stats, 0, sizeof(struct net_device_stats));
+
+ eoe->rx_counter = 0;
+ eoe->tx_counter = 0;
+ eoe->rx_rate = 0;
+ eoe->tx_rate = 0;
+ eoe->rate_jiffies = 0;
+ eoe->rx_idle = 1;
+ eoe->tx_idle = 1;
+
+ /* device name eoe<MASTER>[as]<SLAVE>, because networking scripts don't
+ * like hyphens etc. in interface names. */
+ if (slave->effective_alias) {
+ snprintf(name, EC_DATAGRAM_NAME_SIZE,
+ "eoe%ua%u", slave->master->index, slave->effective_alias);
+ } else {
+ snprintf(name, EC_DATAGRAM_NAME_SIZE,
+ "eoe%us%u", slave->master->index, slave->ring_position);
+ }
+
+ snprintf(eoe->datagram.name, EC_DATAGRAM_NAME_SIZE, name);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0)
+ eoe->dev = alloc_netdev(sizeof(ec_eoe_t *), name, NET_NAME_UNKNOWN,
+ ether_setup);
+#else
+ eoe->dev = alloc_netdev(sizeof(ec_eoe_t *), name, ether_setup);
+#endif
+ if (!eoe->dev) {
+ EC_SLAVE_ERR(slave, "Unable to allocate net_device %s"
+ " for EoE handler!\n", name);
+ ret = -ENODEV;
+ goto out_return;
+ }
+
+ // initialize net_device
+ eoe->dev->netdev_ops = &ec_eoedev_ops;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 15, 0) || (SUSE_VERSION == 15 && SUSE_PATCHLEVEL >= 5)
+ eth_hw_addr_set(eoe->dev, mac_addr);
+#else
+ memcpy(eoe->dev->dev_addr, mac_addr, sizeof(mac_addr));
+#endif
+
+ // initialize private data
+ priv = netdev_priv(eoe->dev);
+ *priv = eoe;
+
+ // Usually setting the MTU appropriately makes the upper layers
+ // do the frame fragmenting. In some cases this doesn't work
+ // so the MTU is left on the Ethernet standard value and fragmenting
+ // is done "manually".
+#if 0
+ eoe->dev->mtu = slave->configured_rx_mailbox_size - ETH_HLEN - 10;
+#endif
+
+ // connect the net_device to the kernel
+ ret = register_netdev(eoe->dev);
+ if (ret) {
+ EC_SLAVE_ERR(slave, "Unable to register net_device:"
+ " error %i\n", ret);
+ goto out_free;
+ }
+
+ // make the last address octet unique
+ mac_addr[ETH_ALEN - 1] = (uint8_t) eoe->dev->ifindex;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 15, 0) || (SUSE_VERSION == 15 && SUSE_PATCHLEVEL >= 5)
+ eth_hw_addr_set(eoe->dev, mac_addr);
+#else
+ memcpy(eoe->dev->dev_addr, mac_addr, sizeof(mac_addr));
+#endif
+
+ return 0;
+
+ out_free:
+ free_netdev(eoe->dev);
+ eoe->dev = NULL;
+ out_return:
+ return ret;
+}
+
+/****************************************************************************/
+
+/** EoE destructor.
+ *
+ * Unregisteres the net_device and frees allocated memory.
+ */
+void ec_eoe_clear(ec_eoe_t *eoe /**< EoE handler */)
+{
+ unregister_netdev(eoe->dev); // possibly calls close callback
+
+ // empty transmit queue
+ ec_eoe_flush(eoe);
+
+ if (eoe->tx_frame) {
+ dev_kfree_skb(eoe->tx_frame->skb);
+ kfree(eoe->tx_frame);
+ }
+
+ if (eoe->rx_skb)
+ dev_kfree_skb(eoe->rx_skb);
+
+ free_netdev(eoe->dev);
+
+ ec_datagram_clear(&eoe->datagram);
+}
+
+/****************************************************************************/
+
+/** Empties the transmit queue.
+ */
+void ec_eoe_flush(ec_eoe_t *eoe /**< EoE handler */)
+{
+ ec_eoe_frame_t *frame, *next;
+ struct list_head tx_queue;
+
+ netif_tx_lock_bh(eoe->dev);
+
+ list_replace_init(&eoe->tx_queue, &tx_queue);
+ eoe->tx_queued_frames = 0;
+
+ netif_tx_unlock_bh(eoe->dev);
+
+ list_for_each_entry_safe(frame, next, &tx_queue, queue) {
+ list_del(&frame->queue);
+ dev_kfree_skb(frame->skb);
+ kfree(frame);
+ }
+}
+
+/****************************************************************************/
+
+/** Sends a frame or the next fragment.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_eoe_send(ec_eoe_t *eoe /**< EoE handler */)
+{
+ size_t remaining_size, current_size, complete_offset;
+ unsigned int last_fragment;
+ uint8_t *data;
+#if EOE_DEBUG_LEVEL >= 3
+ unsigned int i;
+#endif
+
+ remaining_size = eoe->tx_frame->skb->len - eoe->tx_offset;
+
+ if (remaining_size <= eoe->slave->configured_tx_mailbox_size - 10) {
+ current_size = remaining_size;
+ last_fragment = 1;
+ } else {
+ current_size = ((eoe->slave->configured_tx_mailbox_size - 10) / 32) * 32;
+ last_fragment = 0;
+ }
+
+ if (eoe->tx_fragment_number) {
+ complete_offset = eoe->tx_offset / 32;
+ }
+ else {
+ // complete size in 32 bit blocks, rounded up.
+ complete_offset = remaining_size / 32 + 1;
+ }
+
+#if EOE_DEBUG_LEVEL >= 2
+ EC_SLAVE_DBG(eoe->slave, 0, "EoE %s TX sending fragment %u%s"
+ " with %zu octets (%zu). %u frames queued.\n",
+ eoe->dev->name, eoe->tx_fragment_number,
+ last_fragment ? "" : "+", current_size, complete_offset,
+ eoe->tx_queued_frames);
+#endif
+
+#if EOE_DEBUG_LEVEL >= 3
+ EC_SLAVE_DBG(eoe->slave, 0, "");
+ for (i = 0; i < current_size; i++) {
+ printk(KERN_CONT "%02X ",
+ eoe->tx_frame->skb->data[eoe->tx_offset + i]);
+ if ((i + 1) % 16 == 0) {
+ printk(KERN_CONT "\n");
+ EC_SLAVE_DBG(eoe->slave, 0, "");
+ }
+ }
+ printk(KERN_CONT "\n");
+#endif
+
+ data = ec_slave_mbox_prepare_send(eoe->slave, &eoe->datagram,
+ EC_MBOX_TYPE_EOE, current_size + 4);
+ if (IS_ERR(data))
+ return PTR_ERR(data);
+
+ EC_WRITE_U8 (data, EC_EOE_FRAMETYPE_INIT_REQ); // Initiate EoE Request
+ EC_WRITE_U8 (data + 1, last_fragment);
+ EC_WRITE_U16(data + 2, ((eoe->tx_fragment_number & 0x3F) |
+ (complete_offset & 0x3F) << 6 |
+ (eoe->tx_frame_number & 0x0F) << 12));
+
+ memcpy(data + 4, eoe->tx_frame->skb->data + eoe->tx_offset, current_size);
+ eoe->queue_datagram = 1;
+
+ eoe->tx_offset += current_size;
+ eoe->tx_fragment_number++;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Runs the EoE state machine.
+ */
+void ec_eoe_run(ec_eoe_t *eoe /**< EoE handler */)
+{
+ if (!eoe->opened)
+ return;
+
+ // if the datagram was not sent, or is not yet received, skip this cycle
+ if (eoe->queue_datagram || eoe->datagram.state == EC_DATAGRAM_SENT)
+ return;
+
+ // call state function
+ eoe->state(eoe);
+
+ // update statistics
+ if (jiffies - eoe->rate_jiffies > HZ) {
+ eoe->rx_rate = eoe->rx_counter;
+ eoe->tx_rate = eoe->tx_counter;
+ eoe->rx_counter = 0;
+ eoe->tx_counter = 0;
+ eoe->rate_jiffies = jiffies;
+ }
+
+ ec_datagram_output_stats(&eoe->datagram);
+}
+
+/****************************************************************************/
+
+/** Queues the datagram, if necessary.
+ */
+void ec_eoe_queue(ec_eoe_t *eoe /**< EoE handler */)
+{
+ if (eoe->queue_datagram) {
+ ec_master_queue_datagram_ext(eoe->slave->master, &eoe->datagram);
+ eoe->queue_datagram = 0;
+ }
+}
+
+/****************************************************************************/
+
+/** Returns the state of the device.
+ *
+ * \return 1 if the device is "up", 0 if it is "down"
+ */
+int ec_eoe_is_open(const ec_eoe_t *eoe /**< EoE handler */)
+{
+ return eoe->opened;
+}
+
+/****************************************************************************/
+
+/** Returns the idle state.
+ *
+ * \retval 1 The device is idle.
+ * \retval 0 The device is busy.
+ */
+int ec_eoe_is_idle(const ec_eoe_t *eoe /**< EoE handler */)
+{
+ return eoe->rx_idle && eoe->tx_idle;
+}
+
+/*****************************************************************************
+ * STATE PROCESSING FUNCTIONS
+ ****************************************************************************/
+
+/** State: RX_START.
+ *
+ * Starts a new receiving sequence by queueing a datagram that checks the
+ * slave's mailbox for a new EoE datagram.
+ *
+ * \todo Use both devices.
+ */
+void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */)
+{
+ if (eoe->slave->error_flag ||
+ !eoe->slave->master->devices[EC_DEVICE_MAIN].link_state) {
+ eoe->rx_idle = 1;
+ eoe->tx_idle = 1;
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
+ eoe->queue_datagram = 1;
+ eoe->state = ec_eoe_state_rx_check;
+}
+
+/****************************************************************************/
+
+/** State: RX_CHECK.
+ *
+ * Processes the checking datagram sent in RX_START and issues a receive
+ * datagram, if new data is available.
+ */
+void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
+{
+ if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
+ eoe->stats.rx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
+ " check datagram for %s.\n", eoe->dev->name);
+#endif
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ if (!ec_slave_mbox_check(&eoe->datagram)) {
+ eoe->rx_idle = 1;
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ eoe->rx_idle = 0;
+ ec_slave_mbox_prepare_fetch(eoe->slave, &eoe->datagram);
+ eoe->queue_datagram = 1;
+ eoe->state = ec_eoe_state_rx_fetch;
+}
+
+/****************************************************************************/
+
+/** State: RX_FETCH.
+ *
+ * Checks if the requested data of RX_CHECK was received and processes the EoE
+ * datagram.
+ */
+void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
+{
+ size_t rec_size, data_size;
+ uint8_t *data, frame_type, last_fragment, time_appended, mbox_prot;
+ uint8_t fragment_offset, fragment_number;
+#if EOE_DEBUG_LEVEL >= 2
+ uint8_t frame_number;
+#endif
+ off_t offset;
+#if EOE_DEBUG_LEVEL >= 3
+ unsigned int i;
+#endif
+
+ if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
+ eoe->stats.rx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
+ " fetch datagram for %s.\n", eoe->dev->name);
+#endif
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(eoe->slave, &eoe->datagram,
+ &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ eoe->stats.rx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Invalid mailbox response for %s.\n",
+ eoe->dev->name);
+#endif
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_EOE) { // FIXME mailbox handler necessary
+ eoe->stats.rx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Other mailbox protocol response for %s.\n",
+ eoe->dev->name);
+#endif
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ frame_type = EC_READ_U16(data) & 0x000F;
+
+ if (frame_type != EC_EOE_FRAMETYPE_INIT_REQ) { // EoE Fragment Data
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "%s: Other frame received."
+ " Dropping.\n", eoe->dev->name);
+#endif
+ eoe->stats.rx_dropped++;
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ // EoE Fragment Request received
+
+ last_fragment = (EC_READ_U16(data) >> 8) & 0x0001;
+ time_appended = (EC_READ_U16(data) >> 9) & 0x0001;
+ fragment_number = EC_READ_U16(data + 2) & 0x003F;
+ fragment_offset = (EC_READ_U16(data + 2) >> 6) & 0x003F;
+#if EOE_DEBUG_LEVEL >= 2
+ frame_number = (EC_READ_U16(data + 2) >> 12) & 0x000F;
+#endif
+
+#if EOE_DEBUG_LEVEL >= 2
+ EC_SLAVE_DBG(eoe->slave, 0, "EoE %s RX fragment %u%s, offset %u,"
+ " frame %u%s, %zu octets\n", eoe->dev->name, fragment_number,
+ last_fragment ? "" : "+", fragment_offset, frame_number,
+ time_appended ? ", + timestamp" : "",
+ time_appended ? rec_size - 8 : rec_size - 4);
+#endif
+
+#if EOE_DEBUG_LEVEL >= 3
+ EC_SLAVE_DBG(eoe->slave, 0, "");
+ for (i = 0; i < rec_size - 4; i++) {
+ printk(KERN_CONT "%02X ", data[i + 4]);
+ if ((i + 1) % 16 == 0) {
+ printk(KERN_CONT "\n");
+ EC_SLAVE_DBG(eoe->slave, 0, "");
+ }
+ }
+ printk(KERN_CONT "\n");
+#endif
+
+ data_size = time_appended ? rec_size - 8 : rec_size - 4;
+
+ if (!fragment_number) {
+ if (eoe->rx_skb) {
+ EC_SLAVE_WARN(eoe->slave, "EoE RX freeing old socket buffer.\n");
+ dev_kfree_skb(eoe->rx_skb);
+ }
+
+ // new socket buffer
+ if (!(eoe->rx_skb = dev_alloc_skb(fragment_offset * 32))) {
+ if (printk_ratelimit())
+ EC_SLAVE_WARN(eoe->slave, "EoE RX low on mem,"
+ " frame dropped.\n");
+ eoe->stats.rx_dropped++;
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ eoe->rx_skb_offset = 0;
+ eoe->rx_skb_size = fragment_offset * 32;
+ eoe->rx_expected_fragment = 0;
+ }
+ else {
+ if (!eoe->rx_skb) {
+ eoe->stats.rx_dropped++;
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+
+ offset = fragment_offset * 32;
+ if (offset != eoe->rx_skb_offset ||
+ offset + data_size > eoe->rx_skb_size ||
+ fragment_number != eoe->rx_expected_fragment) {
+ dev_kfree_skb(eoe->rx_skb);
+ eoe->rx_skb = NULL;
+ eoe->stats.rx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Fragmenting error at %s.\n",
+ eoe->dev->name);
+#endif
+ eoe->state = ec_eoe_state_tx_start;
+ return;
+ }
+ }
+
+ // copy fragment into socket buffer
+ memcpy(skb_put(eoe->rx_skb, data_size), data + 4, data_size);
+ eoe->rx_skb_offset += data_size;
+
+ if (last_fragment) {
+ // update statistics
+ eoe->stats.rx_packets++;
+ eoe->stats.rx_bytes += eoe->rx_skb->len;
+ eoe->rx_counter += eoe->rx_skb->len;
+
+#if EOE_DEBUG_LEVEL >= 2
+ EC_SLAVE_DBG(eoe->slave, 0, "EoE %s RX frame completed"
+ " with %u octets.\n", eoe->dev->name, eoe->rx_skb->len);
+#endif
+
+ // pass socket buffer to network stack
+ eoe->rx_skb->dev = eoe->dev;
+ eoe->rx_skb->protocol = eth_type_trans(eoe->rx_skb, eoe->dev);
+ eoe->rx_skb->ip_summed = CHECKSUM_UNNECESSARY;
+ if (netif_rx(eoe->rx_skb)) {
+ EC_SLAVE_WARN(eoe->slave, "EoE RX netif_rx failed.\n");
+ }
+ eoe->rx_skb = NULL;
+
+ eoe->state = ec_eoe_state_tx_start;
+ }
+ else {
+ eoe->rx_expected_fragment++;
+#if EOE_DEBUG_LEVEL >= 2
+ EC_SLAVE_DBG(eoe->slave, 0, "EoE %s RX expecting fragment %u\n",
+ eoe->dev->name, eoe->rx_expected_fragment);
+#endif
+ eoe->state = ec_eoe_state_rx_start;
+ }
+}
+
+/****************************************************************************/
+
+/** State: TX START.
+ *
+ * Starts a new transmit sequence. If no data is available, a new receive
+ * sequence is started instead.
+ *
+ * \todo Use both devices.
+ */
+void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */)
+{
+#if EOE_DEBUG_LEVEL >= 2
+ unsigned int wakeup = 0;
+#endif
+
+ if (eoe->slave->error_flag ||
+ !eoe->slave->master->devices[EC_DEVICE_MAIN].link_state) {
+ eoe->rx_idle = 1;
+ eoe->tx_idle = 1;
+ return;
+ }
+
+ netif_tx_lock_bh(eoe->dev);
+
+ if (!eoe->tx_queued_frames || list_empty(&eoe->tx_queue)) {
+ netif_tx_unlock_bh(eoe->dev);
+ eoe->tx_idle = 1;
+ // no data available.
+ // start a new receive immediately.
+ ec_eoe_state_rx_start(eoe);
+ return;
+ }
+
+ // take the first frame out of the queue
+ eoe->tx_frame = list_entry(eoe->tx_queue.next, ec_eoe_frame_t, queue);
+ list_del(&eoe->tx_frame->queue);
+ if (!eoe->tx_queue_active &&
+ eoe->tx_queued_frames == eoe->tx_queue_size / 2) {
+ netif_wake_queue(eoe->dev);
+ eoe->tx_queue_active = 1;
+#if EOE_DEBUG_LEVEL >= 2
+ wakeup = 1;
+#endif
+ }
+
+ eoe->tx_queued_frames--;
+ netif_tx_unlock_bh(eoe->dev);
+
+ eoe->tx_idle = 0;
+
+ eoe->tx_frame_number++;
+ eoe->tx_frame_number %= 16;
+ eoe->tx_fragment_number = 0;
+ eoe->tx_offset = 0;
+
+ if (ec_eoe_send(eoe)) {
+ dev_kfree_skb(eoe->tx_frame->skb);
+ kfree(eoe->tx_frame);
+ eoe->tx_frame = NULL;
+ eoe->stats.tx_errors++;
+ eoe->state = ec_eoe_state_rx_start;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Send error at %s.\n", eoe->dev->name);
+#endif
+ return;
+ }
+
+#if EOE_DEBUG_LEVEL >= 2
+ if (wakeup)
+ EC_SLAVE_DBG(eoe->slave, 0, "EoE %s waking up TX queue...\n",
+ eoe->dev->name);
+#endif
+
+ eoe->tries = EC_EOE_TRIES;
+ eoe->state = ec_eoe_state_tx_sent;
+}
+
+/****************************************************************************/
+
+/** State: TX SENT.
+ *
+ * Checks is the previous transmit datagram succeded and sends the next
+ * fragment, if necessary.
+ */
+void ec_eoe_state_tx_sent(ec_eoe_t *eoe /**< EoE handler */)
+{
+ if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
+ if (eoe->tries) {
+ eoe->tries--; // try again
+ eoe->queue_datagram = 1;
+ } else {
+ eoe->stats.tx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Failed to receive send"
+ " datagram for %s after %u tries.\n",
+ eoe->dev->name, EC_EOE_TRIES);
+#endif
+ eoe->state = ec_eoe_state_rx_start;
+ }
+ return;
+ }
+
+ if (eoe->datagram.working_counter != 1) {
+ if (eoe->tries) {
+ eoe->tries--; // try again
+ eoe->queue_datagram = 1;
+ } else {
+ eoe->stats.tx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "No sending response"
+ " for %s after %u tries.\n",
+ eoe->dev->name, EC_EOE_TRIES);
+#endif
+ eoe->state = ec_eoe_state_rx_start;
+ }
+ return;
+ }
+
+ // frame completely sent
+ if (eoe->tx_offset >= eoe->tx_frame->skb->len) {
+ eoe->stats.tx_packets++;
+ eoe->stats.tx_bytes += eoe->tx_frame->skb->len;
+ eoe->tx_counter += eoe->tx_frame->skb->len;
+ dev_kfree_skb(eoe->tx_frame->skb);
+ kfree(eoe->tx_frame);
+ eoe->tx_frame = NULL;
+ eoe->state = ec_eoe_state_rx_start;
+ }
+ else { // send next fragment
+ if (ec_eoe_send(eoe)) {
+ dev_kfree_skb(eoe->tx_frame->skb);
+ kfree(eoe->tx_frame);
+ eoe->tx_frame = NULL;
+ eoe->stats.tx_errors++;
+#if EOE_DEBUG_LEVEL >= 1
+ EC_SLAVE_WARN(eoe->slave, "Send error at %s.\n", eoe->dev->name);
+#endif
+ eoe->state = ec_eoe_state_rx_start;
+ }
+ }
+}
+
+/*****************************************************************************
+ * NET_DEVICE functions
+ ****************************************************************************/
+
+/** Opens the virtual network device.
+ *
+ * \return Always zero (success).
+ */
+int ec_eoedev_open(struct net_device *dev /**< EoE net_device */)
+{
+ ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
+ ec_eoe_flush(eoe);
+ eoe->opened = 1;
+ eoe->rx_idle = 0;
+ eoe->tx_idle = 0;
+ netif_start_queue(dev);
+ eoe->tx_queue_active = 1;
+#if EOE_DEBUG_LEVEL >= 2
+ EC_SLAVE_DBG(eoe->slave, 0, "%s opened.\n", dev->name);
+#endif
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Stops the virtual network device.
+ *
+ * \return Always zero (success).
+ */
+int ec_eoedev_stop(struct net_device *dev /**< EoE net_device */)
+{
+ ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
+ netif_stop_queue(dev);
+ eoe->rx_idle = 1;
+ eoe->tx_idle = 1;
+ eoe->tx_queue_active = 0;
+ eoe->opened = 0;
+ ec_eoe_flush(eoe);
+#if EOE_DEBUG_LEVEL >= 2
+ EC_SLAVE_DBG(eoe->slave, 0, "%s stopped.\n", dev->name);
+#endif
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Transmits data via the virtual network device.
+ *
+ * \return Zero on success, non-zero on failure.
+ */
+int ec_eoedev_tx(struct sk_buff *skb, /**< transmit socket buffer */
+ struct net_device *dev /**< EoE net_device */
+ )
+{
+ ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
+ ec_eoe_frame_t *frame;
+
+#if 0
+ if (skb->len > eoe->slave->configured_tx_mailbox_size - 10) {
+ EC_SLAVE_WARN(eoe->slave, "EoE TX frame (%u octets)"
+ " exceeds MTU. dropping.\n", skb->len);
+ dev_kfree_skb(skb);
+ eoe->stats.tx_dropped++;
+ return 0;
+ }
+#endif
+
+ WARN_ON_ONCE(skb_get_queue_mapping(skb) != 0);
+ lockdep_assert_held(&netdev_get_tx_queue(dev, 0)->_xmit_lock);
+
+ if (!(frame =
+ (ec_eoe_frame_t *) kmalloc(sizeof(ec_eoe_frame_t), GFP_ATOMIC))) {
+ if (printk_ratelimit())
+ EC_SLAVE_WARN(eoe->slave, "EoE TX: low on mem. frame dropped.\n");
+ return 1;
+ }
+
+ frame->skb = skb;
+
+ list_add_tail(&frame->queue, &eoe->tx_queue);
+ eoe->tx_queued_frames++;
+ if (eoe->tx_queued_frames == eoe->tx_queue_size) {
+ netif_stop_queue(dev);
+ eoe->tx_queue_active = 0;
+ }
+
+#if EOE_DEBUG_LEVEL >= 2
+ EC_SLAVE_DBG(eoe->slave, 0, "EoE %s TX queued frame"
+ " with %u octets (%u frames queued).\n",
+ eoe->dev->name, skb->len, eoe->tx_queued_frames);
+ if (!eoe->tx_queue_active)
+ EC_SLAVE_WARN(eoe->slave, "EoE TX queue is now full.\n");
+#endif
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Gets statistics about the virtual network device.
+ *
+ * \return Statistics.
+ */
+struct net_device_stats *ec_eoedev_stats(
+ struct net_device *dev /**< EoE net_device */
+ )
+{
+ ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
+ return &eoe->stats;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/ethernet.h b/ubuntu/igh-ecat/master/ethernet.h
new file mode 100644
index 00000000000000..ef0a8810621a2e
--- /dev/null
+++ b/ubuntu/igh-ecat/master/ethernet.h
@@ -0,0 +1,122 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Ethernet over EtherCAT (EoE)
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_ETHERNET_H__
+#define __EC_ETHERNET_H__
+
+#include <linux/list.h>
+#include <linux/netdevice.h>
+
+#include "globals.h"
+#include "slave.h"
+#include "datagram.h"
+
+/****************************************************************************/
+
+/** EoE frame types.
+ */
+enum {
+ EC_EOE_FRAMETYPE_INIT_REQ = 0x00, /** Initiate EoE Request. */
+ EC_EOE_FRAMETYPE_SET_IP_REQ = 0x02, /** Set IP Parameter Request. */
+ EC_EOE_FRAMETYPE_SET_IP_RES = 0x03, /** Set IP Parameter Response. */
+ EC_EOE_FRAMETYPE_FILT_REQ = 0x04, /** Set Address Filter Request. */
+ EC_EOE_FRAMETYPE_FILT_RES = 0x05, /** Set Address Filter Response. */
+};
+
+/*****************************************************************************/
+
+/**
+ Queued frame structure.
+*/
+
+typedef struct
+{
+ struct list_head queue; /**< list item */
+ struct sk_buff *skb; /**< socket buffer */
+}
+ec_eoe_frame_t;
+
+/****************************************************************************/
+
+typedef struct ec_eoe ec_eoe_t; /**< \see ec_eoe */
+
+/**
+ Ethernet over EtherCAT (EoE) handler.
+ The master creates one of these objects for each slave that supports the
+ EoE protocol.
+*/
+
+struct ec_eoe
+{
+ struct list_head list; /**< list item */
+ ec_slave_t *slave; /**< pointer to the corresponding slave */
+ ec_datagram_t datagram; /**< datagram */
+ unsigned int queue_datagram; /**< the datagram is ready for queuing */
+ void (*state)(ec_eoe_t *); /**< state function for the state machine */
+ struct net_device *dev; /**< net_device for virtual ethernet device */
+ struct net_device_stats stats; /**< device statistics */
+ unsigned int opened; /**< net_device is opened */
+ unsigned long rate_jiffies; /**< time of last rate output */
+
+ struct sk_buff *rx_skb; /**< current rx socket buffer */
+ off_t rx_skb_offset; /**< current write pointer in the socket buffer */
+ size_t rx_skb_size; /**< size of the allocated socket buffer memory */
+ uint8_t rx_expected_fragment; /**< next expected fragment number */
+ uint32_t rx_counter; /**< octets received during last second */
+ uint32_t rx_rate; /**< receive rate (bps) */
+ unsigned int rx_idle; /**< Idle flag. */
+
+ struct list_head tx_queue; /**< queue for frames to send */
+ unsigned int tx_queue_size; /**< Transmit queue size. */
+ unsigned int tx_queue_active; /**< kernel netif queue started */
+ unsigned int tx_queued_frames; /**< number of frames in the queue */
+ ec_eoe_frame_t *tx_frame; /**< current TX frame */
+ uint8_t tx_frame_number; /**< number of the transmitted frame */
+ uint8_t tx_fragment_number; /**< number of the fragment */
+ size_t tx_offset; /**< number of octets sent */
+ uint32_t tx_counter; /**< octets transmitted during last second */
+ uint32_t tx_rate; /**< transmit rate (bps) */
+ unsigned int tx_idle; /**< Idle flag. */
+
+ unsigned int tries; /**< Tries. */
+};
+
+/****************************************************************************/
+
+int ec_eoe_init(ec_eoe_t *, ec_slave_t *);
+void ec_eoe_clear(ec_eoe_t *);
+void ec_eoe_run(ec_eoe_t *);
+void ec_eoe_queue(ec_eoe_t *);
+int ec_eoe_is_open(const ec_eoe_t *);
+int ec_eoe_is_idle(const ec_eoe_t *);
+
+/****************************************************************************/
+
+#endif
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/flag.c b/ubuntu/igh-ecat/master/flag.c
new file mode 100644
index 00000000000000..652d35b1729259
--- /dev/null
+++ b/ubuntu/igh-ecat/master/flag.c
@@ -0,0 +1,69 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2021 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * Slave Configuration Feature Flag.
+ */
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "flag.h"
+
+/****************************************************************************/
+
+/** SDO request constructor.
+ */
+int ec_flag_init(
+ ec_flag_t *flag, /**< Feature flag. */
+ const char *key, /**< Feature key. */
+ int32_t value /**< Feature value. */
+ )
+{
+ if (!key || strlen(key) == 0) {
+ return -EINVAL;
+ }
+
+ if (!(flag->key = (uint8_t *) kmalloc(strlen(key) + 1, GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ strcpy(flag->key, key); // no strncpy, buffer is alloc'ed with strlen
+ flag->value = value;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** SDO request destructor.
+ */
+void ec_flag_clear(
+ ec_flag_t *flag /**< Feature flag. */
+ )
+{
+ if (flag->key) {
+ kfree(flag->key);
+ flag->key = NULL;
+ }
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/flag.h b/ubuntu/igh-ecat/master/flag.h
new file mode 100644
index 00000000000000..e7679c2f2bef99
--- /dev/null
+++ b/ubuntu/igh-ecat/master/flag.h
@@ -0,0 +1,51 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2021 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT Slave Configuration Feature Flag.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FLAG_H__
+#define __EC_FLAG_H__
+
+#include <linux/list.h>
+
+/****************************************************************************/
+
+/** Slave configutation feature flag.
+ */
+typedef struct {
+ struct list_head list; /**< List item. */
+ char *key; /**< Flag key (null-terminated ASCII string. */
+ int32_t value; /**< Flag value (meaning depends on key). */
+} ec_flag_t;
+
+/****************************************************************************/
+
+int ec_flag_init(ec_flag_t *, const char *, int32_t);
+void ec_flag_clear(ec_flag_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fmmu_config.c b/ubuntu/igh-ecat/master/fmmu_config.c
new file mode 100644
index 00000000000000..9e247a46ddbe0b
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fmmu_config.c
@@ -0,0 +1,91 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT FMMU configuration methods.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "slave_config.h"
+#include "master.h"
+
+#include "fmmu_config.h"
+
+/****************************************************************************/
+
+/** FMMU configuration constructor.
+ *
+ * Inits an FMMU configuration, sets the logical start address and adds the
+ * process data size for the mapped PDOs of the given direction to the domain
+ * data size.
+ */
+void ec_fmmu_config_init(
+ ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
+ ec_slave_config_t *sc, /**< EtherCAT slave configuration. */
+ ec_domain_t *domain, /**< EtherCAT domain. */
+ uint8_t sync_index, /**< Sync manager index to use. */
+ ec_direction_t dir /**< PDO direction. */
+ )
+{
+ INIT_LIST_HEAD(&fmmu->list);
+ fmmu->sc = sc;
+ fmmu->sync_index = sync_index;
+ fmmu->dir = dir;
+
+ fmmu->logical_start_address = domain->data_size;
+ fmmu->data_size = ec_pdo_list_total_size(
+ &sc->sync_configs[sync_index].pdos);
+
+ ec_domain_add_fmmu_config(domain, fmmu);
+}
+
+/****************************************************************************/
+
+/** Initializes an FMMU configuration page.
+ *
+ * The referenced memory (\a data) must be at least EC_FMMU_PAGE_SIZE bytes.
+ */
+void ec_fmmu_config_page(
+ const ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
+ const ec_sync_t *sync, /**< Sync manager. */
+ uint8_t *data /**> Configuration page memory. */
+ )
+{
+ EC_CONFIG_DBG(fmmu->sc, 1, "FMMU: LogAddr 0x%08X, Size %3u,"
+ " PhysAddr 0x%04X, SM%u, Dir %s\n",
+ fmmu->logical_start_address, fmmu->data_size,
+ sync->physical_start_address, fmmu->sync_index,
+ fmmu->dir == EC_DIR_INPUT ? "in" : "out");
+
+ EC_WRITE_U32(data, fmmu->logical_start_address);
+ EC_WRITE_U16(data + 4, fmmu->data_size); // size of fmmu
+ EC_WRITE_U8 (data + 6, 0x00); // logical start bit
+ EC_WRITE_U8 (data + 7, 0x07); // logical end bit
+ EC_WRITE_U16(data + 8, sync->physical_start_address);
+ EC_WRITE_U8 (data + 10, 0x00); // physical start bit
+ EC_WRITE_U8 (data + 11, fmmu->dir == EC_DIR_INPUT ? 0x01 : 0x02);
+ EC_WRITE_U16(data + 12, 0x0001); // enable
+ EC_WRITE_U16(data + 14, 0x0000); // reserved
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fmmu_config.h b/ubuntu/igh-ecat/master/fmmu_config.h
new file mode 100644
index 00000000000000..a4c8dc8ab640f6
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fmmu_config.h
@@ -0,0 +1,58 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT FMMU configuration structure.
+ */
+
+/****************************************************************************/
+
+#ifndef __EC_FMMU_CONFIG_H__
+#define __EC_FMMU_CONFIG_H__
+
+#include "globals.h"
+#include "sync.h"
+
+/****************************************************************************/
+
+/** FMMU configuration.
+ */
+typedef struct {
+ struct list_head list; /**< List node used by domain. */
+ const ec_slave_config_t *sc; /**< EtherCAT slave config. */
+ const ec_domain_t *domain; /**< Domain. */
+ uint8_t sync_index; /**< Index of sync manager to use. */
+ ec_direction_t dir; /**< FMMU direction. */
+ uint32_t logical_start_address; /**< Logical start address. */
+ unsigned int data_size; /**< Covered PDO size. */
+} ec_fmmu_config_t;
+
+/****************************************************************************/
+
+void ec_fmmu_config_init(ec_fmmu_config_t *, ec_slave_config_t *,
+ ec_domain_t *, uint8_t, ec_direction_t);
+
+void ec_fmmu_config_page(const ec_fmmu_config_t *, const ec_sync_t *,
+ uint8_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/foe.h b/ubuntu/igh-ecat/master/foe.h
new file mode 100644
index 00000000000000..ae4fc4b1f44ae8
--- /dev/null
+++ b/ubuntu/igh-ecat/master/foe.h
@@ -0,0 +1,54 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * FoE defines.
+ */
+
+#ifndef __FOE_H__
+#define __FOE_H__
+
+/****************************************************************************/
+
+/** FoE error enumeration type.
+ */
+typedef enum {
+ FOE_BUSY = 0, /**< Busy. */
+ FOE_READY = 1, /**< Ready. */
+ FOE_IDLE = 2, /**< Idle. */
+ FOE_WC_ERROR = 3, /**< Working counter error. */
+ FOE_RECEIVE_ERROR = 4, /**< Receive error. */
+ FOE_PROT_ERROR = 5, /**< Protocol error. */
+ FOE_NODATA_ERROR = 6, /**< No data error. */
+ FOE_PACKETNO_ERROR = 7, /**< Packet number error. */
+ FOE_OPCODE_ERROR = 8, /**< OpCode error. */
+ FOE_TIMEOUT_ERROR = 9, /**< Timeout error. */
+ FOE_SEND_RX_DATA_ERROR = 10, /**< Error sending received data. */
+ FOE_RX_DATA_ACK_ERROR = 11, /**< Error acknowledging received data. */
+ FOE_ACK_ERROR = 12, /**< Acknowledge error. */
+ FOE_MBOX_FETCH_ERROR = 13, /**< Error fetching data from mailbox. */
+ FOE_READ_NODATA_ERROR = 14, /**< No data while reading. */
+ FOE_MBOX_PROT_ERROR = 15, /**< Mailbox protocol error. */
+} ec_foe_error_t;
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/foe_request.c b/ubuntu/igh-ecat/master/foe_request.c
new file mode 100644
index 00000000000000..3b68f734c57910
--- /dev/null
+++ b/ubuntu/igh-ecat/master/foe_request.c
@@ -0,0 +1,195 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
+ * Copyright (C) 2020 Florian Pose, IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * File-over-EtherCAT request functions.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include "foe_request.h"
+#include "foe.h"
+
+/****************************************************************************/
+
+/** Default timeout in ms to wait for FoE transfer responses.
+ */
+#define EC_FOE_REQUEST_RESPONSE_TIMEOUT 3000
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_foe_request_clear_data(ec_foe_request_t *);
+
+/****************************************************************************/
+
+/** FoE request constructor.
+ */
+void ec_foe_request_init(
+ ec_foe_request_t *req, /**< FoE request. */
+ uint8_t* file_name /** filename */)
+{
+ INIT_LIST_HEAD(&req->list);
+ req->buffer = NULL;
+ req->file_name = file_name;
+ req->buffer_size = 0;
+ req->data_size = 0;
+ req->dir = EC_DIR_INVALID;
+ req->issue_timeout = 0; // no timeout
+ req->response_timeout = EC_FOE_REQUEST_RESPONSE_TIMEOUT;
+ req->state = EC_INT_REQUEST_INIT;
+ req->result = FOE_BUSY;
+ req->error_code = 0x00000000;
+}
+
+/****************************************************************************/
+
+/** FoE request destructor.
+ */
+void ec_foe_request_clear(
+ ec_foe_request_t *req /**< FoE request. */
+ )
+{
+ ec_foe_request_clear_data(req);
+}
+
+/****************************************************************************/
+
+/** FoE request destructor.
+ */
+void ec_foe_request_clear_data(
+ ec_foe_request_t *req /**< FoE request. */
+ )
+{
+ if (req->buffer) {
+ vfree(req->buffer);
+ req->buffer = NULL;
+ }
+
+ req->buffer_size = 0;
+ req->data_size = 0;
+}
+
+/****************************************************************************/
+
+/** Pre-allocates the data memory.
+ *
+ * If the internal \a buffer_size is already bigger than \a size, nothing is
+ * done.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_foe_request_alloc(
+ ec_foe_request_t *req, /**< FoE request. */
+ size_t size /**< Data size to allocate. */
+ )
+{
+ if (size <= req->buffer_size) {
+ return 0;
+ }
+
+ ec_foe_request_clear_data(req);
+
+ if (!(req->buffer = (uint8_t *) vmalloc(size))) {
+ EC_ERR("Failed to allocate %zu bytes of FoE memory.\n", size);
+ return -ENOMEM;
+ }
+
+ req->buffer_size = size;
+ req->data_size = 0;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Copies FoE data from an external source.
+ *
+ * If the \a buffer_size is to small, new memory is allocated.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_foe_request_copy_data(
+ ec_foe_request_t *req, /**< FoE request. */
+ const uint8_t *source, /**< Source data. */
+ size_t size /**< Number of bytes in \a source. */
+ )
+{
+ int ret;
+
+ ret = ec_foe_request_alloc(req, size);
+ if (ret) {
+ return ret;
+ }
+
+ memcpy(req->buffer, source, size);
+ req->data_size = size;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Checks, if the timeout was exceeded.
+ *
+ * \return non-zero if the timeout was exceeded, else zero.
+ */
+int ec_foe_request_timed_out(
+ const ec_foe_request_t *req /**< FoE request. */
+ )
+{
+ return req->issue_timeout
+ && jiffies - req->jiffies_start > HZ * req->issue_timeout / 1000;
+}
+
+/****************************************************************************/
+
+/** Prepares a read request (slave to master).
+ */
+void ec_foe_request_read(
+ ec_foe_request_t *req /**< FoE request. */
+ )
+{
+ req->dir = EC_DIR_INPUT;
+ req->state = EC_INT_REQUEST_QUEUED;
+ req->result = FOE_BUSY;
+ req->jiffies_start = jiffies;
+}
+
+/****************************************************************************/
+
+/** Prepares a write request (master to slave).
+ */
+void ec_foe_request_write(
+ ec_foe_request_t *req /**< FoE request. */
+ )
+{
+ req->dir = EC_DIR_OUTPUT;
+ req->state = EC_INT_REQUEST_QUEUED;
+ req->result = FOE_BUSY;
+ req->jiffies_start = jiffies;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/foe_request.h b/ubuntu/igh-ecat/master/foe_request.h
new file mode 100644
index 00000000000000..ba5ceb661e99d1
--- /dev/null
+++ b/ubuntu/igh-ecat/master/foe_request.h
@@ -0,0 +1,78 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT FoE request structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FOE_REQUEST_H__
+#define __EC_FOE_REQUEST_H__
+
+#include <linux/list.h>
+
+#include "../include/ecrt.h"
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** FoE request.
+ */
+typedef struct {
+ struct list_head list; /**< List item. */
+ uint8_t *buffer; /**< Pointer to FoE data. */
+ size_t buffer_size; /**< Size of FoE data memory. */
+ size_t data_size; /**< Size of FoE data. */
+
+ uint32_t issue_timeout; /**< Maximum time in ms, the processing of the
+ request may take. */
+ uint32_t response_timeout; /**< Maximum time in ms, the transfer is
+ retried, if the slave does not respond. */
+ ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means downloading to
+ the slave, EC_DIR_INPUT means uploading from the
+ slave. */
+ ec_internal_request_state_t state; /**< FoE request state. */
+ unsigned long jiffies_start; /**< Jiffies, when the request was issued. */
+ unsigned long jiffies_sent; /**< Jiffies, when the upload/download
+ request was sent. */
+ uint8_t *file_name; /**< Pointer to the filename. */
+ uint32_t result; /**< FoE request abort code. Zero on success. */
+ uint32_t error_code; /**< Error code from an FoE Error Request. */
+} ec_foe_request_t;
+
+/****************************************************************************/
+
+void ec_foe_request_init(ec_foe_request_t *, uint8_t *file_name);
+void ec_foe_request_clear(ec_foe_request_t *);
+
+int ec_foe_request_alloc(ec_foe_request_t *, size_t);
+int ec_foe_request_copy_data(ec_foe_request_t *, const uint8_t *, size_t);
+int ec_foe_request_timed_out(const ec_foe_request_t *);
+
+void ec_foe_request_write(ec_foe_request_t *);
+void ec_foe_request_read(ec_foe_request_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_change.c b/ubuntu/igh-ecat/master/fsm_change.c
new file mode 100644
index 00000000000000..98fae1847acf33
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_change.c
@@ -0,0 +1,615 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT state change FSM.
+*/
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "fsm_change.h"
+#include "slave_config.h"
+
+/****************************************************************************/
+
+unsigned int ec_fsm_change_timeout_ms(const ec_fsm_change_t *);
+
+void ec_fsm_change_state_start(ec_fsm_change_t *);
+void ec_fsm_change_state_check(ec_fsm_change_t *);
+void ec_fsm_change_state_status(ec_fsm_change_t *);
+void ec_fsm_change_state_start_code(ec_fsm_change_t *);
+void ec_fsm_change_state_code(ec_fsm_change_t *);
+void ec_fsm_change_state_ack(ec_fsm_change_t *);
+void ec_fsm_change_state_check_ack(ec_fsm_change_t *);
+void ec_fsm_change_state_end(ec_fsm_change_t *);
+void ec_fsm_change_state_error(ec_fsm_change_t *);
+
+/****************************************************************************/
+
+/**
+ Constructor.
+*/
+
+void ec_fsm_change_init(ec_fsm_change_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< datagram */
+ )
+{
+ fsm->state = NULL;
+ fsm->datagram = datagram;
+ fsm->spontaneous_change = 0;
+}
+
+/****************************************************************************/
+
+/**
+ Destructor.
+*/
+
+void ec_fsm_change_clear(ec_fsm_change_t *fsm /**< finite state machine */)
+{
+}
+
+/****************************************************************************/
+
+/** Get timeout in ms.
+ *
+ * For defaults see ETG2000_S_R_V1i0i15 section 5.3.6.2.
+ */
+unsigned int ec_fsm_change_timeout_ms(
+ const ec_fsm_change_t *fsm /**< finite state machine */
+ )
+{
+ ec_slave_state_t from = fsm->old_state;
+ ec_slave_state_t to = fsm->requested_state;
+
+ /* Search for specified timeout in slave configuration */
+ if (fsm->slave->config) {
+ unsigned int timeout_ms =
+ ec_slave_config_al_timeout(fsm->slave->config, from, to);
+ if (timeout_ms) {
+ return timeout_ms;
+ }
+ }
+
+ /* No specific timeout found. Use defaults from spec. */
+
+ if (from == EC_SLAVE_STATE_INIT &&
+ (to == EC_SLAVE_STATE_PREOP || to == EC_SLAVE_STATE_BOOT)) {
+ return 3000; // PreopTimeout
+ }
+ if ((from == EC_SLAVE_STATE_PREOP && to == EC_SLAVE_STATE_SAFEOP) ||
+ (from == EC_SLAVE_STATE_SAFEOP && to == EC_SLAVE_STATE_OP)) {
+ return 10000; // SafeopOpTimeout
+ }
+ if (to == EC_SLAVE_STATE_INIT ||
+ ((from == EC_SLAVE_STATE_OP || from == EC_SLAVE_STATE_SAFEOP)
+ && to == EC_SLAVE_STATE_PREOP)) {
+ return 5000; // BackToInitTimeout
+ }
+ if (from == EC_SLAVE_STATE_OP && to == EC_SLAVE_STATE_SAFEOP) {
+ return 200; // BackToSafeopTimeout
+ }
+
+ return 10000; // default [ms]
+}
+
+/****************************************************************************/
+
+/**
+ Starts the change state machine.
+*/
+
+void ec_fsm_change_start(ec_fsm_change_t *fsm, /**< finite state machine */
+ ec_slave_t *slave, /**< EtherCAT slave */
+ ec_slave_state_t state /**< requested state */
+ )
+{
+ fsm->mode = EC_FSM_CHANGE_MODE_FULL;
+ fsm->slave = slave;
+ fsm->requested_state = state;
+ fsm->state = ec_fsm_change_state_start;
+}
+
+/****************************************************************************/
+
+/**
+ Starts the change state machine to only acknowlegde a slave's state.
+*/
+
+void ec_fsm_change_ack(ec_fsm_change_t *fsm, /**< finite state machine */
+ ec_slave_t *slave /**< EtherCAT slave */
+ )
+{
+ fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;
+ fsm->slave = slave;
+ fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;
+ fsm->state = ec_fsm_change_state_start_code;
+}
+
+/****************************************************************************/
+
+/**
+ Executes the current state of the state machine.
+ \return false, if the state machine has terminated
+*/
+
+int ec_fsm_change_exec(ec_fsm_change_t *fsm /**< finite state machine */)
+{
+ fsm->state(fsm);
+
+ return fsm->state != ec_fsm_change_state_end
+ && fsm->state != ec_fsm_change_state_error;
+}
+
+/****************************************************************************/
+
+/**
+ Returns, if the state machine terminated with success.
+ \return non-zero if successful.
+*/
+
+int ec_fsm_change_success(ec_fsm_change_t *fsm /**< Finite state machine */)
+{
+ return fsm->state == ec_fsm_change_state_end;
+}
+
+/*****************************************************************************
+ * state change state machine
+ ****************************************************************************/
+
+/**
+ Change state: START.
+*/
+
+void ec_fsm_change_state_start(ec_fsm_change_t *fsm
+ /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ fsm->take_time = 1;
+ fsm->old_state = fsm->slave->current_state;
+
+ // write new state to slave
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0120, 2);
+ EC_WRITE_U16(datagram->data, fsm->requested_state);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_change_state_check;
+}
+
+/****************************************************************************/
+
+/**
+ Change state: CHECK.
+*/
+
+void ec_fsm_change_state_check(ec_fsm_change_t *fsm
+ /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive state datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (fsm->take_time) {
+ fsm->take_time = 0;
+ fsm->jiffies_start = datagram->jiffies_sent;
+ }
+
+ if (datagram->working_counter == 0) {
+ if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) {
+ char state_str[EC_STATE_STRING_SIZE];
+ ec_state_string(fsm->requested_state, state_str, 0);
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set state %s: ", state_str);
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ // repeat writing new state to slave
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0120, 2);
+ EC_WRITE_U16(datagram->data, fsm->requested_state);
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ if (unlikely(datagram->working_counter > 1)) {
+ char state_str[EC_STATE_STRING_SIZE];
+ ec_state_string(fsm->requested_state, state_str, 0);
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set state %s: ", state_str);
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ fsm->take_time = 1;
+
+ // read AL status from slave
+ ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->spontaneous_change = 0;
+ fsm->state = ec_fsm_change_state_status;
+}
+
+/****************************************************************************/
+
+/**
+ Change state: STATUS.
+*/
+
+void ec_fsm_change_state_status(ec_fsm_change_t *fsm
+ /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ unsigned int timeout_ms;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive state checking datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ char req_state[EC_STATE_STRING_SIZE];
+ ec_state_string(fsm->requested_state, req_state, 0);
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Failed to check state %s: ", req_state);
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ if (fsm->take_time) {
+ fsm->take_time = 0;
+ fsm->jiffies_start = datagram->jiffies_sent;
+ }
+
+ slave->current_state = EC_READ_U8(datagram->data);
+
+ if (slave->current_state == fsm->requested_state) {
+ // state has been set successfully
+ fsm->state = ec_fsm_change_state_end;
+ return;
+ }
+
+ if (slave->current_state != fsm->old_state) { // state changed
+ char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];
+
+ ec_state_string(slave->current_state, cur_state, 0);
+
+ if ((slave->current_state & 0x0F) != (fsm->old_state & 0x0F)) {
+ // Slave spontaneously changed its state just before the new state
+ // was written. Accept current state as old state and wait for
+ // state change
+ fsm->spontaneous_change = 1;
+ fsm->old_state = slave->current_state;
+ EC_SLAVE_WARN(slave, "Changed to %s in the meantime.\n",
+ cur_state);
+ goto check_again;
+ }
+
+ // state change error
+
+ slave->error_flag = 1;
+ ec_state_string(fsm->requested_state, req_state, 0);
+
+ EC_SLAVE_ERR(slave, "Failed to set %s state, slave refused state"
+ " change (%s).\n", req_state, cur_state);
+
+ ec_fsm_change_state_start_code(fsm);
+ return;
+ }
+
+ // still old state
+
+ timeout_ms = ec_fsm_change_timeout_ms(fsm);
+ if (datagram->jiffies_received - fsm->jiffies_start >=
+ timeout_ms * HZ / 1000) {
+ // timeout while checking
+ char state_str[EC_STATE_STRING_SIZE];
+ ec_state_string(fsm->requested_state, state_str, 0);
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Timeout after %u ms while setting state %s.\n",
+ timeout_ms, state_str);
+ return;
+ }
+
+ check_again:
+ // no timeout yet. check again
+ ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+}
+
+/****************************************************************************/
+
+/** Enter reading AL status code.
+ */
+void ec_fsm_change_state_start_code(
+ ec_fsm_change_t *fsm /**< finite state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+
+ // fetch AL status error code
+ ec_datagram_fprd(datagram, slave->station_address, 0x0134, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_change_state_code;
+}
+
+/****************************************************************************/
+
+/**
+ Application layer status messages.
+*/
+
+const ec_code_msg_t al_status_messages[] = {
+ {0x0000, "No error"},
+ {0x0001, "Unspecified error"},
+ {0x0002, "No Memory"},
+ {0x0011, "Invalid requested state change"},
+ {0x0012, "Unknown requested state"},
+ {0x0013, "Bootstrap not supported"},
+ {0x0014, "No valid firmware"},
+ {0x0015, "Invalid mailbox configuration"},
+ {0x0016, "Invalid mailbox configuration"},
+ {0x0017, "Invalid sync manager configuration"},
+ {0x0018, "No valid inputs available"},
+ {0x0019, "No valid outputs"},
+ {0x001A, "Synchronization error"},
+ {0x001B, "Sync manager watchdog"},
+ {0x001C, "Invalid sync manager types"},
+ {0x001D, "Invalid output configuration"},
+ {0x001E, "Invalid input configuration"},
+ {0x001F, "Invalid watchdog configuration"},
+ {0x0020, "Slave needs cold start"},
+ {0x0021, "Slave needs INIT"},
+ {0x0022, "Slave needs PREOP"},
+ {0x0023, "Slave needs SAFEOP"},
+ {0x0024, "Invalid Input Mapping"},
+ {0x0025, "Invalid Output Mapping"},
+ {0x0026, "Inconsistent Settings"},
+ {0x0027, "Freerun not supported"},
+ {0x0028, "Synchronization not supported"},
+ {0x0029, "Freerun needs 3 Buffer Mode"},
+ {0x002A, "Background Watchdog"},
+ {0x002B, "No Valid Inputs and Outputs"},
+ {0x002C, "Fatal Sync Error"},
+ {0x002D, "No Sync Error"},
+ {0x0030, "Invalid DC SYNCH configuration"},
+ {0x0031, "Invalid DC latch configuration"},
+ {0x0032, "PLL error"},
+ {0x0033, "DC Sync IO Error"},
+ {0x0034, "DC Sync Timeout Error"},
+ {0x0035, "DC Invalid Sync Cycle Time"},
+ {0x0036, "DC Sync0 Cycle Time"},
+ {0x0037, "DC Sync1 Cycle Time"},
+ {0x0041, "MBX_AOE"},
+ {0x0042, "MBX_EOE"},
+ {0x0043, "MBX_COE"},
+ {0x0044, "MBX_FOE"},
+ {0x0045, "MBX_SOE"},
+ {0x004F, "MBX_VOE"},
+ {0x0050, "EEPROM No Access"},
+ {0x0051, "EEPROM Error"},
+ {0x0060, "Slave Restarted Locally"},
+ {0xffff}
+};
+
+
+/****************************************************************************/
+
+/**
+ Change state: CODE.
+*/
+
+void ec_fsm_change_state_code(ec_fsm_change_t *fsm
+ /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ uint32_t code;
+ const ec_code_msg_t *al_msg;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed to receive"
+ " AL status code datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_WARN(fsm->slave, "Reception of AL status code"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ } else {
+ code = EC_READ_U16(datagram->data);
+ for (al_msg = al_status_messages; al_msg->code != 0xffff; al_msg++) {
+ if (al_msg->code != code) {
+ continue;
+ }
+
+ EC_SLAVE_ERR(fsm->slave, "AL status message 0x%04X: \"%s\".\n",
+ al_msg->code, al_msg->message);
+ break;
+ }
+ if (al_msg->code == 0xffff) { /* not found in our list. */
+ EC_SLAVE_ERR(fsm->slave, "Unknown AL status code 0x%04X.\n",
+ code);
+ }
+ }
+
+ // acknowledge "old" slave state
+ ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x0120, 2);
+ EC_WRITE_U16(datagram->data, fsm->slave->current_state);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_change_state_ack;
+}
+
+/****************************************************************************/
+
+/**
+ Change state: ACK.
+*/
+
+void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive state ack datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Reception of state ack datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ fsm->take_time = 1;
+
+ // read new AL status
+ ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_change_state_check_ack;
+}
+
+/****************************************************************************/
+
+/**
+ Change state: CHECK ACK.
+*/
+
+void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm
+ /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ unsigned int timeout_ms;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive state ack check datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Reception of state ack check datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ if (fsm->take_time) {
+ fsm->take_time = 0;
+ fsm->jiffies_start = datagram->jiffies_sent;
+ }
+
+ slave->current_state = EC_READ_U8(datagram->data);
+
+ if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) {
+ char state_str[EC_STATE_STRING_SIZE];
+ ec_state_string(slave->current_state, state_str, 0);
+ if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) {
+ fsm->state = ec_fsm_change_state_error;
+ }
+ else { // EC_FSM_CHANGE_MODE_ACK_ONLY
+ fsm->state = ec_fsm_change_state_end;
+ }
+ EC_SLAVE_INFO(slave, "Acknowledged state %s.\n", state_str);
+ return;
+ }
+
+ timeout_ms = ec_fsm_change_timeout_ms(fsm);
+ if (datagram->jiffies_received - fsm->jiffies_start >=
+ timeout_ms * HZ / 1000) {
+ // timeout while checking
+ char state_str[EC_STATE_STRING_SIZE];
+ ec_state_string(slave->current_state, state_str, 0);
+ fsm->state = ec_fsm_change_state_error;
+ EC_SLAVE_ERR(slave, "Timeout after %u ms while acknowledging"
+ " state %s.\n", timeout_ms, state_str);
+ return;
+ }
+
+ // reread new AL status
+ ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+}
+
+/****************************************************************************/
+
+/**
+ State: ERROR.
+*/
+
+void ec_fsm_change_state_error(ec_fsm_change_t *fsm
+ /**< finite state machine */)
+{
+}
+
+/****************************************************************************/
+
+/**
+ State: END.
+*/
+
+void ec_fsm_change_state_end(ec_fsm_change_t *fsm
+ /**< finite state machine */)
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_change.h b/ubuntu/igh-ecat/master/fsm_change.h
new file mode 100644
index 00000000000000..c30ac257847912
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_change.h
@@ -0,0 +1,84 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT state change FSM.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_CHANGE_H__
+#define __EC_FSM_CHANGE_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "slave.h"
+
+/****************************************************************************/
+
+/**
+ Mode of the change state machine.
+*/
+
+typedef enum {
+ EC_FSM_CHANGE_MODE_FULL, /**< full state change */
+ EC_FSM_CHANGE_MODE_ACK_ONLY /**< only state acknowledgement */
+}
+ec_fsm_change_mode_t;
+
+/****************************************************************************/
+
+typedef struct ec_fsm_change ec_fsm_change_t; /**< \see ec_fsm_change */
+
+/**
+ EtherCAT state change FSM.
+*/
+
+struct ec_fsm_change
+{
+ ec_slave_t *slave; /**< slave the FSM runs on */
+ ec_datagram_t *datagram; /**< datagram used in the state machine */
+ unsigned int retries; /**< retries upon datagram timeout */
+
+ void (*state)(ec_fsm_change_t *); /**< slave state change state function */
+ ec_fsm_change_mode_t mode; /**< full state change, or ack only. */
+ ec_slave_state_t requested_state; /**< input: state */
+ ec_slave_state_t old_state; /**< prior slave state */
+ unsigned long jiffies_start; /**< change timer */
+ uint8_t take_time; /**< take sending timestamp */
+ uint8_t spontaneous_change; /**< spontaneous state change detected */
+};
+
+/****************************************************************************/
+
+void ec_fsm_change_init(ec_fsm_change_t *, ec_datagram_t *);
+void ec_fsm_change_clear(ec_fsm_change_t *);
+
+void ec_fsm_change_start(ec_fsm_change_t *, ec_slave_t *, ec_slave_state_t);
+void ec_fsm_change_ack(ec_fsm_change_t *, ec_slave_t *);
+
+int ec_fsm_change_exec(ec_fsm_change_t *);
+int ec_fsm_change_success(ec_fsm_change_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_coe.c b/ubuntu/igh-ecat/master/fsm_coe.c
new file mode 100644
index 00000000000000..69765cd006ebac
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_coe.c
@@ -0,0 +1,2537 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT CoE state machines.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "fsm_coe.h"
+#include "slave_config.h"
+
+/****************************************************************************/
+
+/** Maximum time in ms to wait for responses when reading out the dictionary.
+ */
+#define EC_FSM_COE_DICT_TIMEOUT 1000
+
+/** CoE download request header size.
+ */
+#define EC_COE_DOWN_REQ_HEADER_SIZE 10
+
+/** CoE download segment request header size.
+ */
+#define EC_COE_DOWN_SEG_REQ_HEADER_SIZE 3
+
+/** Minimum size of download segment.
+ */
+#define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7
+
+/** Enable debug output for CoE retries.
+ */
+#define DEBUG_RETRIES 0
+
+/** Enable warning output if transfers take too long.
+ */
+#define DEBUG_LONG 0
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_canopen_abort_msg(const ec_slave_t *, uint32_t);
+int ec_fsm_coe_check_emergency(const ec_fsm_coe_t *, const uint8_t *, size_t);
+int ec_fsm_coe_prepare_dict(ec_fsm_coe_t *, ec_datagram_t *);
+int ec_fsm_coe_dict_prepare_desc(ec_fsm_coe_t *, ec_datagram_t *);
+int ec_fsm_coe_dict_prepare_entry(ec_fsm_coe_t *, ec_datagram_t *);
+int ec_fsm_coe_prepare_down_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_prepare_segment_request(ec_fsm_coe_t *, ec_datagram_t *);
+int ec_fsm_coe_prepare_up(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_prepare_segment_request(ec_fsm_coe_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
+
+void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *);
+void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+/** SDO abort messages.
+ *
+ * The "abort SDO transfer request" supplies an abort code, which can be
+ * translated to clear text. This table does the mapping of the codes and
+ * messages.
+ */
+const ec_code_msg_t sdo_abort_messages[] = {
+ {0x05030000, "Toggle bit not changed"},
+ {0x05040000, "SDO protocol timeout"},
+ {0x05040001, "Client/Server command specifier not valid or unknown"},
+ {0x05040005, "Out of memory"},
+ {0x06010000, "Unsupported access to an object"},
+ {0x06010001, "Attempt to read a write-only object"},
+ {0x06010002, "Attempt to write a read-only object"},
+ {0x06020000, "This object does not exist in the object directory"},
+ {0x06040041, "The object cannot be mapped into the PDO"},
+ {0x06040042, "The number and length of the objects to be mapped would"
+ " exceed the PDO length"},
+ {0x06040043, "General parameter incompatibility reason"},
+ {0x06040047, "Gerneral internal incompatibility in device"},
+ {0x06060000, "Access failure due to a hardware error"},
+ {0x06070010, "Data type does not match, length of service parameter does"
+ " not match"},
+ {0x06070012, "Data type does not match, length of service parameter too"
+ " high"},
+ {0x06070013, "Data type does not match, length of service parameter too"
+ " low"},
+ {0x06090011, "Subindex does not exist"},
+ {0x06090030, "Value range of parameter exceeded"},
+ {0x06090031, "Value of parameter written too high"},
+ {0x06090032, "Value of parameter written too low"},
+ {0x06090036, "Maximum value is less than minimum value"},
+ {0x08000000, "General error"},
+ {0x08000020, "Data cannot be transferred or stored to the application"},
+ {0x08000021, "Data cannot be transferred or stored to the application"
+ " because of local control"},
+ {0x08000022, "Data cannot be transferred or stored to the application"
+ " because of the present device state"},
+ {0x08000023, "Object dictionary dynamic generation fails or no object"
+ " dictionary is present"},
+ {}
+};
+
+/****************************************************************************/
+
+/** Outputs an SDO abort message.
+ */
+void ec_canopen_abort_msg(
+ const ec_slave_t *slave, /**< Slave. */
+ uint32_t abort_code /**< Abort code to search for. */
+ )
+{
+ const ec_code_msg_t *abort_msg;
+
+ for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {
+ if (abort_msg->code == abort_code) {
+ EC_SLAVE_ERR(slave, "SDO abort message 0x%08X: \"%s\".\n",
+ abort_msg->code, abort_msg->message);
+ return;
+ }
+ }
+
+ EC_SLAVE_ERR(slave, "Unknown SDO abort code 0x%08X.\n", abort_code);
+}
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_coe_init(
+ ec_fsm_coe_t *fsm /**< Finite state machine */
+ )
+{
+ fsm->state = NULL;
+ fsm->datagram = NULL;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_coe_clear(
+ ec_fsm_coe_t *fsm /**< Finite state machine */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** Starts reading a slaves' SDO dictionary.
+ */
+void ec_fsm_coe_dictionary(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_slave_t *slave /**< EtherCAT slave */
+ )
+{
+ fsm->slave = slave;
+ fsm->state = ec_fsm_coe_dict_start;
+}
+
+/****************************************************************************/
+
+/** Starts to transfer an SDO to/from a slave.
+ */
+void ec_fsm_coe_transfer(
+ ec_fsm_coe_t *fsm, /**< State machine. */
+ ec_slave_t *slave, /**< EtherCAT slave. */
+ ec_sdo_request_t *request /**< SDO request. */
+ )
+{
+ fsm->slave = slave;
+ fsm->request = request;
+
+ if (request->dir == EC_DIR_OUTPUT) {
+ fsm->state = ec_fsm_coe_down_start;
+ }
+ else {
+ fsm->state = ec_fsm_coe_up_start;
+ }
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_coe_exec(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ * \return non-zero if successful.
+ */
+int ec_fsm_coe_success(
+ const ec_fsm_coe_t *fsm /**< Finite state machine */
+ )
+{
+ return fsm->state == ec_fsm_coe_end;
+}
+
+/****************************************************************************/
+
+/** Check if the received data are a CoE emergency request.
+ *
+ * If the check is positive, the emergency request is output.
+ *
+ * \return The data were an emergency request.
+ */
+int ec_fsm_coe_check_emergency(
+ const ec_fsm_coe_t *fsm, /**< Finite state machine */
+ const uint8_t *data, /**< CoE mailbox data. */
+ size_t size /**< CoE mailbox data size. */
+ )
+{
+ if (size < 2 || ((EC_READ_U16(data) >> 12) & 0x0F) != 0x01)
+ return 0;
+
+ if (size < 10) {
+ EC_SLAVE_WARN(fsm->slave, "Received incomplete CoE Emergency"
+ " request:\n");
+ ec_print_data(data, size);
+ return 1;
+ }
+
+ {
+ ec_slave_config_t *sc = fsm->slave->config;
+ if (sc) {
+ ec_coe_emerg_ring_push(&sc->emerg_ring, data + 2);
+ }
+ }
+
+ EC_SLAVE_WARN(fsm->slave, "CoE Emergency Request received:\n"
+ "Error code 0x%04X, Error register 0x%02X, data:\n",
+ EC_READ_U16(data + 2), EC_READ_U8(data + 4));
+ ec_print_data(data + 5, 5);
+ return 1;
+}
+
+/*****************************************************************************
+ * CoE dictionary state machine
+ ****************************************************************************/
+
+/** Prepare a dictionary request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_fsm_coe_prepare_dict(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint8_t *data = ec_slave_mbox_prepare_send(slave, datagram,
+ EC_MBOX_TYPE_COE, 8);
+ if (IS_ERR(data)) {
+ return PTR_ERR(data);
+ }
+
+ EC_WRITE_U16(data, 0x8 << 12); // SDO information
+ EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request
+ EC_WRITE_U8 (data + 3, 0x00);
+ EC_WRITE_U16(data + 4, 0x0000);
+ EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!
+
+ fsm->state = ec_fsm_coe_dict_request;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** CoE state: DICT START.
+ */
+void ec_fsm_coe_dict_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) {
+ EC_SLAVE_ERR(slave, "Slave does not support"
+ " SDO information service!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_coe_prepare_dict(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+}
+
+/****************************************************************************/
+
+/** CoE state: DICT REQUEST.
+ * \todo Timeout behavior
+ */
+void ec_fsm_coe_dict_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_prepare_dict(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
+ " request datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_check;
+}
+
+/****************************************************************************/
+
+/** CoE state: DICT CHECK.
+ */
+void ec_fsm_coe_dict_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Timeout while waiting for"
+ " SDO dictionary list response.\n");
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_response;
+}
+
+/****************************************************************************/
+
+/** Prepare an object description request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_fsm_coe_dict_prepare_desc(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ u8 *data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_COE,
+ 8);
+ if (IS_ERR(data)) {
+ return PTR_ERR(data);
+ }
+
+ EC_WRITE_U16(data, 0x8 << 12); // SDO information
+ EC_WRITE_U8 (data + 2, 0x03); // Get object description request
+ EC_WRITE_U8 (data + 3, 0x00);
+ EC_WRITE_U16(data + 4, 0x0000);
+ EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
+
+ fsm->state = ec_fsm_coe_dict_desc_request;
+ return 0;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DICT RESPONSE.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_dict_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint8_t *data, mbox_prot;
+ size_t rec_size;
+ unsigned int sdo_count, i;
+ uint16_t sdo_index, fragments_left;
+ ec_sdo_t *sdo;
+ bool first_segment;
+ size_t index_list_offset;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
+ " response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_COE) {
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_check;
+ return;
+ }
+
+ if (rec_size < 3) {
+ EC_SLAVE_ERR(slave, "Received corrupted SDO dictionary response"
+ " (size %zu).\n", rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
+ (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
+ EC_SLAVE_ERR(slave, "SDO information error response!\n");
+ if (rec_size < 10) {
+ EC_SLAVE_ERR(slave, "Incomplete SDO information"
+ " error response:\n");
+ ec_print_data(data, rec_size);
+ } else {
+ ec_canopen_abort_msg(slave, EC_READ_U32(data + 6));
+ }
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
+ (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!"
+ " Retrying...\n");
+ ec_print_data(data, rec_size);
+ }
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_check;
+ return;
+ }
+
+ first_segment = list_empty(&slave->sdo_dictionary) ? true : false;
+ index_list_offset = first_segment ? 8 : 6;
+
+ if (rec_size < index_list_offset || rec_size % 2) {
+ EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size);
+ ec_print_data(data, rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ sdo_count = (rec_size - index_list_offset) / 2;
+
+ for (i = 0; i < sdo_count; i++) {
+ sdo_index = EC_READ_U16(data + index_list_offset + i * 2);
+ if (!sdo_index) {
+ EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n");
+ continue;
+ }
+
+ if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate memory for SDO!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ ec_sdo_init(sdo, slave, sdo_index);
+ list_add_tail(&sdo->list, &slave->sdo_dictionary);
+ }
+
+ fragments_left = EC_READ_U16(data + 4);
+ if (fragments_left) {
+ EC_SLAVE_DBG(slave, 1, "SDO list fragments left: %u\n",
+ fragments_left);
+ }
+
+ if (EC_READ_U8(data + 2) & 0x80 || fragments_left) {
+ // more messages waiting. check again.
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_check;
+ return;
+ }
+
+ if (list_empty(&slave->sdo_dictionary)) {
+ // no SDOs in dictionary. finished.
+ fsm->state = ec_fsm_coe_end; // success
+ return;
+ }
+
+ // fetch SDO descriptions
+ fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);
+
+ fsm->retries = EC_FSM_RETRIES;
+ if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DICT DESC REQUEST.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_dict_desc_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
+ " description request datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
+ " request failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_desc_check;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DICT DESC CHECK.
+*/
+
+void ec_fsm_coe_dict_desc_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Timeout while waiting for"
+ " SDO 0x%04x object description response.\n",
+ fsm->sdo->index);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_desc_response;
+}
+
+/****************************************************************************/
+
+/** Prepare an entry description request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_fsm_coe_dict_prepare_entry(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ u8 *data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_COE,
+ 10);
+ if (IS_ERR(data)) {
+ return PTR_ERR(data);
+ }
+
+ EC_WRITE_U16(data, 0x8 << 12); // SDO information
+ EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
+ EC_WRITE_U8 (data + 3, 0x00);
+ EC_WRITE_U16(data + 4, 0x0000);
+ EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index
+ EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex
+ EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only)
+
+ fsm->state = ec_fsm_coe_dict_entry_request;
+ return 0;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DICT DESC RESPONSE.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_dict_desc_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_t *sdo = fsm->sdo;
+ uint8_t *data, mbox_prot;
+ size_t rec_size, name_size;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description"
+ " response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
+ " response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_COE) {
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_desc_check;
+ return;
+ }
+
+ if (rec_size < 3) {
+ EC_SLAVE_ERR(slave, "Received corrupted SDO description response"
+ " (size %zu).\n", rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
+ (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
+ EC_SLAVE_ERR(slave, "SDO information error response while"
+ " fetching SDO 0x%04X!\n", sdo->index);
+ ec_canopen_abort_msg(slave, EC_READ_U32(data + 6));
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (rec_size < 8) {
+ EC_SLAVE_ERR(slave, "Received corrupted SDO"
+ " description response (size %zu).\n", rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
+ (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response
+ EC_READ_U16(data + 6) != sdo->index) { // SDO index
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Invalid object description response while"
+ " fetching SDO 0x%04X!\n", sdo->index);
+ ec_print_data(data, rec_size);
+ }
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_desc_check;
+ return;
+ }
+
+ if (rec_size < 12) {
+ EC_SLAVE_ERR(slave, "Invalid data size!\n");
+ ec_print_data(data, rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ sdo->max_subindex = EC_READ_U8(data + 10);
+ sdo->object_code = EC_READ_U8(data + 11);
+
+ name_size = rec_size - 12;
+ if (name_size) {
+ if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate SDO name!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ memcpy(sdo->name, data + 12, name_size);
+ sdo->name[name_size] = 0;
+ }
+
+ if (EC_READ_U8(data + 2) & 0x80) {
+ EC_SLAVE_ERR(slave, "Fragment follows (not implemented)!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ // start fetching entries
+
+ fsm->subindex = 0;
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DICT ENTRY REQUEST.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_dict_entry_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry"
+ " request datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_entry_check;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DICT ENTRY CHECK.
+*/
+
+void ec_fsm_coe_dict_entry_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Timeout while waiting for"
+ " SDO entry 0x%04x:%x description response.\n",
+ fsm->sdo->index, fsm->subindex);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_entry_response;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DICT ENTRY RESPONSE.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_dict_entry_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_t *sdo = fsm->sdo;
+ uint8_t *data, mbox_prot;
+ size_t rec_size, data_size;
+ ec_sdo_entry_t *entry;
+ u16 word;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
+ " description response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
+ " response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_COE) {
+ EC_SLAVE_ERR(slave, "Received mailbox protocol"
+ " 0x%02X as response.\n", mbox_prot);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_entry_check;
+ return;
+ }
+
+ if (rec_size < 3) {
+ EC_SLAVE_ERR(slave, "Received corrupted SDO entry"
+ " description response (size %zu).\n", rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
+ (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
+ EC_SLAVE_WARN(slave, "SDO information error response while"
+ " fetching SDO entry 0x%04X:%02X!\n",
+ sdo->index, fsm->subindex);
+ ec_canopen_abort_msg(slave, EC_READ_U32(data + 6));
+
+ /* There may be gaps in the subindices, so try to continue with next
+ * subindex. */
+
+ } else {
+
+ if (rec_size < 9) {
+ EC_SLAVE_ERR(slave, "Received corrupted SDO entry"
+ " description response (size %zu).\n", rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
+ (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response
+ EC_READ_U16(data + 6) != sdo->index || // SDO index
+ EC_READ_U8(data + 8) != fsm->subindex) { // SDO subindex
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Invalid entry description response"
+ " while fetching SDO entry 0x%04X:%02X!\n",
+ sdo->index, fsm->subindex);
+ ec_print_data(data, rec_size);
+ }
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_dict_entry_check;
+ return;
+ }
+
+ if (rec_size < 16) {
+ EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size);
+ ec_print_data(data, rec_size);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ data_size = rec_size - 16;
+
+ if (!(entry = (ec_sdo_entry_t *)
+ kmalloc(sizeof(ec_sdo_entry_t), GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate entry!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ ec_sdo_entry_init(entry, sdo, fsm->subindex);
+ entry->data_type = EC_READ_U16(data + 10);
+ entry->bit_length = EC_READ_U16(data + 12);
+
+ // read access rights
+ word = EC_READ_U16(data + 14);
+ entry->read_access[EC_SDO_ENTRY_ACCESS_PREOP] = word & 0x0001;
+ entry->read_access[EC_SDO_ENTRY_ACCESS_SAFEOP] =
+ (word >> 1) & 0x0001;
+ entry->read_access[EC_SDO_ENTRY_ACCESS_OP] = (word >> 2) & 0x0001;
+ entry->write_access[EC_SDO_ENTRY_ACCESS_PREOP] = (word >> 3) & 0x0001;
+ entry->write_access[EC_SDO_ENTRY_ACCESS_SAFEOP] =
+ (word >> 4) & 0x0001;
+ entry->write_access[EC_SDO_ENTRY_ACCESS_OP] = (word >> 5) & 0x0001;
+
+ if (data_size) {
+ uint8_t *desc;
+ if (!(desc = kmalloc(data_size + 1, GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate SDO entry name!\n");
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+ memcpy(desc, data + 16, data_size);
+ desc[data_size] = 0;
+ entry->description = desc;
+ }
+
+ list_add_tail(&entry->list, &sdo->entries);
+ }
+
+ if (fsm->subindex < sdo->max_subindex) {
+
+ fsm->subindex++;
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+
+ return;
+ }
+
+ // another SDO description to fetch?
+ if (fsm->sdo->list.next != &slave->sdo_dictionary) {
+
+ fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list);
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+
+ return;
+ }
+
+ fsm->state = ec_fsm_coe_end;
+}
+
+/*****************************************************************************
+ * CoE state machine
+ ****************************************************************************/
+
+/** Prepare a donwnload request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_fsm_coe_prepare_down_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ u8 *data;
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
+ uint8_t data_set_size;
+
+ if (request->data_size > 0 && request->data_size <= 4) {
+ // use expedited transfer mode for lengths between 1 and 4 bytes
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_COE,
+ EC_COE_DOWN_REQ_HEADER_SIZE);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ return PTR_ERR(data);
+ }
+
+ fsm->remaining = 0;
+
+ data_set_size = 4 - request->data_size;
+
+ EC_WRITE_U16(data, 0x2 << 12); // SDO request
+ EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited
+ | data_set_size << 2
+ | ((request->complete_access ? 1 : 0) << 4)
+ | 0x1 << 5)); // Download request
+ EC_WRITE_U16(data + 3, request->index);
+ EC_WRITE_U8 (data + 5,
+ request->complete_access ? 0x00 : request->subindex);
+ memcpy(data + 6, request->data, request->data_size);
+ memset(data + 6 + request->data_size, 0x00, 4 - request->data_size);
+
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Expedited download request:\n");
+ ec_print_data(data, EC_COE_DOWN_REQ_HEADER_SIZE);
+ }
+ }
+ else { // data_size < 1 or data_size > 4, use normal transfer type
+ size_t data_size,
+ max_data_size =
+ slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE,
+ required_data_size =
+ EC_COE_DOWN_REQ_HEADER_SIZE + request->data_size;
+
+ if (max_data_size < required_data_size) {
+ // segmenting needed
+ data_size = max_data_size;
+ } else {
+ data_size = required_data_size;
+ }
+
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_COE,
+ data_size);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ return PTR_ERR(data);
+ }
+
+ fsm->offset = 0;
+ fsm->remaining = request->data_size;
+
+ EC_WRITE_U16(data, 0x2 << 12); // SDO request
+ EC_WRITE_U8(data + 2,
+ 0x1 // size indicator, normal
+ | ((request->complete_access ? 1 : 0) << 4)
+ | 0x1 << 5); // Download request
+ EC_WRITE_U16(data + 3, request->index);
+ EC_WRITE_U8 (data + 5,
+ request->complete_access ? 0x00 : request->subindex);
+ EC_WRITE_U32(data + 6, request->data_size);
+
+ if (data_size > EC_COE_DOWN_REQ_HEADER_SIZE) {
+ size_t segment_size = data_size - EC_COE_DOWN_REQ_HEADER_SIZE;
+ memcpy(data + EC_COE_DOWN_REQ_HEADER_SIZE,
+ request->data, segment_size);
+ fsm->offset += segment_size;
+ fsm->remaining -= segment_size;
+ }
+
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Normal download request:\n");
+ ec_print_data(data, data_size);
+ }
+ }
+
+ fsm->state = ec_fsm_coe_down_request;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** CoE state: DOWN START.
+ */
+void ec_fsm_coe_down_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
+
+ if (fsm->slave->master->debug_level) {
+ char subidxstr[10];
+ if (request->complete_access) {
+ subidxstr[0] = 0x00;
+ } else {
+ sprintf(subidxstr, ":%02X", request->subindex);
+ }
+ EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n",
+ request->index, subidxstr);
+ ec_print_data(request->data, request->data_size);
+ }
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+ request->errno = EPROTONOSUPPORT;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (slave->configured_rx_mailbox_size <
+ EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) {
+ EC_SLAVE_ERR(slave, "Mailbox too small!\n");
+ request->errno = ENOBUFS;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+
+ fsm->request->jiffies_sent = jiffies;
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DOWN REQUEST.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_down_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ unsigned long diff_ms;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE download"
+ " request datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
+
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
+ if (diff_ms < fsm->request->response_timeout) {
+#if DEBUG_RETRIES
+ EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO"
+ " download request. Retrying after %lu ms...\n",
+ diff_ms);
+#endif
+ // no response; send request datagram again
+ if (ec_fsm_coe_prepare_down_start(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+ }
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE download request"
+ " for SDO 0x%04x:%x failed with timeout after %lu ms: ",
+ fsm->request->index, fsm->request->subindex, diff_ms);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+#if DEBUG_LONG
+ if (diff_ms > 200) {
+ EC_SLAVE_WARN(slave, "SDO 0x%04x:%x download took %lu ms.\n",
+ fsm->request->index, fsm->request->subindex, diff_ms);
+ }
+#endif
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_check;
+}
+
+/****************************************************************************/
+
+/** CoE state: DOWN CHECK.
+ */
+void ec_fsm_coe_down_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
+ " datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= fsm->request->response_timeout) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
+ " for SDO 0x%04x:%x download response.\n", diff_ms,
+ fsm->request->index, fsm->request->subindex);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_response;
+}
+
+/****************************************************************************/
+
+/** Prepare a download segment request.
+ */
+void ec_fsm_coe_down_prepare_segment_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
+ size_t max_segment_size =
+ slave->configured_rx_mailbox_size
+ - EC_MBOX_HEADER_SIZE
+ - EC_COE_DOWN_SEG_REQ_HEADER_SIZE;
+ size_t data_size;
+ uint8_t last_segment, seg_data_size, *data;
+
+ if (fsm->remaining > max_segment_size) {
+ fsm->segment_size = max_segment_size;
+ last_segment = 0;
+ } else {
+ fsm->segment_size = fsm->remaining;
+ last_segment = 1;
+ }
+
+ if (fsm->segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
+ seg_data_size = 0x00;
+ data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size;
+ } else {
+ seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size;
+ data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE
+ + EC_COE_DOWN_SEG_MIN_DATA_SIZE;
+ }
+
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_COE,
+ data_size);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ EC_WRITE_U16(data, 0x2 << 12); // SDO request
+ EC_WRITE_U8(data + 2, (last_segment ? 1 : 0)
+ | (seg_data_size << 1)
+ | (fsm->toggle << 4)
+ | (0x00 << 5)); // Download segment request
+ memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE,
+ request->data + fsm->offset, fsm->segment_size);
+ if (fsm->segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
+ memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size,
+ 0x00, EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size);
+ }
+
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Download segment request:\n");
+ ec_print_data(data, data_size);
+ }
+
+ fsm->state = ec_fsm_coe_down_seg_check;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DOWN RESPONSE.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_down_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint8_t *data, mbox_prot;
+ size_t rec_size;
+ ec_sdo_request_t *request = fsm->request;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE download"
+ " response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_COE) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ return;
+ }
+
+ if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_check;
+ return;
+ }
+
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Download response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (rec_size < 6) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n",
+ rec_size);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+ EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
+ char subidxstr[10];
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ if (request->complete_access) {
+ subidxstr[0] = 0x00;
+ } else {
+ sprintf(subidxstr, ":%02X", request->subindex);
+ }
+ EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n",
+ request->index, subidxstr, request->data_size);
+ if (rec_size < 10) {
+ EC_SLAVE_ERR(slave, "Incomplete abort command:\n");
+ ec_print_data(data, rec_size);
+ } else {
+ fsm->request->abort_code = EC_READ_U32(data + 6);
+ ec_canopen_abort_msg(slave, fsm->request->abort_code);
+ }
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+ EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
+ EC_READ_U16(data + 3) != request->index || // index
+ EC_READ_U8 (data + 5) != request->subindex) { // subindex
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!"
+ " Retrying...\n");
+ ec_print_data(data, rec_size);
+ }
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_check;
+ return;
+ }
+
+ if (fsm->remaining) { // more segments to download
+ fsm->toggle = 0;
+ ec_fsm_coe_down_prepare_segment_request(fsm, datagram);
+ } else {
+ fsm->state = ec_fsm_coe_end; // success
+ }
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DOWN SEG CHECK.
+*/
+
+void ec_fsm_coe_down_seg_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= fsm->request->response_timeout) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download"
+ " segment response.\n");
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_seg_response;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: DOWN SEG RESPONSE.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_down_seg_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint8_t *data, mbox_prot;
+ size_t rec_size;
+ ec_sdo_request_t *request = fsm->request;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE download response"
+ " datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_COE) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ return;
+ }
+
+ if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_check;
+ return;
+ }
+
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Download response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (rec_size < 6) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Received data are too small (%zu bytes):\n",
+ rec_size);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+ EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
+ char subidxstr[10];
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ if (request->complete_access) {
+ subidxstr[0] = 0x00;
+ } else {
+ sprintf(subidxstr, ":%02X", request->subindex);
+ }
+ EC_SLAVE_ERR(slave, "SDO download 0x%04X%s (%zu bytes) aborted.\n",
+ request->index, subidxstr, request->data_size);
+ if (rec_size < 10) {
+ EC_SLAVE_ERR(slave, "Incomplete abort command:\n");
+ ec_print_data(data, rec_size);
+ } else {
+ fsm->request->abort_code = EC_READ_U32(data + 6);
+ ec_canopen_abort_msg(slave, fsm->request->abort_code);
+ }
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x3 ||
+ ((EC_READ_U8(data + 2) >> 5) != 0x01)) { // segment response
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!"
+ " Retrying...\n");
+ ec_print_data(data, rec_size);
+ }
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_down_seg_check;
+ return;
+ }
+
+ if (((EC_READ_U8(data + 2) >> 4) & 0x01) != fsm->toggle) {
+ EC_SLAVE_ERR(slave, "Invalid toggle received during"
+ " segmented download:\n");
+ ec_print_data(data, rec_size);
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ fsm->offset += fsm->segment_size;
+ fsm->remaining -= fsm->segment_size;
+
+ if (fsm->remaining) { // more segments to download
+ fsm->toggle = !fsm->toggle;
+ ec_fsm_coe_down_prepare_segment_request(fsm, datagram);
+ } else {
+ fsm->state = ec_fsm_coe_end; // success
+ }
+}
+
+/****************************************************************************/
+
+/** Prepare an upload request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_fsm_coe_prepare_up(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
+ ec_master_t *master = slave->master;
+
+ u8 *data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_COE,
+ 10);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ return PTR_ERR(data);
+ }
+
+ EC_WRITE_U16(data, 0x2 << 12); // SDO request
+ EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
+ EC_WRITE_U16(data + 3, request->index);
+ EC_WRITE_U8 (data + 5, request->subindex);
+ memset(data + 6, 0x00, 4);
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Upload request:\n");
+ ec_print_data(data, 10);
+ }
+
+ fsm->state = ec_fsm_coe_up_request;
+ return 0;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: UP START.
+*/
+
+void ec_fsm_coe_up_start(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->request;
+
+ EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n",
+ request->index, request->subindex);
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");
+ request->errno = EPROTONOSUPPORT;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->request->jiffies_sent = jiffies;
+
+ if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+}
+
+/****************************************************************************/
+/**
+ CoE state: UP REQUEST.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_up_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ unsigned long diff_ms;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
+
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
+ if (diff_ms < fsm->request->response_timeout) {
+#if DEBUG_RETRIES
+ EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
+ " SDO upload request. Retrying after %lu ms...\n",
+ diff_ms);
+#endif
+ // no response; send request datagram again
+ if (ec_fsm_coe_prepare_up(fsm, datagram)) {
+ fsm->state = ec_fsm_coe_error;
+ }
+ return;
+ }
+ }
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE upload request for"
+ " SDO 0x%04x:%x failed with timeout after %lu ms: ",
+ fsm->request->index, fsm->request->subindex, diff_ms);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+#if DEBUG_LONG
+ if (diff_ms > 200) {
+ EC_SLAVE_WARN(slave, "SDO 0x%04x:%x upload took %lu ms.\n",
+ fsm->request->index, fsm->request->subindex, diff_ms);
+ }
+#endif
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_check;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: UP CHECK.
+*/
+
+void ec_fsm_coe_up_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= fsm->request->response_timeout) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
+ " SDO 0x%04x:%x upload response.\n", diff_ms,
+ fsm->request->index, fsm->request->subindex);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_response;
+}
+
+/****************************************************************************/
+
+/** Prepare an SDO upload segment request.
+ */
+void ec_fsm_coe_up_prepare_segment_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ uint8_t *data =
+ ec_slave_mbox_prepare_send(fsm->slave, datagram, EC_MBOX_TYPE_COE,
+ 10);
+ if (IS_ERR(data)) {
+ fsm->request->errno = PTR_ERR(data);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ EC_WRITE_U16(data, 0x2 << 12); // SDO request
+ EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
+ | 0x3 << 5)); // upload segment request
+ memset(data + 3, 0x00, 7);
+
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(fsm->slave, 1, "Upload segment request:\n");
+ ec_print_data(data, 10);
+ }
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: UP RESPONSE.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_up_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ uint16_t rec_index;
+ uint8_t *data, mbox_prot, rec_subindex;
+ size_t rec_size, data_size;
+ ec_sdo_request_t *request = fsm->request;
+ unsigned int expedited, size_specified;
+ int ret;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"
+ " datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Upload response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_COE) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"
+ " as response.\n", mbox_prot);
+ return;
+ }
+
+ if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_check;
+ return;
+ }
+
+ if (rec_size < 6) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Received currupted SDO upload response"
+ " (%zu bytes)!\n", rec_size);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+ EC_READ_U8(data + 2) >> 5 == 0x4) { // abort SDO transfer request
+ EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n",
+ request->index, request->subindex);
+ if (rec_size >= 10) {
+ request->abort_code = EC_READ_U32(data + 6);
+ ec_canopen_abort_msg(slave, request->abort_code);
+ } else {
+ EC_SLAVE_ERR(slave, "No abort message.\n");
+ }
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+ EC_READ_U8(data + 2) >> 5 != 0x2) { // upload response
+ EC_SLAVE_ERR(slave, "Received unknown response while"
+ " uploading SDO 0x%04X:%02X.\n",
+ request->index, request->subindex);
+ ec_print_data(data, rec_size);
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ rec_index = EC_READ_U16(data + 3);
+ rec_subindex = EC_READ_U8(data + 5);
+
+ if (rec_index != request->index || rec_subindex != request->subindex) {
+ EC_SLAVE_ERR(slave, "Received upload response for wrong SDO"
+ " (0x%04X:%02X, requested: 0x%04X:%02X).\n",
+ rec_index, rec_subindex, request->index, request->subindex);
+ ec_print_data(data, rec_size);
+
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_check;
+ return;
+ }
+
+ // normal or expedited?
+ expedited = EC_READ_U8(data + 2) & 0x02;
+
+ if (expedited) {
+ size_specified = EC_READ_U8(data + 2) & 0x01;
+ if (size_specified) {
+ fsm->complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
+ } else {
+ fsm->complete_size = 4;
+ }
+
+ if (rec_size < 6 + fsm->complete_size) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Received corrupted SDO expedited upload"
+ " response (only %zu bytes)!\n", rec_size);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ ret = ec_sdo_request_copy_data(request, data + 6, fsm->complete_size);
+ if (ret) {
+ request->errno = -ret;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+ } else { // normal
+ if (rec_size < 10) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Received currupted SDO normal upload"
+ " response (only %zu bytes)!\n", rec_size);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ data_size = rec_size - 10;
+ fsm->complete_size = EC_READ_U32(data + 6);
+
+ ret = ec_sdo_request_alloc(request, fsm->complete_size);
+ if (ret) {
+ request->errno = -ret;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ ret = ec_sdo_request_copy_data(request, data + 10, data_size);
+ if (ret) {
+ request->errno = -ret;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ fsm->toggle = 0;
+
+ if (data_size < fsm->complete_size) {
+ EC_SLAVE_DBG(slave, 1, "SDO data incomplete (%zu / %u)."
+ " Segmenting...\n", data_size, fsm->complete_size);
+ ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_seg_request;
+ return;
+ }
+ }
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Uploaded data:\n");
+ ec_print_data(request->data, request->data_size);
+ }
+
+ fsm->state = ec_fsm_coe_end; // success
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: UP REQUEST.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_up_seg_request(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
+ " request datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
+ " request failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_seg_check;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: UP CHECK.
+*/
+
+void ec_fsm_coe_up_seg_check(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
+ " datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"
+ " failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= fsm->request->response_timeout) {
+ fsm->request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload"
+ " segment response.\n");
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_seg_response;
+}
+
+/****************************************************************************/
+
+/**
+ CoE state: UP RESPONSE.
+ \todo Timeout behavior
+*/
+
+void ec_fsm_coe_up_seg_response(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ uint8_t *data, mbox_prot;
+ size_t rec_size, data_size;
+ ec_sdo_request_t *request = fsm->request;
+ unsigned int last_segment;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
+ " response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
+ " response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ request->errno = PTR_ERR(data);
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Upload segment response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_COE) {
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_seg_check;
+ return;
+ }
+
+ if (rec_size < 10) {
+ EC_SLAVE_ERR(slave, "Received currupted SDO upload"
+ " segment response!\n");
+ ec_print_data(data, rec_size);
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+ EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
+ EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X aborted.\n",
+ request->index, request->subindex);
+ request->abort_code = EC_READ_U32(data + 6);
+ ec_canopen_abort_msg(slave, request->abort_code);
+ request->errno = EIO;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+ EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Invalid SDO upload segment response!\n");
+ ec_print_data(data, rec_size);
+ }
+ // check for CoE response again
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_seg_check;
+ return;
+ }
+
+ data_size = rec_size - 3; /* Header of segment upload is smaller than
+ normal upload */
+ if (rec_size == 10) {
+ uint8_t seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
+ data_size -= seg_size;
+ }
+
+ if (request->data_size + data_size > fsm->complete_size) {
+ EC_SLAVE_ERR(slave, "SDO upload 0x%04X:%02X failed: Fragment"
+ " exceeding complete size!\n",
+ request->index, request->subindex);
+ request->errno = ENOBUFS;
+ fsm->state = ec_fsm_coe_error;
+ return;
+ }
+
+ memcpy(request->data + request->data_size, data + 3, data_size);
+ request->data_size += data_size;
+
+ last_segment = EC_READ_U8(data + 2) & 0x01;
+ if (!last_segment) {
+ fsm->toggle = !fsm->toggle;
+ ec_fsm_coe_up_prepare_segment_request(fsm, datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_coe_up_seg_request;
+ return;
+ }
+
+ if (request->data_size != fsm->complete_size) {
+ EC_SLAVE_WARN(slave, "SDO upload 0x%04X:%02X: Assembled data"
+ " size (%zu) does not match complete size (%u)!\n",
+ request->index, request->subindex,
+ request->data_size, fsm->complete_size);
+ }
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Uploaded data:\n");
+ ec_print_data(request->data, request->data_size);
+ }
+
+ fsm->state = ec_fsm_coe_end; // success
+}
+
+/****************************************************************************/
+
+/**
+ State: ERROR.
+*/
+
+void ec_fsm_coe_error(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
+
+/**
+ State: END.
+*/
+
+void ec_fsm_coe_end(
+ ec_fsm_coe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_coe.h b/ubuntu/igh-ecat/master/fsm_coe.h
new file mode 100644
index 00000000000000..846e1447348973
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_coe.h
@@ -0,0 +1,74 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT CoE state machines.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_COE_H__
+#define __EC_FSM_COE_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "slave.h"
+#include "sdo.h"
+#include "sdo_request.h"
+
+/****************************************************************************/
+
+typedef struct ec_fsm_coe ec_fsm_coe_t; /**< \see ec_fsm_coe */
+
+/** Finite state machines for the CANopen over EtherCAT protocol.
+ */
+struct ec_fsm_coe {
+ ec_slave_t *slave; /**< slave the FSM runs on */
+ unsigned int retries; /**< retries upon datagram timeout */
+
+ void (*state)(ec_fsm_coe_t *, ec_datagram_t *); /**< CoE state function */
+ ec_datagram_t *datagram; /**< Datagram used in last step. */
+ unsigned long jiffies_start; /**< CoE timestamp. */
+ ec_sdo_t *sdo; /**< current SDO */
+ uint8_t subindex; /**< current subindex */
+ ec_sdo_request_t *request; /**< SDO request */
+ uint32_t complete_size; /**< Used when segmenting. */
+ uint8_t toggle; /**< toggle bit for segment commands */
+ uint32_t offset; /**< Data offset during segmented download. */
+ uint32_t remaining; /**< Remaining bytes during segmented download. */
+ size_t segment_size; /**< Current segment size. */
+};
+
+/****************************************************************************/
+
+void ec_fsm_coe_init(ec_fsm_coe_t *);
+void ec_fsm_coe_clear(ec_fsm_coe_t *);
+
+void ec_fsm_coe_dictionary(ec_fsm_coe_t *, ec_slave_t *);
+void ec_fsm_coe_transfer(ec_fsm_coe_t *, ec_slave_t *, ec_sdo_request_t *);
+
+int ec_fsm_coe_exec(ec_fsm_coe_t *, ec_datagram_t *);
+int ec_fsm_coe_success(const ec_fsm_coe_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_eoe.c b/ubuntu/igh-ecat/master/fsm_eoe.c
new file mode 100644
index 00000000000000..b3ef6898939f76
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_eoe.c
@@ -0,0 +1,509 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2014 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT EoE state machines.
+*/
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "fsm_eoe.h"
+
+/****************************************************************************/
+
+/** Maximum time to wait for a set IP parameter response.
+ */
+#define EC_EOE_RESPONSE_TIMEOUT 3000 // [ms]
+
+/****************************************************************************/
+
+// prototypes for private functions
+void memcpy_swap32(void *, const void *);
+
+int ec_fsm_eoe_prepare_set(ec_fsm_eoe_t *, ec_datagram_t *);
+
+void ec_fsm_eoe_set_ip_start(ec_fsm_eoe_t *, ec_datagram_t *);
+void ec_fsm_eoe_set_ip_request(ec_fsm_eoe_t *, ec_datagram_t *);
+void ec_fsm_eoe_set_ip_check(ec_fsm_eoe_t *, ec_datagram_t *);
+void ec_fsm_eoe_set_ip_response(ec_fsm_eoe_t *, ec_datagram_t *);
+
+void ec_fsm_eoe_end(ec_fsm_eoe_t *, ec_datagram_t *);
+void ec_fsm_eoe_error(ec_fsm_eoe_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+/** Host-architecture-independent 32-bit swap function.
+ *
+ * The internal storage of struct in_addr is always big-endian.
+ * The mailbox protocol format to supply IPv4 adresses is little-endian
+ * (Yuck!). So we need a swap function, that is independent of the CPU
+ * architecture. ntohl()/htonl() can not be used, because they evaluate to
+ * NOPs if the host architecture matches the target architecture!
+ */
+void memcpy_swap32(void *dst, const void *src)
+{
+ int i;
+ for (i = 0; i < 4; i++) {
+ ((u8 *) dst)[i] = ((const u8 *) src)[3 - i];
+ }
+}
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_eoe_init(
+ ec_fsm_eoe_t *fsm /**< finite state machine */
+ )
+{
+ fsm->slave = NULL;
+ fsm->retries = 0;
+ fsm->state = NULL;
+ fsm->datagram = NULL;
+ fsm->jiffies_start = 0;
+ fsm->request = NULL;
+ fsm->frame_type_retries = 0;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_eoe_clear(
+ ec_fsm_eoe_t *fsm /**< finite state machine */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** Starts to set the EoE IP partameters of a slave.
+ */
+void ec_fsm_eoe_set_ip_param(
+ ec_fsm_eoe_t *fsm, /**< State machine. */
+ ec_slave_t *slave, /**< EtherCAT slave. */
+ ec_eoe_request_t *request /**< EoE request. */
+ )
+{
+ fsm->slave = slave;
+ fsm->request = request;
+ fsm->state = ec_fsm_eoe_set_ip_start;
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_eoe_exec(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_eoe_end && fsm->state != ec_fsm_eoe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ *
+ * \return non-zero if successful.
+ */
+int ec_fsm_eoe_success(const ec_fsm_eoe_t *fsm /**< Finite state machine */)
+{
+ return fsm->state == ec_fsm_eoe_end;
+}
+
+/*****************************************************************************
+ * EoE set IP parameter state machine
+ ****************************************************************************/
+
+/** Prepare a set IP parameters operation.
+ *
+ * \return 0 on success, otherwise a negative error code.
+ */
+int ec_fsm_eoe_prepare_set(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ uint8_t *data, *cur;
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_eoe_request_t *req = fsm->request;
+
+ // Note: based on wireshark packet filter it suggests that the EOE_INIT
+ // information is a fixed size with fixed information positions.
+ // see: packet-ecatmb.h and packet-ecatmb.c
+ // However, TwinCAT 2.1 testing also indicates that if a piece of
+ // information is missing then all subsequent items are ignored
+ // Also, if you want DHCP, then only set the mac address.
+ size_t size = 8 + // header + flags
+ ETH_ALEN + // mac address
+ 4 + // ip address
+ 4 + // subnet mask
+ 4 + // gateway
+ 4 + // dns server
+ EC_MAX_HOSTNAME_SIZE; // dns name
+
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_EOE,
+ size);
+ if (IS_ERR(data)) {
+ return PTR_ERR(data);
+ }
+
+ // zero data
+ memset(data, 0, size);
+
+ // header
+ EC_WRITE_U8(data, EC_EOE_FRAMETYPE_SET_IP_REQ); // Set IP parameter req.
+ EC_WRITE_U8(data + 1, 0x00); // not used
+ EC_WRITE_U16(data + 2, 0x0000); // not used
+
+ EC_WRITE_U32(data + 4,
+ ((req->mac_address_included != 0) << 0) |
+ ((req->ip_address_included != 0) << 1) |
+ ((req->subnet_mask_included != 0) << 2) |
+ ((req->gateway_included != 0) << 3) |
+ ((req->dns_included != 0) << 4) |
+ ((req->name_included != 0) << 5)
+ );
+
+ cur = data + 8;
+
+ if (req->mac_address_included) {
+ memcpy(cur, req->mac_address, ETH_ALEN);
+ }
+ cur += ETH_ALEN;
+
+ if (req->ip_address_included) {
+ memcpy_swap32(cur, &req->ip_address);
+ }
+ cur += 4;
+
+ if (req->subnet_mask_included) {
+ memcpy_swap32(cur, &req->subnet_mask);
+ }
+ cur += 4;
+
+ if (req->gateway_included) {
+ memcpy_swap32(cur, &req->gateway);
+ }
+ cur += 4;
+
+ if (req->dns_included) {
+ memcpy_swap32(cur, &req->dns);
+ }
+ cur += 4;
+
+ if (req->name_included) {
+ memcpy(cur, req->name, EC_MAX_HOSTNAME_SIZE);
+ }
+ cur += EC_MAX_HOSTNAME_SIZE;
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "Set IP parameter request:\n");
+ ec_print_data(data, cur - data);
+ }
+
+ fsm->request->jiffies_sent = jiffies;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** EoE state: SET IP START.
+ */
+void ec_fsm_eoe_set_ip_start(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ EC_SLAVE_DBG(slave, 1, "Setting IP parameters.\n");
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_EOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support EoE!\n");
+ fsm->state = ec_fsm_eoe_error;
+ return;
+ }
+
+ if (ec_fsm_eoe_prepare_set(fsm, datagram)) {
+ fsm->state = ec_fsm_eoe_error;
+ return;
+ }
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_eoe_set_ip_request;
+}
+
+/****************************************************************************/
+
+/** EoE state: SET IP REQUEST.
+ */
+void ec_fsm_eoe_set_ip_request(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_eoe_prepare_set(fsm, datagram)) {
+ fsm->state = ec_fsm_eoe_error;
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive EoE set IP parameter"
+ " request: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ unsigned long diff_ms =
+ (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
+
+ if (!fsm->datagram->working_counter) {
+ if (diff_ms < EC_EOE_RESPONSE_TIMEOUT) {
+ // no response; send request datagram again
+ if (ec_fsm_eoe_prepare_set(fsm, datagram)) {
+ fsm->state = ec_fsm_eoe_error;
+ }
+ return;
+ }
+ }
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Reception of EoE set IP parameter request"
+ " failed after %lu ms: ", diff_ms);
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_eoe_set_ip_check;
+ fsm->frame_type_retries = 10;
+}
+
+/****************************************************************************/
+
+/** EoE state: SET IP CHECK.
+ */
+void ec_fsm_eoe_set_ip_check(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive EoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Reception of EoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= EC_EOE_RESPONSE_TIMEOUT) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
+ " set IP parameter response.\n", diff_ms);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_eoe_set_ip_response;
+}
+
+/****************************************************************************/
+
+/** EoE state: SET IP RESPONSE.
+ */
+void ec_fsm_eoe_set_ip_response(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ uint8_t *data, mbox_prot, frame_type;
+ size_t rec_size;
+ ec_eoe_request_t *req = fsm->request;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive EoE read response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Reception of EoE read response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_eoe_error;
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "Set IP parameter response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_EOE) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ return;
+ }
+
+ if (rec_size < 4) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_ERR(slave, "Received currupted EoE set IP parameter response"
+ " (%zu bytes)!\n", rec_size);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ frame_type = EC_READ_U8(data) & 0x0f;
+
+ if (frame_type != EC_EOE_FRAMETYPE_SET_IP_RES) {
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "Received no set IP parameter response"
+ " (frame type %x).\n", frame_type);
+ ec_print_data(data, rec_size);
+ }
+ if (fsm->frame_type_retries--) {
+ // there may be an EoE segment left in the mailbox.
+ // discard it and receive again.
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_eoe_set_ip_check;
+ return;
+ }
+ else {
+ EC_SLAVE_ERR(slave, "Received no set IP parameter response.\n");
+ fsm->state = ec_fsm_eoe_error;
+ return;
+ }
+ }
+
+ req->result = EC_READ_U16(data + 2); // result code 0x0000 means success
+
+ if (req->result) {
+ fsm->state = ec_fsm_eoe_error;
+ EC_SLAVE_DBG(slave, 1, "EoE set IP parameters failed with result code"
+ " 0x%04X.\n", req->result);
+ } else {
+ fsm->state = ec_fsm_eoe_end; // success
+ }
+}
+
+/****************************************************************************/
+
+/** State: ERROR.
+ */
+void ec_fsm_eoe_error(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** State: END.
+ */
+void ec_fsm_eoe_end(
+ ec_fsm_eoe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_eoe.h b/ubuntu/igh-ecat/master/fsm_eoe.h
new file mode 100644
index 00000000000000..7f901bdd5e3ba0
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_eoe.h
@@ -0,0 +1,67 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2014 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT EoE set IP parameter state machines.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_EOE_H__
+#define __EC_FSM_EOE_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "slave.h"
+#include "eoe_request.h"
+
+/****************************************************************************/
+
+typedef struct ec_fsm_eoe ec_fsm_eoe_t; /**< \see ec_fsm_eoe */
+
+/** Finite state machines for the Ethernet over EtherCAT protocol.
+ */
+struct ec_fsm_eoe {
+ ec_slave_t *slave; /**< slave the FSM runs on */
+ unsigned int retries; /**< retries upon datagram timeout */
+
+ void (*state)(ec_fsm_eoe_t *, ec_datagram_t *); /**< EoE state function */
+ ec_datagram_t *datagram; /**< Datagram used in the previous step. */
+ unsigned long jiffies_start; /**< Timestamp. */
+ ec_eoe_request_t *request; /**< EoE request */
+ unsigned int frame_type_retries; /**< retries upon wrong frame type. */
+};
+
+/****************************************************************************/
+
+void ec_fsm_eoe_init(ec_fsm_eoe_t *);
+void ec_fsm_eoe_clear(ec_fsm_eoe_t *);
+
+void ec_fsm_eoe_set_ip_param(ec_fsm_eoe_t *, ec_slave_t *,
+ ec_eoe_request_t *);
+
+int ec_fsm_eoe_exec(ec_fsm_eoe_t *, ec_datagram_t *);
+int ec_fsm_eoe_success(const ec_fsm_eoe_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_foe.c b/ubuntu/igh-ecat/master/fsm_foe.c
new file mode 100644
index 00000000000000..378ae369401cef
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_foe.c
@@ -0,0 +1,915 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
+ * 2013 Florian Pose <fp at igh.de>
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT FoE state machines.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "fsm_foe.h"
+#include "foe.h"
+
+/****************************************************************************/
+
+/** Maximum time in ms to wait for responses when reading out the dictionary.
+ */
+#define EC_FSM_FOE_TIMEOUT 3000
+
+/** Size of the FoE header.
+ */
+#define EC_FOE_HEADER_SIZE 6
+// uint8_t OpCode
+// uint8_t reserved
+// uint32_t PacketNo, Password, ErrorCode
+
+//#define DEBUG_FOE
+
+/****************************************************************************/
+
+/** FoE OpCodes.
+ */
+enum {
+ EC_FOE_OPCODE_RRQ = 1, /**< Read request. */
+ EC_FOE_OPCODE_WRQ = 2, /**< Write request. */
+ EC_FOE_OPCODE_DATA = 3, /**< Data. */
+ EC_FOE_OPCODE_ACK = 4, /**< Acknowledge. */
+ EC_FOE_OPCODE_ERR = 5, /**< Error. */
+ EC_FOE_OPCODE_BUSY = 6 /**< Busy. */
+};
+
+/****************************************************************************/
+
+int ec_foe_prepare_data_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_wrq_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_rrq_send(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_foe_prepare_send_ack(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);
+void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);
+
+void ec_fsm_foe_end(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_error(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);
+
+void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);
+void ec_fsm_foe_read_start(ec_fsm_foe_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_foe_init(
+ ec_fsm_foe_t *fsm /**< finite state machine */
+ )
+{
+ fsm->state = NULL;
+ fsm->datagram = NULL;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_foe_clear(ec_fsm_foe_t *fsm /**< finite state machine */)
+{
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * \return 1, if the datagram was used, else 0.
+ */
+int ec_fsm_foe_exec(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ * \return non-zero if successful.
+ */
+int ec_fsm_foe_success(const ec_fsm_foe_t *fsm /**< Finite state machine */)
+{
+ return fsm->state == ec_fsm_foe_end;
+}
+
+/****************************************************************************/
+
+/** Prepares an FoE transfer.
+ */
+void ec_fsm_foe_transfer(
+ ec_fsm_foe_t *fsm, /**< State machine. */
+ ec_slave_t *slave, /**< EtherCAT slave. */
+ ec_foe_request_t *request /**< Sdo request. */
+ )
+{
+ fsm->slave = slave;
+ fsm->request = request;
+
+ if (request->dir == EC_DIR_OUTPUT) {
+ fsm->tx_buffer = fsm->request->buffer;
+ fsm->tx_buffer_size = fsm->request->data_size;
+ fsm->tx_buffer_offset = 0;
+
+ fsm->tx_filename = fsm->request->file_name;
+ fsm->tx_filename_len = strlen(fsm->tx_filename);
+
+ fsm->state = ec_fsm_foe_write_start;
+ }
+ else {
+ fsm->rx_buffer = fsm->request->buffer;
+ fsm->rx_buffer_size = fsm->request->buffer_size;
+
+ fsm->rx_filename = fsm->request->file_name;
+ fsm->rx_filename_len = strlen(fsm->rx_filename);
+
+ fsm->state = ec_fsm_foe_read_start;
+ }
+}
+
+/****************************************************************************/
+
+/** State: ERROR.
+ */
+void ec_fsm_foe_error(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+}
+
+/****************************************************************************/
+
+/** State: END.
+ */
+void ec_fsm_foe_end(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+}
+
+/****************************************************************************/
+
+/** Sends a file or the next fragment.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_foe_prepare_data_send(
+ ec_fsm_foe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ size_t remaining_size, current_size;
+ uint8_t *data;
+
+ remaining_size = fsm->tx_buffer_size - fsm->tx_buffer_offset;
+
+ if (remaining_size < fsm->slave->configured_tx_mailbox_size
+ - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {
+ current_size = remaining_size;
+ fsm->tx_last_packet = 1;
+ } else {
+ current_size = fsm->slave->configured_tx_mailbox_size
+ - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;
+ }
+
+ data = ec_slave_mbox_prepare_send(fsm->slave,
+ datagram, EC_MBOX_TYPE_FOE, current_size + EC_FOE_HEADER_SIZE);
+ if (IS_ERR(data)) {
+ return -1;
+ }
+
+ EC_WRITE_U16(data, EC_FOE_OPCODE_DATA); // OpCode = DataBlock req.
+ EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password
+
+ memcpy(data + EC_FOE_HEADER_SIZE,
+ fsm->tx_buffer + fsm->tx_buffer_offset, current_size);
+ fsm->tx_current_size = current_size;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Prepare a write request (WRQ) with filename
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_foe_prepare_wrq_send(
+ ec_fsm_foe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ size_t current_size;
+ uint8_t *data;
+
+ fsm->tx_buffer_offset = 0;
+ fsm->tx_current_size = 0;
+ fsm->tx_packet_no = 0;
+ fsm->tx_last_packet = 0;
+
+ current_size = fsm->tx_filename_len;
+
+ data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
+ EC_MBOX_TYPE_FOE, current_size + EC_FOE_HEADER_SIZE);
+ if (IS_ERR(data)) {
+ return -1;
+ }
+
+ EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request
+ EC_WRITE_U32( data + 2, fsm->tx_packet_no );
+
+ memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_filename, current_size);
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Initializes the FoE write state machine.
+ */
+void ec_fsm_foe_write_start(
+ ec_fsm_foe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ fsm->tx_buffer_offset = 0;
+ fsm->tx_current_size = 0;
+ fsm->tx_packet_no = 0;
+ fsm->tx_last_packet = 0;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_FOE)) {
+ ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);
+ EC_SLAVE_ERR(slave, "Slave does not support FoE!\n");
+ return;
+ }
+
+ if (ec_foe_prepare_wrq_send(fsm, datagram)) {
+ ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
+ return;
+ }
+
+ fsm->state = ec_fsm_foe_state_wrq_sent;
+}
+
+/****************************************************************************/
+
+/** Check for acknowledge.
+ */
+void ec_fsm_foe_state_ack_check(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
+ " failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ // slave did not put anything in the mailbox yet
+ unsigned long diff_ms = (fsm->datagram->jiffies_received -
+ fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
+ ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
+ EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_ack_read;
+}
+
+/****************************************************************************/
+
+/** Acknowledge a read operation.
+ */
+void ec_fsm_foe_state_ack_read(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint8_t *data, mbox_prot;
+ uint8_t opCode;
+ size_t rec_size;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_FOE) {
+ ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ return;
+ }
+
+ opCode = EC_READ_U8(data);
+
+ if (opCode == EC_FOE_OPCODE_BUSY) {
+ // slave not ready
+ if (ec_foe_prepare_data_send(fsm, datagram)) {
+ ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
+ EC_SLAVE_ERR(slave, "Slave is busy.\n");
+ return;
+ }
+ fsm->state = ec_fsm_foe_state_data_sent;
+ return;
+ }
+
+ if (opCode == EC_FOE_OPCODE_ACK) {
+ fsm->tx_packet_no++;
+ fsm->tx_buffer_offset += fsm->tx_current_size;
+
+ if (fsm->tx_last_packet) {
+ fsm->state = ec_fsm_foe_end;
+ return;
+ }
+
+ if (ec_foe_prepare_data_send(fsm, datagram)) {
+ ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
+ return;
+ }
+ fsm->state = ec_fsm_foe_state_data_sent;
+ return;
+ }
+ ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);
+}
+
+/****************************************************************************/
+
+/** State: WRQ SENT.
+ *
+ * Checks is the previous transmit datagram succeded and sends the next
+ * fragment, if necessary.
+ */
+void ec_fsm_foe_state_wrq_sent(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to send FoE WRQ: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ // slave did not put anything in the mailbox yet
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE WRQ failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_ack_check;
+}
+
+/****************************************************************************/
+
+/** State: WRQ SENT.
+ *
+ * Checks is the previous transmit datagram succeded and sends the next
+ * fragment, if necessary.
+ */
+void ec_fsm_foe_state_data_sent(
+ ec_fsm_foe_t *fsm, /**< Foe statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ ec_foe_set_tx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE data send failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram);
+ fsm->jiffies_start = jiffies;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_ack_check;
+}
+
+/****************************************************************************/
+
+/** Prepare a read request (RRQ) with filename
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_foe_prepare_rrq_send(
+ ec_fsm_foe_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ size_t current_size;
+ uint8_t *data;
+
+ current_size = fsm->rx_filename_len;
+
+ data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
+ EC_MBOX_TYPE_FOE, current_size + EC_FOE_HEADER_SIZE);
+ if (IS_ERR(data)) {
+ return -1;
+ }
+
+ EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
+ EC_WRITE_U32(data + 2, 0x00000000); // no passwd
+ memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size);
+
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(fsm->slave, 1, "FoE Read Request:\n");
+ ec_print_data(data, current_size + EC_FOE_HEADER_SIZE);
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Prepare to send an acknowledge.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_foe_prepare_send_ack(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ uint8_t *data;
+
+ data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
+ EC_MBOX_TYPE_FOE, EC_FOE_HEADER_SIZE);
+ if (IS_ERR(data)) {
+ return -1;
+ }
+
+ EC_WRITE_U16(data, EC_FOE_OPCODE_ACK);
+ EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no);
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** State: RRQ SENT.
+ *
+ * Checks is the previous transmit datagram succeeded and sends the next
+ * fragment, if necessary.
+ */
+void ec_fsm_foe_state_rrq_sent(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ // slave did not put anything in the mailbox yet
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_data_check;
+}
+
+/****************************************************************************/
+
+/** Starting state for read operations.
+ */
+void ec_fsm_foe_read_start(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ fsm->rx_buffer_offset = 0;
+ fsm->rx_expected_packet_no = 1;
+ fsm->rx_last_packet = 0;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_FOE)) {
+ ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);
+ EC_SLAVE_ERR(slave, "Slave does not support FoE!\n");
+ return;
+ }
+
+ if (ec_foe_prepare_rrq_send(fsm, datagram)) {
+ ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
+ return;
+ }
+
+ fsm->state = ec_fsm_foe_state_rrq_sent;
+}
+
+/****************************************************************************/
+
+/** Check for data.
+ */
+void ec_fsm_foe_state_data_check(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms = (fsm->datagram->jiffies_received -
+ fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
+ ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
+ EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_data_read;
+}
+
+/****************************************************************************/
+
+/** Start reading data.
+ */
+void ec_fsm_foe_state_data_read(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ size_t rec_size;
+ uint8_t *data, opCode, packet_no, mbox_prot;
+
+ ec_slave_t *slave = fsm->slave;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_FOE) {
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
+ return;
+ }
+
+ opCode = EC_READ_U8(data);
+
+ if (opCode == EC_FOE_OPCODE_BUSY) {
+ if (ec_foe_prepare_send_ack(fsm, datagram)) {
+ ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
+ }
+ return;
+ }
+
+ if (opCode == EC_FOE_OPCODE_ERR) {
+ fsm->request->error_code = EC_READ_U32(data + 2);
+ EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n",
+ fsm->request->error_code);
+ if (rec_size > 6) {
+ uint8_t text[256];
+ strncpy(text, data + 6, min(rec_size - 6, sizeof(text)));
+ EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text);
+ }
+ ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR);
+ return;
+ }
+
+ if (opCode != EC_FOE_OPCODE_DATA) {
+ EC_SLAVE_ERR(slave, "Received OPCODE %x, expected %x.\n",
+ opCode, EC_FOE_OPCODE_DATA);
+ fsm->request->error_code = 0x00000000;
+ ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR);
+ return;
+ }
+
+ packet_no = EC_READ_U16(data + 2);
+ if (packet_no != fsm->rx_expected_packet_no) {
+ EC_SLAVE_ERR(slave, "Received unexpected packet number.\n");
+ ec_foe_set_rx_error(fsm, FOE_PACKETNO_ERROR);
+ return;
+ }
+
+ rec_size -= EC_FOE_HEADER_SIZE;
+
+ if (fsm->rx_buffer_size >= fsm->rx_buffer_offset + rec_size) {
+ memcpy(fsm->rx_buffer + fsm->rx_buffer_offset,
+ data + EC_FOE_HEADER_SIZE, rec_size);
+ fsm->rx_buffer_offset += rec_size;
+ }
+
+ fsm->rx_last_packet =
+ (rec_size + EC_MBOX_HEADER_SIZE + EC_FOE_HEADER_SIZE
+ != slave->configured_rx_mailbox_size);
+
+ if (fsm->rx_last_packet ||
+ (slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE
+ - EC_FOE_HEADER_SIZE + fsm->rx_buffer_offset)
+ <= fsm->rx_buffer_size) {
+ // either it was the last packet or a new packet will fit into the
+ // delivered buffer
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "last_packet=true\n");
+#endif
+ if (ec_foe_prepare_send_ack(fsm, datagram)) {
+ ec_foe_set_rx_error(fsm, FOE_RX_DATA_ACK_ERROR);
+ return;
+ }
+
+ fsm->state = ec_fsm_foe_state_sent_ack;
+ }
+ else {
+ // no more data fits into the delivered buffer
+ // ... wait for new read request
+ EC_SLAVE_ERR(slave, "Data do not fit in receive buffer!\n");
+ printk(KERN_CONT " rx_buffer_size = %d\n", fsm->rx_buffer_size);
+ printk(KERN_CONT "rx_buffer_offset = %d\n", fsm->rx_buffer_offset);
+ printk(KERN_CONT " rec_size = %zd\n", rec_size);
+ printk(KERN_CONT " rx_mailbox_size = %d\n",
+ slave->configured_rx_mailbox_size);
+ printk(KERN_CONT " rx_last_packet = %d\n", fsm->rx_last_packet);
+ fsm->request->result = FOE_READY;
+ }
+}
+
+/****************************************************************************/
+
+/** Sent an acknowledge.
+ */
+void ec_fsm_foe_state_sent_ack(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+#ifdef DEBUG_FOE
+ EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
+#endif
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
+ EC_SLAVE_ERR(slave, "Failed to send FoE ACK: ");
+ ec_datagram_print_state(fsm->datagram);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ // slave did not put anything into the mailbox yet
+ ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
+ EC_SLAVE_ERR(slave, "Reception of FoE ACK failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+
+ if (fsm->rx_last_packet) {
+ fsm->rx_expected_packet_no = 0;
+ fsm->request->data_size = fsm->rx_buffer_offset;
+ fsm->state = ec_fsm_foe_end;
+ }
+ else {
+ fsm->rx_expected_packet_no++;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_foe_state_data_check;
+ }
+}
+
+/****************************************************************************/
+
+/** Set an error code and go to the send error state.
+ */
+void ec_foe_set_tx_error(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ uint32_t errorcode /**< FoE error code. */
+ )
+{
+ fsm->request->result = errorcode;
+ fsm->state = ec_fsm_foe_error;
+}
+
+/****************************************************************************/
+
+/** Set an error code and go to the receive error state.
+ */
+void ec_foe_set_rx_error(
+ ec_fsm_foe_t *fsm, /**< FoE statemachine. */
+ uint32_t errorcode /**< FoE error code. */
+ )
+{
+ fsm->request->result = errorcode;
+ fsm->state = ec_fsm_foe_error;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_foe.h b/ubuntu/igh-ecat/master/fsm_foe.h
new file mode 100644
index 00000000000000..55711dccee0692
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_foe.h
@@ -0,0 +1,87 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2008 Olav Zarges, imc Messsysteme GmbH
+ * 2009-2012 Florian Pose <fp at igh.de>
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT FoE state machines.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_FOE_H__
+#define __EC_FSM_FOE_H__
+
+#include "globals.h"
+#include "../include/ecrt.h"
+#include "datagram.h"
+#include "slave.h"
+#include "foe_request.h"
+
+/****************************************************************************/
+
+typedef struct ec_fsm_foe ec_fsm_foe_t; /**< \see ec_fsm_foe */
+
+/** Finite state machines for the CANopen-over-EtherCAT protocol.
+ */
+struct ec_fsm_foe {
+ ec_slave_t *slave; /**< Slave the FSM runs on. */
+ unsigned int retries; /**< Retries upon datagram timeout */
+
+ void (*state)(ec_fsm_foe_t *, ec_datagram_t *); /**< FoE state function.
+ */
+ ec_datagram_t *datagram; /**< Datagram used in previous step. */
+ unsigned long jiffies_start; /**< FoE timestamp. */
+ uint8_t subindex; /**< Current subindex. */
+ ec_foe_request_t *request; /**< FoE request. */
+ uint8_t toggle; /**< Toggle bit for segment commands. */
+
+ uint8_t *tx_buffer; /**< Buffer with data to transmit. */
+ uint32_t tx_buffer_size; /**< Size of data to transmit. */
+ uint32_t tx_buffer_offset; /**< Offset of data to tranmit next. */
+ uint32_t tx_last_packet; /**< Current packet is last one to send. */
+ uint32_t tx_packet_no; /**< FoE packet number. */
+ uint32_t tx_current_size; /**< Size of current packet to send. */
+ uint8_t *tx_filename; /**< Name of file to transmit. */
+ uint32_t tx_filename_len; /**< Lenth of transmit file name. */
+
+ uint8_t *rx_buffer; /**< Buffer for received data. */
+ uint32_t rx_buffer_size; /**< Size of receive buffer. */
+ uint32_t rx_buffer_offset; /**< Offset in receive buffer. */
+ uint32_t rx_expected_packet_no; /**< Expected receive packet number. */
+ uint32_t rx_last_packet; /**< Current packet is the last to receive. */
+ uint8_t *rx_filename; /**< Name of the file to receive. */
+ uint32_t rx_filename_len; /**< Length of the receive file name. */
+};
+
+/****************************************************************************/
+
+void ec_fsm_foe_init(ec_fsm_foe_t *);
+void ec_fsm_foe_clear(ec_fsm_foe_t *);
+
+int ec_fsm_foe_exec(ec_fsm_foe_t *, ec_datagram_t *);
+int ec_fsm_foe_success(const ec_fsm_foe_t *);
+
+void ec_fsm_foe_transfer(ec_fsm_foe_t *, ec_slave_t *, ec_foe_request_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_master.c b/ubuntu/igh-ecat/master/fsm_master.c
new file mode 100644
index 00000000000000..909c0115f4838b
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_master.c
@@ -0,0 +1,1472 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT master state machine.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "slave_config.h"
+#ifdef EC_EOE
+#include "ethernet.h"
+#endif
+
+#include "fsm_master.h"
+#include "fsm_foe.h"
+
+/****************************************************************************/
+
+/** Time difference [ns] to tolerate without setting a new system time offset.
+ */
+#define EC_SYSTEM_TIME_TOLERANCE_NS 1000000
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_fsm_master_restart(ec_fsm_master_t *);
+int ec_fsm_master_action_process_sii(ec_fsm_master_t *);
+int ec_fsm_master_action_process_int_request(ec_fsm_master_t *);
+void ec_fsm_master_action_idle(ec_fsm_master_t *);
+void ec_fsm_master_action_next_slave_state(ec_fsm_master_t *);
+void ec_fsm_master_action_configure(ec_fsm_master_t *);
+u64 ec_fsm_master_dc_offset32(ec_fsm_master_t *, u64, u64, unsigned long);
+u64 ec_fsm_master_dc_offset64(ec_fsm_master_t *, u64, u64, unsigned long);
+
+/****************************************************************************/
+
+void ec_fsm_master_state_start(ec_fsm_master_t *);
+void ec_fsm_master_state_broadcast(ec_fsm_master_t *);
+void ec_fsm_master_state_read_state(ec_fsm_master_t *);
+void ec_fsm_master_state_acknowledge(ec_fsm_master_t *);
+void ec_fsm_master_state_configure_slave(ec_fsm_master_t *);
+void ec_fsm_master_state_clear_addresses(ec_fsm_master_t *);
+void ec_fsm_master_state_dc_measure_delays(ec_fsm_master_t *);
+void ec_fsm_master_state_scan_slave(ec_fsm_master_t *);
+void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *);
+void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *);
+void ec_fsm_master_state_assign_sii(ec_fsm_master_t *);
+void ec_fsm_master_state_write_sii(ec_fsm_master_t *);
+void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *);
+void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);
+void ec_fsm_master_state_soe_request(ec_fsm_master_t *);
+
+void ec_fsm_master_enter_clear_addresses(ec_fsm_master_t *);
+void ec_fsm_master_enter_write_system_times(ec_fsm_master_t *);
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_master_init(
+ ec_fsm_master_t *fsm, /**< Master state machine. */
+ ec_master_t *master, /**< EtherCAT master. */
+ ec_datagram_t *datagram /**< Datagram object to use. */
+ )
+{
+ fsm->master = master;
+ fsm->datagram = datagram;
+
+ // inits the member variables state, idle, dev_idx, link_state,
+ // slaves_responding, slave_states and rescan_required
+ ec_fsm_master_reset(fsm);
+
+ fsm->retries = 0;
+ fsm->scan_jiffies = 0;
+ fsm->slave = NULL;
+ fsm->sii_request = NULL;
+ fsm->sii_index = 0;
+ fsm->sdo_request = NULL;
+ fsm->soe_request = NULL;
+
+ // init sub-state-machines
+ ec_fsm_coe_init(&fsm->fsm_coe);
+ ec_fsm_soe_init(&fsm->fsm_soe);
+ ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe);
+#ifdef EC_EOE
+ ec_fsm_eoe_init(&fsm->fsm_eoe);
+#endif
+ ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);
+ ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram,
+ &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_soe, &fsm->fsm_pdo,
+ &fsm->fsm_eoe);
+ ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram,
+ &fsm->fsm_slave_config, &fsm->fsm_pdo);
+ ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_master_clear(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ // clear sub-state machines
+ ec_fsm_coe_clear(&fsm->fsm_coe);
+ ec_fsm_soe_clear(&fsm->fsm_soe);
+ ec_fsm_pdo_clear(&fsm->fsm_pdo);
+#ifdef EC_EOE
+ ec_fsm_eoe_clear(&fsm->fsm_eoe);
+#endif
+ ec_fsm_change_clear(&fsm->fsm_change);
+ ec_fsm_slave_config_clear(&fsm->fsm_slave_config);
+ ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan);
+ ec_fsm_sii_clear(&fsm->fsm_sii);
+}
+
+/****************************************************************************/
+
+/** Reset state machine.
+ */
+void ec_fsm_master_reset(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_device_index_t dev_idx;
+
+ fsm->state = ec_fsm_master_state_start;
+ fsm->idle = 0;
+ fsm->dev_idx = EC_DEVICE_MAIN;
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(fsm->master); dev_idx++) {
+ fsm->link_state[dev_idx] = 0;
+ fsm->slaves_responding[dev_idx] = 0;
+ fsm->slave_states[dev_idx] = EC_SLAVE_STATE_UNKNOWN;
+ }
+
+ fsm->rescan_required = 0;
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * If the state machine's datagram is not sent or received yet, the execution
+ * of the state machine is delayed to the next cycle.
+ *
+ * \return true, if the state machine was executed
+ */
+int ec_fsm_master_exec(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ if (fsm->datagram->state == EC_DATAGRAM_SENT
+ || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
+ // datagram was not sent or received yet.
+ return 0;
+ }
+
+ fsm->state(fsm);
+ return 1;
+}
+
+/****************************************************************************/
+
+/**
+ * \return true, if the state machine is in an idle phase
+ */
+int ec_fsm_master_idle(
+ const ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ return fsm->idle;
+}
+
+/****************************************************************************/
+
+/** Restarts the master state machine.
+ */
+void ec_fsm_master_restart(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ fsm->dev_idx = EC_DEVICE_MAIN;
+ fsm->state = ec_fsm_master_state_start;
+ fsm->state(fsm); // execute immediately
+}
+
+/*****************************************************************************
+ * Master state machine
+ ****************************************************************************/
+
+/** Master state: START.
+ *
+ * Starts with getting slave count and slave states.
+ */
+void ec_fsm_master_state_start(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+
+ fsm->idle = 1;
+
+ // check for emergency requests
+ if (!list_empty(&master->emerg_reg_requests)) {
+ ec_reg_request_t *request;
+
+ // get first request
+ request = list_entry(master->emerg_reg_requests.next,
+ ec_reg_request_t, list);
+ list_del_init(&request->list); // dequeue
+ request->state = EC_INT_REQUEST_BUSY;
+
+ if (request->transfer_size > fsm->datagram->mem_size) {
+ EC_MASTER_ERR(master, "Emergency request data too large!\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&master->request_queue);
+ fsm->state(fsm); // continue
+ return;
+ }
+
+ if (request->dir != EC_DIR_OUTPUT) {
+ EC_MASTER_ERR(master, "Emergency requests must be"
+ " write requests!\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&master->request_queue);
+ fsm->state(fsm); // continue
+ return;
+ }
+
+ EC_MASTER_DBG(master, 1, "Writing emergency register request...\n");
+ ec_datagram_apwr(fsm->datagram, request->ring_position,
+ request->address, request->transfer_size);
+ memcpy(fsm->datagram->data, request->data, request->transfer_size);
+ fsm->datagram->device_index = EC_DEVICE_MAIN;
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&master->request_queue);
+ return;
+ }
+
+ ec_datagram_brd(fsm->datagram, 0x0130, 2);
+ ec_datagram_zero(fsm->datagram);
+ fsm->datagram->device_index = fsm->dev_idx;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_broadcast;
+}
+
+/****************************************************************************/
+
+/** Master state: BROADCAST.
+ *
+ * Processes the broadcast read slave count and slaves states.
+ */
+void ec_fsm_master_state_broadcast(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ unsigned int i, size;
+ ec_slave_t *slave;
+ ec_master_t *master = fsm->master;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ return;
+ }
+
+ // bus topology change?
+ if (datagram->working_counter != fsm->slaves_responding[fsm->dev_idx]) {
+ fsm->rescan_required = 1;
+ fsm->slaves_responding[fsm->dev_idx] = datagram->working_counter;
+ EC_MASTER_INFO(master, "%u slave(s) responding on %s device. "
+ "Re-scanning on next possibility.\n",
+ fsm->slaves_responding[fsm->dev_idx],
+ ec_device_names[fsm->dev_idx != 0]);
+ }
+
+ if (fsm->link_state[fsm->dev_idx] &&
+ !master->devices[fsm->dev_idx].link_state) {
+ ec_device_index_t dev_idx;
+
+ EC_MASTER_DBG(master, 1, "Master state machine detected "
+ "link down on %s device. Clearing slave list.\n",
+ ec_device_names[fsm->dev_idx != 0]);
+
+#ifdef EC_EOE
+ ec_master_eoe_stop(master);
+ ec_master_clear_eoe_handlers(master);
+#endif
+ ec_master_clear_slaves(master);
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(master); dev_idx++) {
+ fsm->slave_states[dev_idx] = 0x00;
+ fsm->slaves_responding[dev_idx] = 0; /* Reset to trigger rescan on
+ next link up. */
+ }
+ }
+ fsm->link_state[fsm->dev_idx] = master->devices[fsm->dev_idx].link_state;
+
+ if (datagram->state == EC_DATAGRAM_RECEIVED &&
+ fsm->slaves_responding[fsm->dev_idx]) {
+ uint8_t states = EC_READ_U8(datagram->data);
+ if (states != fsm->slave_states[fsm->dev_idx]) {
+ // slave states changed
+ char state_str[EC_STATE_STRING_SIZE];
+ fsm->slave_states[fsm->dev_idx] = states;
+ ec_state_string(states, state_str, 1);
+ EC_MASTER_INFO(master, "Slave states on %s device: %s.\n",
+ ec_device_names[fsm->dev_idx != 0], state_str);
+ }
+ } else {
+ fsm->slave_states[fsm->dev_idx] = 0x00;
+ }
+
+ fsm->dev_idx++;
+ if (fsm->dev_idx < ec_master_num_devices(master)) {
+ // check number of responding slaves on next device
+ fsm->state = ec_fsm_master_state_start;
+ fsm->state(fsm); // execute immediately
+ return;
+ }
+
+ if (fsm->rescan_required) {
+ down(&master->scan_sem);
+ if (!master->allow_scan) {
+ up(&master->scan_sem);
+ } else {
+ unsigned int count = 0, next_dev_slave, ring_position;
+ ec_device_index_t dev_idx;
+
+ master->scan_busy = 1;
+ master->scan_index = 0;
+ up(&master->scan_sem);
+
+ EC_MASTER_INFO(master, "Re-scanning now.\n");
+
+ // clear all slaves and scan the bus
+ fsm->rescan_required = 0;
+ fsm->idle = 0;
+ fsm->scan_jiffies = jiffies;
+
+#ifdef EC_EOE
+ ec_master_eoe_stop(master);
+ ec_master_clear_eoe_handlers(master);
+#endif
+ ec_master_clear_slaves(master);
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(master); dev_idx++) {
+ count += fsm->slaves_responding[dev_idx];
+ }
+
+ if (!count) {
+ // no slaves present -> finish state machine.
+ master->scan_busy = 0;
+ wake_up_interruptible(&master->scan_queue);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ size = sizeof(ec_slave_t) * count;
+ if (!(master->slaves =
+ (ec_slave_t *) kmalloc(size, GFP_KERNEL))) {
+ EC_MASTER_ERR(master, "Failed to allocate %u bytes"
+ " of slave memory!\n", size);
+ master->scan_busy = 0;
+ wake_up_interruptible(&master->scan_queue);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ // init slaves
+ dev_idx = EC_DEVICE_MAIN;
+ next_dev_slave = fsm->slaves_responding[dev_idx];
+ ring_position = 0;
+ for (i = 0; i < count; i++, ring_position++) {
+ slave = master->slaves + i;
+ while (i >= next_dev_slave) {
+ dev_idx++;
+ next_dev_slave += fsm->slaves_responding[dev_idx];
+ ring_position = 0;
+ }
+
+ ec_slave_init(slave, master, dev_idx, ring_position, i + 1);
+
+ // do not force reconfiguration in operation phase to avoid
+ // unnecesssary process data interruptions
+ if (master->phase != EC_OPERATION) {
+ slave->force_config = 1;
+ }
+ }
+ master->slave_count = count;
+ master->fsm_slave = master->slaves;
+
+ /* start with first device with slaves responding; at least one
+ * has responding slaves, otherwise count would be zero. */
+ fsm->dev_idx = EC_DEVICE_MAIN;
+ while (!fsm->slaves_responding[fsm->dev_idx]) {
+ fsm->dev_idx++;
+ }
+
+ ec_fsm_master_enter_clear_addresses(fsm);
+ return;
+ }
+ }
+
+ if (master->slave_count) {
+
+ // application applied configurations
+ if (master->config_changed) {
+ master->config_changed = 0;
+
+ EC_MASTER_DBG(master, 1, "Configuration changed.\n");
+
+ fsm->slave = master->slaves; // begin with first slave
+ ec_fsm_master_enter_write_system_times(fsm);
+
+ } else {
+ // fetch state from first slave
+ fsm->slave = master->slaves;
+ ec_datagram_fprd(fsm->datagram, fsm->slave->station_address,
+ 0x0130, 2);
+ ec_datagram_zero(datagram);
+ fsm->datagram->device_index = fsm->slave->device_index;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_read_state;
+ }
+ } else {
+ ec_fsm_master_restart(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** Check for pending SII write requests and process one.
+ *
+ * \return non-zero, if an SII write request is processed.
+ */
+int ec_fsm_master_action_process_sii(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+ ec_sii_write_request_t *request;
+ ec_slave_config_t *config;
+ ec_flag_t *flag;
+ int assign_to_pdi;
+
+ // search the first request to be processed
+ while (1) {
+ if (list_empty(&master->sii_requests))
+ break;
+
+ // get first request
+ request = list_entry(master->sii_requests.next,
+ ec_sii_write_request_t, list);
+ list_del_init(&request->list); // dequeue
+ request->state = EC_INT_REQUEST_BUSY;
+
+ assign_to_pdi = 0;
+ config = request->slave->config;
+ if (config) {
+ flag = ec_slave_config_find_flag(config, "AssignToPdi");
+ if (flag) {
+ assign_to_pdi = flag->value;
+ }
+ }
+
+ if (assign_to_pdi) {
+ fsm->sii_request = request;
+ EC_SLAVE_DBG(request->slave, 1,
+ "Assigning SII back to EtherCAT.\n");
+ ec_datagram_fpwr(fsm->datagram, request->slave->station_address,
+ 0x0500, 0x01);
+ EC_WRITE_U8(fsm->datagram->data, 0x00); // EtherCAT
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_assign_sii;
+ return 1;
+ }
+
+ // found pending SII write operation. execute it!
+ EC_SLAVE_DBG(request->slave, 1, "Writing SII data...\n");
+ fsm->sii_request = request;
+ fsm->sii_index = 0;
+ ec_fsm_sii_write(&fsm->fsm_sii, request->slave, request->offset,
+ request->words, EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+ fsm->state = ec_fsm_master_state_write_sii;
+ fsm->state(fsm); // execute immediately
+ return 1;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Check for pending internal SDO/SoE requests and process one.
+ *
+ * \return non-zero, if an SDO request is processed.
+ */
+int ec_fsm_master_action_process_int_request(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+ ec_slave_t *slave;
+ ec_sdo_request_t *sdo_req;
+ ec_soe_request_t *soe_req;
+
+ // search for internal requests to be processed
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+
+ if (!slave->config) {
+ continue;
+ }
+
+ list_for_each_entry(sdo_req, &slave->config->sdo_requests, list) {
+ if (sdo_req->state == EC_INT_REQUEST_QUEUED) {
+
+ if (ec_sdo_request_timed_out(sdo_req)) {
+ sdo_req->state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_DBG(slave, 1, "Internal SDO request"
+ " timed out.\n");
+ continue;
+ }
+
+ if (slave->current_state == EC_SLAVE_STATE_INIT) {
+ sdo_req->state = EC_INT_REQUEST_FAILURE;
+ continue;
+ }
+
+ sdo_req->state = EC_INT_REQUEST_BUSY;
+ EC_SLAVE_DBG(slave, 1, "Processing internal"
+ " SDO request...\n");
+ fsm->idle = 0;
+ fsm->sdo_request = sdo_req;
+ fsm->slave = slave;
+ fsm->state = ec_fsm_master_state_sdo_request;
+ ec_fsm_coe_transfer(&fsm->fsm_coe, slave, sdo_req);
+ ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram);
+ return 1;
+ }
+ }
+
+ list_for_each_entry(soe_req, &slave->config->soe_requests, list) {
+ if (soe_req->state == EC_INT_REQUEST_QUEUED) {
+
+ if (ec_soe_request_timed_out(soe_req)) {
+ soe_req->state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_DBG(slave, 1, "Internal SoE request"
+ " timed out.\n");
+ continue;
+ }
+
+ if (slave->current_state == EC_SLAVE_STATE_INIT) {
+ soe_req->state = EC_INT_REQUEST_FAILURE;
+ continue;
+ }
+
+ soe_req->state = EC_INT_REQUEST_BUSY;
+ EC_SLAVE_DBG(slave, 1, "Processing internal"
+ " SoE request...\n");
+ fsm->idle = 0;
+ fsm->soe_request = soe_req;
+ fsm->slave = slave;
+ fsm->state = ec_fsm_master_state_soe_request;
+ ec_fsm_soe_transfer(&fsm->fsm_soe, slave, soe_req);
+ ec_fsm_soe_exec(&fsm->fsm_soe, fsm->datagram);
+ return 1;
+ }
+ }
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Master action: IDLE.
+ *
+ * Does secondary work.
+ */
+void ec_fsm_master_action_idle(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+ ec_slave_t *slave;
+
+ // Check for pending internal SDO or SoE requests
+ if (ec_fsm_master_action_process_int_request(fsm)) {
+ return;
+ }
+
+ // enable processing of requests
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ ec_fsm_slave_set_ready(&slave->fsm);
+ }
+
+ // check, if slaves have an SDO dictionary to read out.
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)
+ || (slave->sii.has_general
+ && !slave->sii.coe_details.enable_sdo_info)
+ || slave->sdo_dictionary_fetched
+ || slave->current_state == EC_SLAVE_STATE_INIT
+ || slave->current_state == EC_SLAVE_STATE_UNKNOWN
+ || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ
+ ) continue;
+
+ EC_SLAVE_DBG(slave, 1, "Fetching SDO dictionary.\n");
+
+ slave->sdo_dictionary_fetched = 1;
+
+ // start fetching SDO dictionary
+ fsm->idle = 0;
+ fsm->slave = slave;
+ fsm->state = ec_fsm_master_state_sdo_dictionary;
+ ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);
+ ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram); // execute immediately
+ fsm->datagram->device_index = fsm->slave->device_index;
+ return;
+ }
+
+ // check for pending SII write operations.
+ if (ec_fsm_master_action_process_sii(fsm)) {
+ return; // SII write request found
+ }
+
+ ec_fsm_master_restart(fsm);
+}
+
+/****************************************************************************/
+
+/** Master action: Get state of next slave.
+ */
+void ec_fsm_master_action_next_slave_state(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+
+ // is there another slave to query?
+ fsm->slave++;
+ if (fsm->slave < master->slaves + master->slave_count) {
+ // fetch state from next slave
+ fsm->idle = 1;
+ ec_datagram_fprd(fsm->datagram,
+ fsm->slave->station_address, 0x0130, 2);
+ ec_datagram_zero(fsm->datagram);
+ fsm->datagram->device_index = fsm->slave->device_index;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_read_state;
+ return;
+ }
+
+ // all slaves processed
+ ec_fsm_master_action_idle(fsm);
+}
+
+/****************************************************************************/
+
+/** Master action: Configure.
+ */
+void ec_fsm_master_action_configure(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+ ec_slave_t *slave = fsm->slave;
+
+ if (master->config_changed) {
+ master->config_changed = 0;
+
+ // abort iterating through slaves,
+ // first compensate DC system time offsets,
+ // then begin configuring at slave 0
+ EC_MASTER_DBG(master, 1, "Configuration changed"
+ " (aborting state check).\n");
+
+ fsm->slave = master->slaves; // begin with first slave
+ ec_fsm_master_enter_write_system_times(fsm);
+ return;
+ }
+
+ // Does the slave have to be configured?
+ if ((slave->current_state != slave->requested_state
+ || slave->force_config) && !slave->error_flag) {
+
+ // Start slave configuration
+ down(&master->config_sem);
+ master->config_busy = 1;
+ up(&master->config_sem);
+
+ if (master->debug_level) {
+ char old_state[EC_STATE_STRING_SIZE],
+ new_state[EC_STATE_STRING_SIZE];
+ ec_state_string(slave->current_state, old_state, 0);
+ ec_state_string(slave->requested_state, new_state, 0);
+ EC_SLAVE_DBG(slave, 1, "Changing state from %s to %s%s.\n",
+ old_state, new_state,
+ slave->force_config ? " (forced)" : "");
+ }
+
+ fsm->idle = 0;
+ fsm->state = ec_fsm_master_state_configure_slave;
+ ec_fsm_slave_config_start(&fsm->fsm_slave_config, slave);
+ fsm->state(fsm); // execute immediately
+ fsm->datagram->device_index = fsm->slave->device_index;
+ return;
+ }
+
+ // process next slave
+ ec_fsm_master_action_next_slave_state(fsm);
+}
+
+/****************************************************************************/
+
+/** Master state: READ STATE.
+ *
+ * Fetches the AL state of a slave.
+ */
+void ec_fsm_master_state_read_state(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ return;
+ }
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: ");
+ ec_datagram_print_state(datagram);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ // did the slave not respond to its station address?
+ if (datagram->working_counter != 1) {
+ if (!slave->error_flag) {
+ slave->error_flag = 1;
+ EC_SLAVE_DBG(slave, 1, "Slave did not respond to state query.\n");
+ }
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ // A single slave responded
+ ec_slave_set_state(slave, EC_READ_U8(datagram->data));
+
+ if (!slave->error_flag) {
+ // Check, if new slave state has to be acknowledged
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ fsm->idle = 0;
+ fsm->state = ec_fsm_master_state_acknowledge;
+ ec_fsm_change_ack(&fsm->fsm_change, slave);
+ fsm->state(fsm); // execute immediately
+ return;
+ }
+
+ // No acknowlegde necessary; check for configuration
+ ec_fsm_master_action_configure(fsm);
+ return;
+ }
+
+ // slave has error flag set; process next one
+ ec_fsm_master_action_next_slave_state(fsm);
+}
+
+/****************************************************************************/
+
+/** Master state: ACKNOWLEDGE.
+ */
+void ec_fsm_master_state_acknowledge(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (ec_fsm_change_exec(&fsm->fsm_change)) {
+ return;
+ }
+
+ if (!ec_fsm_change_success(&fsm->fsm_change)) {
+ fsm->slave->error_flag = 1;
+ EC_SLAVE_ERR(slave, "Failed to acknowledge state change.\n");
+ }
+
+ ec_fsm_master_action_configure(fsm);
+}
+
+/****************************************************************************/
+
+/** Start clearing slave addresses.
+ */
+void ec_fsm_master_enter_clear_addresses(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ // broadcast clear all station addresses
+ ec_datagram_bwr(fsm->datagram, 0x0010, 2);
+ EC_WRITE_U16(fsm->datagram->data, 0x0000);
+ fsm->datagram->device_index = fsm->dev_idx;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_clear_addresses;
+}
+
+/****************************************************************************/
+
+/** Master state: CLEAR ADDRESSES.
+ */
+void ec_fsm_master_state_clear_addresses(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ return;
+ }
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_MASTER_ERR(master, "Failed to receive address"
+ " clearing datagram on %s link: ",
+ ec_device_names[fsm->dev_idx != 0]);
+ ec_datagram_print_state(datagram);
+ master->scan_busy = 0;
+ master->scan_index = master->slave_count;
+ wake_up_interruptible(&master->scan_queue);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ if (datagram->working_counter != fsm->slaves_responding[fsm->dev_idx]) {
+ EC_MASTER_WARN(master, "Failed to clear station addresses on %s link:"
+ " Cleared %u of %u",
+ ec_device_names[fsm->dev_idx != 0], datagram->working_counter,
+ fsm->slaves_responding[fsm->dev_idx]);
+ }
+
+ EC_MASTER_DBG(master, 1, "Sending broadcast-write"
+ " to measure transmission delays on %s link.\n",
+ ec_device_names[fsm->dev_idx != 0]);
+
+ ec_datagram_bwr(datagram, 0x0900, 1);
+ ec_datagram_zero(datagram);
+ fsm->datagram->device_index = fsm->dev_idx;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_dc_measure_delays;
+}
+
+/****************************************************************************/
+
+/** Master state: DC MEASURE DELAYS.
+ */
+void ec_fsm_master_state_dc_measure_delays(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ return;
+ }
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_MASTER_ERR(master, "Failed to receive delay measuring datagram"
+ " on %s link: ", ec_device_names[fsm->dev_idx != 0]);
+ ec_datagram_print_state(datagram);
+ master->scan_busy = 0;
+ master->scan_index = master->slave_count;
+ wake_up_interruptible(&master->scan_queue);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ EC_MASTER_DBG(master, 1, "%u slaves responded to delay measuring"
+ " on %s link.\n",
+ datagram->working_counter, ec_device_names[fsm->dev_idx != 0]);
+
+ do {
+ fsm->dev_idx++;
+ } while (fsm->dev_idx < ec_master_num_devices(master) &&
+ !fsm->slaves_responding[fsm->dev_idx]);
+ if (fsm->dev_idx < ec_master_num_devices(master)) {
+ ec_fsm_master_enter_clear_addresses(fsm);
+ return;
+ }
+
+ EC_MASTER_INFO(master, "Scanning bus.\n");
+
+ // begin scanning of slaves
+ fsm->slave = master->slaves;
+ master->scan_index = 0;
+ EC_MASTER_DBG(master, 1, "Scanning slave %u on %s link.\n",
+ fsm->slave->ring_position,
+ ec_device_names[fsm->slave->device_index != 0]);
+ fsm->state = ec_fsm_master_state_scan_slave;
+ ec_fsm_slave_scan_start(&fsm->fsm_slave_scan, fsm->slave);
+ ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan); // execute immediately
+ fsm->datagram->device_index = fsm->slave->device_index;
+}
+
+/****************************************************************************/
+
+/** Master state: SCAN SLAVE.
+ *
+ * Executes the sub-statemachine for the scanning of a slave.
+ */
+void ec_fsm_master_state_scan_slave(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+#ifdef EC_EOE
+ ec_slave_t *slave = fsm->slave;
+#endif
+
+ if (ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan)) {
+ return;
+ }
+
+#ifdef EC_EOE
+ if (slave->sii.mailbox_protocols & EC_MBOX_EOE) {
+ // create EoE handler for this slave
+ ec_eoe_t *eoe;
+ if (!(eoe = kmalloc(sizeof(ec_eoe_t), GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate EoE handler memory!\n");
+ } else if (ec_eoe_init(eoe, slave)) {
+ EC_SLAVE_ERR(slave, "Failed to init EoE handler!\n");
+ kfree(eoe);
+ } else {
+ list_add_tail(&eoe->list, &master->eoe_handlers);
+ }
+ }
+#endif
+
+ // another slave to fetch?
+ fsm->slave++;
+ master->scan_index++;
+ if (fsm->slave < master->slaves + master->slave_count) {
+ EC_MASTER_DBG(master, 1, "Scanning slave %u on %s link.\n",
+ fsm->slave->ring_position,
+ ec_device_names[fsm->slave->device_index != 0]);
+ ec_fsm_slave_scan_start(&fsm->fsm_slave_scan, fsm->slave);
+ ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan); // execute immediately
+ fsm->datagram->device_index = fsm->slave->device_index;
+ return;
+ }
+
+ EC_MASTER_INFO(master, "Bus scanning completed in %lu ms.\n",
+ (jiffies - fsm->scan_jiffies) * 1000 / HZ);
+
+ master->scan_busy = 0;
+ master->scan_index = master->slave_count;
+ wake_up_interruptible(&master->scan_queue);
+
+ ec_master_calc_dc(master);
+
+ // Attach slave configurations
+ ec_master_attach_slave_configs(master);
+
+#ifdef EC_EOE
+ // check if EoE processing has to be started
+ ec_master_eoe_start(master);
+#endif
+
+ if (master->slave_count) {
+ master->config_changed = 0;
+
+ fsm->slave = master->slaves; // begin with first slave
+ ec_fsm_master_enter_write_system_times(fsm);
+ } else {
+ ec_fsm_master_restart(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** Master state: CONFIGURE SLAVE.
+ *
+ * Starts configuring a slave.
+ */
+void ec_fsm_master_state_configure_slave(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+
+ if (ec_fsm_slave_config_exec(&fsm->fsm_slave_config)) {
+ return;
+ }
+
+ fsm->slave->force_config = 0;
+
+ // configuration finished
+ master->config_busy = 0;
+ wake_up_interruptible(&master->config_queue);
+
+ if (!ec_fsm_slave_config_success(&fsm->fsm_slave_config)) {
+ // TODO: mark slave_config as failed.
+ }
+
+ fsm->idle = 1;
+ ec_fsm_master_action_next_slave_state(fsm);
+}
+
+/****************************************************************************/
+
+/** Start writing DC system times.
+ */
+void ec_fsm_master_enter_write_system_times(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+
+ if (master->dc_ref_time) {
+
+ while (fsm->slave < master->slaves + master->slave_count) {
+ if (!fsm->slave->base_dc_supported
+ || !fsm->slave->has_dc_system_time) {
+ fsm->slave++;
+ continue;
+ }
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Checking system time offset.\n");
+
+ // read DC system time (0x0910, 64 bit)
+ // gap (64 bit)
+ // and time offset (0x0920, 64 bit)
+ ec_datagram_fprd(fsm->datagram, fsm->slave->station_address,
+ 0x0910, 24);
+ fsm->datagram->device_index = fsm->slave->device_index;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_dc_read_offset;
+ return;
+ }
+
+ } else {
+ if (master->active) {
+ EC_MASTER_WARN(master, "No application time received up to now,"
+ " but master already active.\n");
+ } else {
+ EC_MASTER_DBG(master, 1, "No app_time received up to now.\n");
+ }
+ }
+
+ // scanning and setting system times complete
+ ec_master_request_op(master);
+ EC_MASTER_DBG(master, 1, "After requesting OP, rescan_required is %u.\n",
+ fsm->rescan_required);
+ ec_fsm_master_restart(fsm);
+}
+
+/****************************************************************************/
+
+/** Configure 32 bit time offset.
+ *
+ * \return New offset.
+ */
+u64 ec_fsm_master_dc_offset32(
+ ec_fsm_master_t *fsm, /**< Master state machine. */
+ u64 system_time, /**< System time register. */
+ u64 old_offset, /**< Time offset register. */
+ unsigned long jiffies_since_read /**< Jiffies for correction. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ u32 correction, system_time32, old_offset32, new_offset;
+ s32 time_diff;
+
+ system_time32 = (u32) system_time;
+ old_offset32 = (u32) old_offset;
+
+ // correct read system time by elapsed time since read operation
+ correction = jiffies_since_read * 1000 / HZ * 1000000;
+ system_time32 += correction;
+ time_diff = (u32) slave->master->app_time - system_time32;
+
+ EC_SLAVE_DBG(slave, 1, "DC 32 bit system time offset calculation:"
+ " system_time=%u (corrected with %u),"
+ " app_time=%llu, diff=%i\n",
+ system_time32, correction,
+ slave->master->app_time, time_diff);
+
+ if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
+ new_offset = time_diff + old_offset32;
+ EC_SLAVE_DBG(slave, 1, "Setting time offset to %u (was %u)\n",
+ new_offset, old_offset32);
+ return (u64) new_offset;
+ } else {
+ EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n");
+ return old_offset;
+ }
+}
+
+/****************************************************************************/
+
+/** Configure 64 bit time offset.
+ *
+ * \return New offset.
+ */
+u64 ec_fsm_master_dc_offset64(
+ ec_fsm_master_t *fsm, /**< Master state machine. */
+ u64 system_time, /**< System time register. */
+ u64 old_offset, /**< Time offset register. */
+ unsigned long jiffies_since_read /**< Jiffies for correction. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ u64 new_offset, correction;
+ s64 time_diff;
+
+ // correct read system time by elapsed time since read operation
+ correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000;
+ system_time += correction;
+ time_diff = fsm->slave->master->app_time - system_time;
+
+ EC_SLAVE_DBG(slave, 1, "DC 64 bit system time offset calculation:"
+ " system_time=%llu (corrected with %llu),"
+ " app_time=%llu, diff=%lli\n",
+ system_time, correction,
+ slave->master->app_time, time_diff);
+
+ if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
+ new_offset = time_diff + old_offset;
+ EC_SLAVE_DBG(slave, 1, "Setting time offset to %llu (was %llu)\n",
+ new_offset, old_offset);
+ } else {
+ new_offset = old_offset;
+ EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n");
+ }
+
+ return new_offset;
+}
+
+/****************************************************************************/
+
+/** Master state: DC READ OFFSET.
+ */
+void ec_fsm_master_state_dc_read_offset(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ u64 system_time, old_offset, new_offset;
+ unsigned long jiffies_since_read;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_ERR(slave, "Failed to receive DC times datagram: ");
+ ec_datagram_print_state(datagram);
+ fsm->slave++;
+ ec_fsm_master_enter_write_system_times(fsm);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_WARN(slave, "Failed to get DC times: ");
+ ec_datagram_print_wc_error(datagram);
+ fsm->slave++;
+ ec_fsm_master_enter_write_system_times(fsm);
+ return;
+ }
+
+ system_time = EC_READ_U64(datagram->data); // 0x0910
+ old_offset = EC_READ_U64(datagram->data + 16); // 0x0920
+ jiffies_since_read = jiffies - datagram->jiffies_sent;
+
+ if (slave->base_dc_range == EC_DC_32) {
+ new_offset = ec_fsm_master_dc_offset32(fsm,
+ system_time, old_offset, jiffies_since_read);
+ } else {
+ new_offset = ec_fsm_master_dc_offset64(fsm,
+ system_time, old_offset, jiffies_since_read);
+ }
+
+ // set DC system time offset and transmission delay
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12);
+ EC_WRITE_U64(datagram->data, new_offset);
+ EC_WRITE_U32(datagram->data + 8, slave->transmission_delay);
+ fsm->datagram->device_index = slave->device_index;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_master_state_dc_write_offset;
+}
+
+/****************************************************************************/
+
+/** Master state: DC WRITE OFFSET.
+ */
+void ec_fsm_master_state_dc_write_offset(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_ERR(slave,
+ "Failed to receive DC system time offset datagram: ");
+ ec_datagram_print_state(datagram);
+ fsm->slave++;
+ ec_fsm_master_enter_write_system_times(fsm);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_ERR(slave, "Failed to set DC system time offset: ");
+ ec_datagram_print_wc_error(datagram);
+ fsm->slave++;
+ ec_fsm_master_enter_write_system_times(fsm);
+ return;
+ }
+
+ fsm->slave++;
+ ec_fsm_master_enter_write_system_times(fsm);
+}
+
+/****************************************************************************/
+
+/** Master state: ASSIGN SII.
+ */
+void ec_fsm_master_state_assign_sii(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_sii_write_request_t *request = fsm->sii_request;
+ ec_slave_t *slave = request->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_ERR(slave, "Failed to receive SII assignment datagram: ");
+ ec_datagram_print_state(datagram);
+ goto cont;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_ERR(slave, "Failed to assign SII back to EtherCAT: ");
+ ec_datagram_print_wc_error(datagram);
+ goto cont;
+ }
+
+cont:
+ // found pending SII write operation. execute it!
+ EC_SLAVE_DBG(slave, 1, "Writing SII data (after assignment)...\n");
+ fsm->sii_index = 0;
+ ec_fsm_sii_write(&fsm->fsm_sii, slave, request->offset,
+ request->words, EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+ fsm->state = ec_fsm_master_state_write_sii;
+ fsm->state(fsm); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Master state: WRITE SII.
+ */
+void ec_fsm_master_state_write_sii(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_master_t *master = fsm->master;
+ ec_sii_write_request_t *request = fsm->sii_request;
+ ec_slave_t *slave = request->slave;
+
+ if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
+
+ if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
+ EC_SLAVE_ERR(slave, "Failed to write SII data.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&master->request_queue);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ fsm->sii_index++;
+ if (fsm->sii_index < request->nwords) {
+ ec_fsm_sii_write(&fsm->fsm_sii, slave,
+ request->offset + fsm->sii_index,
+ request->words + fsm->sii_index,
+ EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+ ec_fsm_sii_exec(&fsm->fsm_sii); // execute immediately
+ return;
+ }
+
+ // finished writing SII
+ EC_SLAVE_DBG(slave, 1, "Finished writing %zu words of SII data.\n",
+ request->nwords);
+
+ if (request->offset <= 4 && request->offset + request->nwords > 4) {
+ // alias was written
+ slave->sii.alias = EC_READ_U16(request->words + 4);
+ // TODO: read alias from register 0x0012
+ slave->effective_alias = slave->sii.alias;
+ }
+ // TODO: Evaluate other SII contents!
+
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&master->request_queue);
+
+ // check for another SII write request
+ if (ec_fsm_master_action_process_sii(fsm))
+ return; // processing another request
+
+ ec_fsm_master_restart(fsm);
+}
+
+/****************************************************************************/
+
+/** Master state: SDO DICTIONARY.
+ */
+void ec_fsm_master_state_sdo_dictionary(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = fsm->master;
+
+ if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ // SDO dictionary fetching finished
+
+ if (master->debug_level) {
+ unsigned int sdo_count, entry_count;
+ ec_slave_sdo_dict_info(slave, &sdo_count, &entry_count);
+ EC_SLAVE_DBG(slave, 1, "Fetched %u SDOs and %u entries.\n",
+ sdo_count, entry_count);
+ }
+
+ // attach pdo names from dictionary
+ ec_slave_attach_pdo_names(slave);
+
+ ec_fsm_master_restart(fsm);
+}
+
+/****************************************************************************/
+
+/** Master state: SDO REQUEST.
+ */
+void ec_fsm_master_state_sdo_request(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_sdo_request_t *request = fsm->sdo_request;
+
+ if (!request) {
+ // configuration was cleared in the meantime
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
+ EC_SLAVE_DBG(fsm->slave, 1,
+ "Failed to process internal SDO request.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&fsm->master->request_queue);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ // SDO request finished
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&fsm->master->request_queue);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n");
+
+ // check for another SDO/SoE request
+ if (ec_fsm_master_action_process_int_request(fsm)) {
+ return; // processing another request
+ }
+
+ ec_fsm_master_restart(fsm);
+}
+
+/****************************************************************************/
+
+/** Master state: SoE REQUEST.
+ */
+void ec_fsm_master_state_soe_request(
+ ec_fsm_master_t *fsm /**< Master state machine. */
+ )
+{
+ ec_soe_request_t *request = fsm->soe_request;
+
+ if (!request) {
+ // configuration was cleared in the meantime
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ if (ec_fsm_soe_exec(&fsm->fsm_soe, fsm->datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
+ EC_SLAVE_DBG(fsm->slave, 1,
+ "Failed to process internal SoE request.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&fsm->master->request_queue);
+ ec_fsm_master_restart(fsm);
+ return;
+ }
+
+ // SoE request finished
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&fsm->master->request_queue);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SoE request.\n");
+
+ // check for another CoE/SoE request
+ if (ec_fsm_master_action_process_int_request(fsm)) {
+ return; // processing another request
+ }
+
+ ec_fsm_master_restart(fsm);
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_master.h b/ubuntu/igh-ecat/master/fsm_master.h
new file mode 100644
index 00000000000000..d645786651ee33
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_master.h
@@ -0,0 +1,107 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT master state machine.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_MASTER_H__
+#define __EC_FSM_MASTER_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "foe_request.h"
+#include "sdo_request.h"
+#include "soe_request.h"
+#include "fsm_slave_config.h"
+#include "fsm_slave_scan.h"
+#include "fsm_pdo.h"
+
+/****************************************************************************/
+
+/** SII write request.
+ */
+typedef struct {
+ struct list_head list; /**< List head. */
+ ec_slave_t *slave; /**< EtherCAT slave. */
+ uint16_t offset; /**< SII word offset. */
+ size_t nwords; /**< Number of words. */
+ const uint16_t *words; /**< Pointer to the data words. */
+ ec_internal_request_state_t state; /**< State of the request. */
+} ec_sii_write_request_t;
+
+/****************************************************************************/
+
+typedef struct ec_fsm_master ec_fsm_master_t; /**< \see ec_fsm_master */
+
+/** Finite state machine of an EtherCAT master.
+ */
+struct ec_fsm_master {
+ ec_master_t *master; /**< master the FSM runs on */
+ ec_datagram_t *datagram; /**< datagram used in the state machine */
+ unsigned int retries; /**< retries on datagram timeout. */
+
+ void (*state)(ec_fsm_master_t *); /**< master state function */
+ ec_device_index_t dev_idx; /**< Current device index (for scanning etc.).
+ */
+ int idle; /**< state machine is in idle phase */
+ unsigned long scan_jiffies; /**< beginning of slave scanning */
+ uint8_t link_state[EC_MAX_NUM_DEVICES]; /**< Last link state for every
+ device. */
+ unsigned int slaves_responding[EC_MAX_NUM_DEVICES]; /**< Number of
+ responding slaves
+ for every device. */
+ unsigned int rescan_required; /**< A bus rescan is required. */
+ ec_slave_state_t slave_states[EC_MAX_NUM_DEVICES]; /**< AL states of
+ responding slaves for
+ every device. */
+ ec_slave_t *slave; /**< current slave */
+ ec_sii_write_request_t *sii_request; /**< SII write request */
+ off_t sii_index; /**< index to SII write request data */
+ ec_sdo_request_t *sdo_request; /**< SDO request to process. */
+ ec_soe_request_t *soe_request; /**< SoE request to process. */
+
+ ec_fsm_coe_t fsm_coe; /**< CoE state machine */
+ ec_fsm_soe_t fsm_soe; /**< SoE state machine */
+ ec_fsm_pdo_t fsm_pdo; /**< PDO configuration state machine. */
+ ec_fsm_eoe_t fsm_eoe; /**< EoE state machine */
+ ec_fsm_change_t fsm_change; /**< State change state machine */
+ ec_fsm_slave_config_t fsm_slave_config; /**< slave state machine */
+ ec_fsm_slave_scan_t fsm_slave_scan; /**< slave state machine */
+ ec_fsm_sii_t fsm_sii; /**< SII state machine */
+};
+
+/****************************************************************************/
+
+void ec_fsm_master_init(ec_fsm_master_t *, ec_master_t *, ec_datagram_t *);
+void ec_fsm_master_clear(ec_fsm_master_t *);
+
+void ec_fsm_master_reset(ec_fsm_master_t *);
+
+int ec_fsm_master_exec(ec_fsm_master_t *);
+int ec_fsm_master_idle(const ec_fsm_master_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_pdo.c b/ubuntu/igh-ecat/master/fsm_pdo.c
new file mode 100644
index 00000000000000..0d5f87e16e78c8
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_pdo.c
@@ -0,0 +1,806 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT PDO configuration state machine.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "slave_config.h"
+
+#include "fsm_pdo.h"
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_fsm_pdo_print(const ec_fsm_pdo_t *);
+int ec_fsm_pdo_running(const ec_fsm_pdo_t *);
+ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(const ec_fsm_pdo_t *,
+ const struct list_head *);
+
+/****************************************************************************/
+
+void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_state_end(ec_fsm_pdo_t *, ec_datagram_t *);
+void ec_fsm_pdo_state_error(ec_fsm_pdo_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_pdo_init(
+ ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
+ ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */
+ )
+{
+ fsm->fsm_coe = fsm_coe;
+ ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);
+ ec_pdo_list_init(&fsm->pdos);
+ ec_sdo_request_init(&fsm->request);
+ ec_pdo_init(&fsm->slave_pdo);
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_pdo_clear(
+ ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ )
+{
+ ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);
+ ec_pdo_list_clear(&fsm->pdos);
+ ec_sdo_request_clear(&fsm->request);
+ ec_pdo_clear(&fsm->slave_pdo);
+}
+
+/****************************************************************************/
+
+/** Print the current and desired PDO assignment.
+ */
+void ec_fsm_pdo_print(
+ const ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ )
+{
+ printk(KERN_CONT "Currently assigned PDOs: ");
+ ec_pdo_list_print(&fsm->sync->pdos);
+ printk(KERN_CONT ". PDOs to assign: ");
+ ec_pdo_list_print(&fsm->pdos);
+ printk(KERN_CONT "\n");
+}
+
+/****************************************************************************/
+
+/** Start reading the PDO configuration.
+ */
+void ec_fsm_pdo_start_reading(
+ ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
+ ec_slave_t *slave /**< slave to configure */
+ )
+{
+ fsm->slave = slave;
+ fsm->state = ec_fsm_pdo_read_state_start;
+}
+
+/****************************************************************************/
+
+/** Start writing the PDO configuration.
+ */
+void ec_fsm_pdo_start_configuration(
+ ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
+ ec_slave_t *slave /**< slave to configure */
+ )
+{
+ fsm->slave = slave;
+ fsm->state = ec_fsm_pdo_conf_state_start;
+}
+
+/****************************************************************************/
+
+/** Get running state.
+ *
+ * \return false, if state machine has terminated
+ */
+int ec_fsm_pdo_running(
+ const ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ )
+{
+ return fsm->state != ec_fsm_pdo_state_end
+ && fsm->state != ec_fsm_pdo_state_error;
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * If the state machine's datagram is not sent or received yet, the execution
+ * of the state machine is delayed to the next cycle.
+ *
+ * \return false, if state machine has terminated
+ */
+int ec_fsm_pdo_exec(
+ ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ fsm->state(fsm, datagram);
+
+ return ec_fsm_pdo_running(fsm);
+}
+
+/****************************************************************************/
+
+/** Get execution result.
+ *
+ * \return true, if the state machine terminated gracefully
+ */
+int ec_fsm_pdo_success(
+ const ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
+ )
+{
+ return fsm->state == ec_fsm_pdo_state_end;
+}
+
+/*****************************************************************************
+ * Reading state funtions.
+ ****************************************************************************/
+
+/** Start reading PDO assignment.
+ */
+void ec_fsm_pdo_read_state_start(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ // read PDO assignment for first sync manager not reserved for mailbox
+ fsm->sync_index = 1; // next is 2
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Read PDO assignment of next sync manager.
+ */
+void ec_fsm_pdo_read_action_next_sync(
+ ec_fsm_pdo_t *fsm, /**< finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ fsm->sync_index++;
+
+ for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
+ if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index)))
+ continue;
+
+ EC_SLAVE_DBG(slave, 1, "Reading PDO assignment of SM%u.\n",
+ fsm->sync_index);
+
+ ec_pdo_list_clear_pdos(&fsm->pdos);
+
+ ecrt_sdo_request_index(&fsm->request, 0x1C10 + fsm->sync_index, 0);
+ ecrt_sdo_request_read(&fsm->request);
+ fsm->state = ec_fsm_pdo_read_state_pdo_count;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Reading of PDO configuration finished.\n");
+
+ ec_pdo_list_clear_pdos(&fsm->pdos);
+ fsm->state = ec_fsm_pdo_state_end;
+}
+
+/****************************************************************************/
+
+/** Count assigned PDOs.
+ */
+void ec_fsm_pdo_read_state_pdo_count(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs"
+ " for SM%u.\n", fsm->sync_index);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+ return;
+ }
+
+ if (fsm->request.data_size != sizeof(uint8_t)) {
+ EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
+ " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
+ fsm->request.index, fsm->request.subindex);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+ return;
+ }
+ fsm->pdo_count = EC_READ_U8(fsm->request.data);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "%u PDOs assigned.\n", fsm->pdo_count);
+
+ // read first PDO
+ fsm->pdo_pos = 1;
+ ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Read next PDO.
+ */
+void ec_fsm_pdo_read_action_next_pdo(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (fsm->pdo_pos <= fsm->pdo_count) {
+ ecrt_sdo_request_index(&fsm->request, 0x1C10 + fsm->sync_index,
+ fsm->pdo_pos);
+ ecrt_sdo_request_read(&fsm->request);
+ fsm->state = ec_fsm_pdo_read_state_pdo;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+ return;
+ }
+
+ // finished reading PDO configuration
+
+ ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos);
+ ec_pdo_list_clear_pdos(&fsm->pdos);
+
+ // next sync manager
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Fetch PDO information.
+ */
+void ec_fsm_pdo_read_state_pdo(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_ERR(fsm->slave, "Failed to read index of"
+ " assigned PDO %u from SM%u.\n",
+ fsm->pdo_pos, fsm->sync_index);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+ return;
+ }
+
+ if (fsm->request.data_size != sizeof(uint16_t)) {
+ EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
+ " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
+ fsm->request.index, fsm->request.subindex);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+ return;
+ }
+
+ if (!(fsm->pdo = (ec_pdo_t *)
+ kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
+ EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n");
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+ return;
+ }
+
+ ec_pdo_init(fsm->pdo);
+ fsm->pdo->index = EC_READ_U16(fsm->request.data);
+ fsm->pdo->sync_index = fsm->sync_index;
+
+ EC_SLAVE_DBG(fsm->slave, 1, "PDO 0x%04X.\n", fsm->pdo->index);
+
+ list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
+
+ fsm->state = ec_fsm_pdo_read_state_pdo_entries;
+ ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);
+ fsm->state(fsm, datagram); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Fetch PDO information.
+ */
+void ec_fsm_pdo_read_state_pdo_entries(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {
+ EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries"
+ " for PDO 0x%04X.\n", fsm->pdo->index);
+ ec_fsm_pdo_read_action_next_sync(fsm, datagram);
+ return;
+ }
+
+ // next PDO
+ fsm->pdo_pos++;
+ ec_fsm_pdo_read_action_next_pdo(fsm, datagram);
+}
+
+/*****************************************************************************
+ * Writing state functions.
+ ****************************************************************************/
+
+/** Start PDO configuration.
+ */
+void ec_fsm_pdo_conf_state_start(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (!fsm->slave->config) {
+ fsm->state = ec_fsm_pdo_state_end;
+ return;
+ }
+
+ fsm->sync_index = 1; // next is 2
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Assign next PDO.
+ *
+ * \return Next PDO, or NULL.
+ */
+ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(
+ const ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */
+ const struct list_head *list /**< current PDO list item */
+ )
+{
+ list = list->next;
+ if (list == &fsm->pdos.list)
+ return NULL; // no next PDO
+ return list_entry(list, ec_pdo_t, list);
+}
+
+/****************************************************************************/
+
+/** Get the next sync manager for a pdo configuration.
+ */
+void ec_fsm_pdo_conf_action_next_sync(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ fsm->sync_index++;
+
+ for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
+ if (!fsm->slave->config) {
+ // slave configuration removed in the meantime
+ fsm->state = ec_fsm_pdo_state_error;
+ return;
+ }
+
+ if (ec_pdo_list_copy(&fsm->pdos,
+ &fsm->slave->config->sync_configs[fsm->sync_index].pdos))
+ {
+ fsm->state = ec_fsm_pdo_state_error;
+ return;
+ }
+
+ if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {
+ if (!list_empty(&fsm->pdos.list))
+ EC_SLAVE_WARN(fsm->slave, "PDOs configured for SM%u,"
+ " but slave does not provide the"
+ " sync manager information!\n",
+ fsm->sync_index);
+ continue;
+ }
+
+ // get first configured PDO
+ if (!(fsm->pdo =
+ ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
+ // no pdos configured
+ ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
+ return;
+ }
+
+ ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
+ return;
+ }
+
+ fsm->state = ec_fsm_pdo_state_end;
+}
+
+/****************************************************************************/
+
+/** Check if the mapping has to be read, otherwise start to configure it.
+ */
+void ec_fsm_pdo_conf_action_pdo_mapping(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ const ec_pdo_t *assigned_pdo;
+
+ fsm->slave_pdo.index = fsm->pdo->index;
+
+ if ((assigned_pdo = ec_slave_find_pdo(fsm->slave, fsm->pdo->index))) {
+ ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);
+ } else { // configured PDO is not assigned and thus unknown
+ ec_pdo_clear_entries(&fsm->slave_pdo);
+ }
+
+ if (list_empty(&fsm->slave_pdo.entries)) {
+ EC_SLAVE_DBG(fsm->slave, 1, "Reading mapping of PDO 0x%04X.\n",
+ fsm->pdo->index);
+
+ // pdo mapping is unknown; start loading it
+ ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,
+ &fsm->slave_pdo);
+ fsm->state = ec_fsm_pdo_conf_state_read_mapping;
+ fsm->state(fsm, datagram); // execute immediately
+ return;
+ }
+
+ // pdo mapping is known, check if it most be re-configured
+ ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Execute the PDO entry state machine to read the current PDO's mapping.
+ */
+void ec_fsm_pdo_conf_state_read_mapping(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
+ EC_SLAVE_WARN(fsm->slave,
+ "Failed to read PDO entries for PDO 0x%04X.\n",
+ fsm->pdo->index);
+
+ // check if the mapping must be re-configured
+ ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Check if the mapping has to be re-configured.
+ *
+ * \todo Display mapping differences.
+ */
+void ec_fsm_pdo_conf_action_check_mapping(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ // check, if slave supports PDO configuration
+ if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
+ && fsm->slave->sii.has_general
+ && fsm->slave->sii.coe_details.enable_pdo_configuration) {
+
+ // always write PDO mapping
+ ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
+ fsm->pdo, &fsm->slave_pdo);
+ fsm->state = ec_fsm_pdo_conf_state_mapping;
+ fsm->state(fsm, datagram); // execure immediately
+ return;
+ }
+ else if (!ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
+ EC_SLAVE_WARN(fsm->slave, "Slave does not support"
+ " changing the PDO mapping!\n");
+ EC_SLAVE_WARN(fsm->slave, "");
+ printk(KERN_CONT "Currently mapped PDO entries: ");
+ ec_pdo_print_entries(&fsm->slave_pdo);
+ printk(KERN_CONT ". Entries to map: ");
+ ec_pdo_print_entries(fsm->pdo);
+ printk(KERN_CONT "\n");
+ }
+
+ ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Let the PDO entry state machine configure the current PDO's mapping.
+ */
+void ec_fsm_pdo_conf_state_mapping(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
+ EC_SLAVE_WARN(fsm->slave,
+ "Failed to configure mapping of PDO 0x%04X.\n",
+ fsm->pdo->index);
+
+ ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Check mapping of next PDO, otherwise configure assignment.
+ */
+void ec_fsm_pdo_conf_action_next_pdo_mapping(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ // get next configured PDO
+ if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
+ // no more configured pdos
+ ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);
+ return;
+ }
+
+ ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Check if the PDO assignment of the current SM has to be re-configured.
+ */
+void ec_fsm_pdo_conf_action_check_assignment(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
+ && fsm->slave->sii.has_general
+ && fsm->slave->sii.coe_details.enable_pdo_assign) {
+
+ // always write PDO assignment
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(fsm->slave, 1, "Setting PDO assignment of SM%u:\n",
+ fsm->sync_index);
+ EC_SLAVE_DBG(fsm->slave, 1, ""); ec_fsm_pdo_print(fsm);
+ }
+
+ if (ec_sdo_request_alloc(&fsm->request, 2)) {
+ fsm->state = ec_fsm_pdo_state_error;
+ return;
+ }
+
+ // set mapped PDO count to zero
+ EC_WRITE_U8(fsm->request.data, 0); // zero PDOs mapped
+ fsm->request.data_size = 1;
+ ecrt_sdo_request_index(&fsm->request, 0x1C10 + fsm->sync_index, 0);
+ ecrt_sdo_request_write(&fsm->request);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Setting number of assigned"
+ " PDOs to zero.\n");
+
+ fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+ return;
+ }
+ else if (!ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
+ EC_SLAVE_WARN(fsm->slave, "Slave does not support assigning PDOs!\n");
+ EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
+ }
+
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Set the number of assigned PDOs to zero.
+ */
+void ec_fsm_pdo_conf_state_zero_pdo_count(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n",
+ fsm->sync_index);
+ EC_SLAVE_WARN(fsm->slave, "");
+ ec_fsm_pdo_print(fsm);
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
+ return;
+ }
+
+ // the sync manager's assigned PDOs have been cleared
+ ec_pdo_list_clear_pdos(&fsm->sync->pdos);
+
+ // assign all PDOs belonging to the current sync manager
+
+ // find first PDO
+ if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
+ // check for mapping to be altered
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
+ return;
+ }
+
+ // assign first PDO
+ fsm->pdo_pos = 1;
+ ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Assign a PDO.
+ */
+void ec_fsm_pdo_conf_action_assign_pdo(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ EC_WRITE_U16(fsm->request.data, fsm->pdo->index);
+ fsm->request.data_size = 2;
+ ecrt_sdo_request_index(&fsm->request,
+ 0x1C10 + fsm->sync_index, fsm->pdo_pos);
+ ecrt_sdo_request_write(&fsm->request);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Assigning PDO 0x%04X at position %u.\n",
+ fsm->pdo->index, fsm->pdo_pos);
+
+ fsm->state = ec_fsm_pdo_conf_state_assign_pdo;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Add a PDO to the sync managers PDO assignment.
+ */
+void ec_fsm_pdo_conf_state_assign_pdo(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u"
+ " of SM%u.\n",
+ fsm->pdo->index, fsm->pdo_pos, fsm->sync_index);
+ EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
+ fsm->state = ec_fsm_pdo_state_error;
+ return;
+ }
+
+ // find next PDO
+ if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {
+ // no more PDOs to assign, set PDO count
+ EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);
+ fsm->request.data_size = 1;
+ ecrt_sdo_request_index(&fsm->request, 0x1C10 + fsm->sync_index, 0);
+ ecrt_sdo_request_write(&fsm->request);
+
+ EC_SLAVE_DBG(fsm->slave, 1,
+ "Setting number of assigned PDOs to %u.\n",
+ fsm->pdo_pos);
+
+ fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+ return;
+ }
+
+ // add next PDO to assignment
+ fsm->pdo_pos++;
+ ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Set the number of assigned PDOs.
+ */
+void ec_fsm_pdo_conf_state_set_pdo_count(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_WARN(fsm->slave, "Failed to set number of"
+ " assigned PDOs of SM%u.\n", fsm->sync_index);
+ EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
+ fsm->state = ec_fsm_pdo_state_error;
+ return;
+ }
+
+ // PDOs have been configured
+ ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured"
+ " PDO assignment of SM%u.\n", fsm->sync_index);
+
+ // check if PDO mapping has to be altered
+ ec_fsm_pdo_conf_action_next_sync(fsm, datagram);
+}
+
+/*****************************************************************************
+ * Common state functions
+ ****************************************************************************/
+
+/** State: ERROR.
+ */
+void ec_fsm_pdo_state_error(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** State: END.
+ */
+void ec_fsm_pdo_state_end(
+ ec_fsm_pdo_t *fsm, /**< Finite state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_pdo.h b/ubuntu/igh-ecat/master/fsm_pdo.h
new file mode 100644
index 00000000000000..41fc876a23ba75
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_pdo.h
@@ -0,0 +1,76 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT PDO configuration state machine structures.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_PDO_H__
+#define __EC_FSM_PDO_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "fsm_coe.h"
+#include "fsm_pdo_entry.h"
+
+/****************************************************************************/
+
+/**
+ * \see ec_fsm_pdo
+ */
+typedef struct ec_fsm_pdo ec_fsm_pdo_t;
+
+/** PDO configuration state machine.
+ */
+struct ec_fsm_pdo
+{
+ void (*state)(ec_fsm_pdo_t *, ec_datagram_t *); /**< State function. */
+ ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use. */
+ ec_fsm_pdo_entry_t fsm_pdo_entry; /**< PDO entry state machine. */
+ ec_pdo_list_t pdos; /**< PDO configuration. */
+ ec_sdo_request_t request; /**< SDO request. */
+ ec_pdo_t slave_pdo; /**< PDO actually appearing in a slave. */
+
+ ec_slave_t *slave; /**< Slave the FSM runs on. */
+ uint8_t sync_index; /**< Current sync manager index. */
+ ec_sync_t *sync; /**< Current sync manager. */
+ ec_pdo_t *pdo; /**< Current PDO. */
+ unsigned int pdo_pos; /**< Assignment position of current PDOs. */
+ unsigned int pdo_count; /**< Number of assigned PDOs. */
+};
+
+/****************************************************************************/
+
+void ec_fsm_pdo_init(ec_fsm_pdo_t *, ec_fsm_coe_t *);
+void ec_fsm_pdo_clear(ec_fsm_pdo_t *);
+
+void ec_fsm_pdo_start_reading(ec_fsm_pdo_t *, ec_slave_t *);
+void ec_fsm_pdo_start_configuration(ec_fsm_pdo_t *, ec_slave_t *);
+
+int ec_fsm_pdo_exec(ec_fsm_pdo_t *, ec_datagram_t *);
+int ec_fsm_pdo_success(const ec_fsm_pdo_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_pdo_entry.c b/ubuntu/igh-ecat/master/fsm_pdo_entry.c
new file mode 100644
index 00000000000000..3c7be428925207
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_pdo_entry.c
@@ -0,0 +1,541 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT PDO mapping state machine.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "slave_config.h"
+
+#include "fsm_pdo_entry.h"
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_fsm_pdo_entry_print(const ec_fsm_pdo_entry_t *);
+int ec_fsm_pdo_entry_running(const ec_fsm_pdo_entry_t *);
+ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(const ec_fsm_pdo_entry_t *,
+ const struct list_head *);
+
+/****************************************************************************/
+
+void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *,
+ ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *,
+ ec_datagram_t *);
+void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *,
+ ec_datagram_t *);
+
+void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_pdo_entry_init(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_fsm_coe_t *fsm_coe /**< CoE state machine to use. */
+ )
+{
+ fsm->fsm_coe = fsm_coe;
+ ec_sdo_request_init(&fsm->request);
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_pdo_entry_clear(
+ ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ )
+{
+ ec_sdo_request_clear(&fsm->request);
+}
+
+/****************************************************************************/
+
+/** Print the current and desired PDO mapping.
+ */
+void ec_fsm_pdo_entry_print(
+ const ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ )
+{
+ printk(KERN_CONT "Currently mapped PDO entries: ");
+ ec_pdo_print_entries(fsm->cur_pdo);
+ printk(KERN_CONT ". Entries to map: ");
+ ec_pdo_print_entries(fsm->source_pdo);
+ printk(KERN_CONT "\n");
+}
+
+/****************************************************************************/
+
+/** Start reading a PDO's entries.
+ */
+void ec_fsm_pdo_entry_start_reading(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_slave_t *slave, /**< Slave to configure. */
+ ec_pdo_t *pdo /**< PDO to read entries for. */
+ )
+{
+ fsm->slave = slave;
+ fsm->target_pdo = pdo;
+
+ ec_pdo_clear_entries(fsm->target_pdo);
+
+ fsm->state = ec_fsm_pdo_entry_read_state_start;
+}
+
+/****************************************************************************/
+
+/** Start PDO mapping state machine.
+ */
+void ec_fsm_pdo_entry_start_configuration(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_slave_t *slave, /**< Slave to configure. */
+ const ec_pdo_t *pdo, /**< PDO with the desired entries. */
+ const ec_pdo_t *cur_pdo /**< Current PDO mapping. */
+ )
+{
+ fsm->slave = slave;
+ fsm->source_pdo = pdo;
+ fsm->cur_pdo = cur_pdo;
+
+ if (fsm->slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 1, "Changing mapping of PDO 0x%04X.\n",
+ pdo->index);
+ EC_SLAVE_DBG(slave, 1, ""); ec_fsm_pdo_entry_print(fsm);
+ }
+
+ fsm->state = ec_fsm_pdo_entry_conf_state_start;
+}
+
+/****************************************************************************/
+
+/** Get running state.
+ *
+ * \return false, if state machine has terminated
+ */
+int ec_fsm_pdo_entry_running(
+ const ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ )
+{
+ return fsm->state != ec_fsm_pdo_entry_state_end
+ && fsm->state != ec_fsm_pdo_entry_state_error;
+}
+
+/****************************************************************************/
+
+/** Executes the current state.
+ *
+ * \return false, if state machine has terminated
+ */
+int ec_fsm_pdo_entry_exec(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ fsm->state(fsm, datagram);
+
+ return ec_fsm_pdo_entry_running(fsm);
+}
+
+/****************************************************************************/
+
+/** Get execution result.
+ *
+ * \return true, if the state machine terminated gracefully
+ */
+int ec_fsm_pdo_entry_success(
+ const ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */
+ )
+{
+ return fsm->state == ec_fsm_pdo_entry_state_end;
+}
+
+/*****************************************************************************
+ * Reading state functions.
+ ****************************************************************************/
+
+/** Request reading the number of mapped PDO entries.
+ */
+void ec_fsm_pdo_entry_read_state_start(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index, 0);
+ ecrt_sdo_request_read(&fsm->request);
+
+ fsm->state = ec_fsm_pdo_entry_read_state_count;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Read number of mapped PDO entries.
+ */
+void ec_fsm_pdo_entry_read_state_count(
+ ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_ERR(fsm->slave,
+ "Failed to read number of mapped PDO entries.\n");
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ if (fsm->request.data_size != sizeof(uint8_t)) {
+ EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at uploading"
+ " SDO 0x%04X:%02X.\n",
+ fsm->request.data_size, fsm->request.index,
+ fsm->request.subindex);
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ fsm->entry_count = EC_READ_U8(fsm->request.data);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "%u PDO entries mapped.\n", fsm->entry_count);
+
+ // read first PDO entry
+ fsm->entry_pos = 1;
+ ec_fsm_pdo_entry_read_action_next(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Read next PDO entry.
+ */
+void ec_fsm_pdo_entry_read_action_next(
+ ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (fsm->entry_pos <= fsm->entry_count) {
+ ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index,
+ fsm->entry_pos);
+ ecrt_sdo_request_read(&fsm->request);
+ fsm->state = ec_fsm_pdo_entry_read_state_entry;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+ return;
+ }
+
+ // finished reading entries.
+ fsm->state = ec_fsm_pdo_entry_state_end;
+}
+
+/****************************************************************************/
+
+/** Read PDO entry information.
+ */
+void ec_fsm_pdo_entry_read_state_entry(
+ ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n");
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ if (fsm->request.data_size != sizeof(uint32_t)) {
+ EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at"
+ " uploading SDO 0x%04X:%02X.\n",
+ fsm->request.data_size, fsm->request.index,
+ fsm->request.subindex);
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ } else {
+ uint32_t pdo_entry_info;
+ ec_pdo_entry_t *pdo_entry;
+
+ pdo_entry_info = EC_READ_U32(fsm->request.data);
+
+ if (!(pdo_entry = (ec_pdo_entry_t *)
+ kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
+ EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO entry.\n");
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ ec_pdo_entry_init(pdo_entry);
+ pdo_entry->index = pdo_entry_info >> 16;
+ pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;
+ pdo_entry->bit_length = pdo_entry_info & 0xFF;
+
+ if (!pdo_entry->index && !pdo_entry->subindex) {
+ if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {
+ ec_pdo_entry_clear(pdo_entry);
+ kfree(pdo_entry);
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+ }
+
+ EC_SLAVE_DBG(fsm->slave, 1,
+ "PDO entry 0x%04X:%02X, %u bit, \"%s\".\n",
+ pdo_entry->index, pdo_entry->subindex,
+ pdo_entry->bit_length,
+ pdo_entry->name ? pdo_entry->name : "???");
+
+ list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries);
+
+ // next PDO entry
+ fsm->entry_pos++;
+ ec_fsm_pdo_entry_read_action_next(fsm, datagram);
+ }
+}
+
+/*****************************************************************************
+ * Configuration state functions.
+ ****************************************************************************/
+
+/** Start PDO mapping.
+ */
+void ec_fsm_pdo_entry_conf_state_start(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_sdo_request_alloc(&fsm->request, 4)) {
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ // set mapped PDO entry count to zero
+ EC_WRITE_U8(fsm->request.data, 0);
+ fsm->request.data_size = 1;
+ ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
+ ecrt_sdo_request_write(&fsm->request);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Setting entry count to zero.\n");
+
+ fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Process next PDO entry.
+ *
+ * \return Next PDO entry, or NULL.
+ */
+ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(
+ const ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ const struct list_head *list /**< current entry list item */
+ )
+{
+ list = list->next;
+ if (list == &fsm->source_pdo->entries)
+ return NULL; // no next entry
+ return list_entry(list, ec_pdo_entry_t, list);
+}
+
+/****************************************************************************/
+
+/** Set the number of mapped entries to zero.
+ */
+void ec_fsm_pdo_entry_conf_state_zero_entry_count(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n");
+ EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ // find first entry
+ if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
+ fsm, &fsm->source_pdo->entries))) {
+
+ EC_SLAVE_DBG(fsm->slave, 1, "No entries to map.\n");
+
+ fsm->state = ec_fsm_pdo_entry_state_end; // finished
+ return;
+ }
+
+ // add first entry
+ fsm->entry_pos = 1;
+ ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Starts to add a PDO entry.
+ */
+void ec_fsm_pdo_entry_conf_action_map(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ uint32_t value;
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Mapping PDO entry 0x%04X:%02X (%u bit)"
+ " at position %u.\n",
+ fsm->entry->index, fsm->entry->subindex,
+ fsm->entry->bit_length, fsm->entry_pos);
+
+ value = fsm->entry->index << 16
+ | fsm->entry->subindex << 8 | fsm->entry->bit_length;
+ EC_WRITE_U32(fsm->request.data, value);
+ fsm->request.data_size = 4;
+ ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index,
+ fsm->entry_pos);
+ ecrt_sdo_request_write(&fsm->request);
+
+ fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Add a PDO entry.
+ */
+void ec_fsm_pdo_entry_conf_state_map_entry(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"
+ " 0x%04X:%02X (%u bit) to position %u.\n",
+ fsm->entry->index, fsm->entry->subindex,
+ fsm->entry->bit_length, fsm->entry_pos);
+ EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ // find next entry
+ if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
+ fsm, &fsm->entry->list))) {
+
+ // No more entries to add. Write entry count.
+ EC_WRITE_U8(fsm->request.data, fsm->entry_pos);
+ fsm->request.data_size = 1;
+ ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
+ ecrt_sdo_request_write(&fsm->request);
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Setting number of PDO entries to %u.\n",
+ fsm->entry_pos);
+
+ fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
+ ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
+ return;
+ }
+
+ // add next entry
+ fsm->entry_pos++;
+ ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
+}
+
+/****************************************************************************/
+
+/** Set the number of entries.
+ */
+void ec_fsm_pdo_entry_conf_state_set_entry_count(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n");
+ EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
+ fsm->state = ec_fsm_pdo_entry_state_error;
+ return;
+ }
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured"
+ " mapping for PDO 0x%04X.\n", fsm->source_pdo->index);
+
+ fsm->state = ec_fsm_pdo_entry_state_end; // finished
+}
+
+/*****************************************************************************
+ * Common state functions
+ ****************************************************************************/
+
+/** State: ERROR.
+ */
+void ec_fsm_pdo_entry_state_error(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** State: END.
+ */
+void ec_fsm_pdo_entry_state_end(
+ ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_pdo_entry.h b/ubuntu/igh-ecat/master/fsm_pdo_entry.h
new file mode 100644
index 00000000000000..d6638af65c7005
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_pdo_entry.h
@@ -0,0 +1,75 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT PDO entry configuration state machine structures.
+ */
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_PDO_ENTRY_H__
+#define __EC_FSM_PDO_ENTRY_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "fsm_coe.h"
+
+/****************************************************************************/
+
+/**
+ * \see ec_fsm_pdo_entry
+ */
+typedef struct ec_fsm_pdo_entry ec_fsm_pdo_entry_t;
+
+/** PDO configuration state machine.
+ */
+struct ec_fsm_pdo_entry
+{
+ void (*state)(ec_fsm_pdo_entry_t *, ec_datagram_t *); /**< state function
+ */
+ ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */
+ ec_sdo_request_t request; /**< SDO request. */
+
+ ec_slave_t *slave; /**< Slave the FSM runs on. */
+ ec_pdo_t *target_pdo; /**< PDO to read the mapping for. */
+ const ec_pdo_t *source_pdo; /**< PDO with desired mapping. */
+ const ec_pdo_t *cur_pdo; /**< PDO with current mapping (display only). */
+ const ec_pdo_entry_t *entry; /**< Current entry. */
+ unsigned int entry_count; /**< Number of entries. */
+ unsigned int entry_pos; /**< Position in PDO mapping. */
+};
+
+/****************************************************************************/
+
+void ec_fsm_pdo_entry_init(ec_fsm_pdo_entry_t *, ec_fsm_coe_t *);
+void ec_fsm_pdo_entry_clear(ec_fsm_pdo_entry_t *);
+
+void ec_fsm_pdo_entry_start_reading(ec_fsm_pdo_entry_t *, ec_slave_t *,
+ ec_pdo_t *);
+void ec_fsm_pdo_entry_start_configuration(ec_fsm_pdo_entry_t *, ec_slave_t *,
+ const ec_pdo_t *, const ec_pdo_t *);
+
+int ec_fsm_pdo_entry_exec(ec_fsm_pdo_entry_t *, ec_datagram_t *);
+int ec_fsm_pdo_entry_success(const ec_fsm_pdo_entry_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_sii.c b/ubuntu/igh-ecat/master/fsm_sii.c
new file mode 100644
index 00000000000000..7e5954915499cc
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_sii.c
@@ -0,0 +1,482 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave information interface FSM.
+*/
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "mailbox.h"
+#include "master.h"
+#include "fsm_sii.h"
+
+/** Read/write timeout [ms].
+ *
+ * Used to calculate timeouts bsed on the jiffies counter.
+ *
+ * \attention Must be more than 10 to avoid problems on kernels that run with
+ * a timer interupt frequency of 100 Hz.
+ */
+#define SII_TIMEOUT 20
+
+/** Time before evaluating answer at writing [ms].
+ */
+#define SII_INHIBIT 5
+
+//#define SII_DEBUG
+
+/****************************************************************************/
+
+void ec_fsm_sii_state_start_reading(ec_fsm_sii_t *);
+void ec_fsm_sii_state_read_check(ec_fsm_sii_t *);
+void ec_fsm_sii_state_read_fetch(ec_fsm_sii_t *);
+void ec_fsm_sii_state_start_writing(ec_fsm_sii_t *);
+void ec_fsm_sii_state_write_check(ec_fsm_sii_t *);
+void ec_fsm_sii_state_write_check2(ec_fsm_sii_t *);
+void ec_fsm_sii_state_end(ec_fsm_sii_t *);
+void ec_fsm_sii_state_error(ec_fsm_sii_t *);
+
+/****************************************************************************/
+
+/**
+ Constructor.
+*/
+
+void ec_fsm_sii_init(ec_fsm_sii_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< datagram structure to use */
+ )
+{
+ fsm->state = NULL;
+ fsm->datagram = datagram;
+}
+
+/****************************************************************************/
+
+/**
+ Destructor.
+*/
+
+void ec_fsm_sii_clear(ec_fsm_sii_t *fsm /**< finite state machine */)
+{
+}
+
+/****************************************************************************/
+
+/**
+ Initializes the SII read state machine.
+*/
+
+void ec_fsm_sii_read(ec_fsm_sii_t *fsm, /**< finite state machine */
+ ec_slave_t *slave, /**< slave to read from */
+ uint16_t word_offset, /**< offset to read from */
+ ec_fsm_sii_addressing_t mode /**< addressing scheme */
+ )
+{
+ fsm->state = ec_fsm_sii_state_start_reading;
+ fsm->slave = slave;
+ fsm->word_offset = word_offset;
+ fsm->mode = mode;
+}
+
+/****************************************************************************/
+
+/**
+ Initializes the SII write state machine.
+*/
+
+void ec_fsm_sii_write(ec_fsm_sii_t *fsm, /**< finite state machine */
+ ec_slave_t *slave, /**< slave to read from */
+ uint16_t word_offset, /**< offset to read from */
+ const uint16_t *value, /**< pointer to 2 bytes of data */
+ ec_fsm_sii_addressing_t mode /**< addressing scheme */
+ )
+{
+ fsm->state = ec_fsm_sii_state_start_writing;
+ fsm->slave = slave;
+ fsm->word_offset = word_offset;
+ fsm->mode = mode;
+ memcpy(fsm->value, value, 2);
+}
+
+/****************************************************************************/
+
+/**
+ Executes the SII state machine.
+ \return false, if the state machine has terminated
+*/
+
+int ec_fsm_sii_exec(ec_fsm_sii_t *fsm /**< finite state machine */)
+{
+ fsm->state(fsm);
+
+ return fsm->state != ec_fsm_sii_state_end
+ && fsm->state != ec_fsm_sii_state_error;
+}
+
+/****************************************************************************/
+
+/**
+ Returns, if the master startup state machine terminated with success.
+ \return non-zero if successful.
+*/
+
+int ec_fsm_sii_success(ec_fsm_sii_t *fsm /**< Finite state machine */)
+{
+ return fsm->state == ec_fsm_sii_state_end;
+}
+
+/*****************************************************************************
+ * state functions
+ ****************************************************************************/
+
+/**
+ SII state: START READING.
+ Starts reading the slave information interface.
+*/
+
+void ec_fsm_sii_state_start_reading(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ // initiate read operation
+ switch (fsm->mode) {
+ case EC_FSM_SII_USE_INCREMENT_ADDRESS:
+ ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4);
+ break;
+ case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
+ ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 4);
+ break;
+ }
+
+ EC_WRITE_U8 (datagram->data, 0x80); // two address octets
+ EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation
+ EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
+
+#ifdef SII_DEBUG
+ EC_SLAVE_DBG(fsm->slave, 0, "reading SII data, word %u:\n",
+ fsm->word_offset);
+ ec_print_data(datagram->data, 4);
+#endif
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_sii_state_read_check;
+}
+
+/****************************************************************************/
+
+/**
+ SII state: READ CHECK.
+ Checks, if the SII-read-datagram has been sent and issues a fetch datagram.
+*/
+
+void ec_fsm_sii_state_read_check(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed to receive SII read datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Reception of SII read datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ fsm->jiffies_start = datagram->jiffies_sent;
+ fsm->check_once_more = 1;
+
+ // issue check/fetch datagram
+ switch (fsm->mode) {
+ case EC_FSM_SII_USE_INCREMENT_ADDRESS:
+ ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10);
+ break;
+ case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
+ ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 10);
+ break;
+ }
+
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_sii_state_read_fetch;
+}
+
+/****************************************************************************/
+
+/**
+ SII state: READ FETCH.
+ Fetches the result of an SII-read datagram.
+*/
+void ec_fsm_sii_state_read_fetch(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave,
+ "Failed to receive SII check/fetch datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave,
+ "Reception of SII check/fetch datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+#ifdef SII_DEBUG
+ EC_SLAVE_DBG(fsm->slave, 0, "checking SII read state:\n");
+ ec_print_data(datagram->data, 10);
+#endif
+
+ if (EC_READ_U8(datagram->data + 1) & 0x20) {
+ EC_SLAVE_ERR(fsm->slave, "Error on last command while"
+ " reading from SII word 0x%04x.\n", fsm->word_offset);
+ fsm->state = ec_fsm_sii_state_error;
+ return;
+ }
+
+ // check "busy bit"
+ if (EC_READ_U8(datagram->data + 1) & 0x81) { /* busy bit or
+ read operation busy */
+ // still busy... timeout?
+ unsigned long diff_ms =
+ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= SII_TIMEOUT) {
+ if (fsm->check_once_more) {
+ fsm->check_once_more = 0;
+ } else {
+ EC_SLAVE_ERR(fsm->slave, "SII: Read timeout.\n");
+ fsm->state = ec_fsm_sii_state_error;
+ return;
+ }
+ }
+
+ // issue check/fetch datagram again
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // SII value received.
+ memcpy(fsm->value, datagram->data + 6, 4);
+ fsm->state = ec_fsm_sii_state_end;
+}
+
+/****************************************************************************/
+
+/**
+ SII state: START WRITING.
+ Starts writing a word through the slave information interface.
+*/
+
+void ec_fsm_sii_state_start_writing(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ // initiate write operation
+ ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 8);
+ EC_WRITE_U8 (datagram->data, 0x81); /* two address octets
+ + enable write access */
+ EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation
+ EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
+ memset(datagram->data + 4, 0x00, 2);
+ memcpy(datagram->data + 6, fsm->value, 2);
+
+#ifdef SII_DEBUG
+ EC_SLAVE_DBG(fsm->slave, 0, "writing SII data:\n");
+ ec_print_data(datagram->data, 8);
+#endif
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_sii_state_write_check;
+}
+
+/****************************************************************************/
+
+/**
+ SII state: WRITE CHECK.
+*/
+
+void ec_fsm_sii_state_write_check(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed to receive SII write datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Reception of SII write datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ fsm->jiffies_start = datagram->jiffies_sent;
+ fsm->check_once_more = 1;
+
+ // issue check datagram
+ ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_sii_state_write_check2;
+}
+
+/****************************************************************************/
+
+/**
+ SII state: WRITE CHECK 2.
+*/
+
+void ec_fsm_sii_state_write_check2(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ unsigned long diff_ms;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave,
+ "Failed to receive SII write check datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_sii_state_error;
+ EC_SLAVE_ERR(fsm->slave,
+ "Reception of SII write check datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+#ifdef SII_DEBUG
+ EC_SLAVE_DBG(fsm->slave, 0, "checking SII write state:\n");
+ ec_print_data(datagram->data, 2);
+#endif
+
+ if (EC_READ_U8(datagram->data + 1) & 0x20) {
+ EC_SLAVE_ERR(fsm->slave, "SII: Error on last SII command!\n");
+ fsm->state = ec_fsm_sii_state_error;
+ return;
+ }
+
+ /* FIXME: some slaves never answer with the busy flag set...
+ * wait a few ms for the write operation to complete. */
+ diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms < SII_INHIBIT) {
+#ifdef SII_DEBUG
+ EC_SLAVE_DBG(fsm->slave, 0, "too early.\n");
+#endif
+ // issue check datagram again
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ if (EC_READ_U8(datagram->data + 1) & 0x82) { /* busy bit or
+ write operation busy bit */
+ // still busy... timeout?
+ if (diff_ms >= SII_TIMEOUT) {
+ if (fsm->check_once_more) {
+ fsm->check_once_more = 0;
+ } else {
+ EC_SLAVE_ERR(fsm->slave, "SII: Write timeout.\n");
+ fsm->state = ec_fsm_sii_state_error;
+ return;
+ }
+ }
+
+ // issue check datagram again
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ if (EC_READ_U8(datagram->data + 1) & 0x40) {
+ EC_SLAVE_ERR(fsm->slave, "SII: Write operation failed!\n");
+ fsm->state = ec_fsm_sii_state_error;
+ return;
+ }
+
+ // success
+ fsm->state = ec_fsm_sii_state_end;
+}
+
+/****************************************************************************/
+
+/**
+ State: ERROR.
+*/
+
+void ec_fsm_sii_state_error(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+}
+
+/****************************************************************************/
+
+/**
+ State: END.
+*/
+
+void ec_fsm_sii_state_end(
+ ec_fsm_sii_t *fsm /**< finite state machine */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_sii.h b/ubuntu/igh-ecat/master/fsm_sii.h
new file mode 100644
index 00000000000000..405e2850249247
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_sii.h
@@ -0,0 +1,82 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave information interface FSM structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_SII_H__
+#define __EC_FSM_SII_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "slave.h"
+
+/****************************************************************************/
+
+/** SII access addressing mode.
+ */
+typedef enum {
+ EC_FSM_SII_USE_INCREMENT_ADDRESS, /**< Use auto-increment addressing. */
+ EC_FSM_SII_USE_CONFIGURED_ADDRESS /**< Use configured addresses. */
+} ec_fsm_sii_addressing_t;
+
+/****************************************************************************/
+
+typedef struct ec_fsm_sii ec_fsm_sii_t; /**< \see ec_fsm_sii */
+
+/**
+ Slave information interface FSM.
+*/
+
+struct ec_fsm_sii
+{
+ ec_slave_t *slave; /**< slave the FSM runs on */
+ ec_datagram_t *datagram; /**< datagram used in the state machine */
+ unsigned int retries; /**< retries upon datagram timeout */
+
+ void (*state)(ec_fsm_sii_t *); /**< SII state function */
+ uint16_t word_offset; /**< input: word offset in SII */
+ ec_fsm_sii_addressing_t mode; /**< reading via APRD or NPRD */
+ uint8_t value[4]; /**< raw SII value (32bit) */
+ unsigned long jiffies_start; /**< Start timestamp. */
+ uint8_t check_once_more; /**< one more try after timeout */
+};
+
+/****************************************************************************/
+
+void ec_fsm_sii_init(ec_fsm_sii_t *, ec_datagram_t *);
+void ec_fsm_sii_clear(ec_fsm_sii_t *);
+
+void ec_fsm_sii_read(ec_fsm_sii_t *, ec_slave_t *,
+ uint16_t, ec_fsm_sii_addressing_t);
+void ec_fsm_sii_write(ec_fsm_sii_t *, ec_slave_t *, uint16_t,
+ const uint16_t *, ec_fsm_sii_addressing_t);
+
+int ec_fsm_sii_exec(ec_fsm_sii_t *);
+int ec_fsm_sii_success(ec_fsm_sii_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_slave.c b/ubuntu/igh-ecat/master/fsm_slave.c
new file mode 100644
index 00000000000000..6e1f9c36996594
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_slave.c
@@ -0,0 +1,685 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT slave (SDO) state machine.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "slave_config.h"
+
+#include "fsm_slave.h"
+
+/****************************************************************************/
+
+void ec_fsm_slave_state_idle(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_ready(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);
+int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *);
+#ifdef EC_EOE
+int ec_fsm_slave_action_process_eoe(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_state_eoe_request(ec_fsm_slave_t *, ec_datagram_t *);
+#endif
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_slave_init(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_slave_t *slave /**< EtherCAT slave. */
+ )
+{
+ fsm->slave = slave;
+ INIT_LIST_HEAD(&fsm->list); // mark as unlisted
+
+ fsm->state = ec_fsm_slave_state_idle;
+ fsm->datagram = NULL;
+ fsm->sdo_request = NULL;
+ fsm->reg_request = NULL;
+ fsm->foe_request = NULL;
+ fsm->soe_request = NULL;
+#ifdef EC_EOE
+ fsm->eoe_request = NULL;
+#endif
+
+ // Init sub-state-machines
+ ec_fsm_coe_init(&fsm->fsm_coe);
+ ec_fsm_foe_init(&fsm->fsm_foe);
+ ec_fsm_soe_init(&fsm->fsm_soe);
+#ifdef EC_EOE
+ ec_fsm_eoe_init(&fsm->fsm_eoe);
+#endif
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_slave_clear(
+ ec_fsm_slave_t *fsm /**< Master state machine. */
+ )
+{
+ // signal requests that are currently in operation
+
+ if (fsm->sdo_request) {
+ fsm->sdo_request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&fsm->slave->master->request_queue);
+ }
+
+ if (fsm->reg_request) {
+ fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&fsm->slave->master->request_queue);
+ }
+
+ if (fsm->foe_request) {
+ fsm->foe_request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&fsm->slave->master->request_queue);
+ }
+
+ if (fsm->soe_request) {
+ fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&fsm->slave->master->request_queue);
+ }
+
+#ifdef EC_EOE
+ if (fsm->eoe_request) {
+ fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&fsm->slave->master->request_queue);
+ }
+#endif
+
+ // clear sub-state machines
+ ec_fsm_coe_clear(&fsm->fsm_coe);
+ ec_fsm_foe_clear(&fsm->fsm_foe);
+ ec_fsm_soe_clear(&fsm->fsm_soe);
+#ifdef EC_EOE
+ ec_fsm_eoe_clear(&fsm->fsm_eoe);
+#endif
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if \a datagram was used, else 0.
+ */
+int ec_fsm_slave_exec(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< New datagram to use. */
+ )
+{
+ int datagram_used;
+
+ fsm->state(fsm, datagram);
+
+ datagram_used = fsm->state != ec_fsm_slave_state_idle &&
+ fsm->state != ec_fsm_slave_state_ready;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/****************************************************************************/
+
+/** Sets the current state of the state machine to READY
+ */
+void ec_fsm_slave_set_ready(
+ ec_fsm_slave_t *fsm /**< Slave state machine. */
+ )
+{
+ if (fsm->state == ec_fsm_slave_state_idle) {
+ EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n");
+ fsm->state = ec_fsm_slave_state_ready;
+ }
+}
+
+/****************************************************************************/
+
+/** Returns, if the FSM is currently not busy and ready to execute.
+ *
+ * \return Non-zero if ready.
+ */
+int ec_fsm_slave_is_ready(
+ const ec_fsm_slave_t *fsm /**< Slave state machine. */
+ )
+{
+ return fsm->state == ec_fsm_slave_state_ready;
+}
+
+/*****************************************************************************
+ * Slave state machine
+ ****************************************************************************/
+
+/** Slave state: IDLE.
+ */
+void ec_fsm_slave_state_idle(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ // do nothing
+}
+
+/****************************************************************************/
+
+/** Slave state: READY.
+ */
+void ec_fsm_slave_state_ready(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ // Check for pending external SDO requests
+ if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
+ return;
+ }
+
+ // Check for pending external register requests
+ if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
+ return;
+ }
+
+ // Check for pending FoE requests
+ if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
+ return;
+ }
+
+ // Check for pending SoE requests
+ if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
+ return;
+ }
+
+#ifdef EC_EOE
+ // Check for pending EoE IP parameter requests
+ if (ec_fsm_slave_action_process_eoe(fsm, datagram)) {
+ return;
+ }
+#endif
+}
+
+/****************************************************************************/
+
+/** Check for pending SDO requests and process one.
+ *
+ * \return non-zero, if an SDO request is processed.
+ */
+int ec_fsm_slave_action_process_sdo(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request;
+
+ if (list_empty(&slave->sdo_requests)) {
+ return 0;
+ }
+
+ // take the first request to be processed
+ request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list);
+ list_del_init(&request->list); // dequeue
+
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ EC_SLAVE_WARN(slave, "Aborting SDO request,"
+ " slave has error flag set.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ if (slave->current_state == EC_SLAVE_STATE_INIT) {
+ EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ fsm->sdo_request = request;
+ request->state = EC_INT_REQUEST_BUSY;
+
+ // Found pending SDO request. Execute it!
+ EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
+
+ // Start SDO transfer
+ fsm->state = ec_fsm_slave_state_sdo_request;
+ ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
+ ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Slave state: SDO_REQUEST.
+ */
+void ec_fsm_slave_state_sdo_request(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_sdo_request_t *request = fsm->sdo_request;
+
+ if (ec_fsm_coe_exec(&fsm->fsm_coe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
+ EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->sdo_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n");
+
+ // SDO request finished
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&slave->master->request_queue);
+ fsm->sdo_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+}
+
+/****************************************************************************/
+
+/** Check for pending register requests and process one.
+ *
+ * \return non-zero, if a register request is processed.
+ */
+int ec_fsm_slave_action_process_reg(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_reg_request_t *reg;
+
+ fsm->reg_request = NULL;
+
+ if (slave->config) {
+ // search the first internal register request to be processed
+ list_for_each_entry(reg, &slave->config->reg_requests, list) {
+ if (reg->state == EC_INT_REQUEST_QUEUED) {
+ fsm->reg_request = reg;
+ break;
+ }
+ }
+ }
+
+ if (!fsm->reg_request && !list_empty(&slave->reg_requests)) {
+ // take the first external request to be processed
+ fsm->reg_request =
+ list_entry(slave->reg_requests.next, ec_reg_request_t, list);
+ list_del_init(&fsm->reg_request->list); // dequeue
+ }
+
+ if (!fsm->reg_request) { // no register request to process
+ return 0;
+ }
+
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ EC_SLAVE_WARN(slave, "Aborting register request,"
+ " slave has error flag set.\n");
+ fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->reg_request = NULL;
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ // Found pending register request. Execute it!
+ EC_SLAVE_DBG(slave, 1, "Processing register request...\n");
+
+ fsm->reg_request->state = EC_INT_REQUEST_BUSY;
+
+ // Start register access
+ if (fsm->reg_request->dir == EC_DIR_INPUT) {
+ ec_datagram_fprd(datagram, slave->station_address,
+ fsm->reg_request->address, fsm->reg_request->transfer_size);
+ ec_datagram_zero(datagram);
+ } else {
+ ec_datagram_fpwr(datagram, slave->station_address,
+ fsm->reg_request->address, fsm->reg_request->transfer_size);
+ memcpy(datagram->data, fsm->reg_request->data,
+ fsm->reg_request->transfer_size);
+ }
+ datagram->device_index = slave->device_index;
+ fsm->state = ec_fsm_slave_state_reg_request;
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Slave state: Register request.
+ */
+void ec_fsm_slave_state_reg_request(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_reg_request_t *reg = fsm->reg_request;
+
+ if (!reg) {
+ // configuration was cleared in the meantime
+ fsm->state = ec_fsm_slave_state_ready;
+ fsm->reg_request = NULL;
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_ERR(slave, "Failed to receive register"
+ " request datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ reg->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->reg_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+ return;
+ }
+
+ if (fsm->datagram->working_counter == 1) {
+ if (reg->dir == EC_DIR_INPUT) { // read request
+ memcpy(reg->data, fsm->datagram->data, reg->transfer_size);
+ }
+
+ reg->state = EC_INT_REQUEST_SUCCESS;
+ EC_SLAVE_DBG(slave, 1, "Register request successful.\n");
+ } else {
+ reg->state = EC_INT_REQUEST_FAILURE;
+ ec_datagram_print_state(fsm->datagram);
+ EC_SLAVE_ERR(slave, "Register request failed"
+ " (working counter is %u).\n",
+ fsm->datagram->working_counter);
+ }
+
+ wake_up_all(&slave->master->request_queue);
+ fsm->reg_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+}
+
+/****************************************************************************/
+
+/** Check for pending FoE requests and process one.
+ *
+ * \return non-zero, if an FoE request is processed.
+ */
+int ec_fsm_slave_action_process_foe(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_foe_request_t *request;
+
+ if (list_empty(&slave->foe_requests)) {
+ return 0;
+ }
+
+ // take the first request to be processed
+ request = list_entry(slave->foe_requests.next, ec_foe_request_t, list);
+ list_del_init(&request->list); // dequeue
+
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ EC_SLAVE_WARN(slave, "Aborting FoE request,"
+ " slave has error flag set.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ request->state = EC_INT_REQUEST_BUSY;
+ fsm->foe_request = request;
+
+ EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
+
+ fsm->state = ec_fsm_slave_state_foe_request;
+ ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
+ ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Slave state: FOE REQUEST.
+ */
+void ec_fsm_slave_state_foe_request(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_foe_request_t *request = fsm->foe_request;
+
+ if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
+ EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->foe_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+ return;
+ }
+
+ // finished transferring FoE
+ EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"
+ " data.\n", request->data_size);
+
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&slave->master->request_queue);
+ fsm->foe_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+}
+
+/****************************************************************************/
+
+/** Check for pending SoE requests and process one.
+ *
+ * \return non-zero, if a request is processed.
+ */
+int ec_fsm_slave_action_process_soe(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *req;
+
+ if (list_empty(&slave->soe_requests)) {
+ return 0;
+ }
+
+ // take the first request to be processed
+ req = list_entry(slave->soe_requests.next, ec_soe_request_t, list);
+ list_del_init(&req->list); // dequeue
+
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ EC_SLAVE_WARN(slave, "Aborting SoE request,"
+ " slave has error flag set.\n");
+ req->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ if (slave->current_state == EC_SLAVE_STATE_INIT) {
+ EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
+ req->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ fsm->soe_request = req;
+ req->state = EC_INT_REQUEST_BUSY;
+
+ // Found pending request. Execute it!
+ EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
+
+ // Start SoE transfer
+ fsm->state = ec_fsm_slave_state_soe_request;
+ ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
+ ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Slave state: SOE_REQUEST.
+ */
+void ec_fsm_slave_state_soe_request(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *request = fsm->soe_request;
+
+ if (ec_fsm_soe_exec(&fsm->fsm_soe, datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
+ EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->soe_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
+
+ // SoE request finished
+ request->state = EC_INT_REQUEST_SUCCESS;
+ wake_up_all(&slave->master->request_queue);
+ fsm->soe_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+}
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+/** Check for pending EoE IP parameter requests and process one.
+ *
+ * \return non-zero, if a request is processed.
+ */
+int ec_fsm_slave_action_process_eoe(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_eoe_request_t *request;
+
+ if (list_empty(&slave->eoe_requests)) {
+ return 0;
+ }
+
+ // take the first request to be processed
+ request = list_entry(slave->eoe_requests.next, ec_eoe_request_t, list);
+ list_del_init(&request->list); // dequeue
+
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ EC_SLAVE_WARN(slave, "Aborting EoE request,"
+ " slave has error flag set.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ if (slave->current_state == EC_SLAVE_STATE_INIT) {
+ EC_SLAVE_WARN(slave, "Aborting EoE request, slave is in INIT.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&slave->master->request_queue);
+ fsm->state = ec_fsm_slave_state_idle;
+ return 0;
+ }
+
+ fsm->eoe_request = request;
+ request->state = EC_INT_REQUEST_BUSY;
+
+ // Found pending request. Execute it!
+ EC_SLAVE_DBG(slave, 1, "Processing EoE request...\n");
+
+ // Start EoE command
+ fsm->state = ec_fsm_slave_state_eoe_request;
+ ec_fsm_eoe_set_ip_param(&fsm->fsm_eoe, slave, request);
+ ec_fsm_eoe_exec(&fsm->fsm_eoe, datagram); // execute immediately
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Slave state: EOE_REQUEST.
+ */
+void ec_fsm_slave_state_eoe_request(
+ ec_fsm_slave_t *fsm, /**< Slave state machine. */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_eoe_request_t *req = fsm->eoe_request;
+
+ if (ec_fsm_eoe_exec(&fsm->fsm_eoe, datagram)) {
+ return;
+ }
+
+ if (ec_fsm_eoe_success(&fsm->fsm_eoe)) {
+ req->state = EC_INT_REQUEST_SUCCESS;
+ EC_SLAVE_DBG(slave, 1, "Finished EoE request.\n");
+ }
+ else {
+ req->state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Failed to process EoE request.\n");
+ }
+
+ wake_up_all(&slave->master->request_queue);
+ fsm->eoe_request = NULL;
+ fsm->state = ec_fsm_slave_state_ready;
+}
+
+/****************************************************************************/
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_slave.h b/ubuntu/igh-ecat/master/fsm_slave.h
new file mode 100644
index 00000000000000..079bd5f8b473a6
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_slave.h
@@ -0,0 +1,87 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave request state machine.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_SLAVE_H__
+#define __EC_FSM_SLAVE_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "sdo_request.h"
+#include "reg_request.h"
+#ifdef EC_EOE
+#include "eoe_request.h"
+#endif
+#include "fsm_coe.h"
+#include "fsm_foe.h"
+#include "fsm_soe.h"
+#ifdef EC_EOE
+#include "fsm_eoe.h"
+#endif
+
+/****************************************************************************/
+
+typedef struct ec_fsm_slave ec_fsm_slave_t; /**< \see ec_fsm_slave */
+
+/** Finite state machine of an EtherCAT slave.
+ */
+struct ec_fsm_slave {
+ ec_slave_t *slave; /**< slave the FSM runs on */
+ struct list_head list; /**< Used for execution list. */
+
+ void (*state)(ec_fsm_slave_t *, ec_datagram_t *); /**< State function. */
+ ec_datagram_t *datagram; /**< Previous state datagram. */
+ ec_sdo_request_t *sdo_request; /**< SDO request to process. */
+ ec_reg_request_t *reg_request; /**< Register request to process. */
+ ec_foe_request_t *foe_request; /**< FoE request to process. */
+ off_t foe_index; /**< Index to FoE write request data. */
+ ec_soe_request_t *soe_request; /**< SoE request to process. */
+#ifdef EC_EOE
+ ec_eoe_request_t *eoe_request; /**< SoE request to process. */
+#endif
+
+ ec_fsm_coe_t fsm_coe; /**< CoE state machine. */
+ ec_fsm_foe_t fsm_foe; /**< FoE state machine. */
+ ec_fsm_soe_t fsm_soe; /**< SoE state machine. */
+#ifdef EC_EOE
+ ec_fsm_eoe_t fsm_eoe; /**< EoE state machine. */
+#endif
+};
+
+/****************************************************************************/
+
+void ec_fsm_slave_init(ec_fsm_slave_t *, ec_slave_t *);
+void ec_fsm_slave_clear(ec_fsm_slave_t *);
+
+int ec_fsm_slave_exec(ec_fsm_slave_t *, ec_datagram_t *);
+void ec_fsm_slave_set_ready(ec_fsm_slave_t *);
+int ec_fsm_slave_is_ready(const ec_fsm_slave_t *);
+
+/****************************************************************************/
+
+
+#endif // __EC_FSM_SLAVE_H__
diff --git a/ubuntu/igh-ecat/master/fsm_slave_config.c b/ubuntu/igh-ecat/master/fsm_slave_config.c
new file mode 100644
index 00000000000000..47e750f46b243a
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_slave_config.c
@@ -0,0 +1,1864 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+/** \file
+ *
+ * EtherCAT slave configuration state machine.
+ */
+
+/****************************************************************************/
+
+#include <asm/div64.h>
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "slave_config.h"
+#include "fsm_slave_config.h"
+
+/****************************************************************************/
+
+/** Maximum clock difference (in ns) before going to SAFEOP.
+ *
+ * Wait for DC time difference to drop under this absolute value before
+ * requesting SAFEOP.
+ */
+#define EC_DC_MAX_SYNC_DIFF_NS 10000
+
+/** Maximum time (in ms) to wait for clock discipline.
+ */
+#define EC_DC_SYNC_WAIT_MS 5000
+
+/** Time offset (in ns), that is added to cyclic start time.
+ */
+#define EC_DC_START_OFFSET 100000000ULL
+
+/****************************************************************************/
+
+// prototypes for private methods
+int ec_fsm_slave_config_running(const ec_fsm_slave_config_t *);
+
+/****************************************************************************/
+
+void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);
+#ifdef EC_SII_ASSIGN
+void ec_fsm_slave_config_state_assign_pdi(ec_fsm_slave_config_t *);
+#endif
+void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);
+#ifdef EC_SII_ASSIGN
+void ec_fsm_slave_config_state_assign_ethercat(ec_fsm_slave_config_t *);
+#endif
+void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_soe_conf_preop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_eoe_ip_param(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_wait_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_soe_conf_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);
+
+void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);
+#ifdef EC_SII_ASSIGN
+void ec_fsm_slave_config_enter_assign_pdi(ec_fsm_slave_config_t *);
+#endif
+void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_eoe_ip_param(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_wait_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *);
+
+void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);
+void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);
+
+void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_slave_config_init(
+ ec_fsm_slave_config_t *fsm, /**< slave state machine */
+ ec_datagram_t *datagram, /**< datagram structure to use */
+ ec_fsm_change_t *fsm_change, /**< State change state machine to use. */
+ ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */
+ ec_fsm_soe_t *fsm_soe, /**< SoE state machine to use. */
+ ec_fsm_pdo_t *fsm_pdo, /**< PDO configuration state machine to use. */
+ ec_fsm_eoe_t *fsm_eoe /**< EoE state machine to use. */
+ )
+{
+ ec_sdo_request_init(&fsm->request_copy);
+ ec_soe_request_init(&fsm->soe_request_copy);
+
+ fsm->datagram = datagram;
+ fsm->fsm_change = fsm_change;
+ fsm->fsm_coe = fsm_coe;
+ fsm->fsm_soe = fsm_soe;
+ fsm->fsm_pdo = fsm_pdo;
+ fsm->fsm_eoe = fsm_eoe;
+
+ fsm->wait_ms = 0;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_slave_config_clear(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_sdo_request_clear(&fsm->request_copy);
+ ec_soe_request_clear(&fsm->soe_request_copy);
+}
+
+/****************************************************************************/
+
+/** Start slave configuration state machine.
+ */
+void ec_fsm_slave_config_start(
+ ec_fsm_slave_config_t *fsm, /**< slave state machine */
+ ec_slave_t *slave /**< slave to configure */
+ )
+{
+ fsm->slave = slave;
+ fsm->state = ec_fsm_slave_config_state_start;
+}
+
+/****************************************************************************/
+
+/**
+ * \return false, if state machine has terminated
+ */
+int ec_fsm_slave_config_running(
+ const ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ return fsm->state != ec_fsm_slave_config_state_end
+ && fsm->state != ec_fsm_slave_config_state_error;
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * If the state machine's datagram is not sent or received yet, the execution
+ * of the state machine is delayed to the next cycle.
+ *
+ * \return false, if state machine has terminated
+ */
+int ec_fsm_slave_config_exec(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ if (fsm->datagram->state == EC_DATAGRAM_SENT
+ || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
+ // datagram was not sent or received yet.
+ return ec_fsm_slave_config_running(fsm);
+ }
+
+ fsm->state(fsm);
+ return ec_fsm_slave_config_running(fsm);
+}
+
+/****************************************************************************/
+
+/**
+ * \return true, if the state machine terminated gracefully
+ */
+int ec_fsm_slave_config_success(
+ const ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ return fsm->state == ec_fsm_slave_config_state_end;
+}
+
+/*****************************************************************************
+ * Slave configuration state machine
+ ****************************************************************************/
+
+/** Slave configuration state: START.
+ */
+void ec_fsm_slave_config_state_start(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ EC_SLAVE_DBG(fsm->slave, 1, "Configuring...\n");
+ ec_fsm_slave_config_enter_init(fsm);
+}
+
+/****************************************************************************/
+
+/** Start state change to INIT.
+ */
+void ec_fsm_slave_config_enter_init(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);
+ ec_fsm_change_exec(fsm->fsm_change);
+ fsm->state = ec_fsm_slave_config_state_init;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: INIT.
+ */
+void ec_fsm_slave_config_state_init(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (ec_fsm_change_exec(fsm->fsm_change)) return;
+
+ if (!ec_fsm_change_success(fsm->fsm_change)) {
+ if (!fsm->fsm_change->spontaneous_change)
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Now in INIT.\n");
+
+ if (!slave->base_fmmu_count) { // skip FMMU configuration
+ ec_fsm_slave_config_enter_clear_sync(fsm);
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Clearing FMMU configurations...\n");
+
+ // clear FMMU configurations
+ ec_datagram_fpwr(datagram, slave->station_address,
+ 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_clear_fmmus;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: CLEAR FMMU.
+ */
+void ec_fsm_slave_config_state_clear_fmmus(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed receive FMMU clearing datagram.\n");
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed to clear FMMUs: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ ec_fsm_slave_config_enter_clear_sync(fsm);
+}
+
+/****************************************************************************/
+
+/** Clear the sync manager configurations.
+ */
+void ec_fsm_slave_config_enter_clear_sync(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+ size_t sync_size;
+
+ if (!slave->base_sync_count) {
+ // no sync managers
+ ec_fsm_slave_config_enter_dc_clear_assign(fsm);
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Clearing sync manager configurations...\n");
+
+ sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count;
+
+ // clear sync manager configurations
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_clear_sync;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: CLEAR SYNC.
+ */
+void ec_fsm_slave_config_state_clear_sync(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed receive sync manager"
+ " clearing datagram.\n");
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(fsm->slave,
+ "Failed to clear sync manager configurations: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ ec_fsm_slave_config_enter_dc_clear_assign(fsm);
+}
+
+/****************************************************************************/
+
+/** Clear the DC assignment.
+ */
+void ec_fsm_slave_config_enter_dc_clear_assign(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (!slave->base_dc_supported || !slave->has_dc_system_time) {
+ ec_fsm_slave_config_enter_mbox_sync(fsm);
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Clearing DC assignment...\n");
+
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_dc_clear_assign;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: CLEAR DC ASSIGN.
+ */
+void ec_fsm_slave_config_state_dc_clear_assign(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed receive DC assignment"
+ " clearing datagram.\n");
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ // clearing the DC assignment does not succeed on simple slaves
+ EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: ");
+ ec_datagram_print_wc_error(datagram);
+ }
+
+ ec_fsm_slave_config_enter_mbox_sync(fsm);
+}
+
+/****************************************************************************/
+
+/** Check for mailbox sync managers to be configured.
+ */
+void ec_fsm_slave_config_enter_mbox_sync(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+ unsigned int i;
+
+ // slave is now in INIT
+ if (slave->current_state == slave->requested_state) {
+ fsm->state = ec_fsm_slave_config_state_end; // successful
+ EC_SLAVE_DBG(slave, 1, "Finished configuration.\n");
+ return;
+ }
+
+ if (!slave->sii.mailbox_protocols) {
+ // no mailbox protocols supported
+ EC_SLAVE_DBG(slave, 1, "Slave does not support"
+ " mailbox communication.\n");
+#ifdef EC_SII_ASSIGN
+ ec_fsm_slave_config_enter_assign_pdi(fsm);
+#else
+ ec_fsm_slave_config_enter_boot_preop(fsm);
+#endif
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n");
+
+ if (slave->requested_state == EC_SLAVE_STATE_BOOT) {
+ ec_sync_t sync;
+
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0800,
+ EC_SYNC_PAGE_SIZE * 2);
+ ec_datagram_zero(datagram);
+
+ ec_sync_init(&sync, slave);
+ sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;
+ sync.control_register = 0x26;
+ sync.enable = 1;
+ ec_sync_page(&sync, 0, slave->sii.boot_rx_mailbox_size,
+ EC_DIR_INVALID, // use default direction
+ 0, // no PDO xfer
+ datagram->data);
+ slave->configured_rx_mailbox_offset =
+ slave->sii.boot_rx_mailbox_offset;
+ slave->configured_rx_mailbox_size =
+ slave->sii.boot_rx_mailbox_size;
+
+ ec_sync_init(&sync, slave);
+ sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;
+ sync.control_register = 0x22;
+ sync.enable = 1;
+ ec_sync_page(&sync, 1, slave->sii.boot_tx_mailbox_size,
+ EC_DIR_INVALID, // use default direction
+ 0, // no PDO xfer
+ datagram->data + EC_SYNC_PAGE_SIZE);
+ slave->configured_tx_mailbox_offset =
+ slave->sii.boot_tx_mailbox_offset;
+ slave->configured_tx_mailbox_size =
+ slave->sii.boot_tx_mailbox_size;
+
+ } else if (slave->sii.sync_count >= 2) { // mailbox configuration provided
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0800,
+ EC_SYNC_PAGE_SIZE * slave->sii.sync_count);
+ ec_datagram_zero(datagram);
+
+ for (i = 0; i < 2; i++) {
+ ec_sync_page(&slave->sii.syncs[i], i,
+ slave->sii.syncs[i].default_length,
+ NULL, // use default sync manager configuration
+ 0, // no PDO xfer
+ datagram->data + EC_SYNC_PAGE_SIZE * i);
+ }
+
+ slave->configured_rx_mailbox_offset =
+ slave->sii.syncs[0].physical_start_address;
+ slave->configured_rx_mailbox_size =
+ slave->sii.syncs[0].default_length;
+ slave->configured_tx_mailbox_offset =
+ slave->sii.syncs[1].physical_start_address;
+ slave->configured_tx_mailbox_size =
+ slave->sii.syncs[1].default_length;
+ } else { // no mailbox sync manager configurations provided
+ ec_sync_t sync;
+
+ EC_SLAVE_DBG(slave, 1, "Slave does not provide"
+ " mailbox sync manager configurations.\n");
+
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0800,
+ EC_SYNC_PAGE_SIZE * 2);
+ ec_datagram_zero(datagram);
+
+ ec_sync_init(&sync, slave);
+ sync.physical_start_address = slave->sii.std_rx_mailbox_offset;
+ sync.control_register = 0x26;
+ sync.enable = 1;
+ ec_sync_page(&sync, 0, slave->sii.std_rx_mailbox_size,
+ NULL, // use default sync manager configuration
+ 0, // no PDO xfer
+ datagram->data);
+ slave->configured_rx_mailbox_offset =
+ slave->sii.std_rx_mailbox_offset;
+ slave->configured_rx_mailbox_size =
+ slave->sii.std_rx_mailbox_size;
+
+ ec_sync_init(&sync, slave);
+ sync.physical_start_address = slave->sii.std_tx_mailbox_offset;
+ sync.control_register = 0x22;
+ sync.enable = 1;
+ ec_sync_page(&sync, 1, slave->sii.std_tx_mailbox_size,
+ NULL, // use default sync manager configuration
+ 0, // no PDO xfer
+ datagram->data + EC_SYNC_PAGE_SIZE);
+ slave->configured_tx_mailbox_offset =
+ slave->sii.std_tx_mailbox_offset;
+ slave->configured_tx_mailbox_size =
+ slave->sii.std_tx_mailbox_size;
+ }
+
+ fsm->take_time = 1;
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_mbox_sync;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: SYNC.
+ *
+ * \todo Timeout for response.
+ */
+void ec_fsm_slave_config_state_mbox_sync(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive sync manager"
+ " configuration datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (fsm->take_time) {
+ fsm->take_time = 0;
+ fsm->jiffies_start = datagram->jiffies_sent;
+ }
+
+ /* Because the sync manager configurations are cleared during the last
+ * cycle, some slaves do not immediately respond to the mailbox sync
+ * manager configuration datagram. Therefore, resend the datagram for
+ * a certain time, if the slave does not respond.
+ */
+ if (datagram->working_counter == 0) {
+ unsigned long diff = datagram->jiffies_received - fsm->jiffies_start;
+
+ if (diff >= HZ) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Timeout while configuring"
+ " mailbox sync managers.\n");
+ return;
+ } else {
+ EC_SLAVE_DBG(slave, 1, "Resending after %u ms...\n",
+ (unsigned int) diff * 1000 / HZ);
+ }
+
+ // send configuration datagram again
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+ else if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set sync managers: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+#ifdef EC_SII_ASSIGN
+ ec_fsm_slave_config_enter_assign_pdi(fsm);
+#else
+ ec_fsm_slave_config_enter_boot_preop(fsm);
+#endif
+}
+
+/****************************************************************************/
+
+#ifdef EC_SII_ASSIGN
+
+/** Assign SII to PDI.
+ */
+void ec_fsm_slave_config_enter_assign_pdi(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) {
+ EC_SLAVE_DBG(slave, 1, "Assigning SII access to PDI.\n");
+
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0500, 0x01);
+ EC_WRITE_U8(datagram->data, 0x01); // PDI
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_assign_pdi;
+ }
+ else {
+ ec_fsm_slave_config_enter_boot_preop(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: ASSIGN_PDI.
+ */
+void ec_fsm_slave_config_state_assign_pdi(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ return;
+ }
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_WARN(slave, "Failed receive SII assignment datagram: ");
+ ec_datagram_print_state(datagram);
+ goto cont_preop;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_WARN(slave, "Failed to assign SII to PDI: ");
+ ec_datagram_print_wc_error(datagram);
+ }
+
+cont_preop:
+ ec_fsm_slave_config_enter_boot_preop(fsm);
+}
+
+#endif
+
+/****************************************************************************/
+
+/** Request PREOP state.
+ */
+void ec_fsm_slave_config_enter_boot_preop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ fsm->state = ec_fsm_slave_config_state_boot_preop;
+
+ if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) {
+ ec_fsm_change_start(fsm->fsm_change,
+ fsm->slave, EC_SLAVE_STATE_PREOP);
+ } else { // BOOT
+ ec_fsm_change_start(fsm->fsm_change,
+ fsm->slave, EC_SLAVE_STATE_BOOT);
+ }
+
+ ec_fsm_change_exec(fsm->fsm_change); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: BOOT/PREOP.
+ */
+void ec_fsm_slave_config_state_boot_preop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+#ifdef EC_SII_ASSIGN
+ int assign_to_pdi;
+ ec_slave_config_t *config;
+ ec_flag_t *flag;
+#endif
+
+ if (ec_fsm_change_exec(fsm->fsm_change)) {
+ return;
+ }
+
+ if (!ec_fsm_change_success(fsm->fsm_change)) {
+ if (!fsm->fsm_change->spontaneous_change)
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ return;
+ }
+
+ // slave is now in BOOT or PREOP
+ slave->jiffies_preop = fsm->datagram->jiffies_received;
+
+ EC_SLAVE_DBG(slave, 1, "Now in %s.\n",
+ slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT");
+
+#ifdef EC_SII_ASSIGN
+ assign_to_pdi = 0;
+ config = fsm->slave->config;
+ if (config) {
+ flag = ec_slave_config_find_flag(config, "AssignToPdi");
+ if (flag) {
+ assign_to_pdi = flag->value;
+ }
+ }
+
+ if (assign_to_pdi) {
+ EC_SLAVE_DBG(slave, 1, "Skipping SII assignment back to EtherCAT.\n");
+ if (slave->current_state == slave->requested_state) {
+ fsm->state = ec_fsm_slave_config_state_end; // successful
+ EC_SLAVE_DBG(slave, 1, "Finished configuration.\n");
+ return;
+ }
+
+ ec_fsm_slave_config_enter_sdo_conf(fsm);
+ }
+ else {
+ EC_SLAVE_DBG(slave, 1, "Assigning SII access back to EtherCAT.\n");
+
+ ec_datagram_fpwr(fsm->datagram, slave->station_address, 0x0500, 0x01);
+ EC_WRITE_U8(fsm->datagram->data, 0x00); // EtherCAT
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_assign_ethercat;
+ }
+#else
+ if (slave->current_state == slave->requested_state) {
+ fsm->state = ec_fsm_slave_config_state_end; // successful
+ EC_SLAVE_DBG(slave, 1, "Finished configuration.\n");
+ return;
+ }
+
+ ec_fsm_slave_config_enter_sdo_conf(fsm);
+#endif
+}
+
+/****************************************************************************/
+
+#ifdef EC_SII_ASSIGN
+
+/** Slave configuration state: ASSIGN_ETHERCAT.
+ */
+void ec_fsm_slave_config_state_assign_ethercat(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ return;
+ }
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_WARN(slave, "Failed receive SII assignment datagram: ");
+ ec_datagram_print_state(datagram);
+ goto cont_sdo_conf;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_WARN(slave, "Failed to assign SII back to EtherCAT: ");
+ ec_datagram_print_wc_error(datagram);
+ }
+
+cont_sdo_conf:
+ if (slave->current_state == slave->requested_state) {
+ fsm->state = ec_fsm_slave_config_state_end; // successful
+ EC_SLAVE_DBG(slave, 1, "Finished configuration.\n");
+ return;
+ }
+
+ ec_fsm_slave_config_enter_sdo_conf(fsm);
+}
+
+#endif
+
+/****************************************************************************/
+
+/** Check for SDO configurations to be applied.
+ */
+void ec_fsm_slave_config_enter_sdo_conf(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (!slave->config) {
+ ec_fsm_slave_config_enter_pdo_sync(fsm);
+ return;
+ }
+
+ // No CoE configuration to be applied?
+ if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration
+ ec_fsm_slave_config_enter_soe_conf_preop(fsm);
+ return;
+ }
+
+ // start SDO configuration
+ fsm->state = ec_fsm_slave_config_state_sdo_conf;
+ fsm->request = list_entry(fsm->slave->config->sdo_configs.next,
+ ec_sdo_request_t, list);
+ ec_sdo_request_copy(&fsm->request_copy, fsm->request);
+ ecrt_sdo_request_write(&fsm->request_copy);
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
+ ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: SDO_CONF.
+ */
+void ec_fsm_slave_config_state_sdo_conf(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ if (ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_coe_success(fsm->fsm_coe)) {
+ EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n");
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ return;
+ }
+
+ if (!fsm->slave->config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ // Another SDO to configure?
+ if (fsm->request->list.next != &fsm->slave->config->sdo_configs) {
+ fsm->request = list_entry(fsm->request->list.next,
+ ec_sdo_request_t, list);
+ ec_sdo_request_copy(&fsm->request_copy, fsm->request);
+ ecrt_sdo_request_write(&fsm->request_copy);
+ ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy);
+ ec_fsm_coe_exec(fsm->fsm_coe, fsm->datagram); // execute immediately
+ return;
+ }
+
+ // All SDOs are now configured.
+ ec_fsm_slave_config_enter_soe_conf_preop(fsm);
+}
+
+/****************************************************************************/
+
+/** Check for SoE configurations to be applied.
+ */
+void ec_fsm_slave_config_enter_soe_conf_preop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *req;
+
+ if (!slave->config) {
+ ec_fsm_slave_config_enter_pdo_sync(fsm);
+ return;
+ }
+
+ list_for_each_entry(req, &slave->config->soe_configs, list) {
+ if (req->al_state == EC_AL_STATE_PREOP) {
+ // start SoE configuration
+ fsm->state = ec_fsm_slave_config_state_soe_conf_preop;
+ fsm->soe_request = req;
+ ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+ ec_soe_request_write(&fsm->soe_request_copy);
+ ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
+ &fsm->soe_request_copy);
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
+ return;
+ }
+ }
+
+ // No SoE configuration to be applied in PREOP
+ ec_fsm_slave_config_enter_eoe_ip_param(fsm);
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: SOE_CONF.
+ */
+void ec_fsm_slave_config_state_soe_conf_preop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_soe_success(fsm->fsm_soe)) {
+ EC_SLAVE_ERR(slave, "SoE configuration failed.\n");
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ return;
+ }
+
+ if (!fsm->slave->config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ // Another IDN to configure in PREOP?
+ while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) {
+ fsm->soe_request = list_entry(fsm->soe_request->list.next,
+ ec_soe_request_t, list);
+ if (fsm->soe_request->al_state == EC_AL_STATE_PREOP) {
+ ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+ ec_soe_request_write(&fsm->soe_request_copy);
+ ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
+ &fsm->soe_request_copy);
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
+ return;
+ }
+ }
+
+ // All PREOP IDNs are now configured.
+ ec_fsm_slave_config_enter_eoe_ip_param(fsm);
+}
+
+/****************************************************************************/
+
+/** EOE_IP_PARAM entry function.
+ */
+void ec_fsm_slave_config_enter_eoe_ip_param(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+#ifdef EC_EOE
+ ec_slave_t *slave = fsm->slave;
+ ec_eoe_request_t *request = &slave->config->eoe_ip_param_request;
+
+ if (ec_eoe_request_valid(request)) {
+ EC_SLAVE_DBG(slave, 1, "Setting EoE IP parameters...\n");
+
+ // Start EoE command
+ fsm->state = ec_fsm_slave_config_state_eoe_ip_param;
+ ec_fsm_eoe_set_ip_param(fsm->fsm_eoe, slave, request);
+ ec_fsm_eoe_exec(fsm->fsm_eoe, fsm->datagram); // execute immediately
+ return;
+ }
+#endif
+
+ ec_fsm_slave_config_enter_pdo_conf(fsm);
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: EOE_IP_PARAM.
+ */
+void ec_fsm_slave_config_state_eoe_ip_param(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+#ifdef EC_EOE
+ ec_slave_t *slave = fsm->slave;
+
+ if (ec_fsm_eoe_exec(fsm->fsm_eoe, fsm->datagram)) {
+ return;
+ }
+
+ if (ec_fsm_eoe_success(fsm->fsm_eoe)) {
+ EC_SLAVE_DBG(slave, 1, "Finished setting EoE IP parameters.\n");
+ }
+ else {
+ EC_SLAVE_ERR(slave, "Failed to set EoE IP parameters.\n");
+ }
+#endif
+ ec_fsm_slave_config_enter_pdo_conf(fsm);
+}
+
+/****************************************************************************/
+
+/** PDO_CONF entry function.
+ */
+void ec_fsm_slave_config_enter_pdo_conf(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ // Start configuring PDOs
+ ec_fsm_pdo_start_configuration(fsm->fsm_pdo, fsm->slave);
+ fsm->state = ec_fsm_slave_config_state_pdo_conf;
+ fsm->state(fsm); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: PDO_CONF.
+ */
+void ec_fsm_slave_config_state_pdo_conf(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ // TODO check for config here
+
+ if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
+ return;
+ }
+
+ if (!fsm->slave->config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
+ EC_SLAVE_WARN(fsm->slave, "PDO configuration failed.\n");
+ }
+
+ ec_fsm_slave_config_enter_watchdog_divider(fsm);
+}
+
+/****************************************************************************/
+
+/** WATCHDOG_DIVIDER entry function.
+ */
+void ec_fsm_slave_config_enter_watchdog_divider(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_config_t *config = slave->config;
+
+ if (config && config->watchdog_divider) {
+ EC_SLAVE_DBG(slave, 1, "Setting watchdog divider to %u.\n",
+ config->watchdog_divider);
+
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0400, 2);
+ EC_WRITE_U16(datagram->data, config->watchdog_divider);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_watchdog_divider;
+ } else {
+ ec_fsm_slave_config_enter_watchdog(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: WATCHDOG_DIVIDER.
+ */
+void ec_fsm_slave_config_state_watchdog_divider(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive watchdog divider"
+ " configuration datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ EC_SLAVE_WARN(slave, "Failed to set watchdog divider: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ ec_fsm_slave_config_enter_watchdog(fsm);
+}
+
+/****************************************************************************/
+
+/** WATCHDOG entry function
+ */
+void ec_fsm_slave_config_enter_watchdog(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ ec_slave_config_t *config = slave->config;
+
+ if (config && config->watchdog_intervals) {
+ EC_SLAVE_DBG(slave, 1, "Setting process data"
+ " watchdog intervals to %u.\n", config->watchdog_intervals);
+
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0420, 2);
+ EC_WRITE_U16(datagram->data, config->watchdog_intervals);
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_watchdog;
+ } else {
+ ec_fsm_slave_config_enter_pdo_sync(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: WATCHDOG.
+ */
+
+void ec_fsm_slave_config_state_watchdog(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive sync manager"
+ " watchdog configuration datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_WARN(slave, "Failed to set process data"
+ " watchdog intervals: ");
+ ec_datagram_print_wc_error(datagram);
+ }
+
+ ec_fsm_slave_config_enter_pdo_sync(fsm);
+}
+
+/****************************************************************************/
+
+/** Check for PDO sync managers to be configured.
+ */
+void ec_fsm_slave_config_enter_pdo_sync(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+ unsigned int i, j, offset, num_pdo_syncs;
+ uint8_t sync_index;
+ const ec_sync_t *sync;
+ uint16_t size;
+
+ if (slave->sii.mailbox_protocols) {
+ offset = 2; // slave has mailboxes
+ } else {
+ offset = 0;
+ }
+
+ if (slave->sii.sync_count <= offset) {
+ // no PDO sync managers to configure
+ ec_fsm_slave_config_enter_fmmu(fsm);
+ return;
+ }
+
+ num_pdo_syncs = slave->sii.sync_count - offset;
+
+ // configure sync managers for process data
+ ec_datagram_fpwr(datagram, slave->station_address,
+ 0x0800 + EC_SYNC_PAGE_SIZE * offset,
+ EC_SYNC_PAGE_SIZE * num_pdo_syncs);
+ ec_datagram_zero(datagram);
+
+ for (i = 0; i < num_pdo_syncs; i++) {
+ const ec_sync_config_t *sync_config;
+ uint8_t pdo_xfer = 0;
+ sync_index = i + offset;
+ sync = &slave->sii.syncs[sync_index];
+
+ if (slave->config) {
+ const ec_slave_config_t *sc = slave->config;
+ sync_config = &sc->sync_configs[sync_index];
+ size = ec_pdo_list_total_size(&sync_config->pdos);
+
+ // determine, if PDOs shall be transferred via this SM
+ // inthat case, enable sync manager in every case
+ for (j = 0; j < sc->used_fmmus; j++) {
+ if (sc->fmmu_configs[j].sync_index == sync_index) {
+ pdo_xfer = 1;
+ break;
+ }
+ }
+
+ } else {
+ sync_config = NULL;
+ size = sync->default_length;
+ }
+
+ ec_sync_page(sync, sync_index, size, sync_config, pdo_xfer,
+ datagram->data + EC_SYNC_PAGE_SIZE * i);
+ }
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_pdo_sync;
+}
+
+/****************************************************************************/
+
+/** Configure PDO sync managers.
+ */
+void ec_fsm_slave_config_state_pdo_sync(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive process data sync"
+ " manager configuration datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set process data sync managers: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ ec_fsm_slave_config_enter_fmmu(fsm);
+}
+
+/****************************************************************************/
+
+/** Check for FMMUs to be configured.
+ */
+void ec_fsm_slave_config_enter_fmmu(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_datagram_t *datagram = fsm->datagram;
+ unsigned int i;
+ const ec_fmmu_config_t *fmmu;
+ const ec_sync_t *sync;
+
+ if (!slave->config) {
+ ec_fsm_slave_config_enter_wait_safeop(fsm);
+ return;
+ }
+
+ if (slave->base_fmmu_count < slave->config->used_fmmus) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Slave has less FMMUs (%u)"
+ " than requested (%u).\n", slave->base_fmmu_count,
+ slave->config->used_fmmus);
+ return;
+ }
+
+ if (!slave->base_fmmu_count) { // skip FMMU configuration
+ ec_fsm_slave_config_enter_dc_cycle(fsm);
+ return;
+ }
+
+ // configure FMMUs
+ ec_datagram_fpwr(datagram, slave->station_address,
+ 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);
+ ec_datagram_zero(datagram);
+ for (i = 0; i < slave->config->used_fmmus; i++) {
+ fmmu = &slave->config->fmmu_configs[i];
+ if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to determine PDO sync manager"
+ " for FMMU!\n");
+ return;
+ }
+ ec_fmmu_config_page(fmmu, sync,
+ datagram->data + EC_FMMU_PAGE_SIZE * i);
+ }
+
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_fmmu;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: FMMU.
+ */
+void ec_fsm_slave_config_state_fmmu(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive FMMUs datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set FMMUs: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ ec_fsm_slave_config_enter_dc_cycle(fsm);
+}
+
+/****************************************************************************/
+
+/** Check for DC to be configured.
+ */
+void ec_fsm_slave_config_enter_dc_cycle(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ ec_slave_config_t *config = slave->config;
+
+ if (!config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ if (config->dc_assign_activate) {
+ if (!slave->base_dc_supported || !slave->has_dc_system_time) {
+ EC_SLAVE_WARN(slave, "Slave seems not to support"
+ " distributed clocks!\n");
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Setting DC cycle times to %u / %u.\n",
+ config->dc_sync[0].cycle_time, config->dc_sync[1].cycle_time);
+
+ // set DC cycle times
+ ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8);
+ EC_WRITE_U32(datagram->data, config->dc_sync[0].cycle_time);
+ EC_WRITE_U32(datagram->data + 4, config->dc_sync[1].cycle_time);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_dc_cycle;
+ } else {
+ // DC are unused
+ ec_fsm_slave_config_enter_wait_safeop(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: DC CYCLE.
+ */
+void ec_fsm_slave_config_state_dc_cycle(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ ec_slave_config_t *config = slave->config;
+
+ if (!config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set DC cycle times: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Checking for synchrony.\n");
+
+ fsm->jiffies_start = jiffies;
+ ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_dc_sync_check;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: DC SYNC CHECK.
+ */
+void ec_fsm_slave_config_state_dc_sync_check(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_slave_config_t *config = slave->config;
+ uint32_t abs_sync_diff;
+ unsigned long diff_ms;
+ ec_sync_signal_t *sync0 = &config->dc_sync[0];
+ ec_sync_signal_t *sync1 = &config->dc_sync[1];
+ u64 start_time;
+
+ if (!config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive DC sync check datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to check DC synchrony: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff;
+ diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+
+ if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) {
+
+ if (diff_ms >= EC_DC_SYNC_WAIT_MS) {
+ EC_SLAVE_WARN(slave, "Slave did not sync after %lu ms.\n",
+ diff_ms);
+ } else {
+ EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n",
+ diff_ms, abs_sync_diff);
+
+ // check synchrony again
+ ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4);
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+ } else {
+ EC_SLAVE_DBG(slave, 1, "%u ns difference after %lu ms.\n",
+ abs_sync_diff, diff_ms);
+ }
+
+ // set DC start time (roughly in the future, not in-phase)
+ start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns
+
+ if (sync0->cycle_time) {
+ // find correct phase
+ if (master->dc_ref_time) {
+ u64 diff, start;
+ u32 remainder, cycle;
+
+ diff = start_time - master->dc_ref_time;
+ cycle = sync0->cycle_time + sync1->cycle_time;
+ remainder = do_div(diff, cycle);
+
+ start = start_time + cycle - remainder + sync0->shift_time;
+
+ EC_SLAVE_DBG(slave, 1, " ref_time=%llu\n", master->dc_ref_time);
+ EC_SLAVE_DBG(slave, 1, " app_time=%llu\n", master->app_time);
+ EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", start_time);
+ EC_SLAVE_DBG(slave, 1, " cycle=%u\n", cycle);
+ EC_SLAVE_DBG(slave, 1, " shift_time=%i\n", sync0->shift_time);
+ EC_SLAVE_DBG(slave, 1, " remainder=%u\n", remainder);
+ EC_SLAVE_DBG(slave, 1, " start=%llu\n", start);
+ start_time = start;
+ } else {
+ EC_SLAVE_WARN(slave, "No application time supplied."
+ " Cyclic start time will not be in phase.\n");
+ }
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Setting DC cyclic operation"
+ " start time to %llu.\n", start_time);
+
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8);
+ EC_WRITE_U64(datagram->data, start_time);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_dc_start;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: DC START.
+ */
+void ec_fsm_slave_config_state_dc_start(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ ec_slave_config_t *config = slave->config;
+
+ if (!config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive DC start time datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to set DC start time: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Setting DC AssignActivate to 0x%04x.\n",
+ config->dc_assign_activate);
+
+ // assign sync unit to EtherCAT or PDI
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);
+ EC_WRITE_U16(datagram->data, config->dc_assign_activate);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_config_state_dc_assign;
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: DC ASSIGN.
+ */
+void ec_fsm_slave_config_state_dc_assign(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive DC activation datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ EC_SLAVE_ERR(slave, "Failed to activate DC: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ ec_fsm_slave_config_enter_wait_safeop(fsm);
+}
+
+/****************************************************************************/
+
+/** Wait before SAFEOP transition.
+ *
+ * The feature flag WaitBeforeSAFEOPms can be used to add a wait time before
+ * going to SAFEOP. This can be used as a workaround for slaves that need some
+ * extra time for initialisation.
+ */
+void ec_fsm_slave_config_enter_wait_safeop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_config_t *config = fsm->slave->config;
+ fsm->wait_ms = 0UL;
+ if (config) {
+ ec_flag_t *flag = ec_slave_config_find_flag(config,
+ "WaitBeforeSAFEOPms");
+ if (flag && flag->value > 0) {
+ fsm->wait_ms = (unsigned long) flag->value;
+ }
+ }
+
+ if (fsm->wait_ms > 0) {
+ fsm->state = ec_fsm_slave_config_state_wait_safeop;
+
+ /* dummy read */
+ ec_datagram_fprd(fsm->datagram, fsm->slave->station_address,
+ 0x0600, 1);
+
+ fsm->jiffies_start = jiffies;
+ }
+ else {
+ ec_fsm_slave_config_enter_safeop(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: WAIT SAFEOP.
+ */
+void ec_fsm_slave_config_state_wait_safeop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ unsigned long diff = jiffies - fsm->jiffies_start;
+
+ if (diff * 1000 / HZ < fsm->wait_ms) {
+ return;
+ }
+
+ ec_fsm_slave_config_enter_safeop(fsm);
+}
+
+/****************************************************************************/
+
+/** Request SAFEOP state.
+ */
+void ec_fsm_slave_config_enter_safeop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ fsm->state = ec_fsm_slave_config_state_safeop;
+ ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAFEOP);
+ ec_fsm_change_exec(fsm->fsm_change); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: SAFEOP.
+ */
+void ec_fsm_slave_config_state_safeop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (ec_fsm_change_exec(fsm->fsm_change)) return;
+
+ if (!ec_fsm_change_success(fsm->fsm_change)) {
+ if (!fsm->fsm_change->spontaneous_change)
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ return;
+ }
+
+ // slave is now in SAFEOP
+
+ EC_SLAVE_DBG(slave, 1, "Now in SAFEOP.\n");
+
+ if (fsm->slave->current_state == fsm->slave->requested_state) {
+ fsm->state = ec_fsm_slave_config_state_end; // successful
+ EC_SLAVE_DBG(slave, 1, "Finished configuration.\n");
+ return;
+ }
+
+ ec_fsm_slave_config_enter_soe_conf_safeop(fsm);
+}
+
+/****************************************************************************/
+
+/** Check for SoE configurations to be applied in SAFEOP.
+ */
+void ec_fsm_slave_config_enter_soe_conf_safeop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *req;
+
+ if (!slave->config) {
+ ec_fsm_slave_config_enter_op(fsm);
+ return;
+ }
+
+ list_for_each_entry(req, &slave->config->soe_configs, list) {
+ if (req->al_state == EC_AL_STATE_SAFEOP) {
+ // start SoE configuration
+ fsm->state = ec_fsm_slave_config_state_soe_conf_safeop;
+ fsm->soe_request = req;
+ ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+ ec_soe_request_write(&fsm->soe_request_copy);
+ ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
+ &fsm->soe_request_copy);
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
+ return;
+ }
+ }
+
+ // No SoE configuration to be applied in SAFEOP
+ ec_fsm_slave_config_enter_op(fsm);
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: SOE_CONF.
+ */
+void ec_fsm_slave_config_state_soe_conf_safeop(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_soe_success(fsm->fsm_soe)) {
+ EC_SLAVE_ERR(slave, "SoE configuration failed.\n");
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ return;
+ }
+
+ if (!fsm->slave->config) { // config removed in the meantime
+ ec_fsm_slave_config_reconfigure(fsm);
+ return;
+ }
+
+ // Another IDN to configure in SAFEOP?
+ while (fsm->soe_request->list.next != &fsm->slave->config->soe_configs) {
+ fsm->soe_request = list_entry(fsm->soe_request->list.next,
+ ec_soe_request_t, list);
+ if (fsm->soe_request->al_state == EC_AL_STATE_SAFEOP) {
+ ec_soe_request_copy(&fsm->soe_request_copy, fsm->soe_request);
+ ec_soe_request_write(&fsm->soe_request_copy);
+ ec_fsm_soe_transfer(fsm->fsm_soe, fsm->slave,
+ &fsm->soe_request_copy);
+ ec_fsm_soe_exec(fsm->fsm_soe, fsm->datagram);
+ return;
+ }
+ }
+
+ // All SAFEOP IDNs are now configured.
+ ec_fsm_slave_config_enter_op(fsm);
+}
+
+/****************************************************************************/
+
+/** Bring slave to OP.
+ */
+void ec_fsm_slave_config_enter_op(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ // set state to OP
+ fsm->state = ec_fsm_slave_config_state_op;
+ ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_OP);
+ ec_fsm_change_exec(fsm->fsm_change); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Slave configuration state: OP
+ */
+void ec_fsm_slave_config_state_op(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (ec_fsm_change_exec(fsm->fsm_change)) return;
+
+ if (!ec_fsm_change_success(fsm->fsm_change)) {
+ if (!fsm->fsm_change->spontaneous_change)
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_config_state_error;
+ return;
+ }
+
+ // slave is now in OP
+
+ EC_SLAVE_DBG(slave, 1, "Now in OP. Finished configuration.\n");
+
+ fsm->state = ec_fsm_slave_config_state_end; // successful
+}
+
+/****************************************************************************/
+
+/** Reconfigure the slave starting at INIT.
+ */
+void ec_fsm_slave_config_reconfigure(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+ EC_SLAVE_DBG(fsm->slave, 1, "Slave configuration detached during "
+ "configuration. Reconfiguring.");
+
+ ec_fsm_slave_config_enter_init(fsm); // reconfigure
+}
+
+/*****************************************************************************
+ * Common state functions
+ ****************************************************************************/
+
+/** State: ERROR.
+ */
+void ec_fsm_slave_config_state_error(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** State: END.
+ */
+void ec_fsm_slave_config_state_end(
+ ec_fsm_slave_config_t *fsm /**< slave state machine */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_slave_config.h b/ubuntu/igh-ecat/master/fsm_slave_config.h
new file mode 100644
index 00000000000000..fa33c83b76de97
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_slave_config.h
@@ -0,0 +1,81 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+/**
+ \file
+ EtherCAT slave configuration state machine.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_SLAVE_CONFIG_H__
+#define __EC_FSM_SLAVE_CONFIG_H__
+
+#include "globals.h"
+#include "slave.h"
+#include "datagram.h"
+#include "fsm_change.h"
+#include "fsm_coe.h"
+#include "fsm_pdo.h"
+#include "fsm_eoe.h"
+
+/****************************************************************************/
+
+/** \see ec_fsm_slave_config */
+typedef struct ec_fsm_slave_config ec_fsm_slave_config_t;
+
+/** Finite state machine to configure an EtherCAT slave.
+ */
+struct ec_fsm_slave_config
+{
+ ec_datagram_t *datagram; /**< Datagram used in the state machine. */
+ ec_fsm_change_t *fsm_change; /**< State change state machine. */
+ ec_fsm_coe_t *fsm_coe; /**< CoE state machine. */
+ ec_fsm_soe_t *fsm_soe; /**< SoE state machine. */
+ ec_fsm_pdo_t *fsm_pdo; /**< PDO configuration state machine. */
+ ec_fsm_eoe_t *fsm_eoe; /**< EoE state machine. */
+
+ ec_slave_t *slave; /**< Slave the FSM runs on. */
+ void (*state)(ec_fsm_slave_config_t *); /**< State function. */
+ unsigned int retries; /**< Retries on datagram timeout. */
+ ec_sdo_request_t *request; /**< SDO request for SDO configuration. */
+ ec_sdo_request_t request_copy; /**< Copied SDO request. */
+ ec_soe_request_t *soe_request; /**< SDO request for SDO configuration. */
+ ec_soe_request_t soe_request_copy; /**< Copied SDO request. */
+ unsigned long jiffies_start; /**< For timeout calculations. */
+ unsigned int take_time; /**< Store jiffies after datagram reception. */
+ unsigned long wait_ms; /**< Wait time (used to wait before SAFEOP). */
+};
+
+/****************************************************************************/
+
+void ec_fsm_slave_config_init(ec_fsm_slave_config_t *, ec_datagram_t *,
+ ec_fsm_change_t *, ec_fsm_coe_t *, ec_fsm_soe_t *, ec_fsm_pdo_t *,
+ ec_fsm_eoe_t *);
+void ec_fsm_slave_config_clear(ec_fsm_slave_config_t *);
+
+void ec_fsm_slave_config_start(ec_fsm_slave_config_t *, ec_slave_t *);
+
+int ec_fsm_slave_config_exec(ec_fsm_slave_config_t *);
+int ec_fsm_slave_config_success(const ec_fsm_slave_config_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_slave_scan.c b/ubuntu/igh-ecat/master/fsm_slave_scan.c
new file mode 100644
index 00000000000000..e4b563583e0cea
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_slave_scan.c
@@ -0,0 +1,1121 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave state machines.
+*/
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "slave_config.h"
+
+#include "fsm_slave_scan.h"
+
+/****************************************************************************/
+
+// prototypes for private methods
+int ec_fsm_slave_scan_running(const ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_enter_sii_size(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_enter_assign_sii(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_enter_datalink(ec_fsm_slave_scan_t *);
+#ifdef EC_REGALIAS
+void ec_fsm_slave_scan_enter_regalias(ec_fsm_slave_scan_t *);
+#endif
+void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);
+
+/****************************************************************************/
+
+void ec_fsm_slave_scan_state_start(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_address(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_state(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_dc_cap(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_dc_times(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *);
+#ifdef EC_SII_ASSIGN
+void ec_fsm_slave_scan_state_assign_sii(ec_fsm_slave_scan_t *);
+#endif
+void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);
+#ifdef EC_REGALIAS
+void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *);
+#endif
+void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);
+
+void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_slave_scan_init(
+ ec_fsm_slave_scan_t *fsm, /**< Slave scanning state machine. */
+ ec_datagram_t *datagram, /**< Datagram to use. */
+ ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration
+ state machine to use. */
+ ec_fsm_pdo_t *fsm_pdo /**< PDO configuration machine to use. */
+ )
+{
+ fsm->datagram = datagram;
+ fsm->fsm_slave_config = fsm_slave_config;
+ fsm->fsm_pdo = fsm_pdo;
+
+ // init sub state machines
+ ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_slave_scan_clear(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
+{
+ // clear sub state machines
+ ec_fsm_sii_clear(&fsm->fsm_sii);
+}
+
+/****************************************************************************/
+
+/**
+ * Start slave scan state machine.
+ */
+
+void ec_fsm_slave_scan_start(
+ ec_fsm_slave_scan_t *fsm, /**< slave state machine */
+ ec_slave_t *slave /**< slave to configure */
+ )
+{
+ fsm->slave = slave;
+ fsm->state = ec_fsm_slave_scan_state_start;
+}
+
+/****************************************************************************/
+
+/**
+ \return false, if state machine has terminated
+*/
+
+int ec_fsm_slave_scan_running(
+ const ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ return fsm->state != ec_fsm_slave_scan_state_end
+ && fsm->state != ec_fsm_slave_scan_state_error;
+}
+
+/****************************************************************************/
+
+/**
+ Executes the current state of the state machine.
+ If the state machine's datagram is not sent or received yet, the execution
+ of the state machine is delayed to the next cycle.
+ \return false, if state machine has terminated
+*/
+
+int ec_fsm_slave_scan_exec(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
+{
+ if (fsm->datagram->state == EC_DATAGRAM_SENT
+ || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
+ // datagram was not sent or received yet.
+ return ec_fsm_slave_scan_running(fsm);
+ }
+
+ fsm->state(fsm);
+ return ec_fsm_slave_scan_running(fsm);
+}
+
+/****************************************************************************/
+
+/**
+ \return true, if the state machine terminated gracefully
+*/
+
+int ec_fsm_slave_scan_success(const ec_fsm_slave_scan_t *fsm /**< slave state machine */)
+{
+ return fsm->state == ec_fsm_slave_scan_state_end;
+}
+
+/*****************************************************************************
+ * slave scan state machine
+ ****************************************************************************/
+
+/**
+ Slave scan state: START.
+ First state of the slave state machine. Writes the station address to the
+ slave, according to its ring position.
+*/
+
+void ec_fsm_slave_scan_state_start(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
+{
+ // write station address
+ ec_datagram_apwr(fsm->datagram, fsm->slave->ring_position, 0x0010, 2);
+ EC_WRITE_U16(fsm->datagram->data, fsm->slave->station_address);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_address;
+}
+
+/****************************************************************************/
+
+/**
+ Slave scan state: ADDRESS.
+*/
+
+void ec_fsm_slave_scan_state_address(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(fsm->slave,
+ "Failed to receive station address datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(fsm->slave, "Failed to write station address: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ // Read AL state
+ ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0130, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_state;
+}
+
+/****************************************************************************/
+
+/**
+ Slave scan state: STATE.
+*/
+
+void ec_fsm_slave_scan_state_state(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to read AL state: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ slave->current_state = EC_READ_U8(datagram->data);
+ if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
+ char state_str[EC_STATE_STRING_SIZE];
+ ec_state_string(slave->current_state, state_str, 0);
+ EC_SLAVE_WARN(slave, "Slave has state error bit set (%s)!\n",
+ state_str);
+ }
+
+ // read base data
+ ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0000, 12);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_base;
+}
+
+/****************************************************************************/
+
+/** Slave scan state: BASE.
+ */
+void ec_fsm_slave_scan_state_base(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ u8 octet;
+ int i;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive base data datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to read base data: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ slave->base_type = EC_READ_U8 (datagram->data);
+ slave->base_revision = EC_READ_U8 (datagram->data + 1);
+ slave->base_build = EC_READ_U16(datagram->data + 2);
+
+ slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4);
+ if (slave->base_fmmu_count > EC_MAX_FMMUS) {
+ EC_SLAVE_WARN(slave, "Slave has more FMMUs (%u) than the master can"
+ " handle (%u).\n", slave->base_fmmu_count, EC_MAX_FMMUS);
+ slave->base_fmmu_count = EC_MAX_FMMUS;
+ }
+
+ slave->base_sync_count = EC_READ_U8(datagram->data + 5);
+ if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) {
+ EC_SLAVE_WARN(slave, "Slave provides more sync managers (%u)"
+ " than the master can handle (%u).\n",
+ slave->base_sync_count, EC_MAX_SYNC_MANAGERS);
+ slave->base_sync_count = EC_MAX_SYNC_MANAGERS;
+ }
+
+ octet = EC_READ_U8(datagram->data + 7);
+ for (i = 0; i < EC_MAX_PORTS; i++) {
+ slave->ports[i].desc = (octet >> (2 * i)) & 0x03;
+ }
+
+ octet = EC_READ_U8(datagram->data + 8);
+ slave->base_fmmu_bit_operation = octet & 0x01;
+ slave->base_dc_supported = (octet >> 2) & 0x01;
+ slave->base_dc_range = ((octet >> 3) & 0x01) ? EC_DC_64 : EC_DC_32;
+
+ if (slave->base_dc_supported) {
+ // read DC capabilities
+ ec_datagram_fprd(datagram, slave->station_address, 0x0910,
+ slave->base_dc_range == EC_DC_64 ? 8 : 4);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_dc_cap;
+ } else {
+ ec_fsm_slave_scan_enter_datalink(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/**
+ Slave scan state: DC CAPABILITIES.
+*/
+
+void ec_fsm_slave_scan_state_dc_cap(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive system time datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter == 1) {
+ slave->has_dc_system_time = 1;
+ EC_SLAVE_DBG(slave, 1, "Slave has the System Time register.\n");
+ } else if (datagram->working_counter == 0) {
+ EC_SLAVE_DBG(slave, 1, "Slave has no System Time register; delay "
+ "measurement only.\n");
+ } else {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to determine, if system time register is "
+ "supported: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ // read DC port receive times
+ ec_datagram_fprd(datagram, slave->station_address, 0x0900, 16);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_dc_times;
+}
+
+/****************************************************************************/
+
+/**
+ Slave scan state: DC TIMES.
+*/
+
+void ec_fsm_slave_scan_state_dc_times(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ int i;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive system time datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to get DC receive times: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ for (i = 0; i < EC_MAX_PORTS; i++) {
+ slave->ports[i].receive_time = EC_READ_U32(datagram->data + 4 * i);
+ }
+
+ ec_fsm_slave_scan_enter_datalink(fsm);
+}
+
+/****************************************************************************/
+
+/**
+ Slave scan entry function: DATALINK.
+*/
+
+void ec_fsm_slave_scan_enter_datalink(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ // read data link status
+ ec_datagram_fprd(datagram, slave->station_address, 0x0110, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_datalink;
+}
+
+/****************************************************************************/
+
+/** Enter slave scan state SII_SIZE.
+ */
+void ec_fsm_slave_scan_enter_sii_size(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ // Start fetching SII size
+
+ EC_SLAVE_DBG(fsm->slave, 1, "Determining SII size.\n");
+
+ fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header
+ ec_fsm_sii_read(&fsm->fsm_sii, fsm->slave, fsm->sii_offset,
+ EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+ fsm->state = ec_fsm_slave_scan_state_sii_size;
+ fsm->state(fsm); // execute state immediately
+}
+
+/****************************************************************************/
+
+#ifdef EC_SII_ASSIGN
+
+/** Enter slave scan state ASSIGN_SII.
+ */
+void ec_fsm_slave_scan_enter_assign_sii(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ EC_SLAVE_DBG(slave, 1, "Assigning SII access to EtherCAT.\n");
+
+ // assign SII to ECAT
+ ec_datagram_fpwr(datagram, slave->station_address, 0x0500, 1);
+ EC_WRITE_U8(datagram->data, 0x00); // EtherCAT
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_assign_sii;
+}
+
+#endif
+
+/****************************************************************************/
+
+/**
+ Slave scan state: DATALINK.
+*/
+
+void ec_fsm_slave_scan_state_datalink(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ uint16_t dl_status;
+ unsigned int i;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive DL status datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to read DL status: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ dl_status = EC_READ_U16(datagram->data);
+ for (i = 0; i < EC_MAX_PORTS; i++) {
+ slave->ports[i].link.link_up =
+ dl_status & (1 << (4 + i)) ? 1 : 0;
+ slave->ports[i].link.loop_closed =
+ dl_status & (1 << (8 + i * 2)) ? 1 : 0;
+ slave->ports[i].link.signal_detected =
+ dl_status & (1 << (9 + i * 2)) ? 1 : 0;
+ }
+
+#ifdef EC_SII_ASSIGN
+ ec_fsm_slave_scan_enter_assign_sii(fsm);
+#else
+ ec_fsm_slave_scan_enter_sii_size(fsm);
+#endif
+}
+
+/****************************************************************************/
+
+#ifdef EC_SII_ASSIGN
+
+/**
+ Slave scan state: ASSIGN_SII.
+*/
+
+void ec_fsm_slave_scan_state_assign_sii(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ return;
+ }
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ EC_SLAVE_WARN(slave, "Failed to receive SII assignment datagram: ");
+ ec_datagram_print_state(datagram);
+ // Try to go on, probably assignment is correct
+ goto continue_with_sii_size;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_WARN(slave, "Failed to assign SII to EtherCAT: ");
+ ec_datagram_print_wc_error(datagram);
+ // Try to go on, probably assignment is correct
+ }
+
+continue_with_sii_size:
+ ec_fsm_slave_scan_enter_sii_size(fsm);
+}
+
+#endif
+
+/****************************************************************************/
+
+/**
+ Slave scan state: SII SIZE.
+*/
+
+void ec_fsm_slave_scan_state_sii_size(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint16_t cat_type, cat_size;
+
+ if (ec_fsm_sii_exec(&fsm->fsm_sii))
+ return;
+
+ if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to determine SII content size:"
+ " Reading word offset 0x%04x failed. Assuming %u words.\n",
+ fsm->sii_offset, EC_FIRST_SII_CATEGORY_OFFSET);
+ slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;
+ goto alloc_sii;
+ }
+
+ cat_type = EC_READ_U16(fsm->fsm_sii.value);
+ cat_size = EC_READ_U16(fsm->fsm_sii.value + 2);
+
+ if (cat_type != 0xFFFF) { // not the last category
+ off_t next_offset = 2UL + fsm->sii_offset + cat_size;
+
+ EC_SLAVE_DBG(slave, 1, "Found category type %u with size %u."
+ " Proceeding to offset %zd.\n",
+ cat_type, cat_size, (ssize_t)next_offset);
+
+ if (next_offset >= EC_MAX_SII_SIZE) {
+ EC_SLAVE_WARN(slave, "SII size exceeds %u words"
+ " (0xffff limiter missing?).\n", EC_MAX_SII_SIZE);
+ // cut off category data...
+ slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;
+ goto alloc_sii;
+ }
+ fsm->sii_offset = next_offset;
+ ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
+ EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+ ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
+ return;
+ }
+
+ slave->sii_nwords = fsm->sii_offset + 1;
+
+alloc_sii:
+ if (slave->sii_words) {
+ EC_SLAVE_WARN(slave, "Freeing old SII data...\n");
+ kfree(slave->sii_words);
+ }
+
+ if (!(slave->sii_words =
+ (uint16_t *) kmalloc(slave->sii_nwords * 2, GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate %zu words of SII data.\n",
+ slave->sii_nwords);
+ slave->sii_nwords = 0;
+ slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ return;
+ }
+
+ // Start fetching SII contents
+
+ fsm->state = ec_fsm_slave_scan_state_sii_data;
+ fsm->sii_offset = 0x0000;
+ ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
+ EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+ ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
+}
+
+/****************************************************************************/
+
+/**
+ Slave scan state: SII DATA.
+*/
+
+void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm
+ /**< slave state machine */)
+{
+ ec_slave_t *slave = fsm->slave;
+ uint16_t *cat_word, cat_type, cat_size;
+
+ if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
+
+ if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to fetch SII contents.\n");
+ return;
+ }
+
+ // 2 words fetched
+
+ if (fsm->sii_offset + 2 <= slave->sii_nwords) { // 2 words fit
+ memcpy(slave->sii_words + fsm->sii_offset, fsm->fsm_sii.value, 4);
+ } else { // copy the last word
+ memcpy(slave->sii_words + fsm->sii_offset, fsm->fsm_sii.value, 2);
+ }
+
+ if (fsm->sii_offset + 2 < slave->sii_nwords) {
+ // fetch the next 2 words
+ fsm->sii_offset += 2;
+ ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
+ EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+ ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
+ return;
+ }
+
+ // Evaluate SII contents
+
+ ec_slave_clear_sync_managers(slave);
+
+ slave->sii.alias =
+ EC_READ_U16(slave->sii_words + 0x0004);
+ slave->effective_alias = slave->sii.alias;
+ slave->sii.vendor_id =
+ EC_READ_U32(slave->sii_words + 0x0008);
+ slave->sii.product_code =
+ EC_READ_U32(slave->sii_words + 0x000A);
+ slave->sii.revision_number =
+ EC_READ_U32(slave->sii_words + 0x000C);
+ slave->sii.serial_number =
+ EC_READ_U32(slave->sii_words + 0x000E);
+ slave->sii.boot_rx_mailbox_offset =
+ EC_READ_U16(slave->sii_words + 0x0014);
+ slave->sii.boot_rx_mailbox_size =
+ EC_READ_U16(slave->sii_words + 0x0015);
+ slave->sii.boot_tx_mailbox_offset =
+ EC_READ_U16(slave->sii_words + 0x0016);
+ slave->sii.boot_tx_mailbox_size =
+ EC_READ_U16(slave->sii_words + 0x0017);
+ slave->sii.std_rx_mailbox_offset =
+ EC_READ_U16(slave->sii_words + 0x0018);
+ slave->sii.std_rx_mailbox_size =
+ EC_READ_U16(slave->sii_words + 0x0019);
+ slave->sii.std_tx_mailbox_offset =
+ EC_READ_U16(slave->sii_words + 0x001A);
+ slave->sii.std_tx_mailbox_size =
+ EC_READ_U16(slave->sii_words + 0x001B);
+ slave->sii.mailbox_protocols =
+ EC_READ_U16(slave->sii_words + 0x001C);
+ if (slave->sii.mailbox_protocols) {
+ int need_delim = 0;
+ uint16_t all = EC_MBOX_AOE | EC_MBOX_COE | EC_MBOX_FOE |
+ EC_MBOX_SOE | EC_MBOX_VOE;
+ if ((slave->sii.mailbox_protocols & all) &&
+ slave->master->debug_level >= 1) {
+ EC_SLAVE_DBG(slave, 1, "Slave announces to support ");
+ if (slave->sii.mailbox_protocols & EC_MBOX_AOE) {
+ printk(KERN_CONT "AoE");
+ need_delim = 1;
+ }
+ if (slave->sii.mailbox_protocols & EC_MBOX_COE) {
+ if (need_delim) {
+ printk(KERN_CONT ", ");
+ }
+ printk(KERN_CONT "CoE");
+ need_delim = 1;
+ }
+ if (slave->sii.mailbox_protocols & EC_MBOX_FOE) {
+ if (need_delim) {
+ printk(KERN_CONT ", ");
+ }
+ printk(KERN_CONT "FoE");
+ need_delim = 1;
+ }
+ if (slave->sii.mailbox_protocols & EC_MBOX_SOE) {
+ if (need_delim) {
+ printk(KERN_CONT ", ");
+ }
+ printk(KERN_CONT "SoE");
+ need_delim = 1;
+ }
+ if (slave->sii.mailbox_protocols & EC_MBOX_VOE) {
+ if (need_delim) {
+ printk(KERN_CONT ", ");
+ }
+ printk(KERN_CONT "VoE");
+ need_delim = 1;
+ }
+ printk(KERN_CONT ".\n");
+ }
+ if (slave->sii.mailbox_protocols & ~all) {
+ EC_SLAVE_DBG(slave, 1, "Slave announces to support unknown"
+ " mailbox protocols 0x%04X.",
+ slave->sii.mailbox_protocols & ~all);
+ }
+ }
+ else {
+ EC_SLAVE_DBG(slave, 1, "Slave announces to support no mailbox"
+ " protocols.");
+ }
+
+ if (slave->sii.boot_rx_mailbox_offset == 0xffff ||
+ slave->sii.boot_rx_mailbox_size == 0xffff ||
+ slave->sii.boot_tx_mailbox_offset == 0xffff ||
+ slave->sii.boot_tx_mailbox_size == 0xffff ||
+ slave->sii.std_rx_mailbox_offset == 0xffff ||
+ slave->sii.std_rx_mailbox_size == 0xffff ||
+ slave->sii.std_tx_mailbox_offset == 0xffff ||
+ slave->sii.std_tx_mailbox_size == 0xffff) {
+ slave->sii.mailbox_protocols = 0x0000;
+ EC_SLAVE_ERR(slave, "Invalid mailbox settings in SII."
+ " Disabling mailbox communication.");
+ }
+
+ if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {
+ // sii does not contain category data
+ fsm->state = ec_fsm_slave_scan_state_end;
+ return;
+ }
+
+ if (slave->sii_nwords < EC_FIRST_SII_CATEGORY_OFFSET + 1) {
+ EC_SLAVE_ERR(slave, "Unexpected end of SII data:"
+ " First category header missing.\n");
+ goto end;
+ }
+
+ // evaluate category data
+ cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;
+ while (EC_READ_U16(cat_word) != 0xFFFF) {
+
+ // type and size words must fit
+ if (cat_word + 2 - slave->sii_words > slave->sii_nwords) {
+ EC_SLAVE_ERR(slave, "Unexpected end of SII data:"
+ " Category header incomplete.\n");
+ goto end;
+ }
+
+ cat_type = EC_READ_U16(cat_word) & 0x7FFF;
+ cat_size = EC_READ_U16(cat_word + 1);
+ cat_word += 2;
+
+ if (cat_word + cat_size - slave->sii_words > slave->sii_nwords) {
+ EC_SLAVE_WARN(slave, "Unexpected end of SII data:"
+ " Category data incomplete.\n");
+ goto end;
+ }
+
+ switch (cat_type) {
+ case 0x000A:
+ if (ec_slave_fetch_sii_strings(slave, (uint8_t *) cat_word,
+ cat_size * 2))
+ goto end;
+ break;
+ case 0x001E:
+ if (ec_slave_fetch_sii_general(slave, (uint8_t *) cat_word,
+ cat_size * 2))
+ goto end;
+ break;
+ case 0x0028:
+ break;
+ case 0x0029:
+ if (ec_slave_fetch_sii_syncs(slave, (uint8_t *) cat_word,
+ cat_size * 2))
+ goto end;
+ break;
+ case 0x0032:
+ if (ec_slave_fetch_sii_pdos( slave, (uint8_t *) cat_word,
+ cat_size * 2, EC_DIR_INPUT)) // TxPDO
+ goto end;
+ break;
+ case 0x0033:
+ if (ec_slave_fetch_sii_pdos( slave, (uint8_t *) cat_word,
+ cat_size * 2, EC_DIR_OUTPUT)) // RxPDO
+ goto end;
+ break;
+ default:
+ EC_SLAVE_DBG(slave, 1, "Unknown category type 0x%04X.\n",
+ cat_type);
+ }
+
+ cat_word += cat_size;
+ if (cat_word - slave->sii_words >= slave->sii_nwords) {
+ EC_SLAVE_WARN(slave, "Unexpected end of SII data:"
+ " Next category header missing.\n");
+ goto end;
+ }
+ }
+
+#ifdef EC_REGALIAS
+ ec_fsm_slave_scan_enter_regalias(fsm);
+#else
+ if (slave->sii.mailbox_protocols & EC_MBOX_COE) {
+ ec_fsm_slave_scan_enter_preop(fsm);
+ } else {
+ fsm->state = ec_fsm_slave_scan_state_end;
+ }
+#endif
+ return;
+
+end:
+ EC_SLAVE_ERR(slave, "Failed to analyze category data.\n");
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+}
+
+/****************************************************************************/
+
+#ifdef EC_REGALIAS
+
+/** Slave scan entry function: REGALIAS.
+ */
+void ec_fsm_slave_scan_enter_regalias(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ // read alias from register
+ EC_SLAVE_DBG(slave, 1, "Reading alias from register.\n");
+ ec_datagram_fprd(datagram, slave->station_address, 0x0012, 2);
+ ec_datagram_zero(datagram);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_regalias;
+}
+
+/****************************************************************************/
+
+/** Slave scan state: REGALIAS.
+ */
+void ec_fsm_slave_scan_state_regalias(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive register alias datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ EC_SLAVE_DBG(slave, 1, "Failed to read register alias.\n");
+ } else {
+ slave->effective_alias = EC_READ_U16(datagram->data);
+ EC_SLAVE_DBG(slave, 1, "Read alias %u from register.\n",
+ slave->effective_alias);
+ }
+
+ if (slave->sii.mailbox_protocols & EC_MBOX_COE) {
+ ec_fsm_slave_scan_enter_preop(fsm);
+ } else {
+ fsm->state = ec_fsm_slave_scan_state_end;
+ }
+}
+
+#endif // defined EC_REGALIAS
+
+/****************************************************************************/
+
+/** Enter slave scan state PREOP.
+ */
+void ec_fsm_slave_scan_enter_preop(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ uint8_t current_state = slave->current_state & EC_SLAVE_STATE_MASK;
+
+ if (current_state != EC_SLAVE_STATE_PREOP
+ && current_state != EC_SLAVE_STATE_SAFEOP
+ && current_state != EC_SLAVE_STATE_OP) {
+ if (slave->master->debug_level) {
+ char str[EC_STATE_STRING_SIZE];
+ ec_state_string(current_state, str, 0);
+ EC_SLAVE_DBG(slave, 0, "Slave is not in the state"
+ " to do mailbox com (%s), setting to PREOP.\n", str);
+ }
+
+ fsm->state = ec_fsm_slave_scan_state_preop;
+ ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
+ ec_fsm_slave_config_start(fsm->fsm_slave_config, slave);
+ ec_fsm_slave_config_exec(fsm->fsm_slave_config);
+ } else {
+ EC_SLAVE_DBG(slave, 1, "Reading mailbox"
+ " sync manager configuration.\n");
+
+ /* Scan current sync manager configuration to get configured mailbox
+ * sizes. */
+ ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0800,
+ EC_SYNC_PAGE_SIZE * 2);
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_slave_scan_state_sync;
+ }
+}
+
+/****************************************************************************/
+
+/** Slave scan state: PREOP.
+ */
+void ec_fsm_slave_scan_state_preop(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ if (ec_fsm_slave_config_exec(fsm->fsm_slave_config))
+ return;
+
+ if (!ec_fsm_slave_config_success(fsm->fsm_slave_config)) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ return;
+ }
+
+ ec_fsm_slave_scan_enter_pdos(fsm);
+}
+
+/****************************************************************************/
+
+/** Slave scan state: SYNC.
+ */
+void ec_fsm_slave_scan_state_sync(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ uint16_t tx_offset, tx_size, rx_offset, rx_size;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to receive sync manager"
+ " configuration datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->slave->error_flag = 1;
+ fsm->state = ec_fsm_slave_scan_state_error;
+ EC_SLAVE_ERR(slave, "Failed to read DL status: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ rx_offset = EC_READ_U16(datagram->data);
+ rx_size = EC_READ_U16(datagram->data + 2);
+ tx_offset = EC_READ_U16(datagram->data + 8);
+ tx_size = EC_READ_U16(datagram->data + 10);
+
+ if (rx_size == 0xffff) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ slave->sii.mailbox_protocols = 0x0000;
+ EC_SLAVE_ERR(slave, "Invalid RX mailbox size (%u) configured."
+ " Disabling mailbox communication.", rx_size);
+ return;
+ }
+
+ if (tx_size == 0xffff) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ slave->sii.mailbox_protocols = 0x0000;
+ EC_SLAVE_ERR(slave, "Invalid TX mailbox size (%u) configured."
+ " Disabling mailbox communication.", tx_size);
+ return;
+ }
+
+ slave->configured_rx_mailbox_offset = rx_offset;
+ slave->configured_rx_mailbox_size = rx_size;
+ slave->configured_tx_mailbox_offset = tx_offset;
+ slave->configured_tx_mailbox_size = tx_size;
+
+ EC_SLAVE_DBG(slave, 1, "Mailbox configuration:\n");
+ EC_SLAVE_DBG(slave, 1, " RX offset=0x%04x size=%u\n",
+ slave->configured_rx_mailbox_offset,
+ slave->configured_rx_mailbox_size);
+ EC_SLAVE_DBG(slave, 1, " TX offset=0x%04x size=%u\n",
+ slave->configured_tx_mailbox_offset,
+ slave->configured_tx_mailbox_size);
+
+ ec_fsm_slave_scan_enter_pdos(fsm);
+}
+
+/****************************************************************************/
+
+/** Enter slave scan state PDOS.
+ */
+void ec_fsm_slave_scan_enter_pdos(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ EC_SLAVE_DBG(slave, 1, "Scanning PDO assignment and mapping.\n");
+ fsm->state = ec_fsm_slave_scan_state_pdos;
+ ec_fsm_pdo_start_reading(fsm->fsm_pdo, slave);
+ ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram); // execute immediately
+}
+
+/****************************************************************************/
+
+/** Slave scan state: PDOS.
+ */
+void ec_fsm_slave_scan_state_pdos(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+ if (ec_fsm_pdo_exec(fsm->fsm_pdo, fsm->datagram)) {
+ return;
+ }
+
+ if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {
+ fsm->state = ec_fsm_slave_scan_state_error;
+ return;
+ }
+
+ // reading PDO configuration finished
+ fsm->state = ec_fsm_slave_scan_state_end;
+}
+
+/*****************************************************************************
+ * Common state functions
+ ****************************************************************************/
+
+/** State: ERROR.
+ */
+void ec_fsm_slave_scan_state_error(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** State: END.
+ */
+void ec_fsm_slave_scan_state_end(
+ ec_fsm_slave_scan_t *fsm /**< slave state machine */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_slave_scan.h b/ubuntu/igh-ecat/master/fsm_slave_scan.h
new file mode 100644
index 00000000000000..72c8a39ace1490
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_slave_scan.h
@@ -0,0 +1,75 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave scanning state machine.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_SLAVE_SCAN_H__
+#define __EC_FSM_SLAVE_SCAN_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "slave.h"
+#include "fsm_sii.h"
+#include "fsm_change.h"
+#include "fsm_coe.h"
+#include "fsm_pdo.h"
+
+/****************************************************************************/
+
+/** \see ec_fsm_slave_scan */
+typedef struct ec_fsm_slave_scan ec_fsm_slave_scan_t;
+
+/** Finite state machine for scanning an EtherCAT slave.
+ */
+struct ec_fsm_slave_scan
+{
+ ec_slave_t *slave; /**< Slave the FSM runs on. */
+ ec_datagram_t *datagram; /**< Datagram used in the state machine. */
+ ec_fsm_slave_config_t *fsm_slave_config; /**< Slave configuration state
+ machine to use. */
+ ec_fsm_pdo_t *fsm_pdo; /**< PDO configuration state machine to use. */
+ unsigned int retries; /**< Retries on datagram timeout. */
+
+ void (*state)(ec_fsm_slave_scan_t *); /**< State function. */
+ uint16_t sii_offset; /**< SII offset in words. */
+
+ ec_fsm_sii_t fsm_sii; /**< SII state machine. */
+};
+
+/****************************************************************************/
+
+void ec_fsm_slave_scan_init(ec_fsm_slave_scan_t *, ec_datagram_t *,
+ ec_fsm_slave_config_t *, ec_fsm_pdo_t *);
+void ec_fsm_slave_scan_clear(ec_fsm_slave_scan_t *);
+
+void ec_fsm_slave_scan_start(ec_fsm_slave_scan_t *, ec_slave_t *);
+
+int ec_fsm_slave_scan_exec(ec_fsm_slave_scan_t *);
+int ec_fsm_slave_scan_success(const ec_fsm_slave_scan_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/fsm_soe.c b/ubuntu/igh-ecat/master/fsm_soe.c
new file mode 100644
index 00000000000000..b24f03a9417b88
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_soe.c
@@ -0,0 +1,844 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2020 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT SoE state machines.
+*/
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "master.h"
+#include "mailbox.h"
+#include "fsm_soe.h"
+
+/****************************************************************************/
+
+/** SoE operations
+ */
+enum {
+ OPCODE_READ_REQUEST = 0x01, /**< Read request. */
+ OPCODE_READ_RESPONSE = 0x02, /**< Read response. */
+ OPCODE_WRITE_REQUEST = 0x03, /**< Write request. */
+ OPCODE_WRITE_RESPONSE = 0x04 /**< Write response. */
+};
+
+/** Size of all SoE headers.
+ */
+#define EC_SOE_SIZE 0x04
+
+/** SoE header size.
+ */
+#define EC_SOE_HEADER_SIZE (EC_MBOX_HEADER_SIZE + EC_SOE_SIZE)
+
+/** SoE response timeout [ms].
+ */
+#define EC_SOE_RESPONSE_TIMEOUT 1000
+
+/****************************************************************************/
+
+// prototypes for private methods
+void ec_print_soe_error(const ec_slave_t *, uint16_t);
+void ec_fsm_soe_print_error(ec_fsm_soe_t *);
+int ec_fsm_soe_prepare_read(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_next_fragment(ec_fsm_soe_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);
+
+void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);
+void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);
+
+/****************************************************************************/
+
+extern const ec_code_msg_t soe_error_codes[];
+
+/****************************************************************************/
+
+/** Outputs an SoE error code.
+*/
+void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)
+{
+ const ec_code_msg_t *error_msg;
+
+ for (error_msg = soe_error_codes; error_msg->code; error_msg++) {
+ if (error_msg->code == error_code) {
+ EC_SLAVE_ERR(slave, "SoE error 0x%04X: \"%s\".\n",
+ error_msg->code, error_msg->message);
+ return;
+ }
+ }
+
+ EC_SLAVE_ERR(slave, "Unknown SoE error 0x%04X.\n", error_code);
+}
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_fsm_soe_init(
+ ec_fsm_soe_t *fsm /**< finite state machine */
+ )
+{
+ fsm->state = NULL;
+ fsm->datagram = NULL;
+ fsm->fragment_size = 0;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_fsm_soe_clear(
+ ec_fsm_soe_t *fsm /**< finite state machine */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** Starts to transfer an IDN to/from a slave.
+ */
+void ec_fsm_soe_transfer(
+ ec_fsm_soe_t *fsm, /**< State machine. */
+ ec_slave_t *slave, /**< EtherCAT slave. */
+ ec_soe_request_t *request /**< SoE request. */
+ )
+{
+ fsm->slave = slave;
+ fsm->request = request;
+
+ if (request->dir == EC_DIR_OUTPUT) {
+ fsm->state = ec_fsm_soe_write_start;
+ } else {
+ fsm->state = ec_fsm_soe_read_start;
+ }
+}
+
+/****************************************************************************/
+
+/** Executes the current state of the state machine.
+ *
+ * \return 1 if the datagram was used, else 0.
+ */
+int ec_fsm_soe_exec(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ int datagram_used = 0;
+
+ if (fsm->datagram &&
+ (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT)) {
+ // datagram not received yet
+ return datagram_used;
+ }
+
+ fsm->state(fsm, datagram);
+
+ datagram_used =
+ fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
+
+ if (datagram_used) {
+ fsm->datagram = datagram;
+ } else {
+ fsm->datagram = NULL;
+ }
+
+ return datagram_used;
+}
+
+/****************************************************************************/
+
+/** Returns, if the state machine terminated with success.
+ *
+ * \return non-zero if successful.
+ */
+int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */)
+{
+ return fsm->state == ec_fsm_soe_end;
+}
+
+/****************************************************************************/
+
+/** Output information about a failed SoE transfer.
+ */
+void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
+{
+ ec_soe_request_t *request = fsm->request;
+
+ EC_SLAVE_ERR(fsm->slave, "");
+
+ if (request->dir == EC_DIR_OUTPUT) {
+ printk(KERN_CONT "Writing");
+ } else {
+ printk(KERN_CONT "Reading");
+ }
+
+ printk(KERN_CONT " IDN 0x%04X failed.\n", request->idn);
+}
+
+/*****************************************************************************
+ * SoE read state machine
+ ****************************************************************************/
+
+/** Prepare a read operation.
+ *
+ * \return 0 on success, otherwise a negative error code.
+ */
+int ec_fsm_soe_prepare_read(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ uint8_t *data;
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_soe_request_t *request = fsm->request;
+
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
+ EC_SOE_SIZE);
+ if (IS_ERR(data)) {
+ return PTR_ERR(data);
+ }
+
+ EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
+ EC_WRITE_U8(data + 1, 1 << 6); // request value
+ EC_WRITE_U16(data + 2, request->idn);
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "SSC read request:\n");
+ ec_print_data(data, EC_SOE_SIZE);
+ }
+
+ fsm->request->jiffies_sent = jiffies;
+ fsm->state = ec_fsm_soe_read_request;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** SoE state: READ START.
+ */
+void ec_fsm_soe_read_start(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *request = fsm->request;
+
+ EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
+ request->drive_no);
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ request->data_size = 0;
+ fsm->retries = EC_FSM_RETRIES;
+
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
+}
+
+/****************************************************************************/
+
+/** SoE state: READ REQUEST.
+ */
+void ec_fsm_soe_read_request(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ unsigned long diff_ms;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
+
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
+ if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
+ // no response; send request datagram again
+ if (ec_fsm_soe_prepare_read(fsm, datagram)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ }
+ return;
+ }
+ }
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Reception of SoE read request"
+ " failed after %lu ms: ", diff_ms);
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_read_check;
+}
+
+/****************************************************************************/
+
+/** CoE state: READ CHECK.
+ */
+void ec_fsm_soe_read_check(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (fsm->datagram->jiffies_received - fsm->jiffies_start) *
+ 1000 / HZ;
+ if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
+ " read response.\n", diff_ms);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_read_response;
+}
+
+/****************************************************************************/
+
+/** SoE state: READ RESPONSE.
+ */
+void ec_fsm_soe_read_response(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
+ value_included;
+ size_t rec_size, data_size;
+ ec_soe_request_t *req = fsm->request;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "SSC read response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_SOE) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (rec_size < EC_SOE_SIZE) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Received currupted SoE read response"
+ " (%zu bytes)!\n", rec_size);
+ ec_print_data(data, rec_size);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ header = EC_READ_U8(data);
+ opcode = header & 0x7;
+ incomplete = (header >> 3) & 1;
+ error_flag = (header >> 4) & 1;
+
+ if (opcode != OPCODE_READ_RESPONSE) {
+ EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n",
+ opcode);
+ ec_print_data(data, rec_size);
+ ec_fsm_soe_print_error(fsm);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ if (error_flag) {
+ req->error_code = EC_READ_U16(data + rec_size - 2);
+ EC_SLAVE_ERR(slave, "Received error response:\n");
+ ec_print_soe_error(slave, req->error_code);
+ ec_fsm_soe_print_error(fsm);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ } else {
+ req->error_code = 0x0000;
+ }
+
+ value_included = (EC_READ_U8(data + 1) >> 6) & 1;
+ if (!value_included) {
+ EC_SLAVE_ERR(slave, "No value included!\n");
+ ec_fsm_soe_print_error(fsm);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ data_size = rec_size - EC_SOE_SIZE;
+ if (ec_soe_request_append_data(req,
+ data + EC_SOE_SIZE, data_size)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (incomplete) {
+ EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
+ " at offset %zu.\n", req->data_size);
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_read_check;
+ } else {
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "IDN data:\n");
+ ec_print_data(req->data, req->data_size);
+ }
+
+ fsm->state = ec_fsm_soe_end; // success
+ }
+}
+
+/*****************************************************************************
+ * SoE write state machine
+ ****************************************************************************/
+
+/** Write next fragment.
+ */
+void ec_fsm_soe_write_next_fragment(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_soe_request_t *req = fsm->request;
+ uint8_t incomplete, *data;
+ size_t max_fragment_size, remaining_size;
+ uint16_t fragments_left;
+
+ remaining_size = req->data_size - fsm->offset;
+ max_fragment_size = slave->configured_rx_mailbox_size - EC_SOE_HEADER_SIZE;
+ incomplete = remaining_size > max_fragment_size;
+ fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
+ fragments_left = remaining_size / fsm->fragment_size - 1;
+ if (remaining_size % fsm->fragment_size) {
+ fragments_left++;
+ }
+
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
+ EC_SOE_SIZE + fsm->fragment_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 |
+ (req->drive_no & 0x07) << 5);
+ EC_WRITE_U8(data + 1, 1 << 6); // only value included
+ EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
+ memcpy(data + EC_SOE_SIZE, req->data + fsm->offset, fsm->fragment_size);
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "SSC write request:\n");
+ ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
+ }
+
+ fsm->state = ec_fsm_soe_write_request;
+}
+
+/****************************************************************************/
+
+/** SoE state: WRITE START.
+ */
+void ec_fsm_soe_write_start(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_soe_request_t *req = fsm->request;
+
+ EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n",
+ req->idn, req->drive_no, req->data_size);
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (slave->configured_rx_mailbox_size <= EC_SOE_HEADER_SIZE) {
+ EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n",
+ slave->configured_rx_mailbox_size);
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ fsm->offset = 0;
+ fsm->retries = EC_FSM_RETRIES;
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ req->jiffies_sent = jiffies;
+}
+
+/****************************************************************************/
+
+/** SoE state: WRITE REQUEST.
+ */
+void ec_fsm_soe_write_request(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ unsigned long diff_ms;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
+
+ if (fsm->datagram->working_counter != 1) {
+ if (!fsm->datagram->working_counter) {
+ if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
+ // no response; send request datagram again
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ return;
+ }
+ }
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Reception of SoE write request"
+ " failed after %lu ms: ", diff_ms);
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ // fragment successfully sent
+ fsm->offset += fsm->fragment_size;
+
+ if (fsm->offset < fsm->request->data_size) {
+ // next fragment
+ fsm->retries = EC_FSM_RETRIES;
+ ec_fsm_soe_write_next_fragment(fsm, datagram);
+ fsm->request->jiffies_sent = jiffies;
+ } else {
+ // all fragments sent; query response
+ fsm->jiffies_start = fsm->datagram->jiffies_sent;
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_write_check;
+ }
+}
+
+/****************************************************************************/
+
+/** CoE state: WRITE CHECK.
+ */
+void ec_fsm_soe_write_check(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ return;
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(fsm->datagram)) {
+ unsigned long diff_ms =
+ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
+ " for write response.\n", diff_ms);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_write_response;
+}
+
+/****************************************************************************/
+
+/** SoE state: WRITE RESPONSE.
+ */
+void ec_fsm_soe_write_response(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_soe_request_t *req = fsm->request;
+ uint8_t *data, mbox_prot, opcode, error_flag;
+ uint16_t idn;
+ size_t rec_size;
+
+ if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ return; // FIXME: request again?
+ }
+
+ if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Failed to receive SoE write"
+ " response datagram: ");
+ ec_datagram_print_state(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (fsm->datagram->working_counter != 1) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
+ ec_datagram_print_wc_error(fsm->datagram);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_soe_error;
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "SSC write response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_SOE) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
+ mbox_prot);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ if (rec_size < EC_SOE_SIZE) {
+ fsm->state = ec_fsm_soe_error;
+ EC_SLAVE_ERR(slave, "Received corrupted SoE write response"
+ " (%zu bytes)!\n", rec_size);
+ ec_print_data(data, rec_size);
+ ec_fsm_soe_print_error(fsm);
+ return;
+ }
+
+ opcode = EC_READ_U8(data) & 0x7;
+ if (opcode != OPCODE_WRITE_RESPONSE) {
+ EC_SLAVE_ERR(slave, "Received no write response"
+ " (opcode %x).\n", opcode);
+ ec_print_data(data, rec_size);
+ ec_fsm_soe_print_error(fsm);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ idn = EC_READ_U16(data + 2);
+ if (idn != req->idn) {
+ EC_SLAVE_ERR(slave, "Received response for"
+ " wrong IDN 0x%04x.\n", idn);
+ ec_print_data(data, rec_size);
+ ec_fsm_soe_print_error(fsm);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ error_flag = (EC_READ_U8(data) >> 4) & 1;
+ if (error_flag) {
+ if (rec_size < EC_SOE_SIZE + 2) {
+ EC_SLAVE_ERR(slave, "Received corrupted error response"
+ " - error flag set, but received size is %zu.\n",
+ rec_size);
+ } else {
+ req->error_code = EC_READ_U16(data + EC_SOE_SIZE);
+ EC_SLAVE_ERR(slave, "Received error response:\n");
+ ec_print_soe_error(slave, req->error_code);
+ }
+ ec_print_data(data, rec_size);
+ ec_fsm_soe_print_error(fsm);
+ fsm->state = ec_fsm_soe_error;
+ } else {
+ req->error_code = 0x0000;
+ fsm->state = ec_fsm_soe_end; // success
+ }
+}
+
+/****************************************************************************/
+
+/** State: ERROR.
+ */
+void ec_fsm_soe_error(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
+
+/** State: END.
+ */
+void ec_fsm_soe_end(
+ ec_fsm_soe_t *fsm, /**< finite state machine */
+ ec_datagram_t *datagram /**< Datagram to use. */
+ )
+{
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/fsm_soe.h b/ubuntu/igh-ecat/master/fsm_soe.h
new file mode 100644
index 00000000000000..b99253d8d16c64
--- /dev/null
+++ b/ubuntu/igh-ecat/master/fsm_soe.h
@@ -0,0 +1,67 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT CoE state machines.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_FSM_SOE_H__
+#define __EC_FSM_SOE_H__
+
+#include "globals.h"
+#include "datagram.h"
+#include "slave.h"
+#include "soe_request.h"
+
+/****************************************************************************/
+
+typedef struct ec_fsm_soe ec_fsm_soe_t; /**< \see ec_fsm_soe */
+
+/** Finite state machines for the Sercos over EtherCAT protocol.
+ */
+struct ec_fsm_soe {
+ ec_slave_t *slave; /**< slave the FSM runs on */
+ unsigned int retries; /**< retries upon datagram timeout */
+
+ void (*state)(ec_fsm_soe_t *, ec_datagram_t *); /**< CoE state function */
+ ec_datagram_t *datagram; /**< Datagram used in the previous step. */
+ unsigned long jiffies_start; /**< Timestamp. */
+ ec_soe_request_t *request; /**< SoE request */
+ off_t offset; /**< IDN data offset during fragmented write. */
+ size_t fragment_size; /**< Size of the current fragment. */
+};
+
+/****************************************************************************/
+
+void ec_fsm_soe_init(ec_fsm_soe_t *);
+void ec_fsm_soe_clear(ec_fsm_soe_t *);
+
+void ec_fsm_soe_transfer(ec_fsm_soe_t *, ec_slave_t *, ec_soe_request_t *);
+
+int ec_fsm_soe_exec(ec_fsm_soe_t *, ec_datagram_t *);
+int ec_fsm_soe_success(const ec_fsm_soe_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/globals.h b/ubuntu/igh-ecat/master/globals.h
new file mode 100644
index 00000000000000..da738e8b77e04f
--- /dev/null
+++ b/ubuntu/igh-ecat/master/globals.h
@@ -0,0 +1,314 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT master.
+ *
+ * The file is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU Lesser General Public License as published by the
+ * Free Software Foundation; version 2.1 of the License.
+ *
+ * This file is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this file. If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/** \file
+ * Global definitions and macros.
+ */
+
+/****************************************************************************/
+
+#ifndef __EC_MASTER_GLOBALS_H__
+#define __EC_MASTER_GLOBALS_H__
+
+#include "../globals.h"
+#include "../include/ecrt.h"
+
+/*****************************************************************************
+ * EtherCAT master
+ ****************************************************************************/
+
+/** Datagram timeout in microseconds. */
+#define EC_IO_TIMEOUT 500
+
+/** Time to send a byte in nanoseconds.
+ *
+ * t_ns = 1 / (100 MBit/s / 8 bit/byte) = 80 ns/byte
+ */
+#define EC_BYTE_TRANSMISSION_TIME_NS 80
+
+/** Number of state machine retries on datagram timeout. */
+#define EC_FSM_RETRIES 3
+
+/** Seconds to wait before fetching SDO dictionary
+ after slave entered PREOP state. */
+#define EC_WAIT_SDO_DICT 3
+
+/** Minimum size of a buffer used with ec_state_string(). */
+#define EC_STATE_STRING_SIZE 32
+
+/** Maximum SII size in words, to avoid infinite reading. */
+#define EC_MAX_SII_SIZE 4096
+
+/** Number of statistic rate intervals to maintain. */
+#define EC_RATE_COUNT 3
+
+/*****************************************************************************
+ * EtherCAT protocol
+ ****************************************************************************/
+
+/** Size of an EtherCAT frame header. */
+#define EC_FRAME_HEADER_SIZE 2
+
+/** Size of an EtherCAT datagram header. */
+#define EC_DATAGRAM_HEADER_SIZE 10
+
+/** Size of an EtherCAT datagram footer. */
+#define EC_DATAGRAM_FOOTER_SIZE 2
+
+/** Size of the EtherCAT address field. */
+#define EC_ADDR_LEN 4
+
+/** Resulting maximum data size of a single datagram in a frame. */
+#define EC_MAX_DATA_SIZE (ETH_DATA_LEN - EC_FRAME_HEADER_SIZE \
+ - EC_DATAGRAM_HEADER_SIZE - EC_DATAGRAM_FOOTER_SIZE)
+
+/** Mailbox header size. */
+#define EC_MBOX_HEADER_SIZE 6
+
+/** Word offset of first SII category. */
+#define EC_FIRST_SII_CATEGORY_OFFSET 0x40
+
+/** Size of a sync manager configuration page. */
+#define EC_SYNC_PAGE_SIZE 8
+
+/** Maximum number of FMMUs per slave. */
+#define EC_MAX_FMMUS 16
+
+/** Size of an FMMU configuration page. */
+#define EC_FMMU_PAGE_SIZE 16
+
+/** Number of DC sync signals. */
+#define EC_SYNC_SIGNAL_COUNT 2
+
+/** Size of the datagram description string.
+ *
+ * This is also used as the maximum lenth of EoE device names.
+ **/
+#define EC_DATAGRAM_NAME_SIZE 20
+
+/** Maximum hostname size.
+ *
+ * Used inside the EoE set IP parameter request.
+ */
+#define EC_MAX_HOSTNAME_SIZE 32
+
+/** Slave state mask.
+ *
+ * Apply this mask to a slave state byte to get the slave state without
+ * the error flag.
+ */
+#define EC_SLAVE_STATE_MASK 0x0F
+
+/** State of an EtherCAT slave.
+ */
+typedef enum {
+ EC_SLAVE_STATE_UNKNOWN = 0x00,
+ /**< unknown state */
+ EC_SLAVE_STATE_INIT = 0x01,
+ /**< INIT state (no mailbox communication, no IO) */
+ EC_SLAVE_STATE_PREOP = 0x02,
+ /**< PREOP state (mailbox communication, no IO) */
+ EC_SLAVE_STATE_BOOT = 0x03,
+ /**< Bootstrap state (mailbox communication, firmware update) */
+ EC_SLAVE_STATE_SAFEOP = 0x04,
+ /**< SAFEOP (mailbox communication and input update) */
+ EC_SLAVE_STATE_OP = 0x08,
+ /**< OP (mailbox communication and input/output update) */
+ EC_SLAVE_STATE_ACK_ERR = 0x10
+ /**< Acknowledge/Error bit (no actual state) */
+} ec_slave_state_t;
+
+/** Supported mailbox protocols.
+ *
+ * Not to mix up with the mailbox type field in the mailbox header defined in
+ * master/mailbox.h.
+ */
+enum {
+ EC_MBOX_AOE = 0x01, /**< ADS over EtherCAT */
+ EC_MBOX_EOE = 0x02, /**< Ethernet over EtherCAT */
+ EC_MBOX_COE = 0x04, /**< CANopen over EtherCAT */
+ EC_MBOX_FOE = 0x08, /**< File-Access over EtherCAT */
+ EC_MBOX_SOE = 0x10, /**< Servo-Profile over EtherCAT */
+ EC_MBOX_VOE = 0x20 /**< Vendor specific */
+};
+
+/** Slave information interface CANopen over EtherCAT details flags.
+ */
+typedef struct {
+ uint8_t enable_sdo : 1; /**< Enable SDO access. */
+ uint8_t enable_sdo_info : 1; /**< SDO information service available. */
+ uint8_t enable_pdo_assign : 1; /**< PDO mapping configurable. */
+ uint8_t enable_pdo_configuration : 1; /**< PDO configuration possible. */
+ uint8_t enable_upload_at_startup : 1; /**< ?. */
+ uint8_t enable_sdo_complete_access : 1; /**< Complete access possible. */
+} ec_sii_coe_details_t;
+
+/** Slave information interface general flags.
+ */
+typedef struct {
+ uint8_t enable_safeop : 1; /**< ?. */
+ uint8_t enable_not_lrw : 1; /**< Slave does not support LRW. */
+} ec_sii_general_flags_t;
+
+/** EtherCAT slave distributed clocks range.
+ */
+typedef enum {
+ EC_DC_32, /**< 32 bit. */
+ EC_DC_64 /*< 64 bit for system time, system time offset and
+ port 0 receive time. */
+} ec_slave_dc_range_t;
+
+/** EtherCAT slave sync signal configuration.
+ */
+typedef struct {
+ uint32_t cycle_time; /**< Cycle time [ns]. */
+ int32_t shift_time; /**< Shift time [ns]. */
+} ec_sync_signal_t;
+
+/** Access states for SDO entries.
+ *
+ * The access rights are managed per AL state.
+ */
+enum {
+ EC_SDO_ENTRY_ACCESS_PREOP, /**< Access rights in PREOP. */
+ EC_SDO_ENTRY_ACCESS_SAFEOP, /**< Access rights in SAFEOP. */
+ EC_SDO_ENTRY_ACCESS_OP, /**< Access rights in OP. */
+ EC_SDO_ENTRY_ACCESS_COUNT /**< Number of states. */
+};
+
+/** Master devices.
+ */
+typedef enum {
+ EC_DEVICE_MAIN, /**< Main device. */
+ EC_DEVICE_BACKUP /**< Backup device */
+} ec_device_index_t;
+
+extern const char *ec_device_names[2]; // only main and backup!
+
+/****************************************************************************/
+
+/** Convenience macro for printing EtherCAT-specific information to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT: ".
+ *
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_INFO(fmt, args...) \
+ printk(KERN_INFO "EtherCAT: " fmt, ##args)
+
+/** Convenience macro for printing EtherCAT-specific errors to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT ERROR: ".
+ *
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_ERR(fmt, args...) \
+ printk(KERN_ERR "EtherCAT ERROR: " fmt, ##args)
+
+/** Convenience macro for printing EtherCAT-specific warnings to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT WARNING: ".
+ *
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_WARN(fmt, args...) \
+ printk(KERN_WARNING "EtherCAT WARNING: " fmt, ##args)
+
+/** Convenience macro for printing EtherCAT debug messages to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT DEBUG: ".
+ *
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_DBG(fmt, args...) \
+ printk(KERN_DEBUG "EtherCAT DEBUG: " fmt, ##args)
+
+/****************************************************************************/
+
+/** Absolute value.
+ */
+#define EC_ABS(X) ((X) >= 0 ? (X) : -(X))
+
+/****************************************************************************/
+
+extern char *ec_master_version_str;
+
+/****************************************************************************/
+
+unsigned int ec_master_count(void);
+void ec_print_data(const uint8_t *, size_t);
+void ec_print_data_diff(const uint8_t *, const uint8_t *, size_t);
+size_t ec_state_string(uint8_t, char *, uint8_t);
+ssize_t ec_mac_print(const uint8_t *, char *);
+int ec_mac_is_zero(const uint8_t *);
+
+ec_master_t *ecrt_request_master_err(unsigned int);
+
+/****************************************************************************/
+
+/** Code/Message pair.
+ *
+ * Some EtherCAT datagrams support reading a status code to display a certain
+ * message. This type allows to map a code to a message string.
+ */
+typedef struct {
+ uint32_t code; /**< Code. */
+ const char *message; /**< Message belonging to \a code. */
+} ec_code_msg_t;
+
+/****************************************************************************/
+
+/** Generic request state.
+ *
+ * \attention If ever changing this, please be sure to adjust the \a
+ * state_table in master/sdo_request.c.
+ */
+typedef enum {
+ EC_INT_REQUEST_INIT,
+ EC_INT_REQUEST_QUEUED,
+ EC_INT_REQUEST_BUSY,
+ EC_INT_REQUEST_SUCCESS,
+ EC_INT_REQUEST_FAILURE
+} ec_internal_request_state_t;
+
+/****************************************************************************/
+
+extern const ec_request_state_t ec_request_state_translation_table[];
+
+/****************************************************************************/
+
+/** Origin type.
+ */
+typedef enum {
+ EC_ORIG_INTERNAL, /**< Internal. */
+ EC_ORIG_EXTERNAL /**< External. */
+} ec_origin_t;
+
+/****************************************************************************/
+
+typedef struct ec_slave ec_slave_t; /**< \see ec_slave. */
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/ioctl.c b/ubuntu/igh-ecat/master/ioctl.c
new file mode 100644
index 00000000000000..c590d5d24d94a7
--- /dev/null
+++ b/ubuntu/igh-ecat/master/ioctl.c
@@ -0,0 +1,5591 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+/**
+ \file
+ EtherCAT master character device.
+*/
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+
+#include "master.h"
+#include "slave_config.h"
+#include "voe_handler.h"
+#include "ethernet.h"
+#include "ioctl.h"
+
+/** Set to 1 to enable ioctl() latency tracing.
+ *
+ * Requires CPU timestamp counter!
+ */
+#define DEBUG_LATENCY 0
+
+/** Optional compiler attributes fo ioctl() functions.
+ */
+#if 0
+#define ATTRIBUTES __attribute__ ((__noinline__))
+#else
+#define ATTRIBUTES
+#endif
+
+#ifdef EC_IOCTL_RTDM
+# include "rtdm_details.h"
+/* RTDM does not support locking yet,
+ * therefore no send/receive callbacks are set too. */
+# define ec_ioctl_lock(lock) do {} while(0)
+# define ec_ioctl_unlock(lock) do {} while(0)
+# define ec_ioctl_lock_interruptible(lock) (0)
+# define ec_copy_to_user(to, from, n, ctx) \
+ rtdm_safe_copy_to_user(ec_ioctl_to_rtdm(ctx), to, from, n)
+# define ec_copy_from_user(to, from, n, ctx) \
+ rtdm_safe_copy_from_user(ec_ioctl_to_rtdm(ctx), to, from, n)
+#else
+# define ec_ioctl_lock(lock) rt_mutex_lock(lock)
+# define ec_ioctl_unlock(lock) rt_mutex_unlock(lock)
+# if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0) || \
+ (defined(CONFIG_PREEMPT_RT_FULL) && LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0))
+# define ec_ioctl_lock_interruptible(lock) \
+ rt_mutex_lock_interruptible(lock)
+# else
+# define ec_ioctl_lock_interruptible(lock) \
+ rt_mutex_lock_interruptible(lock, 0)
+# endif
+# define ec_copy_to_user(to, from, n, ctx) copy_to_user(to, from, n)
+# define ec_copy_from_user(to, from, n, ctx) copy_from_user(to, from, n)
+#endif // EC_IOCTL_RTDM
+
+/****************************************************************************/
+
+/** Copies a string to an ioctl structure.
+ */
+static void ec_ioctl_strcpy(
+ char *target, /**< Target. */
+ const char *source /**< Source. */
+ )
+{
+ if (source) {
+ strncpy(target, source, EC_IOCTL_STRING_SIZE);
+ target[EC_IOCTL_STRING_SIZE - 1] = 0;
+ } else {
+ target[0] = 0;
+ }
+}
+
+/****************************************************************************/
+
+/** Get module information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_module(
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_module_t data;
+
+ data.ioctl_version_magic = EC_IOCTL_VERSION_MAGIC;
+ data.master_count = ec_master_count();
+
+ if (ec_copy_to_user((void __user *) arg, &data, sizeof(data), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get master information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_master(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_master_t io;
+ unsigned int dev_idx, j;
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ io.slave_count = master->slave_count;
+ io.scan_index = master->scan_index;
+ io.config_count = ec_master_config_count(master);
+ io.domain_count = ec_master_domain_count(master);
+#ifdef EC_EOE
+ io.eoe_handler_count = ec_master_eoe_handler_count(master);
+#else
+ io.eoe_handler_count = 0;
+#endif
+ io.phase = (uint8_t) master->phase;
+ io.active = (uint8_t) master->active;
+ io.scan_busy = master->scan_busy;
+
+ up(&master->master_sem);
+
+ if (down_interruptible(&master->device_sem)) {
+ return -EINTR;
+ }
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(master); dev_idx++) {
+ ec_device_t *device = &master->devices[dev_idx];
+
+ if (device->dev) {
+ memcpy(io.devices[dev_idx].address, device->dev->dev_addr,
+ ETH_ALEN);
+ } else {
+ memcpy(io.devices[dev_idx].address, master->macs[dev_idx],
+ ETH_ALEN);
+ }
+ io.devices[dev_idx].attached = device->dev ? 1 : 0;
+ io.devices[dev_idx].link_state = device->link_state ? 1 : 0;
+ io.devices[dev_idx].tx_count = device->tx_count;
+ io.devices[dev_idx].rx_count = device->rx_count;
+ io.devices[dev_idx].tx_bytes = device->tx_bytes;
+ io.devices[dev_idx].rx_bytes = device->rx_bytes;
+ io.devices[dev_idx].tx_errors = device->tx_errors;
+ for (j = 0; j < EC_RATE_COUNT; j++) {
+ io.devices[dev_idx].tx_frame_rates[j] =
+ device->tx_frame_rates[j];
+ io.devices[dev_idx].rx_frame_rates[j] =
+ device->rx_frame_rates[j];
+ io.devices[dev_idx].tx_byte_rates[j] =
+ device->tx_byte_rates[j];
+ io.devices[dev_idx].rx_byte_rates[j] =
+ device->rx_byte_rates[j];
+ }
+ }
+ io.num_devices = ec_master_num_devices(master);
+
+ io.tx_count = master->device_stats.tx_count;
+ io.rx_count = master->device_stats.rx_count;
+ io.tx_bytes = master->device_stats.tx_bytes;
+ io.rx_bytes = master->device_stats.rx_bytes;
+ for (j = 0; j < EC_RATE_COUNT; j++) {
+ io.tx_frame_rates[j] =
+ master->device_stats.tx_frame_rates[j];
+ io.rx_frame_rates[j] =
+ master->device_stats.rx_frame_rates[j];
+ io.tx_byte_rates[j] =
+ master->device_stats.tx_byte_rates[j];
+ io.rx_byte_rates[j] =
+ master->device_stats.rx_byte_rates[j];
+ io.loss_rates[j] =
+ master->device_stats.loss_rates[j];
+ }
+
+ up(&master->device_sem);
+
+ io.app_time = master->app_time;
+ io.dc_ref_time = master->dc_ref_time;
+ io.ref_clock =
+ master->dc_ref_clock ? master->dc_ref_clock->ring_position : 0xffff;
+
+ if (copy_to_user((void __user *) arg, &io, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_slave_t data;
+ const ec_slave_t *slave;
+ int i;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave_const(
+ master, 0, data.position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n", data.position);
+ return -EINVAL;
+ }
+
+ data.device_index = slave->device_index;
+ data.vendor_id = slave->sii.vendor_id;
+ data.product_code = slave->sii.product_code;
+ data.revision_number = slave->sii.revision_number;
+ data.serial_number = slave->sii.serial_number;
+ data.alias = slave->effective_alias;
+ data.boot_rx_mailbox_offset = slave->sii.boot_rx_mailbox_offset;
+ data.boot_rx_mailbox_size = slave->sii.boot_rx_mailbox_size;
+ data.boot_tx_mailbox_offset = slave->sii.boot_tx_mailbox_offset;
+ data.boot_tx_mailbox_size = slave->sii.boot_tx_mailbox_size;
+ data.std_rx_mailbox_offset = slave->sii.std_rx_mailbox_offset;
+ data.std_rx_mailbox_size = slave->sii.std_rx_mailbox_size;
+ data.std_tx_mailbox_offset = slave->sii.std_tx_mailbox_offset;
+ data.std_tx_mailbox_size = slave->sii.std_tx_mailbox_size;
+ data.mailbox_protocols = slave->sii.mailbox_protocols;
+ data.has_general_category = slave->sii.has_general;
+ data.coe_details = slave->sii.coe_details;
+ data.general_flags = slave->sii.general_flags;
+ data.current_on_ebus = slave->sii.current_on_ebus;
+ for (i = 0; i < EC_MAX_PORTS; i++) {
+ data.ports[i].desc = slave->ports[i].desc;
+ data.ports[i].link.link_up = slave->ports[i].link.link_up;
+ data.ports[i].link.loop_closed = slave->ports[i].link.loop_closed;
+ data.ports[i].link.signal_detected =
+ slave->ports[i].link.signal_detected;
+ data.ports[i].receive_time = slave->ports[i].receive_time;
+ if (slave->ports[i].next_slave) {
+ data.ports[i].next_slave =
+ slave->ports[i].next_slave->ring_position;
+ } else {
+ data.ports[i].next_slave = 0xffff;
+ }
+ data.ports[i].delay_to_next_dc = slave->ports[i].delay_to_next_dc;
+ }
+ data.fmmu_bit = slave->base_fmmu_bit_operation;
+ data.dc_supported = slave->base_dc_supported;
+ data.dc_range = slave->base_dc_range;
+ data.has_dc_system_time = slave->has_dc_system_time;
+ data.transmission_delay = slave->transmission_delay;
+ data.al_state = slave->current_state;
+ data.error_flag = slave->error_flag;
+
+ data.sync_count = slave->sii.sync_count;
+ data.sdo_count = ec_slave_sdo_count(slave);
+ data.sii_nwords = slave->sii_nwords;
+ ec_ioctl_strcpy(data.group, slave->sii.group);
+ ec_ioctl_strcpy(data.image, slave->sii.image);
+ ec_ioctl_strcpy(data.order, slave->sii.order);
+ ec_ioctl_strcpy(data.name, slave->sii.name);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave sync manager information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sync(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_slave_sync_t data;
+ const ec_slave_t *slave;
+ const ec_sync_t *sync;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave_const(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ return -EINVAL;
+ }
+
+ if (data.sync_index >= slave->sii.sync_count) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n",
+ data.sync_index);
+ return -EINVAL;
+ }
+
+ sync = &slave->sii.syncs[data.sync_index];
+
+ data.physical_start_address = sync->physical_start_address;
+ data.default_size = sync->default_length;
+ data.control_register = sync->control_register;
+ data.enable = sync->enable;
+ data.pdo_count = ec_pdo_list_count(&sync->pdos);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave sync manager PDO information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sync_pdo(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_slave_sync_pdo_t data;
+ const ec_slave_t *slave;
+ const ec_sync_t *sync;
+ const ec_pdo_t *pdo;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave_const(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ return -EINVAL;
+ }
+
+ if (data.sync_index >= slave->sii.sync_count) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n",
+ data.sync_index);
+ return -EINVAL;
+ }
+
+ sync = &slave->sii.syncs[data.sync_index];
+ if (!(pdo = ec_pdo_list_find_pdo_by_pos_const(
+ &sync->pdos, data.pdo_pos))) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with "
+ "position %u!\n", data.sync_index, data.pdo_pos);
+ return -EINVAL;
+ }
+
+ data.index = pdo->index;
+ data.entry_count = ec_pdo_entry_count(pdo);
+ ec_ioctl_strcpy(data.name, pdo->name);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave sync manager PDO entry information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sync_pdo_entry(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_slave_sync_pdo_entry_t data;
+ const ec_slave_t *slave;
+ const ec_sync_t *sync;
+ const ec_pdo_t *pdo;
+ const ec_pdo_entry_t *entry;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave_const(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ return -EINVAL;
+ }
+
+ if (data.sync_index >= slave->sii.sync_count) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "Sync manager %u does not exist!\n",
+ data.sync_index);
+ return -EINVAL;
+ }
+
+ sync = &slave->sii.syncs[data.sync_index];
+ if (!(pdo = ec_pdo_list_find_pdo_by_pos_const(
+ &sync->pdos, data.pdo_pos))) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "Sync manager %u does not contain a PDO with "
+ "position %u!\n", data.sync_index, data.pdo_pos);
+ return -EINVAL;
+ }
+
+ if (!(entry = ec_pdo_find_entry_by_pos_const(
+ pdo, data.entry_pos))) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "PDO 0x%04X does not contain an entry with "
+ "position %u!\n", data.pdo_pos, data.entry_pos);
+ return -EINVAL;
+ }
+
+ data.index = entry->index;
+ data.subindex = entry->subindex;
+ data.bit_length = entry->bit_length;
+ ec_ioctl_strcpy(data.name, entry->name);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get domain information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_domain_t data;
+ const ec_domain_t *domain;
+ unsigned int dev_idx;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(domain = ec_master_find_domain_const(master, data.index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Domain %u does not exist!\n", data.index);
+ return -EINVAL;
+ }
+
+ data.data_size = domain->data_size;
+ data.logical_base_address = domain->logical_base_address;
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
+ data.working_counter[dev_idx] = domain->working_counter[dev_idx];
+ }
+ data.expected_working_counter = domain->expected_working_counter;
+ data.fmmu_count = ec_domain_fmmu_count(domain);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get domain FMMU information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain_fmmu(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_domain_fmmu_t data;
+ const ec_domain_t *domain;
+ const ec_fmmu_config_t *fmmu;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(domain = ec_master_find_domain_const(master, data.domain_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Domain %u does not exist!\n",
+ data.domain_index);
+ return -EINVAL;
+ }
+
+ if (!(fmmu = ec_domain_find_fmmu(domain, data.fmmu_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Domain %u has less than %u"
+ " fmmu configurations.\n",
+ data.domain_index, data.fmmu_index + 1);
+ return -EINVAL;
+ }
+
+ data.slave_config_alias = fmmu->sc->alias;
+ data.slave_config_position = fmmu->sc->position;
+ data.sync_index = fmmu->sync_index;
+ data.dir = fmmu->dir;
+ data.logical_address = fmmu->logical_start_address;
+ data.data_size = fmmu->data_size;
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get domain data.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain_data(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< Userspace address to store the results. */
+ )
+{
+ ec_ioctl_domain_data_t data;
+ const ec_domain_t *domain;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(domain = ec_master_find_domain_const(master, data.domain_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Domain %u does not exist!\n",
+ data.domain_index);
+ return -EINVAL;
+ }
+
+ if (domain->data_size != data.data_size) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Data size mismatch %u/%zu!\n",
+ data.data_size, domain->data_size);
+ return -EFAULT;
+ }
+
+ if (copy_to_user((void __user *) data.target, domain->data,
+ domain->data_size)) {
+ up(&master->master_sem);
+ return -EFAULT;
+ }
+
+ up(&master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Set master debug level.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_master_debug(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ return ec_master_debug_level(master, (unsigned long) arg);
+}
+
+/****************************************************************************/
+
+/** Issue a bus scan.
+ *
+ * \return Always zero (success).
+ */
+static ATTRIBUTES int ec_ioctl_master_rescan(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ EC_MASTER_DBG(master, 1, "Got rescan command via ioctl()."
+ " Re-scanning on next possibility.\n");
+ master->fsm.rescan_required = 1;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Set slave state.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_state_t data;
+ ec_slave_t *slave;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ return -EINVAL;
+ }
+
+ ec_slave_request_state(slave, data.al_state);
+
+ up(&master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave SDO information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sdo(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_sdo_t data;
+ const ec_slave_t *slave;
+ const ec_sdo_t *sdo;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave_const(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ return -EINVAL;
+ }
+
+ if (!(sdo = ec_slave_get_sdo_by_pos_const(
+ slave, data.sdo_position))) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", data.sdo_position);
+ return -EINVAL;
+ }
+
+ data.sdo_index = sdo->index;
+ data.max_subindex = sdo->max_subindex;
+ ec_ioctl_strcpy(data.name, sdo->name);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave SDO entry information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sdo_entry(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_sdo_entry_t data;
+ const ec_slave_t *slave;
+ const ec_sdo_t *sdo;
+ const ec_sdo_entry_t *entry;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave_const(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ return -EINVAL;
+ }
+
+ if (data.sdo_spec <= 0) {
+ if (!(sdo = ec_slave_get_sdo_by_pos_const(
+ slave, -data.sdo_spec))) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "SDO %u does not exist!\n", -data.sdo_spec);
+ return -EINVAL;
+ }
+ } else {
+ if (!(sdo = ec_slave_get_sdo_const(
+ slave, data.sdo_spec))) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "SDO 0x%04X does not exist!\n",
+ data.sdo_spec);
+ return -EINVAL;
+ }
+ }
+
+ if (!(entry = ec_sdo_get_entry_const(
+ sdo, data.sdo_entry_subindex))) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "SDO entry 0x%04X:%02X does not exist!\n",
+ sdo->index, data.sdo_entry_subindex);
+ return -EINVAL;
+ }
+
+ data.data_type = entry->data_type;
+ data.bit_length = entry->bit_length;
+ data.read_access[EC_SDO_ENTRY_ACCESS_PREOP] =
+ entry->read_access[EC_SDO_ENTRY_ACCESS_PREOP];
+ data.read_access[EC_SDO_ENTRY_ACCESS_SAFEOP] =
+ entry->read_access[EC_SDO_ENTRY_ACCESS_SAFEOP];
+ data.read_access[EC_SDO_ENTRY_ACCESS_OP] =
+ entry->read_access[EC_SDO_ENTRY_ACCESS_OP];
+ data.write_access[EC_SDO_ENTRY_ACCESS_PREOP] =
+ entry->write_access[EC_SDO_ENTRY_ACCESS_PREOP];
+ data.write_access[EC_SDO_ENTRY_ACCESS_SAFEOP] =
+ entry->write_access[EC_SDO_ENTRY_ACCESS_SAFEOP];
+ data.write_access[EC_SDO_ENTRY_ACCESS_OP] =
+ entry->write_access[EC_SDO_ENTRY_ACCESS_OP];
+ ec_ioctl_strcpy(data.description, entry->description);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Upload SDO.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sdo_upload(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_sdo_upload_t data;
+ uint8_t *target;
+ int ret;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (!(target = kmalloc(data.target_size, GFP_KERNEL))) {
+ EC_MASTER_ERR(master, "Failed to allocate %zu bytes"
+ " for SDO upload.\n", data.target_size);
+ return -ENOMEM;
+ }
+
+ ret = ecrt_master_sdo_upload(master, data.slave_position,
+ data.sdo_index, data.sdo_entry_subindex, target,
+ data.target_size, &data.data_size, &data.abort_code);
+
+ if (!ret) {
+ if (copy_to_user((void __user *) data.target,
+ target, data.data_size)) {
+ kfree(target);
+ return -EFAULT;
+ }
+ }
+
+ kfree(target);
+
+ if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Download SDO.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sdo_download(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_sdo_download_t data;
+ uint8_t *sdo_data;
+ int retval;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (!(sdo_data = kmalloc(data.data_size, GFP_KERNEL))) {
+ EC_MASTER_ERR(master, "Failed to allocate %zu bytes"
+ " for SDO download.\n", data.data_size);
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(sdo_data, (void __user *) data.data, data.data_size)) {
+ kfree(sdo_data);
+ return -EFAULT;
+ }
+
+ if (data.complete_access) {
+ retval = ecrt_master_sdo_download_complete(master, data.slave_position,
+ data.sdo_index, sdo_data, data.data_size, &data.abort_code);
+ } else {
+ retval = ecrt_master_sdo_download(master, data.slave_position,
+ data.sdo_index, data.sdo_entry_subindex, sdo_data,
+ data.data_size, &data.abort_code);
+ }
+
+ kfree(sdo_data);
+
+ if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
+ retval = -EFAULT;
+ }
+
+ return retval;
+}
+
+/****************************************************************************/
+
+/** Read a slave's SII.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sii_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_sii_t data;
+ const ec_slave_t *slave;
+ int retval;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(slave = ec_master_find_slave_const(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ return -EINVAL;
+ }
+
+ if (!data.nwords
+ || data.offset + data.nwords > slave->sii_nwords) {
+ up(&master->master_sem);
+ EC_SLAVE_ERR(slave, "Invalid SII read offset/size %u/%u for slave SII"
+ " size %zu!\n", data.offset, data.nwords, slave->sii_nwords);
+ return -EINVAL;
+ }
+
+ if (copy_to_user((void __user *) data.words,
+ slave->sii_words + data.offset, data.nwords * 2))
+ retval = -EFAULT;
+ else
+ retval = 0;
+
+ up(&master->master_sem);
+ return retval;
+}
+
+/****************************************************************************/
+
+/** Write a slave's SII.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_sii_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_sii_t data;
+ ec_slave_t *slave;
+ unsigned int byte_size;
+ uint16_t *words;
+ ec_sii_write_request_t request;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (!data.nwords) {
+ return 0;
+ }
+
+ byte_size = sizeof(uint16_t) * data.nwords;
+ if (!(words = kmalloc(byte_size, GFP_KERNEL))) {
+ EC_MASTER_ERR(master, "Failed to allocate %u bytes"
+ " for SII contents.\n", byte_size);
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(words,
+ (void __user *) data.words, byte_size)) {
+ kfree(words);
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(words);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(
+ master, 0, data.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ data.slave_position);
+ kfree(words);
+ return -EINVAL;
+ }
+
+ // init SII write request
+ INIT_LIST_HEAD(&request.list);
+ request.slave = slave;
+ request.words = words;
+ request.offset = data.offset;
+ request.nwords = data.nwords;
+ request.state = EC_INT_REQUEST_QUEUED;
+
+ // schedule SII write request.
+ list_add_tail(&request.list, &master->sii_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ // abort request
+ list_del(&request.list);
+ up(&master->master_sem);
+ kfree(words);
+ return -EINTR;
+ }
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ kfree(words);
+
+ return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+}
+
+/****************************************************************************/
+
+/** Read a slave's registers.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_reg_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_reg_t io;
+ ec_slave_t *slave;
+ ec_reg_request_t request;
+ int ret;
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ if (!io.size) {
+ return 0;
+ }
+
+ // init register request
+ ret = ec_reg_request_init(&request, io.size);
+ if (ret) {
+ return ret;
+ }
+
+ ret = ecrt_reg_request_read(&request, io.address, io.size);
+ if (ret) {
+ return ret;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_reg_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(
+ master, 0, io.slave_position))) {
+ up(&master->master_sem);
+ ec_reg_request_clear(&request);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ io.slave_position);
+ return -EINVAL;
+ }
+
+ // schedule request.
+ list_add_tail(&request.list, &slave->reg_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ // abort request
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_reg_request_clear(&request);
+ return -EINTR;
+ }
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ if (request.state == EC_INT_REQUEST_SUCCESS) {
+ if (copy_to_user((void __user *) io.data, request.data, io.size)) {
+ return -EFAULT;
+ }
+ }
+ ec_reg_request_clear(&request);
+
+ return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+}
+
+/****************************************************************************/
+
+/** Write a slave's registers.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_reg_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_reg_t io;
+ ec_slave_t *slave;
+ ec_reg_request_t request;
+ int ret;
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ if (!io.size) {
+ return 0;
+ }
+
+ // init register request
+ ret = ec_reg_request_init(&request, io.size);
+ if (ret) {
+ return ret;
+ }
+
+ if (copy_from_user(request.data, (void __user *) io.data, io.size)) {
+ ec_reg_request_clear(&request);
+ return -EFAULT;
+ }
+
+ ret = ecrt_reg_request_write(&request, io.address, io.size);
+ if (ret) {
+ return ret;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_reg_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (io.emergency) {
+ request.ring_position = io.slave_position;
+ // schedule request.
+ list_add_tail(&request.list, &master->emerg_reg_requests);
+ }
+ else {
+ if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) {
+ up(&master->master_sem);
+ ec_reg_request_clear(&request);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ io.slave_position);
+ return -EINVAL;
+ }
+
+ // schedule request.
+ list_add_tail(&request.list, &slave->reg_requests);
+ }
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ // abort request
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_reg_request_clear(&request);
+ return -EINTR;
+ }
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ ec_reg_request_clear(&request);
+
+ return request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+}
+
+/****************************************************************************/
+
+/** Get slave configuration information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_config(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_config_t data;
+ const ec_slave_config_t *sc;
+ uint8_t i;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config_const(
+ master, data.config_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave config %u does not exist!\n",
+ data.config_index);
+ return -EINVAL;
+ }
+
+ data.alias = sc->alias;
+ data.position = sc->position;
+ data.vendor_id = sc->vendor_id;
+ data.product_code = sc->product_code;
+ for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++) {
+ data.syncs[i].dir = sc->sync_configs[i].dir;
+ data.syncs[i].watchdog_mode = sc->sync_configs[i].watchdog_mode;
+ data.syncs[i].pdo_count =
+ ec_pdo_list_count(&sc->sync_configs[i].pdos);
+ }
+ data.watchdog_divider = sc->watchdog_divider;
+ data.watchdog_intervals = sc->watchdog_intervals;
+ data.sdo_count = ec_slave_config_sdo_count(sc);
+ data.idn_count = ec_slave_config_idn_count(sc);
+ data.flag_count = ec_slave_config_flag_count(sc);
+ data.slave_position = sc->slave ? sc->slave->ring_position : -1;
+ data.dc_assign_activate = sc->dc_assign_activate;
+ for (i = 0; i < EC_SYNC_SIGNAL_COUNT; i++) {
+ data.dc_sync[i] = sc->dc_sync[i];
+ }
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave configuration PDO information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_config_pdo(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_config_pdo_t data;
+ const ec_slave_config_t *sc;
+ const ec_pdo_t *pdo;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (data.sync_index >= EC_MAX_SYNC_MANAGERS) {
+ EC_MASTER_ERR(master, "Invalid sync manager index %u!\n",
+ data.sync_index);
+ return -EINVAL;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config_const(
+ master, data.config_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave config %u does not exist!\n",
+ data.config_index);
+ return -EINVAL;
+ }
+
+ if (!(pdo = ec_pdo_list_find_pdo_by_pos_const(
+ &sc->sync_configs[data.sync_index].pdos,
+ data.pdo_pos))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Invalid PDO position!\n");
+ return -EINVAL;
+ }
+
+ data.index = pdo->index;
+ data.entry_count = ec_pdo_entry_count(pdo);
+ ec_ioctl_strcpy(data.name, pdo->name);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave configuration PDO entry information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_config_pdo_entry(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_config_pdo_entry_t data;
+ const ec_slave_config_t *sc;
+ const ec_pdo_t *pdo;
+ const ec_pdo_entry_t *entry;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (data.sync_index >= EC_MAX_SYNC_MANAGERS) {
+ EC_MASTER_ERR(master, "Invalid sync manager index %u!\n",
+ data.sync_index);
+ return -EINVAL;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config_const(
+ master, data.config_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave config %u does not exist!\n",
+ data.config_index);
+ return -EINVAL;
+ }
+
+ if (!(pdo = ec_pdo_list_find_pdo_by_pos_const(
+ &sc->sync_configs[data.sync_index].pdos,
+ data.pdo_pos))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Invalid PDO position!\n");
+ return -EINVAL;
+ }
+
+ if (!(entry = ec_pdo_find_entry_by_pos_const(
+ pdo, data.entry_pos))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Entry not found!\n");
+ return -EINVAL;
+ }
+
+ data.index = entry->index;
+ data.subindex = entry->subindex;
+ data.bit_length = entry->bit_length;
+ ec_ioctl_strcpy(data.name, entry->name);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave configuration SDO information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_config_sdo(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_config_sdo_t *ioctl;
+ const ec_slave_config_t *sc;
+ const ec_sdo_request_t *req;
+
+ if (!(ioctl = kmalloc(sizeof(*ioctl), GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(ioctl, (void __user *) arg, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(ioctl);
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config_const(
+ master, ioctl->config_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave config %u does not exist!\n",
+ ioctl->config_index);
+ kfree(ioctl);
+ return -EINVAL;
+ }
+
+ if (!(req = ec_slave_config_get_sdo_by_pos_const(
+ sc, ioctl->sdo_pos))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Invalid SDO position!\n");
+ kfree(ioctl);
+ return -EINVAL;
+ }
+
+ ioctl->index = req->index;
+ ioctl->subindex = req->subindex;
+ ioctl->size = req->data_size;
+ memcpy(ioctl->data, req->data,
+ min((u32) ioctl->size, (u32) EC_MAX_SDO_DATA_SIZE));
+ ioctl->complete_access = req->complete_access;
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ kfree(ioctl);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave configuration IDN information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_config_idn(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_config_idn_t *ioctl;
+ const ec_slave_config_t *sc;
+ const ec_soe_request_t *req;
+
+ if (!(ioctl = kmalloc(sizeof(*ioctl), GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(ioctl, (void __user *) arg, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(ioctl);
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config_const(
+ master, ioctl->config_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave config %u does not exist!\n",
+ ioctl->config_index);
+ kfree(ioctl);
+ return -EINVAL;
+ }
+
+ if (!(req = ec_slave_config_get_idn_by_pos_const(
+ sc, ioctl->idn_pos))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Invalid IDN position!\n");
+ kfree(ioctl);
+ return -EINVAL;
+ }
+
+ ioctl->drive_no = req->drive_no;
+ ioctl->idn = req->idn;
+ ioctl->state = req->al_state;
+ ioctl->size = req->data_size;
+ memcpy(ioctl->data, req->data,
+ min((u32) ioctl->size, (u32) EC_MAX_IDN_DATA_SIZE));
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ kfree(ioctl);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get slave configuration feature flag information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_config_flag(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_config_flag_t *ioctl;
+ const ec_slave_config_t *sc;
+ const ec_flag_t *flag;
+ size_t size;
+
+ if (!(ioctl = kmalloc(sizeof(*ioctl), GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(ioctl, (void __user *) arg, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(ioctl);
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config_const(
+ master, ioctl->config_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave config %u does not exist!\n",
+ ioctl->config_index);
+ kfree(ioctl);
+ return -EINVAL;
+ }
+
+ if (!(flag = ec_slave_config_get_flag_by_pos_const(
+ sc, ioctl->flag_pos))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Invalid flag position!\n");
+ kfree(ioctl);
+ return -EINVAL;
+ }
+
+ size = min((u32) strlen(flag->key), (u32) EC_MAX_FLAG_KEY_SIZE - 1);
+ memcpy(ioctl->key, flag->key, size);
+ ioctl->key[size] = 0x00;
+ ioctl->value = flag->value;
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ kfree(ioctl);
+ return 0;
+}
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+
+/** Get configured EoE IP parameters for a given slave configuration.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_config_ip(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_eoe_ip_t *ioctl;
+ const ec_slave_config_t *sc;
+ const ec_eoe_request_t *req;
+
+ if (!(ioctl = kmalloc(sizeof(*ioctl), GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(ioctl, (void __user *) arg, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(ioctl);
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config_const(master, ioctl->config_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave config %u does not exist!\n",
+ ioctl->config_index);
+ kfree(ioctl);
+ return -EINVAL;
+ }
+
+ req = &sc->eoe_ip_param_request;
+
+ ioctl->mac_address_included = req->mac_address_included;
+ ioctl->ip_address_included = req->ip_address_included;
+ ioctl->subnet_mask_included = req->subnet_mask_included;
+ ioctl->gateway_included = req->gateway_included;
+ ioctl->dns_included = req->dns_included;
+ ioctl->name_included = req->name_included;
+
+ memcpy(ioctl->mac_address, req->mac_address, EC_ETH_ALEN);
+ ioctl->ip_address = req->ip_address;
+ ioctl->subnet_mask = req->subnet_mask;
+ ioctl->gateway = req->gateway;
+ ioctl->dns = req->dns;
+ strncpy(ioctl->name, req->name, EC_MAX_HOSTNAME_SIZE);
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, ioctl, sizeof(*ioctl))) {
+ kfree(ioctl);
+ return -EFAULT;
+ }
+
+ kfree(ioctl);
+ return 0;
+}
+
+#endif
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+
+/** Get EoE handler information.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_eoe_handler(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_eoe_handler_t data;
+ const ec_eoe_t *eoe;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(eoe = ec_master_get_eoe_handler_const(master, data.eoe_index))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "EoE handler %u does not exist!\n",
+ data.eoe_index);
+ return -EINVAL;
+ }
+
+ if (eoe->slave) {
+ data.slave_position = eoe->slave->ring_position;
+ } else {
+ data.slave_position = 0xffff;
+ }
+ snprintf(data.name, EC_DATAGRAM_NAME_SIZE, eoe->dev->name);
+ data.open = eoe->opened;
+ data.rx_bytes = eoe->stats.tx_bytes;
+ data.rx_rate = eoe->tx_rate;
+ data.tx_bytes = eoe->stats.rx_bytes;
+ data.tx_rate = eoe->tx_rate;
+ data.tx_queued_frames = eoe->tx_queued_frames;
+ data.tx_queue_size = eoe->tx_queue_size;
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+#endif
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+/** Request EoE IP parameter setting.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_eoe_ip_param(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_eoe_ip_t io;
+ ec_eoe_request_t req;
+ ec_slave_t *slave;
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ // init EoE request
+ ec_eoe_request_init(&req);
+
+ req.mac_address_included = io.mac_address_included;
+ req.ip_address_included = io.ip_address_included;
+ req.subnet_mask_included = io.subnet_mask_included;
+ req.gateway_included = io.gateway_included;
+ req.dns_included = io.dns_included;
+ req.name_included = io.name_included;
+
+ memcpy(req.mac_address, io.mac_address, EC_ETH_ALEN);
+ req.ip_address = io.ip_address;
+ req.subnet_mask = io.subnet_mask;
+ req.gateway = io.gateway;
+ req.dns = io.dns;
+ memcpy(req.name, io.name, EC_MAX_HOSTNAME_SIZE);
+
+ req.state = EC_INT_REQUEST_QUEUED;
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(
+ master, 0, io.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ io.slave_position);
+ return -EINVAL;
+ }
+
+ EC_MASTER_DBG(master, 1, "Scheduling EoE request.\n");
+
+ // schedule request.
+ list_add_tail(&req.list, &slave->eoe_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ req.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (req.state == EC_INT_REQUEST_QUEUED) {
+ // abort request
+ list_del(&req.list);
+ up(&master->master_sem);
+ return -EINTR;
+ }
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, req.state != EC_INT_REQUEST_BUSY);
+
+ io.result = req.result;
+
+ if (copy_to_user((void __user *) arg, &io, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ return req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+}
+#endif
+
+/*****************************************************************************/
+
+/** Request the master from userspace.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_request(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_master_t *m;
+ int ret = 0;
+
+ m = ecrt_request_master_err(master->index);
+ if (IS_ERR(m)) {
+ ret = PTR_ERR(m);
+ } else {
+ ctx->requested = 1;
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Create a domain.
+ *
+ * \return Domain index on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_create_domain(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_domain_t *domain;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ domain = ecrt_master_create_domain_err(master);
+ if (IS_ERR(domain))
+ return PTR_ERR(domain);
+
+ return domain->index;
+}
+
+/****************************************************************************/
+
+/** Create a slave configuration.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_create_slave_config(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_config_t data;
+ ec_slave_config_t *sc, *entry;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ sc = ecrt_master_slave_config_err(master, data.alias, data.position,
+ data.vendor_id, data.product_code);
+ if (IS_ERR(sc))
+ return PTR_ERR(sc);
+
+ data.config_index = 0;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ list_for_each_entry(entry, &master->configs, list) {
+ if (entry == sc)
+ break;
+ data.config_index++;
+ }
+
+ up(&master->master_sem);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Select the DC reference clock.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_select_ref_clock(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ unsigned long config_index = (unsigned long) arg;
+ ec_slave_config_t *sc = NULL;
+ int ret = 0;
+
+ if (unlikely(!ctx->requested)) {
+ ret = -EPERM;
+ goto out_return;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ ret = -EINTR;
+ goto out_return;
+ }
+
+ if (config_index != 0xFFFFFFFF) {
+ if (!(sc = ec_master_get_config(master, config_index))) {
+ ret = -ENOENT;
+ goto out_up;
+ }
+ }
+
+ ecrt_master_select_reference_clock(master, sc);
+
+out_up:
+ up(&master->master_sem);
+out_return:
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Activates the master.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_activate(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_master_activate_t io;
+ ec_domain_t *domain;
+ off_t offset;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ io.process_data = NULL;
+
+ /* Get the sum of the domains' process data sizes. */
+
+ ctx->process_data_size = 0;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ list_for_each_entry(domain, &master->domains, list) {
+ ctx->process_data_size += ecrt_domain_size(domain);
+ }
+
+ up(&master->master_sem);
+
+ if (ctx->process_data_size) {
+ ctx->process_data = vmalloc(ctx->process_data_size);
+ if (!ctx->process_data) {
+ ctx->process_data_size = 0;
+ return -ENOMEM;
+ }
+
+ /* Set the memory as external process data memory for the
+ * domains.
+ */
+ offset = 0;
+ list_for_each_entry(domain, &master->domains, list) {
+ ecrt_domain_external_memory(domain,
+ ctx->process_data + offset);
+ offset += ecrt_domain_size(domain);
+ }
+
+#if defined(EC_IOCTL_RTDM) && !defined(EC_RTDM_XENOMAI_V3)
+ /* RTDM uses a different approach for memory-mapping, which has to be
+ * initiated by the kernel.
+ */
+ ret = ec_rtdm_mmap(ctx, &io.process_data);
+ if (ret < 0) {
+ EC_MASTER_ERR(master, "Failed to map process data"
+ " memory to user space (code %i).\n", ret);
+ return ret;
+ }
+#endif
+ }
+
+ io.process_data_size = ctx->process_data_size;
+
+#ifndef EC_IOCTL_RTDM
+ /* RTDM does not support locking yet. */
+ ecrt_master_callbacks(master, ec_master_internal_send_cb,
+ ec_master_internal_receive_cb, master);
+#endif
+
+ ret = ecrt_master_activate(master);
+ if (ret < 0)
+ return ret;
+
+ if (copy_to_user((void __user *) arg, &io,
+ sizeof(ec_ioctl_master_activate_t)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Deactivates the master.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_deactivate(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ return ecrt_master_deactivate(master);
+}
+
+/****************************************************************************/
+
+/** Set max. number of databytes in a cycle
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_set_send_interval(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ size_t send_interval;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (copy_from_user(&send_interval, (void __user *) arg,
+ sizeof(send_interval))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ ec_master_set_send_interval(master, send_interval);
+
+ up(&master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Send frames.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_send(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ ret = ecrt_master_send(master);
+ ec_ioctl_unlock(&master->io_mutex);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Receive frames.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_receive(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ ret = ecrt_master_receive(master);
+ ec_ioctl_unlock(&master->io_mutex);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Get the master state.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_master_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_master_state_t data;
+ int ret;
+
+ ret = ecrt_master_state(master, &data);
+ if (ret)
+ return ret;
+
+ if (ec_copy_to_user((void __user *) arg, &data, sizeof(data), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get the link state.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_master_link_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_link_state_t ioctl;
+ ec_master_link_state_t state;
+ int ret;
+
+ if (ec_copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl), ctx)) {
+ return -EFAULT;
+ }
+
+ ret = ecrt_master_link_state(master, ioctl.dev_idx, &state);
+ if (ret < 0) {
+ return ret;
+ }
+
+ if (ec_copy_to_user((void __user *) ioctl.state,
+ &state, sizeof(state), ctx)) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Set the master DC application time.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_app_time(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ uint64_t time;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&time, (void __user *) arg, sizeof(time), ctx)) {
+ return -EFAULT;
+ }
+
+ return ecrt_master_application_time(master, time);
+}
+
+/****************************************************************************/
+
+/** Sync the reference clock.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sync_ref(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ ret = ecrt_master_sync_reference_clock(master);
+ ec_ioctl_unlock(&master->io_mutex);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Sync the reference clock.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sync_ref_to(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ int ret;
+ uint64_t time;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&time, (void __user *) arg, sizeof(time), ctx)) {
+ return -EFAULT;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ ret = ecrt_master_sync_reference_clock_to(master, time);
+ ec_ioctl_unlock(&master->io_mutex);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Sync the slave clocks.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sync_slaves(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ ret = ecrt_master_sync_slave_clocks(master);
+ ec_ioctl_unlock(&master->io_mutex);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Get the system time of the reference clock.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_ref_clock_time(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ uint32_t time;
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ ret = ecrt_master_reference_clock_time(master, &time);
+ if (ret) {
+ return ret;
+ }
+
+ if (ec_copy_to_user((void __user *) arg, &time, sizeof(time), ctx)) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Queue the sync monitoring datagram.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sync_mon_queue(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ ret = ecrt_master_sync_monitor_queue(master);
+ ec_ioctl_unlock(&master->io_mutex);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Processes the sync monitoring datagram.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sync_mon_process(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ uint32_t time_diff;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ time_diff = ecrt_master_sync_monitor_process(master);
+
+ if (ec_copy_to_user((void __user *) arg, &time_diff,
+ sizeof(time_diff), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Reset configuration.
+ *
+ * \return Always zero (success).
+ */
+static ATTRIBUTES int ec_ioctl_reset(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+#ifdef EC_IOCTL_RTDM
+ /* Xenomai/LXRT is like NMI context, so we do a two-stage schedule. */
+ irq_work_queue(&master->sc_reset_work_kicker);
+#else
+ schedule_work(&master->sc_reset_work);
+#endif
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Configure a sync manager.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_sync(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_config_t data;
+ ec_slave_config_t *sc;
+ unsigned int i;
+ int ret = 0;
+
+ if (unlikely(!ctx->requested)) {
+ ret = -EPERM;
+ goto out_return;
+ }
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ ret = -EFAULT;
+ goto out_return;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ ret = -EINTR;
+ goto out_return;
+ }
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ ret = -ENOENT;
+ goto out_up;
+ }
+
+ for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++) {
+ if (data.syncs[i].config_this) {
+ ret = ecrt_slave_config_sync_manager(sc, i, data.syncs[i].dir,
+ data.syncs[i].watchdog_mode);
+ if (ret) {
+ goto out_up;
+ }
+ }
+ }
+
+out_up:
+ up(&master->master_sem);
+out_return:
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Configure a slave's watchdogs.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_watchdog(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_config_t data;
+ ec_slave_config_t *sc;
+ int ret = 0;
+
+ if (unlikely(!ctx->requested)) {
+ ret = -EPERM;
+ goto out_return;
+ }
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ ret = -EFAULT;
+ goto out_return;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ ret = -EINTR;
+ goto out_return;
+ }
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ ret = -ENOENT;
+ goto out_up;
+ }
+
+ ret = ecrt_slave_config_watchdog(sc,
+ data.watchdog_divider, data.watchdog_intervals);
+
+out_up:
+ up(&master->master_sem);
+out_return:
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Add a PDO to the assignment.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_add_pdo(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_config_pdo_t data;
+ ec_slave_config_t *sc;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
+ return -EFAULT;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ return ecrt_slave_config_pdo_assign_add(sc, data.sync_index, data.index);
+}
+
+/****************************************************************************/
+
+/** Clears the PDO assignment.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_clear_pdos(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_config_pdo_t data;
+ ec_slave_config_t *sc;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
+ return -EFAULT;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ return ecrt_slave_config_pdo_assign_clear(sc, data.sync_index);
+}
+
+/****************************************************************************/
+
+/** Add an entry to a PDO's mapping.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_add_entry(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_add_pdo_entry_t data;
+ ec_slave_config_t *sc;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
+ return -EFAULT;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ return ecrt_slave_config_pdo_mapping_add(sc, data.pdo_index,
+ data.entry_index, data.entry_subindex, data.entry_bit_length);
+}
+
+/****************************************************************************/
+
+/** Clears the mapping of a PDO.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_clear_entries(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_config_pdo_t data;
+ ec_slave_config_t *sc;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
+ return -EFAULT;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ return ecrt_slave_config_pdo_mapping_clear(sc, data.index);
+}
+
+/****************************************************************************/
+
+/** Registers a PDO entry.
+ *
+ * \return Process data offset on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_reg_pdo_entry(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_reg_pdo_entry_t data;
+ ec_slave_config_t *sc;
+ ec_domain_t *domain;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
+ return -EFAULT;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ if (!(domain = ec_master_find_domain(master, data.domain_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc or domain could be invalidated */
+
+ ret = ecrt_slave_config_reg_pdo_entry(sc, data.entry_index,
+ data.entry_subindex, domain, &data.bit_position);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Registers a PDO entry by its position.
+ *
+ * \return Process data offset on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_reg_pdo_pos(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_reg_pdo_pos_t io;
+ ec_slave_config_t *sc;
+ ec_domain_t *domain;
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ if (!(domain = ec_master_find_domain(master, io.domain_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc or domain could be invalidated */
+
+ ret = ecrt_slave_config_reg_pdo_entry_pos(sc, io.sync_index,
+ io.pdo_pos, io.entry_pos, domain, &io.bit_position);
+
+ if (copy_to_user((void __user *) arg, &io, sizeof(io)))
+ return -EFAULT;
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Sets the DC AssignActivate word and the sync signal times.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_dc(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_config_t data;
+ ec_slave_config_t *sc;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
+ return -EFAULT;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ ret = ecrt_slave_config_dc(sc, data.dc_assign_activate,
+ data.dc_sync[0].cycle_time,
+ data.dc_sync[0].shift_time,
+ data.dc_sync[1].cycle_time,
+ data.dc_sync[1].shift_time);
+
+ up(&master->master_sem);
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Configures an SDO.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_sdo(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_sdo_t data;
+ ec_slave_config_t *sc;
+ uint8_t *sdo_data = NULL;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data)))
+ return -EFAULT;
+
+ if (!data.size)
+ return -EINVAL;
+
+ if (!(sdo_data = kmalloc(data.size, GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(sdo_data, (void __user *) data.data, data.size)) {
+ kfree(sdo_data);
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(sdo_data);
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ up(&master->master_sem);
+ kfree(sdo_data);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ if (data.complete_access) {
+ ret = ecrt_slave_config_complete_sdo(sc,
+ data.index, sdo_data, data.size);
+ } else {
+ ret = ecrt_slave_config_sdo(sc, data.index, data.subindex, sdo_data,
+ data.size);
+ }
+ kfree(sdo_data);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Set the emergency ring buffer size.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_emerg_size(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_emerg_t io;
+ ec_slave_config_t *sc;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io)))
+ return -EFAULT;
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ ret = ecrt_slave_config_emerg_size(sc, io.size);
+
+ up(&master->master_sem);
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Get an emergency message from the ring.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_emerg_pop(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_emerg_t io;
+ ec_slave_config_t *sc;
+ u8 msg[EC_COE_EMERGENCY_MSG_SIZE];
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_copy_from_user(&io, (void __user *) arg, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because configuration will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ return -ENOENT;
+ }
+
+ ret = ecrt_slave_config_emerg_pop(sc, msg);
+ if (ret < 0) {
+ return ret;
+ }
+
+ if (ec_copy_to_user((void __user *) io.target, msg, sizeof(msg), ctx)) {
+ return -EFAULT;
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Clear the emergency ring.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_emerg_clear(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_emerg_t io;
+ ec_slave_config_t *sc;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_copy_from_user(&io, (void __user *) arg, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because configuration will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_slave_config_emerg_clear(sc);
+}
+
+/****************************************************************************/
+
+/** Get the number of emergency overruns.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_emerg_overruns(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_emerg_t io;
+ ec_slave_config_t *sc;
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_copy_from_user(&io, (void __user *) arg, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because configuration will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ return -ENOENT;
+ }
+
+ ret = ecrt_slave_config_emerg_overruns(sc);
+ if (ret < 0) {
+ return ret;
+ }
+
+ io.overruns = ret;
+
+ if (ec_copy_to_user((void __user *) arg, &io, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Create an SDO request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_create_sdo_request(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sdo_request_t data;
+ ec_slave_config_t *sc;
+ ec_sdo_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ data.request_index = 0;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ sc = ec_master_get_config(master, data.config_index);
+ if (!sc) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ list_for_each_entry(req, &sc->sdo_requests, list) {
+ data.request_index++;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ req = ecrt_slave_config_create_sdo_request_err(sc, data.sdo_index,
+ data.sdo_subindex, data.size);
+ if (IS_ERR(req))
+ return PTR_ERR(req);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Create an SoE request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_create_soe_request(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_soe_request_t data;
+ ec_slave_config_t *sc;
+ ec_soe_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ data.request_index = 0;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ sc = ec_master_get_config(master, data.config_index);
+ if (!sc) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ list_for_each_entry(req, &sc->soe_requests, list) {
+ data.request_index++;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ req = ecrt_slave_config_create_soe_request_err(sc, data.drive_no,
+ data.idn, data.size);
+ if (IS_ERR(req)) {
+ return PTR_ERR(req);
+ }
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Create a register request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_create_reg_request(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_reg_request_t io;
+ ec_slave_config_t *sc;
+ ec_reg_request_t *reg;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ io.request_index = 0;
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ sc = ec_master_get_config(master, io.config_index);
+ if (!sc) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ list_for_each_entry(reg, &sc->reg_requests, list) {
+ io.request_index++;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ reg = ecrt_slave_config_create_reg_request_err(sc, io.mem_size);
+ if (IS_ERR(reg)) {
+ return PTR_ERR(reg);
+ }
+
+ if (copy_to_user((void __user *) arg, &io, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Create a VoE handler.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_create_voe_handler(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ return -EFAULT;
+ }
+
+ data.voe_index = 0;
+
+ if (down_interruptible(&master->master_sem))
+ return -EINTR;
+
+ sc = ec_master_get_config(master, data.config_index);
+ if (!sc) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ list_for_each_entry(voe, &sc->voe_handlers, list) {
+ data.voe_index++;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ voe = ecrt_slave_config_create_voe_handler_err(sc, data.size);
+ if (IS_ERR(voe))
+ return PTR_ERR(voe);
+
+ if (copy_to_user((void __user *) arg, &data, sizeof(data)))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Get the slave configuration's state.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_state_t data;
+ const ec_slave_config_t *sc;
+ ec_slave_config_state_t state;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because sc will not be deleted in the
+ * meantime. */
+
+ if (!(sc = ec_master_get_config_const(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ ret = ecrt_slave_config_state(sc, &state);
+ if (ret)
+ return ret;
+
+ if (ec_copy_to_user((void __user *) data.state,
+ &state, sizeof(state), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Configures an IDN.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_idn(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_idn_t ioctl;
+ ec_slave_config_t *sc;
+ uint8_t *data = NULL;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl)))
+ return -EFAULT;
+
+ if (!ioctl.size)
+ return -EINVAL;
+
+ if (!(data = kmalloc(ioctl.size, GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(data, (void __user *) ioctl.data, ioctl.size)) {
+ kfree(data);
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(data);
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config(master, ioctl.config_index))) {
+ up(&master->master_sem);
+ kfree(data);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ ret = ecrt_slave_config_idn(
+ sc, ioctl.drive_no, ioctl.idn, ioctl.al_state, data, ioctl.size);
+ kfree(data);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Configures a feature flag.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_flag(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_flag_t ioctl;
+ ec_slave_config_t *sc;
+ uint8_t *key;
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
+ return -EFAULT;
+ }
+
+ if (!ioctl.key_size) {
+ return -EINVAL;
+ }
+
+ if (!(key = kmalloc(ioctl.key_size + 1, GFP_KERNEL))) {
+ return -ENOMEM;
+ }
+
+ if (copy_from_user(key, (void __user *) ioctl.key, ioctl.key_size)) {
+ kfree(key);
+ return -EFAULT;
+ }
+ key[ioctl.key_size] = '\0';
+
+ if (down_interruptible(&master->master_sem)) {
+ kfree(key);
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config(master, ioctl.config_index))) {
+ up(&master->master_sem);
+ kfree(key);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ ret = ecrt_slave_config_flag(sc, key, ioctl.value);
+ kfree(key);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Sets an AL state transition timeout.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_state_timeout(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sc_state_timeout_t ioctl;
+ ec_slave_config_t *sc;
+ int ret;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config(master, ioctl.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ ret = ecrt_slave_config_state_timeout(sc, ioctl.from_state,
+ ioctl.to_state, ioctl.timeout_ms);
+ return ret;
+}
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+
+/** Configures EoE IP parameters.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sc_ip(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_eoe_ip_t io;
+ ec_slave_config_t *sc;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ up(&master->master_sem);
+ return -ENOENT;
+ }
+
+ up(&master->master_sem); /** \todo sc could be invalidated */
+
+ /* the kernel versions of the EoE set IP methods never fail. */
+ if (io.mac_address_included) {
+ ecrt_slave_config_eoe_mac_address(sc, io.mac_address);
+ }
+ if (io.ip_address_included) {
+ ecrt_slave_config_eoe_ip_address(sc, io.ip_address);
+ }
+ if (io.subnet_mask_included) {
+ ecrt_slave_config_eoe_subnet_mask(sc, io.subnet_mask);
+ }
+ if (io.gateway_included) {
+ ecrt_slave_config_eoe_default_gateway(sc, io.gateway);
+ }
+ if (io.dns_included) {
+ ecrt_slave_config_eoe_dns_address(sc, io.dns);
+ }
+ if (io.name_included) {
+ ecrt_slave_config_eoe_hostname(sc, io.name);
+ }
+
+ return 0;
+}
+
+#endif
+
+/****************************************************************************/
+
+/** Gets the domain's data size.
+ *
+ * \return Domain size, or a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain_size(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ const ec_domain_t *domain;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ list_for_each_entry(domain, &master->domains, list) {
+ if (domain->index == (unsigned long) arg) {
+ size_t size = ecrt_domain_size(domain);
+ up(&master->master_sem);
+ return size;
+ }
+ }
+
+ up(&master->master_sem);
+ return -ENOENT;
+}
+
+/****************************************************************************/
+
+/** Gets the domain's offset in the total process data.
+ *
+ * \return Domain offset, or a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain_offset(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ int offset = 0;
+ const ec_domain_t *domain;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ list_for_each_entry(domain, &master->domains, list) {
+ if (domain->index == (unsigned long) arg) {
+ up(&master->master_sem);
+ return offset;
+ }
+ offset += ecrt_domain_size(domain);
+ }
+
+ up(&master->master_sem);
+ return -ENOENT;
+}
+
+/****************************************************************************/
+
+/** Process the domain.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain_process(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_domain_t *domain;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ /* no locking of master_sem needed, because domain will not be deleted in
+ * the meantime. */
+
+ if (!(domain = ec_master_find_domain(master, (unsigned long) arg))) {
+ return -ENOENT;
+ }
+
+ return ecrt_domain_process(domain);
+}
+
+/****************************************************************************/
+
+/** Queue the domain.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain_queue(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_domain_t *domain;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ /* no locking of master_sem needed, because domain will not be deleted in
+ * the meantime. */
+
+ if (!(domain = ec_master_find_domain(master, (unsigned long) arg))) {
+ return -ENOENT;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ ret = ecrt_domain_queue(domain);
+ ec_ioctl_unlock(&master->io_mutex);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Get the domain state.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_domain_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_domain_state_t data;
+ const ec_domain_t *domain;
+ ec_domain_state_t state;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because domain will not be deleted in
+ * the meantime. */
+
+ if (!(domain = ec_master_find_domain_const(master, data.domain_index))) {
+ return -ENOENT;
+ }
+
+ ret = ecrt_domain_state(domain, &state);
+ if (ret)
+ return ret;
+
+ if (ec_copy_to_user((void __user *) data.state, &state, sizeof(state),
+ ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Sets an SDO request's SDO index and subindex.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sdo_request_index(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sdo_request_t data;
+ ec_slave_config_t *sc;
+ ec_sdo_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_sdo_request_index(req, data.sdo_index, data.sdo_subindex);
+}
+
+/****************************************************************************/
+
+/** Sets an SDO request's timeout.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sdo_request_timeout(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sdo_request_t data;
+ ec_slave_config_t *sc;
+ ec_sdo_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_sdo_request_timeout(req, data.timeout);
+}
+
+/****************************************************************************/
+
+/** Gets an SDO request's state.
+ *
+ * Also pre-fetches the size of incoming data.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sdo_request_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sdo_request_t data;
+ ec_slave_config_t *sc;
+ ec_sdo_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ data.state = ecrt_sdo_request_state(req);
+ if (data.state == EC_REQUEST_SUCCESS && req->dir == EC_DIR_INPUT)
+ data.size = ecrt_sdo_request_data_size(req);
+ else
+ data.size = 0;
+
+ if (ec_copy_to_user((void __user *) arg, &data, sizeof(data), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Starts an SDO read operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sdo_request_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sdo_request_t data;
+ ec_slave_config_t *sc;
+ ec_sdo_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_sdo_request_read(req);
+}
+
+/****************************************************************************/
+
+/** Starts an SDO write operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sdo_request_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sdo_request_t data;
+ ec_slave_config_t *sc;
+ ec_sdo_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ if (!data.size) {
+ EC_MASTER_ERR(master, "SDO download: Data size may not be zero!\n");
+ return -EINVAL;
+ }
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ if (data.size > req->mem_size)
+ return -ENOBUFS;
+
+ if (ec_copy_from_user(req->data, (void __user *) data.data,
+ data.size, ctx))
+ return -EFAULT;
+
+ req->data_size = data.size;
+ return ecrt_sdo_request_write(req);
+}
+
+/****************************************************************************/
+
+/** Read SDO data.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_sdo_request_data(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_sdo_request_t data;
+ ec_slave_config_t *sc;
+ ec_sdo_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_sdo_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ if (ec_copy_to_user((void __user *) data.data, ecrt_sdo_request_data(req),
+ ecrt_sdo_request_data_size(req), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Sets an SoE request's drive number and IDN.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_soe_request_index(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_soe_request_t data;
+ ec_slave_config_t *sc;
+ ec_soe_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_soe_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_soe_request_idn(req, data.drive_no, data.idn);
+}
+
+/****************************************************************************/
+
+/** Sets an CoE request's timeout.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_soe_request_timeout(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_soe_request_t data;
+ ec_slave_config_t *sc;
+ ec_soe_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_soe_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_soe_request_timeout(req, data.timeout);
+}
+
+/****************************************************************************/
+
+/** Gets an SoE request's state.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_soe_request_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_soe_request_t data;
+ ec_slave_config_t *sc;
+ ec_soe_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_soe_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ data.state = ecrt_soe_request_state(req);
+ if (data.state == EC_REQUEST_SUCCESS && req->dir == EC_DIR_INPUT) {
+ data.size = ecrt_soe_request_data_size(req);
+ }
+ else {
+ data.size = 0;
+ }
+
+ if (ec_copy_to_user((void __user *) arg, &data, sizeof(data), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Starts an SoE IDN read operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_soe_request_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_soe_request_t data;
+ ec_slave_config_t *sc;
+ ec_soe_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_soe_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_soe_request_read(req);
+}
+
+/****************************************************************************/
+
+/** Starts an SoE IDN write operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_soe_request_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_soe_request_t data;
+ ec_slave_config_t *sc;
+ ec_soe_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ if (!data.size) {
+ EC_MASTER_ERR(master, "IDN write: Data size may not be zero!\n");
+ return -EINVAL;
+ }
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_soe_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ if (data.size > req->mem_size)
+ return -ENOBUFS;
+
+ if (ec_copy_from_user(req->data, (void __user *) data.data,
+ data.size, ctx))
+ return -EFAULT;
+
+ req->data_size = data.size;
+ return ecrt_soe_request_write(req);
+}
+
+/****************************************************************************/
+
+/** Read SoE IDN data.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_soe_request_data(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_soe_request_t data;
+ ec_slave_config_t *sc;
+ ec_soe_request_t *req;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor req will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(req = ec_slave_config_find_soe_request(sc, data.request_index))) {
+ return -ENOENT;
+ }
+
+ if (ec_copy_to_user((void __user *) data.data, ecrt_soe_request_data(req),
+ ecrt_soe_request_data_size(req), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Read register data.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_reg_request_data(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_reg_request_t io;
+ ec_slave_config_t *sc;
+ ec_reg_request_t *reg;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_copy_from_user(&io, (void __user *) arg, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ if (io.mem_size <= 0) {
+ return 0;
+ }
+
+ /* no locking of master_sem needed, because neither sc nor reg will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) {
+ return -ENOENT;
+ }
+
+ if (ec_copy_to_user((void __user *) io.data, ecrt_reg_request_data(reg),
+ min(reg->mem_size, io.mem_size), ctx)) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Gets an register request's state.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_reg_request_state(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_reg_request_t io;
+ ec_slave_config_t *sc;
+ ec_reg_request_t *reg;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_copy_from_user(&io, (void __user *) arg, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because neither sc nor reg will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) {
+ return -ENOENT;
+ }
+
+ io.state = ecrt_reg_request_state(reg);
+ io.new_data = io.state == EC_REQUEST_SUCCESS && reg->dir == EC_DIR_INPUT;
+
+ if (ec_copy_to_user((void __user *) arg, &io, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Starts an register write operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_reg_request_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_reg_request_t io;
+ ec_slave_config_t *sc;
+ ec_reg_request_t *reg;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_copy_from_user(&io, (void __user *) arg, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because neither sc nor reg will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) {
+ return -ENOENT;
+ }
+
+ if (io.transfer_size > reg->mem_size) {
+ return -ENOBUFS;
+ }
+
+ if (ec_copy_from_user(reg->data, (void __user *) io.data,
+ io.transfer_size, ctx)) {
+ return -EFAULT;
+ }
+
+ return ecrt_reg_request_write(reg, io.address, io.transfer_size);
+}
+
+/****************************************************************************/
+
+/** Starts an register read operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_reg_request_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_reg_request_t io;
+ ec_slave_config_t *sc;
+ ec_reg_request_t *reg;
+
+ if (unlikely(!ctx->requested)) {
+ return -EPERM;
+ }
+
+ if (ec_copy_from_user(&io, (void __user *) arg, sizeof(io), ctx)) {
+ return -EFAULT;
+ }
+
+ /* no locking of master_sem needed, because neither sc nor reg will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, io.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(reg = ec_slave_config_find_reg_request(sc, io.request_index))) {
+ return -ENOENT;
+ }
+
+ if (io.transfer_size > reg->mem_size) {
+ return -ENOBUFS;
+ }
+
+ return ecrt_reg_request_read(reg, io.address, io.transfer_size);
+}
+
+/****************************************************************************/
+
+/** Sets the VoE send header.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_voe_send_header(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+ uint32_t vendor_id;
+ uint16_t vendor_type;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ if (ec_copy_from_user(&vendor_id, data.vendor_id, sizeof(vendor_id), ctx))
+ return -EFAULT;
+
+ if (ec_copy_from_user(&vendor_type, data.vendor_type,
+ sizeof(vendor_type), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor voe will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_voe_handler_send_header(voe, vendor_id, vendor_type);
+}
+
+/****************************************************************************/
+
+/** Gets the received VoE header.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_voe_rec_header(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+ uint32_t vendor_id;
+ uint16_t vendor_type;
+ int ret;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor voe will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
+ return -ENOENT;
+ }
+
+ ret = ecrt_voe_handler_received_header(voe, &vendor_id, &vendor_type);
+ if (ret)
+ return ret;
+
+ if (likely(data.vendor_id))
+ if (ec_copy_to_user(data.vendor_id, &vendor_id,
+ sizeof(vendor_id), ctx))
+ return -EFAULT;
+
+ if (likely(data.vendor_type))
+ if (ec_copy_to_user(data.vendor_type, &vendor_type,
+ sizeof(vendor_type), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Starts a VoE read operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_voe_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor voe will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_voe_handler_read(voe);
+}
+
+/****************************************************************************/
+
+/** Starts a VoE read operation without sending a sync message first.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_voe_read_nosync(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor voe will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
+ return -ENOENT;
+ }
+
+ return ecrt_voe_handler_read_nosync(voe);
+}
+
+/****************************************************************************/
+
+/** Starts a VoE write operation.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_voe_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor voe will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
+ return -ENOENT;
+ }
+
+ if (data.size) {
+ if (data.size > ec_voe_handler_mem_size(voe))
+ return -ENOBUFS;
+
+ if (ec_copy_from_user(ecrt_voe_handler_data(voe),
+ (void __user *) data.data, data.size, ctx))
+ return -EFAULT;
+ }
+
+ return ecrt_voe_handler_write(voe, data.size);
+}
+
+/****************************************************************************/
+
+/** Executes the VoE state machine.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_voe_exec(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor voe will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
+ return -ENOENT;
+ }
+
+ if (ec_ioctl_lock_interruptible(&master->io_mutex))
+ return -EINTR;
+
+ data.state = ecrt_voe_handler_execute(voe);
+ ec_ioctl_unlock(&master->io_mutex);
+ if (data.state == EC_REQUEST_SUCCESS && voe->dir == EC_DIR_INPUT)
+ data.size = ecrt_voe_handler_data_size(voe);
+ else
+ data.size = 0;
+
+ if (ec_copy_to_user((void __user *) arg, &data, sizeof(data), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Reads the received VoE data.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_voe_data(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg, /**< ioctl() argument. */
+ ec_ioctl_context_t *ctx /**< Private data structure of file handle. */
+ )
+{
+ ec_ioctl_voe_t data;
+ ec_slave_config_t *sc;
+ ec_voe_handler_t *voe;
+
+ if (unlikely(!ctx->requested))
+ return -EPERM;
+
+ if (ec_copy_from_user(&data, (void __user *) arg, sizeof(data), ctx))
+ return -EFAULT;
+
+ /* no locking of master_sem needed, because neither sc nor voe will not be
+ * deleted in the meantime. */
+
+ if (!(sc = ec_master_get_config(master, data.config_index))) {
+ return -ENOENT;
+ }
+
+ if (!(voe = ec_slave_config_find_voe_handler(sc, data.voe_index))) {
+ return -ENOENT;
+ }
+
+ if (ec_copy_to_user((void __user *) data.data, ecrt_voe_handler_data(voe),
+ ecrt_voe_handler_data_size(voe), ctx))
+ return -EFAULT;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Read a file from a slave via FoE.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_foe_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_foe_t io;
+ ec_foe_request_t request;
+ ec_slave_t *slave;
+ int ret;
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ ec_foe_request_init(&request, io.file_name);
+ ret = ec_foe_request_alloc(&request, 10000); // FIXME
+ if (ret) {
+ ec_foe_request_clear(&request);
+ return ret;
+ }
+
+ ec_foe_request_read(&request);
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_foe_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) {
+ up(&master->master_sem);
+ ec_foe_request_clear(&request);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ io.slave_position);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Scheduling FoE read request.\n");
+
+ // schedule request.
+ list_add_tail(&request.list, &slave->foe_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_foe_request_clear(&request);
+ return -EINTR;
+ }
+ // request already processing: interrupt not possible.
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ io.result = request.result;
+ io.error_code = request.error_code;
+
+ if (request.state != EC_INT_REQUEST_SUCCESS) {
+ io.data_size = 0;
+ ret = -EIO;
+ } else {
+ if (request.data_size > io.buffer_size) {
+ EC_SLAVE_ERR(slave, "%s(): Buffer too small.\n", __func__);
+ ec_foe_request_clear(&request);
+ return -ENOBUFS;
+ }
+ io.data_size = request.data_size;
+ if (copy_to_user((void __user *) io.buffer,
+ request.buffer, io.data_size)) {
+ ec_foe_request_clear(&request);
+ return -EFAULT;
+ }
+ ret = 0;
+ }
+
+ if (__copy_to_user((void __user *) arg, &io, sizeof(io))) {
+ ret = -EFAULT;
+ }
+
+ ec_foe_request_clear(&request);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Write a file to a slave via FoE
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_foe_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_foe_t io;
+ ec_foe_request_t request;
+ ec_slave_t *slave;
+ int ret;
+
+ if (copy_from_user(&io, (void __user *) arg, sizeof(io))) {
+ return -EFAULT;
+ }
+
+ ec_foe_request_init(&request, io.file_name);
+
+ ret = ec_foe_request_alloc(&request, io.buffer_size);
+ if (ret) {
+ ec_foe_request_clear(&request);
+ return ret;
+ }
+
+ if (copy_from_user(request.buffer,
+ (void __user *) io.buffer, io.buffer_size)) {
+ ec_foe_request_clear(&request);
+ return -EFAULT;
+ }
+
+ request.data_size = io.buffer_size;
+ ec_foe_request_write(&request);
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_foe_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ io.slave_position);
+ ec_foe_request_clear(&request);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Scheduling FoE write request.\n");
+
+ // schedule FoE write request.
+ list_add_tail(&request.list, &slave->foe_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ // abort request
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_foe_request_clear(&request);
+ return -EINTR;
+ }
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ io.result = request.result;
+ io.error_code = request.error_code;
+
+ ret = request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+
+ if (__copy_to_user((void __user *) arg, &io, sizeof(io))) {
+ ret = -EFAULT;
+ }
+
+ ec_foe_request_clear(&request);
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Read an SoE IDN.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_soe_read(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_soe_read_t ioctl;
+ u8 *data;
+ int retval;
+
+ if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
+ return -EFAULT;
+ }
+
+ data = kmalloc(ioctl.mem_size, GFP_KERNEL);
+ if (!data) {
+ EC_MASTER_ERR(master, "Failed to allocate %zu bytes of IDN data.\n",
+ ioctl.mem_size);
+ return -ENOMEM;
+ }
+
+ retval = ecrt_master_read_idn(master, ioctl.slave_position,
+ ioctl.drive_no, ioctl.idn, data, ioctl.mem_size, &ioctl.data_size,
+ &ioctl.error_code);
+ if (retval) {
+ kfree(data);
+ return retval;
+ }
+
+ if (copy_to_user((void __user *) ioctl.data,
+ data, ioctl.data_size)) {
+ kfree(data);
+ return -EFAULT;
+ }
+ kfree(data);
+
+ if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
+ retval = -EFAULT;
+ }
+
+ EC_MASTER_DBG(master, 1, "Finished SoE read request.\n");
+ return retval;
+}
+
+/****************************************************************************/
+
+/** Write an IDN to a slave via SoE.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+static ATTRIBUTES int ec_ioctl_slave_soe_write(
+ ec_master_t *master, /**< EtherCAT master. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ ec_ioctl_slave_soe_write_t ioctl;
+ u8 *data;
+ int retval;
+
+ if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
+ return -EFAULT;
+ }
+
+ data = kmalloc(ioctl.data_size, GFP_KERNEL);
+ if (!data) {
+ EC_MASTER_ERR(master, "Failed to allocate %zu bytes of IDN data.\n",
+ ioctl.data_size);
+ return -ENOMEM;
+ }
+ if (copy_from_user(data, (void __user *) ioctl.data, ioctl.data_size)) {
+ kfree(data);
+ return -EFAULT;
+ }
+
+ retval = ecrt_master_write_idn(master, ioctl.slave_position,
+ ioctl.drive_no, ioctl.idn, data, ioctl.data_size,
+ &ioctl.error_code);
+ kfree(data);
+ if (retval) {
+ return retval;
+ }
+
+ if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
+ retval = -EFAULT;
+ }
+
+ EC_MASTER_DBG(master, 1, "Finished SoE write request.\n");
+ return retval;
+}
+
+/*****************************************************************************
+ * ioctl() file operation functions
+ ****************************************************************************/
+
+/** ioctl() function to use.
+ *
+ * For RTDM, there will be ec_ioctl_rtdm_rt and ec_ioctl_rtdm_nrt.
+ * For "normal" cdev, there will be ec_ioctl_rt only.
+ */
+#ifndef EC_IOCTL_RTDM
+static long ec_ioctl_nrt(
+ ec_master_t *master, /**< EtherCAT master. */
+ ec_ioctl_context_t *ctx, /**< Device context. */
+ unsigned int cmd, /**< ioctl() command identifier. */
+ void *arg /**< ioctl() argument. */);
+#endif
+
+/****************************************************************************/
+
+/** Called when an ioctl() command is issued.
+ * Both RT and nRT context.
+ *
+ * \return ioctl() return code.
+ */
+static long ec_ioctl_both(
+ ec_master_t *master, /**< EtherCAT master. */
+ ec_ioctl_context_t *ctx, /**< Device context. */
+ unsigned int cmd, /**< ioctl() command identifier. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+ int ret;
+
+ switch (cmd) {
+ case EC_IOCTL_MODULE:
+ ret = ec_ioctl_module(arg, ctx);
+ break;
+ case EC_IOCTL_MASTER_RESCAN:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_master_rescan(master, arg);
+ break;
+ case EC_IOCTL_MASTER_STATE:
+ ret = ec_ioctl_master_state(master, arg, ctx);
+ break;
+ case EC_IOCTL_MASTER_LINK_STATE:
+ ret = ec_ioctl_master_link_state(master, arg, ctx);
+ break;
+ case EC_IOCTL_SDO_REQUEST_TIMEOUT:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sdo_request_timeout(master, arg, ctx);
+ break;
+ case EC_IOCTL_SDO_REQUEST_DATA:
+ ret = ec_ioctl_sdo_request_data(master, arg, ctx);
+ break;
+ case EC_IOCTL_SOE_REQUEST_TIMEOUT:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_soe_request_timeout(master, arg, ctx);
+ break;
+ case EC_IOCTL_SOE_REQUEST_DATA:
+ ret = ec_ioctl_soe_request_data(master, arg, ctx);
+ break;
+ case EC_IOCTL_REG_REQUEST_DATA:
+ ret = ec_ioctl_reg_request_data(master, arg, ctx);
+ break;
+ case EC_IOCTL_VOE_SEND_HEADER:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_voe_send_header(master, arg, ctx);
+ break;
+ case EC_IOCTL_VOE_DATA:
+ ret = ec_ioctl_voe_data(master, arg, ctx);
+ break;
+ default:
+#ifdef EC_IOCTL_RTDM
+ ret = -ENOTTY;
+#else
+ /* chain non-rt commands for normal cdev */
+ ret = ec_ioctl_nrt(master, ctx, cmd, arg);
+#endif
+ break;
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Called when an ioctl() command is issued.
+ * RTDM: RT only.
+ *
+ * \return ioctl() return code.
+ */
+#ifdef EC_IOCTL_RTDM
+long ec_ioctl_rtdm_rt
+#else
+long ec_ioctl
+#endif
+ (
+ ec_master_t *master, /**< EtherCAT master. */
+ ec_ioctl_context_t *ctx, /**< Device context. */
+ unsigned int cmd, /**< ioctl() command identifier. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+#if DEBUG_LATENCY
+ cycles_t a = get_cycles(), b;
+ unsigned int t;
+#endif
+ long ret;
+
+ switch (cmd) {
+ case EC_IOCTL_SEND:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_send(master, arg, ctx);
+ break;
+ case EC_IOCTL_RECEIVE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_receive(master, arg, ctx);
+ break;
+ case EC_IOCTL_APP_TIME:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_app_time(master, arg, ctx);
+ break;
+ case EC_IOCTL_SYNC_REF:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sync_ref(master, arg, ctx);
+ break;
+ case EC_IOCTL_SYNC_REF_TO:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sync_ref_to(master, arg, ctx);
+ break;
+ case EC_IOCTL_SYNC_SLAVES:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sync_slaves(master, arg, ctx);
+ break;
+ case EC_IOCTL_REF_CLOCK_TIME:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_ref_clock_time(master, arg, ctx);
+ break;
+ case EC_IOCTL_SYNC_MON_QUEUE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sync_mon_queue(master, arg, ctx);
+ break;
+ case EC_IOCTL_SYNC_MON_PROCESS:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sync_mon_process(master, arg, ctx);
+ break;
+ case EC_IOCTL_RESET:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_reset(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_EMERG_POP:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_emerg_pop(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_EMERG_CLEAR:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_emerg_clear(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_EMERG_OVERRUNS:
+ ret = ec_ioctl_sc_emerg_overruns(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_STATE:
+ ret = ec_ioctl_sc_state(master, arg, ctx);
+ break;
+ case EC_IOCTL_DOMAIN_PROCESS:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_domain_process(master, arg, ctx);
+ break;
+ case EC_IOCTL_DOMAIN_QUEUE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_domain_queue(master, arg, ctx);
+ break;
+ case EC_IOCTL_DOMAIN_STATE:
+ ret = ec_ioctl_domain_state(master, arg, ctx);
+ break;
+ case EC_IOCTL_SDO_REQUEST_INDEX:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sdo_request_index(master, arg, ctx);
+ break;
+ case EC_IOCTL_SDO_REQUEST_STATE:
+ ret = ec_ioctl_sdo_request_state(master, arg, ctx);
+ break;
+ case EC_IOCTL_SDO_REQUEST_READ:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sdo_request_read(master, arg, ctx);
+ break;
+ case EC_IOCTL_SDO_REQUEST_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sdo_request_write(master, arg, ctx);
+ break;
+ case EC_IOCTL_SOE_REQUEST_IDN:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_soe_request_index(master, arg, ctx);
+ break;
+ case EC_IOCTL_SOE_REQUEST_STATE:
+ ret = ec_ioctl_soe_request_state(master, arg, ctx);
+ break;
+ case EC_IOCTL_SOE_REQUEST_READ:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_soe_request_read(master, arg, ctx);
+ break;
+ case EC_IOCTL_SOE_REQUEST_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_soe_request_write(master, arg, ctx);
+ break;
+ case EC_IOCTL_REG_REQUEST_STATE:
+ ret = ec_ioctl_reg_request_state(master, arg, ctx);
+ break;
+ case EC_IOCTL_REG_REQUEST_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_reg_request_write(master, arg, ctx);
+ break;
+ case EC_IOCTL_REG_REQUEST_READ:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_reg_request_read(master, arg, ctx);
+ break;
+ case EC_IOCTL_VOE_REC_HEADER:
+ ret = ec_ioctl_voe_rec_header(master, arg, ctx);
+ break;
+ case EC_IOCTL_VOE_READ:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_voe_read(master, arg, ctx);
+ break;
+ case EC_IOCTL_VOE_READ_NOSYNC:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_voe_read_nosync(master, arg, ctx);
+ break;
+ case EC_IOCTL_VOE_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_voe_write(master, arg, ctx);
+ break;
+ case EC_IOCTL_VOE_EXEC:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_voe_exec(master, arg, ctx);
+ break;
+ default:
+ ret = ec_ioctl_both(master, ctx, cmd, arg);
+ break;
+ }
+
+#if DEBUG_LATENCY
+ b = get_cycles();
+ t = (unsigned int) ((b - a) * 1000LL) / cpu_khz;
+ if (t > 50) {
+ EC_MASTER_WARN(master, "ioctl(0x%02x) took %u us.\n",
+ _IOC_NR(cmd), t);
+ }
+#endif
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Called when an ioctl() command is issued.
+ * nRT context only.
+ *
+ * \return ioctl() return code.
+ */
+#ifdef EC_IOCTL_RTDM
+long ec_ioctl_rtdm_nrt
+#else
+static long ec_ioctl_nrt
+#endif
+ (
+ ec_master_t *master, /**< EtherCAT master. */
+ ec_ioctl_context_t *ctx, /**< Device context. */
+ unsigned int cmd, /**< ioctl() command identifier. */
+ void *arg /**< ioctl() argument. */
+ )
+{
+#if DEBUG_LATENCY && !defined(EC_IOCTL_RTDM)
+ cycles_t a = get_cycles(), b;
+ unsigned int t;
+#endif
+ int ret;
+
+ switch (cmd) {
+ case EC_IOCTL_MASTER:
+ ret = ec_ioctl_master(master, arg);
+ break;
+ case EC_IOCTL_SLAVE:
+ ret = ec_ioctl_slave(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SYNC:
+ ret = ec_ioctl_slave_sync(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SYNC_PDO:
+ ret = ec_ioctl_slave_sync_pdo(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SYNC_PDO_ENTRY:
+ ret = ec_ioctl_slave_sync_pdo_entry(master, arg);
+ break;
+ case EC_IOCTL_DOMAIN:
+ ret = ec_ioctl_domain(master, arg);
+ break;
+ case EC_IOCTL_DOMAIN_FMMU:
+ ret = ec_ioctl_domain_fmmu(master, arg);
+ break;
+ case EC_IOCTL_DOMAIN_DATA:
+ ret = ec_ioctl_domain_data(master, arg);
+ break;
+ case EC_IOCTL_MASTER_DEBUG:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_master_debug(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_STATE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_state(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SDO:
+ ret = ec_ioctl_slave_sdo(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SDO_ENTRY:
+ ret = ec_ioctl_slave_sdo_entry(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SDO_UPLOAD:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_sdo_upload(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SDO_DOWNLOAD:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_sdo_download(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SII_READ:
+ ret = ec_ioctl_slave_sii_read(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SII_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_sii_write(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_REG_READ:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_reg_read(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_REG_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_reg_write(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_FOE_READ:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_foe_read(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_FOE_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_foe_write(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SOE_READ:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_soe_read(master, arg);
+ break;
+ case EC_IOCTL_SLAVE_SOE_WRITE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_soe_write(master, arg);
+ break;
+#ifdef EC_EOE
+ case EC_IOCTL_SLAVE_EOE_IP_PARAM:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_slave_eoe_ip_param(master, arg);
+ break;
+#endif
+ case EC_IOCTL_CONFIG:
+ ret = ec_ioctl_config(master, arg);
+ break;
+ case EC_IOCTL_CONFIG_PDO:
+ ret = ec_ioctl_config_pdo(master, arg);
+ break;
+ case EC_IOCTL_CONFIG_PDO_ENTRY:
+ ret = ec_ioctl_config_pdo_entry(master, arg);
+ break;
+ case EC_IOCTL_CONFIG_SDO:
+ ret = ec_ioctl_config_sdo(master, arg);
+ break;
+ case EC_IOCTL_CONFIG_IDN:
+ ret = ec_ioctl_config_idn(master, arg);
+ break;
+ case EC_IOCTL_CONFIG_FLAG:
+ ret = ec_ioctl_config_flag(master, arg);
+ break;
+#ifdef EC_EOE
+ case EC_IOCTL_CONFIG_EOE_IP_PARAM:
+ ret = ec_ioctl_config_ip(master, arg);
+ break;
+ case EC_IOCTL_EOE_HANDLER:
+ ret = ec_ioctl_eoe_handler(master, arg);
+ break;
+#endif
+
+ /* Application interface */
+
+ case EC_IOCTL_REQUEST:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_request(master, arg, ctx);
+ break;
+ case EC_IOCTL_CREATE_DOMAIN:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_create_domain(master, arg, ctx);
+ break;
+ case EC_IOCTL_CREATE_SLAVE_CONFIG:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_create_slave_config(master, arg, ctx);
+ break;
+ case EC_IOCTL_SELECT_REF_CLOCK:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_select_ref_clock(master, arg, ctx);
+ break;
+ case EC_IOCTL_ACTIVATE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_activate(master, arg, ctx);
+ break;
+ case EC_IOCTL_DEACTIVATE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_deactivate(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_SYNC:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_sync(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_WATCHDOG:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_watchdog(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_ADD_PDO:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_add_pdo(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_CLEAR_PDOS:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_clear_pdos(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_ADD_ENTRY:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_add_entry(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_CLEAR_ENTRIES:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_clear_entries(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_REG_PDO_ENTRY:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_reg_pdo_entry(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_REG_PDO_POS:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_reg_pdo_pos(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_DC:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_dc(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_SDO:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_sdo(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_EMERG_SIZE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_emerg_size(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_SDO_REQUEST:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_create_sdo_request(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_SOE_REQUEST:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_create_soe_request(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_REG_REQUEST:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_create_reg_request(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_VOE:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_create_voe_handler(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_IDN:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_idn(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_FLAG:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_flag(master, arg, ctx);
+ break;
+ case EC_IOCTL_SC_STATE_TIMEOUT:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_state_timeout(master, arg, ctx);
+ break;
+#ifdef EC_EOE
+ case EC_IOCTL_SC_EOE_IP_PARAM:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_sc_ip(master, arg, ctx);
+ break;
+#endif
+ case EC_IOCTL_DOMAIN_SIZE:
+ ret = ec_ioctl_domain_size(master, arg, ctx);
+ break;
+ case EC_IOCTL_DOMAIN_OFFSET:
+ ret = ec_ioctl_domain_offset(master, arg, ctx);
+ break;
+ case EC_IOCTL_SET_SEND_INTERVAL:
+ if (!ctx->writable) {
+ ret = -EPERM;
+ break;
+ }
+ ret = ec_ioctl_set_send_interval(master, arg, ctx);
+ break;
+ default:
+#ifdef EC_IOCTL_RTDM
+ ret = ec_ioctl_both(master, ctx, cmd, arg);
+#else
+ ret = -ENOTTY;
+#endif
+ break;
+ }
+
+#if DEBUG_LATENCY && !defined(EC_IOCTL_RTDM)
+ b = get_cycles();
+ t = (unsigned int) ((b - a) * 1000LL) / cpu_khz;
+ if (t > 50) {
+ EC_MASTER_WARN(master, "ioctl(0x%02x) took %u us.\n",
+ _IOC_NR(cmd), t);
+ }
+#endif
+
+ return ret;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/ioctl.h b/ubuntu/igh-ecat/master/ioctl.h
new file mode 100644
index 00000000000000..0c51e6380f4365
--- /dev/null
+++ b/ubuntu/igh-ecat/master/ioctl.h
@@ -0,0 +1,879 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT master.
+ *
+ * The file is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU Lesser General Public License as published by the
+ * Free Software Foundation; version 2.1 of the License.
+ *
+ * This file is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this file. If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT master character device IOCTL commands.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_IOCTL_H__
+#define __EC_IOCTL_H__
+
+#include <linux/ioctl.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** \cond */
+
+#define EC_IOCTL_TYPE 0xa4
+
+#define EC_IO(nr) _IO(EC_IOCTL_TYPE, nr)
+#define EC_IOR(nr, type) _IOR(EC_IOCTL_TYPE, nr, type)
+#define EC_IOW(nr, type) _IOW(EC_IOCTL_TYPE, nr, type)
+#define EC_IOWR(nr, type) _IOWR(EC_IOCTL_TYPE, nr, type)
+
+/** EtherCAT master ioctl() version magic.
+ *
+ * Increment this when changing the ioctl interface!
+ */
+#define EC_IOCTL_VERSION_MAGIC 37
+
+// Command-line tool
+#define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t)
+#define EC_IOCTL_MASTER EC_IOR(0x01, ec_ioctl_master_t)
+#define EC_IOCTL_SLAVE EC_IOWR(0x02, ec_ioctl_slave_t)
+#define EC_IOCTL_SLAVE_SYNC EC_IOWR(0x03, ec_ioctl_slave_sync_t)
+#define EC_IOCTL_SLAVE_SYNC_PDO EC_IOWR(0x04, ec_ioctl_slave_sync_pdo_t)
+#define EC_IOCTL_SLAVE_SYNC_PDO_ENTRY EC_IOWR(0x05, ec_ioctl_slave_sync_pdo_entry_t)
+#define EC_IOCTL_DOMAIN EC_IOWR(0x06, ec_ioctl_domain_t)
+#define EC_IOCTL_DOMAIN_FMMU EC_IOWR(0x07, ec_ioctl_domain_fmmu_t)
+#define EC_IOCTL_DOMAIN_DATA EC_IOWR(0x08, ec_ioctl_domain_data_t)
+#define EC_IOCTL_MASTER_DEBUG EC_IO(0x09)
+#define EC_IOCTL_MASTER_RESCAN EC_IO(0x0a)
+#define EC_IOCTL_SLAVE_STATE EC_IOW(0x0b, ec_ioctl_slave_state_t)
+#define EC_IOCTL_SLAVE_SDO EC_IOWR(0x0c, ec_ioctl_slave_sdo_t)
+#define EC_IOCTL_SLAVE_SDO_ENTRY EC_IOWR(0x0d, ec_ioctl_slave_sdo_entry_t)
+#define EC_IOCTL_SLAVE_SDO_UPLOAD EC_IOWR(0x0e, ec_ioctl_slave_sdo_upload_t)
+#define EC_IOCTL_SLAVE_SDO_DOWNLOAD EC_IOWR(0x0f, ec_ioctl_slave_sdo_download_t)
+#define EC_IOCTL_SLAVE_SII_READ EC_IOWR(0x10, ec_ioctl_slave_sii_t)
+#define EC_IOCTL_SLAVE_SII_WRITE EC_IOW(0x11, ec_ioctl_slave_sii_t)
+#define EC_IOCTL_SLAVE_REG_READ EC_IOWR(0x12, ec_ioctl_slave_reg_t)
+#define EC_IOCTL_SLAVE_REG_WRITE EC_IOW(0x13, ec_ioctl_slave_reg_t)
+#define EC_IOCTL_SLAVE_FOE_READ EC_IOWR(0x14, ec_ioctl_slave_foe_t)
+#define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x15, ec_ioctl_slave_foe_t)
+#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x16, ec_ioctl_slave_soe_read_t)
+#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x17, ec_ioctl_slave_soe_write_t)
+#ifdef EC_EOE
+#define EC_IOCTL_SLAVE_EOE_IP_PARAM EC_IOW(0x18, ec_ioctl_eoe_ip_t)
+#endif
+#define EC_IOCTL_CONFIG EC_IOWR(0x19, ec_ioctl_config_t)
+#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x1a, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x1b, ec_ioctl_config_pdo_entry_t)
+#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x1c, ec_ioctl_config_sdo_t)
+#define EC_IOCTL_CONFIG_IDN EC_IOWR(0x1d, ec_ioctl_config_idn_t)
+#define EC_IOCTL_CONFIG_FLAG EC_IOWR(0x1e, ec_ioctl_config_flag_t)
+#ifdef EC_EOE
+#define EC_IOCTL_CONFIG_EOE_IP_PARAM EC_IOWR(0x1f, ec_ioctl_eoe_ip_t)
+#define EC_IOCTL_EOE_HANDLER EC_IOWR(0x20, ec_ioctl_eoe_handler_t)
+#endif
+
+// Application interface
+#define EC_IOCTL_REQUEST EC_IO(0x21)
+#define EC_IOCTL_CREATE_DOMAIN EC_IO(0x22)
+#define EC_IOCTL_CREATE_SLAVE_CONFIG EC_IOWR(0x23, ec_ioctl_config_t)
+#define EC_IOCTL_SELECT_REF_CLOCK EC_IOW(0x24, uint32_t)
+#define EC_IOCTL_ACTIVATE EC_IOR(0x25, ec_ioctl_master_activate_t)
+#define EC_IOCTL_DEACTIVATE EC_IO(0x26)
+#define EC_IOCTL_SEND EC_IO(0x27)
+#define EC_IOCTL_RECEIVE EC_IO(0x28)
+#define EC_IOCTL_MASTER_STATE EC_IOR(0x29, ec_master_state_t)
+#define EC_IOCTL_MASTER_LINK_STATE EC_IOWR(0x2a, ec_ioctl_link_state_t)
+#define EC_IOCTL_APP_TIME EC_IOW(0x2b, uint64_t)
+#define EC_IOCTL_SYNC_REF EC_IO(0x2c)
+#define EC_IOCTL_SYNC_REF_TO EC_IOW(0x2d, uint64_t)
+#define EC_IOCTL_SYNC_SLAVES EC_IO(0x2e)
+#define EC_IOCTL_REF_CLOCK_TIME EC_IOR(0x2f, uint32_t)
+#define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x30)
+#define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x31, uint32_t)
+#define EC_IOCTL_RESET EC_IO(0x32)
+#define EC_IOCTL_SC_SYNC EC_IOW(0x33, ec_ioctl_config_t)
+#define EC_IOCTL_SC_WATCHDOG EC_IOW(0x34, ec_ioctl_config_t)
+#define EC_IOCTL_SC_ADD_PDO EC_IOW(0x35, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_SC_CLEAR_PDOS EC_IOW(0x36, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_SC_ADD_ENTRY EC_IOW(0x37, ec_ioctl_add_pdo_entry_t)
+#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x38, ec_ioctl_config_pdo_t)
+#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x39, ec_ioctl_reg_pdo_entry_t)
+#define EC_IOCTL_SC_REG_PDO_POS EC_IOWR(0x3a, ec_ioctl_reg_pdo_pos_t)
+#define EC_IOCTL_SC_DC EC_IOW(0x3b, ec_ioctl_config_t)
+#define EC_IOCTL_SC_SDO EC_IOW(0x3c, ec_ioctl_sc_sdo_t)
+#define EC_IOCTL_SC_EMERG_SIZE EC_IOW(0x3d, ec_ioctl_sc_emerg_t)
+#define EC_IOCTL_SC_EMERG_POP EC_IOWR(0x3e, ec_ioctl_sc_emerg_t)
+#define EC_IOCTL_SC_EMERG_CLEAR EC_IOW(0x3f, ec_ioctl_sc_emerg_t)
+#define EC_IOCTL_SC_EMERG_OVERRUNS EC_IOWR(0x40, ec_ioctl_sc_emerg_t)
+#define EC_IOCTL_SC_SDO_REQUEST EC_IOWR(0x41, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SC_SOE_REQUEST EC_IOWR(0x42, ec_ioctl_soe_request_t)
+#define EC_IOCTL_SC_REG_REQUEST EC_IOWR(0x43, ec_ioctl_reg_request_t)
+#define EC_IOCTL_SC_VOE EC_IOWR(0x44, ec_ioctl_voe_t)
+#define EC_IOCTL_SC_STATE EC_IOWR(0x45, ec_ioctl_sc_state_t)
+#define EC_IOCTL_SC_IDN EC_IOW(0x46, ec_ioctl_sc_idn_t)
+#define EC_IOCTL_SC_FLAG EC_IOW(0x47, ec_ioctl_sc_flag_t)
+#ifdef EC_EOE
+#define EC_IOCTL_SC_EOE_IP_PARAM EC_IOW(0x48, ec_ioctl_eoe_ip_t)
+#endif
+#define EC_IOCTL_SC_STATE_TIMEOUT EC_IOW(0x49, ec_ioctl_sc_state_timeout_t)
+#define EC_IOCTL_DOMAIN_SIZE EC_IO(0x4a)
+#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x4b)
+#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x4c)
+#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x4d)
+#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x4e, ec_ioctl_domain_state_t)
+#define EC_IOCTL_SDO_REQUEST_INDEX EC_IOWR(0x4f, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_TIMEOUT EC_IOWR(0x50, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_STATE EC_IOWR(0x51, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_READ EC_IOWR(0x52, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_WRITE EC_IOWR(0x53, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SDO_REQUEST_DATA EC_IOWR(0x54, ec_ioctl_sdo_request_t)
+#define EC_IOCTL_SOE_REQUEST_IDN EC_IOWR(0x55, ec_ioctl_soe_request_t)
+#define EC_IOCTL_SOE_REQUEST_TIMEOUT EC_IOWR(0x56, ec_ioctl_soe_request_t)
+#define EC_IOCTL_SOE_REQUEST_STATE EC_IOWR(0x57, ec_ioctl_soe_request_t)
+#define EC_IOCTL_SOE_REQUEST_READ EC_IOWR(0x58, ec_ioctl_soe_request_t)
+#define EC_IOCTL_SOE_REQUEST_WRITE EC_IOWR(0x59, ec_ioctl_soe_request_t)
+#define EC_IOCTL_SOE_REQUEST_DATA EC_IOWR(0x5a, ec_ioctl_soe_request_t)
+#define EC_IOCTL_REG_REQUEST_DATA EC_IOWR(0x5b, ec_ioctl_reg_request_t)
+#define EC_IOCTL_REG_REQUEST_STATE EC_IOWR(0x5c, ec_ioctl_reg_request_t)
+#define EC_IOCTL_REG_REQUEST_WRITE EC_IOWR(0x5d, ec_ioctl_reg_request_t)
+#define EC_IOCTL_REG_REQUEST_READ EC_IOWR(0x5e, ec_ioctl_reg_request_t)
+#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x5f, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x60, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_READ EC_IOW(0x61, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_READ_NOSYNC EC_IOW(0x62, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_WRITE EC_IOWR(0x63, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_EXEC EC_IOWR(0x64, ec_ioctl_voe_t)
+#define EC_IOCTL_VOE_DATA EC_IOWR(0x65, ec_ioctl_voe_t)
+#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x66, size_t)
+
+/****************************************************************************/
+
+#define EC_IOCTL_STRING_SIZE 64
+
+/****************************************************************************/
+
+typedef struct {
+ uint32_t ioctl_version_magic;
+ uint32_t master_count;
+} ec_ioctl_module_t;
+
+/****************************************************************************/
+
+typedef struct {
+ uint32_t slave_count;
+ uint32_t scan_index;
+ uint32_t config_count;
+ uint32_t domain_count;
+ uint32_t eoe_handler_count;
+ uint8_t phase;
+ uint8_t active;
+ uint8_t scan_busy;
+ struct ec_ioctl_device {
+ uint8_t address[6];
+ uint8_t attached;
+ uint8_t link_state;
+ uint64_t tx_count;
+ uint64_t rx_count;
+ uint64_t tx_bytes;
+ uint64_t rx_bytes;
+ uint64_t tx_errors;
+ int32_t tx_frame_rates[EC_RATE_COUNT];
+ int32_t rx_frame_rates[EC_RATE_COUNT];
+ int32_t tx_byte_rates[EC_RATE_COUNT];
+ int32_t rx_byte_rates[EC_RATE_COUNT];
+ } devices[EC_MAX_NUM_DEVICES];
+ uint32_t num_devices;
+ uint64_t tx_count;
+ uint64_t rx_count;
+ uint64_t tx_bytes;
+ uint64_t rx_bytes;
+ int32_t tx_frame_rates[EC_RATE_COUNT];
+ int32_t rx_frame_rates[EC_RATE_COUNT];
+ int32_t tx_byte_rates[EC_RATE_COUNT];
+ int32_t rx_byte_rates[EC_RATE_COUNT];
+ int32_t loss_rates[EC_RATE_COUNT];
+ uint64_t app_time;
+ uint64_t dc_ref_time;
+ uint16_t ref_clock;
+} ec_ioctl_master_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // input
+ uint16_t position;
+
+ // outputs
+ unsigned int device_index;
+ uint32_t vendor_id;
+ uint32_t product_code;
+ uint32_t revision_number;
+ uint32_t serial_number;
+ uint16_t alias;
+ uint16_t boot_rx_mailbox_offset;
+ uint16_t boot_rx_mailbox_size;
+ uint16_t boot_tx_mailbox_offset;
+ uint16_t boot_tx_mailbox_size;
+ uint16_t std_rx_mailbox_offset;
+ uint16_t std_rx_mailbox_size;
+ uint16_t std_tx_mailbox_offset;
+ uint16_t std_tx_mailbox_size;
+ uint16_t mailbox_protocols;
+ uint8_t has_general_category;
+ ec_sii_coe_details_t coe_details;
+ ec_sii_general_flags_t general_flags;
+ int16_t current_on_ebus;
+ struct {
+ ec_slave_port_desc_t desc;
+ ec_slave_port_link_t link;
+ uint32_t receive_time;
+ uint16_t next_slave;
+ uint32_t delay_to_next_dc;
+ } ports[EC_MAX_PORTS];
+ uint8_t fmmu_bit;
+ uint8_t dc_supported;
+ ec_slave_dc_range_t dc_range;
+ uint8_t has_dc_system_time;
+ uint32_t transmission_delay;
+ uint8_t al_state;
+ uint8_t error_flag;
+ uint8_t sync_count;
+ uint16_t sdo_count;
+ uint32_t sii_nwords;
+ char group[EC_IOCTL_STRING_SIZE];
+ char image[EC_IOCTL_STRING_SIZE];
+ char order[EC_IOCTL_STRING_SIZE];
+ char name[EC_IOCTL_STRING_SIZE];
+} ec_ioctl_slave_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint32_t sync_index;
+
+ // outputs
+ uint16_t physical_start_address;
+ uint16_t default_size;
+ uint8_t control_register;
+ uint8_t enable;
+ uint8_t pdo_count;
+} ec_ioctl_slave_sync_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint32_t sync_index;
+ uint32_t pdo_pos;
+
+ // outputs
+ uint16_t index;
+ uint8_t entry_count;
+ int8_t name[EC_IOCTL_STRING_SIZE];
+} ec_ioctl_slave_sync_pdo_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint32_t sync_index;
+ uint32_t pdo_pos;
+ uint32_t entry_pos;
+
+ // outputs
+ uint16_t index;
+ uint8_t subindex;
+ uint8_t bit_length;
+ int8_t name[EC_IOCTL_STRING_SIZE];
+} ec_ioctl_slave_sync_pdo_entry_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t index;
+
+ // outputs
+ uint32_t data_size;
+ uint32_t logical_base_address;
+ uint16_t working_counter[EC_MAX_NUM_DEVICES];
+ uint16_t expected_working_counter;
+ uint32_t fmmu_count;
+} ec_ioctl_domain_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t domain_index;
+ uint32_t fmmu_index;
+
+ // outputs
+ uint16_t slave_config_alias;
+ uint16_t slave_config_position;
+ uint8_t sync_index;
+ ec_direction_t dir;
+ uint32_t logical_address;
+ uint32_t data_size;
+} ec_ioctl_domain_fmmu_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t domain_index;
+ uint32_t data_size;
+ uint8_t *target;
+} ec_ioctl_domain_data_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint8_t al_state;
+} ec_ioctl_slave_state_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint16_t sdo_position;
+
+ // outputs
+ uint16_t sdo_index;
+ uint8_t max_subindex;
+ int8_t name[EC_IOCTL_STRING_SIZE];
+} ec_ioctl_slave_sdo_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ int sdo_spec; // positive: index, negative: list position
+ uint8_t sdo_entry_subindex;
+
+ // outputs
+ uint16_t data_type;
+ uint16_t bit_length;
+ uint8_t read_access[EC_SDO_ENTRY_ACCESS_COUNT];
+ uint8_t write_access[EC_SDO_ENTRY_ACCESS_COUNT];
+ int8_t description[EC_IOCTL_STRING_SIZE];
+} ec_ioctl_slave_sdo_entry_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint16_t sdo_index;
+ uint8_t sdo_entry_subindex;
+ size_t target_size;
+ uint8_t *target;
+
+ // outputs
+ size_t data_size;
+ uint32_t abort_code;
+} ec_ioctl_slave_sdo_upload_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint16_t sdo_index;
+ uint8_t sdo_entry_subindex;
+ uint8_t complete_access;
+ size_t data_size;
+ uint8_t *data;
+
+ // outputs
+ uint32_t abort_code;
+} ec_ioctl_slave_sdo_download_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint16_t offset;
+ uint32_t nwords;
+ uint16_t *words;
+} ec_ioctl_slave_sii_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint8_t emergency;
+ uint16_t address;
+ size_t size;
+ uint8_t *data;
+} ec_ioctl_slave_reg_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint16_t offset;
+ size_t buffer_size;
+ uint8_t *buffer;
+
+ // outputs
+ size_t data_size;
+ uint32_t result;
+ uint32_t error_code;
+ char file_name[32];
+} ec_ioctl_slave_foe_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint8_t drive_no;
+ uint16_t idn;
+ size_t mem_size;
+ uint8_t *data;
+
+ // outputs
+ size_t data_size;
+ uint16_t error_code;
+} ec_ioctl_slave_soe_read_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint8_t drive_no;
+ uint16_t idn;
+ size_t data_size;
+ uint8_t *data;
+
+ // outputs
+ uint16_t error_code;
+} ec_ioctl_slave_soe_write_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+
+ // outputs
+ uint16_t alias;
+ uint16_t position;
+ uint32_t vendor_id;
+ uint32_t product_code;
+ struct {
+ ec_direction_t dir;
+ ec_watchdog_mode_t watchdog_mode;
+ uint32_t pdo_count;
+ uint8_t config_this;
+ } syncs[EC_MAX_SYNC_MANAGERS];
+ uint16_t watchdog_divider;
+ uint16_t watchdog_intervals;
+ uint32_t sdo_count;
+ uint32_t idn_count;
+ uint32_t flag_count;
+ int32_t slave_position;
+ uint16_t dc_assign_activate;
+ ec_sync_signal_t dc_sync[EC_SYNC_SIGNAL_COUNT];
+} ec_ioctl_config_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint8_t sync_index;
+ uint16_t pdo_pos;
+
+ // outputs
+ uint16_t index;
+ uint8_t entry_count;
+ int8_t name[EC_IOCTL_STRING_SIZE];
+} ec_ioctl_config_pdo_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint8_t sync_index;
+ uint16_t pdo_pos;
+ uint8_t entry_pos;
+
+ // outputs
+ uint16_t index;
+ uint8_t subindex;
+ uint8_t bit_length;
+ int8_t name[EC_IOCTL_STRING_SIZE];
+} ec_ioctl_config_pdo_entry_t;
+
+/****************************************************************************/
+
+/** Maximum size for displayed SDO data.
+ * \todo Make this dynamic.
+ */
+#define EC_MAX_SDO_DATA_SIZE 1024
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint32_t sdo_pos;
+
+ // outputs
+ uint16_t index;
+ uint8_t subindex;
+ size_t size;
+ uint8_t data[EC_MAX_SDO_DATA_SIZE];
+ uint8_t complete_access;
+} ec_ioctl_config_sdo_t;
+
+/****************************************************************************/
+
+/** Maximum size for displayed IDN data.
+ * \todo Make this dynamic.
+ */
+#define EC_MAX_IDN_DATA_SIZE 1024
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint32_t idn_pos;
+
+ // outputs
+ uint8_t drive_no;
+ uint16_t idn;
+ ec_al_state_t state;
+ size_t size;
+ uint8_t data[EC_MAX_IDN_DATA_SIZE];
+} ec_ioctl_config_idn_t;
+
+/****************************************************************************/
+
+/** Maximum size for key.
+ */
+#define EC_MAX_FLAG_KEY_SIZE 128
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint32_t flag_pos;
+
+ // outputs
+ char key[EC_MAX_FLAG_KEY_SIZE];
+ int32_t value;
+} ec_ioctl_config_flag_t;
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+
+typedef struct {
+ // input
+ uint16_t eoe_index;
+
+ // outputs
+ char name[EC_DATAGRAM_NAME_SIZE];
+ uint16_t slave_position;
+ uint8_t open;
+ uint32_t rx_bytes;
+ uint32_t rx_rate;
+ uint32_t tx_bytes;
+ uint32_t tx_rate;
+ uint32_t tx_queued_frames;
+ uint32_t tx_queue_size;
+} ec_ioctl_eoe_handler_t;
+
+#endif
+
+/****************************************************************************/
+
+#define EC_ETH_ALEN 6
+#ifdef ETH_ALEN
+#if ETH_ALEN != EC_ETH_ALEN
+#error Ethernet address length mismatch
+#endif
+#endif
+
+typedef struct {
+ // input
+ uint16_t slave_position;
+ uint16_t config_index; // alternatively
+
+ uint8_t mac_address_included;
+ uint8_t ip_address_included;
+ uint8_t subnet_mask_included;
+ uint8_t gateway_included;
+ uint8_t dns_included;
+ uint8_t name_included;
+
+ unsigned char mac_address[EC_ETH_ALEN];
+ struct in_addr ip_address;
+ struct in_addr subnet_mask;
+ struct in_addr gateway;
+ struct in_addr dns;
+ char name[EC_MAX_HOSTNAME_SIZE];
+
+ // output
+ uint16_t result;
+} ec_ioctl_eoe_ip_t;
+
+/*****************************************************************************/
+
+typedef struct {
+ // outputs
+ void *process_data;
+ size_t process_data_size;
+} ec_ioctl_master_activate_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint16_t pdo_index;
+ uint16_t entry_index;
+ uint8_t entry_subindex;
+ uint8_t entry_bit_length;
+} ec_ioctl_add_pdo_entry_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint16_t entry_index;
+ uint8_t entry_subindex;
+ uint32_t domain_index;
+
+ // outputs
+ unsigned int bit_position;
+} ec_ioctl_reg_pdo_entry_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint32_t sync_index;
+ uint32_t pdo_pos;
+ uint32_t entry_pos;
+ uint32_t domain_index;
+
+ // outputs
+ unsigned int bit_position;
+} ec_ioctl_reg_pdo_pos_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint16_t index;
+ uint8_t subindex;
+ const uint8_t *data;
+ size_t size;
+ uint8_t complete_access;
+} ec_ioctl_sc_sdo_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ size_t size;
+ uint8_t *target;
+
+ // outputs
+ int32_t overruns;
+} ec_ioctl_sc_emerg_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+
+ // outputs
+ ec_slave_config_state_t *state;
+} ec_ioctl_sc_state_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ uint8_t drive_no;
+ uint16_t idn;
+ ec_al_state_t al_state;
+ const uint8_t *data;
+ size_t size;
+} ec_ioctl_sc_idn_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ size_t key_size;
+ char *key;
+ int32_t value;
+} ec_ioctl_sc_flag_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ ec_al_state_t from_state;
+ ec_al_state_t to_state;
+ uint32_t timeout_ms;
+} ec_ioctl_sc_state_timeout_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t domain_index;
+
+ // outputs
+ ec_domain_state_t *state;
+} ec_ioctl_domain_state_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+
+ // inputs/outputs
+ uint32_t request_index;
+ uint16_t sdo_index;
+ uint8_t sdo_subindex;
+ size_t size;
+ uint8_t *data;
+ uint32_t timeout;
+ ec_request_state_t state;
+} ec_ioctl_sdo_request_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+
+ // inputs/outputs
+ uint32_t request_index;
+ uint8_t drive_no;
+ uint16_t idn;
+ size_t size;
+ uint8_t *data;
+ uint32_t timeout;
+ ec_request_state_t state;
+} ec_ioctl_soe_request_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+ size_t mem_size;
+
+ // inputs/outputs
+ uint32_t request_index;
+ uint8_t *data;
+ ec_request_state_t state;
+ uint8_t new_data;
+ uint16_t address;
+ size_t transfer_size;
+} ec_ioctl_reg_request_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t config_index;
+
+ // inputs/outputs
+ uint32_t voe_index;
+ uint32_t *vendor_id;
+ uint16_t *vendor_type;
+ size_t size;
+ uint8_t *data;
+ ec_request_state_t state;
+} ec_ioctl_voe_t;
+
+/****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint32_t dev_idx;
+
+ // outputs
+ ec_master_link_state_t *state;
+} ec_ioctl_link_state_t;
+
+/****************************************************************************/
+
+#ifdef __KERNEL__
+
+/** Context data structure for file handles.
+ */
+typedef struct {
+ unsigned int writable; /**< Device was opened with write permission. */
+ unsigned int requested; /**< Master was requested via this file handle. */
+ uint8_t *process_data; /**< Total process data area. */
+ size_t process_data_size; /**< Size of the \a process_data. */
+} ec_ioctl_context_t;
+
+long ec_ioctl(ec_master_t *, ec_ioctl_context_t *, unsigned int,
+ void __user *);
+
+#ifdef EC_RTDM
+
+long ec_ioctl_rtdm_rt(ec_master_t *, ec_ioctl_context_t *, unsigned int,
+ void __user *);
+long ec_ioctl_rtdm_nrt(ec_master_t *, ec_ioctl_context_t *, unsigned int,
+ void __user *);
+
+#ifndef EC_RTDM_XENOMAI_V3
+int ec_rtdm_mmap(ec_ioctl_context_t *, void **);
+#endif
+
+#endif
+
+#endif
+
+/****************************************************************************/
+
+/** \endcond */
+
+#endif
diff --git a/ubuntu/igh-ecat/master/mailbox.c b/ubuntu/igh-ecat/master/mailbox.c
new file mode 100644
index 00000000000000..05a10fd1c3b12c
--- /dev/null
+++ b/ubuntu/igh-ecat/master/mailbox.c
@@ -0,0 +1,203 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Mailbox functionality.
+*/
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+#include <linux/delay.h>
+
+#include "mailbox.h"
+#include "datagram.h"
+#include "master.h"
+
+/****************************************************************************/
+
+/**
+ Prepares a mailbox-send datagram.
+ \return Pointer to mailbox datagram data, or ERR_PTR() code.
+*/
+
+uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
+ ec_datagram_t *datagram, /**< datagram */
+ uint8_t type, /**< mailbox protocol */
+ size_t size /**< size of the data */
+ )
+{
+ size_t total_size;
+ int ret;
+
+ if (unlikely(!slave->sii.mailbox_protocols)) {
+ EC_SLAVE_ERR(slave, "Slave does not support mailbox"
+ " communication!\n");
+ return ERR_PTR(-EPROTONOSUPPORT);
+ }
+
+ total_size = EC_MBOX_HEADER_SIZE + size;
+
+ if (unlikely(total_size > slave->configured_rx_mailbox_size)) {
+ EC_SLAVE_ERR(slave, "Data size (%zu) does not fit in mailbox (%u)!\n",
+ total_size, slave->configured_rx_mailbox_size);
+ return ERR_PTR(-ENOBUFS);
+ }
+
+ ret = ec_datagram_fpwr(datagram, slave->station_address,
+ slave->configured_rx_mailbox_offset,
+ slave->configured_rx_mailbox_size);
+ if (ret)
+ return ERR_PTR(ret);
+
+ EC_WRITE_U16(datagram->data, size); // mailbox service data length
+ EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr.
+ EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority
+ EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type
+
+ return datagram->data + EC_MBOX_HEADER_SIZE;
+}
+
+/****************************************************************************/
+
+/**
+ Prepares a datagram for checking the mailbox state.
+ \todo Determine sync manager used for receive mailbox
+ \return 0 in case of success, else < 0
+*/
+
+int ec_slave_mbox_prepare_check(const ec_slave_t *slave, /**< slave */
+ ec_datagram_t *datagram /**< datagram */
+ )
+{
+ int ret = ec_datagram_fprd(datagram, slave->station_address, 0x808, 8);
+ if (ret)
+ return ret;
+
+ ec_datagram_zero(datagram);
+ return 0;
+}
+
+/****************************************************************************/
+
+/**
+ Processes a mailbox state checking datagram.
+ \return 0 in case of success, else < 0
+*/
+
+int ec_slave_mbox_check(const ec_datagram_t *datagram /**< datagram */)
+{
+ return EC_READ_U8(datagram->data + 5) & 8 ? 1 : 0;
+}
+
+/****************************************************************************/
+
+/**
+ Prepares a datagram to fetch mailbox data.
+ \return 0 in case of success, else < 0
+*/
+
+int ec_slave_mbox_prepare_fetch(const ec_slave_t *slave, /**< slave */
+ ec_datagram_t *datagram /**< datagram */
+ )
+{
+ int ret = ec_datagram_fprd(datagram, slave->station_address,
+ slave->configured_tx_mailbox_offset,
+ slave->configured_tx_mailbox_size);
+ if (ret)
+ return ret;
+
+ ec_datagram_zero(datagram);
+ return 0;
+}
+
+/****************************************************************************/
+
+/**
+ Mailbox error codes.
+*/
+
+const ec_code_msg_t mbox_error_messages[] = {
+ {0x00000001, "MBXERR_SYNTAX"},
+ {0x00000002, "MBXERR_UNSUPPORTEDPROTOCOL"},
+ {0x00000003, "MBXERR_INVAILDCHANNEL"},
+ {0x00000004, "MBXERR_SERVICENOTSUPPORTED"},
+ {0x00000005, "MBXERR_INVALIDHEADER"},
+ {0x00000006, "MBXERR_SIZETOOSHORT"},
+ {0x00000007, "MBXERR_NOMOREMEMORY"},
+ {0x00000008, "MBXERR_INVALIDSIZE"},
+ {}
+};
+
+/****************************************************************************/
+
+/** Processes received mailbox data.
+ *
+ * \return Pointer to the received data, or ERR_PTR() code.
+ */
+uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
+ const ec_datagram_t *datagram, /**< datagram */
+ uint8_t *type, /**< expected mailbox protocol */
+ size_t *size /**< size of the received data */
+ )
+{
+ size_t data_size;
+
+ data_size = EC_READ_U16(datagram->data);
+
+ if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) {
+ EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n");
+ ec_print_data(datagram->data, slave->configured_tx_mailbox_size);
+ return ERR_PTR(-EPROTO);
+ }
+
+ *type = EC_READ_U8(datagram->data + 5) & 0x0F;
+ *size = data_size;
+
+ if (*type == 0x00) {
+ const ec_code_msg_t *mbox_msg;
+ uint16_t code = EC_READ_U16(datagram->data + 8);
+
+ EC_SLAVE_ERR(slave, "Mailbox error response received - ");
+
+ for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) {
+ if (mbox_msg->code != code)
+ continue;
+ printk(KERN_CONT "Code 0x%04X: \"%s\".\n",
+ mbox_msg->code, mbox_msg->message);
+ break;
+ }
+
+ if (!mbox_msg->code) {
+ printk(KERN_CONT "Unknown error reply code 0x%04X.\n", code);
+ }
+
+ if (slave->master->debug_level)
+ ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size);
+
+ return ERR_PTR(-EPROTO);
+ }
+
+ return datagram->data + EC_MBOX_HEADER_SIZE;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/mailbox.h b/ubuntu/igh-ecat/master/mailbox.h
new file mode 100644
index 00000000000000..e9de78cf0ab89d
--- /dev/null
+++ b/ubuntu/igh-ecat/master/mailbox.h
@@ -0,0 +1,64 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Mailbox functionality.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_MAILBOX_H__
+#define __EC_MAILBOX_H__
+
+#include "slave.h"
+
+/****************************************************************************/
+
+/** Size of the mailbox header.
+ */
+#define EC_MBOX_HEADER_SIZE 6
+
+/** Mailbox types.
+ *
+ * These are used in the 'Type' field of the mailbox header.
+ */
+enum {
+ EC_MBOX_TYPE_EOE = 0x02,
+ EC_MBOX_TYPE_COE = 0x03,
+ EC_MBOX_TYPE_FOE = 0x04,
+ EC_MBOX_TYPE_SOE = 0x05,
+ EC_MBOX_TYPE_VOE = 0x0f,
+};
+
+/****************************************************************************/
+
+uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *, ec_datagram_t *,
+ uint8_t, size_t);
+int ec_slave_mbox_prepare_check(const ec_slave_t *, ec_datagram_t *);
+int ec_slave_mbox_check(const ec_datagram_t *);
+int ec_slave_mbox_prepare_fetch(const ec_slave_t *, ec_datagram_t *);
+uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, const ec_datagram_t *,
+ uint8_t *, size_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/master.c b/ubuntu/igh-ecat/master/master.c
new file mode 100644
index 00000000000000..180471aaa3d6a1
--- /dev/null
+++ b/ubuntu/igh-ecat/master/master.c
@@ -0,0 +1,3350 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2020 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * vim: expandtab
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT master methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/version.h>
+#include <linux/hrtimer.h>
+#include <linux/kthread.h>
+
+#include "globals.h"
+#include "slave.h"
+#include "slave_config.h"
+#include "device.h"
+#include "datagram.h"
+
+#ifdef EC_EOE
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0)
+#include <uapi/linux/sched/types.h> // struct sched_param
+#include <linux/sched/types.h> // sched_setscheduler
+#endif
+#include "ethernet.h"
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 17, 0) || \
+ (defined(CONFIG_PREEMPT_RT_FULL) && LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0))
+# define ec_rt_lock_interruptible(lock) \
+ rt_mutex_lock_interruptible(lock)
+#else
+# define ec_rt_lock_interruptible(lock) \
+ rt_mutex_lock_interruptible(lock, 0)
+#endif
+
+#include "master.h"
+
+/****************************************************************************/
+
+/** Set to 1 to enable external datagram injection debugging.
+ */
+#define DEBUG_INJECT 0
+
+/** Always output corrupted frames.
+ */
+#define FORCE_OUTPUT_CORRUPTED 0
+
+/** SDO injection timeout in microseconds. */
+#define EC_SDO_INJECTION_TIMEOUT 10000
+
+#ifdef EC_HAVE_CYCLES
+
+/** Frame timeout in cycles.
+ */
+static cycles_t timeout_cycles;
+
+/** Timeout for external datagram injection [cycles].
+ */
+static cycles_t ext_injection_timeout_cycles;
+
+#else
+
+/** Frame timeout in jiffies.
+ */
+static unsigned long timeout_jiffies;
+
+/** Timeout for external datagram injection [jiffies].
+ */
+static unsigned long ext_injection_timeout_jiffies;
+
+#endif
+
+/** List of intervals for statistics [s].
+ */
+const unsigned int rate_intervals[] = {
+ 1, 10, 60
+};
+
+/****************************************************************************/
+
+void ec_master_clear_config(ec_master_t *);
+void ec_master_clear_slave_configs(ec_master_t *);
+void ec_master_clear_domains(ec_master_t *);
+int ec_master_thread_start(ec_master_t *, int (*)(void *), const char *);
+void ec_master_thread_stop(ec_master_t *);
+void ec_master_inject_external_datagrams(ec_master_t *);
+ec_datagram_t *ec_master_get_external_datagram(ec_master_t *);
+void ec_master_exec_slave_fsms(ec_master_t *);
+void ec_master_send_datagrams(ec_master_t *, ec_device_index_t);
+int ec_master_calc_topology_rec(ec_master_t *, ec_slave_t *, unsigned int *);
+void ec_master_calc_topology(ec_master_t *);
+void ec_master_calc_transmission_delays(ec_master_t *);
+static int ec_master_idle_thread(void *);
+static int ec_master_operation_thread(void *);
+#ifdef EC_EOE
+static int ec_master_eoe_thread(void *);
+#endif
+void ec_master_find_dc_ref_clock(ec_master_t *);
+void ec_master_clear_device_stats(ec_master_t *);
+void ec_master_update_device_stats(ec_master_t *);
+void ec_master_nanosleep(const unsigned long);
+static void sc_reset_task_kicker(struct irq_work *work);
+static void sc_reset_task(struct work_struct *work);
+
+/****************************************************************************/
+
+/** Static variables initializer.
+*/
+void ec_master_init_static(void)
+{
+#ifdef EC_HAVE_CYCLES
+ timeout_cycles = (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000);
+ ext_injection_timeout_cycles =
+ (cycles_t) EC_SDO_INJECTION_TIMEOUT /* us */ * (cpu_khz / 1000);
+#else
+ // one jiffy may always elapse between time measurement
+ timeout_jiffies = max(EC_IO_TIMEOUT * HZ / 1000000, 1);
+ ext_injection_timeout_jiffies =
+ max(EC_SDO_INJECTION_TIMEOUT * HZ / 1000000, 1);
+#endif
+}
+
+/****************************************************************************/
+
+/**
+ Master constructor.
+ \return 0 in case of success, else < 0
+*/
+
+int ec_master_init(ec_master_t *master, /**< EtherCAT master */
+ unsigned int index, /**< master index */
+ const uint8_t *main_mac, /**< MAC address of main device */
+ const uint8_t *backup_mac, /**< MAC address of backup device */
+ dev_t device_number, /**< Character device number. */
+ struct class *class, /**< Device class. */
+ unsigned int debug_level, /**< Debug level (module parameter). */
+ unsigned int run_on_cpu /**< bind created kernel threads to a cpu */
+ )
+{
+ int ret;
+ unsigned int dev_idx, i;
+
+ master->index = index;
+ master->reserved = 0;
+
+ sema_init(&master->master_sem, 1);
+
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_MAX_NUM_DEVICES; dev_idx++) {
+ master->macs[dev_idx] = NULL;
+ }
+
+ master->macs[EC_DEVICE_MAIN] = main_mac;
+
+#if EC_MAX_NUM_DEVICES > 1
+ master->macs[EC_DEVICE_BACKUP] = backup_mac;
+ master->num_devices = 1 + !ec_mac_is_zero(backup_mac);
+#else
+ if (!ec_mac_is_zero(backup_mac)) {
+ EC_MASTER_WARN(master, "Ignoring backup MAC address!");
+ }
+#endif
+
+ ec_master_clear_device_stats(master);
+
+ sema_init(&master->device_sem, 1);
+
+ master->phase = EC_ORPHANED;
+ master->active = 0;
+ master->config_changed = 0;
+ master->injection_seq_fsm = 0;
+ master->injection_seq_rt = 0;
+
+ master->slaves = NULL;
+ master->slave_count = 0;
+
+ INIT_LIST_HEAD(&master->configs);
+ INIT_LIST_HEAD(&master->domains);
+
+ master->app_time = 0ULL;
+ master->dc_ref_time = 0ULL;
+
+ master->scan_busy = 0;
+ master->scan_index = 0;
+ master->allow_scan = 1;
+ sema_init(&master->scan_sem, 1);
+ init_waitqueue_head(&master->scan_queue);
+
+ master->config_busy = 0;
+ sema_init(&master->config_sem, 1);
+ init_waitqueue_head(&master->config_queue);
+
+ INIT_LIST_HEAD(&master->datagram_queue);
+ master->datagram_index = 0;
+
+ INIT_LIST_HEAD(&master->ext_datagram_queue);
+ sema_init(&master->ext_queue_sem, 1);
+
+ master->ext_ring_idx_rt = 0;
+ master->ext_ring_idx_fsm = 0;
+
+ // init external datagram ring
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_t *datagram = &master->ext_datagram_ring[i];
+ ec_datagram_init(datagram);
+ snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, "ext-%u", i);
+ }
+
+ // send interval in IDLE phase
+ ec_master_set_send_interval(master, 1000000 / HZ);
+
+ master->fsm_slave = NULL;
+ INIT_LIST_HEAD(&master->fsm_exec_list);
+ master->fsm_exec_count = 0U;
+
+ master->debug_level = debug_level;
+ master->run_on_cpu = run_on_cpu;
+ master->stats.timeouts = 0;
+ master->stats.corrupted = 0;
+ master->stats.unmatched = 0;
+ master->stats.output_jiffies = 0;
+
+ master->thread = NULL;
+
+#ifdef EC_EOE
+ master->eoe_thread = NULL;
+ INIT_LIST_HEAD(&master->eoe_handlers);
+#endif
+
+ rt_mutex_init(&master->io_mutex);
+ master->send_cb = NULL;
+ master->receive_cb = NULL;
+ master->cb_data = NULL;
+ master->app_send_cb = NULL;
+ master->app_receive_cb = NULL;
+ master->app_cb_data = NULL;
+
+ INIT_LIST_HEAD(&master->sii_requests);
+ INIT_LIST_HEAD(&master->emerg_reg_requests);
+
+ init_waitqueue_head(&master->request_queue);
+
+ // init devices
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ ret = ec_device_init(&master->devices[dev_idx], master);
+ if (ret < 0) {
+ goto out_clear_devices;
+ }
+ }
+
+ // init state machine datagram
+ ec_datagram_init(&master->fsm_datagram);
+ snprintf(master->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "master-fsm");
+ ret = ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE);
+ if (ret < 0) {
+ ec_datagram_clear(&master->fsm_datagram);
+ EC_MASTER_ERR(master, "Failed to allocate FSM datagram.\n");
+ goto out_clear_devices;
+ }
+
+ // create state machine object
+ ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram);
+
+ // alloc external datagram ring
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_t *datagram = &master->ext_datagram_ring[i];
+ ret = ec_datagram_prealloc(datagram, EC_MAX_DATA_SIZE);
+ if (ret) {
+ EC_MASTER_ERR(master, "Failed to allocate external"
+ " datagram %u.\n", i);
+ goto out_clear_ext_datagrams;
+ }
+ }
+
+ // init reference sync datagram
+ ec_datagram_init(&master->ref_sync_datagram);
+ snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE,
+ "refsync");
+ ret = ec_datagram_prealloc(&master->ref_sync_datagram, 4);
+ if (ret < 0) {
+ ec_datagram_clear(&master->ref_sync_datagram);
+ EC_MASTER_ERR(master, "Failed to allocate reference"
+ " synchronisation datagram.\n");
+ goto out_clear_ext_datagrams;
+ }
+
+ // init sync datagram
+ ec_datagram_init(&master->sync_datagram);
+ snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync");
+ ret = ec_datagram_prealloc(&master->sync_datagram, 4);
+ if (ret < 0) {
+ ec_datagram_clear(&master->sync_datagram);
+ EC_MASTER_ERR(master, "Failed to allocate"
+ " synchronisation datagram.\n");
+ goto out_clear_ref_sync;
+ }
+
+ // init sync monitor datagram
+ ec_datagram_init(&master->sync_mon_datagram);
+ snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE,
+ "syncmon");
+ ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4);
+ if (ret < 0) {
+ ec_datagram_clear(&master->sync_mon_datagram);
+ EC_MASTER_ERR(master, "Failed to allocate sync"
+ " monitoring datagram.\n");
+ goto out_clear_sync;
+ }
+
+ master->dc_ref_config = NULL;
+ master->dc_ref_clock = NULL;
+
+ INIT_WORK(&master->sc_reset_work, sc_reset_task);
+ init_irq_work(&master->sc_reset_work_kicker, sc_reset_task_kicker);
+
+ // init character device
+ ret = ec_cdev_init(&master->cdev, master, device_number);
+ if (ret)
+ goto out_clear_sync_mon;
+
+ master->class_device = device_create(class, NULL,
+ MKDEV(MAJOR(device_number), master->index), NULL,
+ "EtherCAT%u", master->index);
+ if (IS_ERR(master->class_device)) {
+ EC_MASTER_ERR(master, "Failed to create class device!\n");
+ ret = PTR_ERR(master->class_device);
+ goto out_clear_cdev;
+ }
+
+#ifdef EC_RTDM
+ // init RTDM device
+ ret = ec_rtdm_dev_init(&master->rtdm_dev, master);
+ if (ret) {
+ goto out_unregister_class_device;
+ }
+#endif
+
+ return 0;
+
+#ifdef EC_RTDM
+out_unregister_class_device:
+ device_unregister(master->class_device);
+#endif
+out_clear_cdev:
+ ec_cdev_clear(&master->cdev);
+out_clear_sync_mon:
+ ec_datagram_clear(&master->sync_mon_datagram);
+out_clear_sync:
+ ec_datagram_clear(&master->sync_datagram);
+out_clear_ref_sync:
+ ec_datagram_clear(&master->ref_sync_datagram);
+out_clear_ext_datagrams:
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_clear(&master->ext_datagram_ring[i]);
+ }
+ ec_fsm_master_clear(&master->fsm);
+ ec_datagram_clear(&master->fsm_datagram);
+out_clear_devices:
+ for (; dev_idx > 0; dev_idx--) {
+ ec_device_clear(&master->devices[dev_idx - 1]);
+ }
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+*/
+void ec_master_clear(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ unsigned int dev_idx, i;
+
+#ifdef EC_RTDM
+ ec_rtdm_dev_clear(&master->rtdm_dev);
+#endif
+
+ device_unregister(master->class_device);
+
+ ec_cdev_clear(&master->cdev);
+
+ irq_work_sync(&master->sc_reset_work_kicker);
+ cancel_work_sync(&master->sc_reset_work);
+
+#ifdef EC_EOE
+ ec_master_clear_eoe_handlers(master);
+#endif
+ ec_master_clear_domains(master);
+ ec_master_clear_slave_configs(master);
+ ec_master_clear_slaves(master);
+
+ ec_datagram_clear(&master->sync_mon_datagram);
+ ec_datagram_clear(&master->sync_datagram);
+ ec_datagram_clear(&master->ref_sync_datagram);
+
+ for (i = 0; i < EC_EXT_RING_SIZE; i++) {
+ ec_datagram_clear(&master->ext_datagram_ring[i]);
+ }
+
+ ec_fsm_master_clear(&master->fsm);
+ ec_datagram_clear(&master->fsm_datagram);
+
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ ec_device_clear(&master->devices[dev_idx]);
+ }
+}
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+/** Clear and free all EoE handlers.
+ */
+void ec_master_clear_eoe_handlers(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ ec_eoe_t *eoe, *next;
+
+ list_for_each_entry_safe(eoe, next, &master->eoe_handlers, list) {
+ list_del(&eoe->list);
+ ec_eoe_clear(eoe);
+ kfree(eoe);
+ }
+}
+#endif
+
+/****************************************************************************/
+
+/** Clear all slave configurations.
+ */
+void ec_master_clear_slave_configs(ec_master_t *master)
+{
+ ec_slave_config_t *sc, *next;
+
+ master->dc_ref_config = NULL;
+ master->fsm.sdo_request = NULL; // mark sdo_request as invalid
+
+ list_for_each_entry_safe(sc, next, &master->configs, list) {
+ list_del(&sc->list);
+ ec_slave_config_clear(sc);
+ kfree(sc);
+ }
+}
+
+/****************************************************************************/
+
+/** Clear all slaves.
+ */
+void ec_master_clear_slaves(ec_master_t *master)
+{
+ ec_slave_t *slave;
+
+ master->dc_ref_clock = NULL;
+
+ // External requests are obsolete, so we wake pending waiters and remove
+ // them from the list.
+
+ while (!list_empty(&master->sii_requests)) {
+ ec_sii_write_request_t *request =
+ list_entry(master->sii_requests.next,
+ ec_sii_write_request_t, list);
+ list_del_init(&request->list); // dequeue
+ EC_MASTER_WARN(master, "Discarding SII request, slave %u about"
+ " to be deleted.\n", request->slave->ring_position);
+ request->state = EC_INT_REQUEST_FAILURE;
+ wake_up_all(&master->request_queue);
+ }
+
+ master->fsm_slave = NULL;
+ INIT_LIST_HEAD(&master->fsm_exec_list);
+ master->fsm_exec_count = 0;
+
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ ec_slave_clear(slave);
+ }
+
+ if (master->slaves) {
+ kfree(master->slaves);
+ master->slaves = NULL;
+ }
+
+ master->slave_count = 0;
+}
+
+/****************************************************************************/
+
+/** Clear all domains.
+ */
+void ec_master_clear_domains(ec_master_t *master)
+{
+ ec_domain_t *domain, *next;
+
+ list_for_each_entry_safe(domain, next, &master->domains, list) {
+ list_del(&domain->list);
+ ec_domain_clear(domain);
+ kfree(domain);
+ }
+}
+
+/****************************************************************************/
+
+/** Clear the configuration applied by the application.
+ */
+void ec_master_clear_config(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ down(&master->master_sem);
+ ec_master_clear_domains(master);
+ ec_master_clear_slave_configs(master);
+ up(&master->master_sem);
+}
+
+/****************************************************************************/
+
+/** Internal sending callback.
+ */
+void ec_master_internal_send_cb(
+ void *cb_data /**< Callback data. */
+ )
+{
+ ec_master_t *master = (ec_master_t *) cb_data;
+ if (ec_rt_lock_interruptible(&master->io_mutex))
+ return;
+ ecrt_master_send_ext(master);
+ rt_mutex_unlock(&master->io_mutex);
+}
+
+/****************************************************************************/
+
+/** Internal receiving callback.
+ */
+void ec_master_internal_receive_cb(
+ void *cb_data /**< Callback data. */
+ )
+{
+ ec_master_t *master = (ec_master_t *) cb_data;
+ if (ec_rt_lock_interruptible(&master->io_mutex))
+ return;
+ ecrt_master_receive(master);
+ rt_mutex_unlock(&master->io_mutex);
+}
+
+/****************************************************************************/
+
+/** Starts the master thread.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_master_thread_start(
+ ec_master_t *master, /**< EtherCAT master */
+ int (*thread_func)(void *), /**< thread function to start */
+ const char *name /**< Thread name. */
+ )
+{
+ EC_MASTER_INFO(master, "Starting %s thread.\n", name);
+ master->thread = kthread_create(thread_func, master, name);
+ if (IS_ERR(master->thread)) {
+ int err = (int) PTR_ERR(master->thread);
+ EC_MASTER_ERR(master, "Failed to start master thread (error %i)!\n",
+ err);
+ master->thread = NULL;
+ return err;
+ }
+ if (0xffffffff != master->run_on_cpu) {
+ EC_MASTER_INFO(master, " binding thread to cpu %u\n",master->run_on_cpu);
+ kthread_bind(master->thread,master->run_on_cpu);
+ }
+ /* Ignoring return value of wake_up_process */
+ (void) wake_up_process(master->thread);
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Stops the master thread.
+ */
+void ec_master_thread_stop(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ unsigned long sleep_jiffies;
+
+ if (!master->thread) {
+ EC_MASTER_WARN(master, "%s(): Already finished!\n", __func__);
+ return;
+ }
+
+ EC_MASTER_DBG(master, 1, "Stopping master thread.\n");
+
+ kthread_stop(master->thread);
+ master->thread = NULL;
+ EC_MASTER_INFO(master, "Master thread exited.\n");
+
+ if (master->fsm_datagram.state != EC_DATAGRAM_SENT) {
+ return;
+ }
+
+ // wait for FSM datagram
+ sleep_jiffies = max(HZ / 100, 1); // 10 ms, at least 1 jiffy
+ schedule_timeout(sleep_jiffies);
+}
+
+/****************************************************************************/
+
+/** Transition function from ORPHANED to IDLE phase.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_master_enter_idle_phase(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ int ret;
+ ec_device_index_t dev_idx;
+
+ EC_MASTER_DBG(master, 1, "ORPHANED -> IDLE.\n");
+
+ master->send_cb = ec_master_internal_send_cb;
+ master->receive_cb = ec_master_internal_receive_cb;
+ master->cb_data = master;
+
+ master->phase = EC_IDLE;
+
+ // reset number of responding slaves to trigger scanning
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ master->fsm.slaves_responding[dev_idx] = 0;
+ }
+
+ ret = ec_master_thread_start(master, ec_master_idle_thread,
+ "EtherCAT-IDLE");
+ if (ret)
+ master->phase = EC_ORPHANED;
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Transition function from IDLE to ORPHANED phase.
+ */
+void ec_master_leave_idle_phase(ec_master_t *master /**< EtherCAT master */)
+{
+ EC_MASTER_DBG(master, 1, "IDLE -> ORPHANED.\n");
+
+ master->phase = EC_ORPHANED;
+
+#ifdef EC_EOE
+ ec_master_eoe_stop(master);
+#endif
+ ec_master_thread_stop(master);
+
+ down(&master->master_sem);
+ ec_master_clear_slaves(master);
+ up(&master->master_sem);
+
+ ec_fsm_master_reset(&master->fsm);
+}
+
+/****************************************************************************/
+
+/** Transition function from IDLE to OPERATION phase.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_master_enter_operation_phase(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ int ret = 0;
+ ec_slave_t *slave;
+
+ EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n");
+
+ down(&master->config_sem);
+ if (master->config_busy) {
+ up(&master->config_sem);
+
+ // wait for slave configuration to complete
+ ret = wait_event_interruptible(master->config_queue,
+ !master->config_busy);
+ if (ret) {
+ EC_MASTER_INFO(master, "Finishing slave configuration"
+ " interrupted by signal.\n");
+ goto out_return;
+ }
+
+ EC_MASTER_DBG(master, 1, "Waiting for pending slave"
+ " configuration returned.\n");
+ } else {
+ up(&master->config_sem);
+ }
+
+ down(&master->scan_sem);
+ master->allow_scan = 0; // 'lock' the slave list
+ if (!master->scan_busy) {
+ up(&master->scan_sem);
+ } else {
+ up(&master->scan_sem);
+
+ // wait for slave scan to complete
+ ret = wait_event_interruptible(master->scan_queue,
+ !master->scan_busy);
+ if (ret) {
+ EC_MASTER_INFO(master, "Waiting for slave scan"
+ " interrupted by signal.\n");
+ goto out_allow;
+ }
+
+ EC_MASTER_DBG(master, 1, "Waiting for pending"
+ " slave scan returned.\n");
+ }
+
+ // set states for all slaves
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
+ }
+
+ master->phase = EC_OPERATION;
+ master->app_send_cb = NULL;
+ master->app_receive_cb = NULL;
+ master->app_cb_data = NULL;
+ return ret;
+
+out_allow:
+ master->allow_scan = 1;
+out_return:
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Transition function from OPERATION to IDLE phase.
+ */
+void ec_master_leave_operation_phase(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ if (master->active) {
+ ecrt_master_deactivate(master); // also clears config
+ } else {
+ ec_master_clear_config(master);
+ }
+
+ /* Re-allow scanning for IDLE phase. */
+ master->allow_scan = 1;
+
+ EC_MASTER_DBG(master, 1, "OPERATION -> IDLE.\n");
+
+ master->phase = EC_IDLE;
+}
+
+/****************************************************************************/
+
+/** Injects external datagrams that fit into the datagram queue.
+ */
+void ec_master_inject_external_datagrams(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ ec_datagram_t *datagram;
+ size_t queue_size = 0, new_queue_size = 0;
+#if DEBUG_INJECT
+ unsigned int datagram_count = 0;
+#endif
+
+ if (master->ext_ring_idx_rt == master->ext_ring_idx_fsm) {
+ // nothing to inject
+ return;
+ }
+
+ list_for_each_entry(datagram, &master->datagram_queue, queue) {
+ if (datagram->state == EC_DATAGRAM_QUEUED) {
+ queue_size += datagram->data_size;
+ }
+ }
+
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Injecting datagrams, queue_size=%zu\n",
+ queue_size);
+#endif
+
+ while (master->ext_ring_idx_rt != master->ext_ring_idx_fsm) {
+ datagram = &master->ext_datagram_ring[master->ext_ring_idx_rt];
+
+ if (datagram->state != EC_DATAGRAM_INIT) {
+ // skip datagram
+ master->ext_ring_idx_rt =
+ (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
+ continue;
+ }
+
+ new_queue_size = queue_size + datagram->data_size;
+ if (new_queue_size <= master->max_queue_size) {
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Injecting datagram %s"
+ " size=%zu, queue_size=%zu\n", datagram->name,
+ datagram->data_size, new_queue_size);
+ datagram_count++;
+#endif
+#ifdef EC_HAVE_CYCLES
+ datagram->cycles_sent = 0;
+#endif
+ datagram->jiffies_sent = 0;
+ ec_master_queue_datagram(master, datagram);
+ queue_size = new_queue_size;
+ }
+ else if (datagram->data_size > master->max_queue_size) {
+ datagram->state = EC_DATAGRAM_ERROR;
+ EC_MASTER_ERR(master, "External datagram %s is too large,"
+ " size=%zu, max_queue_size=%zu\n",
+ datagram->name, datagram->data_size,
+ master->max_queue_size);
+ }
+ else { // datagram does not fit in the current cycle
+#ifdef EC_HAVE_CYCLES
+ cycles_t cycles_now = get_cycles();
+
+ if (cycles_now - datagram->cycles_sent
+ > ext_injection_timeout_cycles)
+#else
+ if (jiffies - datagram->jiffies_sent
+ > ext_injection_timeout_jiffies)
+#endif
+ {
+#if defined EC_RT_SYSLOG || DEBUG_INJECT
+ unsigned int time_us;
+#endif
+
+ datagram->state = EC_DATAGRAM_ERROR;
+
+#if defined EC_RT_SYSLOG || DEBUG_INJECT
+#ifdef EC_HAVE_CYCLES
+ time_us = (unsigned int)
+ ((cycles_now - datagram->cycles_sent) * 1000LL)
+ / cpu_khz;
+#else
+ time_us = (unsigned int)
+ ((jiffies - datagram->jiffies_sent) * 1000000 / HZ);
+#endif
+ EC_MASTER_ERR(master, "Timeout %u us: Injecting"
+ " external datagram %s size=%zu,"
+ " max_queue_size=%zu\n", time_us, datagram->name,
+ datagram->data_size, master->max_queue_size);
+#endif
+ }
+ else {
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Deferred injecting"
+ " external datagram %s size=%u, queue_size=%u\n",
+ datagram->name, datagram->data_size, queue_size);
+#endif
+ break;
+ }
+ }
+
+ master->ext_ring_idx_rt =
+ (master->ext_ring_idx_rt + 1) % EC_EXT_RING_SIZE;
+ }
+
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Injected %u datagrams.\n", datagram_count);
+#endif
+}
+
+/****************************************************************************/
+
+/** Sets the expected interval between calls to ecrt_master_send
+ * and calculates the maximum amount of data to queue.
+ */
+void ec_master_set_send_interval(
+ ec_master_t *master, /**< EtherCAT master */
+ unsigned int send_interval /**< Send interval */
+ )
+{
+ master->send_interval = send_interval;
+ master->max_queue_size =
+ (send_interval * 1000) / EC_BYTE_TRANSMISSION_TIME_NS;
+ master->max_queue_size -= master->max_queue_size / 10;
+}
+
+/****************************************************************************/
+
+/** Searches for a free datagram in the external datagram ring.
+ *
+ * \return Next free datagram, or NULL.
+ */
+ec_datagram_t *ec_master_get_external_datagram(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ if ((master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE !=
+ master->ext_ring_idx_rt) {
+ ec_datagram_t *datagram =
+ &master->ext_datagram_ring[master->ext_ring_idx_fsm];
+ return datagram;
+ }
+ else {
+ return NULL;
+ }
+}
+
+/****************************************************************************/
+
+/** Places a datagram in the datagram queue.
+ */
+void ec_master_queue_datagram(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_datagram_t *datagram /**< datagram */
+ )
+{
+ ec_datagram_t *queued_datagram;
+
+ /* It is possible, that a datagram in the queue is re-initialized with the
+ * ec_datagram_<type>() methods and then shall be queued with this method.
+ * In that case, the state is already reset to EC_DATAGRAM_INIT. Check if
+ * the datagram is queued to avoid duplicate queuing (which results in an
+ * infinite loop!). Set the state to EC_DATAGRAM_QUEUED again, probably
+ * causing an unmatched datagram. */
+ list_for_each_entry(queued_datagram, &master->datagram_queue, queue) {
+ if (queued_datagram == datagram) {
+ datagram->skip_count++;
+#ifdef EC_RT_SYSLOG
+ EC_MASTER_DBG(master, 1,
+ "Datagram %p already queued (skipping).\n", datagram);
+#endif
+ datagram->state = EC_DATAGRAM_QUEUED;
+ return;
+ }
+ }
+
+ list_add_tail(&datagram->queue, &master->datagram_queue);
+ datagram->state = EC_DATAGRAM_QUEUED;
+}
+
+/****************************************************************************/
+
+/** Places a datagram in the non-application datagram queue.
+ */
+void ec_master_queue_datagram_ext(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_datagram_t *datagram /**< datagram */
+ )
+{
+ down(&master->ext_queue_sem);
+ list_add_tail(&datagram->ext_queue, &master->ext_datagram_queue);
+ up(&master->ext_queue_sem);
+}
+
+/****************************************************************************/
+
+/** Sends the datagrams in the queue for a certain device.
+ *
+ */
+void ec_master_send_datagrams(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_device_index_t device_index /**< Device index. */
+ )
+{
+ ec_datagram_t *datagram, *next;
+ size_t datagram_size;
+ uint8_t *frame_data, *cur_data = NULL;
+ void *follows_word;
+#ifdef EC_HAVE_CYCLES
+ cycles_t cycles_start, cycles_sent, cycles_end;
+#endif
+ unsigned long jiffies_sent;
+ unsigned int frame_count, more_datagrams_waiting;
+ struct list_head sent_datagrams;
+
+#ifdef EC_HAVE_CYCLES
+ cycles_start = get_cycles();
+#endif
+ frame_count = 0;
+ INIT_LIST_HEAD(&sent_datagrams);
+
+ EC_MASTER_DBG(master, 2, "%s(device_index = %u)\n",
+ __func__, device_index);
+
+ do {
+ frame_data = NULL;
+ follows_word = NULL;
+ more_datagrams_waiting = 0;
+
+ // fill current frame with datagrams
+ list_for_each_entry(datagram, &master->datagram_queue, queue) {
+ if (datagram->state != EC_DATAGRAM_QUEUED ||
+ datagram->device_index != device_index) {
+ continue;
+ }
+
+ if (!frame_data) {
+ // fetch pointer to transmit socket buffer
+ frame_data =
+ ec_device_tx_data(&master->devices[device_index]);
+ cur_data = frame_data + EC_FRAME_HEADER_SIZE;
+ }
+
+ // does the current datagram fit in the frame?
+ datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size
+ + EC_DATAGRAM_FOOTER_SIZE;
+ if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) {
+ more_datagrams_waiting = 1;
+ break;
+ }
+
+ list_add_tail(&datagram->sent, &sent_datagrams);
+ datagram->index = master->datagram_index++;
+
+ EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n",
+ datagram->index);
+
+ // set "datagram following" flag in previous datagram
+ if (follows_word) {
+ EC_WRITE_U16(follows_word,
+ EC_READ_U16(follows_word) | 0x8000);
+ }
+
+ // EtherCAT datagram header
+ EC_WRITE_U8 (cur_data, datagram->type);
+ EC_WRITE_U8 (cur_data + 1, datagram->index);
+ memcpy(cur_data + 2, datagram->address, EC_ADDR_LEN);
+ EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF);
+ EC_WRITE_U16(cur_data + 8, 0x0000);
+ follows_word = cur_data + 6;
+ cur_data += EC_DATAGRAM_HEADER_SIZE;
+
+ // EtherCAT datagram data
+ memcpy(cur_data, datagram->data, datagram->data_size);
+ cur_data += datagram->data_size;
+
+ // EtherCAT datagram footer
+ EC_WRITE_U16(cur_data, 0x0000); // reset working counter
+ cur_data += EC_DATAGRAM_FOOTER_SIZE;
+ }
+
+ if (list_empty(&sent_datagrams)) {
+ EC_MASTER_DBG(master, 2, "nothing to send.\n");
+ break;
+ }
+
+ // EtherCAT frame header
+ EC_WRITE_U16(frame_data, ((cur_data - frame_data
+ - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);
+
+ // pad frame
+ while (cur_data - frame_data < ETH_ZLEN - ETH_HLEN)
+ EC_WRITE_U8(cur_data++, 0x00);
+
+ EC_MASTER_DBG(master, 2, "frame size: %zu\n", cur_data - frame_data);
+
+ // send frame
+ ec_device_send(&master->devices[device_index],
+ cur_data - frame_data);
+#ifdef EC_HAVE_CYCLES
+ cycles_sent = get_cycles();
+#endif
+ jiffies_sent = jiffies;
+
+ // set datagram states and sending timestamps
+ list_for_each_entry_safe(datagram, next, &sent_datagrams, sent) {
+ datagram->state = EC_DATAGRAM_SENT;
+#ifdef EC_HAVE_CYCLES
+ datagram->cycles_sent = cycles_sent;
+#endif
+ datagram->jiffies_sent = jiffies_sent;
+ list_del_init(&datagram->sent); // empty list of sent datagrams
+ }
+
+ frame_count++;
+ }
+ while (more_datagrams_waiting);
+
+#ifdef EC_HAVE_CYCLES
+ if (unlikely(master->debug_level > 1)) {
+ cycles_end = get_cycles();
+ EC_MASTER_DBG(master, 0, "%s()"
+ " sent %u frames in %uus.\n", __func__, frame_count,
+ (unsigned int) (cycles_end - cycles_start) * 1000 / cpu_khz);
+ }
+#endif
+}
+
+/****************************************************************************/
+
+/** Processes a received frame.
+ *
+ * This function is called by the network driver for every received frame.
+ *
+ * \return 0 in case of success, else < 0
+ */
+void ec_master_receive_datagrams(
+ ec_master_t *master, /**< EtherCAT master */
+ ec_device_t *device, /**< EtherCAT device */
+ const uint8_t *frame_data, /**< frame data */
+ size_t size /**< size of the received data */
+ )
+{
+ size_t frame_size, data_size;
+ uint8_t datagram_type, datagram_index;
+ unsigned int cmd_follows, matched;
+ const uint8_t *cur_data;
+ ec_datagram_t *datagram;
+
+ if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
+ if (master->debug_level || FORCE_OUTPUT_CORRUPTED) {
+ EC_MASTER_DBG(master, 0, "Corrupted frame received"
+ " on %s (size %zu < %u byte):\n",
+ device->dev->name, size, EC_FRAME_HEADER_SIZE);
+ ec_print_data(frame_data, size);
+ }
+ master->stats.corrupted++;
+#ifdef EC_RT_SYSLOG
+ ec_master_output_stats(master);
+#endif
+ return;
+ }
+
+ cur_data = frame_data;
+
+ // check length of entire frame
+ frame_size = EC_READ_U16(cur_data) & 0x07FF;
+ cur_data += EC_FRAME_HEADER_SIZE;
+
+ if (unlikely(frame_size > size)) {
+ if (master->debug_level || FORCE_OUTPUT_CORRUPTED) {
+ EC_MASTER_DBG(master, 0, "Corrupted frame received"
+ " on %s (invalid frame size %zu for "
+ "received size %zu):\n", device->dev->name,
+ frame_size, size);
+ ec_print_data(frame_data, size);
+ }
+ master->stats.corrupted++;
+#ifdef EC_RT_SYSLOG
+ ec_master_output_stats(master);
+#endif
+ return;
+ }
+
+ cmd_follows = 1;
+ while (cmd_follows) {
+ // process datagram header
+ datagram_type = EC_READ_U8 (cur_data);
+ datagram_index = EC_READ_U8 (cur_data + 1);
+ data_size = EC_READ_U16(cur_data + 6) & 0x07FF;
+ cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000;
+ cur_data += EC_DATAGRAM_HEADER_SIZE;
+
+ if (unlikely(cur_data - frame_data
+ + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) {
+ if (master->debug_level || FORCE_OUTPUT_CORRUPTED) {
+ EC_MASTER_DBG(master, 0, "Corrupted frame received"
+ " on %s (invalid data size %zu):\n",
+ device->dev->name, data_size);
+ ec_print_data(frame_data, size);
+ }
+ master->stats.corrupted++;
+#ifdef EC_RT_SYSLOG
+ ec_master_output_stats(master);
+#endif
+ return;
+ }
+
+ // search for matching datagram in the queue
+ matched = 0;
+ list_for_each_entry(datagram, &master->datagram_queue, queue) {
+ if (datagram->index == datagram_index
+ && datagram->state == EC_DATAGRAM_SENT
+ && datagram->type == datagram_type
+ && datagram->data_size == data_size) {
+ matched = 1;
+ break;
+ }
+ }
+
+ // no matching datagram was found
+ if (!matched) {
+ master->stats.unmatched++;
+#ifdef EC_RT_SYSLOG
+ ec_master_output_stats(master);
+#endif
+
+ if (unlikely(master->debug_level > 0)) {
+ EC_MASTER_DBG(master, 0, "UNMATCHED datagram:\n");
+ ec_print_data(cur_data - EC_DATAGRAM_HEADER_SIZE,
+ EC_DATAGRAM_HEADER_SIZE + data_size
+ + EC_DATAGRAM_FOOTER_SIZE);
+#ifdef EC_DEBUG_RING
+ ec_device_debug_ring_print(&master->devices[EC_DEVICE_MAIN]);
+#endif
+ }
+
+ cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
+ continue;
+ }
+
+ if (datagram->type != EC_DATAGRAM_APWR &&
+ datagram->type != EC_DATAGRAM_FPWR &&
+ datagram->type != EC_DATAGRAM_BWR &&
+ datagram->type != EC_DATAGRAM_LWR) {
+ // copy received data into the datagram memory,
+ // if something has been read
+ memcpy(datagram->data, cur_data, data_size);
+ }
+ cur_data += data_size;
+
+ // set the datagram's working counter
+ datagram->working_counter = EC_READ_U16(cur_data);
+ cur_data += EC_DATAGRAM_FOOTER_SIZE;
+
+ // dequeue the received datagram
+ datagram->state = EC_DATAGRAM_RECEIVED;
+#ifdef EC_HAVE_CYCLES
+ datagram->cycles_received =
+ master->devices[EC_DEVICE_MAIN].cycles_poll;
+#endif
+ datagram->jiffies_received =
+ master->devices[EC_DEVICE_MAIN].jiffies_poll;
+ list_del_init(&datagram->queue);
+ }
+}
+
+/****************************************************************************/
+
+/** Output master statistics.
+ *
+ * This function outputs statistical data on demand, but not more often than
+ * necessary. The output happens at most once a second.
+ */
+void ec_master_output_stats(ec_master_t *master /**< EtherCAT master */)
+{
+ if (unlikely(jiffies - master->stats.output_jiffies >= HZ)) {
+ master->stats.output_jiffies = jiffies;
+
+ if (master->stats.timeouts) {
+ EC_MASTER_WARN(master, "%u datagram%s TIMED OUT!\n",
+ master->stats.timeouts,
+ master->stats.timeouts == 1 ? "" : "s");
+ master->stats.timeouts = 0;
+ }
+ if (master->stats.corrupted) {
+ EC_MASTER_WARN(master, "%u frame%s CORRUPTED!\n",
+ master->stats.corrupted,
+ master->stats.corrupted == 1 ? "" : "s");
+ master->stats.corrupted = 0;
+ }
+ if (master->stats.unmatched) {
+ EC_MASTER_WARN(master, "%u datagram%s UNMATCHED!\n",
+ master->stats.unmatched,
+ master->stats.unmatched == 1 ? "" : "s");
+ master->stats.unmatched = 0;
+ }
+ }
+}
+
+/****************************************************************************/
+
+/** Clears the common device statistics.
+ */
+void ec_master_clear_device_stats(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ unsigned int i;
+
+ // zero frame statistics
+ master->device_stats.tx_count = 0;
+ master->device_stats.last_tx_count = 0;
+ master->device_stats.rx_count = 0;
+ master->device_stats.last_rx_count = 0;
+ master->device_stats.tx_bytes = 0;
+ master->device_stats.last_tx_bytes = 0;
+ master->device_stats.rx_bytes = 0;
+ master->device_stats.last_rx_bytes = 0;
+ master->device_stats.last_loss = 0;
+
+ for (i = 0; i < EC_RATE_COUNT; i++) {
+ master->device_stats.tx_frame_rates[i] = 0;
+ master->device_stats.rx_frame_rates[i] = 0;
+ master->device_stats.tx_byte_rates[i] = 0;
+ master->device_stats.rx_byte_rates[i] = 0;
+ master->device_stats.loss_rates[i] = 0;
+ }
+
+ master->device_stats.jiffies = 0;
+}
+
+/****************************************************************************/
+
+/** Updates the common device statistics.
+ */
+void ec_master_update_device_stats(
+ ec_master_t *master /**< EtherCAT master */
+ )
+{
+ ec_device_stats_t *s = &master->device_stats;
+ s32 tx_frame_rate, rx_frame_rate, tx_byte_rate, rx_byte_rate, loss_rate;
+ u64 loss;
+ unsigned int i, dev_idx;
+
+ // frame statistics
+ if (likely(jiffies - s->jiffies < HZ)) {
+ return;
+ }
+
+ tx_frame_rate = (s->tx_count - s->last_tx_count) * 1000;
+ rx_frame_rate = (s->rx_count - s->last_rx_count) * 1000;
+ tx_byte_rate = s->tx_bytes - s->last_tx_bytes;
+ rx_byte_rate = s->rx_bytes - s->last_rx_bytes;
+ loss = s->tx_count - s->rx_count;
+ loss_rate = (loss - s->last_loss) * 1000;
+
+ /* Low-pass filter:
+ * Y_n = y_(n - 1) + T / tau * (x - y_(n - 1)) | T = 1
+ * -> Y_n += (x - y_(n - 1)) / tau
+ */
+ for (i = 0; i < EC_RATE_COUNT; i++) {
+ s32 n = rate_intervals[i];
+ s->tx_frame_rates[i] += (tx_frame_rate - s->tx_frame_rates[i]) / n;
+ s->rx_frame_rates[i] += (rx_frame_rate - s->rx_frame_rates[i]) / n;
+ s->tx_byte_rates[i] += (tx_byte_rate - s->tx_byte_rates[i]) / n;
+ s->rx_byte_rates[i] += (rx_byte_rate - s->rx_byte_rates[i]) / n;
+ s->loss_rates[i] += (loss_rate - s->loss_rates[i]) / n;
+ }
+
+ s->last_tx_count = s->tx_count;
+ s->last_rx_count = s->rx_count;
+ s->last_tx_bytes = s->tx_bytes;
+ s->last_rx_bytes = s->rx_bytes;
+ s->last_loss = loss;
+
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ ec_device_update_stats(&master->devices[dev_idx]);
+ }
+
+ s->jiffies = jiffies;
+}
+
+/****************************************************************************/
+
+#ifdef EC_USE_HRTIMER
+
+/*
+ * Sleep related functions:
+ */
+static enum hrtimer_restart ec_master_nanosleep_wakeup(struct hrtimer *timer)
+{
+ struct hrtimer_sleeper *t =
+ container_of(timer, struct hrtimer_sleeper, timer);
+ struct task_struct *task = t->task;
+
+ t->task = NULL;
+ if (task)
+ wake_up_process(task);
+
+ return HRTIMER_NORESTART;
+}
+
+/****************************************************************************/
+
+void ec_master_nanosleep(const unsigned long nsecs)
+{
+ struct hrtimer_sleeper t;
+ enum hrtimer_mode mode = HRTIMER_MODE_REL;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(6, 15, 0)
+ hrtimer_setup(&t.timer, ec_master_nanosleep_wakeup,
+ CLOCK_MONOTONIC, mode);
+#else
+ hrtimer_init(&t.timer, CLOCK_MONOTONIC, mode);
+ t.timer.function = ec_master_nanosleep_wakeup;
+#endif
+ t.task = current;
+ hrtimer_set_expires(&t.timer, ktime_set(0, nsecs));
+
+ do {
+ set_current_state(TASK_INTERRUPTIBLE);
+ hrtimer_start(&t.timer, hrtimer_get_expires(&t.timer), mode);
+
+ if (likely(t.task))
+ schedule();
+
+ hrtimer_cancel(&t.timer);
+ mode = HRTIMER_MODE_ABS;
+
+ } while (t.task && !signal_pending(current));
+}
+
+#endif // EC_USE_HRTIMER
+
+/****************************************************************************/
+
+/** Execute slave FSMs.
+ */
+void ec_master_exec_slave_fsms(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ ec_datagram_t *datagram;
+ ec_fsm_slave_t *fsm, *next;
+ unsigned int count = 0;
+
+ list_for_each_entry_safe(fsm, next, &master->fsm_exec_list, list) {
+ if (!fsm->datagram) {
+ EC_MASTER_WARN(master, "Slave %u FSM has zero datagram."
+ "This is a bug!\n", fsm->slave->ring_position);
+ list_del_init(&fsm->list);
+ master->fsm_exec_count--;
+ return;
+ }
+
+ if (fsm->datagram->state == EC_DATAGRAM_INIT ||
+ fsm->datagram->state == EC_DATAGRAM_QUEUED ||
+ fsm->datagram->state == EC_DATAGRAM_SENT) {
+ // previous datagram was not sent or received yet.
+ // wait until next thread execution
+ return;
+ }
+
+ datagram = ec_master_get_external_datagram(master);
+ if (!datagram) {
+ // no free datagrams at the moment
+ EC_MASTER_WARN(master, "No free datagram during"
+ " slave FSM execution. This is a bug!\n");
+ continue;
+ }
+
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "Executing slave %u FSM.\n",
+ fsm->slave->ring_position);
+#endif
+ if (ec_fsm_slave_exec(fsm, datagram)) {
+ // FSM consumed datagram
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "FSM consumed datagram %s\n",
+ datagram->name);
+#endif
+ master->ext_ring_idx_fsm =
+ (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
+ }
+ else {
+ // FSM finished
+ list_del_init(&fsm->list);
+ master->fsm_exec_count--;
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "FSM finished. %u remaining.\n",
+ master->fsm_exec_count);
+#endif
+ }
+ }
+
+ while (master->fsm_exec_count < EC_EXT_RING_SIZE / 2
+ && count < master->slave_count) {
+
+ if (ec_fsm_slave_is_ready(&master->fsm_slave->fsm)) {
+ datagram = ec_master_get_external_datagram(master);
+
+ if (ec_fsm_slave_exec(&master->fsm_slave->fsm, datagram)) {
+ master->ext_ring_idx_fsm =
+ (master->ext_ring_idx_fsm + 1) % EC_EXT_RING_SIZE;
+ list_add_tail(&master->fsm_slave->fsm.list,
+ &master->fsm_exec_list);
+ master->fsm_exec_count++;
+#if DEBUG_INJECT
+ EC_MASTER_DBG(master, 1, "New slave %u FSM"
+ " consumed datagram %s, now %u FSMs in list.\n",
+ master->fsm_slave->ring_position, datagram->name,
+ master->fsm_exec_count);
+#endif
+ }
+ }
+
+ master->fsm_slave++;
+ if (master->fsm_slave >= master->slaves + master->slave_count) {
+ master->fsm_slave = master->slaves;
+ }
+ count++;
+ }
+}
+
+/****************************************************************************/
+
+/** Master kernel thread function for IDLE phase.
+ */
+static int ec_master_idle_thread(void *priv_data)
+{
+ ec_master_t *master = (ec_master_t *) priv_data;
+ int fsm_exec;
+#ifdef EC_USE_HRTIMER
+ size_t sent_bytes;
+#endif
+
+ // send interval in IDLE phase
+ ec_master_set_send_interval(master, 1000000 / HZ);
+
+ EC_MASTER_DBG(master, 1, "Idle thread running with send interval = %u us,"
+ " max data size=%zu\n", master->send_interval,
+ master->max_queue_size);
+
+ while (!kthread_should_stop()) {
+ ec_datagram_output_stats(&master->fsm_datagram);
+
+ // receive
+ if (ec_rt_lock_interruptible(&master->io_mutex))
+ break;
+ ecrt_master_receive(master);
+ rt_mutex_unlock(&master->io_mutex);
+
+ // execute master & slave state machines
+ if (down_interruptible(&master->master_sem)) {
+ break;
+ }
+
+ fsm_exec = ec_fsm_master_exec(&master->fsm);
+
+ ec_master_exec_slave_fsms(master);
+
+ up(&master->master_sem);
+
+ // queue and send
+ if (ec_rt_lock_interruptible(&master->io_mutex))
+ break;
+ if (fsm_exec) {
+ ec_master_queue_datagram(master, &master->fsm_datagram);
+ }
+ ecrt_master_send(master);
+#ifdef EC_USE_HRTIMER
+ sent_bytes = master->devices[EC_DEVICE_MAIN].tx_skb[
+ master->devices[EC_DEVICE_MAIN].tx_ring_index]->len;
+#endif
+ rt_mutex_unlock(&master->io_mutex);
+
+ if (ec_fsm_master_idle(&master->fsm)) {
+#ifdef EC_USE_HRTIMER
+ ec_master_nanosleep(master->send_interval * 1000);
+#else
+ set_current_state(TASK_INTERRUPTIBLE);
+ schedule_timeout(1);
+#endif
+ } else {
+#ifdef EC_USE_HRTIMER
+ ec_master_nanosleep(sent_bytes * EC_BYTE_TRANSMISSION_TIME_NS);
+#else
+ schedule();
+#endif
+ }
+ }
+
+ EC_MASTER_DBG(master, 1, "Master IDLE thread exiting...\n");
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Master kernel thread function for OPERATION phase.
+ */
+static int ec_master_operation_thread(void *priv_data)
+{
+ ec_master_t *master = (ec_master_t *) priv_data;
+
+ EC_MASTER_DBG(master, 1, "Operation thread running"
+ " with fsm interval = %u us, max data size=%zu\n",
+ master->send_interval, master->max_queue_size);
+
+ while (!kthread_should_stop()) {
+ ec_datagram_output_stats(&master->fsm_datagram);
+
+ if (master->injection_seq_rt == master->injection_seq_fsm) {
+ // output statistics
+ ec_master_output_stats(master);
+
+ // execute master & slave state machines
+ if (down_interruptible(&master->master_sem)) {
+ break;
+ }
+
+ if (ec_fsm_master_exec(&master->fsm)) {
+ // Inject datagrams (let the RT thread queue them, see
+ // ecrt_master_send())
+ master->injection_seq_fsm++;
+ }
+
+ ec_master_exec_slave_fsms(master);
+
+ up(&master->master_sem);
+ }
+
+#ifdef EC_USE_HRTIMER
+ // the op thread should not work faster than the sending RT thread
+ ec_master_nanosleep(master->send_interval * 1000);
+#else
+ if (ec_fsm_master_idle(&master->fsm)) {
+ set_current_state(TASK_INTERRUPTIBLE);
+ schedule_timeout(1);
+ }
+ else {
+ schedule();
+ }
+#endif
+ }
+
+ EC_MASTER_DBG(master, 1, "Master OP thread exiting...\n");
+ return 0;
+}
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+
+/* compatibility for priority changes */
+static inline void set_normal_priority(struct task_struct *p, int nice)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 9, 0)
+ sched_set_normal(p, nice);
+#else
+ struct sched_param param = { .sched_priority = 0 };
+ sched_setscheduler(p, SCHED_NORMAL, ¶m);
+ set_user_nice(p, nice);
+#endif
+}
+
+/****************************************************************************/
+
+/** Starts Ethernet over EtherCAT processing on demand.
+ */
+void ec_master_eoe_start(ec_master_t *master /**< EtherCAT master */)
+{
+ if (master->eoe_thread) {
+ EC_MASTER_WARN(master, "EoE already running!\n");
+ return;
+ }
+
+ if (list_empty(&master->eoe_handlers))
+ return;
+
+ if (!master->send_cb || !master->receive_cb) {
+ EC_MASTER_WARN(master, "No EoE processing"
+ " because of missing callbacks!\n");
+ return;
+ }
+
+ EC_MASTER_INFO(master, "Starting EoE thread.\n");
+ master->eoe_thread = kthread_run(ec_master_eoe_thread, master,
+ "EtherCAT-EoE");
+ if (IS_ERR(master->eoe_thread)) {
+ int err = (int) PTR_ERR(master->eoe_thread);
+ EC_MASTER_ERR(master, "Failed to start EoE thread (error %i)!\n",
+ err);
+ master->eoe_thread = NULL;
+ return;
+ }
+
+ set_normal_priority(master->eoe_thread, 0);
+}
+
+/****************************************************************************/
+
+/** Stops the Ethernet over EtherCAT processing.
+ */
+void ec_master_eoe_stop(ec_master_t *master /**< EtherCAT master */)
+{
+ if (master->eoe_thread) {
+ EC_MASTER_INFO(master, "Stopping EoE thread.\n");
+
+ kthread_stop(master->eoe_thread);
+ master->eoe_thread = NULL;
+ EC_MASTER_INFO(master, "EoE thread exited.\n");
+ }
+}
+
+/****************************************************************************/
+
+/** Does the Ethernet over EtherCAT processing.
+ */
+static int ec_master_eoe_thread(void *priv_data)
+{
+ ec_master_t *master = (ec_master_t *) priv_data;
+ ec_eoe_t *eoe;
+ unsigned int none_open, sth_to_send, all_idle;
+
+ EC_MASTER_DBG(master, 1, "EoE thread running.\n");
+
+ while (!kthread_should_stop()) {
+ none_open = 1;
+ all_idle = 1;
+
+ list_for_each_entry(eoe, &master->eoe_handlers, list) {
+ if (ec_eoe_is_open(eoe)) {
+ none_open = 0;
+ break;
+ }
+ }
+ if (none_open)
+ goto schedule;
+
+ // receive datagrams
+ master->receive_cb(master->cb_data);
+
+ // actual EoE processing
+ sth_to_send = 0;
+ list_for_each_entry(eoe, &master->eoe_handlers, list) {
+ ec_eoe_run(eoe);
+ if (eoe->queue_datagram) {
+ sth_to_send = 1;
+ }
+ if (!ec_eoe_is_idle(eoe)) {
+ all_idle = 0;
+ }
+ }
+
+ if (sth_to_send) {
+ list_for_each_entry(eoe, &master->eoe_handlers, list) {
+ ec_eoe_queue(eoe);
+ }
+ // (try to) send datagrams
+ master->send_cb(master->cb_data);
+ }
+
+schedule:
+ if (all_idle) {
+ set_current_state(TASK_INTERRUPTIBLE);
+ schedule_timeout(1);
+ } else {
+ schedule();
+ }
+ }
+
+ EC_MASTER_DBG(master, 1, "EoE thread exiting...\n");
+ return 0;
+}
+
+#endif
+
+/****************************************************************************/
+
+/** Attaches the slave configurations to the slaves.
+ */
+void ec_master_attach_slave_configs(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ ec_slave_config_t *sc;
+
+ list_for_each_entry(sc, &master->configs, list) {
+ ec_slave_config_attach(sc);
+ }
+}
+
+/****************************************************************************/
+
+/** Common implementation for ec_master_find_slave()
+ * and ec_master_find_slave_const().
+ */
+#define EC_FIND_SLAVE \
+ do { \
+ if (alias) { \
+ for (; slave < master->slaves + master->slave_count; \
+ slave++) { \
+ if (slave->effective_alias == alias) \
+ break; \
+ } \
+ if (slave == master->slaves + master->slave_count) \
+ return NULL; \
+ } \
+ \
+ slave += position; \
+ if (slave < master->slaves + master->slave_count) { \
+ return slave; \
+ } else { \
+ return NULL; \
+ } \
+ } while (0)
+
+/** Finds a slave in the bus, given the alias and position.
+ *
+ * \return Search result, or NULL.
+ */
+ec_slave_t *ec_master_find_slave(
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t alias, /**< Slave alias. */
+ uint16_t position /**< Slave position. */
+ )
+{
+ ec_slave_t *slave = master->slaves;
+ EC_FIND_SLAVE;
+}
+
+/** Finds a slave in the bus, given the alias and position.
+ *
+ * Const version.
+ *
+ * \return Search result, or NULL.
+ */
+const ec_slave_t *ec_master_find_slave_const(
+ const ec_master_t *master, /**< EtherCAT master. */
+ uint16_t alias, /**< Slave alias. */
+ uint16_t position /**< Slave position. */
+ )
+{
+ const ec_slave_t *slave = master->slaves;
+ EC_FIND_SLAVE;
+}
+
+/****************************************************************************/
+
+/** Get the number of slave configurations provided by the application.
+ *
+ * \return Number of configurations.
+ */
+unsigned int ec_master_config_count(
+ const ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ const ec_slave_config_t *sc;
+ unsigned int count = 0;
+
+ list_for_each_entry(sc, &master->configs, list) {
+ count++;
+ }
+
+ return count;
+}
+
+/****************************************************************************/
+
+/** Common implementation for ec_master_get_config()
+ * and ec_master_get_config_const().
+ */
+#define EC_FIND_CONFIG \
+ do { \
+ list_for_each_entry(sc, &master->configs, list) { \
+ if (pos--) \
+ continue; \
+ return sc; \
+ } \
+ return NULL; \
+ } while (0)
+
+/** Get a slave configuration via its position in the list.
+ *
+ * \return Slave configuration or \a NULL.
+ */
+ec_slave_config_t *ec_master_get_config(
+ const ec_master_t *master, /**< EtherCAT master. */
+ unsigned int pos /**< List position. */
+ )
+{
+ ec_slave_config_t *sc;
+ EC_FIND_CONFIG;
+}
+
+/** Get a slave configuration via its position in the list.
+ *
+ * Const version.
+ *
+ * \return Slave configuration or \a NULL.
+ */
+const ec_slave_config_t *ec_master_get_config_const(
+ const ec_master_t *master, /**< EtherCAT master. */
+ unsigned int pos /**< List position. */
+ )
+{
+ const ec_slave_config_t *sc;
+ EC_FIND_CONFIG;
+}
+
+/****************************************************************************/
+
+/** Get the number of domains.
+ *
+ * \return Number of domains.
+ */
+unsigned int ec_master_domain_count(
+ const ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ const ec_domain_t *domain;
+ unsigned int count = 0;
+
+ list_for_each_entry(domain, &master->domains, list) {
+ count++;
+ }
+
+ return count;
+}
+
+/****************************************************************************/
+
+/** Common implementation for ec_master_find_domain() and
+ * ec_master_find_domain_const().
+ */
+#define EC_FIND_DOMAIN \
+ do { \
+ list_for_each_entry(domain, &master->domains, list) { \
+ if (index--) \
+ continue; \
+ return domain; \
+ } \
+ \
+ return NULL; \
+ } while (0)
+
+/** Get a domain via its position in the list.
+ *
+ * \return Domain pointer, or \a NULL if not found.
+ */
+ec_domain_t *ec_master_find_domain(
+ ec_master_t *master, /**< EtherCAT master. */
+ unsigned int index /**< Domain index. */
+ )
+{
+ ec_domain_t *domain;
+ EC_FIND_DOMAIN;
+}
+
+/** Get a domain via its position in the list.
+ *
+ * Const version.
+ *
+ * \return Domain pointer, or \a NULL if not found.
+ */
+const ec_domain_t *ec_master_find_domain_const(
+ const ec_master_t *master, /**< EtherCAT master. */
+ unsigned int index /**< Domain index. */
+ )
+{
+ const ec_domain_t *domain;
+ EC_FIND_DOMAIN;
+}
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+
+/** Get the number of EoE handlers.
+ *
+ * \return Number of EoE handlers.
+ */
+uint16_t ec_master_eoe_handler_count(
+ const ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ const ec_eoe_t *eoe;
+ unsigned int count = 0;
+
+ list_for_each_entry(eoe, &master->eoe_handlers, list) {
+ count++;
+ }
+
+ return count;
+}
+
+/****************************************************************************/
+
+/** Get an EoE handler via its position in the list.
+ *
+ * Const version.
+ *
+ * \return EoE handler pointer, or \a NULL if not found.
+ */
+const ec_eoe_t *ec_master_get_eoe_handler_const(
+ const ec_master_t *master, /**< EtherCAT master. */
+ uint16_t index /**< EoE handler index. */
+ )
+{
+ const ec_eoe_t *eoe;
+
+ list_for_each_entry(eoe, &master->eoe_handlers, list) {
+ if (index--)
+ continue;
+ return eoe;
+ }
+
+ return NULL;
+}
+
+#endif
+
+/****************************************************************************/
+
+/** Set the debug level.
+ *
+ * \retval 0 Success.
+ * \retval -EINVAL Invalid debug level.
+ */
+int ec_master_debug_level(
+ ec_master_t *master, /**< EtherCAT master. */
+ unsigned int level /**< Debug level. May be 0, 1 or 2. */
+ )
+{
+ if (level > 2) {
+ EC_MASTER_ERR(master, "Invalid debug level %u!\n", level);
+ return -EINVAL;
+ }
+
+ if (level != master->debug_level) {
+ master->debug_level = level;
+ EC_MASTER_INFO(master, "Master debug level set to %u.\n",
+ master->debug_level);
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Finds the DC reference clock.
+ */
+void ec_master_find_dc_ref_clock(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ ec_slave_t *slave, *ref = NULL;
+
+ if (master->dc_ref_config) {
+ // Use application-selected reference clock
+ slave = master->dc_ref_config->slave;
+
+ if (slave) {
+ if (slave->base_dc_supported && slave->has_dc_system_time) {
+ ref = slave;
+ }
+ else {
+ EC_MASTER_WARN(master, "Slave %u can not act as a"
+ " DC reference clock!", slave->ring_position);
+ }
+ }
+ else {
+ EC_MASTER_WARN(master, "DC reference clock config (%u-%u)"
+ " has no slave attached!\n", master->dc_ref_config->alias,
+ master->dc_ref_config->position);
+ }
+ }
+ else {
+ // Use first slave with DC support as reference clock
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ if (slave->base_dc_supported && slave->has_dc_system_time) {
+ ref = slave;
+ break;
+ }
+ }
+
+ }
+
+ master->dc_ref_clock = ref;
+
+ if (ref) {
+ EC_MASTER_INFO(master, "Using slave %u as DC reference clock.\n",
+ ref->ring_position);
+ }
+ else {
+ EC_MASTER_INFO(master, "No DC reference clock found.\n");
+ }
+
+ // These calls always succeed, because the
+ // datagrams have been pre-allocated.
+ ec_datagram_fpwr(&master->ref_sync_datagram,
+ ref ? ref->station_address : 0xffff, 0x0910, 4);
+ ec_datagram_frmw(&master->sync_datagram,
+ ref ? ref->station_address : 0xffff, 0x0910, 4);
+}
+
+/****************************************************************************/
+
+/** Calculates the bus topology; recursion function.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_master_calc_topology_rec(
+ ec_master_t *master, /**< EtherCAT master. */
+ ec_slave_t *port0_slave, /**< Slave at port 0. */
+ unsigned int *slave_position /**< Slave position. */
+ )
+{
+ ec_slave_t *slave = master->slaves + *slave_position;
+ unsigned int port_index;
+ int ret;
+
+ static const unsigned int next_table[EC_MAX_PORTS] = {
+ 3, 2, 0, 1
+ };
+
+ slave->ports[0].next_slave = port0_slave;
+
+ port_index = 3;
+ while (port_index != 0) {
+ if (!slave->ports[port_index].link.loop_closed) {
+ *slave_position = *slave_position + 1;
+ if (*slave_position < master->slave_count) {
+ slave->ports[port_index].next_slave =
+ master->slaves + *slave_position;
+ ret = ec_master_calc_topology_rec(master,
+ slave, slave_position);
+ if (ret) {
+ return ret;
+ }
+ } else {
+ return -1;
+ }
+ }
+
+ port_index = next_table[port_index];
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Calculates the bus topology.
+ */
+void ec_master_calc_topology(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ unsigned int slave_position = 0;
+
+ if (master->slave_count == 0)
+ return;
+
+ if (ec_master_calc_topology_rec(master, NULL, &slave_position))
+ EC_MASTER_ERR(master, "Failed to calculate bus topology.\n");
+}
+
+/****************************************************************************/
+
+/** Calculates the bus transmission delays.
+ */
+void ec_master_calc_transmission_delays(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ ec_slave_t *slave;
+
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+ ec_slave_calc_port_delays(slave);
+ }
+
+ if (master->dc_ref_clock) {
+ uint32_t delay = 0;
+ ec_slave_calc_transmission_delays_rec(master->dc_ref_clock, &delay);
+ }
+}
+
+/****************************************************************************/
+
+/** Distributed-clocks calculations.
+ */
+void ec_master_calc_dc(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ // find DC reference clock
+ ec_master_find_dc_ref_clock(master);
+
+ // calculate bus topology
+ ec_master_calc_topology(master);
+
+ ec_master_calc_transmission_delays(master);
+}
+
+/****************************************************************************/
+
+/** Request OP state for configured slaves.
+ */
+void ec_master_request_op(
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ unsigned int i;
+ ec_slave_t *slave;
+
+ if (!master->active)
+ return;
+
+ EC_MASTER_DBG(master, 1, "Requesting OP...\n");
+
+ // request OP for all configured slaves
+ for (i = 0; i < master->slave_count; i++) {
+ slave = master->slaves + i;
+ if (slave->config) {
+ ec_slave_request_state(slave, EC_SLAVE_STATE_OP);
+ }
+ }
+
+ // always set DC reference clock to OP
+ if (master->dc_ref_clock) {
+ ec_slave_request_state(master->dc_ref_clock, EC_SLAVE_STATE_OP);
+ }
+}
+
+/*****************************************************************************
+ * Application interface
+ ****************************************************************************/
+
+/** Same as ecrt_master_create_domain(), but with ERR_PTR() return value.
+ *
+ * \return New domain, or ERR_PTR() return value.
+ */
+ec_domain_t *ecrt_master_create_domain_err(
+ ec_master_t *master /**< master */
+ )
+{
+ ec_domain_t *domain, *last_domain;
+ unsigned int index;
+
+ EC_MASTER_DBG(master, 1, "ecrt_master_create_domain(master = 0x%p)\n",
+ master);
+
+ if (!(domain =
+ (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
+ EC_MASTER_ERR(master, "Error allocating domain memory!\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ down(&master->master_sem);
+
+ if (list_empty(&master->domains)) {
+ index = 0;
+ } else {
+ last_domain = list_entry(master->domains.prev, ec_domain_t, list);
+ index = last_domain->index + 1;
+ }
+
+ ec_domain_init(domain, master, index);
+ list_add_tail(&domain->list, &master->domains);
+
+ up(&master->master_sem);
+
+ EC_MASTER_DBG(master, 1, "Created domain %u.\n", domain->index);
+
+ return domain;
+}
+
+/****************************************************************************/
+
+ec_domain_t *ecrt_master_create_domain(
+ ec_master_t *master /**< master */
+ )
+{
+ ec_domain_t *d = ecrt_master_create_domain_err(master);
+ return IS_ERR(d) ? NULL : d;
+}
+
+/****************************************************************************/
+
+int ecrt_master_activate(ec_master_t *master)
+{
+ uint32_t domain_offset;
+ ec_domain_t *domain;
+ int ret;
+#ifdef EC_EOE
+ int eoe_was_running;
+#endif
+
+ EC_MASTER_DBG(master, 1, "ecrt_master_activate(master = 0x%p)\n", master);
+
+ if (master->active) {
+ EC_MASTER_WARN(master, "%s: Master already active!\n", __func__);
+ return 0;
+ }
+
+ down(&master->master_sem);
+
+ // finish all domains
+ domain_offset = 0;
+ list_for_each_entry(domain, &master->domains, list) {
+ ret = ec_domain_finish(domain, domain_offset);
+ if (ret < 0) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Failed to finish domain 0x%p!\n", domain);
+ return ret;
+ }
+ domain_offset += domain->data_size;
+ }
+
+ up(&master->master_sem);
+
+ // restart EoE process and master thread with new locking
+
+ ec_master_thread_stop(master);
+#ifdef EC_EOE
+ eoe_was_running = master->eoe_thread != NULL;
+ ec_master_eoe_stop(master);
+#endif
+
+ EC_MASTER_DBG(master, 1, "FSM datagram is %p.\n", &master->fsm_datagram);
+
+ master->injection_seq_fsm = 0;
+ master->injection_seq_rt = 0;
+
+ master->send_cb = master->app_send_cb;
+ master->receive_cb = master->app_receive_cb;
+ master->cb_data = master->app_cb_data;
+
+#ifdef EC_EOE
+ if (eoe_was_running) {
+ ec_master_eoe_start(master);
+ }
+#endif
+ ret = ec_master_thread_start(master, ec_master_operation_thread,
+ "EtherCAT-OP");
+ if (ret < 0) {
+ EC_MASTER_ERR(master, "Failed to start master thread!\n");
+ return ret;
+ }
+
+ /* Allow scanning after a topology change. */
+ master->allow_scan = 1;
+
+ master->active = 1;
+
+ // notify state machine, that the configuration shall now be applied
+ master->config_changed = 1;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_deactivate(ec_master_t *master)
+{
+ ec_slave_t *slave;
+#ifdef EC_EOE
+ ec_eoe_t *eoe;
+ int eoe_was_running;
+#endif
+
+ EC_MASTER_DBG(master, 1, "%s(master = 0x%p)\n", __func__, master);
+
+ if (!master->active) {
+ EC_MASTER_WARN(master, "%s: Master not active.\n", __func__);
+ return -EINVAL;
+ }
+
+ ec_master_thread_stop(master);
+#ifdef EC_EOE
+ eoe_was_running = master->eoe_thread != NULL;
+ ec_master_eoe_stop(master);
+#endif
+
+ master->send_cb = ec_master_internal_send_cb;
+ master->receive_cb = ec_master_internal_receive_cb;
+ master->cb_data = master;
+
+ ec_master_clear_config(master);
+
+ for (slave = master->slaves;
+ slave < master->slaves + master->slave_count;
+ slave++) {
+
+ // set states for all slaves
+ ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
+
+ // mark for reconfiguration, because the master could have no
+ // possibility for a reconfiguration between two sequential operation
+ // phases.
+ slave->force_config = 1;
+ }
+
+#ifdef EC_EOE
+ // ... but leave EoE slaves in OP
+ list_for_each_entry(eoe, &master->eoe_handlers, list) {
+ if (ec_eoe_is_open(eoe))
+ ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP);
+ }
+#endif
+
+ master->app_time = 0ULL;
+ master->dc_ref_time = 0ULL;
+
+#ifdef EC_EOE
+ if (eoe_was_running) {
+ ec_master_eoe_start(master);
+ }
+#endif
+ if (ec_master_thread_start(master, ec_master_idle_thread,
+ "EtherCAT-IDLE")) {
+ EC_MASTER_WARN(master, "Failed to restart master thread!\n");
+ }
+
+ /* Disallow scanning to get into the same state like after a master
+ * request (after ec_master_enter_operation_phase() is called). */
+ master->allow_scan = 0;
+
+ master->active = 0;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_send(ec_master_t *master)
+{
+ ec_datagram_t *datagram, *n;
+ ec_device_index_t dev_idx;
+
+ if (master->injection_seq_rt != master->injection_seq_fsm) {
+ // inject datagram produced by master FSM
+ ec_master_queue_datagram(master, &master->fsm_datagram);
+ master->injection_seq_rt = master->injection_seq_fsm;
+ }
+
+ ec_master_inject_external_datagrams(master);
+
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ if (unlikely(!master->devices[dev_idx].link_state)) {
+ // link is down, no datagram can be sent
+ list_for_each_entry_safe(datagram, n,
+ &master->datagram_queue, queue) {
+ if (datagram->device_index == dev_idx) {
+ datagram->state = EC_DATAGRAM_ERROR;
+ list_del_init(&datagram->queue);
+ }
+ }
+
+ if (!master->devices[dev_idx].dev) {
+ continue;
+ }
+
+ // query link state
+ ec_device_poll(&master->devices[dev_idx]);
+
+ // clear frame statistics
+ ec_device_clear_stats(&master->devices[dev_idx]);
+ continue;
+ }
+
+ // send frames
+ ec_master_send_datagrams(master, dev_idx);
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_receive(ec_master_t *master)
+{
+ unsigned int dev_idx;
+ ec_datagram_t *datagram, *next;
+
+ // receive datagrams
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ ec_device_poll(&master->devices[dev_idx]);
+ }
+ ec_master_update_device_stats(master);
+
+ // dequeue all datagrams that timed out
+ list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) {
+ if (datagram->state != EC_DATAGRAM_SENT) continue;
+
+#ifdef EC_HAVE_CYCLES
+ if (master->devices[EC_DEVICE_MAIN].cycles_poll -
+ datagram->cycles_sent > timeout_cycles) {
+#else
+ if (master->devices[EC_DEVICE_MAIN].jiffies_poll -
+ datagram->jiffies_sent > timeout_jiffies) {
+#endif
+ list_del_init(&datagram->queue);
+ datagram->state = EC_DATAGRAM_TIMED_OUT;
+ master->stats.timeouts++;
+
+#ifdef EC_RT_SYSLOG
+ ec_master_output_stats(master);
+
+ if (unlikely(master->debug_level > 0)) {
+ unsigned int time_us;
+#ifdef EC_HAVE_CYCLES
+ time_us = (unsigned int)
+ (master->devices[EC_DEVICE_MAIN].cycles_poll -
+ datagram->cycles_sent) * 1000 / cpu_khz;
+#else
+ time_us = (unsigned int)
+ ((master->devices[EC_DEVICE_MAIN].jiffies_poll -
+ datagram->jiffies_sent) * 1000000 / HZ);
+#endif
+ EC_MASTER_DBG(master, 0, "TIMED OUT datagram %p,"
+ " index %02X waited %u us.\n",
+ datagram, datagram->index, time_us);
+ }
+#endif /* RT_SYSLOG */
+ }
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_send_ext(ec_master_t *master)
+{
+ ec_datagram_t *datagram, *next;
+
+ if (down_trylock(&master->ext_queue_sem))
+ return -EAGAIN;
+
+ list_for_each_entry_safe(datagram, next, &master->ext_datagram_queue,
+ ext_queue) {
+ list_del_init(&datagram->ext_queue);
+ ec_master_queue_datagram(master, datagram);
+ }
+ up(&master->ext_queue_sem);
+
+ return ecrt_master_send(master);
+}
+
+/****************************************************************************/
+
+/** Same as ecrt_master_slave_config(), but with ERR_PTR() return value.
+ */
+ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *master,
+ uint16_t alias, uint16_t position, uint32_t vendor_id,
+ uint32_t product_code)
+{
+ ec_slave_config_t *sc;
+ unsigned int found = 0;
+
+
+ EC_MASTER_DBG(master, 1, "ecrt_master_slave_config(master = 0x%p,"
+ " alias = %u, position = %u, vendor_id = 0x%08x,"
+ " product_code = 0x%08x)\n",
+ master, alias, position, vendor_id, product_code);
+
+ list_for_each_entry(sc, &master->configs, list) {
+ if (sc->alias == alias && sc->position == position) {
+ found = 1;
+ break;
+ }
+ }
+
+ if (found) { // config with same alias/position already existing
+ if (sc->vendor_id != vendor_id || sc->product_code != product_code) {
+ EC_MASTER_ERR(master, "Slave type mismatch. Slave was"
+ " configured as 0x%08X/0x%08X before. Now configuring"
+ " with 0x%08X/0x%08X.\n", sc->vendor_id, sc->product_code,
+ vendor_id, product_code);
+ return ERR_PTR(-ENOENT);
+ }
+ } else {
+ EC_MASTER_DBG(master, 1, "Creating slave configuration for %u:%u,"
+ " 0x%08X/0x%08X.\n",
+ alias, position, vendor_id, product_code);
+
+ if (!(sc = (ec_slave_config_t *) kmalloc(sizeof(ec_slave_config_t),
+ GFP_KERNEL))) {
+ EC_MASTER_ERR(master, "Failed to allocate memory"
+ " for slave configuration.\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ ec_slave_config_init(sc, master,
+ alias, position, vendor_id, product_code);
+
+ down(&master->master_sem);
+
+ // try to find the addressed slave
+ ec_slave_config_attach(sc);
+ ec_slave_config_load_default_sync_config(sc);
+ list_add_tail(&sc->list, &master->configs);
+
+ up(&master->master_sem);
+ }
+
+ return sc;
+}
+
+/****************************************************************************/
+
+ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
+ uint16_t alias, uint16_t position, uint32_t vendor_id,
+ uint32_t product_code)
+{
+ ec_slave_config_t *sc = ecrt_master_slave_config_err(master, alias,
+ position, vendor_id, product_code);
+ return IS_ERR(sc) ? NULL : sc;
+}
+
+/****************************************************************************/
+
+int ecrt_master_select_reference_clock(ec_master_t *master,
+ ec_slave_config_t *sc)
+{
+ if (sc) {
+ ec_slave_t *slave = sc->slave;
+
+ // output an early warning
+ if (slave &&
+ (!slave->base_dc_supported || !slave->has_dc_system_time)) {
+ EC_MASTER_WARN(master, "Slave %u can not act as"
+ " a reference clock!", slave->ring_position);
+ }
+ }
+
+ master->dc_ref_config = sc;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master(ec_master_t *master, ec_master_info_t *master_info)
+{
+ EC_MASTER_DBG(master, 1, "ecrt_master(master = 0x%p,"
+ " master_info = 0x%p)\n", master, master_info);
+
+ master_info->slave_count = master->slave_count;
+ master_info->link_up = master->devices[EC_DEVICE_MAIN].link_state;
+ master_info->scan_busy = master->scan_busy;
+ master_info->app_time = master->app_time;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_scan_progress(ec_master_t *master,
+ ec_master_scan_progress_t *progress)
+{
+ EC_MASTER_DBG(master, 1, "ecrt_master_scan_progress(master = 0x%p,"
+ " progress = 0x%p)\n", master, progress);
+
+ progress->slave_count = master->slave_count;
+ progress->scan_index = master->scan_index;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
+ ec_slave_info_t *slave_info)
+{
+ const ec_slave_t *slave;
+ unsigned int i;
+ int ret = 0;
+
+ if (down_interruptible(&master->master_sem)) {
+ return -EINTR;
+ }
+
+ slave = ec_master_find_slave_const(master, 0, slave_position);
+
+ if (slave == NULL) {
+ ret = -ENOENT;
+ goto out_get_slave;
+ }
+
+ slave_info->position = slave->ring_position;
+ slave_info->vendor_id = slave->sii.vendor_id;
+ slave_info->product_code = slave->sii.product_code;
+ slave_info->revision_number = slave->sii.revision_number;
+ slave_info->serial_number = slave->sii.serial_number;
+ slave_info->alias = slave->effective_alias;
+ slave_info->current_on_ebus = slave->sii.current_on_ebus;
+
+ for (i = 0; i < EC_MAX_PORTS; i++) {
+ slave_info->ports[i].desc = slave->ports[i].desc;
+ slave_info->ports[i].link.link_up = slave->ports[i].link.link_up;
+ slave_info->ports[i].link.loop_closed =
+ slave->ports[i].link.loop_closed;
+ slave_info->ports[i].link.signal_detected =
+ slave->ports[i].link.signal_detected;
+ slave_info->ports[i].receive_time = slave->ports[i].receive_time;
+ if (slave->ports[i].next_slave) {
+ slave_info->ports[i].next_slave =
+ slave->ports[i].next_slave->ring_position;
+ } else {
+ slave_info->ports[i].next_slave = 0xffff;
+ }
+ slave_info->ports[i].delay_to_next_dc =
+ slave->ports[i].delay_to_next_dc;
+ }
+
+ slave_info->al_state = slave->current_state;
+ slave_info->error_flag = slave->error_flag;
+ slave_info->sync_count = slave->sii.sync_count;
+ slave_info->sdo_count = ec_slave_sdo_count(slave);
+ if (slave->sii.name) {
+ strncpy(slave_info->name, slave->sii.name, EC_MAX_STRING_LENGTH);
+ } else {
+ slave_info->name[0] = 0;
+ }
+
+out_get_slave:
+ up(&master->master_sem);
+
+ return ret;
+}
+
+/****************************************************************************/
+
+void ecrt_master_callbacks(ec_master_t *master,
+ void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data)
+{
+ EC_MASTER_DBG(master, 1, "ecrt_master_callbacks(master = 0x%p,"
+ " send_cb = 0x%p, receive_cb = 0x%p, cb_data = 0x%p)\n",
+ master, send_cb, receive_cb, cb_data);
+
+ master->app_send_cb = send_cb;
+ master->app_receive_cb = receive_cb;
+ master->app_cb_data = cb_data;
+}
+
+/****************************************************************************/
+
+int ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
+{
+ ec_device_index_t dev_idx;
+
+ state->slaves_responding = 0U;
+ state->al_states = 0;
+ state->link_up = 0U;
+
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ /* Announce sum of responding slaves on all links. */
+ state->slaves_responding += master->fsm.slaves_responding[dev_idx];
+
+ /* Binary-or slave states of all links. */
+ state->al_states |= master->fsm.slave_states[dev_idx];
+
+ /* Signal link up if at least one device has link. */
+ state->link_up |= master->devices[dev_idx].link_state;
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_link_state(const ec_master_t *master, unsigned int dev_idx,
+ ec_master_link_state_t *state)
+{
+ if (dev_idx >= ec_master_num_devices(master)) {
+ return -EINVAL;
+ }
+
+ state->slaves_responding = master->fsm.slaves_responding[dev_idx];
+ state->al_states = master->fsm.slave_states[dev_idx];
+ state->link_up = master->devices[dev_idx].link_state;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
+{
+ master->app_time = app_time;
+
+ if (unlikely(!master->dc_ref_time)) {
+ master->dc_ref_time = app_time;
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_reference_clock_time(const ec_master_t *master,
+ uint32_t *time)
+{
+ if (!master->dc_ref_clock) {
+ return -ENXIO;
+ }
+
+ if (master->sync_datagram.state != EC_DATAGRAM_RECEIVED) {
+ return -EIO;
+ }
+
+ // Get returned datagram time, transmission delay removed.
+ *time = EC_READ_U32(master->sync_datagram.data) -
+ master->dc_ref_clock->transmission_delay;
+
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_sync_reference_clock(ec_master_t *master)
+{
+ if (master->dc_ref_clock) {
+ EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time);
+ ec_master_queue_datagram(master, &master->ref_sync_datagram);
+ } else {
+ return -ENXIO;
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_sync_reference_clock_to(
+ ec_master_t *master,
+ uint64_t sync_time
+ )
+{
+ if (master->dc_ref_clock) {
+ EC_WRITE_U32(master->ref_sync_datagram.data, sync_time);
+ ec_master_queue_datagram(master, &master->ref_sync_datagram);
+ } else {
+ return -ENXIO;
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_sync_slave_clocks(ec_master_t *master)
+{
+ if (master->dc_ref_clock) {
+ ec_datagram_zero(&master->sync_datagram);
+ ec_master_queue_datagram(master, &master->sync_datagram);
+ } else {
+ return -ENXIO;
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_master_sync_monitor_queue(ec_master_t *master)
+{
+ ec_datagram_zero(&master->sync_mon_datagram);
+ ec_master_queue_datagram(master, &master->sync_mon_datagram);
+ return 0;
+}
+
+/****************************************************************************/
+
+uint32_t ecrt_master_sync_monitor_process(const ec_master_t *master)
+{
+ if (master->sync_mon_datagram.state == EC_DATAGRAM_RECEIVED) {
+ return EC_READ_U32(master->sync_mon_datagram.data) & 0x7fffffff;
+ } else {
+ return 0xffffffff;
+ }
+}
+
+/****************************************************************************/
+
+int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
+ uint16_t index, uint8_t subindex, const uint8_t *data,
+ size_t data_size, uint32_t *abort_code)
+{
+ ec_sdo_request_t request;
+ ec_slave_t *slave;
+ int ret;
+
+ EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
+ " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
+ " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n",
+ __func__, master, slave_position, index, subindex,
+ data, data_size, abort_code);
+
+ ec_sdo_request_init(&request);
+ ecrt_sdo_request_index(&request, index, subindex);
+ ret = ec_sdo_request_alloc(&request, data_size);
+ if (ret) {
+ ec_sdo_request_clear(&request);
+ return ret;
+ }
+
+ memcpy(request.data, data, data_size);
+ request.data_size = data_size;
+ ecrt_sdo_request_write(&request);
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_sdo_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+ ec_sdo_request_clear(&request);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Scheduling SDO download request.\n");
+
+ // schedule request.
+ list_add_tail(&request.list, &slave->sdo_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_sdo_request_clear(&request);
+ return -EINTR;
+ }
+ // request already processing: interrupt not possible.
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ *abort_code = request.abort_code;
+
+ if (request.state == EC_INT_REQUEST_SUCCESS) {
+ ret = 0;
+ } else if (request.errno) {
+ ret = -request.errno;
+ } else {
+ ret = -EIO;
+ }
+
+ ec_sdo_request_clear(&request);
+ return ret;
+}
+
+/****************************************************************************/
+
+int ecrt_master_sdo_download_complete(ec_master_t *master,
+ uint16_t slave_position, uint16_t index, const uint8_t *data,
+ size_t data_size, uint32_t *abort_code)
+{
+ ec_sdo_request_t request;
+ ec_slave_t *slave;
+ int ret;
+
+ EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
+ " slave_position = %u, index = 0x%04X,"
+ " data = 0x%p, data_size = %zu, abort_code = 0x%p)\n",
+ __func__, master, slave_position, index, data, data_size,
+ abort_code);
+
+ ec_sdo_request_init(&request);
+ ecrt_sdo_request_index(&request, index, 0);
+ ret = ec_sdo_request_alloc(&request, data_size);
+ if (ret) {
+ ec_sdo_request_clear(&request);
+ return ret;
+ }
+
+ request.complete_access = 1;
+ memcpy(request.data, data, data_size);
+ request.data_size = data_size;
+ ecrt_sdo_request_write(&request);
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_sdo_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+ ec_sdo_request_clear(&request);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Scheduling SDO download request"
+ " (complete access).\n");
+
+ // schedule request.
+ list_add_tail(&request.list, &slave->sdo_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_sdo_request_clear(&request);
+ return -EINTR;
+ }
+ // request already processing: interrupt not possible.
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ *abort_code = request.abort_code;
+
+ if (request.state == EC_INT_REQUEST_SUCCESS) {
+ ret = 0;
+ } else if (request.errno) {
+ ret = -request.errno;
+ } else {
+ ret = -EIO;
+ }
+
+ ec_sdo_request_clear(&request);
+ return ret;
+}
+
+/****************************************************************************/
+
+int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
+ uint16_t index, uint8_t subindex, uint8_t *target,
+ size_t target_size, size_t *result_size, uint32_t *abort_code)
+{
+ ec_sdo_request_t request;
+ ec_slave_t *slave;
+ int ret = 0;
+
+ EC_MASTER_DBG(master, 1, "%s(master = 0x%p,"
+ " slave_position = %u, index = 0x%04X, subindex = 0x%02X,"
+ " target = 0x%p, target_size = %zu, result_size = 0x%p,"
+ " abort_code = 0x%p)\n",
+ __func__, master, slave_position, index, subindex,
+ target, target_size, result_size, abort_code);
+
+ ec_sdo_request_init(&request);
+ ecrt_sdo_request_index(&request, index, subindex);
+ ecrt_sdo_request_read(&request);
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_sdo_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
+ up(&master->master_sem);
+ ec_sdo_request_clear(&request);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Scheduling SDO upload request.\n");
+
+ // schedule request.
+ list_add_tail(&request.list, &slave->sdo_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_sdo_request_clear(&request);
+ return -EINTR;
+ }
+ // request already processing: interrupt not possible.
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ *abort_code = request.abort_code;
+
+ if (request.state != EC_INT_REQUEST_SUCCESS) {
+ *result_size = 0;
+ if (request.errno) {
+ ret = -request.errno;
+ } else {
+ ret = -EIO;
+ }
+ } else {
+ if (request.data_size > target_size) {
+ EC_SLAVE_ERR(slave, "%s(): Buffer too small.\n", __func__);
+ ret = -ENOBUFS;
+ }
+ else {
+ memcpy(target, request.data, request.data_size);
+ *result_size = request.data_size;
+ ret = 0;
+ }
+ }
+
+ ec_sdo_request_clear(&request);
+ return ret;
+}
+
+/****************************************************************************/
+
+int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
+ uint8_t drive_no, uint16_t idn, const uint8_t *data, size_t data_size,
+ uint16_t *error_code)
+{
+ ec_soe_request_t request;
+ ec_slave_t *slave;
+ int ret;
+
+ if (drive_no > 7) {
+ EC_MASTER_ERR(master, "Invalid drive number!\n");
+ return -EINVAL;
+ }
+
+ ec_soe_request_init(&request);
+ ec_soe_request_set_drive_no(&request, drive_no);
+ ec_soe_request_set_idn(&request, idn);
+
+ ret = ec_soe_request_alloc(&request, data_size);
+ if (ret) {
+ ec_soe_request_clear(&request);
+ return ret;
+ }
+
+ memcpy(request.data, data, data_size);
+ request.data_size = data_size;
+ ec_soe_request_write(&request);
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_soe_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
+ up(&master->master_sem);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n",
+ slave_position);
+ ec_soe_request_clear(&request);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Scheduling SoE write request.\n");
+
+ // schedule SoE write request.
+ list_add_tail(&request.list, &slave->soe_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ // abort request
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_soe_request_clear(&request);
+ return -EINTR;
+ }
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ if (error_code) {
+ *error_code = request.error_code;
+ }
+ ret = request.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
+ ec_soe_request_clear(&request);
+
+ return ret;
+}
+
+/****************************************************************************/
+
+int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
+ uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,
+ size_t *result_size, uint16_t *error_code)
+{
+ ec_soe_request_t request;
+ ec_slave_t *slave;
+ int ret;
+
+ if (drive_no > 7) {
+ EC_MASTER_ERR(master, "Invalid drive number!\n");
+ return -EINVAL;
+ }
+
+ ec_soe_request_init(&request);
+ ec_soe_request_set_drive_no(&request, drive_no);
+ ec_soe_request_set_idn(&request, idn);
+ ec_soe_request_read(&request);
+
+ if (down_interruptible(&master->master_sem)) {
+ ec_soe_request_clear(&request);
+ return -EINTR;
+ }
+
+ if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
+ up(&master->master_sem);
+ ec_soe_request_clear(&request);
+ EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
+ return -EINVAL;
+ }
+
+ EC_SLAVE_DBG(slave, 1, "Scheduling SoE read request.\n");
+
+ // schedule request.
+ list_add_tail(&request.list, &slave->soe_requests);
+
+ up(&master->master_sem);
+
+ // wait for processing through FSM
+ if (wait_event_interruptible(master->request_queue,
+ request.state != EC_INT_REQUEST_QUEUED)) {
+ // interrupted by signal
+ down(&master->master_sem);
+ if (request.state == EC_INT_REQUEST_QUEUED) {
+ list_del(&request.list);
+ up(&master->master_sem);
+ ec_soe_request_clear(&request);
+ return -EINTR;
+ }
+ // request already processing: interrupt not possible.
+ up(&master->master_sem);
+ }
+
+ // wait until master FSM has finished processing
+ wait_event(master->request_queue, request.state != EC_INT_REQUEST_BUSY);
+
+ if (error_code) {
+ *error_code = request.error_code;
+ }
+
+ if (request.state != EC_INT_REQUEST_SUCCESS) {
+ if (result_size) {
+ *result_size = 0;
+ }
+ ret = -EIO;
+ } else { // success
+ if (request.data_size > target_size) {
+ EC_SLAVE_ERR(slave, "%s(): Buffer too small.\n", __func__);
+ ret = -ENOBUFS;
+ }
+ else { // data fits in buffer
+ if (result_size) {
+ *result_size = request.data_size;
+ }
+ memcpy(target, request.data, request.data_size);
+ ret = 0;
+ }
+ }
+
+ ec_soe_request_clear(&request);
+ return ret;
+}
+
+/****************************************************************************/
+
+int ecrt_master_reset(ec_master_t *master)
+{
+ ec_slave_config_t *sc;
+
+ list_for_each_entry(sc, &master->configs, list) {
+ if (sc->slave) {
+ ec_slave_request_state(sc->slave, EC_SLAVE_STATE_OP);
+ }
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+static void sc_reset_task_kicker(struct irq_work *work)
+{
+ struct ec_master *master =
+ container_of(work, struct ec_master, sc_reset_work_kicker);
+ schedule_work(&master->sc_reset_work);
+}
+
+/****************************************************************************/
+
+static void sc_reset_task(struct work_struct *work)
+{
+ struct ec_master *master =
+ container_of(work, struct ec_master, sc_reset_work);
+
+ down(&master->master_sem);
+ ecrt_master_reset(master);
+ up(&master->master_sem);
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecrt_master_create_domain);
+EXPORT_SYMBOL(ecrt_master_activate);
+EXPORT_SYMBOL(ecrt_master_deactivate);
+EXPORT_SYMBOL(ecrt_master_send);
+EXPORT_SYMBOL(ecrt_master_send_ext);
+EXPORT_SYMBOL(ecrt_master_receive);
+EXPORT_SYMBOL(ecrt_master_callbacks);
+EXPORT_SYMBOL(ecrt_master);
+EXPORT_SYMBOL(ecrt_master_scan_progress);
+EXPORT_SYMBOL(ecrt_master_get_slave);
+EXPORT_SYMBOL(ecrt_master_slave_config);
+EXPORT_SYMBOL(ecrt_master_select_reference_clock);
+EXPORT_SYMBOL(ecrt_master_state);
+EXPORT_SYMBOL(ecrt_master_link_state);
+EXPORT_SYMBOL(ecrt_master_application_time);
+EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
+EXPORT_SYMBOL(ecrt_master_sync_reference_clock_to);
+EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
+EXPORT_SYMBOL(ecrt_master_reference_clock_time);
+EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
+EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
+EXPORT_SYMBOL(ecrt_master_sdo_download);
+EXPORT_SYMBOL(ecrt_master_sdo_download_complete);
+EXPORT_SYMBOL(ecrt_master_sdo_upload);
+EXPORT_SYMBOL(ecrt_master_write_idn);
+EXPORT_SYMBOL(ecrt_master_read_idn);
+EXPORT_SYMBOL(ecrt_master_reset);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/master.h b/ubuntu/igh-ecat/master/master.h
new file mode 100644
index 00000000000000..a1125e85a360e6
--- /dev/null
+++ b/ubuntu/igh-ecat/master/master.h
@@ -0,0 +1,386 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT master structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_MASTER_H__
+#define __EC_MASTER_H__
+
+#include <linux/version.h>
+#include <linux/irq_work.h>
+#include <linux/list.h>
+#include <linux/timer.h>
+#include <linux/wait.h>
+#include <linux/kthread.h>
+#include <linux/rtmutex.h>
+#include <linux/workqueue.h>
+
+#include "device.h"
+#include "domain.h"
+#include "ethernet.h"
+#include "fsm_master.h"
+#include "cdev.h"
+
+#ifdef EC_RTDM
+#include "rtdm.h"
+#endif
+
+/****************************************************************************/
+
+/** Convenience macro for printing master-specific information to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT <INDEX>: ",
+ * where INDEX is the master index.
+ *
+ * \param master EtherCAT master
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_MASTER_INFO(master, fmt, args...) \
+ printk(KERN_INFO "EtherCAT %u: " fmt, master->index, ##args)
+
+/** Convenience macro for printing master-specific errors to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT <INDEX>: ",
+ * where INDEX is the master index.
+ *
+ * \param master EtherCAT master
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_MASTER_ERR(master, fmt, args...) \
+ printk(KERN_ERR "EtherCAT ERROR %u: " fmt, master->index, ##args)
+
+/** Convenience macro for printing master-specific warnings to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT <INDEX>: ",
+ * where INDEX is the master index.
+ *
+ * \param master EtherCAT master
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_MASTER_WARN(master, fmt, args...) \
+ printk(KERN_WARNING "EtherCAT WARNING %u: " fmt, master->index, ##args)
+
+/** Convenience macro for printing master-specific debug messages to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed "EtherCAT <INDEX>: ",
+ * where INDEX is the master index.
+ *
+ * \param master EtherCAT master
+ * \param level Debug level. Master's debug level must be >= \a level for
+ * output.
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_MASTER_DBG(master, level, fmt, args...) \
+ do { \
+ if (master->debug_level >= level) { \
+ printk(KERN_DEBUG "EtherCAT DEBUG %u: " fmt, \
+ master->index, ##args); \
+ } \
+ } while (0)
+
+
+/** Size of the external datagram ring.
+ *
+ * The external datagram ring is used for slave FSMs.
+ */
+#define EC_EXT_RING_SIZE 32
+
+/** Maximum number of masters.
+ */
+#define EC_MAX_MASTERS 32
+
+/****************************************************************************/
+
+/** EtherCAT master phase.
+ */
+typedef enum {
+ EC_ORPHANED, /**< Orphaned phase. The master has no Ethernet device
+ attached. */
+ EC_IDLE, /**< Idle phase. An Ethernet device is attached, but the master
+ is not in use, yet. */
+ EC_OPERATION /**< Operation phase. The master was requested by a realtime
+ application. */
+} ec_master_phase_t;
+
+/****************************************************************************/
+
+/** Cyclic statistics.
+ */
+typedef struct {
+ unsigned int timeouts; /**< datagram timeouts */
+ unsigned int corrupted; /**< corrupted frames */
+ unsigned int unmatched; /**< unmatched datagrams (received, but not
+ queued any longer) */
+ unsigned long output_jiffies; /**< time of last output */
+} ec_stats_t;
+
+/****************************************************************************/
+
+/** Device statistics.
+ */
+typedef struct {
+ u64 tx_count; /**< Number of frames sent. */
+ u64 last_tx_count; /**< Number of frames sent of last statistics cycle. */
+ u64 rx_count; /**< Number of frames received. */
+ u64 last_rx_count; /**< Number of frames received of last statistics
+ cycle. */
+ u64 tx_bytes; /**< Number of bytes sent. */
+ u64 last_tx_bytes; /**< Number of bytes sent of last statistics cycle. */
+ u64 rx_bytes; /**< Number of bytes received. */
+ u64 last_rx_bytes; /**< Number of bytes received of last statistics cycle.
+ */
+ u64 last_loss; /**< Tx/Rx difference of last statistics cycle. */
+ s32 tx_frame_rates[EC_RATE_COUNT]; /**< Transmit rates in frames/s for
+ different statistics cycle periods.
+ */
+ s32 rx_frame_rates[EC_RATE_COUNT]; /**< Receive rates in frames/s for
+ different statistics cycle periods.
+ */
+ s32 tx_byte_rates[EC_RATE_COUNT]; /**< Transmit rates in byte/s for
+ different statistics cycle periods. */
+ s32 rx_byte_rates[EC_RATE_COUNT]; /**< Receive rates in byte/s for
+ different statistics cycle periods. */
+ s32 loss_rates[EC_RATE_COUNT]; /**< Frame loss rates for different
+ statistics cycle periods. */
+ unsigned long jiffies; /**< Jiffies of last statistic cycle. */
+} ec_device_stats_t;
+
+/****************************************************************************/
+
+#if EC_MAX_NUM_DEVICES < 1
+#error Invalid number of devices
+#endif
+
+/****************************************************************************/
+
+/** EtherCAT master.
+ *
+ * Manages slaves, domains and IO.
+ */
+struct ec_master {
+ unsigned int index; /**< Index. */
+ unsigned int reserved; /**< \a True, if the master is in use. */
+
+ ec_cdev_t cdev; /**< Master character device. */
+ struct device *class_device; /**< Master class device. */
+
+#ifdef EC_RTDM
+ ec_rtdm_dev_t rtdm_dev; /**< RTDM device. */
+#endif
+
+ struct semaphore master_sem; /**< Master semaphore. */
+
+ ec_device_t devices[EC_MAX_NUM_DEVICES]; /**< EtherCAT devices. */
+ const uint8_t *macs[EC_MAX_NUM_DEVICES]; /**< Device MAC addresses. */
+#if EC_MAX_NUM_DEVICES > 1
+ unsigned int num_devices; /**< Number of devices. Access this always via
+ ec_master_num_devices(), because it may be
+ optimized! */
+#endif
+ struct semaphore device_sem; /**< Device semaphore. */
+ ec_device_stats_t device_stats; /**< Device statistics. */
+
+ ec_fsm_master_t fsm; /**< Master state machine. */
+ ec_datagram_t fsm_datagram; /**< Datagram used for state machines. */
+ ec_master_phase_t phase; /**< Master phase. */
+ unsigned int active; /**< Master has been activated. */
+ unsigned int config_changed; /**< The configuration changed. */
+ unsigned int injection_seq_fsm; /**< Datagram injection sequence number
+ for the FSM side. */
+ unsigned int injection_seq_rt; /**< Datagram injection sequence number
+ for the realtime side. */
+
+ ec_slave_t *slaves; /**< Array of slaves on the bus. */
+ unsigned int slave_count; /**< Number of slaves on the bus. */
+
+ /* Configuration applied by the application. */
+ struct list_head configs; /**< List of slave configurations. */
+ struct list_head domains; /**< List of domains. */
+
+ u64 app_time; /**< Time of the last ecrt_master_sync() call. */
+ u64 dc_ref_time; /**< Common reference timestamp for DC start times. */
+ ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the
+ reference clock to the master clock. */
+ ec_datagram_t sync_datagram; /**< Datagram used for DC drift
+ compensation. */
+ ec_datagram_t sync_mon_datagram; /**< Datagram used for DC synchronisation
+ monitoring. */
+ ec_slave_config_t *dc_ref_config; /**< Application-selected DC reference
+ clock slave config. */
+ ec_slave_t *dc_ref_clock; /**< DC reference clock slave. */
+
+ unsigned int scan_busy; /**< Current scan state. */
+ unsigned int scan_index; /**< Index of slave currently scanned. */
+ unsigned int allow_scan; /**< \a True, if slave scanning is allowed. */
+ struct semaphore scan_sem; /**< Semaphore protecting the \a scan_busy
+ variable and the \a allow_scan flag. */
+ wait_queue_head_t scan_queue; /**< Queue for processes that wait for
+ slave scanning. */
+
+ unsigned int config_busy; /**< State of slave configuration. */
+ struct semaphore config_sem; /**< Semaphore protecting the \a config_busy
+ variable and the allow_config flag. */
+ wait_queue_head_t config_queue; /**< Queue for processes that wait for
+ slave configuration. */
+
+ struct list_head datagram_queue; /**< Datagram queue. */
+ uint8_t datagram_index; /**< Current datagram index. */
+
+ struct list_head ext_datagram_queue; /**< Queue for non-application
+ datagrams. */
+ struct semaphore ext_queue_sem; /**< Semaphore protecting the \a
+ ext_datagram_queue. */
+
+ ec_datagram_t ext_datagram_ring[EC_EXT_RING_SIZE]; /**< External datagram
+ ring. */
+ unsigned int ext_ring_idx_rt; /**< Index in external datagram ring for RT
+ side. */
+ unsigned int ext_ring_idx_fsm; /**< Index in external datagram ring for
+ FSM side. */
+ unsigned int send_interval; /**< Interval between two calls to
+ ecrt_master_send(). */
+ size_t max_queue_size; /**< Maximum size of datagram queue */
+
+ ec_slave_t *fsm_slave; /**< Slave that is queried next for FSM exec. */
+ struct list_head fsm_exec_list; /**< Slave FSM execution list. */
+ unsigned int fsm_exec_count; /**< Number of entries in execution list. */
+
+ unsigned int debug_level; /**< Master debug level. */
+ unsigned int run_on_cpu; /**< bind kernel threads to this cpu */
+ ec_stats_t stats; /**< Cyclic statistics. */
+
+ struct task_struct *thread; /**< Master thread. */
+
+#ifdef EC_EOE
+ struct task_struct *eoe_thread; /**< EoE thread. */
+ struct list_head eoe_handlers; /**< Ethernet over EtherCAT handlers. */
+#endif
+
+ struct rt_mutex io_mutex; /**< Mutex used in \a IDLE and \a OP phase. */
+
+ void (*send_cb)(void *); /**< Current send datagrams callback. */
+ void (*receive_cb)(void *); /**< Current receive datagrams callback. */
+ void *cb_data; /**< Current callback data. */
+ void (*app_send_cb)(void *); /**< Application's send datagrams
+ callback. */
+ void (*app_receive_cb)(void *); /**< Application's receive datagrams
+ callback. */
+ void *app_cb_data; /**< Application callback data. */
+
+ struct list_head sii_requests; /**< SII write requests. */
+ struct list_head emerg_reg_requests; /**< Emergency register access
+ requests. */
+
+ wait_queue_head_t request_queue; /**< Wait queue for external requests
+ from user space. */
+ struct work_struct sc_reset_work; /**< Task to reset slave configuration. */
+ struct irq_work sc_reset_work_kicker; /**< NMI-Safe kicker to trigger
+ reset task above. */
+};
+
+/****************************************************************************/
+
+// static funtions
+void ec_master_init_static(void);
+
+// master creation/deletion
+int ec_master_init(ec_master_t *, unsigned int, const uint8_t *,
+ const uint8_t *, dev_t, struct class *, unsigned int, unsigned int);
+void ec_master_clear(ec_master_t *);
+
+/** Number of Ethernet devices.
+ */
+#if EC_MAX_NUM_DEVICES > 1
+#define ec_master_num_devices(MASTER) ((MASTER)->num_devices)
+#else
+#define ec_master_num_devices(MASTER) 1
+#endif
+
+// phase transitions
+int ec_master_enter_idle_phase(ec_master_t *);
+void ec_master_leave_idle_phase(ec_master_t *);
+int ec_master_enter_operation_phase(ec_master_t *);
+void ec_master_leave_operation_phase(ec_master_t *);
+
+#ifdef EC_EOE
+// EoE
+void ec_master_eoe_start(ec_master_t *);
+void ec_master_eoe_stop(ec_master_t *);
+#endif
+
+// datagram IO
+void ec_master_receive_datagrams(ec_master_t *, ec_device_t *,
+ const uint8_t *, size_t);
+void ec_master_queue_datagram(ec_master_t *, ec_datagram_t *);
+void ec_master_queue_datagram_ext(ec_master_t *, ec_datagram_t *);
+
+// misc.
+void ec_master_set_send_interval(ec_master_t *, unsigned int);
+void ec_master_attach_slave_configs(ec_master_t *);
+ec_slave_t *ec_master_find_slave(ec_master_t *, uint16_t, uint16_t);
+const ec_slave_t *ec_master_find_slave_const(const ec_master_t *, uint16_t,
+ uint16_t);
+void ec_master_output_stats(ec_master_t *);
+#ifdef EC_EOE
+void ec_master_clear_eoe_handlers(ec_master_t *);
+#endif
+void ec_master_clear_slaves(ec_master_t *);
+
+unsigned int ec_master_config_count(const ec_master_t *);
+ec_slave_config_t *ec_master_get_config(
+ const ec_master_t *, unsigned int);
+const ec_slave_config_t *ec_master_get_config_const(
+ const ec_master_t *, unsigned int);
+unsigned int ec_master_domain_count(const ec_master_t *);
+ec_domain_t *ec_master_find_domain(ec_master_t *, unsigned int);
+const ec_domain_t *ec_master_find_domain_const(const ec_master_t *,
+ unsigned int);
+#ifdef EC_EOE
+uint16_t ec_master_eoe_handler_count(const ec_master_t *);
+const ec_eoe_t *ec_master_get_eoe_handler_const(const ec_master_t *, uint16_t);
+#endif
+
+int ec_master_debug_level(ec_master_t *, unsigned int);
+
+ec_domain_t *ecrt_master_create_domain_err(ec_master_t *);
+ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *, uint16_t,
+ uint16_t, uint32_t, uint32_t);
+
+void ec_master_calc_dc(ec_master_t *);
+void ec_master_request_op(ec_master_t *);
+
+void ec_master_internal_send_cb(void *);
+void ec_master_internal_receive_cb(void *);
+
+extern const unsigned int rate_intervals[EC_RATE_COUNT]; // see master.c
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/module.c b/ubuntu/igh-ecat/master/module.c
new file mode 100644
index 00000000000000..b8156a1d3d5261
--- /dev/null
+++ b/ubuntu/igh-ecat/master/module.c
@@ -0,0 +1,681 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT master driver module.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/err.h>
+
+#include "globals.h"
+#include "master.h"
+#include "device.h"
+
+/****************************************************************************/
+
+/****************************************************************************/
+
+int __init ec_init_module(void);
+void __exit ec_cleanup_module(void);
+
+static int ec_mac_parse(uint8_t *, const char *, int);
+
+// prototypes for private functions
+int ec_mac_equal(const uint8_t *, const uint8_t *);
+int ec_mac_is_broadcast(const uint8_t *);
+
+/****************************************************************************/
+
+static char *main_devices[EC_MAX_MASTERS]; /**< Main devices parameter. */
+static unsigned int master_count; /**< Number of masters. */
+static char *backup_devices[EC_MAX_MASTERS]; /**< Backup devices parameter. */
+static unsigned int backup_count; /**< Number of backup devices. */
+static unsigned int debug_level; /**< Debug level parameter. */
+static unsigned int run_on_cpu = 0xffffffff; /**< Bind created kernel threads
+ to a cpu. Default do not bind.
+ */
+
+static ec_master_t *masters; /**< Array of masters. */
+static struct semaphore master_sem; /**< Master semaphore. */
+
+dev_t device_number; /**< Device number for master cdevs. */
+struct class *class; /**< Device class. */
+
+static uint8_t macs[EC_MAX_MASTERS][2][ETH_ALEN]; /**< MAC addresses. */
+
+char *ec_master_version_str = EC_MASTER_VERSION; /**< Version string. */
+
+/****************************************************************************/
+
+/** \cond */
+
+MODULE_AUTHOR("Florian Pose <fp at igh.de>");
+MODULE_DESCRIPTION("EtherCAT master driver module");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(EC_MASTER_VERSION);
+
+module_param_array(main_devices, charp, &master_count, S_IRUGO);
+MODULE_PARM_DESC(main_devices, "MAC addresses of main devices");
+module_param_array(backup_devices, charp, &backup_count, S_IRUGO);
+MODULE_PARM_DESC(backup_devices, "MAC addresses of backup devices");
+module_param_named(debug_level, debug_level, uint, S_IRUGO);
+MODULE_PARM_DESC(debug_level, "Debug level");
+module_param_named(run_on_cpu, run_on_cpu, uint, S_IRUGO);
+MODULE_PARM_DESC(run_on_cpu, "Bind kthreads to a specific cpu");
+
+/** \endcond */
+
+/****************************************************************************/
+
+/** Module initialization.
+ *
+ * Initializes \a master_count masters.
+ * \return 0 on success, else < 0
+ */
+int __init ec_init_module(void)
+{
+ int i, ret = 0;
+
+ EC_INFO("Master driver %s\n", EC_MASTER_VERSION);
+
+ sema_init(&master_sem, 1);
+
+ if (master_count) {
+ if (alloc_chrdev_region(&device_number,
+ 0, master_count, "EtherCAT")) {
+ EC_ERR("Failed to obtain device number(s)!\n");
+ ret = -EBUSY;
+ goto out_return;
+ }
+ }
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 4, 0)
+ class = class_create(THIS_MODULE, "EtherCAT");
+#else
+ class = class_create("EtherCAT");
+#endif
+ if (IS_ERR(class)) {
+ EC_ERR("Failed to create device class.\n");
+ ret = PTR_ERR(class);
+ goto out_cdev;
+ }
+
+ // zero MAC addresses
+ memset(macs, 0x00, sizeof(uint8_t) * EC_MAX_MASTERS * 2 * ETH_ALEN);
+
+ // process MAC parameters
+ for (i = 0; i < master_count; i++) {
+ ret = ec_mac_parse(macs[i][0], main_devices[i], 0);
+ if (ret)
+ goto out_class;
+
+ if (i < backup_count) {
+ ret = ec_mac_parse(macs[i][1], backup_devices[i], 1);
+ if (ret)
+ goto out_class;
+ }
+ }
+
+ // initialize static master variables
+ ec_master_init_static();
+
+ if (master_count) {
+ if (!(masters = kmalloc(sizeof(ec_master_t) * master_count,
+ GFP_KERNEL))) {
+ EC_ERR("Failed to allocate memory"
+ " for EtherCAT masters.\n");
+ ret = -ENOMEM;
+ goto out_class;
+ }
+ }
+
+ for (i = 0; i < master_count; i++) {
+ ret = ec_master_init(&masters[i], i, macs[i][0], macs[i][1],
+ device_number, class, debug_level, run_on_cpu);
+ if (ret)
+ goto out_free_masters;
+ }
+
+ EC_INFO("%u master%s waiting for devices.\n",
+ master_count, (master_count == 1 ? "" : "s"));
+ return ret;
+
+out_free_masters:
+ for (i--; i >= 0; i--)
+ ec_master_clear(&masters[i]);
+ kfree(masters);
+out_class:
+ class_destroy(class);
+out_cdev:
+ if (master_count)
+ unregister_chrdev_region(device_number, master_count);
+out_return:
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Module cleanup.
+ *
+ * Clears all master instances.
+ */
+void __exit ec_cleanup_module(void)
+{
+ unsigned int i;
+
+ for (i = 0; i < master_count; i++) {
+ ec_master_clear(&masters[i]);
+ }
+
+ if (master_count)
+ kfree(masters);
+
+ class_destroy(class);
+
+ if (master_count)
+ unregister_chrdev_region(device_number, master_count);
+
+ EC_INFO("Master module cleaned up.\n");
+}
+
+/****************************************************************************/
+
+/** Get the number of masters.
+ */
+unsigned int ec_master_count(void)
+{
+ return master_count;
+}
+
+/*****************************************************************************
+ * MAC address functions
+ ****************************************************************************/
+
+/**
+ * \return true, if two MAC addresses are equal.
+ */
+int ec_mac_equal(
+ const uint8_t *mac1, /**< First MAC address. */
+ const uint8_t *mac2 /**< Second MAC address. */
+ )
+{
+ unsigned int i;
+
+ for (i = 0; i < ETH_ALEN; i++)
+ if (mac1[i] != mac2[i])
+ return 0;
+
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Maximum MAC string size.
+ */
+#define EC_MAX_MAC_STRING_SIZE (3 * ETH_ALEN)
+
+/** Print a MAC address to a buffer.
+ *
+ * The buffer size must be at least EC_MAX_MAC_STRING_SIZE.
+ *
+ * \return number of bytes written.
+ */
+ssize_t ec_mac_print(
+ const uint8_t *mac, /**< MAC address */
+ char *buffer /**< Target buffer. */
+ )
+{
+ off_t off = 0;
+ unsigned int i;
+
+ for (i = 0; i < ETH_ALEN; i++) {
+ off += sprintf(buffer + off, "%02X", mac[i]);
+ if (i < ETH_ALEN - 1) off += sprintf(buffer + off, ":");
+ }
+
+ return off;
+}
+
+/****************************************************************************/
+
+/**
+ * \return true, if the MAC address is all-zero.
+ */
+int ec_mac_is_zero(
+ const uint8_t *mac /**< MAC address. */
+ )
+{
+ unsigned int i;
+
+ for (i = 0; i < ETH_ALEN; i++)
+ if (mac[i])
+ return 0;
+
+ return 1;
+}
+
+/****************************************************************************/
+
+/**
+ * \return true, if the given MAC address is the broadcast address.
+ */
+int ec_mac_is_broadcast(
+ const uint8_t *mac /**< MAC address. */
+ )
+{
+ unsigned int i;
+
+ for (i = 0; i < ETH_ALEN; i++)
+ if (mac[i] != 0xff)
+ return 0;
+
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Parse a MAC address from a string.
+ *
+ * The MAC address must match the regular expression
+ * "([0-9a-fA-F]{2}:){5}[0-9a-fA-F]{2}".
+ *
+ * \return 0 on success, else < 0
+ */
+static int ec_mac_parse(uint8_t *mac, const char *src, int allow_empty)
+{
+ unsigned int i, value;
+ const char *orig = src;
+ char *rem;
+
+ if (!strlen(src)) {
+ if (allow_empty){
+ return 0;
+ } else {
+ EC_ERR("MAC address may not be empty.\n");
+ return -EINVAL;
+ }
+ }
+
+ for (i = 0; i < ETH_ALEN; i++) {
+ value = simple_strtoul(src, &rem, 16);
+ if (rem != src + 2
+ || value > 0xFF
+ || (i < ETH_ALEN - 1 && *rem != ':')) {
+ EC_ERR("Invalid MAC address \"%s\".\n", orig);
+ return -EINVAL;
+ }
+ mac[i] = value;
+ if (i < ETH_ALEN - 1) {
+ src = rem + 1; // skip colon
+ }
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Outputs frame contents for debugging purposes.
+ * If the data block is larger than 256 bytes, only the first 128
+ * and the last 128 bytes will be shown
+ */
+void ec_print_data(const uint8_t *data, /**< pointer to data */
+ size_t size /**< number of bytes to output */
+ )
+{
+ unsigned int i;
+
+ EC_DBG("");
+ for (i = 0; i < size; i++) {
+ printk(KERN_CONT "%02X ", data[i]);
+
+ if ((i + 1) % 16 == 0 && i < size - 1) {
+ printk(KERN_CONT "\n");
+ EC_DBG("");
+ }
+
+ if (i + 1 == 128 && size > 256) {
+ printk(KERN_CONT "dropped %zu bytes\n", size - 128 - i);
+ i = size - 128;
+ EC_DBG("");
+ }
+ }
+ printk(KERN_CONT "\n");
+}
+
+/****************************************************************************/
+
+/** Outputs frame contents and differences for debugging purposes.
+ */
+void ec_print_data_diff(const uint8_t *d1, /**< first data */
+ const uint8_t *d2, /**< second data */
+ size_t size /** number of bytes to output */
+ )
+{
+ unsigned int i;
+
+ EC_DBG("");
+ for (i = 0; i < size; i++) {
+ if (d1[i] == d2[i]) {
+ printk(KERN_CONT ".. ");
+ }
+ else {
+ printk(KERN_CONT "%02X ", d2[i]);
+ }
+ if ((i + 1) % 16 == 0) {
+ printk(KERN_CONT "\n");
+ EC_DBG("");
+ }
+ }
+ printk(KERN_CONT "\n");
+}
+
+/****************************************************************************/
+
+/** Prints slave states in clear text.
+ *
+ * \return Size of the created string.
+ */
+size_t ec_state_string(uint8_t states, /**< slave states */
+ char *buffer, /**< target buffer
+ (min. EC_STATE_STRING_SIZE bytes) */
+ uint8_t multi /**< Show multi-state mask. */
+ )
+{
+ off_t off = 0;
+ unsigned int first = 1;
+
+ if (!states) {
+ off += sprintf(buffer + off, "(unknown)");
+ return off;
+ }
+
+ if (multi) { // multiple slaves
+ if (states & EC_SLAVE_STATE_INIT) {
+ off += sprintf(buffer + off, "INIT");
+ first = 0;
+ }
+ if (states & EC_SLAVE_STATE_PREOP) {
+ if (!first) off += sprintf(buffer + off, ", ");
+ off += sprintf(buffer + off, "PREOP");
+ first = 0;
+ }
+ if (states & EC_SLAVE_STATE_SAFEOP) {
+ if (!first) off += sprintf(buffer + off, ", ");
+ off += sprintf(buffer + off, "SAFEOP");
+ first = 0;
+ }
+ if (states & EC_SLAVE_STATE_OP) {
+ if (!first) off += sprintf(buffer + off, ", ");
+ off += sprintf(buffer + off, "OP");
+ }
+ } else { // single slave
+ if ((states & EC_SLAVE_STATE_MASK) == EC_SLAVE_STATE_INIT) {
+ off += sprintf(buffer + off, "INIT");
+ } else if ((states & EC_SLAVE_STATE_MASK) == EC_SLAVE_STATE_PREOP) {
+ off += sprintf(buffer + off, "PREOP");
+ } else if ((states & EC_SLAVE_STATE_MASK) == EC_SLAVE_STATE_BOOT) {
+ off += sprintf(buffer + off, "BOOT");
+ } else if ((states & EC_SLAVE_STATE_MASK) == EC_SLAVE_STATE_SAFEOP) {
+ off += sprintf(buffer + off, "SAFEOP");
+ } else if ((states & EC_SLAVE_STATE_MASK) == EC_SLAVE_STATE_OP) {
+ off += sprintf(buffer + off, "OP");
+ } else {
+ off += sprintf(buffer + off, "(invalid)");
+ }
+ first = 0;
+ }
+
+ if (states & EC_SLAVE_STATE_ACK_ERR) {
+ if (!first) off += sprintf(buffer + off, " + ");
+ off += sprintf(buffer + off, "ERROR");
+ }
+
+ return off;
+}
+
+/*****************************************************************************
+ * Device interface
+ ****************************************************************************/
+
+/** Device names.
+ */
+const char *ec_device_names[2] = {
+ "main",
+ "backup"
+};
+
+/** Offers an EtherCAT device to a certain master.
+ *
+ * The master decides, if it wants to use the device for EtherCAT operation,
+ * or not. It is important, that the offered net_device is not used by the
+ * kernel IP stack. If the master, accepted the offer, the address of the
+ * newly created EtherCAT device is returned, else \a NULL is returned.
+ *
+ * \return Pointer to device, if accepted, or NULL if declined.
+ * \ingroup DeviceInterface
+ */
+ec_device_t *ecdev_offer(
+ struct net_device *net_dev, /**< net_device to offer */
+ ec_pollfunc_t poll, /**< device poll function */
+ struct module *module /**< pointer to the module */
+ )
+{
+ ec_master_t *master;
+ char str[EC_MAX_MAC_STRING_SIZE];
+ unsigned int i, dev_idx;
+
+ for (i = 0; i < master_count; i++) {
+ master = &masters[i];
+ ec_mac_print(net_dev->dev_addr, str);
+
+ if (down_interruptible(&master->device_sem)) {
+ EC_MASTER_WARN(master, "%s() interrupted!\n", __func__);
+ return NULL;
+ }
+
+ for (dev_idx = EC_DEVICE_MAIN;
+ dev_idx < ec_master_num_devices(master); dev_idx++) {
+ if (!master->devices[dev_idx].dev
+ && (ec_mac_equal(master->macs[dev_idx], net_dev->dev_addr)
+ || ec_mac_is_broadcast(master->macs[dev_idx]))) {
+
+ EC_INFO("Accepting %s as %s device for master %u.\n",
+ str, ec_device_names[dev_idx != 0], master->index);
+
+ ec_device_attach(&master->devices[dev_idx],
+ net_dev, poll, module);
+ up(&master->device_sem);
+
+ snprintf(net_dev->name, IFNAMSIZ, "ec%c%u",
+ ec_device_names[dev_idx != 0][0], master->index);
+
+ return &master->devices[dev_idx]; // offer accepted
+ }
+ }
+
+ up(&master->device_sem);
+
+ EC_MASTER_DBG(master, 1, "Master declined device %s.\n", str);
+ }
+
+ return NULL; // offer declined
+}
+
+/*****************************************************************************
+ * Application interface
+ ****************************************************************************/
+
+/** Request a master.
+ *
+ * Same as ecrt_request_master(), but with ERR_PTR() return value.
+ *
+ * \return Requested master.
+ */
+ec_master_t *ecrt_request_master_err(
+ unsigned int master_index /**< Master index. */
+ )
+{
+ ec_master_t *master, *errptr = NULL;
+ unsigned int dev_idx = EC_DEVICE_MAIN;
+
+ EC_INFO("Requesting master %u...\n", master_index);
+
+ if (master_index >= master_count) {
+ EC_ERR("Invalid master index %u.\n", master_index);
+ errptr = ERR_PTR(-EINVAL);
+ goto out_return;
+ }
+ master = &masters[master_index];
+
+ if (down_interruptible(&master_sem)) {
+ errptr = ERR_PTR(-EINTR);
+ goto out_return;
+ }
+
+ if (master->reserved) {
+ up(&master_sem);
+ EC_MASTER_ERR(master, "Master already in use!\n");
+ errptr = ERR_PTR(-EBUSY);
+ goto out_return;
+ }
+ master->reserved = 1;
+ up(&master_sem);
+
+ if (down_interruptible(&master->device_sem)) {
+ errptr = ERR_PTR(-EINTR);
+ goto out_release;
+ }
+
+ if (master->phase != EC_IDLE) {
+ up(&master->device_sem);
+ EC_MASTER_ERR(master, "Master still waiting for devices!\n");
+ errptr = ERR_PTR(-ENODEV);
+ goto out_release;
+ }
+
+ for (; dev_idx < ec_master_num_devices(master); dev_idx++) {
+ ec_device_t *device = &master->devices[dev_idx];
+ if (!try_module_get(device->module)) {
+ up(&master->device_sem);
+ EC_MASTER_ERR(master, "Device module is unloading!\n");
+ errptr = ERR_PTR(-ENODEV);
+ goto out_module_put;
+ }
+ }
+
+ up(&master->device_sem);
+
+ if (ec_master_enter_operation_phase(master)) {
+ EC_MASTER_ERR(master, "Failed to enter OPERATION phase!\n");
+ errptr = ERR_PTR(-EIO);
+ goto out_module_put;
+ }
+
+ EC_INFO("Successfully requested master %u.\n", master_index);
+ return master;
+
+ out_module_put:
+ for (; dev_idx > 0; dev_idx--) {
+ ec_device_t *device = &master->devices[dev_idx - 1];
+ module_put(device->module);
+ }
+ out_release:
+ master->reserved = 0;
+ out_return:
+ return errptr;
+}
+
+/****************************************************************************/
+
+ec_master_t *ecrt_request_master(unsigned int master_index)
+{
+ ec_master_t *master = ecrt_request_master_err(master_index);
+ return IS_ERR(master) ? NULL : master;
+}
+
+/****************************************************************************/
+
+void ecrt_release_master(ec_master_t *master)
+{
+ unsigned int dev_idx;
+
+ EC_MASTER_INFO(master, "Releasing master...\n");
+
+ if (!master->reserved) {
+ EC_MASTER_WARN(master, "%s(): Master was was not requested!\n",
+ __func__);
+ return;
+ }
+
+ ec_master_leave_operation_phase(master);
+
+ for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
+ dev_idx++) {
+ module_put(master->devices[dev_idx].module);
+ }
+
+ master->reserved = 0;
+
+ EC_MASTER_INFO(master, "Released.\n");
+}
+
+/****************************************************************************/
+
+unsigned int ecrt_version_magic(void)
+{
+ return ECRT_VERSION_MAGIC;
+}
+
+/****************************************************************************/
+
+/** Global request state type translation table.
+ *
+ * Translates an internal request state to an external one.
+ */
+const ec_request_state_t ec_request_state_translation_table[] = {
+ EC_REQUEST_UNUSED, // EC_INT_REQUEST_INIT,
+ EC_REQUEST_BUSY, // EC_INT_REQUEST_QUEUED,
+ EC_REQUEST_BUSY, // EC_INT_REQUEST_BUSY,
+ EC_REQUEST_SUCCESS, // EC_INT_REQUEST_SUCCESS,
+ EC_REQUEST_ERROR // EC_INT_REQUEST_FAILURE
+};
+
+/****************************************************************************/
+
+/** \cond */
+
+module_init(ec_init_module);
+module_exit(ec_cleanup_module);
+
+EXPORT_SYMBOL(ecdev_offer);
+
+EXPORT_SYMBOL(ecrt_request_master);
+EXPORT_SYMBOL(ecrt_release_master);
+EXPORT_SYMBOL(ecrt_version_magic);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/pdo.c b/ubuntu/igh-ecat/master/pdo.c
new file mode 100644
index 00000000000000..9c145939574567
--- /dev/null
+++ b/ubuntu/igh-ecat/master/pdo.c
@@ -0,0 +1,309 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT process data object methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+#include <linux/err.h>
+
+#include "pdo.h"
+
+/****************************************************************************/
+
+/** PDO constructor.
+ */
+void ec_pdo_init(
+ ec_pdo_t *pdo /**< EtherCAT PDO */
+ )
+{
+ pdo->sync_index = -1; // not assigned
+ pdo->name = NULL;
+ INIT_LIST_HEAD(&pdo->entries);
+}
+
+/****************************************************************************/
+
+/** PDO copy constructor.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_pdo_init_copy(
+ ec_pdo_t *pdo, /**< PDO to create. */
+ const ec_pdo_t *other_pdo /**< PDO to copy from. */
+ )
+{
+ int ret = 0;
+
+ pdo->index = other_pdo->index;
+ pdo->sync_index = other_pdo->sync_index;
+ pdo->name = NULL;
+ INIT_LIST_HEAD(&pdo->entries);
+
+ ret = ec_pdo_set_name(pdo, other_pdo->name);
+ if (ret < 0)
+ goto out_return;
+
+ ret = ec_pdo_copy_entries(pdo, other_pdo);
+ if (ret < 0)
+ goto out_clear;
+
+ return 0;
+
+out_clear:
+ ec_pdo_clear(pdo);
+out_return:
+ return ret;
+}
+
+/****************************************************************************/
+
+/** PDO destructor.
+ */
+void ec_pdo_clear(ec_pdo_t *pdo /**< EtherCAT PDO. */)
+{
+ if (pdo->name)
+ kfree(pdo->name);
+
+ ec_pdo_clear_entries(pdo);
+}
+
+/****************************************************************************/
+
+/** Clear PDO entry list.
+ */
+void ec_pdo_clear_entries(ec_pdo_t *pdo /**< EtherCAT PDO. */)
+{
+ ec_pdo_entry_t *entry, *next;
+
+ // free all PDO entries
+ list_for_each_entry_safe(entry, next, &pdo->entries, list) {
+ list_del(&entry->list);
+ ec_pdo_entry_clear(entry);
+ kfree(entry);
+ }
+}
+
+/****************************************************************************/
+
+/** Set PDO name.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_pdo_set_name(
+ ec_pdo_t *pdo, /**< PDO. */
+ const char *name /**< New name. */
+ )
+{
+ unsigned int len;
+
+ if (pdo->name && name && !strcmp(pdo->name, name))
+ return 0;
+
+ if (pdo->name)
+ kfree(pdo->name);
+
+ if (name && (len = strlen(name))) {
+ if (!(pdo->name = (char *) kmalloc(len + 1, GFP_KERNEL))) {
+ EC_ERR("Failed to allocate PDO name.\n");
+ return -ENOMEM;
+ }
+ memcpy(pdo->name, name, len + 1);
+ } else {
+ pdo->name = NULL;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Add a new PDO entry to the configuration.
+ *
+ * \retval Pointer to the added entry, otherwise a ERR_PTR() code.
+ */
+ec_pdo_entry_t *ec_pdo_add_entry(
+ ec_pdo_t *pdo, /**< PDO. */
+ uint16_t index, /**< New entry's index. */
+ uint8_t subindex, /**< New entry's subindex. */
+ uint8_t bit_length /**< New entry's bit length. */
+ )
+{
+ ec_pdo_entry_t *entry;
+
+ if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
+ EC_ERR("Failed to allocate memory for PDO entry.\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ ec_pdo_entry_init(entry);
+ entry->index = index;
+ entry->subindex = subindex;
+ entry->bit_length = bit_length;
+ list_add_tail(&entry->list, &pdo->entries);
+ return entry;
+}
+
+/****************************************************************************/
+
+/** Copy PDO entries from another PDO.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_pdo_copy_entries(
+ ec_pdo_t *pdo, /**< PDO whos entries shall be replaced. */
+ const ec_pdo_t *other /**< Pdo with entries to copy. */
+ )
+{
+ ec_pdo_entry_t *entry, *other_entry;
+ int ret;
+
+ ec_pdo_clear_entries(pdo);
+
+ list_for_each_entry(other_entry, &other->entries, list) {
+ if (!(entry = (ec_pdo_entry_t *)
+ kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
+ EC_ERR("Failed to allocate memory for PDO entry copy.\n");
+ return -ENOMEM;
+ }
+
+ ret = ec_pdo_entry_init_copy(entry, other_entry);
+ if (ret < 0) {
+ kfree(entry);
+ return ret;
+ }
+
+ list_add_tail(&entry->list, &pdo->entries);
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Compares the entries of two PDOs.
+ *
+ * \retval 1 The entries of the given PDOs are equal.
+ * \retval 0 The entries of the given PDOs differ.
+ */
+int ec_pdo_equal_entries(
+ const ec_pdo_t *pdo1, /**< First PDO. */
+ const ec_pdo_t *pdo2 /**< Second PDO. */
+ )
+{
+ const struct list_head *head1, *head2, *item1, *item2;
+ const ec_pdo_entry_t *entry1, *entry2;
+
+ head1 = item1 = &pdo1->entries;
+ head2 = item2 = &pdo2->entries;
+
+ while (1) {
+ item1 = item1->next;
+ item2 = item2->next;
+
+ if ((item1 == head1) ^ (item2 == head2)) // unequal lengths
+ return 0;
+ if (item1 == head1) // both finished
+ break;
+
+ entry1 = list_entry(item1, ec_pdo_entry_t, list);
+ entry2 = list_entry(item2, ec_pdo_entry_t, list);
+ if (!ec_pdo_entry_equal(entry1, entry2))
+ return 0;
+ }
+
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Get the number of PDO entries.
+ *
+ * \return Number of PDO entries.
+ */
+unsigned int ec_pdo_entry_count(
+ const ec_pdo_t *pdo /**< PDO. */
+ )
+{
+ const ec_pdo_entry_t *entry;
+ unsigned int num = 0;
+
+ list_for_each_entry(entry, &pdo->entries, list) {
+ num++;
+ }
+
+ return num;
+}
+
+/****************************************************************************/
+
+/** Finds a PDO entry via its position in the list.
+ *
+ * Const version.
+ *
+ * \return Search result, or NULL.
+ */
+const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(
+ const ec_pdo_t *pdo, /**< PDO. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ const ec_pdo_entry_t *entry;
+
+ list_for_each_entry(entry, &pdo->entries, list) {
+ if (pos--)
+ continue;
+ return entry;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Outputs the PDOs in the list.
+ */
+void ec_pdo_print_entries(
+ const ec_pdo_t *pdo /**< PDO. */
+ )
+{
+ const ec_pdo_entry_t *entry;
+
+ if (list_empty(&pdo->entries)) {
+ printk(KERN_CONT "(none)");
+ } else {
+ list_for_each_entry(entry, &pdo->entries, list) {
+ printk(KERN_CONT "0x%04X:%02X/%u",
+ entry->index, entry->subindex, entry->bit_length);
+ if (entry->list.next != &pdo->entries)
+ printk(KERN_CONT " ");
+ }
+ }
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/pdo.h b/ubuntu/igh-ecat/master/pdo.h
new file mode 100644
index 00000000000000..1819c836578f23
--- /dev/null
+++ b/ubuntu/igh-ecat/master/pdo.h
@@ -0,0 +1,67 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT Process data object structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_PDO_H__
+#define __EC_PDO_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+#include "pdo_entry.h"
+
+/****************************************************************************/
+
+/** PDO description.
+ */
+typedef struct {
+ struct list_head list; /**< List item. */
+ uint16_t index; /**< PDO index. */
+ int8_t sync_index; /**< Assigned sync manager. \todo remove? */
+ char *name; /**< PDO name. */
+ struct list_head entries; /**< List of PDO entries. */
+} ec_pdo_t;
+
+/****************************************************************************/
+
+void ec_pdo_init(ec_pdo_t *);
+int ec_pdo_init_copy(ec_pdo_t *, const ec_pdo_t *);
+void ec_pdo_clear(ec_pdo_t *);
+void ec_pdo_clear_entries(ec_pdo_t *);
+int ec_pdo_set_name(ec_pdo_t *, const char *);
+ec_pdo_entry_t *ec_pdo_add_entry(ec_pdo_t *, uint16_t, uint8_t, uint8_t);
+int ec_pdo_copy_entries(ec_pdo_t *, const ec_pdo_t *);
+int ec_pdo_equal_entries(const ec_pdo_t *, const ec_pdo_t *);
+unsigned int ec_pdo_entry_count(const ec_pdo_t *);
+const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(
+ const ec_pdo_t *, unsigned int);
+
+void ec_pdo_print_entries(const ec_pdo_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/pdo_entry.c b/ubuntu/igh-ecat/master/pdo_entry.c
new file mode 100644
index 00000000000000..836cb3df45979a
--- /dev/null
+++ b/ubuntu/igh-ecat/master/pdo_entry.c
@@ -0,0 +1,124 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT process data object entry methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "pdo_entry.h"
+
+/****************************************************************************/
+
+/** PDO entry constructor.
+ */
+void ec_pdo_entry_init(
+ ec_pdo_entry_t *entry /**< PDO entry. */
+ )
+{
+ entry->name = NULL;
+}
+
+/****************************************************************************/
+
+/** PDO entry copy constructor.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_pdo_entry_init_copy(
+ ec_pdo_entry_t *entry, /**< PDO entry. */
+ const ec_pdo_entry_t *other /**< PDO entry to copy from. */
+ )
+{
+ entry->index = other->index;
+ entry->subindex = other->subindex;
+ entry->name = NULL;
+ entry->bit_length = other->bit_length;
+
+ return ec_pdo_entry_set_name(entry, other->name);
+}
+
+/****************************************************************************/
+
+/** PDO entry destructor.
+ */
+void ec_pdo_entry_clear(ec_pdo_entry_t *entry /**< PDO entry. */)
+{
+ if (entry->name)
+ kfree(entry->name);
+}
+
+/****************************************************************************/
+
+/** Set PDO entry name.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_pdo_entry_set_name(
+ ec_pdo_entry_t *entry, /**< PDO entry. */
+ const char *name /**< New name. */
+ )
+{
+ unsigned int len;
+
+ if (entry->name && name && !strcmp(entry->name, name))
+ return 0;
+
+ if (entry->name)
+ kfree(entry->name);
+
+ if (name && (len = strlen(name))) {
+ if (!(entry->name = (char *) kmalloc(len + 1, GFP_KERNEL))) {
+ EC_ERR("Failed to allocate PDO entry name.\n");
+ return -ENOMEM;
+ }
+ memcpy(entry->name, name, len + 1);
+ } else {
+ entry->name = NULL;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Compares two PDO entries.
+ *
+ * \retval 1 The entries are equal.
+ * \retval 0 The entries differ.
+ */
+int ec_pdo_entry_equal(
+ const ec_pdo_entry_t *entry1, /**< First PDO entry. */
+ const ec_pdo_entry_t *entry2 /**< Second PDO entry. */
+ )
+{
+ return entry1->index == entry2->index
+ && entry1->subindex == entry2->subindex
+ && entry1->bit_length == entry2->bit_length;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/pdo_entry.h b/ubuntu/igh-ecat/master/pdo_entry.h
new file mode 100644
index 00000000000000..96dc41ebfd57be
--- /dev/null
+++ b/ubuntu/igh-ecat/master/pdo_entry.h
@@ -0,0 +1,58 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT Process data object structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_PDO_ENTRY_H__
+#define __EC_PDO_ENTRY_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** PDO entry description.
+ */
+typedef struct {
+ struct list_head list; /**< list item */
+ uint16_t index; /**< PDO entry index */
+ uint8_t subindex; /**< PDO entry subindex */
+ char *name; /**< entry name */
+ uint8_t bit_length; /**< entry length in bit */
+} ec_pdo_entry_t;
+
+/****************************************************************************/
+
+void ec_pdo_entry_init(ec_pdo_entry_t *);
+int ec_pdo_entry_init_copy(ec_pdo_entry_t *, const ec_pdo_entry_t *);
+void ec_pdo_entry_clear(ec_pdo_entry_t *);
+int ec_pdo_entry_set_name(ec_pdo_entry_t *, const char *);
+int ec_pdo_entry_equal(const ec_pdo_entry_t *, const ec_pdo_entry_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/pdo_list.c b/ubuntu/igh-ecat/master/pdo_list.c
new file mode 100644
index 00000000000000..f26b1c880c28e1
--- /dev/null
+++ b/ubuntu/igh-ecat/master/pdo_list.c
@@ -0,0 +1,338 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT PDO list methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/module.h>
+
+#include "globals.h"
+#include "pdo.h"
+#include "slave_config.h"
+#include "master.h"
+
+#include "pdo_list.h"
+
+/****************************************************************************/
+
+/** PDO list constructor.
+ */
+void ec_pdo_list_init(
+ ec_pdo_list_t *pl /**< PDO list. */
+ )
+{
+ INIT_LIST_HEAD(&pl->list);
+}
+
+/****************************************************************************/
+
+/** PDO list destructor.
+ */
+void ec_pdo_list_clear(ec_pdo_list_t *pl /**< PDO list. */)
+{
+ ec_pdo_list_clear_pdos(pl);
+}
+
+/****************************************************************************/
+
+/** Clears the list of mapped PDOs.
+ */
+void ec_pdo_list_clear_pdos(ec_pdo_list_t *pl /**< PDO list. */)
+{
+ ec_pdo_t *pdo, *next;
+
+ list_for_each_entry_safe(pdo, next, &pl->list, list) {
+ list_del_init(&pdo->list);
+ ec_pdo_clear(pdo);
+ kfree(pdo);
+ }
+}
+
+/****************************************************************************/
+
+/** Calculates the total size of the mapped PDO entries.
+ *
+ * \retval Data size in byte.
+ */
+uint16_t ec_pdo_list_total_size(
+ const ec_pdo_list_t *pl /**< PDO list. */
+ )
+{
+ unsigned int bit_size;
+ const ec_pdo_t *pdo;
+ const ec_pdo_entry_t *pdo_entry;
+ uint16_t byte_size;
+
+ bit_size = 0;
+ list_for_each_entry(pdo, &pl->list, list) {
+ list_for_each_entry(pdo_entry, &pdo->entries, list) {
+ bit_size += pdo_entry->bit_length;
+ }
+ }
+
+ if (bit_size % 8) // round up to full bytes
+ byte_size = bit_size / 8 + 1;
+ else
+ byte_size = bit_size / 8;
+
+ return byte_size;
+}
+
+/****************************************************************************/
+
+/** Add a new PDO to the list.
+ *
+ * \return Pointer to new PDO, otherwise an ERR_PTR() code.
+ */
+ec_pdo_t *ec_pdo_list_add_pdo(
+ ec_pdo_list_t *pl, /**< PDO list. */
+ uint16_t index /**< PDO index. */
+ )
+{
+ ec_pdo_t *pdo;
+
+ if (!(pdo = (ec_pdo_t *) kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
+ EC_ERR("Failed to allocate memory for PDO.\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ ec_pdo_init(pdo);
+ pdo->index = index;
+ list_add_tail(&pdo->list, &pl->list);
+ return pdo;
+}
+
+/****************************************************************************/
+
+/** Add the copy of an existing PDO to the list.
+ *
+ * \return 0 on success, else < 0
+ */
+int ec_pdo_list_add_pdo_copy(
+ ec_pdo_list_t *pl, /**< PDO list. */
+ const ec_pdo_t *pdo /**< PDO to add. */
+ )
+{
+ ec_pdo_t *mapped_pdo;
+ int ret;
+
+ // PDO already mapped?
+ list_for_each_entry(mapped_pdo, &pl->list, list) {
+ if (mapped_pdo->index != pdo->index) continue;
+ EC_ERR("PDO 0x%04X is already mapped!\n", pdo->index);
+ return -EEXIST;
+ }
+
+ if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
+ EC_ERR("Failed to allocate PDO memory.\n");
+ return -ENOMEM;
+ }
+
+ ret = ec_pdo_init_copy(mapped_pdo, pdo);
+ if (ret < 0) {
+ kfree(mapped_pdo);
+ return ret;
+ }
+
+ list_add_tail(&mapped_pdo->list, &pl->list);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Makes a deep copy of another PDO list.
+ *
+ * \return 0 on success, else < 0
+ */
+int ec_pdo_list_copy(
+ ec_pdo_list_t *pl, /**< PDO list. */
+ const ec_pdo_list_t *other /**< PDO list to copy from. */
+ )
+{
+ ec_pdo_t *other_pdo;
+ int ret;
+
+ ec_pdo_list_clear_pdos(pl);
+
+ // PDO already mapped?
+ list_for_each_entry(other_pdo, &other->list, list) {
+ ret = ec_pdo_list_add_pdo_copy(pl, other_pdo);
+ if (ret)
+ return ret;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Compares two PDO lists.
+ *
+ * Only the list is compared, not the PDO entries (i. e. the PDO
+ * mapping).
+ *
+ * \retval 1 The given PDO lists are equal.
+ * \retval 0 The given PDO lists differ.
+ */
+int ec_pdo_list_equal(
+ const ec_pdo_list_t *pl1, /**< First list. */
+ const ec_pdo_list_t *pl2 /**< Second list. */
+ )
+{
+ const struct list_head *h1, *h2, *l1, *l2;
+ const ec_pdo_t *p1, *p2;
+
+ h1 = l1 = &pl1->list;
+ h2 = l2 = &pl2->list;
+
+ while (1) {
+ l1 = l1->next;
+ l2 = l2->next;
+
+ if ((l1 == h1) ^ (l2 == h2)) // unequal lengths
+ return 0;
+ if (l1 == h1) // both finished
+ break;
+
+ p1 = list_entry(l1, ec_pdo_t, list);
+ p2 = list_entry(l2, ec_pdo_t, list);
+
+ if (p1->index != p2->index)
+ return 0;
+ }
+
+ return 1;
+}
+
+/****************************************************************************/
+
+/** Finds a PDO with the given index.
+ *
+ * \return Search result, or NULL.
+ */
+ec_pdo_t *ec_pdo_list_find_pdo(
+ const ec_pdo_list_t *pl, /**< PDO list. */
+ uint16_t index /**< PDO index. */
+ )
+{
+ ec_pdo_t *pdo;
+
+ list_for_each_entry(pdo, &pl->list, list) {
+ if (pdo->index != index)
+ continue;
+ return pdo;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Finds a PDO with the given index and returns a const pointer.
+ *
+ * \return Search result, or NULL.
+ */
+const ec_pdo_t *ec_pdo_list_find_pdo_const(
+ const ec_pdo_list_t *pl, /**< PDO list. */
+ uint16_t index /**< PDO index. */
+ )
+{
+ const ec_pdo_t *pdo;
+
+ list_for_each_entry(pdo, &pl->list, list) {
+ if (pdo->index != index)
+ continue;
+ return pdo;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Finds a PDO via its position in the list.
+ *
+ * Const version.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(
+ const ec_pdo_list_t *pl, /**< PDO list. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ const ec_pdo_t *pdo;
+
+ list_for_each_entry(pdo, &pl->list, list) {
+ if (pos--)
+ continue;
+ return pdo;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Get the number of PDOs in the list.
+ *
+ * \return Number of PDOs.
+ */
+unsigned int ec_pdo_list_count(
+ const ec_pdo_list_t *pl /**< PDO list. */
+ )
+{
+ const ec_pdo_t *pdo;
+ unsigned int num = 0;
+
+ list_for_each_entry(pdo, &pl->list, list) {
+ num++;
+ }
+
+ return num;
+}
+
+/****************************************************************************/
+
+/** Outputs the PDOs in the list.
+ */
+void ec_pdo_list_print(
+ const ec_pdo_list_t *pl /**< PDO list. */
+ )
+{
+ const ec_pdo_t *pdo;
+
+ if (list_empty(&pl->list)) {
+ printk(KERN_CONT "(none)");
+ } else {
+ list_for_each_entry(pdo, &pl->list, list) {
+ printk(KERN_CONT "0x%04X", pdo->index);
+ if (pdo->list.next != &pl->list)
+ printk(KERN_CONT " ");
+ }
+ }
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/pdo_list.h b/ubuntu/igh-ecat/master/pdo_list.h
new file mode 100644
index 00000000000000..5bd9be7f9a9035
--- /dev/null
+++ b/ubuntu/igh-ecat/master/pdo_list.h
@@ -0,0 +1,71 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT PDO list structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_PDO_LIST_H__
+#define __EC_PDO_LIST_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+#include "pdo.h"
+
+/****************************************************************************/
+
+/** EtherCAT PDO list.
+ */
+typedef struct {
+ struct list_head list; /**< List of PDOs. */
+} ec_pdo_list_t;
+
+/****************************************************************************/
+
+void ec_pdo_list_init(ec_pdo_list_t *);
+void ec_pdo_list_clear(ec_pdo_list_t *);
+
+void ec_pdo_list_clear_pdos(ec_pdo_list_t *);
+
+ec_pdo_t *ec_pdo_list_add_pdo(ec_pdo_list_t *, uint16_t);
+int ec_pdo_list_add_pdo_copy(ec_pdo_list_t *, const ec_pdo_t *);
+
+int ec_pdo_list_copy(ec_pdo_list_t *, const ec_pdo_list_t *);
+
+uint16_t ec_pdo_list_total_size(const ec_pdo_list_t *);
+int ec_pdo_list_equal(const ec_pdo_list_t *, const ec_pdo_list_t *);
+
+ec_pdo_t *ec_pdo_list_find_pdo(const ec_pdo_list_t *, uint16_t);
+const ec_pdo_t *ec_pdo_list_find_pdo_const(const ec_pdo_list_t *,
+ uint16_t);
+const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(
+ const ec_pdo_list_t *, unsigned int);
+unsigned int ec_pdo_list_count(const ec_pdo_list_t *);
+
+void ec_pdo_list_print(const ec_pdo_list_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/reg_request.c b/ubuntu/igh-ecat/master/reg_request.c
new file mode 100644
index 00000000000000..eab70b1d140b0c
--- /dev/null
+++ b/ubuntu/igh-ecat/master/reg_request.c
@@ -0,0 +1,125 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * Register request functions.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+
+#include "reg_request.h"
+
+/****************************************************************************/
+
+/** Register request constructor.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_reg_request_init(
+ ec_reg_request_t *reg, /**< Register request. */
+ size_t size /**< Memory size. */
+ )
+{
+ if (!(reg->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
+ EC_ERR("Failed to allocate %zu bytes of register memory.\n", size);
+ return -ENOMEM;
+ }
+
+ INIT_LIST_HEAD(®->list);
+ reg->mem_size = size;
+ memset(reg->data, 0x00, size);
+ reg->dir = EC_DIR_INVALID;
+ reg->address = 0;
+ reg->transfer_size = 0;
+ reg->state = EC_INT_REQUEST_INIT;
+ reg->ring_position = 0;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Register request destructor.
+ */
+void ec_reg_request_clear(
+ ec_reg_request_t *reg /**< Register request. */
+ )
+{
+ if (reg->data) {
+ kfree(reg->data);
+ }
+}
+
+/*****************************************************************************
+ * Application interface.
+ ****************************************************************************/
+
+uint8_t *ecrt_reg_request_data(const ec_reg_request_t *reg)
+{
+ return reg->data;
+}
+
+/****************************************************************************/
+
+ec_request_state_t ecrt_reg_request_state(const ec_reg_request_t *reg)
+{
+ return ec_request_state_translation_table[reg->state];
+}
+
+/****************************************************************************/
+
+int ecrt_reg_request_write(ec_reg_request_t *reg, uint16_t address,
+ size_t size)
+{
+ reg->dir = EC_DIR_OUTPUT;
+ reg->address = address;
+ reg->transfer_size = min(size, reg->mem_size);
+ reg->state = EC_INT_REQUEST_QUEUED;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_reg_request_read(ec_reg_request_t *reg, uint16_t address,
+ size_t size)
+{
+ reg->dir = EC_DIR_INPUT;
+ reg->address = address;
+ reg->transfer_size = min(size, reg->mem_size);
+ reg->state = EC_INT_REQUEST_QUEUED;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecrt_reg_request_data);
+EXPORT_SYMBOL(ecrt_reg_request_state);
+EXPORT_SYMBOL(ecrt_reg_request_write);
+EXPORT_SYMBOL(ecrt_reg_request_read);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/reg_request.h b/ubuntu/igh-ecat/master/reg_request.h
new file mode 100644
index 00000000000000..86876b6479169c
--- /dev/null
+++ b/ubuntu/igh-ecat/master/reg_request.h
@@ -0,0 +1,59 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT register request structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_REG_REQUEST_H__
+#define __EC_REG_REQUEST_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** Register request.
+ */
+struct ec_reg_request {
+ struct list_head list; /**< List item. */
+ size_t mem_size; /**< Size of data memory. */
+ uint8_t *data; /**< Pointer to data memory. */
+ ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means writing to the
+ slave, EC_DIR_INPUT means reading from the slave. */
+ uint16_t address; /**< Register address. */
+ size_t transfer_size; /**< Size of the data to transfer. */
+ ec_internal_request_state_t state; /**< Request state. */
+ uint16_t ring_position; /**< Ring position for emergency requests. */
+};
+
+/****************************************************************************/
+
+int ec_reg_request_init(ec_reg_request_t *, size_t);
+void ec_reg_request_clear(ec_reg_request_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/rt_locks.h b/ubuntu/igh-ecat/master/rt_locks.h
new file mode 100644
index 00000000000000..bfe592171f29c2
--- /dev/null
+++ b/ubuntu/igh-ecat/master/rt_locks.h
@@ -0,0 +1,75 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Abstract locks for realtime part of interface.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_LOCKS_H__
+#define __EC_LOCKS_H__
+
+#include "globals.h"
+#include <linux/version.h>
+
+#include <linux/semaphore.h>
+
+/****************************************************************************/
+
+#ifdef EC_USE_RTMUTEX
+
+#include <linux/rtmutex.h>
+
+typedef struct rt_mutex ec_lock_t;
+
+static inline void ec_lock_init(ec_lock_t *sem) { rt_mutex_init(sem); }
+static inline void ec_lock_down(ec_lock_t *sem) { rt_mutex_lock(sem); }
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 34)
+static inline int ec_lock_down_interruptible(ec_lock_t *sem) {
+ return rt_mutex_lock_interruptible(sem);
+}
+#else
+static inline int ec_lock_down_interruptible(ec_lock_t *sem) {
+ return rt_mutex_lock_interruptible(sem, 1);
+}
+#endif
+static inline void ec_lock_up(ec_lock_t *sem) { rt_mutex_unlock(sem); }
+
+#else
+
+typedef struct semaphore ec_lock_t;
+
+static inline void ec_lock_init(ec_lock_t *sem) { sema_init(sem, 1); }
+static inline void ec_lock_down(ec_lock_t *sem) { down(sem); }
+static inline int ec_lock_down_interruptible(ec_lock_t *sem) {
+ return down_interruptible(sem);
+}
+static inline void ec_lock_up(ec_lock_t *sem) { up(sem); }
+
+#endif
+
+/****************************************************************************/
+
+#endif
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/rtdm-ioctl.c b/ubuntu/igh-ecat/master/rtdm-ioctl.c
new file mode 120000
index 00000000000000..738e33d1a82792
--- /dev/null
+++ b/ubuntu/igh-ecat/master/rtdm-ioctl.c
@@ -0,0 +1 @@
+ioctl.c
\ No newline at end of file
diff --git a/ubuntu/igh-ecat/master/rtdm.c b/ubuntu/igh-ecat/master/rtdm.c
new file mode 100644
index 00000000000000..0112e1a7ba166e
--- /dev/null
+++ b/ubuntu/igh-ecat/master/rtdm.c
@@ -0,0 +1,259 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2009-2010 Moehwald GmbH B. Benner
+ * 2011 IgH Andreas Stewering-Bone
+ * 2012 Florian Pose <fp at igh.de>
+ *
+ * This file is part of the IgH EtherCAT master.
+ *
+ * The IgH EtherCAT master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation; version 2 of the License.
+ *
+ * The IgH EtherCAT master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT master. If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/** \file
+ * RTDM interface.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/mman.h>
+
+
+#include "master.h"
+#include "ioctl.h"
+#include "rtdm.h"
+#include "rtdm_details.h"
+
+/* include last because it does some redefinitions */
+#include <rtdm/rtdm_driver.h>
+
+/** Set to 1 to enable device operations debugging.
+ */
+#define DEBUG 0
+
+/****************************************************************************/
+
+static int ec_rtdm_open(struct rtdm_dev_context *, rtdm_user_info_t *, int);
+static int ec_rtdm_close(struct rtdm_dev_context *, rtdm_user_info_t *);
+static int ec_rtdm_ioctl_nrt_handler(struct rtdm_dev_context *,
+ rtdm_user_info_t *, unsigned int, void __user *);
+static int ec_rtdm_ioctl_rt_handler(struct rtdm_dev_context *,
+ rtdm_user_info_t *, unsigned int, void __user *);
+
+/****************************************************************************/
+
+/** Initialize an RTDM device.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_rtdm_dev_init(
+ ec_rtdm_dev_t *rtdm_dev, /**< EtherCAT RTDM device. */
+ ec_master_t *master /**< EtherCAT master. */
+ )
+{
+ int ret;
+
+ rtdm_dev->master = master;
+
+ rtdm_dev->dev = kzalloc(sizeof(struct rtdm_device), GFP_KERNEL);
+ if (!rtdm_dev->dev) {
+ EC_MASTER_ERR(master, "Failed to reserve memory for RTDM device.\n");
+ return -ENOMEM;
+ }
+
+ rtdm_dev->dev->struct_version = RTDM_DEVICE_STRUCT_VER;
+ rtdm_dev->dev->device_flags = RTDM_NAMED_DEVICE;
+ rtdm_dev->dev->context_size = sizeof(ec_rtdm_context_t);
+ snprintf(rtdm_dev->dev->device_name, RTDM_MAX_DEVNAME_LEN,
+ "EtherCAT%u", master->index);
+ rtdm_dev->dev->open_nrt = ec_rtdm_open;
+ rtdm_dev->dev->ops.close_nrt = ec_rtdm_close;
+ rtdm_dev->dev->ops.ioctl_rt = ec_rtdm_ioctl_rt_handler;
+ rtdm_dev->dev->ops.ioctl_nrt = ec_rtdm_ioctl_nrt_handler;
+ rtdm_dev->dev->device_class = RTDM_CLASS_EXPERIMENTAL;
+ rtdm_dev->dev->device_sub_class = 222;
+ rtdm_dev->dev->driver_name = "EtherCAT";
+ rtdm_dev->dev->driver_version = RTDM_DRIVER_VER(1, 0, 2);
+ rtdm_dev->dev->peripheral_name = rtdm_dev->dev->device_name;
+ rtdm_dev->dev->provider_name = "EtherLab Community";
+ rtdm_dev->dev->proc_name = rtdm_dev->dev->device_name;
+ rtdm_dev->dev->device_data = rtdm_dev; /* pointer to parent */
+
+ EC_MASTER_INFO(master, "Registering RTDM device %s.\n",
+ rtdm_dev->dev->driver_name);
+ ret = rtdm_dev_register(rtdm_dev->dev);
+ if (ret) {
+ EC_MASTER_ERR(master, "Initialization of RTDM interface failed"
+ " (return value %i).\n", ret);
+ kfree(rtdm_dev->dev);
+ }
+
+ return ret;
+}
+
+/****************************************************************************/
+
+/** Clear an RTDM device.
+ */
+void ec_rtdm_dev_clear(
+ ec_rtdm_dev_t *rtdm_dev /**< EtherCAT RTDM device. */
+ )
+{
+ int ret;
+
+ EC_MASTER_INFO(rtdm_dev->master, "Unregistering RTDM device %s.\n",
+ rtdm_dev->dev->driver_name);
+ ret = rtdm_dev_unregister(rtdm_dev->dev, 1000 /* poll delay [ms] */);
+ if (ret < 0) {
+ EC_MASTER_WARN(rtdm_dev->master,
+ "Failed to unregister RTDM device (code %i).\n", ret);
+ }
+
+ kfree(rtdm_dev->dev);
+}
+
+/****************************************************************************/
+
+/** Driver open.
+ *
+ * \return Always zero (success).
+ */
+static int ec_rtdm_open(
+ struct rtdm_dev_context *context, /**< Context. */
+ rtdm_user_info_t *user_info, /**< User data. */
+ int oflags /**< Open flags. */
+ )
+{
+ ec_rtdm_context_t *ctx = (ec_rtdm_context_t *) context->dev_private;
+#if DEBUG
+ ec_rtdm_dev_t *rtdm_dev = (ec_rtdm_dev_t *) context->device->device_data;
+#endif
+
+ ctx->user_fd = user_info;
+ ctx->ioctl_ctx.writable = oflags & O_WRONLY || oflags & O_RDWR;
+ ctx->ioctl_ctx.requested = 0;
+ ctx->ioctl_ctx.process_data = NULL;
+ ctx->ioctl_ctx.process_data_size = 0;
+
+#if DEBUG
+ EC_MASTER_INFO(rtdm_dev->master, "RTDM device %s opened.\n",
+ context->device->device_name);
+#endif
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Driver close.
+ *
+ * \return Always zero (success).
+ */
+static int ec_rtdm_close(
+ struct rtdm_dev_context *context, /**< Context. */
+ rtdm_user_info_t *user_info /**< User data. */
+ )
+{
+ ec_rtdm_context_t *ctx = (ec_rtdm_context_t *) context->dev_private;
+ ec_rtdm_dev_t *rtdm_dev = (ec_rtdm_dev_t *) context->device->device_data;
+
+ if (ctx->ioctl_ctx.requested) {
+ ecrt_release_master(rtdm_dev->master);
+ }
+
+#if DEBUG
+ EC_MASTER_INFO(rtdm_dev->master, "RTDM device %s closed.\n",
+ context->device->device_name);
+#endif
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Driver ioctl.
+ *
+ * \return ioctl() return code.
+ */
+static int ec_rtdm_ioctl_nrt_handler(
+ struct rtdm_dev_context *context, /**< Context. */
+ rtdm_user_info_t *user_info, /**< User data. */
+ unsigned int request, /**< Request. */
+ void __user *arg /**< Argument. */
+ )
+{
+ ec_rtdm_context_t *ctx = (ec_rtdm_context_t *) context->dev_private;
+ ec_rtdm_dev_t *rtdm_dev = (ec_rtdm_dev_t *) context->device->device_data;
+
+#if DEBUG
+ EC_MASTER_INFO(rtdm_dev->master, "ioctl(request = %u, ctl = %02x)"
+ " on RTDM device %s.\n", request, _IOC_NR(request),
+ context->device->device_name);
+#endif
+ return ec_ioctl_rtdm_nrt(rtdm_dev->master, &ctx->ioctl_ctx, request, arg);
+}
+
+/****************************************************************************/
+
+static int ec_rtdm_ioctl_rt_handler(
+ struct rtdm_dev_context *context, /**< Context. */
+ rtdm_user_info_t *user_info, /**< User data. */
+ unsigned int request, /**< Request. */
+ void __user *arg /**< Argument. */
+ )
+{
+ int result;
+ ec_rtdm_context_t *ctx = (ec_rtdm_context_t *) context->dev_private;
+ ec_rtdm_dev_t *rtdm_dev = (ec_rtdm_dev_t *) context->device->device_data;
+
+#if DEBUG
+ EC_MASTER_INFO(rtdm_dev->master, "ioctl(request = %u, ctl = %02x)"
+ " on RTDM device %s.\n", request, _IOC_NR(request),
+ context->device->device_name);
+#endif
+ result =
+ ec_ioctl_rtdm_rt(rtdm_dev->master, &ctx->ioctl_ctx, request, arg);
+
+ if (result == -ENOTTY) {
+ /* Try again with nrt ioctl handler above in secondary mode. */
+ return -ENOSYS;
+ }
+ return result;
+}
+
+/****************************************************************************/
+
+/** Memory-map process data to user space.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_rtdm_mmap(
+ ec_ioctl_context_t *ioctl_ctx, /**< Context. */
+ void **user_address /**< Userspace address. */
+ )
+{
+ ec_rtdm_context_t *ctx =
+ container_of(ioctl_ctx, ec_rtdm_context_t, ioctl_ctx);
+ int ret;
+
+ ret = rtdm_mmap_to_user(ctx->user_fd,
+ ioctl_ctx->process_data, ioctl_ctx->process_data_size,
+ PROT_READ | PROT_WRITE,
+ user_address,
+ NULL, NULL);
+ if (ret < 0) {
+ return ret;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/rtdm.h b/ubuntu/igh-ecat/master/rtdm.h
new file mode 100644
index 00000000000000..3c2fa1380a0b3f
--- /dev/null
+++ b/ubuntu/igh-ecat/master/rtdm.h
@@ -0,0 +1,50 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2012 Florian Pose <fp at igh.de>
+ *
+ * This file is part of the IgH EtherCAT master.
+ *
+ * The IgH EtherCAT master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation; version 2 of the License.
+ *
+ * The IgH EtherCAT master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT master. If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/** \file
+ * RTDM interface.
+ */
+
+#ifndef __EC_RTDM_H__
+#define __EC_RTDM_H__
+
+#include "../include/ecrt.h" /* ec_master_t */
+
+/****************************************************************************/
+
+struct rtdm_device;
+
+/****************************************************************************/
+
+/** EtherCAT RTDM device.
+ */
+typedef struct ec_rtdm_dev {
+ ec_master_t *master; /**< Master pointer. */
+ struct rtdm_device *dev; /**< RTDM device. */
+} ec_rtdm_dev_t;
+
+/****************************************************************************/
+
+int ec_rtdm_dev_init(ec_rtdm_dev_t *, ec_master_t *);
+void ec_rtdm_dev_clear(ec_rtdm_dev_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/rtdm_details.h b/ubuntu/igh-ecat/master/rtdm_details.h
new file mode 100644
index 00000000000000..d57ce01e5febc3
--- /dev/null
+++ b/ubuntu/igh-ecat/master/rtdm_details.h
@@ -0,0 +1,60 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2024 Bjarne von Horn <vh at igh.de>
+ *
+ * This file is part of the IgH EtherCAT master.
+ *
+ * The IgH EtherCAT master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation; version 2 of the License.
+ *
+ * The IgH EtherCAT master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT master. If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+#ifndef __EC_RTDM_DETAILS_H__
+#define __EC_RTDM_DETAILS_H__
+
+#include "../config.h"
+#include "ioctl.h"
+
+#include <linux/kernel.h>
+
+#ifdef EC_RTDM_XENOMAI_V3
+
+#include <rtdm/driver.h>
+#define EC_RTDM_USERFD_T struct rtdm_fd
+
+#else // EC_RTDM_XENOMAI_V3
+
+#include <rtdm/rtdm_driver.h>
+
+#define EC_RTDM_USERFD_T rtdm_user_info_t
+
+#endif // EC_RTDM_XENOMAI_V3
+
+/****************************************************************************/
+
+/** Context structure for an open RTDM file handle.
+ */
+typedef struct ec_rtdm_context {
+ EC_RTDM_USERFD_T *user_fd; /**< RTDM user data. */
+ ec_ioctl_context_t ioctl_ctx; /**< Context structure. */
+} ec_rtdm_context_t;
+
+/****************************************************************************/
+
+static inline EC_RTDM_USERFD_T *ec_ioctl_to_rtdm(ec_ioctl_context_t *ctx)
+{
+ return container_of(ctx, ec_rtdm_context_t, ioctl_ctx)->user_fd;
+}
+
+/****************************************************************************/
+
+#endif // __EC_RTDM_DETAILS_H__
diff --git a/ubuntu/igh-ecat/master/rtdm_xenomai_v3.c b/ubuntu/igh-ecat/master/rtdm_xenomai_v3.c
new file mode 100644
index 00000000000000..230e4172f9c778
--- /dev/null
+++ b/ubuntu/igh-ecat/master/rtdm_xenomai_v3.c
@@ -0,0 +1,193 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2009-2010 Moehwald GmbH B. Benner
+ * 2011 IgH Andreas Stewering-Bone
+ * 2012 Florian Pose <fp at igh.de>
+ *
+ * This file is part of the IgH EtherCAT master.
+ *
+ * The IgH EtherCAT master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation; version 2 of the License.
+ *
+ * The IgH EtherCAT master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT master. If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/** \file
+ * RTDM interface.
+ */
+
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include <rtdm/driver.h>
+
+#include "master.h"
+#include "ioctl.h"
+#include "rtdm.h"
+#include "rtdm_details.h"
+
+/** Set to 1 to enable device operations debugging.
+ */
+#define DEBUG_RTDM 0
+
+/****************************************************************************/
+
+static int ec_rtdm_open(struct rtdm_fd *fd, int oflags)
+{
+ struct ec_rtdm_context *ctx = rtdm_fd_to_private(fd);
+#if DEBUG_RTDM
+ struct rtdm_device *dev = rtdm_fd_device(fd);
+ ec_rtdm_dev_t *rtdm_dev = dev->device_data;
+#endif
+
+ ctx->user_fd = fd;
+
+ ctx->ioctl_ctx.writable = oflags & O_WRONLY || oflags & O_RDWR;
+ ctx->ioctl_ctx.requested = 0;
+ ctx->ioctl_ctx.process_data = NULL;
+ ctx->ioctl_ctx.process_data_size = 0;
+
+#if DEBUG_RTDM
+ EC_MASTER_INFO(rtdm_dev->master, "RTDM device %s opened.\n",
+ dev->name);
+#endif
+
+ return 0;
+}
+
+/****************************************************************************/
+
+static void ec_rtdm_close(struct rtdm_fd *fd)
+{
+ struct ec_rtdm_context *ctx = rtdm_fd_to_private(fd);
+ struct rtdm_device *dev = rtdm_fd_device(fd);
+ ec_rtdm_dev_t *rtdm_dev = dev->device_data;
+
+ if (ctx->ioctl_ctx.requested)
+ ecrt_release_master(rtdm_dev->master);
+
+ if (ctx->ioctl_ctx.process_data)
+ vfree(ctx->ioctl_ctx.process_data);
+
+#if DEBUG_RTDM
+ EC_MASTER_INFO(rtdm_dev->master, "RTDM device %s closed.\n",
+ dev->name);
+#endif
+}
+
+/****************************************************************************/
+
+static int ec_rtdm_ioctl_rt_handler(struct rtdm_fd *fd, unsigned int request,
+ void __user *arg)
+{
+ int result;
+ struct ec_rtdm_context *ctx = rtdm_fd_to_private(fd);
+ struct rtdm_device *dev = rtdm_fd_device(fd);
+ ec_rtdm_dev_t *rtdm_dev = dev->device_data;
+
+ result =
+ ec_ioctl_rtdm_rt(rtdm_dev->master, &ctx->ioctl_ctx, request, arg);
+
+ if (result == -ENOTTY)
+ /* Try again with nrt ioctl handler below in secondary mode. */
+ return -ENOSYS;
+
+ return result;
+}
+
+/****************************************************************************/
+
+static int ec_rtdm_ioctl_nrt_handler(struct rtdm_fd *fd, unsigned int request,
+ void __user *arg)
+{
+ struct ec_rtdm_context *ctx = rtdm_fd_to_private(fd);
+ struct rtdm_device *dev = rtdm_fd_device(fd);
+ ec_rtdm_dev_t *rtdm_dev = dev->device_data;
+
+ return ec_ioctl_rtdm_nrt(rtdm_dev->master, &ctx->ioctl_ctx, request, arg);
+}
+
+/****************************************************************************/
+
+static int ec_rtdm_mmap(struct rtdm_fd *fd, struct vm_area_struct *vma)
+{
+ struct ec_rtdm_context *ctx =
+ (struct ec_rtdm_context *) rtdm_fd_to_private(fd);
+ return rtdm_mmap_kmem(vma, (void *)ctx->ioctl_ctx.process_data);
+}
+
+/****************************************************************************/
+
+static struct rtdm_driver ec_rtdm_driver = {
+ .profile_info = RTDM_PROFILE_INFO(ec_rtdm,
+ RTDM_CLASS_EXPERIMENTAL,
+ 222,
+ 0),
+ .device_flags = RTDM_NAMED_DEVICE,
+ .device_count = EC_MAX_MASTERS,
+ .context_size = sizeof(struct ec_rtdm_context),
+ .ops = {
+ .open = ec_rtdm_open,
+ .close = ec_rtdm_close,
+ .ioctl_rt = ec_rtdm_ioctl_rt_handler,
+ .ioctl_nrt = ec_rtdm_ioctl_nrt_handler,
+ .mmap = ec_rtdm_mmap,
+ },
+};
+
+/****************************************************************************/
+
+int ec_rtdm_dev_init(ec_rtdm_dev_t *rtdm_dev, ec_master_t *master)
+{
+ struct rtdm_device *dev;
+ int ret;
+
+ rtdm_dev->master = master;
+
+ rtdm_dev->dev = kzalloc(sizeof(struct rtdm_device), GFP_KERNEL);
+ if (!rtdm_dev->dev) {
+ EC_MASTER_ERR(master,
+ "Failed to reserve memory for RTDM device.\n");
+ return -ENOMEM;
+ }
+
+ dev = rtdm_dev->dev;
+
+ dev->driver = &ec_rtdm_driver;
+ dev->device_data = rtdm_dev;
+ dev->label = "EtherCAT%u";
+ dev->minor = master->index;
+
+ ret = rtdm_dev_register(dev);
+ if (ret) {
+ EC_MASTER_ERR(master, "Initialization of RTDM interface failed"
+ " (return value %i).\n", ret);
+ kfree(dev);
+ return ret;
+ }
+
+ EC_MASTER_INFO(master, "Registered RTDM device %s.\n", dev->name);
+
+ return 0;
+}
+
+/****************************************************************************/
+
+void ec_rtdm_dev_clear(ec_rtdm_dev_t *rtdm_dev)
+{
+ rtdm_dev_unregister(rtdm_dev->dev);
+
+ EC_MASTER_INFO(rtdm_dev->master, "Unregistered RTDM device %s.\n",
+ rtdm_dev->dev->name);
+
+ kfree(rtdm_dev->dev);
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/sdo.c b/ubuntu/igh-ecat/master/sdo.c
new file mode 100644
index 00000000000000..3b8d3ac6ce7a1b
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sdo.c
@@ -0,0 +1,124 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ CANopen SDO functions.
+*/
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "master.h"
+
+#include "sdo.h"
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_sdo_init(
+ ec_sdo_t *sdo, /**< SDO. */
+ ec_slave_t *slave, /**< Parent slave. */
+ uint16_t index /**< SDO index. */
+ )
+{
+ sdo->slave = slave;
+ sdo->index = index;
+ sdo->object_code = 0x00;
+ sdo->name = NULL;
+ sdo->max_subindex = 0;
+ INIT_LIST_HEAD(&sdo->entries);
+}
+
+/****************************************************************************/
+
+/** SDO destructor.
+ *
+ * Clears and frees an SDO object.
+ */
+void ec_sdo_clear(
+ ec_sdo_t *sdo /**< SDO. */
+ )
+{
+ ec_sdo_entry_t *entry, *next;
+
+ // free all entries
+ list_for_each_entry_safe(entry, next, &sdo->entries, list) {
+ list_del(&entry->list);
+ ec_sdo_entry_clear(entry);
+ kfree(entry);
+ }
+
+ if (sdo->name)
+ kfree(sdo->name);
+}
+
+/****************************************************************************/
+
+/** Get an SDO entry from an SDO via its subindex.
+ *
+ * \retval >0 Pointer to the requested SDO entry.
+ * \retval NULL SDO entry not found.
+ */
+ec_sdo_entry_t *ec_sdo_get_entry(
+ ec_sdo_t *sdo, /**< SDO. */
+ uint8_t subindex /**< Entry subindex. */
+ )
+{
+ ec_sdo_entry_t *entry;
+
+ list_for_each_entry(entry, &sdo->entries, list) {
+ if (entry->subindex != subindex)
+ continue;
+ return entry;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Get an SDO entry from an SDO via its subindex.
+ *
+ * const version.
+ *
+ * \retval >0 Pointer to the requested SDO entry.
+ * \retval NULL SDO entry not found.
+ */
+const ec_sdo_entry_t *ec_sdo_get_entry_const(
+ const ec_sdo_t *sdo, /**< SDO. */
+ uint8_t subindex /**< Entry subindex. */
+ )
+{
+ const ec_sdo_entry_t *entry;
+
+ list_for_each_entry(entry, &sdo->entries, list) {
+ if (entry->subindex != subindex)
+ continue;
+ return entry;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/sdo.h b/ubuntu/igh-ecat/master/sdo.h
new file mode 100644
index 00000000000000..5426bcaa2ec7f3
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sdo.h
@@ -0,0 +1,61 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT CANopen SDO structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_SDO_H__
+#define __EC_SDO_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+#include "sdo_entry.h"
+
+/****************************************************************************/
+
+/** CANopen SDO.
+ */
+struct ec_sdo {
+ struct list_head list; /**< List item. */
+ ec_slave_t *slave; /**< Parent slave. */
+ uint16_t index; /**< SDO index. */
+ uint8_t object_code; /**< Object code. */
+ char *name; /**< SDO name. */
+ uint8_t max_subindex; /**< Maximum subindex. */
+ struct list_head entries; /**< List of entries. */
+};
+
+/****************************************************************************/
+
+void ec_sdo_init(ec_sdo_t *, ec_slave_t *, uint16_t);
+void ec_sdo_clear(ec_sdo_t *);
+
+ec_sdo_entry_t *ec_sdo_get_entry(ec_sdo_t *, uint8_t);
+const ec_sdo_entry_t *ec_sdo_get_entry_const(const ec_sdo_t *, uint8_t);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/sdo_entry.c b/ubuntu/igh-ecat/master/sdo_entry.c
new file mode 100644
index 00000000000000..ae4b36619828bb
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sdo_entry.c
@@ -0,0 +1,69 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ CANopen over EtherCAT SDO entry functions.
+*/
+
+/****************************************************************************/
+
+#include <linux/slab.h>
+
+#include "sdo_entry.h"
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_sdo_entry_init(
+ ec_sdo_entry_t *entry, /**< SDO entry. */
+ ec_sdo_t *sdo, /**< Parent SDO. */
+ uint8_t subindex /**< Subindex. */
+ )
+{
+ entry->sdo = sdo;
+ entry->subindex = subindex;
+ entry->data_type = 0x0000;
+ entry->bit_length = 0;
+ entry->read_access[EC_SDO_ENTRY_ACCESS_PREOP] = 0;
+ entry->read_access[EC_SDO_ENTRY_ACCESS_SAFEOP] = 0;
+ entry->read_access[EC_SDO_ENTRY_ACCESS_OP] = 0;
+ entry->write_access[EC_SDO_ENTRY_ACCESS_PREOP] = 0;
+ entry->write_access[EC_SDO_ENTRY_ACCESS_SAFEOP] = 0;
+ entry->write_access[EC_SDO_ENTRY_ACCESS_OP] = 0;
+ entry->description = NULL;
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_sdo_entry_clear(
+ ec_sdo_entry_t *entry /**< SDO entry. */
+ )
+{
+
+ if (entry->description)
+ kfree(entry->description);
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/sdo_entry.h b/ubuntu/igh-ecat/master/sdo_entry.h
new file mode 100644
index 00000000000000..87c92bbd803cae
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sdo_entry.h
@@ -0,0 +1,64 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT CANopen SDO entry structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_SDO_ENTRY_H__
+#define __EC_SDO_ENTRY_H__
+
+#include <linux/list.h>
+#include <linux/kobject.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+struct ec_sdo;
+typedef struct ec_sdo ec_sdo_t; /**< \see ec_sdo. */
+
+/****************************************************************************/
+
+/** CANopen SDO entry.
+ */
+typedef struct {
+ struct list_head list; /**< List item. */
+ ec_sdo_t *sdo; /**< Parent SDO. */
+ uint8_t subindex; /**< Subindex. */
+ uint16_t data_type; /**< Data type. */
+ uint16_t bit_length; /**< Data size in bit. */
+ uint8_t read_access[EC_SDO_ENTRY_ACCESS_COUNT]; /**< Read access. */
+ uint8_t write_access[EC_SDO_ENTRY_ACCESS_COUNT]; /**< Write access. */
+ char *description; /**< Description. */
+} ec_sdo_entry_t;
+
+/****************************************************************************/
+
+void ec_sdo_entry_init(ec_sdo_entry_t *, ec_sdo_t *, uint8_t);
+void ec_sdo_entry_clear(ec_sdo_entry_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/sdo_request.c b/ubuntu/igh-ecat/master/sdo_request.c
new file mode 100644
index 00000000000000..040c6b5d5a8a0c
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sdo_request.c
@@ -0,0 +1,256 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * Canopen over EtherCAT SDO request functions.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+
+#include "sdo_request.h"
+
+/****************************************************************************/
+
+/** Default timeout in ms to wait for SDO transfer responses.
+ */
+#define EC_SDO_REQUEST_RESPONSE_TIMEOUT 1000
+
+/****************************************************************************/
+
+void ec_sdo_request_clear_data(ec_sdo_request_t *);
+
+/****************************************************************************/
+
+/** SDO request constructor.
+ */
+void ec_sdo_request_init(
+ ec_sdo_request_t *req /**< SDO request. */
+ )
+{
+ req->complete_access = 0;
+ req->data = NULL;
+ req->mem_size = 0;
+ req->data_size = 0;
+ req->issue_timeout = 0; // no timeout
+ req->response_timeout = EC_SDO_REQUEST_RESPONSE_TIMEOUT;
+ req->dir = EC_DIR_INVALID;
+ req->state = EC_INT_REQUEST_INIT;
+ req->jiffies_start = 0U;
+ req->jiffies_sent = 0U;
+ req->errno = 0;
+ req->abort_code = 0x00000000;
+}
+
+/****************************************************************************/
+
+/** SDO request destructor.
+ */
+void ec_sdo_request_clear(
+ ec_sdo_request_t *req /**< SDO request. */
+ )
+{
+ ec_sdo_request_clear_data(req);
+}
+
+/****************************************************************************/
+
+/** Copy another SDO request.
+ *
+ * \attention Only the index subindex and data are copied.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_sdo_request_copy(
+ ec_sdo_request_t *req, /**< SDO request. */
+ const ec_sdo_request_t *other /**< Other SDO request to copy from. */
+ )
+{
+ req->complete_access = other->complete_access;
+ req->index = other->index;
+ req->subindex = other->subindex;
+ return ec_sdo_request_copy_data(req, other->data, other->data_size);
+}
+
+/****************************************************************************/
+
+/** SDO request destructor.
+ */
+void ec_sdo_request_clear_data(
+ ec_sdo_request_t *req /**< SDO request. */
+ )
+{
+ if (req->data) {
+ kfree(req->data);
+ req->data = NULL;
+ }
+
+ req->mem_size = 0;
+ req->data_size = 0;
+}
+
+/****************************************************************************/
+
+/** Pre-allocates the data memory.
+ *
+ * If the \a mem_size is already bigger than \a size, nothing is done.
+ *
+ * \return 0 on success, otherwise -ENOMEM.
+ */
+int ec_sdo_request_alloc(
+ ec_sdo_request_t *req, /**< SDO request. */
+ size_t size /**< Data size to allocate. */
+ )
+{
+ if (size <= req->mem_size)
+ return 0;
+
+ ec_sdo_request_clear_data(req);
+
+ if (!(req->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
+ EC_ERR("Failed to allocate %zu bytes of SDO memory.\n", size);
+ return -ENOMEM;
+ }
+
+ req->mem_size = size;
+ req->data_size = 0;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Copies SDO data from an external source.
+ *
+ * If the \a mem_size is to small, new memory is allocated.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_sdo_request_copy_data(
+ ec_sdo_request_t *req, /**< SDO request. */
+ const uint8_t *source, /**< Source data. */
+ size_t size /**< Number of bytes in \a source. */
+ )
+{
+ int ret = ec_sdo_request_alloc(req, size);
+ if (ret < 0)
+ return ret;
+
+ memcpy(req->data, source, size);
+ req->data_size = size;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Checks, if the timeout was exceeded.
+ *
+ * \return non-zero if the timeout was exceeded, else zero.
+ */
+int ec_sdo_request_timed_out(const ec_sdo_request_t *req /**< SDO request. */)
+{
+ return req->issue_timeout
+ && jiffies - req->jiffies_start > HZ * req->issue_timeout / 1000;
+}
+
+/*****************************************************************************
+ * Application interface.
+ ****************************************************************************/
+
+int ecrt_sdo_request_index(ec_sdo_request_t *req, uint16_t index,
+ uint8_t subindex)
+{
+ req->index = index;
+ req->subindex = subindex;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_sdo_request_timeout(ec_sdo_request_t *req, uint32_t timeout)
+{
+ req->issue_timeout = timeout;
+ return 0;
+}
+
+/****************************************************************************/
+
+uint8_t *ecrt_sdo_request_data(const ec_sdo_request_t *req)
+{
+ return req->data;
+}
+
+/****************************************************************************/
+
+size_t ecrt_sdo_request_data_size(const ec_sdo_request_t *req)
+{
+ return req->data_size;
+}
+
+/****************************************************************************/
+
+ec_request_state_t ecrt_sdo_request_state(const ec_sdo_request_t *req)
+{
+ return ec_request_state_translation_table[req->state];
+}
+
+/****************************************************************************/
+
+int ecrt_sdo_request_read(ec_sdo_request_t *req)
+{
+ req->dir = EC_DIR_INPUT;
+ req->state = EC_INT_REQUEST_QUEUED;
+ req->errno = 0;
+ req->abort_code = 0x00000000;
+ req->jiffies_start = jiffies;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_sdo_request_write(ec_sdo_request_t *req)
+{
+ req->dir = EC_DIR_OUTPUT;
+ req->state = EC_INT_REQUEST_QUEUED;
+ req->errno = 0;
+ req->abort_code = 0x00000000;
+ req->jiffies_start = jiffies;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecrt_sdo_request_index);
+EXPORT_SYMBOL(ecrt_sdo_request_timeout);
+EXPORT_SYMBOL(ecrt_sdo_request_data);
+EXPORT_SYMBOL(ecrt_sdo_request_data_size);
+EXPORT_SYMBOL(ecrt_sdo_request_state);
+EXPORT_SYMBOL(ecrt_sdo_request_read);
+EXPORT_SYMBOL(ecrt_sdo_request_write);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/sdo_request.h b/ubuntu/igh-ecat/master/sdo_request.h
new file mode 100644
index 00000000000000..aa2d1849cec329
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sdo_request.h
@@ -0,0 +1,75 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT CANopen SDO request structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_SDO_REQUEST_H__
+#define __EC_SDO_REQUEST_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** CANopen SDO request.
+ */
+struct ec_sdo_request {
+ struct list_head list; /**< List item. */
+ uint16_t index; /**< SDO index. */
+ uint8_t subindex; /**< SDO subindex. */
+ uint8_t *data; /**< Pointer to SDO data. */
+ size_t mem_size; /**< Size of SDO data memory. */
+ size_t data_size; /**< Size of SDO data. */
+ uint8_t complete_access; /**< SDO shall be transferred completely. */
+ uint32_t issue_timeout; /**< Maximum time in ms, the processing of the
+ request may take. */
+ uint32_t response_timeout; /**< Maximum time in ms, the transfer is
+ retried, if the slave does not respond. */
+ ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means downloading to
+ the slave, EC_DIR_INPUT means uploading from the
+ slave. */
+ ec_internal_request_state_t state; /**< SDO request state. */
+ unsigned long jiffies_start; /**< Jiffies, when the request was issued. */
+ unsigned long jiffies_sent; /**< Jiffies, when the upload/download
+ request was sent. */
+ int errno; /**< Error number. */
+ uint32_t abort_code; /**< SDO request abort code. Zero on success. */
+};
+
+/****************************************************************************/
+
+void ec_sdo_request_init(ec_sdo_request_t *);
+void ec_sdo_request_clear(ec_sdo_request_t *);
+
+int ec_sdo_request_copy(ec_sdo_request_t *, const ec_sdo_request_t *);
+int ec_sdo_request_alloc(ec_sdo_request_t *, size_t);
+int ec_sdo_request_copy_data(ec_sdo_request_t *, const uint8_t *, size_t);
+int ec_sdo_request_timed_out(const ec_sdo_request_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/slave.c b/ubuntu/igh-ecat/master/slave.c
new file mode 100644
index 00000000000000..04cf2151daff7e
--- /dev/null
+++ b/ubuntu/igh-ecat/master/slave.c
@@ -0,0 +1,1010 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave methods.
+*/
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/delay.h>
+
+#include "globals.h"
+#include "datagram.h"
+#include "master.h"
+#include "slave_config.h"
+
+#include "slave.h"
+
+/****************************************************************************/
+
+extern const ec_code_msg_t al_status_messages[];
+
+/****************************************************************************/
+
+// prototypes for private methods
+char *ec_slave_sii_string(const ec_slave_t *, unsigned int);
+void ec_slave_find_names_for_pdo(ec_slave_t *, ec_pdo_t *);
+unsigned int ec_slave_get_previous_port(const ec_slave_t *, unsigned int);
+unsigned int ec_slave_get_next_port(const ec_slave_t *, unsigned int);
+uint32_t ec_slave_calc_rtt_sum(const ec_slave_t *);
+ec_slave_t *ec_slave_find_next_dc_slave(ec_slave_t *);
+
+/****************************************************************************/
+
+/**
+ Slave constructor.
+ \return 0 in case of success, else < 0
+*/
+
+void ec_slave_init(
+ ec_slave_t *slave, /**< EtherCAT slave */
+ ec_master_t *master, /**< EtherCAT master */
+ ec_device_index_t dev_idx, /**< Device index. */
+ uint16_t ring_position, /**< ring position */
+ uint16_t station_address /**< station address to configure */
+ )
+{
+ unsigned int i;
+
+ slave->master = master;
+ slave->device_index = dev_idx;
+ slave->ring_position = ring_position;
+ slave->station_address = station_address;
+ slave->effective_alias = 0x0000;
+
+ slave->config = NULL;
+ slave->requested_state = EC_SLAVE_STATE_PREOP;
+ slave->current_state = EC_SLAVE_STATE_UNKNOWN;
+ slave->error_flag = 0;
+ slave->force_config = 0;
+ slave->configured_rx_mailbox_offset = 0x0000;
+ slave->configured_rx_mailbox_size = 0x0000;
+ slave->configured_tx_mailbox_offset = 0x0000;
+ slave->configured_tx_mailbox_size = 0x0000;
+
+ slave->base_type = 0;
+ slave->base_revision = 0;
+ slave->base_build = 0;
+ slave->base_fmmu_count = 0;
+ slave->base_sync_count = 0;
+
+ for (i = 0; i < EC_MAX_PORTS; i++) {
+ slave->ports[i].desc = EC_PORT_NOT_IMPLEMENTED;
+
+ slave->ports[i].link.link_up = 0;
+ slave->ports[i].link.loop_closed = 0;
+ slave->ports[i].link.signal_detected = 0;
+ slave->sii.physical_layer[i] = 0xFF;
+
+ slave->ports[i].receive_time = 0U;
+
+ slave->ports[i].next_slave = NULL;
+ slave->ports[i].delay_to_next_dc = 0U;
+ }
+
+ slave->base_fmmu_bit_operation = 0;
+ slave->base_dc_supported = 0;
+ slave->base_dc_range = EC_DC_32;
+ slave->has_dc_system_time = 0;
+ slave->transmission_delay = 0U;
+
+ slave->sii_words = NULL;
+ slave->sii_nwords = 0;
+
+ slave->sii.alias = 0x0000;
+ slave->sii.vendor_id = 0x00000000;
+ slave->sii.product_code = 0x00000000;
+ slave->sii.revision_number = 0x00000000;
+ slave->sii.serial_number = 0x00000000;
+ slave->sii.boot_rx_mailbox_offset = 0x0000;
+ slave->sii.boot_rx_mailbox_size = 0x0000;
+ slave->sii.boot_tx_mailbox_offset = 0x0000;
+ slave->sii.boot_tx_mailbox_size = 0x0000;
+ slave->sii.std_rx_mailbox_offset = 0x0000;
+ slave->sii.std_rx_mailbox_size = 0x0000;
+ slave->sii.std_tx_mailbox_offset = 0x0000;
+ slave->sii.std_tx_mailbox_size = 0x0000;
+ slave->sii.mailbox_protocols = 0;
+
+ slave->sii.strings = NULL;
+ slave->sii.string_count = 0;
+
+ slave->sii.has_general = 0;
+ slave->sii.group = NULL;
+ slave->sii.image = NULL;
+ slave->sii.order = NULL;
+ slave->sii.name = NULL;
+ memset(&slave->sii.coe_details, 0x00, sizeof(ec_sii_coe_details_t));
+ memset(&slave->sii.general_flags, 0x00, sizeof(ec_sii_general_flags_t));
+ slave->sii.current_on_ebus = 0;
+
+ slave->sii.syncs = NULL;
+ slave->sii.sync_count = 0;
+
+ INIT_LIST_HEAD(&slave->sii.pdos);
+
+ INIT_LIST_HEAD(&slave->sdo_dictionary);
+
+ slave->sdo_dictionary_fetched = 0;
+ slave->jiffies_preop = 0;
+
+ INIT_LIST_HEAD(&slave->sdo_requests);
+ INIT_LIST_HEAD(&slave->reg_requests);
+ INIT_LIST_HEAD(&slave->foe_requests);
+ INIT_LIST_HEAD(&slave->soe_requests);
+ INIT_LIST_HEAD(&slave->eoe_requests);
+
+ // create state machine object
+ ec_fsm_slave_init(&slave->fsm, slave);
+}
+
+/****************************************************************************/
+
+/**
+ Slave destructor.
+ Clears and frees a slave object.
+*/
+
+void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
+{
+ ec_sdo_t *sdo, *next_sdo;
+ unsigned int i;
+ ec_pdo_t *pdo, *next_pdo;
+
+ // abort all pending requests
+
+ while (!list_empty(&slave->sdo_requests)) {
+ ec_sdo_request_t *request =
+ list_entry(slave->sdo_requests.next, ec_sdo_request_t, list);
+ list_del_init(&request->list); // dequeue
+ EC_SLAVE_WARN(slave, "Discarding SDO request,"
+ " slave about to be deleted.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ }
+
+ while (!list_empty(&slave->reg_requests)) {
+ ec_reg_request_t *reg =
+ list_entry(slave->reg_requests.next, ec_reg_request_t, list);
+ list_del_init(®->list); // dequeue
+ EC_SLAVE_WARN(slave, "Discarding register request,"
+ " slave about to be deleted.\n");
+ reg->state = EC_INT_REQUEST_FAILURE;
+ }
+
+ while (!list_empty(&slave->foe_requests)) {
+ ec_foe_request_t *request =
+ list_entry(slave->foe_requests.next, ec_foe_request_t, list);
+ list_del_init(&request->list); // dequeue
+ EC_SLAVE_WARN(slave, "Discarding FoE request,"
+ " slave about to be deleted.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ }
+
+ while (!list_empty(&slave->soe_requests)) {
+ ec_soe_request_t *request =
+ list_entry(slave->soe_requests.next, ec_soe_request_t, list);
+ list_del_init(&request->list); // dequeue
+ EC_SLAVE_WARN(slave, "Discarding SoE request,"
+ " slave about to be deleted.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ }
+
+#ifdef EC_EOE
+ while (!list_empty(&slave->eoe_requests)) {
+ ec_eoe_request_t *request =
+ list_entry(slave->eoe_requests.next, ec_eoe_request_t, list);
+ list_del_init(&request->list); // dequeue
+ EC_SLAVE_WARN(slave, "Discarding EoE request,"
+ " slave about to be deleted.\n");
+ request->state = EC_INT_REQUEST_FAILURE;
+ }
+#endif
+
+ wake_up_all(&slave->master->request_queue);
+
+ if (slave->config) {
+ ec_slave_config_detach(slave->config);
+ }
+
+ // free all SDOs
+ list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
+ list_del(&sdo->list);
+ ec_sdo_clear(sdo);
+ kfree(sdo);
+ }
+
+ // free all strings
+ if (slave->sii.strings) {
+ for (i = 0; i < slave->sii.string_count; i++)
+ kfree(slave->sii.strings[i]);
+ kfree(slave->sii.strings);
+ }
+
+ // free all sync managers
+ ec_slave_clear_sync_managers(slave);
+
+ // free all SII PDOs
+ list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) {
+ list_del(&pdo->list);
+ ec_pdo_clear(pdo);
+ kfree(pdo);
+ }
+
+ if (slave->sii_words) {
+ kfree(slave->sii_words);
+ }
+
+ ec_fsm_slave_clear(&slave->fsm);
+}
+
+/****************************************************************************/
+
+/** Clear the sync manager array.
+ */
+void ec_slave_clear_sync_managers(ec_slave_t *slave /**< EtherCAT slave. */)
+{
+ unsigned int i;
+
+ if (slave->sii.syncs) {
+ for (i = 0; i < slave->sii.sync_count; i++) {
+ ec_sync_clear(&slave->sii.syncs[i]);
+ }
+ kfree(slave->sii.syncs);
+ slave->sii.syncs = NULL;
+ }
+}
+
+/****************************************************************************/
+
+/**
+ * Sets the application state of a slave.
+ */
+
+void ec_slave_set_state(ec_slave_t *slave, /**< EtherCAT slave */
+ ec_slave_state_t new_state /**< new application state */
+ )
+{
+ if (new_state != slave->current_state) {
+ if (slave->master->debug_level) {
+ char old_state[EC_STATE_STRING_SIZE],
+ cur_state[EC_STATE_STRING_SIZE];
+ ec_state_string(slave->current_state, old_state, 0);
+ ec_state_string(new_state, cur_state, 0);
+ EC_SLAVE_DBG(slave, 0, "%s -> %s.\n", old_state, cur_state);
+ }
+ slave->current_state = new_state;
+ }
+}
+
+/****************************************************************************/
+
+/**
+ * Request a slave state and resets the error flag.
+ */
+
+void ec_slave_request_state(ec_slave_t *slave, /**< EtherCAT slave */
+ ec_slave_state_t state /**< new state */
+ )
+{
+ slave->requested_state = state;
+ slave->error_flag = 0;
+}
+
+/****************************************************************************/
+
+/**
+ Fetches data from a STRING category.
+ \todo range checking
+ \return 0 in case of success, else < 0
+*/
+
+int ec_slave_fetch_sii_strings(
+ ec_slave_t *slave, /**< EtherCAT slave */
+ const uint8_t *data, /**< category data */
+ size_t data_size /**< number of bytes */
+ )
+{
+ int i, err;
+ size_t size;
+ off_t offset;
+
+ slave->sii.string_count = data[0];
+
+ if (slave->sii.string_count) {
+ if (!(slave->sii.strings =
+ kmalloc(sizeof(char *) * slave->sii.string_count,
+ GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate string array memory.\n");
+ err = -ENOMEM;
+ goto out_zero;
+ }
+
+ offset = 1;
+ for (i = 0; i < slave->sii.string_count; i++) {
+ size = data[offset];
+ // allocate memory for string structure and data at a single blow
+ if (!(slave->sii.strings[i] =
+ kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate string memory.\n");
+ err = -ENOMEM;
+ goto out_free;
+ }
+ memcpy(slave->sii.strings[i], data + offset + 1, size);
+ slave->sii.strings[i][size] = 0x00; // append binary zero
+ offset += 1 + size;
+ }
+ }
+
+ return 0;
+
+out_free:
+ for (i--; i >= 0; i--)
+ kfree(slave->sii.strings[i]);
+ kfree(slave->sii.strings);
+ slave->sii.strings = NULL;
+out_zero:
+ slave->sii.string_count = 0;
+ return err;
+}
+
+/****************************************************************************/
+
+/**
+ Fetches data from a GENERAL category.
+ \return 0 in case of success, else < 0
+*/
+
+int ec_slave_fetch_sii_general(
+ ec_slave_t *slave, /**< EtherCAT slave */
+ const uint8_t *data, /**< category data */
+ size_t data_size /**< size in bytes */
+ )
+{
+ unsigned int i;
+ uint8_t flags;
+
+ if (data_size != 32) {
+ EC_SLAVE_ERR(slave, "Wrong size of general category (%zu/32).\n",
+ data_size);
+ return -EINVAL;
+ }
+
+ slave->sii.group = ec_slave_sii_string(slave, data[0]);
+ slave->sii.image = ec_slave_sii_string(slave, data[1]);
+ slave->sii.order = ec_slave_sii_string(slave, data[2]);
+ slave->sii.name = ec_slave_sii_string(slave, data[3]);
+
+ for (i = 0; i < 4; i++)
+ slave->sii.physical_layer[i] =
+ (data[4] & (0x03 << (i * 2))) >> (i * 2);
+
+ // read CoE details
+ flags = EC_READ_U8(data + 5);
+ slave->sii.coe_details.enable_sdo = (flags >> 0) & 0x01;
+ slave->sii.coe_details.enable_sdo_info = (flags >> 1) & 0x01;
+ slave->sii.coe_details.enable_pdo_assign = (flags >> 2) & 0x01;
+ slave->sii.coe_details.enable_pdo_configuration = (flags >> 3) & 0x01;
+ slave->sii.coe_details.enable_upload_at_startup = (flags >> 4) & 0x01;
+ slave->sii.coe_details.enable_sdo_complete_access = (flags >> 5) & 0x01;
+
+ // read general flags
+ flags = EC_READ_U8(data + 0x000B);
+ slave->sii.general_flags.enable_safeop = (flags >> 0) & 0x01;
+ slave->sii.general_flags.enable_not_lrw = (flags >> 1) & 0x01;
+
+ slave->sii.current_on_ebus = EC_READ_S16(data + 0x0C);
+ slave->sii.has_general = 1;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Fetches data from a SYNC MANAGER category.
+ *
+ * Appends the sync managers described in the category to the existing ones.
+ *
+ * \return 0 in case of success, else < 0
+ */
+int ec_slave_fetch_sii_syncs(
+ ec_slave_t *slave, /**< EtherCAT slave. */
+ const uint8_t *data, /**< Category data. */
+ size_t data_size /**< Number of bytes. */
+ )
+{
+ unsigned int i, count, total_count;
+ ec_sync_t *sync;
+ size_t memsize;
+ ec_sync_t *syncs;
+ uint8_t index;
+
+ // one sync manager struct is 4 words long
+ if (data_size % 8) {
+ EC_SLAVE_ERR(slave, "Invalid SII sync manager category size %zu.\n",
+ data_size);
+ return -EINVAL;
+ }
+
+ count = data_size / 8;
+
+ if (count) {
+ total_count = count + slave->sii.sync_count;
+ if (total_count > EC_MAX_SYNC_MANAGERS) {
+ EC_SLAVE_ERR(slave, "Exceeded maximum number of"
+ " sync managers!\n");
+ return -EOVERFLOW;
+ }
+ memsize = sizeof(ec_sync_t) * total_count;
+ if (!(syncs = kmalloc(memsize, GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate %zu bytes"
+ " for sync managers.\n", memsize);
+ return -ENOMEM;
+ }
+
+ for (i = 0; i < slave->sii.sync_count; i++)
+ ec_sync_init_copy(syncs + i, slave->sii.syncs + i);
+
+ // initialize new sync managers
+ for (i = 0; i < count; i++, data += 8) {
+ index = i + slave->sii.sync_count;
+ sync = &syncs[index];
+
+ ec_sync_init(sync, slave);
+ sync->physical_start_address = EC_READ_U16(data);
+ sync->default_length = EC_READ_U16(data + 2);
+ sync->control_register = EC_READ_U8(data + 4);
+ sync->enable = EC_READ_U8(data + 6);
+ }
+
+ if (slave->sii.syncs)
+ kfree(slave->sii.syncs);
+ slave->sii.syncs = syncs;
+ slave->sii.sync_count = total_count;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/**
+ Fetches data from a [RT]xPDO category.
+ \return 0 in case of success, else < 0
+*/
+
+int ec_slave_fetch_sii_pdos(
+ ec_slave_t *slave, /**< EtherCAT slave */
+ const uint8_t *data, /**< category data */
+ size_t data_size, /**< number of bytes */
+ ec_direction_t dir /**< PDO direction. */
+ )
+{
+ int ret;
+ ec_pdo_t *pdo;
+ ec_pdo_entry_t *entry;
+ unsigned int entry_count, i;
+
+ while (data_size >= 8) {
+ if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate PDO memory.\n");
+ return -ENOMEM;
+ }
+
+ ec_pdo_init(pdo);
+ pdo->index = EC_READ_U16(data);
+ entry_count = EC_READ_U8(data + 2);
+ pdo->sync_index = EC_READ_U8(data + 3);
+ ret = ec_pdo_set_name(pdo,
+ ec_slave_sii_string(slave, EC_READ_U8(data + 5)));
+ if (ret) {
+ ec_pdo_clear(pdo);
+ kfree(pdo);
+ return ret;
+ }
+ list_add_tail(&pdo->list, &slave->sii.pdos);
+
+ data_size -= 8;
+ data += 8;
+
+ for (i = 0; i < entry_count; i++) {
+ if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
+ EC_SLAVE_ERR(slave, "Failed to allocate PDO entry memory.\n");
+ return -ENOMEM;
+ }
+
+ ec_pdo_entry_init(entry);
+ entry->index = EC_READ_U16(data);
+ entry->subindex = EC_READ_U8(data + 2);
+ ret = ec_pdo_entry_set_name(entry,
+ ec_slave_sii_string(slave, EC_READ_U8(data + 3)));
+ if (ret) {
+ ec_pdo_entry_clear(entry);
+ kfree(entry);
+ return ret;
+ }
+ entry->bit_length = EC_READ_U8(data + 5);
+ list_add_tail(&entry->list, &pdo->entries);
+
+ data_size -= 8;
+ data += 8;
+ }
+
+ // if sync manager index is positive, the PDO is mapped by default
+ if (pdo->sync_index >= 0) {
+ ec_sync_t *sync;
+
+ if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) {
+ EC_SLAVE_ERR(slave, "Invalid SM index %i for PDO 0x%04X.",
+ pdo->sync_index, pdo->index);
+ return -ENOENT;
+ }
+
+ ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo);
+ if (ret)
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/**
+ Searches the string list for an index.
+ \return 0 in case of success, else < 0
+*/
+
+char *ec_slave_sii_string(
+ const ec_slave_t *slave, /**< EtherCAT slave */
+ unsigned int index /**< string index */
+ )
+{
+ if (!index--)
+ return NULL;
+
+ if (index >= slave->sii.string_count) {
+ EC_SLAVE_DBG(slave, 1, "String %u not found.\n", index);
+ return NULL;
+ }
+
+ return slave->sii.strings[index];
+}
+
+/****************************************************************************/
+
+/** Get the sync manager given an index.
+ *
+ * \return pointer to sync manager, or NULL.
+ */
+ec_sync_t *ec_slave_get_sync(
+ ec_slave_t *slave, /**< EtherCAT slave. */
+ uint8_t sync_index /**< Sync manager index. */
+ )
+{
+ if (sync_index < slave->sii.sync_count) {
+ return &slave->sii.syncs[sync_index];
+ } else {
+ return NULL;
+ }
+}
+
+/****************************************************************************/
+
+/**
+ Counts the total number of SDOs and entries in the dictionary.
+*/
+
+void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */
+ unsigned int *sdo_count, /**< number of SDOs */
+ unsigned int *entry_count /**< total number of
+ entries */
+ )
+{
+ unsigned int sdos = 0, entries = 0;
+ ec_sdo_t *sdo;
+ ec_sdo_entry_t *entry;
+
+ list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
+ sdos++;
+ list_for_each_entry(entry, &sdo->entries, list) {
+ entries++;
+ }
+ }
+
+ *sdo_count = sdos;
+ *entry_count = entries;
+}
+
+/****************************************************************************/
+
+/**
+ * Get an SDO from the dictionary.
+ * \returns The desired SDO, or NULL.
+ */
+
+ec_sdo_t *ec_slave_get_sdo(
+ ec_slave_t *slave, /**< EtherCAT slave */
+ uint16_t index /**< SDO index */
+ )
+{
+ ec_sdo_t *sdo;
+
+ list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
+ if (sdo->index != index)
+ continue;
+ return sdo;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/**
+ * Get an SDO from the dictionary.
+ *
+ * const version.
+ *
+ * \returns The desired SDO, or NULL.
+ */
+
+const ec_sdo_t *ec_slave_get_sdo_const(
+ const ec_slave_t *slave, /**< EtherCAT slave */
+ uint16_t index /**< SDO index */
+ )
+{
+ const ec_sdo_t *sdo;
+
+ list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
+ if (sdo->index != index)
+ continue;
+ return sdo;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Get an SDO from the dictionary, given its position in the list.
+ * \returns The desired SDO, or NULL.
+ */
+
+const ec_sdo_t *ec_slave_get_sdo_by_pos_const(
+ const ec_slave_t *slave, /**< EtherCAT slave. */
+ uint16_t sdo_position /**< SDO list position. */
+ )
+{
+ const ec_sdo_t *sdo;
+
+ list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
+ if (sdo_position--)
+ continue;
+ return sdo;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Get the number of SDOs in the dictionary.
+ * \returns SDO count.
+ */
+
+uint16_t ec_slave_sdo_count(
+ const ec_slave_t *slave /**< EtherCAT slave. */
+ )
+{
+ const ec_sdo_t *sdo;
+ uint16_t count = 0;
+
+ list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
+ count++;
+ }
+
+ return count;
+}
+
+/****************************************************************************/
+
+/** Finds a mapped PDO.
+ * \returns The desired PDO object, or NULL.
+ */
+const ec_pdo_t *ec_slave_find_pdo(
+ const ec_slave_t *slave, /**< Slave. */
+ uint16_t index /**< PDO index to find. */
+ )
+{
+ unsigned int i;
+ const ec_sync_t *sync;
+ const ec_pdo_t *pdo;
+
+ for (i = 0; i < slave->sii.sync_count; i++) {
+ sync = &slave->sii.syncs[i];
+
+ if (!(pdo = ec_pdo_list_find_pdo_const(&sync->pdos, index)))
+ continue;
+
+ return pdo;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Find name for a PDO and its entries.
+ */
+void ec_slave_find_names_for_pdo(
+ ec_slave_t *slave,
+ ec_pdo_t *pdo
+ )
+{
+ const ec_sdo_t *sdo;
+ ec_pdo_entry_t *pdo_entry;
+ const ec_sdo_entry_t *sdo_entry;
+
+ list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
+ if (sdo->index == pdo->index) {
+ ec_pdo_set_name(pdo, sdo->name);
+ } else {
+ list_for_each_entry(pdo_entry, &pdo->entries, list) {
+ if (sdo->index == pdo_entry->index) {
+ sdo_entry = ec_sdo_get_entry_const(
+ sdo, pdo_entry->subindex);
+ if (sdo_entry) {
+ ec_pdo_entry_set_name(pdo_entry,
+ sdo_entry->description);
+ }
+ }
+ }
+ }
+ }
+}
+
+/****************************************************************************/
+
+/** Attach PDO names.
+ */
+void ec_slave_attach_pdo_names(
+ ec_slave_t *slave
+ )
+{
+ unsigned int i;
+ ec_sync_t *sync;
+ ec_pdo_t *pdo;
+
+ for (i = 0; i < slave->sii.sync_count; i++) {
+ sync = slave->sii.syncs + i;
+ list_for_each_entry(pdo, &sync->pdos.list, list) {
+ ec_slave_find_names_for_pdo(slave, pdo);
+ }
+ }
+}
+
+/****************************************************************************/
+
+/** Returns the previous connected port of a given port.
+ *
+ * \return Port index.
+ */
+unsigned int ec_slave_get_previous_port(
+ const ec_slave_t *slave, /**< EtherCAT slave. */
+ unsigned int port_index /**< Port index. */
+ )
+{
+ static const unsigned int prev_table[EC_MAX_PORTS] = {
+ 2, 3, 1, 0
+ };
+
+ if (port_index >= EC_MAX_PORTS) {
+ EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n",
+ __func__, port_index);
+ }
+
+ do {
+ port_index = prev_table[port_index];
+ if (slave->ports[port_index].next_slave) {
+ return port_index;
+ }
+ } while (port_index);
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Returns the next connected port of a given port.
+ *
+ * \return Port index.
+ */
+unsigned int ec_slave_get_next_port(
+ const ec_slave_t *slave, /**< EtherCAT slave. */
+ unsigned int port_index /**< Port index. */
+ )
+{
+ static const unsigned int next_table[EC_MAX_PORTS] = {
+ 3, 2, 0, 1
+ };
+
+ if (port_index >= EC_MAX_PORTS) {
+ EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n",
+ __func__, port_index);
+ }
+
+ do {
+ port_index = next_table[port_index];
+ if (slave->ports[port_index].next_slave) {
+ return port_index;
+ }
+ } while (port_index);
+
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Calculates the sum of round-trip-times of connected ports 1-3.
+ *
+ * \return Round-trip-time in ns.
+ */
+uint32_t ec_slave_calc_rtt_sum(
+ const ec_slave_t *slave /**< EtherCAT slave. */
+ )
+{
+ uint32_t rtt_sum = 0, rtt;
+ unsigned int port_index = ec_slave_get_next_port(slave, 0);
+
+ while (port_index != 0) {
+ unsigned int prev_index =
+ ec_slave_get_previous_port(slave, port_index);
+
+ rtt = slave->ports[port_index].receive_time -
+ slave->ports[prev_index].receive_time;
+ rtt_sum += rtt;
+ port_index = ec_slave_get_next_port(slave, port_index);
+ }
+
+ return rtt_sum;
+}
+
+/****************************************************************************/
+
+/** Finds the next slave supporting DC delay measurement.
+ *
+ * \return Next DC slave, or NULL.
+ */
+ec_slave_t *ec_slave_find_next_dc_slave(
+ ec_slave_t *slave /**< EtherCAT slave. */
+ )
+{
+ unsigned int port_index;
+ ec_slave_t *dc_slave = NULL;
+
+ if (slave->base_dc_supported) {
+ dc_slave = slave;
+ } else {
+ port_index = ec_slave_get_next_port(slave, 0);
+
+ while (port_index != 0) {
+ ec_slave_t *next = slave->ports[port_index].next_slave;
+
+ if (next) {
+ dc_slave = ec_slave_find_next_dc_slave(next);
+
+ if (dc_slave) {
+ break;
+ }
+ }
+ port_index = ec_slave_get_next_port(slave, port_index);
+ }
+ }
+
+ return dc_slave;
+}
+
+/****************************************************************************/
+
+/** Calculates the port transmission delays.
+ */
+void ec_slave_calc_port_delays(
+ ec_slave_t *slave /**< EtherCAT slave. */
+ )
+{
+ unsigned int port_index;
+ ec_slave_t *next_slave, *next_dc;
+ uint32_t rtt, next_rtt_sum;
+
+ if (!slave->base_dc_supported)
+ return;
+
+ port_index = ec_slave_get_next_port(slave, 0);
+
+ while (port_index != 0) {
+ next_slave = slave->ports[port_index].next_slave;
+ next_dc = ec_slave_find_next_dc_slave(next_slave);
+
+ if (next_dc) {
+ unsigned int prev_port =
+ ec_slave_get_previous_port(slave, port_index);
+
+ rtt = slave->ports[port_index].receive_time -
+ slave->ports[prev_port].receive_time;
+ next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
+
+ slave->ports[port_index].delay_to_next_dc =
+ (rtt - next_rtt_sum) / 2; // FIXME
+ next_dc->ports[0].delay_to_next_dc =
+ (rtt - next_rtt_sum) / 2;
+
+#if 0
+ EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u"
+ " next_rtt_sum=%u delay=%u\n",
+ slave->ring_position, port_index, rtt, next_rtt_sum,
+ slave->ports[port_index].delay_to_next_dc);
+#endif
+ }
+
+ port_index = ec_slave_get_next_port(slave, port_index);
+ }
+}
+
+/****************************************************************************/
+
+/** Recursively calculates transmission delays.
+ */
+void ec_slave_calc_transmission_delays_rec(
+ ec_slave_t *slave, /**< Current slave. */
+ uint32_t *delay /**< Sum of delays. */
+ )
+{
+ unsigned int i;
+ ec_slave_t *next_dc;
+
+ EC_SLAVE_DBG(slave, 1, "%s(delay = %u ns)\n", __func__, *delay);
+
+ slave->transmission_delay = *delay;
+
+ i = ec_slave_get_next_port(slave, 0);
+
+ while (i != 0) {
+ ec_slave_port_t *port = &slave->ports[i];
+ next_dc = ec_slave_find_next_dc_slave(port->next_slave);
+ if (next_dc) {
+ *delay = *delay + port->delay_to_next_dc;
+#if 0
+ EC_SLAVE_DBG(slave, 1, "%u:%u %u\n",
+ slave->ring_position, i, *delay);
+#endif
+ ec_slave_calc_transmission_delays_rec(next_dc, delay);
+ }
+
+ i = ec_slave_get_next_port(slave, i);
+ }
+
+ *delay = *delay + slave->ports[0].delay_to_next_dc;
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/slave.h b/ubuntu/igh-ecat/master/slave.h
new file mode 100644
index 00000000000000..4e113e288e1a68
--- /dev/null
+++ b/ubuntu/igh-ecat/master/slave.h
@@ -0,0 +1,266 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_SLAVE_H__
+#define __EC_SLAVE_H__
+
+#include <linux/list.h>
+#include <linux/kobject.h>
+
+#include "globals.h"
+#include "datagram.h"
+#include "pdo.h"
+#include "sync.h"
+#include "sdo.h"
+#include "fsm_slave.h"
+
+/****************************************************************************/
+
+/** Convenience macro for printing slave-specific information to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX>-<POSITION>: ", where INDEX is the master index and
+ * POSITION is the slave's ring position.
+ *
+ * \param slave EtherCAT slave
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_SLAVE_INFO(slave, fmt, args...) \
+ printk(KERN_INFO "EtherCAT %u-%u: " fmt, slave->master->index, \
+ slave->ring_position, ##args)
+
+/** Convenience macro for printing slave-specific errors to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX>-<POSITION>: ", where INDEX is the master index and
+ * POSITION is the slave's ring position.
+ *
+ * \param slave EtherCAT slave
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_SLAVE_ERR(slave, fmt, args...) \
+ printk(KERN_ERR "EtherCAT ERROR %u-%u: " fmt, slave->master->index, \
+ slave->ring_position, ##args)
+
+/** Convenience macro for printing slave-specific warnings to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX>-<POSITION>: ", where INDEX is the master index and
+ * POSITION is the slave's ring position.
+ *
+ * \param slave EtherCAT slave
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_SLAVE_WARN(slave, fmt, args...) \
+ printk(KERN_WARNING "EtherCAT WARNING %u-%u: " fmt, \
+ slave->master->index, slave->ring_position, ##args)
+
+/** Convenience macro for printing slave-specific debug messages to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX>-<POSITION>: ", where INDEX is the master index and
+ * POSITION is the slave's ring position.
+ *
+ * \param slave EtherCAT slave
+ * \param level Debug level. Master's debug level must be >= \a level for
+ * output.
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_SLAVE_DBG(slave, level, fmt, args...) \
+ do { \
+ if (slave->master->debug_level >= level) { \
+ printk(KERN_DEBUG "EtherCAT DEBUG %u-%u: " fmt, \
+ slave->master->index, slave->ring_position, ##args); \
+ } \
+ } while (0)
+
+/****************************************************************************/
+
+/** Slave port.
+ */
+typedef struct {
+ ec_slave_port_desc_t desc; /**< Port descriptors. */
+ ec_slave_port_link_t link; /**< Port link status. */
+ ec_slave_t *next_slave; /**< Connected slaves. */
+ uint32_t receive_time; /**< Port receive times for delay
+ measurement. */
+ uint32_t delay_to_next_dc; /**< Delay to next slave with DC support behind
+ this port [ns]. */
+} ec_slave_port_t;
+
+/****************************************************************************/
+
+/** Slave information interface data.
+ */
+typedef struct {
+ // Non-category data
+ uint16_t alias; /**< Configured station alias. */
+ uint32_t vendor_id; /**< Vendor ID. */
+ uint32_t product_code; /**< Vendor-specific product code. */
+ uint32_t revision_number; /**< Revision number. */
+ uint32_t serial_number; /**< Serial number. */
+ uint16_t boot_rx_mailbox_offset; /**< Bootstrap receive mailbox address. */
+ uint16_t boot_rx_mailbox_size; /**< Bootstrap receive mailbox size. */
+ uint16_t boot_tx_mailbox_offset; /**< Bootstrap transmit mailbox address. */
+ uint16_t boot_tx_mailbox_size; /**< Bootstrap transmit mailbox size. */
+ uint16_t std_rx_mailbox_offset; /**< Standard receive mailbox address. */
+ uint16_t std_rx_mailbox_size; /**< Standard receive mailbox size. */
+ uint16_t std_tx_mailbox_offset; /**< Standard transmit mailbox address. */
+ uint16_t std_tx_mailbox_size; /**< Standard transmit mailbox size. */
+ uint16_t mailbox_protocols; /**< Supported mailbox protocols. */
+
+ // Strings
+ char **strings; /**< Strings in SII categories. */
+ unsigned int string_count; /**< Number of SII strings. */
+
+ // General
+ unsigned int has_general; /**< General category present. */
+ char *group; /**< Group name. */
+ char *image; /**< Image name. */
+ char *order; /**< Order number. */
+ char *name; /**< Slave name. */
+ uint8_t physical_layer[EC_MAX_PORTS]; /**< Port media. */
+ ec_sii_coe_details_t coe_details; /**< CoE detail flags. */
+ ec_sii_general_flags_t general_flags; /**< General flags. */
+ int16_t current_on_ebus; /**< Power consumption in mA. */
+
+ // SyncM
+ ec_sync_t *syncs; /**< SYNC MANAGER categories. */
+ unsigned int sync_count; /**< Number of sync managers. */
+
+ // [RT]XPDO
+ struct list_head pdos; /**< SII [RT]XPDO categories. */
+} ec_sii_t;
+
+/****************************************************************************/
+
+/** EtherCAT slave.
+ */
+struct ec_slave
+{
+ ec_master_t *master; /**< Master owning the slave. */
+ ec_device_index_t device_index; /**< Index of device the slave responds
+ on. */
+
+ // addresses
+ uint16_t ring_position; /**< Ring position. */
+ uint16_t station_address; /**< Configured station address. */
+ uint16_t effective_alias; /**< Effective alias address. */
+
+ ec_slave_port_t ports[EC_MAX_PORTS]; /**< Ports. */
+
+ // configuration
+ ec_slave_config_t *config; /**< Current configuration. */
+ ec_slave_state_t requested_state; /**< Requested application state. */
+ ec_slave_state_t current_state; /**< Current application state. */
+ unsigned int error_flag; /**< Stop processing after an error. */
+ unsigned int force_config; /**< Force (re-)configuration. */
+ uint16_t configured_rx_mailbox_offset; /**< Configured receive mailbox
+ offset. */
+ uint16_t configured_rx_mailbox_size; /**< Configured receive mailbox size.
+ */
+ uint16_t configured_tx_mailbox_offset; /**< Configured send mailbox
+ offset. */
+ uint16_t configured_tx_mailbox_size; /**< Configured send mailbox size. */
+
+ // base data
+ uint8_t base_type; /**< Slave type. */
+ uint8_t base_revision; /**< Revision. */
+ uint16_t base_build; /**< Build number. */
+ uint8_t base_fmmu_count; /**< Number of supported FMMUs. */
+ uint8_t base_sync_count; /**< Number of supported sync managers. */
+ uint8_t base_fmmu_bit_operation; /**< FMMU bit operation is supported. */
+ uint8_t base_dc_supported; /**< Distributed clocks are supported. */
+ ec_slave_dc_range_t base_dc_range; /**< DC range. */
+ uint8_t has_dc_system_time; /**< The slave supports the DC system time
+ register. Otherwise it can only be used for
+ delay measurement. */
+ uint32_t transmission_delay; /**< DC system time transmission delay
+ (offset from reference clock). */
+
+ // SII
+ uint16_t *sii_words; /**< Complete SII image. */
+ size_t sii_nwords; /**< Size of the SII contents in words. */
+
+ // Slave information interface
+ ec_sii_t sii; /**< Extracted SII data. */
+
+ struct list_head sdo_dictionary; /**< SDO dictionary list */
+ uint8_t sdo_dictionary_fetched; /**< Dictionary has been fetched. */
+ unsigned long jiffies_preop; /**< Time, the slave went to PREOP. */
+
+ struct list_head sdo_requests; /**< SDO access requests. */
+ struct list_head reg_requests; /**< Register access requests. */
+ struct list_head foe_requests; /**< FoE requests. */
+ struct list_head soe_requests; /**< SoE requests. */
+ struct list_head eoe_requests; /**< EoE set IP parameter requests. */
+
+ ec_fsm_slave_t fsm; /**< Slave state machine. */
+};
+
+/****************************************************************************/
+
+// slave construction/destruction
+void ec_slave_init(ec_slave_t *, ec_master_t *, ec_device_index_t,
+ uint16_t, uint16_t);
+void ec_slave_clear(ec_slave_t *);
+
+void ec_slave_clear_sync_managers(ec_slave_t *);
+
+void ec_slave_request_state(ec_slave_t *, ec_slave_state_t);
+void ec_slave_set_state(ec_slave_t *, ec_slave_state_t);
+
+// SII categories
+int ec_slave_fetch_sii_strings(ec_slave_t *, const uint8_t *, size_t);
+int ec_slave_fetch_sii_general(ec_slave_t *, const uint8_t *, size_t);
+int ec_slave_fetch_sii_syncs(ec_slave_t *, const uint8_t *, size_t);
+int ec_slave_fetch_sii_pdos(ec_slave_t *, const uint8_t *, size_t,
+ ec_direction_t);
+
+// misc.
+ec_sync_t *ec_slave_get_sync(ec_slave_t *, uint8_t);
+
+void ec_slave_sdo_dict_info(const ec_slave_t *,
+ unsigned int *, unsigned int *);
+ec_sdo_t *ec_slave_get_sdo(ec_slave_t *, uint16_t);
+const ec_sdo_t *ec_slave_get_sdo_const(const ec_slave_t *, uint16_t);
+const ec_sdo_t *ec_slave_get_sdo_by_pos_const(const ec_slave_t *, uint16_t);
+uint16_t ec_slave_sdo_count(const ec_slave_t *);
+const ec_pdo_t *ec_slave_find_pdo(const ec_slave_t *, uint16_t);
+void ec_slave_attach_pdo_names(ec_slave_t *);
+
+void ec_slave_calc_port_delays(ec_slave_t *);
+void ec_slave_calc_transmission_delays_rec(ec_slave_t *, uint32_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/slave_config.c b/ubuntu/igh-ecat/master/slave_config.c
new file mode 100644
index 00000000000000..87e81a6c8895fe
--- /dev/null
+++ b/ubuntu/igh-ecat/master/slave_config.c
@@ -0,0 +1,1652 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * vim: expandtab
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave configuration methods.
+*/
+
+/****************************************************************************/
+
+#include "slave_config.h"
+
+#include "globals.h"
+#include "master.h"
+#include "voe_handler.h"
+#include "flag.h"
+#include "ioctl.h"
+
+#ifdef EC_EOE
+#include "eoe_request.h"
+#endif
+
+#include <linux/module.h>
+#include <linux/slab.h>
+
+/****************************************************************************/
+
+// prototypes for private methods
+int ec_slave_config_prepare_fmmu(ec_slave_config_t *, ec_domain_t *, uint8_t,
+ ec_direction_t);
+void ec_slave_config_load_default_mapping(const ec_slave_config_t *,
+ ec_pdo_t *);
+
+/****************************************************************************/
+
+/** EtherCAT application-layer transition timeout.
+ */
+typedef struct {
+ struct list_head list;
+ ec_slave_state_t from;
+ ec_slave_state_t to;
+ unsigned int timeout_ms;
+} ec_al_timeout_t;
+
+/****************************************************************************/
+
+/** Slave configuration constructor.
+ *
+ * See ecrt_master_slave_config() for the usage of the \a alias and \a
+ * position parameters.
+ */
+void ec_slave_config_init(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ ec_master_t *master, /**< EtherCAT master. */
+ uint16_t alias, /**< Slave alias. */
+ uint16_t position, /**< Slave position. */
+ uint32_t vendor_id, /**< Expected vendor ID. */
+ uint32_t product_code /**< Expected product code. */
+ )
+{
+ unsigned int i;
+
+ sc->master = master;
+
+ sc->alias = alias;
+ sc->position = position;
+ sc->vendor_id = vendor_id;
+ sc->product_code = product_code;
+ sc->watchdog_divider = 0; // use default
+ sc->watchdog_intervals = 0; // use default
+
+ sc->slave = NULL;
+
+ for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)
+ ec_sync_config_init(&sc->sync_configs[i]);
+
+ sc->used_fmmus = 0;
+ sc->dc_assign_activate = 0x0000;
+ sc->dc_sync[0].cycle_time = 0U;
+ sc->dc_sync[1].cycle_time = 0;
+ sc->dc_sync[0].shift_time = 0U;
+ sc->dc_sync[1].shift_time = 0;
+
+ INIT_LIST_HEAD(&sc->sdo_configs);
+ INIT_LIST_HEAD(&sc->sdo_requests);
+ INIT_LIST_HEAD(&sc->soe_requests);
+ INIT_LIST_HEAD(&sc->reg_requests);
+ INIT_LIST_HEAD(&sc->voe_handlers);
+ INIT_LIST_HEAD(&sc->soe_configs);
+ INIT_LIST_HEAD(&sc->flags);
+ INIT_LIST_HEAD(&sc->al_timeouts);
+
+#ifdef EC_EOE
+ ec_eoe_request_init(&sc->eoe_ip_param_request);
+#endif
+
+ ec_coe_emerg_ring_init(&sc->emerg_ring, sc);
+}
+
+/****************************************************************************/
+
+/** Slave configuration destructor.
+ *
+ * Clears and frees a slave configuration object.
+ */
+void ec_slave_config_clear(
+ ec_slave_config_t *sc /**< Slave configuration. */
+ )
+{
+ unsigned int i;
+ ec_sdo_request_t *req, *next_req;
+ ec_voe_handler_t *voe, *next_voe;
+ ec_reg_request_t *reg, *next_reg;
+ ec_soe_request_t *soe, *next_soe;
+ ec_flag_t *flag, *next_flag;
+ ec_al_timeout_t *timeout, *next_timeout;
+
+ ec_slave_config_detach(sc);
+
+ // Free sync managers
+ for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)
+ ec_sync_config_clear(&sc->sync_configs[i]);
+
+ // free all SDO configurations
+ list_for_each_entry_safe(req, next_req, &sc->sdo_configs, list) {
+ list_del(&req->list);
+ ec_sdo_request_clear(req);
+ kfree(req);
+ }
+
+ // free all SDO requests
+ list_for_each_entry_safe(req, next_req, &sc->sdo_requests, list) {
+ list_del(&req->list);
+ ec_sdo_request_clear(req);
+ kfree(req);
+ }
+
+ // free all SoE requests
+ list_for_each_entry_safe(soe, next_soe, &sc->soe_requests, list) {
+ list_del(&soe->list);
+ ec_soe_request_clear(soe);
+ kfree(soe);
+ }
+
+ // free all register requests
+ list_for_each_entry_safe(reg, next_reg, &sc->reg_requests, list) {
+ list_del(®->list);
+ ec_reg_request_clear(reg);
+ kfree(reg);
+ }
+
+ // free all VoE handlers
+ list_for_each_entry_safe(voe, next_voe, &sc->voe_handlers, list) {
+ list_del(&voe->list);
+ ec_voe_handler_clear(voe);
+ kfree(voe);
+ }
+
+ // free all SoE configurations
+ list_for_each_entry_safe(soe, next_soe, &sc->soe_configs, list) {
+ list_del(&soe->list);
+ ec_soe_request_clear(soe);
+ kfree(soe);
+ }
+
+ // free all flags
+ list_for_each_entry_safe(flag, next_flag, &sc->flags, list) {
+ list_del(&flag->list);
+ ec_flag_clear(flag);
+ kfree(flag);
+ }
+
+ // free all AL timeouts
+ list_for_each_entry_safe(timeout, next_timeout, &sc->al_timeouts, list) {
+ list_del(&timeout->list);
+ kfree(timeout);
+ }
+
+ ec_coe_emerg_ring_clear(&sc->emerg_ring);
+}
+
+/****************************************************************************/
+
+/** Prepares an FMMU configuration.
+ *
+ * Configuration data for the FMMU is saved in the slave config structure and
+ * is written to the slave during the configuration. The FMMU configuration
+ * is done in a way, that the complete data range of the corresponding sync
+ * manager is covered. Seperate FMMUs are configured for each domain. If the
+ * FMMU configuration is already prepared, the function does nothing and
+ * returns with success.
+ *
+ * \retval >=0 Success, logical offset byte address.
+ * \retval <0 Error code.
+ */
+int ec_slave_config_prepare_fmmu(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ ec_domain_t *domain, /**< Domain. */
+ uint8_t sync_index, /**< Sync manager index. */
+ ec_direction_t dir /**< PDO direction. */
+ )
+{
+ unsigned int i;
+ ec_fmmu_config_t *fmmu;
+
+ // FMMU configuration already prepared?
+ for (i = 0; i < sc->used_fmmus; i++) {
+ fmmu = &sc->fmmu_configs[i];
+ if (fmmu->domain == domain && fmmu->sync_index == sync_index)
+ return fmmu->logical_start_address;
+ }
+
+ if (sc->used_fmmus == EC_MAX_FMMUS) {
+ EC_CONFIG_ERR(sc, "FMMU limit reached!\n");
+ return -EOVERFLOW;
+ }
+
+ fmmu = &sc->fmmu_configs[sc->used_fmmus++];
+
+ down(&sc->master->master_sem);
+ ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir);
+ up(&sc->master->master_sem);
+
+ return fmmu->logical_start_address;
+}
+
+/****************************************************************************/
+
+/** Attaches the configuration to the addressed slave object.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_slave_config_attach(
+ ec_slave_config_t *sc /**< Slave configuration. */
+ )
+{
+ ec_slave_t *slave;
+
+ if (sc->slave)
+ return 0; // already attached
+
+ if (!(slave = ec_master_find_slave(
+ sc->master, sc->alias, sc->position))) {
+ EC_CONFIG_DBG(sc, 1, "Failed to find slave for configuration.\n");
+ return -ENOENT;
+ }
+
+ if (slave->config) {
+ EC_CONFIG_DBG(sc, 1, "Failed to attach configuration. Slave %u"
+ " already has a configuration!\n", slave->ring_position);
+ return -EEXIST;
+ }
+
+ if (
+#ifdef EC_IDENT_WILDCARDS
+ sc->vendor_id != 0xffffffff &&
+#endif
+ slave->sii.vendor_id != sc->vendor_id
+ ) {
+ EC_CONFIG_DBG(sc, 1, "Slave %u has no matching vendor ID (0x%08X)"
+ " for configuration (0x%08X).\n",
+ slave->ring_position, slave->sii.vendor_id, sc->vendor_id);
+ return -EINVAL;
+ }
+
+ if (
+#ifdef EC_IDENT_WILDCARDS
+ sc->product_code != 0xffffffff &&
+#endif
+ slave->sii.product_code != sc->product_code
+ ) {
+ EC_CONFIG_DBG(sc, 1, "Slave %u has no matching product code (0x%08X)"
+ " for configuration (0x%08X).\n",
+ slave->ring_position, slave->sii.product_code,
+ sc->product_code);
+ return -EINVAL;
+ }
+
+ // attach slave
+ slave->config = sc;
+ sc->slave = slave;
+
+ EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Detaches the configuration from a slave object.
+ */
+void ec_slave_config_detach(
+ ec_slave_config_t *sc /**< Slave configuration. */
+ )
+{
+ if (sc->slave) {
+ ec_reg_request_t *reg;
+
+ sc->slave->config = NULL;
+
+ // invalidate processing register request
+ list_for_each_entry(reg, &sc->reg_requests, list) {
+ if (sc->slave->fsm.reg_request == reg) {
+ sc->slave->fsm.reg_request = NULL;
+ break;
+ }
+ }
+
+ sc->slave = NULL;
+ }
+}
+
+/****************************************************************************/
+
+/** Loads the default PDO assignment from the slave object.
+ */
+void ec_slave_config_load_default_sync_config(ec_slave_config_t *sc)
+{
+ uint8_t sync_index;
+ ec_sync_config_t *sync_config;
+ const ec_sync_t *sync;
+
+ if (!sc->slave)
+ return;
+
+ for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
+ sync_config = &sc->sync_configs[sync_index];
+ if ((sync = ec_slave_get_sync(sc->slave, sync_index))) {
+ sync_config->dir = ec_sync_default_direction(sync);
+ if (sync_config->dir == EC_DIR_INVALID)
+ EC_SLAVE_WARN(sc->slave,
+ "SM%u has an invalid direction field!\n", sync_index);
+ ec_pdo_list_copy(&sync_config->pdos, &sync->pdos);
+ }
+ }
+}
+
+/****************************************************************************/
+
+/** Loads the default mapping for a PDO from the slave object.
+ */
+void ec_slave_config_load_default_mapping(
+ const ec_slave_config_t *sc,
+ ec_pdo_t *pdo
+ )
+{
+ unsigned int i;
+ const ec_sync_t *sync;
+ const ec_pdo_t *default_pdo;
+
+ if (!sc->slave)
+ return;
+
+ EC_CONFIG_DBG(sc, 1, "Loading default mapping for PDO 0x%04X.\n",
+ pdo->index);
+
+ // find PDO in any sync manager (it could be reassigned later)
+ for (i = 0; i < sc->slave->sii.sync_count; i++) {
+ sync = &sc->slave->sii.syncs[i];
+
+ list_for_each_entry(default_pdo, &sync->pdos.list, list) {
+ if (default_pdo->index != pdo->index)
+ continue;
+
+ if (default_pdo->name) {
+ EC_CONFIG_DBG(sc, 1, "Found PDO name \"%s\".\n",
+ default_pdo->name);
+
+ // take PDO name from assigned one
+ ec_pdo_set_name(pdo, default_pdo->name);
+ }
+
+ // copy entries (= default PDO mapping)
+ if (ec_pdo_copy_entries(pdo, default_pdo))
+ return;
+
+ if (sc->master->debug_level) {
+ const ec_pdo_entry_t *entry;
+ list_for_each_entry(entry, &pdo->entries, list) {
+ EC_CONFIG_DBG(sc, 1, "Entry 0x%04X:%02X.\n",
+ entry->index, entry->subindex);
+ }
+ }
+
+ return;
+ }
+ }
+
+ EC_CONFIG_DBG(sc, 1, "No default mapping found.\n");
+}
+
+/****************************************************************************/
+
+/** Get the number of SDO configurations.
+ *
+ * \return Number of SDO configurations.
+ */
+unsigned int ec_slave_config_sdo_count(
+ const ec_slave_config_t *sc /**< Slave configuration. */
+ )
+{
+ const ec_sdo_request_t *req;
+ unsigned int count = 0;
+
+ list_for_each_entry(req, &sc->sdo_configs, list) {
+ count++;
+ }
+
+ return count;
+}
+
+/****************************************************************************/
+
+/** Finds an SDO configuration via its position in the list.
+ *
+ * Const version.
+ *
+ * \return Search result, or NULL.
+ */
+const ec_sdo_request_t *ec_slave_config_get_sdo_by_pos_const(
+ const ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ const ec_sdo_request_t *req;
+
+ list_for_each_entry(req, &sc->sdo_configs, list) {
+ if (pos--)
+ continue;
+ return req;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Get the number of IDN configurations.
+ *
+ * \return Number of SDO configurations.
+ */
+unsigned int ec_slave_config_idn_count(
+ const ec_slave_config_t *sc /**< Slave configuration. */
+ )
+{
+ const ec_soe_request_t *req;
+ unsigned int count = 0;
+
+ list_for_each_entry(req, &sc->soe_configs, list) {
+ count++;
+ }
+
+ return count;
+}
+
+/****************************************************************************/
+
+/** Finds an IDN configuration via its position in the list.
+ *
+ * Const version.
+ *
+ * \return Search result, or NULL.
+ */
+const ec_soe_request_t *ec_slave_config_get_idn_by_pos_const(
+ const ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ const ec_soe_request_t *req;
+
+ list_for_each_entry(req, &sc->soe_configs, list) {
+ if (pos--)
+ continue;
+ return req;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Get the number of feature flags.
+ *
+ * \return Number of feature flags.
+ */
+unsigned int ec_slave_config_flag_count(
+ const ec_slave_config_t *sc /**< Slave configuration. */
+ )
+{
+ const ec_flag_t *flag;
+ unsigned int count = 0;
+
+ list_for_each_entry(flag, &sc->flags, list) {
+ count++;
+ }
+
+ return count;
+}
+
+/****************************************************************************/
+
+/** Finds a flag via its position in the list.
+ *
+ * Const version.
+ *
+ * \return Search result, or NULL.
+ */
+const ec_flag_t *ec_slave_config_get_flag_by_pos_const(
+ const ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ const ec_flag_t *flag;
+
+ list_for_each_entry(flag, &sc->flags, list) {
+ if (pos--)
+ continue;
+ return flag;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Finds a CoE SDO request via its position in the list.
+ *
+ * \return Search result, or NULL.
+ */
+ec_sdo_request_t *ec_slave_config_find_sdo_request(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ ec_sdo_request_t *req;
+
+ list_for_each_entry(req, &sc->sdo_requests, list) {
+ if (pos--)
+ continue;
+ return req;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Finds a SoE request via its position in the list.
+ *
+ * \return Search result, or NULL.
+ */
+ec_soe_request_t *ec_slave_config_find_soe_request(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ ec_soe_request_t *req;
+
+ list_for_each_entry(req, &sc->soe_requests, list) {
+ if (pos--)
+ continue;
+ return req;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Finds a register handler via its position in the list.
+ *
+ * \return Search result, or NULL.
+ */
+ec_reg_request_t *ec_slave_config_find_reg_request(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ ec_reg_request_t *reg;
+
+ list_for_each_entry(reg, &sc->reg_requests, list) {
+ if (pos--)
+ continue;
+ return reg;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Finds a VoE handler via its position in the list.
+ *
+ * \return Search result, or NULL.
+ */
+ec_voe_handler_t *ec_slave_config_find_voe_handler(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ unsigned int pos /**< Position in the list. */
+ )
+{
+ ec_voe_handler_t *voe;
+
+ list_for_each_entry(voe, &sc->voe_handlers, list) {
+ if (pos--)
+ continue;
+ return voe;
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Finds a flag.
+ *
+ * \return Search result, or NULL.
+ */
+ec_flag_t *ec_slave_config_find_flag(
+ ec_slave_config_t *sc, /**< Slave configuration. */
+ const char *key /**< Flag key. */
+ )
+{
+ if (sc) {
+ ec_flag_t *flag;
+ list_for_each_entry(flag, &sc->flags, list) {
+ if (!strcmp(flag->key, key)) {
+ return flag;
+ }
+ }
+ }
+
+ return NULL;
+}
+
+/****************************************************************************/
+
+/** Return an AL state timeout.
+ *
+ * \return Search result, or 0.
+ */
+unsigned int ec_slave_config_al_timeout(const ec_slave_config_t *sc,
+ ec_slave_state_t from, ec_slave_state_t to)
+{
+ ec_al_timeout_t *timeout;
+
+ list_for_each_entry(timeout, &sc->al_timeouts, list) {
+ if (timeout->from == from && timeout->to == to) {
+ return timeout->timeout_ms;
+ }
+ }
+
+ return 0;
+}
+
+/*****************************************************************************
+ * Application interface
+ ****************************************************************************/
+
+int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
+ ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)
+{
+ ec_sync_config_t *sync_config;
+
+ EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p,"
+ " sync_index = %u, dir = %i, watchdog_mode = %i)\n",
+ sc, sync_index, dir, watchdog_mode);
+
+ if (sync_index >= EC_MAX_SYNC_MANAGERS) {
+ EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
+ return -ENOENT;
+ }
+
+ if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) {
+ EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (unsigned int) dir);
+ return -EINVAL;
+ }
+
+ sync_config = &sc->sync_configs[sync_index];
+ sync_config->dir = dir;
+ sync_config->watchdog_mode = watchdog_mode;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_watchdog(ec_slave_config_t *sc,
+ uint16_t divider, uint16_t intervals)
+{
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, divider = %u, intervals = %u)\n",
+ __func__, sc, divider, intervals);
+
+ sc->watchdog_divider = divider;
+ sc->watchdog_intervals = intervals;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
+ uint8_t sync_index, uint16_t pdo_index)
+{
+ ec_pdo_t *pdo;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, "
+ "pdo_index = 0x%04X)\n", __func__, sc, sync_index, pdo_index);
+
+ if (sync_index >= EC_MAX_SYNC_MANAGERS) {
+ EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
+ return -EINVAL;
+ }
+
+ down(&sc->master->master_sem);
+
+ pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index);
+ if (IS_ERR(pdo)) {
+ up(&sc->master->master_sem);
+ return PTR_ERR(pdo);
+ }
+ pdo->sync_index = sync_index;
+
+ ec_slave_config_load_default_mapping(sc, pdo);
+
+ up(&sc->master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,
+ uint8_t sync_index)
+{
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u)\n",
+ __func__, sc, sync_index);
+
+ if (sync_index >= EC_MAX_SYNC_MANAGERS) {
+ EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
+ return -EINVAL;
+ }
+
+ down(&sc->master->master_sem);
+ ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);
+ up(&sc->master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
+ uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,
+ uint8_t entry_bit_length)
+{
+ uint8_t sync_index;
+ ec_pdo_t *pdo = NULL;
+ ec_pdo_entry_t *entry;
+ int retval = 0;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
+ "pdo_index = 0x%04X, entry_index = 0x%04X, "
+ "entry_subindex = 0x%02X, entry_bit_length = %u)\n",
+ __func__, sc, pdo_index, entry_index, entry_subindex,
+ entry_bit_length);
+
+ for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
+ if ((pdo = ec_pdo_list_find_pdo(
+ &sc->sync_configs[sync_index].pdos, pdo_index)))
+ break;
+
+ if (pdo) {
+ down(&sc->master->master_sem);
+ entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex,
+ entry_bit_length);
+ up(&sc->master->master_sem);
+ if (IS_ERR(entry))
+ retval = PTR_ERR(entry);
+ } else {
+ EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
+ retval = -ENOENT;
+ }
+
+ return retval;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
+ uint16_t pdo_index)
+{
+ uint8_t sync_index;
+ ec_pdo_t *pdo = NULL;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n",
+ __func__, sc, pdo_index);
+
+ for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
+ if ((pdo = ec_pdo_list_find_pdo(
+ &sc->sync_configs[sync_index].pdos, pdo_index)))
+ break;
+
+ if (pdo) {
+ down(&sc->master->master_sem);
+ ec_pdo_clear_entries(pdo);
+ up(&sc->master->master_sem);
+ } else {
+ EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_pdos(ec_slave_config_t *sc,
+ unsigned int n_syncs, const ec_sync_info_t syncs[])
+{
+ int ret;
+ unsigned int i, j, k;
+ const ec_sync_info_t *sync_info;
+ const ec_pdo_info_t *pdo_info;
+ const ec_pdo_entry_info_t *entry_info;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, n_syncs = %u, syncs = 0x%p)\n",
+ __func__, sc, n_syncs, syncs);
+
+ if (!syncs)
+ return 0;
+
+ for (i = 0; i < n_syncs; i++) {
+ sync_info = &syncs[i];
+
+ if (sync_info->index == (uint8_t) EC_END)
+ break;
+
+ if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {
+ EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n",
+ sync_info->index);
+ return -ENOENT;
+ }
+
+ ret = ecrt_slave_config_sync_manager(sc, sync_info->index,
+ sync_info->dir, sync_info->watchdog_mode);
+ if (ret)
+ return ret;
+
+ ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);
+
+ if (sync_info->n_pdos && sync_info->pdos) {
+
+ for (j = 0; j < sync_info->n_pdos; j++) {
+ pdo_info = &sync_info->pdos[j];
+
+ ret = ecrt_slave_config_pdo_assign_add(
+ sc, sync_info->index, pdo_info->index);
+ if (ret)
+ return ret;
+
+ ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);
+
+ if (pdo_info->n_entries && pdo_info->entries) {
+ for (k = 0; k < pdo_info->n_entries; k++) {
+ entry_info = &pdo_info->entries[k];
+
+ ret = ecrt_slave_config_pdo_mapping_add(sc,
+ pdo_info->index, entry_info->index,
+ entry_info->subindex,
+ entry_info->bit_length);
+ if (ret)
+ return ret;
+ }
+ }
+ }
+ }
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_reg_pdo_entry(
+ ec_slave_config_t *sc,
+ uint16_t index,
+ uint8_t subindex,
+ ec_domain_t *domain,
+ unsigned int *bit_position
+ )
+{
+ uint8_t sync_index;
+ const ec_sync_config_t *sync_config;
+ unsigned int bit_offset, bit_pos;
+ ec_pdo_t *pdo;
+ ec_pdo_entry_t *entry;
+ int sync_offset;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
+ "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n",
+ __func__, sc, index, subindex, domain, bit_position);
+
+ for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
+ sync_config = &sc->sync_configs[sync_index];
+ bit_offset = 0;
+
+ list_for_each_entry(pdo, &sync_config->pdos.list, list) {
+ list_for_each_entry(entry, &pdo->entries, list) {
+ if (entry->index != index || entry->subindex != subindex) {
+ bit_offset += entry->bit_length;
+ } else {
+ bit_pos = bit_offset % 8;
+ if (bit_position) {
+ *bit_position = bit_pos;
+ } else if (bit_pos) {
+ EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does"
+ " not byte-align.\n", index, subindex);
+ return -EFAULT;
+ }
+
+ sync_offset = ec_slave_config_prepare_fmmu(
+ sc, domain, sync_index, sync_config->dir);
+ if (sync_offset < 0)
+ return sync_offset;
+
+ return sync_offset + bit_offset / 8;
+ }
+ }
+ }
+ }
+
+ EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X is not mapped.\n",
+ index, subindex);
+ return -ENOENT;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_reg_pdo_entry_pos(
+ ec_slave_config_t *sc,
+ uint8_t sync_index,
+ unsigned int pdo_pos,
+ unsigned int entry_pos,
+ ec_domain_t *domain,
+ unsigned int *bit_position
+ )
+{
+ const ec_sync_config_t *sync_config;
+ unsigned int bit_offset, pp, ep;
+ ec_pdo_t *pdo;
+ ec_pdo_entry_t *entry;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, pdo_pos = %u,"
+ " entry_pos = %u, domain = 0x%p, bit_position = 0x%p)\n",
+ __func__, sc, sync_index, pdo_pos, entry_pos,
+ domain, bit_position);
+
+ if (sync_index >= EC_MAX_SYNC_MANAGERS) {
+ EC_CONFIG_ERR(sc, "Invalid syncmanager position %u.\n", sync_index);
+ return -EINVAL;
+ }
+
+ sync_config = &sc->sync_configs[sync_index];
+ bit_offset = 0;
+ pp = 0;
+
+ list_for_each_entry(pdo, &sync_config->pdos.list, list) {
+ ep = 0;
+ list_for_each_entry(entry, &pdo->entries, list) {
+ if (pp != pdo_pos || ep != entry_pos) {
+ bit_offset += entry->bit_length;
+ } else {
+ unsigned int bit_pos = bit_offset % 8;
+ int sync_offset;
+
+ if (bit_position) {
+ *bit_position = bit_pos;
+ } else if (bit_pos) {
+ EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does"
+ " not byte-align.\n",
+ pdo->index, entry->subindex);
+ return -EFAULT;
+ }
+
+ sync_offset = ec_slave_config_prepare_fmmu(
+ sc, domain, sync_index, sync_config->dir);
+ if (sync_offset < 0)
+ return sync_offset;
+
+ return sync_offset + bit_offset / 8;
+ }
+ ep++;
+ }
+ pp++;
+ }
+
+ EC_CONFIG_ERR(sc, "PDO entry specification %u/%u/%u out of range.\n",
+ sync_index, pdo_pos, entry_pos);
+ return -ENOENT;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,
+ uint32_t sync0_cycle_time, int32_t sync0_shift_time,
+ uint32_t sync1_cycle_time, int32_t sync1_shift_time)
+{
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, assign_activate = 0x%04X,"
+ " sync0_cycle = %u, sync0_shift = %i,"
+ " sync1_cycle = %u, sync1_shift = %i\n",
+ __func__, sc, assign_activate, sync0_cycle_time, sync0_shift_time,
+ sync1_cycle_time, sync1_shift_time);
+
+ sc->dc_assign_activate = assign_activate;
+ sc->dc_sync[0].cycle_time = sync0_cycle_time;
+ sc->dc_sync[0].shift_time = sync0_shift_time;
+ sc->dc_sync[1].cycle_time = sync1_cycle_time;
+ sc->dc_sync[1].shift_time = sync1_shift_time;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,
+ uint8_t subindex, const uint8_t *data, size_t size)
+{
+ ec_slave_t *slave = sc->slave;
+ ec_sdo_request_t *req;
+ int ret;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
+ "subindex = 0x%02X, data = 0x%p, size = %zu)\n",
+ __func__, sc, index, subindex, data, size);
+
+ if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n");
+ }
+
+ if (!(req = (ec_sdo_request_t *)
+ kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate memory for"
+ " SDO configuration!\n");
+ return -ENOMEM;
+ }
+
+ ec_sdo_request_init(req);
+ ecrt_sdo_request_index(req, index, subindex);
+
+ ret = ec_sdo_request_copy_data(req, data, size);
+ if (ret < 0) {
+ ec_sdo_request_clear(req);
+ kfree(req);
+ return ret;
+ }
+
+ down(&sc->master->master_sem);
+ list_add_tail(&req->list, &sc->sdo_configs);
+ up(&sc->master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,
+ uint8_t subindex, uint8_t value)
+{
+ uint8_t data[1];
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
+ "subindex = 0x%02X, value = %u)\n",
+ __func__, sc, index, subindex, (unsigned int) value);
+
+ EC_WRITE_U8(data, value);
+ return ecrt_slave_config_sdo(sc, index, subindex, data, 1);
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,
+ uint8_t subindex, uint16_t value)
+{
+ uint8_t data[2];
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
+ "subindex = 0x%02X, value = %u)\n",
+ __func__, sc, index, subindex, value);
+
+ EC_WRITE_U16(data, value);
+ return ecrt_slave_config_sdo(sc, index, subindex, data, 2);
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,
+ uint8_t subindex, uint32_t value)
+{
+ uint8_t data[4];
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
+ "subindex = 0x%02X, value = %u)\n",
+ __func__, sc, index, subindex, value);
+
+ EC_WRITE_U32(data, value);
+ return ecrt_slave_config_sdo(sc, index, subindex, data, 4);
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,
+ const uint8_t *data, size_t size)
+{
+ ec_slave_t *slave = sc->slave;
+ ec_sdo_request_t *req;
+ int ret;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
+ "data = 0x%p, size = %zu)\n", __func__, sc, index, data, size);
+
+ if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
+ EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n");
+ }
+
+ if (!(req = (ec_sdo_request_t *)
+ kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate memory for"
+ " SDO configuration!\n");
+ return -ENOMEM;
+ }
+
+ ec_sdo_request_init(req);
+ ecrt_sdo_request_index(req, index, 0);
+ req->complete_access = 1;
+
+ ret = ec_sdo_request_copy_data(req, data, size);
+ if (ret < 0) {
+ ec_sdo_request_clear(req);
+ kfree(req);
+ return ret;
+ }
+
+ down(&sc->master->master_sem);
+ list_add_tail(&req->list, &sc->sdo_configs);
+ up(&sc->master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements)
+{
+ return ec_coe_emerg_ring_size(&sc->emerg_ring, elements);
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target)
+{
+ return ec_coe_emerg_ring_pop(&sc->emerg_ring, target);
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_emerg_clear(ec_slave_config_t *sc)
+{
+ return ec_coe_emerg_ring_clear_ring(&sc->emerg_ring);
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_emerg_overruns(const ec_slave_config_t *sc)
+{
+ return ec_coe_emerg_ring_overruns(&sc->emerg_ring);
+}
+
+/****************************************************************************/
+
+/** Same as ecrt_slave_config_create_sdo_request(), but with ERR_PTR() return
+ * value.
+ */
+ec_sdo_request_t *ecrt_slave_config_create_sdo_request_err(
+ ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
+{
+ ec_sdo_request_t *req;
+ int ret;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
+ "index = 0x%04X, subindex = 0x%02X, size = %zu)\n",
+ __func__, sc, index, subindex, size);
+
+ if (!(req = (ec_sdo_request_t *)
+ kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ ec_sdo_request_init(req);
+ ecrt_sdo_request_index(req, index, subindex);
+
+ ret = ec_sdo_request_alloc(req, size);
+ if (ret < 0) {
+ ec_sdo_request_clear(req);
+ kfree(req);
+ return ERR_PTR(ret);
+ }
+
+ // prepare data for optional writing
+ memset(req->data, 0x00, size);
+ req->data_size = size;
+
+ down(&sc->master->master_sem);
+ list_add_tail(&req->list, &sc->sdo_requests);
+ up(&sc->master->master_sem);
+
+ return req;
+}
+
+/****************************************************************************/
+
+ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
+ ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
+{
+ ec_sdo_request_t *s = ecrt_slave_config_create_sdo_request_err(sc, index,
+ subindex, size);
+ return IS_ERR(s) ? NULL : s;
+}
+
+/****************************************************************************/
+
+/** Same as ecrt_slave_config_create_soe_request(), but with ERR_PTR() return
+ * value.
+ */
+ec_soe_request_t *ecrt_slave_config_create_soe_request_err(
+ ec_slave_config_t *sc, uint8_t drive_no, uint16_t idn, size_t size)
+{
+ ec_soe_request_t *req;
+ int ret;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
+ "drive_no = 0x%02X, idn = 0x%04X, size = %zu)\n",
+ __func__, sc, drive_no, idn, size);
+
+ if (!(req = (ec_soe_request_t *)
+ kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate IDN request memory!\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ ec_soe_request_init(req);
+ ecrt_soe_request_idn(req, drive_no, idn);
+
+ ret = ec_soe_request_alloc(req, size);
+ if (ret < 0) {
+ ec_soe_request_clear(req);
+ kfree(req);
+ return ERR_PTR(ret);
+ }
+
+ // prepare data for optional writing
+ memset(req->data, 0x00, size);
+ req->data_size = size;
+
+ down(&sc->master->master_sem);
+ list_add_tail(&req->list, &sc->soe_requests);
+ up(&sc->master->master_sem);
+
+ return req;
+}
+
+/****************************************************************************/
+
+ec_soe_request_t *ecrt_slave_config_create_soe_request(
+ ec_slave_config_t *sc, uint8_t drive_no, uint16_t idn, size_t size)
+{
+ ec_soe_request_t *req = ecrt_slave_config_create_soe_request_err(sc,
+ drive_no, idn, size);
+ return IS_ERR(req) ? NULL : req;
+}
+
+/****************************************************************************/
+
+/** Same as ecrt_slave_config_create_reg_request(), but with ERR_PTR() return
+ * value.
+ */
+ec_reg_request_t *ecrt_slave_config_create_reg_request_err(
+ ec_slave_config_t *sc, size_t size)
+{
+ ec_reg_request_t *reg;
+ int ret;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n",
+ __func__, sc, size);
+
+ if (!(reg = (ec_reg_request_t *)
+ kmalloc(sizeof(ec_reg_request_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate register request memory!\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ ret = ec_reg_request_init(reg, size);
+ if (ret) {
+ kfree(reg);
+ return ERR_PTR(ret);
+ }
+
+ down(&sc->master->master_sem);
+ list_add_tail(®->list, &sc->reg_requests);
+ up(&sc->master->master_sem);
+
+ return reg;
+}
+
+/****************************************************************************/
+
+ec_reg_request_t *ecrt_slave_config_create_reg_request(
+ ec_slave_config_t *sc, size_t size)
+{
+ ec_reg_request_t *reg =
+ ecrt_slave_config_create_reg_request_err(sc, size);
+ return IS_ERR(reg) ? NULL : reg;
+}
+
+/****************************************************************************/
+
+/** Same as ecrt_slave_config_create_voe_handler(), but with ERR_PTR() return
+ * value.
+ */
+ec_voe_handler_t *ecrt_slave_config_create_voe_handler_err(
+ ec_slave_config_t *sc, size_t size)
+{
+ ec_voe_handler_t *voe;
+ int ret;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n", __func__, sc, size);
+
+ if (!(voe = (ec_voe_handler_t *)
+ kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate VoE request memory!\n");
+ return ERR_PTR(-ENOMEM);
+ }
+
+ ret = ec_voe_handler_init(voe, sc, size);
+ if (ret < 0) {
+ kfree(voe);
+ return ERR_PTR(ret);
+ }
+
+ down(&sc->master->master_sem);
+ list_add_tail(&voe->list, &sc->voe_handlers);
+ up(&sc->master->master_sem);
+
+ return voe;
+}
+
+/****************************************************************************/
+
+ec_voe_handler_t *ecrt_slave_config_create_voe_handler(
+ ec_slave_config_t *sc, size_t size)
+{
+ ec_voe_handler_t *voe = ecrt_slave_config_create_voe_handler_err(sc,
+ size);
+ return IS_ERR(voe) ? NULL : voe;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_state(const ec_slave_config_t *sc,
+ ec_slave_config_state_t *state)
+{
+ const ec_slave_t *slave = sc->slave;
+
+ state->online = slave ? 1 : 0;
+ if (state->online) {
+ state->operational =
+ slave->current_state == EC_SLAVE_STATE_OP && !slave->force_config;
+ state->al_state = slave->current_state;
+ } else {
+ state->operational = 0;
+ state->al_state = EC_SLAVE_STATE_UNKNOWN;
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
+ uint16_t idn, ec_al_state_t state, const uint8_t *data,
+ size_t size)
+{
+ ec_slave_t *slave = sc->slave;
+ ec_soe_request_t *req;
+ int ret;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, drive_no = %u, idn = 0x%04X, "
+ "state = %u, data = 0x%p, size = %zu)\n",
+ __func__, sc, drive_no, idn, state, data, size);
+
+ if (drive_no > 7) {
+ EC_CONFIG_ERR(sc, "Invalid drive number %u!\n",
+ (unsigned int) drive_no);
+ return -EINVAL;
+ }
+
+ if (state != EC_AL_STATE_PREOP && state != EC_AL_STATE_SAFEOP) {
+ EC_CONFIG_ERR(sc, "AL state for IDN config"
+ " must be PREOP or SAFEOP!\n");
+ return -EINVAL;
+ }
+
+ if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+ EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n");
+ }
+
+ if (!(req = (ec_soe_request_t *)
+ kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate memory for"
+ " IDN configuration!\n");
+ return -ENOMEM;
+ }
+
+ ec_soe_request_init(req);
+ ec_soe_request_set_drive_no(req, drive_no);
+ ec_soe_request_set_idn(req, idn);
+ req->al_state = state;
+
+ ret = ec_soe_request_copy_data(req, data, size);
+ if (ret < 0) {
+ ec_soe_request_clear(req);
+ kfree(req);
+ return ret;
+ }
+
+ down(&sc->master->master_sem);
+ list_add_tail(&req->list, &sc->soe_configs);
+ up(&sc->master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_flag(ec_slave_config_t *sc, const char *key,
+ int32_t value)
+{
+ ec_flag_t *flag;
+
+ EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, key = %s, value = %i)\n",
+ __func__, sc, key, value);
+
+
+ flag = ec_slave_config_find_flag(sc, key);
+ if (flag) {
+ flag->value = value; // overwrite value
+ }
+ else { // new flag
+ int ret;
+
+ if (!(flag = (ec_flag_t *) kmalloc(sizeof(ec_flag_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate memory for flag!\n");
+ return -ENOMEM;
+ }
+
+ ret = ec_flag_init(flag, key, value);
+ if (ret) {
+ kfree(flag);
+ return ret;
+ }
+
+ down(&sc->master->master_sem);
+ list_add_tail(&flag->list, &sc->flags);
+ up(&sc->master->master_sem);
+ }
+ return 0;
+}
+
+/****************************************************************************/
+
+#ifdef EC_EOE
+
+int ecrt_slave_config_eoe_mac_address(ec_slave_config_t *sc,
+ const unsigned char *mac_address)
+{
+ memcpy(sc->eoe_ip_param_request.mac_address, mac_address, EC_ETH_ALEN);
+ sc->eoe_ip_param_request.mac_address_included = 1;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_eoe_ip_address(ec_slave_config_t *sc,
+ struct in_addr ip_address)
+{
+ sc->eoe_ip_param_request.ip_address = ip_address;
+ sc->eoe_ip_param_request.ip_address_included = 1;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_eoe_subnet_mask(ec_slave_config_t *sc,
+ struct in_addr subnet_mask)
+{
+ sc->eoe_ip_param_request.subnet_mask = subnet_mask;
+ sc->eoe_ip_param_request.subnet_mask_included = 1;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_eoe_default_gateway(ec_slave_config_t *sc,
+ struct in_addr gateway_address)
+{
+ sc->eoe_ip_param_request.gateway = gateway_address;
+ sc->eoe_ip_param_request.gateway_included = 1;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_eoe_dns_address(ec_slave_config_t *sc,
+ struct in_addr dns_address)
+{
+ sc->eoe_ip_param_request.dns = dns_address;
+ sc->eoe_ip_param_request.dns_included = 1;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_slave_config_eoe_hostname(ec_slave_config_t *sc,
+ const char *name)
+{
+ strncpy(sc->eoe_ip_param_request.name, name, EC_MAX_HOSTNAME_SIZE - 1);
+ sc->eoe_ip_param_request.name_included = 1;
+ return 0;
+}
+
+#endif
+
+/****************************************************************************/
+
+int ecrt_slave_config_state_timeout(ec_slave_config_t *sc,
+ ec_al_state_t from, ec_al_state_t to, unsigned int timeout_ms)
+{
+ ec_al_timeout_t *timeout;
+ ec_slave_state_t from_state, to_state;
+
+ if (from != EC_AL_STATE_INIT && from != EC_AL_STATE_PREOP &&
+ from != EC_AL_STATE_SAFEOP && from != EC_AL_STATE_OP) {
+ EC_CONFIG_ERR(sc, "Invalid from state %i.\n", from);
+ return -EINVAL;
+ }
+ if (to != EC_AL_STATE_INIT && to != EC_AL_STATE_PREOP &&
+ to != EC_AL_STATE_SAFEOP && to != EC_AL_STATE_OP) {
+ EC_CONFIG_ERR(sc, "Invalid to state %i.\n", to);
+ return -EINVAL;
+ }
+
+ from_state = (ec_slave_state_t) from;
+ to_state = (ec_slave_state_t) to;
+
+ /* try to find an already configured timeout. */
+ list_for_each_entry(timeout, &sc->al_timeouts, list) {
+ if (timeout->from == from_state && timeout->to == to_state) {
+ if (timeout_ms == 0) {
+ // delete configured value
+ list_del(&timeout->list);
+ kfree(timeout);
+ return 0;
+ }
+ timeout->timeout_ms = timeout_ms;
+ return 0;
+ }
+ }
+
+ if (timeout_ms == 0) {
+ return 0;
+ }
+
+ /* no timeout found. create one. */
+ if (!(timeout = (ec_al_timeout_t *)
+ kmalloc(sizeof(ec_al_timeout_t), GFP_KERNEL))) {
+ EC_CONFIG_ERR(sc, "Failed to allocate memory for"
+ " AL timeout configuration!\n");
+ return -ENOMEM;
+ }
+
+ timeout->from = from_state;
+ timeout->to = to_state;
+ timeout->timeout_ms = timeout_ms;
+
+ down(&sc->master->master_sem);
+ list_add_tail(&timeout->list, &sc->al_timeouts);
+ up(&sc->master->master_sem);
+ return 0;
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecrt_slave_config_sync_manager);
+EXPORT_SYMBOL(ecrt_slave_config_watchdog);
+EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add);
+EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear);
+EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add);
+EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_clear);
+EXPORT_SYMBOL(ecrt_slave_config_pdos);
+EXPORT_SYMBOL(ecrt_slave_config_reg_pdo_entry);
+EXPORT_SYMBOL(ecrt_slave_config_reg_pdo_entry_pos);
+EXPORT_SYMBOL(ecrt_slave_config_dc);
+EXPORT_SYMBOL(ecrt_slave_config_sdo);
+EXPORT_SYMBOL(ecrt_slave_config_sdo8);
+EXPORT_SYMBOL(ecrt_slave_config_sdo16);
+EXPORT_SYMBOL(ecrt_slave_config_sdo32);
+EXPORT_SYMBOL(ecrt_slave_config_complete_sdo);
+EXPORT_SYMBOL(ecrt_slave_config_emerg_size);
+EXPORT_SYMBOL(ecrt_slave_config_emerg_pop);
+EXPORT_SYMBOL(ecrt_slave_config_emerg_clear);
+EXPORT_SYMBOL(ecrt_slave_config_emerg_overruns);
+EXPORT_SYMBOL(ecrt_slave_config_create_sdo_request);
+EXPORT_SYMBOL(ecrt_slave_config_create_soe_request);
+EXPORT_SYMBOL(ecrt_slave_config_create_voe_handler);
+EXPORT_SYMBOL(ecrt_slave_config_create_reg_request);
+EXPORT_SYMBOL(ecrt_slave_config_state);
+EXPORT_SYMBOL(ecrt_slave_config_idn);
+EXPORT_SYMBOL(ecrt_slave_config_flag);
+#ifdef EOE
+EXPORT_SYMBOL(ecrt_slave_config_eoe_mac_address);
+EXPORT_SYMBOL(ecrt_slave_config_eoe_ip_address);
+EXPORT_SYMBOL(ecrt_slave_config_eoe_subnet_mask);
+EXPORT_SYMBOL(ecrt_slave_config_eoe_default_gateway);
+EXPORT_SYMBOL(ecrt_slave_config_eoe_dns_address);
+EXPORT_SYMBOL(ecrt_slave_config_eoe_hostname);
+#endif
+EXPORT_SYMBOL(ecrt_slave_config_state_timeout);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/slave_config.h b/ubuntu/igh-ecat/master/slave_config.h
new file mode 100644
index 00000000000000..124995c28150d9
--- /dev/null
+++ b/ubuntu/igh-ecat/master/slave_config.h
@@ -0,0 +1,196 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2024 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT slave configuration structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_SLAVE_CONFIG_H__
+#define __EC_SLAVE_CONFIG_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+#include "slave.h"
+#include "sync_config.h"
+#include "fmmu_config.h"
+#include "coe_emerg_ring.h"
+#include "flag.h"
+
+/****************************************************************************/
+
+/** Convenience macro for printing configuration-specific information to
+ * syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX> <ALIAS>:<POSITION>: ", where INDEX is the master index
+ * and ALIAS and POSITION identify the configuration.
+ *
+ * \param sc EtherCAT slave configuration
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_CONFIG_INFO(sc, fmt, args...) \
+ printk(KERN_INFO "EtherCAT %u %u:%u: " fmt, sc->master->index, \
+ sc->alias, sc->position, ##args)
+
+/** Convenience macro for printing configuration-specific errors to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX> <ALIAS>:<POSITION>: ", where INDEX is the master index
+ * and ALIAS and POSITION identify the configuration.
+ *
+ * \param sc EtherCAT slave configuration
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_CONFIG_ERR(sc, fmt, args...) \
+ printk(KERN_ERR "EtherCAT ERROR %u %u:%u: " fmt, sc->master->index, \
+ sc->alias, sc->position, ##args)
+
+/** Convenience macro for printing configuration-specific warnings to syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX> <ALIAS>:<POSITION>: ", where INDEX is the master index
+ * and ALIAS and POSITION identify the configuration.
+ *
+ * \param sc EtherCAT slave configuration
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_CONFIG_WARN(sc, fmt, args...) \
+ printk(KERN_WARNING "EtherCAT WARNING %u %u:%u: " fmt, \
+ sc->master->index, sc->alias, sc->position, ##args)
+
+/** Convenience macro for printing configuration-specific debug messages to
+ * syslog.
+ *
+ * This will print the message in \a fmt with a prefixed
+ * "EtherCAT <INDEX> <ALIAS>:<POSITION>: ", where INDEX is the master index
+ * and ALIAS and POSITION identify the configuration.
+ *
+ * \param sc EtherCAT slave configuration
+ * \param level Debug level. Master's debug level must be >= \a level for
+ * output.
+ * \param fmt format string (like in printf())
+ * \param args arguments (optional)
+ */
+#define EC_CONFIG_DBG(sc, level, fmt, args...) \
+ do { \
+ if (sc->master->debug_level >= level) { \
+ printk(KERN_DEBUG "EtherCAT DEBUG %u %u:%u: " fmt, \
+ sc->master->index, sc->alias, sc->position, ##args); \
+ } \
+ } while (0)
+
+/****************************************************************************/
+
+/** EtherCAT slave configuration.
+ */
+struct ec_slave_config {
+ struct list_head list; /**< List item. */
+ ec_master_t *master; /**< Master owning the slave configuration. */
+
+ uint16_t alias; /**< Slave alias. */
+ uint16_t position; /**< Index after alias. If alias is zero, this is the
+ ring position. */
+ uint32_t vendor_id; /**< Slave vendor ID. */
+ uint32_t product_code; /**< Slave product code. */
+
+ uint16_t watchdog_divider; /**< Watchdog divider as a number of 40ns
+ intervals (see spec. reg. 0x0400). */
+ uint16_t watchdog_intervals; /**< Process data watchdog intervals (see
+ spec. reg. 0x0420). */
+
+ ec_slave_t *slave; /**< Slave pointer. This is \a NULL, if the slave is
+ offline. */
+
+ ec_sync_config_t sync_configs[EC_MAX_SYNC_MANAGERS]; /**< Sync manager
+ configurations. */
+ ec_fmmu_config_t fmmu_configs[EC_MAX_FMMUS]; /**< FMMU configurations. */
+ uint8_t used_fmmus; /**< Number of FMMUs used. */
+ uint16_t dc_assign_activate; /**< Vendor-specific AssignActivate word. */
+ ec_sync_signal_t dc_sync[EC_SYNC_SIGNAL_COUNT]; /**< DC sync signals. */
+
+ struct list_head sdo_configs; /**< List of SDO configurations. */
+ struct list_head sdo_requests; /**< List of SDO requests. */
+ struct list_head soe_requests; /**< List of SoE requests. */
+ struct list_head voe_handlers; /**< List of VoE handlers. */
+ struct list_head reg_requests; /**< List of register requests. */
+ struct list_head soe_configs; /**< List of SoE configurations. */
+ struct list_head flags; /**< List of feature flags. */
+ struct list_head al_timeouts; /**< List of specific AL state timeouts. */
+
+#ifdef EC_EOE
+ ec_eoe_request_t eoe_ip_param_request; /**< EoE IP parameters. */
+#endif
+
+ ec_coe_emerg_ring_t emerg_ring; /**< CoE emergency ring buffer. */
+};
+
+/****************************************************************************/
+
+void ec_slave_config_init(ec_slave_config_t *, ec_master_t *, uint16_t,
+ uint16_t, uint32_t, uint32_t);
+void ec_slave_config_clear(ec_slave_config_t *);
+
+int ec_slave_config_attach(ec_slave_config_t *);
+void ec_slave_config_detach(ec_slave_config_t *);
+
+void ec_slave_config_load_default_sync_config(ec_slave_config_t *);
+
+unsigned int ec_slave_config_sdo_count(const ec_slave_config_t *);
+const ec_sdo_request_t *ec_slave_config_get_sdo_by_pos_const(
+ const ec_slave_config_t *, unsigned int);
+unsigned int ec_slave_config_idn_count(const ec_slave_config_t *);
+const ec_soe_request_t *ec_slave_config_get_idn_by_pos_const(
+ const ec_slave_config_t *, unsigned int);
+unsigned int ec_slave_config_flag_count(const ec_slave_config_t *);
+const ec_flag_t *ec_slave_config_get_flag_by_pos_const(
+ const ec_slave_config_t *, unsigned int);
+ec_sdo_request_t *ec_slave_config_find_sdo_request(ec_slave_config_t *,
+ unsigned int);
+ec_soe_request_t *ec_slave_config_find_soe_request(ec_slave_config_t *,
+ unsigned int);
+ec_reg_request_t *ec_slave_config_find_reg_request(ec_slave_config_t *,
+ unsigned int);
+ec_voe_handler_t *ec_slave_config_find_voe_handler(ec_slave_config_t *,
+ unsigned int);
+ec_flag_t *ec_slave_config_find_flag(ec_slave_config_t *, const char *);
+
+ec_sdo_request_t *ecrt_slave_config_create_sdo_request_err(
+ ec_slave_config_t *, uint16_t, uint8_t, size_t);
+ec_soe_request_t *ecrt_slave_config_create_soe_request_err(
+ ec_slave_config_t *, uint8_t, uint16_t, size_t);
+ec_voe_handler_t *ecrt_slave_config_create_voe_handler_err(
+ ec_slave_config_t *, size_t);
+ec_reg_request_t *ecrt_slave_config_create_reg_request_err(
+ ec_slave_config_t *, size_t);
+
+unsigned int ec_slave_config_al_timeout(const ec_slave_config_t *,
+ ec_slave_state_t, ec_slave_state_t);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/soe_errors.c b/ubuntu/igh-ecat/master/soe_errors.c
new file mode 100644
index 00000000000000..937803b2e6ca5e
--- /dev/null
+++ b/ubuntu/igh-ecat/master/soe_errors.c
@@ -0,0 +1,88 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT SoE errors.
+*/
+
+/****************************************************************************/
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** SoE error codes.
+ */
+const ec_code_msg_t soe_error_codes[] = {
+ {0x1001, "No IDN"},
+ {0x1009, "Invalid access to element 1"},
+ {0x2001, "No name"},
+ {0x2002, "Name transmission too short"},
+ {0x2003, "Name transmission too long"},
+ {0x2004, "Name cannot be changed, read only"},
+ {0x2005, "Name is write protected at this time"},
+ {0x3002, "Attribute transmission too short"},
+ {0x3003, "Attribute transmission too long"},
+ {0x3004, "Attribute cannot be changed, read only"},
+ {0x3005, "Attribute is write protected at this time"},
+ {0x4001, "No unit"},
+ {0x4002, "Unit transmission too short"},
+ {0x4003, "Unit transmission too long"},
+ {0x4004, "Unit cannot be changed, read only"},
+ {0x4005, "Unit is write proteced at this time"},
+ {0x5001, "No minimum input value"},
+ {0x5002, "Minimum input value transmission too short"},
+ {0x5003, "Minimum input value transmission too long"},
+ {0x5004, "Minimum input value cannot be changed, read only"},
+ {0x5005, "Minimum input value is write protected at this time"},
+ {0x6001, "No maximum input value"},
+ {0x6002, "Maximum input value transmission too short"},
+ {0x6003, "Maximum input value transmission too long"},
+ {0x6004, "Maximum input value cannot be changed, read only"},
+ {0x6005, "Maximum input value is write protected at this time"},
+ {0x7002, "Operation data value transmission too short"},
+ {0x7003, "Operation data value transmission too long"},
+ {0x7004, "Operation data value cannot be changed, read only"},
+ {0x7005, "Operation data value is write protected at this time"},
+ {0x7006, "Operation data value is smaller than the minimum input value"},
+ {0x7007, "Operation data value is greater than the minimum input value"},
+ {0x7008, "Invalid operation data"},
+ {0x7009, "Operation data is write protected by a password"},
+ {0x700A, "Operation data is write protected"},
+ {0x700B, "Invalid indirect addressing"},
+ {0x700C, "Operation data is write protected due to other settings"},
+ {0x700D, "Reserved"},
+ {0x7010, "Procedure command already active"},
+ {0x7011, "Procedure command not interruptible"},
+ {0x7012, "Procedure command is at this time not executable"},
+ {0x7013, "Procedure command not executable"},
+ {0x7014, "No data state"},
+ {0x8001, "No default value"},
+ {0x8002, "Default value transmission too long"},
+ {0x8004, "Default value cannot be changed, read only"},
+ {0x800A, "Invalid drive number"},
+ {0x800B, "General error"},
+ {0x800C, "No element addressed"},
+ {}
+};
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/soe_request.c b/ubuntu/igh-ecat/master/soe_request.c
new file mode 100644
index 00000000000000..ef90124ba35a72
--- /dev/null
+++ b/ubuntu/igh-ecat/master/soe_request.c
@@ -0,0 +1,333 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * Sercos-over-EtherCAT request functions.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+
+#include "soe_request.h"
+
+/****************************************************************************/
+
+/** Default timeout in ms to wait for SoE responses.
+ */
+#define EC_SOE_REQUEST_RESPONSE_TIMEOUT 1000
+
+/****************************************************************************/
+
+void ec_soe_request_clear_data(ec_soe_request_t *);
+
+/****************************************************************************/
+
+/** SoE request constructor.
+ */
+void ec_soe_request_init(
+ ec_soe_request_t *req /**< SoE request. */
+ )
+{
+ INIT_LIST_HEAD(&req->list);
+ req->drive_no = 0x00;
+ req->idn = 0x0000;
+ req->al_state = EC_AL_STATE_INIT;
+ req->data = NULL;
+ req->mem_size = 0;
+ req->data_size = 0;
+ req->issue_timeout = 0; // no timeout
+ req->dir = EC_DIR_INVALID;
+ req->state = EC_INT_REQUEST_INIT;
+ req->jiffies_start = 0U;
+ req->jiffies_sent = 0U;
+ req->error_code = 0x0000;
+}
+
+/****************************************************************************/
+
+/** SoE request destructor.
+ */
+void ec_soe_request_clear(
+ ec_soe_request_t *req /**< SoE request. */
+ )
+{
+ ec_soe_request_clear_data(req);
+}
+
+/****************************************************************************/
+
+/** Copy another SoE request.
+ *
+ * \return Zero on success, otherwise a negative error code.
+ */
+int ec_soe_request_copy(
+ ec_soe_request_t *req, /**< SoE request. */
+ const ec_soe_request_t *other /**< Other SoE request to copy from. */
+ )
+{
+ req->drive_no = other->drive_no;
+ req->idn = other->idn;
+ req->al_state = other->al_state;
+ return ec_soe_request_copy_data(req, other->data, other->data_size);
+}
+
+/****************************************************************************/
+
+/** Set drive number.
+ */
+void ec_soe_request_set_drive_no(
+ ec_soe_request_t *req, /**< SoE request. */
+ uint8_t drive_no /** Drive Number. */
+ )
+{
+ req->drive_no = drive_no;
+}
+
+/****************************************************************************/
+
+/** Set IDN.
+ */
+void ec_soe_request_set_idn(
+ ec_soe_request_t *req, /**< SoE request. */
+ uint16_t idn /** IDN. */
+ )
+{
+ req->idn = idn;
+}
+
+/****************************************************************************/
+
+/** Free allocated memory.
+ */
+void ec_soe_request_clear_data(
+ ec_soe_request_t *req /**< SoE request. */
+ )
+{
+ if (req->data) {
+ kfree(req->data);
+ req->data = NULL;
+ }
+
+ req->mem_size = 0;
+ req->data_size = 0;
+}
+
+/****************************************************************************/
+
+/** Pre-allocates the data memory.
+ *
+ * If the \a mem_size is already bigger than \a size, nothing is done.
+ *
+ * \return 0 on success, otherwise -ENOMEM.
+ */
+int ec_soe_request_alloc(
+ ec_soe_request_t *req, /**< SoE request. */
+ size_t size /**< Data size to allocate. */
+ )
+{
+ if (size <= req->mem_size)
+ return 0;
+
+ ec_soe_request_clear_data(req);
+
+ if (!(req->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
+ EC_ERR("Failed to allocate %zu bytes of SoE memory.\n", size);
+ return -ENOMEM;
+ }
+
+ req->mem_size = size;
+ req->data_size = 0;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Copies SoE data from an external source.
+ *
+ * If the \a mem_size is to small, new memory is allocated.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_soe_request_copy_data(
+ ec_soe_request_t *req, /**< SoE request. */
+ const uint8_t *source, /**< Source data. */
+ size_t size /**< Number of bytes in \a source. */
+ )
+{
+ int ret = ec_soe_request_alloc(req, size);
+ if (ret < 0)
+ return ret;
+
+ memcpy(req->data, source, size);
+ req->data_size = size;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Copies SoE data from an external source.
+ *
+ * If the \a mem_size is to small, new memory is allocated.
+ *
+ * \retval 0 Success.
+ * \retval <0 Error code.
+ */
+int ec_soe_request_append_data(
+ ec_soe_request_t *req, /**< SoE request. */
+ const uint8_t *source, /**< Source data. */
+ size_t size /**< Number of bytes in \a source. */
+ )
+{
+ if (req->data_size + size > req->mem_size) {
+ size_t new_size = req->mem_size ? req->mem_size * 2 : size;
+ uint8_t *new_data = (uint8_t *) kmalloc(new_size, GFP_KERNEL);
+ if (!new_data) {
+ EC_ERR("Failed to allocate %zu bytes of SoE memory.\n",
+ new_size);
+ return -ENOMEM;
+ }
+ memcpy(new_data, req->data, req->data_size);
+ kfree(req->data);
+ req->data = new_data;
+ req->mem_size = new_size;
+ }
+
+ memcpy(req->data + req->data_size, source, size);
+ req->data_size += size;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Request a read operation.
+ */
+int ec_soe_request_read(
+ ec_soe_request_t *req /**< SoE request. */
+ )
+{
+ req->dir = EC_DIR_INPUT;
+ req->state = EC_INT_REQUEST_QUEUED;
+ req->error_code = 0x0000;
+ req->jiffies_start = jiffies;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Request a write operation.
+ */
+int ec_soe_request_write(
+ ec_soe_request_t *req /**< SoE request. */
+ )
+{
+ req->dir = EC_DIR_OUTPUT;
+ req->state = EC_INT_REQUEST_QUEUED;
+ req->error_code = 0x0000;
+ req->jiffies_start = jiffies;
+ return 0;
+}
+
+/****************************************************************************/
+
+/** Checks, if the timeout was exceeded.
+ *
+ * \return non-zero if the timeout was exceeded, else zero.
+ */
+int ec_soe_request_timed_out(const ec_soe_request_t *req /**< SDO request. */)
+{
+ return req->issue_timeout
+ && jiffies - req->jiffies_start > HZ * req->issue_timeout / 1000;
+}
+
+/*****************************************************************************
+ * Application interface.
+ ****************************************************************************/
+
+int ecrt_soe_request_idn(ec_soe_request_t *req, uint8_t drive_no,
+ uint16_t idn)
+{
+ req->drive_no = drive_no;
+ req->idn = idn;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_soe_request_timeout(ec_soe_request_t *req, uint32_t timeout)
+{
+ req->issue_timeout = timeout;
+ return 0;
+}
+
+/****************************************************************************/
+
+uint8_t *ecrt_soe_request_data(const ec_soe_request_t *req)
+{
+ return req->data;
+}
+
+/****************************************************************************/
+
+size_t ecrt_soe_request_data_size(const ec_soe_request_t *req)
+{
+ return req->data_size;
+}
+
+/****************************************************************************/
+
+ec_request_state_t ecrt_soe_request_state(const ec_soe_request_t *req)
+{
+ return ec_request_state_translation_table[req->state];
+}
+
+/****************************************************************************/
+
+int ecrt_soe_request_read(ec_soe_request_t *req)
+{
+ return ec_soe_request_read(req);
+}
+
+/****************************************************************************/
+
+int ecrt_soe_request_write(ec_soe_request_t *req)
+{
+ return ec_soe_request_write(req);
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecrt_soe_request_idn);
+EXPORT_SYMBOL(ecrt_soe_request_timeout);
+EXPORT_SYMBOL(ecrt_soe_request_data);
+EXPORT_SYMBOL(ecrt_soe_request_data_size);
+EXPORT_SYMBOL(ecrt_soe_request_state);
+EXPORT_SYMBOL(ecrt_soe_request_read);
+EXPORT_SYMBOL(ecrt_soe_request_write);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/soe_request.h b/ubuntu/igh-ecat/master/soe_request.h
new file mode 100644
index 00000000000000..ced4c9a196c048
--- /dev/null
+++ b/ubuntu/igh-ecat/master/soe_request.h
@@ -0,0 +1,76 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ EtherCAT SoE request structure.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_SOE_REQUEST_H__
+#define __EC_SOE_REQUEST_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+
+/****************************************************************************/
+
+/** Sercos-over-EtherCAT request.
+ */
+struct ec_soe_request {
+ struct list_head list; /**< List item. */
+ uint8_t drive_no; /**< Drive number. */
+ uint16_t idn; /**< Sercos ID-Number. */
+ ec_al_state_t al_state; /**< AL state (only valid for IDN config). */
+ uint8_t *data; /**< Pointer to SDO data. */
+ size_t mem_size; /**< Size of SDO data memory. */
+ size_t data_size; /**< Size of SDO data. */
+ uint32_t issue_timeout; /**< Maximum time in ms, the processing of the
+ request may take. */
+ ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means writing to the
+ slave, EC_DIR_INPUT means reading from the slave. */
+ ec_internal_request_state_t state; /**< Request state. */
+ unsigned long jiffies_start; /**< Jiffies, when the request was issued. */
+ unsigned long jiffies_sent; /**< Jiffies, when the upload/download
+ request was sent. */
+ uint16_t error_code; /**< SoE error code. */
+};
+
+/****************************************************************************/
+
+void ec_soe_request_init(ec_soe_request_t *);
+void ec_soe_request_clear(ec_soe_request_t *);
+
+int ec_soe_request_copy(ec_soe_request_t *, const ec_soe_request_t *);
+void ec_soe_request_set_drive_no(ec_soe_request_t *, uint8_t);
+void ec_soe_request_set_idn(ec_soe_request_t *, uint16_t);
+int ec_soe_request_alloc(ec_soe_request_t *, size_t);
+int ec_soe_request_copy_data(ec_soe_request_t *, const uint8_t *, size_t);
+int ec_soe_request_append_data(ec_soe_request_t *, const uint8_t *, size_t);
+int ec_soe_request_read(ec_soe_request_t *);
+int ec_soe_request_write(ec_soe_request_t *);
+int ec_soe_request_timed_out(const ec_soe_request_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/sync.c b/ubuntu/igh-ecat/master/sync.c
new file mode 100644
index 00000000000000..bcfe63fcafe602
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sync.c
@@ -0,0 +1,170 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT sync manager methods.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "slave.h"
+#include "master.h"
+#include "pdo.h"
+#include "sync.h"
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_sync_init(
+ ec_sync_t *sync, /**< EtherCAT sync manager. */
+ ec_slave_t *slave /**< EtherCAT slave. */
+ )
+{
+ sync->slave = slave;
+ sync->physical_start_address = 0x0000;
+ sync->default_length = 0x0000;
+ sync->control_register = 0x00;
+ sync->enable = 0x00;
+ ec_pdo_list_init(&sync->pdos);
+}
+
+/****************************************************************************/
+
+/** Copy constructor.
+ */
+void ec_sync_init_copy(
+ ec_sync_t *sync, /**< EtherCAT sync manager. */
+ const ec_sync_t *other /**< Sync manager to copy from. */
+ )
+{
+ sync->slave = other->slave;
+ sync->physical_start_address = other->physical_start_address;
+ sync->default_length = other->default_length;
+ sync->control_register = other->control_register;
+ sync->enable = other->enable;
+ ec_pdo_list_init(&sync->pdos);
+ ec_pdo_list_copy(&sync->pdos, &other->pdos);
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_sync_clear(
+ ec_sync_t *sync /**< EtherCAT sync manager. */
+ )
+{
+ ec_pdo_list_clear(&sync->pdos);
+}
+
+/****************************************************************************/
+
+/** Initializes a sync manager configuration page.
+ *
+ * The referenced memory (\a data) must be at least \a EC_SYNC_SIZE bytes.
+ */
+void ec_sync_page(
+ const ec_sync_t *sync, /**< Sync manager. */
+ uint8_t sync_index, /**< Index of the sync manager. */
+ uint16_t data_size, /**< Data size. */
+ const ec_sync_config_t *sync_config, /**< Configuration. */
+ uint8_t pdo_xfer, /**< Non-zero, if PDOs will be transferred via this
+ sync manager. */
+ uint8_t *data /**> Configuration memory. */
+ )
+{
+ // enable only if (SII enable is set or PDO xfer)
+ // and size is > 0 and SM is not virtual
+ uint16_t enable = ((sync->enable & 0x01) || pdo_xfer)
+ && data_size
+ && ((sync->enable & 0x04) == 0);
+ uint8_t control = sync->control_register;
+
+ if (sync_config) {
+
+ switch (sync_config->dir) {
+ case EC_DIR_OUTPUT:
+ case EC_DIR_INPUT:
+ EC_WRITE_BIT(&control, 2,
+ sync_config->dir == EC_DIR_OUTPUT ? 1 : 0);
+ EC_WRITE_BIT(&control, 3, 0);
+ break;
+ default:
+ break;
+ }
+
+ switch (sync_config->watchdog_mode) {
+ case EC_WD_ENABLE:
+ case EC_WD_DISABLE:
+ EC_WRITE_BIT(&control, 6,
+ sync_config->watchdog_mode == EC_WD_ENABLE);
+ break;
+ default:
+ break;
+ }
+ }
+
+ EC_SLAVE_DBG(sync->slave, 1, "SM%u: Addr 0x%04X, Size %3u,"
+ " Ctrl 0x%02X, En %u\n",
+ sync_index, sync->physical_start_address,
+ data_size, control, enable);
+
+ EC_WRITE_U16(data, sync->physical_start_address);
+ EC_WRITE_U16(data + 2, data_size);
+ EC_WRITE_U8 (data + 4, control);
+ EC_WRITE_U8 (data + 5, 0x00); // status byte (read only)
+ EC_WRITE_U16(data + 6, enable);
+}
+
+/****************************************************************************/
+
+/** Adds a PDO to the list of known mapped PDOs.
+ *
+ * \return 0 on success, else < 0
+ */
+int ec_sync_add_pdo(
+ ec_sync_t *sync, /**< EtherCAT sync manager. */
+ const ec_pdo_t *pdo /**< PDO to map. */
+ )
+{
+ return ec_pdo_list_add_pdo_copy(&sync->pdos, pdo);
+}
+
+/****************************************************************************/
+
+/** Determines the default direction from the control register.
+ *
+ * \return Direction.
+ */
+ec_direction_t ec_sync_default_direction(
+ const ec_sync_t *sync /**< EtherCAT sync manager. */
+ )
+{
+ switch ((sync->control_register & 0x0C) >> 2) {
+ case 0x0: return EC_DIR_INPUT;
+ case 0x1: return EC_DIR_OUTPUT;
+ default: return EC_DIR_INVALID;
+ }
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/sync.h b/ubuntu/igh-ecat/master/sync.h
new file mode 100644
index 00000000000000..9d0ddced610fbb
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sync.h
@@ -0,0 +1,60 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT sync manager.
+ */
+
+/****************************************************************************/
+
+#ifndef __EC_SYNC_H__
+#define __EC_SYNC_H__
+
+#include "globals.h"
+#include "pdo_list.h"
+#include "sync_config.h"
+
+/****************************************************************************/
+
+/** Sync manager.
+ */
+typedef struct {
+ ec_slave_t *slave; /**< Slave, the sync manager belongs to. */
+ uint16_t physical_start_address; /**< Physical start address. */
+ uint16_t default_length; /**< Data length in bytes. */
+ uint8_t control_register; /**< Control register value. */
+ uint8_t enable; /**< Enable bit. */
+ ec_pdo_list_t pdos; /**< Current PDO assignment. */
+} ec_sync_t;
+
+/****************************************************************************/
+
+void ec_sync_init(ec_sync_t *, ec_slave_t *);
+void ec_sync_init_copy(ec_sync_t *, const ec_sync_t *);
+void ec_sync_clear(ec_sync_t *);
+void ec_sync_page(const ec_sync_t *, uint8_t, uint16_t,
+ const ec_sync_config_t *, uint8_t, uint8_t *);
+int ec_sync_add_pdo(ec_sync_t *, const ec_pdo_t *);
+ec_direction_t ec_sync_default_direction(const ec_sync_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/sync_config.c b/ubuntu/igh-ecat/master/sync_config.c
new file mode 100644
index 00000000000000..d2d87a74948d32
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sync_config.c
@@ -0,0 +1,55 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT sync manager configuration methods.
+ */
+
+/****************************************************************************/
+
+#include "globals.h"
+#include "sync_config.h"
+
+/****************************************************************************/
+
+/** Constructor.
+ */
+void ec_sync_config_init(
+ ec_sync_config_t *sync_config /**< Sync manager configuration. */
+ )
+{
+ sync_config->dir = EC_DIR_INVALID;
+ sync_config->watchdog_mode = EC_WD_DEFAULT;
+ ec_pdo_list_init(&sync_config->pdos);
+}
+
+/****************************************************************************/
+
+/** Destructor.
+ */
+void ec_sync_config_clear(
+ ec_sync_config_t *sync_config /**< Sync manager configuration. */
+ )
+{
+ ec_pdo_list_clear(&sync_config->pdos);
+}
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/sync_config.h b/ubuntu/igh-ecat/master/sync_config.h
new file mode 100644
index 00000000000000..ca31aa72014165
--- /dev/null
+++ b/ubuntu/igh-ecat/master/sync_config.h
@@ -0,0 +1,51 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * EtherCAT sync manager.
+ */
+
+/****************************************************************************/
+
+#ifndef __EC_SYNC_CONFIG_H__
+#define __EC_SYNC_CONFIG_H__
+
+#include "globals.h"
+#include "pdo_list.h"
+
+/****************************************************************************/
+
+/** Sync manager configuration.
+ */
+typedef struct {
+ ec_direction_t dir; /**< Sync manager direction. */
+ ec_watchdog_mode_t watchdog_mode; /**< Watchdog mode. */
+ ec_pdo_list_t pdos; /**< Current PDO assignment. */
+} ec_sync_config_t;
+
+/****************************************************************************/
+
+void ec_sync_config_init(ec_sync_config_t *);
+void ec_sync_config_clear(ec_sync_config_t *);
+
+/****************************************************************************/
+
+#endif
diff --git a/ubuntu/igh-ecat/master/voe_handler.c b/ubuntu/igh-ecat/master/voe_handler.c
new file mode 100644
index 00000000000000..fc8cf307b34d83
--- /dev/null
+++ b/ubuntu/igh-ecat/master/voe_handler.c
@@ -0,0 +1,553 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/** \file
+ * Vendor specific over EtherCAT protocol handler functions.
+ */
+
+/****************************************************************************/
+
+#include <linux/module.h>
+
+#include "master.h"
+#include "slave_config.h"
+#include "mailbox.h"
+#include "voe_handler.h"
+
+/** VoE header size.
+ */
+#define EC_VOE_HEADER_SIZE 6
+
+/** VoE response timeout in [ms].
+ */
+#define EC_VOE_RESPONSE_TIMEOUT 500
+
+/****************************************************************************/
+
+void ec_voe_handler_state_write_start(ec_voe_handler_t *);
+void ec_voe_handler_state_write_response(ec_voe_handler_t *);
+
+void ec_voe_handler_state_read_start(ec_voe_handler_t *);
+void ec_voe_handler_state_read_check(ec_voe_handler_t *);
+void ec_voe_handler_state_read_response(ec_voe_handler_t *);
+
+void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *);
+void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *);
+
+void ec_voe_handler_state_end(ec_voe_handler_t *);
+void ec_voe_handler_state_error(ec_voe_handler_t *);
+
+/****************************************************************************/
+
+/** VoE handler constructor.
+ *
+ * \return Return value of ec_datagram_prealloc().
+ */
+int ec_voe_handler_init(
+ ec_voe_handler_t *voe, /**< VoE handler. */
+ ec_slave_config_t *sc, /**< Parent slave configuration. */
+ size_t size /**< Size of memory to reserve. */
+ )
+{
+ voe->config = sc;
+ voe->vendor_id = 0x00000000;
+ voe->vendor_type = 0x0000;
+ voe->data_size = 0;
+ voe->dir = EC_DIR_INVALID;
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_INIT;
+
+ ec_datagram_init(&voe->datagram);
+ return ec_datagram_prealloc(&voe->datagram,
+ size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);
+}
+
+/****************************************************************************/
+
+/** VoE handler destructor.
+ */
+void ec_voe_handler_clear(
+ ec_voe_handler_t *voe /**< VoE handler. */
+ )
+{
+ ec_datagram_clear(&voe->datagram);
+}
+
+/****************************************************************************/
+
+/** Get usable memory size.
+ *
+ * \return Memory size.
+ */
+size_t ec_voe_handler_mem_size(
+ const ec_voe_handler_t *voe /**< VoE handler. */
+ )
+{
+ if (voe->datagram.mem_size >= EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE)
+ return voe->datagram.mem_size -
+ (EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);
+ else
+ return 0;
+}
+
+/*****************************************************************************
+ * Application interface.
+ ****************************************************************************/
+
+int ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
+ uint16_t vendor_type)
+{
+ voe->vendor_id = vendor_id;
+ voe->vendor_type = vendor_type;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,
+ uint32_t *vendor_id, uint16_t *vendor_type)
+{
+ uint8_t *header = voe->datagram.data + EC_MBOX_HEADER_SIZE;
+
+ if (vendor_id)
+ *vendor_id = EC_READ_U32(header);
+ if (vendor_type)
+ *vendor_type = EC_READ_U16(header + 4);
+ return 0;
+}
+
+/****************************************************************************/
+
+uint8_t *ecrt_voe_handler_data(const ec_voe_handler_t *voe)
+{
+ return voe->datagram.data + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE;
+}
+
+/****************************************************************************/
+
+size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)
+{
+ return voe->data_size;
+}
+
+/****************************************************************************/
+
+int ecrt_voe_handler_read(ec_voe_handler_t *voe)
+{
+ voe->dir = EC_DIR_INPUT;
+ voe->state = ec_voe_handler_state_read_start;
+ voe->request_state = EC_INT_REQUEST_BUSY;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)
+{
+ voe->dir = EC_DIR_INPUT;
+ voe->state = ec_voe_handler_state_read_nosync_start;
+ voe->request_state = EC_INT_REQUEST_BUSY;
+ return 0;
+}
+
+/****************************************************************************/
+
+int ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
+{
+ voe->dir = EC_DIR_OUTPUT;
+ voe->data_size = size;
+ voe->state = ec_voe_handler_state_write_start;
+ voe->request_state = EC_INT_REQUEST_BUSY;
+ return 0;
+}
+
+/****************************************************************************/
+
+ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
+{
+ if (voe->config->slave) { // FIXME locking?
+ voe->state(voe);
+ if (voe->request_state == EC_INT_REQUEST_BUSY) {
+ ec_master_queue_datagram(voe->config->master, &voe->datagram);
+ }
+ } else {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ }
+
+ return ec_request_state_translation_table[voe->request_state];
+}
+
+/*****************************************************************************
+ * State functions.
+ ****************************************************************************/
+
+/** Start writing VoE data.
+ */
+void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
+{
+ ec_slave_t *slave = voe->config->slave;
+ uint8_t *data;
+
+ if (slave->master->debug_level) {
+ EC_SLAVE_DBG(slave, 0, "Writing %zu bytes of VoE data.\n",
+ voe->data_size);
+ ec_print_data(ecrt_voe_handler_data(voe), voe->data_size);
+ }
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ return;
+ }
+
+ data = ec_slave_mbox_prepare_send(slave, &voe->datagram,
+ EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size);
+ if (IS_ERR(data)) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ return;
+ }
+
+ EC_WRITE_U32(data, voe->vendor_id);
+ EC_WRITE_U16(data + 4, voe->vendor_type);
+ /* data already in datagram */
+
+ voe->retries = EC_FSM_RETRIES;
+ voe->jiffies_start = jiffies;
+ voe->state = ec_voe_handler_state_write_response;
+}
+
+/****************************************************************************/
+
+/** Wait for the mailbox response.
+ */
+void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
+{
+ ec_datagram_t *datagram = &voe->datagram;
+ ec_slave_t *slave = voe->config->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Failed to receive VoE write request datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ if (!datagram->working_counter) {
+ unsigned long diff_ms =
+ (jiffies - voe->jiffies_start) * 1000 / HZ;
+ if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {
+ EC_SLAVE_DBG(slave, 1, "Slave did not respond to"
+ " VoE write request. Retrying after %lu ms...\n",
+ diff_ms);
+ // no response; send request datagram again
+ return;
+ }
+ }
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Reception of VoE write request failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ EC_CONFIG_DBG(voe->config, 1, "VoE write request successful.\n");
+
+ voe->request_state = EC_INT_REQUEST_SUCCESS;
+ voe->state = ec_voe_handler_state_end;
+}
+
+/****************************************************************************/
+
+/** Start reading VoE data.
+ */
+void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
+{
+ ec_datagram_t *datagram = &voe->datagram;
+ ec_slave_t *slave = voe->config->slave;
+
+ EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+
+ voe->jiffies_start = jiffies;
+ voe->retries = EC_FSM_RETRIES;
+ voe->state = ec_voe_handler_state_read_check;
+}
+
+/****************************************************************************/
+
+/** Check for new data in the mailbox.
+ */
+void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
+{
+ ec_datagram_t *datagram = &voe->datagram;
+ ec_slave_t *slave = voe->config->slave;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Reception of VoE mailbox check"
+ " datagram failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(datagram)) {
+ unsigned long diff_ms =
+ (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n");
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ voe->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ voe->retries = EC_FSM_RETRIES;
+ voe->state = ec_voe_handler_state_read_response;
+}
+
+/****************************************************************************/
+
+/** Read the pending mailbox data.
+ */
+void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
+{
+ ec_datagram_t *datagram = &voe->datagram;
+ ec_slave_t *slave = voe->config->slave;
+ ec_master_t *master = voe->config->master;
+ uint8_t *data, mbox_prot;
+ size_t rec_size;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Reception of VoE read response failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_VOE) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"
+ " as response.\n", mbox_prot);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ if (rec_size < EC_VOE_HEADER_SIZE) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Received VoE header is"
+ " incomplete (%zu bytes)!\n", rec_size);
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_CONFIG_DBG(voe->config, 0, "VoE data:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
+ voe->request_state = EC_INT_REQUEST_SUCCESS;
+ voe->state = ec_voe_handler_state_end; // success
+}
+
+/****************************************************************************/
+
+/** Start reading VoE data without sending a sync message before.
+ */
+void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)
+{
+ ec_datagram_t *datagram = &voe->datagram;
+ ec_slave_t *slave = voe->config->slave;
+
+ EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
+ EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ return;
+ }
+
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+
+ voe->jiffies_start = jiffies;
+ voe->retries = EC_FSM_RETRIES;
+ voe->state = ec_voe_handler_state_read_nosync_response;
+}
+
+/****************************************************************************/
+
+/** Read the pending mailbox data without sending a sync message before. This
+ * might lead to an empty reponse from the client.
+ */
+void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
+{
+ ec_datagram_t *datagram = &voe->datagram;
+ ec_slave_t *slave = voe->config->slave;
+ ec_master_t *master = voe->config->master;
+ uint8_t *data, mbox_prot;
+ size_t rec_size;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
+ return;
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter == 0) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_DBG(slave, 1, "Slave did not send VoE data.\n");
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_WARN(slave, "Reception of VoE read response failed: ");
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ if (!(data = ec_slave_mbox_fetch(slave, datagram,
+ &mbox_prot, &rec_size))) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ return;
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_VOE) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"
+ " as response.\n", mbox_prot);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ if (rec_size < EC_VOE_HEADER_SIZE) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_SLAVE_ERR(slave, "Received VoE header is"
+ " incomplete (%zu bytes)!\n", rec_size);
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_CONFIG_DBG(voe->config, 1, "VoE data:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
+ voe->request_state = EC_INT_REQUEST_SUCCESS;
+ voe->state = ec_voe_handler_state_end; // success
+}
+
+/****************************************************************************/
+
+/** Successful termination state function.
+ */
+void ec_voe_handler_state_end(ec_voe_handler_t *voe)
+{
+}
+
+/****************************************************************************/
+
+/** Failure termination state function.
+ */
+void ec_voe_handler_state_error(ec_voe_handler_t *voe)
+{
+}
+
+/****************************************************************************/
+
+/** \cond */
+
+EXPORT_SYMBOL(ecrt_voe_handler_send_header);
+EXPORT_SYMBOL(ecrt_voe_handler_received_header);
+EXPORT_SYMBOL(ecrt_voe_handler_data);
+EXPORT_SYMBOL(ecrt_voe_handler_data_size);
+EXPORT_SYMBOL(ecrt_voe_handler_read);
+EXPORT_SYMBOL(ecrt_voe_handler_write);
+EXPORT_SYMBOL(ecrt_voe_handler_execute);
+
+/** \endcond */
+
+/****************************************************************************/
diff --git a/ubuntu/igh-ecat/master/voe_handler.h b/ubuntu/igh-ecat/master/voe_handler.h
new file mode 100644
index 00000000000000..8e85372ccf051d
--- /dev/null
+++ b/ubuntu/igh-ecat/master/voe_handler.h
@@ -0,0 +1,65 @@
+/*****************************************************************************
+ *
+ * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ * This file is part of the IgH EtherCAT Master.
+ *
+ * The IgH EtherCAT Master is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * The IgH EtherCAT Master is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with the IgH EtherCAT Master; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ ****************************************************************************/
+
+/**
+ \file
+ Vendor specific over EtherCAT protocol handler.
+*/
+
+/****************************************************************************/
+
+#ifndef __EC_VOE_HANDLER_H__
+#define __EC_VOE_HANDLER_H__
+
+#include <linux/list.h>
+
+#include "globals.h"
+#include "datagram.h"
+
+/****************************************************************************/
+
+/** Vendor specific over EtherCAT handler.
+ */
+struct ec_voe_handler {
+ struct list_head list; /**< List item. */
+ ec_slave_config_t *config; /**< Parent slave configuration. */
+ ec_datagram_t datagram; /**< State machine datagram. */
+ uint32_t vendor_id; /**< Vendor ID for the header. */
+ uint16_t vendor_type; /**< Vendor type for the header. */
+ size_t data_size; /**< Size of VoE data. */
+ ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means writing to
+ the slave, EC_DIR_INPUT means reading from the
+ slave. */
+ void (*state)(ec_voe_handler_t *); /**< State function */
+ ec_internal_request_state_t request_state; /**< Handler state. */
+ unsigned int retries; /**< retries upon datagram timeout */
+ unsigned long jiffies_start; /**< Timestamp for timeout calculation. */
+};
+
+/****************************************************************************/
+
+int ec_voe_handler_init(ec_voe_handler_t *, ec_slave_config_t *, size_t);
+void ec_voe_handler_clear(ec_voe_handler_t *);
+size_t ec_voe_handler_mem_size(const ec_voe_handler_t *);
+
+/****************************************************************************/
+
+#endif
--
2.53.0
More information about the kernel-team
mailing list