Added devices/rtdmnet.c, heavily based on devices/generic.c, and with similar purpose. It uses Xenomai's RTNet/RTDM socket instead of Linux socket. stable-1.5
authorEdouard Tisserant <edouard.tisserant@gmail.com>
Wed, 12 Sep 2018 22:15:18 +0200
branchstable-1.5
changeset 2709 e0c2f17ebed1
parent 2708 742c70a97622
child 2710 05359dee3578
Added devices/rtdmnet.c, heavily based on devices/generic.c, and with similar purpose. It uses Xenomai's RTNet/RTDM socket instead of Linux socket.
configure.ac
devices/Kbuild.in
devices/Makefile.am
devices/rtdmnet.c
--- a/configure.ac	Wed Sep 12 21:22:46 2018 +0200
+++ b/configure.ac	Wed Sep 12 22:15:18 2018 +0200
@@ -209,6 +209,29 @@
 AC_SUBST(ENABLE_GENERIC,[$enablegeneric])
 
 #------------------------------------------------------------------------------
+# RTDM Ethernet driver
+#------------------------------------------------------------------------------
+
+AC_ARG_ENABLE([rtdmnet],
+    AS_HELP_STRING([--enable-rtdmnet],
+                   [Enable RTDM Ethernet driver]),
+    [
+        case "${enableval}" in
+            yes) enablertdmnet=1
+                ;;
+            no) enablertdmnet=0
+                ;;
+            *) AC_MSG_ERROR([Invalid value for --enable-rtdmnet])
+                ;;
+        esac
+    ],
+    [enablertdmnet=$enablekernel]
+)
+
+AM_CONDITIONAL(ENABLE_RTDMNET, test "x$enablertdmnet" = "x1")
+AC_SUBST(ENABLE_RTDMNET,[$enablertdmnet])
+
+#------------------------------------------------------------------------------
 # 8139too driver
 #------------------------------------------------------------------------------
 
--- a/devices/Kbuild.in	Wed Sep 12 21:22:46 2018 +0200
+++ b/devices/Kbuild.in	Wed Sep 12 22:15:18 2018 +0200
@@ -44,6 +44,13 @@
 	CFLAGS_$(EC_GENERIC_OBJ) = -DREV=$(REV)
 endif
 
+ifeq (@ENABLE_RTDMNET@,1)
+	EC_RTDMNET_OBJ := rtdmnet.o
+	obj-m += ec_rtdmnet.o
+	ec_rtdmnet-objs := $(EC_RTDMNET_OBJ)
+	CFLAGS_$(EC_RTDMNET_OBJ) = -DREV=$(REV) -Idrivers/xenomai/net/stack/include
+endif
+
 ifeq (@ENABLE_8139TOO@,1)
 	EC_8139TOO_OBJ := 8139too-@KERNEL_8139TOO@-ethercat.o
 	obj-m += ec_8139too.o
--- a/devices/Makefile.am	Wed Sep 12 21:22:46 2018 +0200
+++ b/devices/Makefile.am	Wed Sep 12 22:15:18 2018 +0200
@@ -137,6 +137,7 @@
 	e100-3.8-orig.c \
 	ecdev.h \
 	generic.c \
