u03-master-in-userspace

esben · Mar 4, 2015 · cbab463 · cbab463
1 parent 01361fe
commit cbab463
Show file tree

Hide file tree

Showing 18 changed files with 712 additions and 6 deletions.
diff --git a/devices/generic.c b/devices/generic.c
@@ -37,6 +37,7 @@
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/version.h>
+#include <linux/netdevice.h>
 #include <linux/if_arp.h> /* ARPHRD_ETHER */
 #include <linux/etherdevice.h>
 
@@ -214,6 +215,10 @@ int ec_gen_device_create_socket(
         return ret;
     }
 
+#ifdef EC_MASTER_IN_USERSPACE
+    netif_up(dev->socket, desc->name);
+#endif
+
     printk(KERN_INFO PFX "Binding socket to interface %i (%s).\n",
             desc->ifindex, desc->name);
 

diff --git a/include/ecrt.h b/include/ecrt.h
@@ -437,6 +437,16 @@ typedef enum {
 extern "C" {
 #endif
 
+#ifdef EC_MASTER_IN_USERSPACE
+
+int ecrt_init(unsigned int master_count_, const char *const *master_macs,
+              unsigned int backup_count_, const char *const *backup_macs,
+              unsigned int debug_level_);
+
+void ecrt_done(void);
+
+#endif
+
 /** Returns the version magic of the realtime interface.
  *
  * \return Value of ECRT_VERSION_MAGIC() at EtherCAT master compile time.
@@ -507,7 +517,7 @@ int ecrt_master_reserve(
 
 #endif // #ifndef __KERNEL__
 
-#ifdef __KERNEL__
+#if defined(__KERNEL__) || defined(EC_MASTER_IN_USERSPACE)
 
 /** Sets the locking callbacks.
  *
@@ -1293,7 +1303,7 @@ int ecrt_domain_reg_pdo_entry_list(
                                                    registrations. */
         );
 
-#ifdef __KERNEL__
+#if defined(__KERNEL__) || defined(EC_MASTER_IN_USERSPACE)
 
 /** Returns the current size of the domain's process data.
  *
@@ -1630,7 +1640,7 @@ ec_request_state_t ecrt_voe_handler_execute(
  * Byte-swapping functions for user space
  *****************************************************************************/
 
-#ifndef __KERNEL__
+#if defined(EC_MASTER_IN_USERSPACE) || !defined(__KERNEL__)
 
 #if __BYTE_ORDER == __LITTLE_ENDIAN
 

diff --git a/lib/common.c b/lib/common.c
@@ -87,9 +87,16 @@ ec_master_t *ecrt_open_master(unsigned int master_index)
     master->first_domain = NULL;
     master->first_config = NULL;
 
+#ifdef EC_MASTER_IN_USERSPACE
+    snprintf(path, MAX_PATH_LEN - 1, "TCP socket %i",
+      ECRT_PORT_BASE + master_index);
+    master->fd = ioctl_client_open(master_index, NULL);
+#else
     snprintf(path, MAX_PATH_LEN - 1, "/dev/EtherCAT%u", master_index);
 
     master->fd = open(path, O_RDWR);
+#endif
+
     if (master->fd == -1) {
         fprintf(stderr, "Failed to open %s: %s\n", path, strerror(errno));
         goto out_clear;

diff --git a/lib/master.c b/lib/master.c
@@ -482,8 +482,13 @@ int ecrt_master_activate(ec_master_t *master)
     }
 
     if (master->process_data_size) {
+#ifdef EC_MASTER_IN_USERSPACE
+        master->process_data = MAP_FAILED;  // not yet supported
+        errno = EINVAL;
+#else
         master->process_data = mmap(0, master->process_data_size,
                 PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0);
+#endif
         if (master->process_data == MAP_FAILED) {
             fprintf(stderr, "Failed to map process data: %s\n", strerror(errno));
             master->process_data = NULL;

diff --git a/master/cdev.c b/master/cdev.c
@@ -60,6 +60,9 @@
 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);
+
+#ifndef EC_MASTER_IN_USERSPACE
+
 static int eccdev_mmap(struct file *, struct vm_area_struct *);
 
 /** This is the kernel version from which the .fault member of the
@@ -96,6 +99,8 @@ struct vm_operations_struct eccdev_vm_ops = {
 #endif
 };
 
+#endif
+
 /*****************************************************************************/
 
 /** Private data structure for file handles.
@@ -109,6 +114,138 @@ typedef struct {
 
 /*****************************************************************************/
 
+#ifdef EC_MASTER_IN_USERSPACE
+
+static int ec_cdev_thread(void *priv_data)
+{
+    ec_master_t *master = (ec_master_t *) priv_data;
+    EC_MASTER_DBG(master, 1, "cdev thread running\n");
+    ec_cdev_t cdev;
+    cdev.master = master;
+    cdev.cdev.owner = NULL;
+    struct inode inode;
+    inode.data = &cdev;
+    struct connection {
+        struct list_head list;
+        int socket;
+        struct file f;
+    };
+    struct list_head connections;
+    INIT_LIST_HEAD(&connections);
+    while (!kthread_should_stop()) {
+        fd_set r;
+        FD_ZERO(&r);
+        int m = 0;
+        struct socket_list *s;
+        list_for_each_entry(s, &master->cdev_sockets, list) {
+            m = max(m, s->socket);
+            FD_SET(s->socket, &r);
+        }
+        struct connection *c, *next;
+        list_for_each_entry(c, &connections, list) {
+            m = max(m, c->socket);
+            FD_SET(c->socket, &r);
+        }
+        if (select(m + 1, &r, NULL, NULL, NULL) <= 0)
+            continue;
+        list_for_each_entry_safe(c, next, &connections, list) {
+            master->cdev_filp = &c->f;
+            if (FD_ISSET(c->socket, &r)
+                  && ioctl_server(c->socket, eccdev_ioctl, &c->f) < 0) {
+                close(c->socket);
+                eccdev_release(&inode, &c->f);
+                list_del(&c->list);
+            }
+        }
+        list_for_each_entry(s, &master->cdev_sockets, list) {
+            int n;
+            if (FD_ISSET(s->socket, &r)
+                && (n = accept(s->socket, NULL, NULL)) > 0) {
+                c = kmalloc(sizeof(struct connection), GFP_KERNEL);
+                if (!c)
+                    close(n);
+                else {
+                    c->socket = n;
+                    c->f.f_mode = FMODE_WRITE;
+                    c->f.data_from_user = NULL;
+                    c->f.data_to_user = NULL;
+                    if (eccdev_open(&inode, &c->f) < 0) {
+                        kfree(c);
+                        close(n);
+                    } else {
+                        list_add_tail(&c->list, &connections);
+                    }
+                }
+            }
+        }
+    }
+    struct connection *c;
+    list_for_each_entry(c, &connections, list) {
+        close(c->socket);
+        free (c->f.data_from_user);
+        free (c->f.data_to_user);
+        eccdev_release(&inode, &c->f);
+    }
+    EC_MASTER_DBG(master, 1, "cdev thread exiting...\n");
+    return 0;
+}
+
+static void ec_close_cdev_sockets(ec_master_t *master)
+{
+    struct socket_list *s, *next;
+    list_for_each_entry_safe(s, next, &master->cdev_sockets, list) {
+        close(s->socket);
+        list_del(&s->list);
+    }
+}
+
+int ec_cdev_thread_start(ec_master_t *master)
+{
+    EC_MASTER_INFO(master, "Starting cdev thread.\n");
+    if (!list_empty(&master->cdev_sockets) || master->cdev_thread) {
+        EC_MASTER_WARN(master, "cdev thread already started!\n");
+        return -EBUSY;
+    }
+    ioctl_server_open(master->index, &master->cdev_sockets);
+    if (list_empty(&master->cdev_sockets)) {
+        EC_MASTER_ERR(master, "Cannot open cdev socket (%s)!\n",
+                      strerror(errno));
+        return -errno;
+    }
+    master->cdev_thread = kthread_run(ec_cdev_thread, master,
+      "EtherCAT-cdev %i", master->index);
+    if (IS_ERR(master->cdev_thread)) {
+        int err = (int) PTR_ERR(master->cdev_thread);
+        EC_MASTER_ERR(master, "Cannot start cdev thread (%s)!\n",
+                      strerror(err));
+        master->cdev_thread = NULL;
+        ec_close_cdev_sockets(master);
+        return err;
+    }
+    return 0;
+}
+
+void ec_cdev_thread_stop(ec_master_t *master)
+{
+    if (!master->cdev_thread)
+        return;
+    EC_MASTER_DBG(master, 1, "Stopping cdev thread.\n");
+    kthread_stop(master->cdev_thread);
+    master->cdev_thread = NULL;
+    ec_close_cdev_sockets(master);
+    EC_MASTER_INFO(master, "cdev thread exited.\n");
+}
+
+#define copy_from_user(DST, SRC, SIZE) \
+  (memcpy((DST), (SRC) == (void *)arg ? (SRC) : master->cdev_filp->data_from_user, (SIZE)), 0)
+
+#define copy_to_user(DST, SRC, SIZE) \
+  (((DST) == (void *)arg ? memcpy((DST), (SRC), (SIZE)) : memcpy(master->cdev_filp->data_to_user, (SRC), (master->cdev_filp->data_to_user_size = (SIZE)))), 0)
+
+#define __copy_to_user copy_to_user
+
+#endif
+
 /** Constructor.
  * 
  * \return 0 in case of success, else < 0
@@ -166,6 +303,7 @@ void ec_cdev_strcpy(
 /** Get module information.
  */
 int ec_cdev_ioctl_module(
+        ec_master_t *master, /**< EtherCAT master. */
         unsigned long arg /**< Userspace address to store the results. */
         )
 {
@@ -3381,7 +3519,11 @@ int ec_cdev_ioctl_slave_soe_write(
  */
 int eccdev_open(struct inode *inode, struct file *filp)
 {
+#ifdef EC_MASTER_IN_USERSPACE
+    ec_cdev_t *cdev = inode->data;
+#else
     ec_cdev_t *cdev = container_of(inode->i_cdev, ec_cdev_t, cdev);
+#endif
     ec_cdev_priv_t *priv;
 
     priv = kmalloc(sizeof(ec_cdev_priv_t), GFP_KERNEL);
@@ -3448,7 +3590,7 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 
     switch (cmd) {
         case EC_IOCTL_MODULE:
-            ret = ec_cdev_ioctl_module(arg);
+            ret = ec_cdev_ioctl_module(master, arg);
             break;
         case EC_IOCTL_MASTER:
             ret = ec_cdev_ioctl_master(master, arg);
@@ -3864,6 +4006,8 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
     return ret;
 }
 
+#ifndef EC_MASTER_IN_USERSPACE
+
 /*****************************************************************************/
 
 /** Memory-map callback for the EtherCAT character device.
@@ -3959,4 +4103,6 @@ struct page *eccdev_vma_nopage(
 
 #endif
 
+#endif
+
 /*****************************************************************************/
diff --git a/master/cdev.h b/master/cdev.h
@@ -56,6 +56,11 @@ typedef struct {
 int ec_cdev_init(ec_cdev_t *, ec_master_t *, dev_t);
 void ec_cdev_clear(ec_cdev_t *);
 
+#ifdef EC_MASTER_IN_USERSPACE
+int ec_cdev_thread_start(ec_master_t *);
+void ec_cdev_thread_stop(ec_master_t *);
+#endif
+
 /*****************************************************************************/
 
 #endif