+	rtdmnet.c \
 	r8169-2.6.24-ethercat.c \
 	r8169-2.6.24-orig.c \
 	r8169-2.6.27-ethercat.c \
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/devices/rtdmnet.c	Wed Sep 12 22:15:18 2018 +0200
@@ -0,0 +1,509 @@
+/******************************************************************************
+ *
+ *  $Id$
+ *
+ *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
+ *  Copyright (C) 2014-2018  Edouard Tisserant
+ *
+ *  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
+ *
+ *  ---
+ *
+ *  The license mentioned above concerns the source code only. Using the
+ *  EtherCAT technology and brand is only permitted in compliance with the
+ *  industrial property and similar rights of Beckhoff Automation GmbH.
+ *
+ *****************************************************************************/
+
+/** \file
+ * EtherCAT generic Xenomai's RTDM RAW Ethernet socket device module.
+ * Heavily based on generic.c. Should be merged in a single file with #ifdefs
+ */
+
+/*****************************************************************************/
+
+#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 <rtdm/rtdm.h>
+
+// for rtnetif_carrier_ok
+// This needs -I@XENOMAI_DIR@/kernel/drivers/net/stack/include in Kbuild.in
+#include <rtnet_port.h>
+
+#include "../globals.h"
+#include "ecdev.h"
+
+#define PFX "ec_rtnet: "
+
+#define ETH_P_ETHERCAT 0x88A4
+
+#define EC_GEN_RX_BUF_SIZE 1600
+
+/*****************************************************************************/
+
+int __init ec_gen_init_module(void);
+void __exit ec_gen_cleanup_module(void);
+
+/*****************************************************************************/
+
+/** \cond */
+
+MODULE_AUTHOR("Edouard Tisserant <edouard.tisserant@gmail.com>");
+MODULE_DESCRIPTION("EtherCAT generic Xenomai's RTDM RAW Ethernet socket 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 rtnet_device *used_netdev;
+    int socket;
+    ec_device_t *ecdev;
+    uint8_t *rx_buf;
+    // struct sockaddr_ll dest_addr;
+} ec_gen_device_t;
+
+typedef struct {
+    struct list_head list;
+    struct rtnet_device *netdev;
+    char name[IFNAMSIZ];
+    int ifindex;
+    uint8_t dev_addr[ETH_ALEN];
+} 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 *);
+
+/*****************************************************************************/
+
+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);
+}
+
+/*****************************************************************************/
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
+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,
+};
+#endif
+
+/*****************************************************************************/
+
+/** 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 = -1;
+    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;
+    }
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
+    dev->netdev->netdev_ops = &ec_gen_netdev_ops;
+#else
+    dev->netdev->open = ec_gen_netdev_open;
+    dev->netdev->stop = ec_gen_netdev_stop;
+    dev->netdev->hard_start_xmit = ec_gen_netdev_start_xmit;
+#endif
+
+    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 < 0)) {
+        rtdm_close(dev->socket);
+        dev->socket = -1;
+    }
+    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;
+    }
+
+    /* create rt-socket */
+    dev->socket = rtdm_socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ETHERCAT));
+    if (dev->socket < 0) {
+        printk(" rtdm_socket() = %d!\n", dev->socket);
+        return dev->socket;
+    }
+
+    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 = rtdm_bind(dev->socket, (struct sockaddr *)&sa,
+                      sizeof(struct sockaddr_ll));
+    if (ret < 0) {
+        printk(" rtdm_bind() = %d!\n", ret);
+        rtdm_close(dev->socket);
+        dev->socket = -1;
+        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;
+    memcpy(dev->netdev->dev_addr, desc->dev_addr, ETH_ALEN);
+
+    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, rtnetif_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 user_msghdr msg;
+    struct iovec iov;
+    size_t len = skb->len;
+    int ret;
+
+    ecdev_set_link(dev->ecdev, rtnetif_carrier_ok(dev->used_netdev));
+    //ecdev_set_link(dev->ecdev, 1); // FIXME
+
+    iov.iov_base = skb->data;
+    iov.iov_len = len;
+    memset(&msg, 0, sizeof(msg));
+    // msg.msg_name    = &dev->dest_addr;
+    // msg.msg_namelen = sizeof(dev->dest_addr);
+    msg.msg_iov     = &iov;
+    msg.msg_iovlen  = 1;
+
+    ret = rtdm_sendmsg(dev->socket, &msg, 0);
+    // printk("sendmsg: %d\n", ret);
+    // if (ret != len && ret != -ENOSYS)
+    //     printk(" rtdm_sendmsg() = %d!\n", ret);
+
+    return ret == len ? NETDEV_TX_OK : NETDEV_TX_BUSY;
+}
+
+/*****************************************************************************/
+
+/** Polls the device.
+ */
+void ec_gen_device_poll(
+        ec_gen_device_t *dev
+        )
+{
+    struct user_msghdr msg;
+    struct iovec iov;
+    int ret, budget = 128; // FIXME
+
+    ecdev_set_link(dev->ecdev, rtnetif_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));
+        msg.msg_iov     = &iov;
+        msg.msg_iovlen  = 1;
+
+        ret = rtdm_recvmsg(dev->socket, &msg, MSG_DONTWAIT);
+        if (ret > 0) {
+            ecdev_receive(dev->ecdev, dev->rx_buf, ret);
+        } else if (ret < 0) {
+            break;
+        }
+        //printk("revmsg: %d\n", ret);
+        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;
+    int devices = 0;
+    int i;
+    struct list_head descs;
+    struct rtnet_device *rtdev;
+    ec_gen_interface_desc_t *desc, *next;
+
+    printk(KERN_INFO PFX "EtherCAT master RTnet Ethernet device module %s\n",
+            EC_MASTER_VERSION);
+
+    INIT_LIST_HEAD(&generic_devices);
+    INIT_LIST_HEAD(&descs);
+
+    for (i = 0; i < MAX_RT_DEVICES; i++) {
+
+        rtdev = rtdev_get_by_index(i);
+        if (rtdev != NULL) {
+            mutex_lock(&rtdev->nrt_lock);
+
+            //if (test_bit(PRIV_FLAG_UP, &rtdev->priv_flags)) {
+            //    mutex_unlock(&rtdev->nrt_lock);
+            //    printk(KERN_ERR PFX "%s busy, skipping device!\n", rtdev->name);
+            //    rtdev_dereference(rtdev);
+            //    continue;
+            //}
+
+            desc = kmalloc(sizeof(ec_gen_interface_desc_t), GFP_ATOMIC);
+            if (!desc) {
+                ret = -ENOMEM;
+                goto out_err;
+            }
+            strncpy(desc->name, rtdev->name, IFNAMSIZ);
+            desc->netdev = rtdev;
+            desc->ifindex = rtdev->ifindex;
+            memcpy(desc->dev_addr, rtdev->dev_addr, ETH_ALEN);
+            list_add_tail(&desc->list, &descs);
+              mutex_unlock(&rtdev->nrt_lock);
+
+            devices++;
+        }
+    }
+
+    if (devices == 0) {
+        printk(KERN_ERR PFX "no real-time devices found!\n");
+        ret = -ENODEV;
+        goto out_err;
+    }
+
+    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 */
+
+/*****************************************************************************/