--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtdm/module.c Wed Mar 16 22:12:23 2011 +0100
@@ -0,0 +1,719 @@
+/******************************************************************************
+ *
+ * $Id$
+ *
+ * ec_rtdm.c Copyright (C) 2009-2010 Moehwald GmbH B.Benner
+ * 2011 IgH Andreas Stewering-Bone
+ *
+ * This file is used for Prisma RT to interface to EtherCAT devices
+ *
+ * This file is part of ethercatrtdm interface to IgH EtherCAT master
+ *
+ * The Moehwald ec_rtdm interface 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/>.
+ *
+ * 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.
+ *
+ *****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/mman.h>
+#include <rtdm/rtdm_driver.h>
+#include <native/task.h>
+#include <native/sem.h>
+#include <native/mutex.h>
+#include <native/timer.h>
+
+#include "../include/ecrt.h"
+#include "../include/ec_rtdm.h"
+
+#define EC_RTDM_MAX_MASTERS 5 /**< Maximum number of masters. */
+
+#define EC_RTDM_GINFO(fmt, args...) \
+ printk(KERN_INFO "EtherCATrtdm: " fmt, ##args)
+
+#define EC_RTDM_GERR(fmt, args...) \
+ printk(KERN_ERR "EtherCATrtdm ERROR: " fmt, ##args)
+
+#define EC_RTDM_GWARN(fmt, args...) \
+ printk(KERN_WARNING "EtherCATrtdm WARNING: " fmt, ##args)
+
+
+#define EC_RTDM_INFO(devno, fmt, args...) \
+ printk(KERN_INFO "EtherCATrtdm %u: " fmt, devno, ##args)
+
+#define EC_RTDM_ERR(devno, fmt, args...) \
+ printk(KERN_ERR "EtherCATrtdm %u ERROR: " fmt, devno, ##args)
+
+#define EC_RTDM_WARN(devno, fmt, args...) \
+ printk(KERN_WARNING "EtherCATrtdm %u WARNING: " fmt, devno, ##args)
+
+
+
+
+typedef struct _EC_RTDM_DRV_STRUCT {
+ unsigned int isattached;
+ ec_master_t * master;
+ ec_domain_t * domain;
+ RT_MUTEX masterlock;
+ unsigned int sendcnt;
+ unsigned int reccnt;
+ unsigned int sendcntlv;
+ unsigned int reccntlv;
+ char mutexname[64];
+ unsigned int masterno;
+} EC_RTDM_DRV_STRUCT;
+
+
+static EC_RTDM_DRV_STRUCT ec_rtdm_masterintf[EC_RTDM_MAX_MASTERS];
+
+
+/* import from ethercat */
+ec_master_t *ecrt_attach_master(unsigned int master_index /**< Index of the master to request. */
+ );
+
+// driver context struct: used for storing various information
+typedef struct _EC_RTDM_DRV_CONTEXT {
+ int dev_id;
+ EC_RTDM_DRV_STRUCT* pdrvstruc;
+} EC_RTDM_DRV_CONTEXT;
+
+
+
+/**********************************************************/
+/* Utilities */
+/**********************************************************/
+
+static int _atoi(const char* text)
+{
+ char b;
+ int wd=-1;
+ int nfak=1;
+
+ wd=0;
+
+ while ((*text==' ') || (*text=='\t')) text++;
+ if (*text=='-')
+ {
+ nfak=-1;
+ text++;
+ }
+ if (*text=='+')
+ {
+ text++;
+ }
+ while (*text!=0)
+ {
+ b = *text;
+
+ if ( (b>='0') && (b<='9') )
+ {
+ b=b-'0';
+ wd=wd*10+b;
+ }
+ text++;
+ }
+ return (nfak*wd);
+}
+
+
+/**********************************************************/
+/* DRIVER sendcallback */
+/**********************************************************/
+void send_callback(void *cb_data)
+{
+ EC_RTDM_DRV_STRUCT * pdrvstruc;
+
+ pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+ ecrt_master_send_ext(pdrvstruc->master);
+ rt_mutex_release(&pdrvstruc->masterlock);
+ }
+}
+
+/*****************************************************************************/
+
+void receive_callback(void *cb_data)
+{
+ EC_RTDM_DRV_STRUCT * pdrvstruc;
+
+ pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+ ecrt_master_receive(pdrvstruc->master);
+ rt_mutex_release(&pdrvstruc->masterlock);
+ }
+}
+
+void receive_process(EC_RTDM_DRV_STRUCT * pdrvstruc)
+{
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+ ecrt_master_receive(pdrvstruc->master);
+ ecrt_domain_process(pdrvstruc->domain);
+ pdrvstruc->reccnt++;
+ rt_mutex_release(&pdrvstruc->masterlock);
+ }
+}
+
+void send_process(EC_RTDM_DRV_STRUCT * pdrvstruc)
+{
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+ ecrt_domain_queue(pdrvstruc->domain);
+ ecrt_master_send(pdrvstruc->master);
+ pdrvstruc->sendcnt++;
+ rt_mutex_release(&pdrvstruc->masterlock);
+ }
+}
+
+void detach_master(EC_RTDM_DRV_STRUCT * pdrvstruc)
+{
+
+ if (pdrvstruc->isattached)
+ {
+ EC_RTDM_INFO(pdrvstruc->masterno,"reseting callbacks!\n");
+ ecrt_master_callbacks(pdrvstruc->master,NULL,NULL,NULL);
+ EC_RTDM_INFO(pdrvstruc->masterno,"deleting mutex!\n");
+ rt_mutex_delete(&pdrvstruc->masterlock);
+ pdrvstruc->master = NULL;
+ pdrvstruc->isattached=0;
+ EC_RTDM_INFO(pdrvstruc->masterno,"master detach done!\n");
+ }
+}
+
+
+
+
+/**********************************************************/
+/* DRIVER OPEN */
+/**********************************************************/
+int ec_rtdm_open_rt(struct rtdm_dev_context *context,
+ rtdm_user_info_t *user_info,
+ int oflags)
+{
+ EC_RTDM_DRV_CONTEXT* my_context;
+ EC_RTDM_DRV_STRUCT * pdrvstruc;
+ const char * p;
+ int dev_no;
+ unsigned int namelen;
+
+ //int ret;
+ int dev_id;
+
+ // get the context for our driver - used to store driver info
+ my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
+
+ dev_no = -1;
+ namelen = strlen(context->device->driver_name);
+ p = &context->device->driver_name[namelen-1];
+ if (p!=&context->device->driver_name[0])
+ {
+ while ((*p>='0') && (*p<='9'))
+ {
+ p--;
+ if (p==&context->device->driver_name[0]) break;
+ }
+ dev_no=_atoi(p);
+ if ((dev_no!=-1) && (dev_no<EC_RTDM_MAX_MASTERS))
+ {
+ dev_id = context->device->device_id;
+ pdrvstruc = (EC_RTDM_DRV_STRUCT*)&ec_rtdm_masterintf[dev_no];
+
+ my_context->dev_id = dev_id;
+ my_context->pdrvstruc = pdrvstruc;
+
+ // enable interrupt in RTDM
+ return 0;
+ }
+ }
+ EC_RTDM_GERR("open - Cannot detect master device no\n");
+ return -EFAULT;
+}
+
+/**********************************************************/
+/* DRIVER CLOSE */
+/**********************************************************/
+int ec_rtdm_close_rt(struct rtdm_dev_context *context,
+ rtdm_user_info_t *user_info)
+{
+ EC_RTDM_DRV_CONTEXT* my_context;
+ EC_RTDM_DRV_STRUCT * pdrvstruc;
+
+ // get the context
+ my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
+
+ pdrvstruc = my_context->pdrvstruc;
+ EC_RTDM_INFO(pdrvstruc->masterno,"close called!\n");
+ detach_master(pdrvstruc);
+ return 0;
+
+}
+
+/**********************************************************/
+/* DRIVER IOCTL */
+/**********************************************************/
+int ec_rtdm_ioctl_rt(struct rtdm_dev_context *context,
+ rtdm_user_info_t *user_info,
+ int request,
+ void *arg)
+{
+ EC_RTDM_DRV_CONTEXT* my_context;
+ EC_RTDM_DRV_STRUCT * pdrvstruc;
+ int ret;
+ unsigned int l_ioctlvalue[]={0,0,0,0,0,0,0,0};
+ ec_domain_state_t ds;
+ ec_master_state_t ms;
+ uint64_t app_time;
+
+
+ ret = 0;
+
+ // get the context
+ my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
+ pdrvstruc = my_context->pdrvstruc;
+
+ switch (request) {
+ case EC_RTDM_MASTERSTATE:
+ {
+ if (!pdrvstruc->isattached)
+ {
+ return -EFAULT;
+ }
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+
+ ecrt_master_state(pdrvstruc->master, &ms);
+
+ rt_mutex_release(&pdrvstruc->masterlock);
+
+ }
+ if (rtdm_rw_user_ok(user_info, arg, sizeof(ms)))
+ {
+ // copy data to user
+ if (rtdm_copy_to_user(user_info, arg, &ms,sizeof(ms)))
+ {
+ return -EFAULT;
+ }
+ }
+
+ }
+ break;
+ case EC_RTDM_DOMAINSTATE:
+ {
+ if (!pdrvstruc->isattached)
+ {
+ return -EFAULT;
+ }
+ if (pdrvstruc->domain)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+
+ ecrt_domain_state(pdrvstruc->domain, &ds);
+
+ rt_mutex_release(&pdrvstruc->masterlock);
+ }
+ if (rtdm_rw_user_ok(user_info, arg, sizeof(ds)))
+ {
+ // copy data to user
+ if (rtdm_copy_to_user(user_info, arg, &ds,sizeof(ds)))
+ {
+ return -EFAULT;
+ }
+ }
+ }
+ break;
+
+ case EC_RTDM_MSTRRECEIVE:
+ {
+ if (pdrvstruc->isattached)
+ {
+ receive_process(pdrvstruc);
+ }
+ }
+ break;
+ case EC_RTDM_MSTRSEND:
+ {
+
+ if (pdrvstruc->isattached)
+ {
+ send_process(pdrvstruc);
+ }
+ }
+ break;
+ case EC_RTDM_MASTER_APP_TIME:
+ {
+ if (!pdrvstruc->isattached)
+ {
+ return -EFAULT;
+ }
+ if (rtdm_rw_user_ok(user_info, arg, sizeof(app_time)))
+ {
+ // copy data from user
+ if (rtdm_copy_from_user(user_info, &app_time, arg, sizeof(app_time)))
+ {
+ return -EFAULT;
+ }
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+
+ ecrt_master_application_time(pdrvstruc->master, app_time);
+
+ rt_mutex_release(&pdrvstruc->masterlock);
+
+ }
+ }
+ }
+ break;
+ case EC_RTDM_SYNC_REF_CLOCK:
+ {
+ if (!pdrvstruc->isattached)
+ {
+ return -EFAULT;
+ }
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+
+ ecrt_master_sync_reference_clock(pdrvstruc->master);
+
+ rt_mutex_release(&pdrvstruc->masterlock);
+
+ }
+ }
+ break;
+ case EC_RTDM_SYNC_SLAVE_CLOCK:
+ {
+ if (!pdrvstruc->isattached)
+ {
+ return -EFAULT;
+ }
+ if (pdrvstruc->master)
+ {
+ rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
+
+ ecrt_master_sync_slave_clocks(pdrvstruc->master);
+
+ rt_mutex_release(&pdrvstruc->masterlock);
+
+ }
+ }
+ break;
+ case EC_RTDM_MSTRATTACH:
+ {
+ unsigned int mstridx;
+
+ mstridx = 0;
+ ret = 0;
+
+ EC_RTDM_INFO(pdrvstruc->masterno,"Master attach start!\n");
+ if (user_info)
+ {
+ if (rtdm_read_user_ok(user_info, arg, sizeof(unsigned int)))
+ {
+ if (rtdm_copy_from_user(user_info, &l_ioctlvalue[0], arg,sizeof(unsigned int))==0)
+ {
+ pdrvstruc->domain = (ec_domain_t*)l_ioctlvalue[0];
+ }
+ else
+ {
+ EC_RTDM_ERR(pdrvstruc->masterno,"copy user param failed!\n");
+ ret=-EFAULT;
+ }
+ }
+ else
+ {
+ EC_RTDM_ERR(pdrvstruc->masterno,"user parameter domain missing!\n");
+ ret=-EFAULT;
+ }
+ }
+ if (ret!=0)
+ {
+ return ret;
+ }
+
+ if ( (pdrvstruc->master) && (pdrvstruc->isattached))
+ // master is allready attached
+ {
+ // master is allready attached
+ EC_RTDM_ERR(pdrvstruc->masterno,"Master is allready attached!\n");
+ ret = -EFAULT;
+ }
+ else
+ {
+ //mstr=ecrt_request_master(0);
+ mstridx = pdrvstruc->masterno;
+
+ pdrvstruc->master=ecrt_attach_master(mstridx);
+
+ if (pdrvstruc->master)
+ {
+ // Ok
+ EC_RTDM_INFO(pdrvstruc->masterno,"Master searching for domain!\n");
+ pdrvstruc->domain = ecrt_master_find_domain(pdrvstruc->master,l_ioctlvalue[0]);
+ if (!pdrvstruc->domain)
+ {
+ //
+ EC_RTDM_ERR(pdrvstruc->masterno,"Cannot find domain from index %u!\n",l_ioctlvalue[0]);
+ ret = -EFAULT;
+ }
+ else
+ {
+
+ // set device name
+ snprintf(&pdrvstruc->mutexname[0],sizeof(pdrvstruc->mutexname)-1,"ETHrtdmLOCK%d",pdrvstruc->masterno);
+ EC_RTDM_INFO(pdrvstruc->masterno,"Creating Master mutex %s!\n",&pdrvstruc->mutexname[0]);
+ rt_mutex_create(&pdrvstruc->masterlock,&pdrvstruc->mutexname[0]);
+ //ecrt_release_master(mstr);
+ ecrt_master_callbacks(pdrvstruc->master, send_callback, receive_callback, pdrvstruc);
+ EC_RTDM_INFO(pdrvstruc->masterno,"MSTR ATTACH done domain=%u!\n",(unsigned int)pdrvstruc->domain);
+ pdrvstruc->isattached=1;
+ ret = 0;
+ }
+
+ }
+ else
+ {
+ EC_RTDM_ERR(pdrvstruc->masterno,"Master attach failed!\n");
+ pdrvstruc->master = NULL;
+ ret = -EFAULT;
+ }
+ }
+ }
+ break;
+ default:
+ ret = -ENOTTY;
+ }
+ return ret;
+}
+
+
+/**********************************************************/
+/* DRIVER READ */
+/**********************************************************/
+int ec_rtdm_read_rt(struct rtdm_dev_context *context,
+ rtdm_user_info_t *user_info, void *buf, size_t nbyte)
+{
+ int ret;
+#if defined(USE_THIS)
+ EC_RTDM_DRV_CONTEXT* my_context;
+ char *out_pos;
+ int dev_id;
+ rtdm_toseq_t timeout_seq;
+ int ret;
+
+ out_pos = (char *)buf;
+
+ my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
+
+ // zero bytes requested ? return!
+ if (nbyte == 0)
+ return 0;
+
+ // check if R/W actions to user-space are allowed
+ if (user_info && !rtdm_rw_user_ok(user_info, buf, nbyte))
+ return -EFAULT;
+
+ dev_id = my_context->dev_id;
+
+ // in case we need to check if reading is allowed (locking)
+/* if (test_and_set_bit(0, &ctx->in_lock))
+ return -EBUSY;
+*/
+/* // if we need to do some stuff with preemption disabled:
+ rtdm_lock_get_irqsave(&ctx->lock, lock_ctx);
+ // stuff here
+ rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);
+*/
+
+ // wait: if ctx->timeout = 0, it will block infintely until
+ // rtdm_event_signal(&ctx->irq_event); is called from our
+ // interrupt routine
+ //ret = rtdm_event_timedwait(&ctx->irq_event, ctx->timeout, &timeout_seq);
+
+ // now write the requested stuff to user-space
+ if (rtdm_copy_to_user(user_info, out_pos,
+ dummy_buffer, BUFSIZE) != 0) {
+ ret = -EFAULT;
+ } else {
+ ret = BUFSIZE;
+ }
+#else
+ ret = -EFAULT;
+#endif
+ return ret;
+}
+
+/**********************************************************/
+/* DRIVER WRITE */
+/**********************************************************/
+int ec_rtdm_write_rt(struct rtdm_dev_context *context,
+ rtdm_user_info_t *user_info,
+ const void *buf, size_t nbyte)
+{
+ int ret;
+
+#if defined(USE_THIS)
+ int dev_id;
+ char *in_pos = (char *)buf;
+
+ EC_RTDM_DRV_CONTEXT* my_context;
+
+
+ my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
+
+
+ if (nbyte == 0)
+ return 0;
+ if (user_info && !rtdm_read_user_ok(user_info, buf, nbyte))
+ return -EFAULT;
+
+ dev_id = my_context->dev_id;
+
+ if (rtdm_copy_from_user(user_info, dummy_buffer,
+ in_pos, BUFSIZE) != 0) {
+ ret = -EFAULT;
+ } else {
+ ret = BUFSIZE;
+ }
+#else
+ ret = -EFAULT;
+#endif
+ // used when it is atomic
+// rtdm_mutex_unlock(&ctx->out_lock);
+ return ret;
+}
+
+/**********************************************************/
+/* DRIVER OPERATIONS */
+/**********************************************************/
+
+// Template
+
+static struct rtdm_device ec_rtdm_device_t = {
+ struct_version: RTDM_DEVICE_STRUCT_VER,
+
+ device_flags: RTDM_NAMED_DEVICE,
+ context_size: sizeof(EC_RTDM_DRV_CONTEXT),
+ device_name: EC_RTDM_DEV_FILE_NAME,
+
+/* open and close functions are not real-time safe due kmalloc
+ and kfree. If you do not use kmalloc and kfree, and you made
+ sure that there is no syscall in the open/close handler, you
+ can declare the open_rt and close_rt handler.
+*/
+ open_rt: NULL,
+ open_nrt: ec_rtdm_open_rt,
+
+ ops: {
+ close_rt: NULL,
+ close_nrt: ec_rtdm_close_rt,
+
+ ioctl_rt: ec_rtdm_ioctl_rt,
+ ioctl_nrt: ec_rtdm_ioctl_rt, // rtdm_mmap_to_user is not RT safe
+
+ read_rt: ec_rtdm_read_rt,
+ read_nrt: NULL,
+
+ write_rt: ec_rtdm_write_rt,
+ write_nrt: NULL,
+
+ recvmsg_rt: NULL,
+ recvmsg_nrt: NULL,
+
+ sendmsg_rt: NULL,
+ sendmsg_nrt: NULL,
+ },
+
+ device_class: RTDM_CLASS_EXPERIMENTAL,
+ device_sub_class: 222,
+ driver_name: EC_RTDM_DEV_FILE_NAME,
+ driver_version: RTDM_DRIVER_VER(1,0,1),
+ peripheral_name: EC_RTDM_DEV_FILE_NAME,
+ provider_name: "Moehwald GmbH - Bosch Group",
+// proc_name: ethcatrtdm_device.device_name,
+};
+
+
+static struct rtdm_device ec_rtdm_devices[EC_RTDM_MAX_MASTERS];
+
+
+/**********************************************************/
+/* INIT DRIVER */
+/**********************************************************/
+int init_module(void)
+{
+ unsigned int i;
+ int ret;
+
+ ret = 0;
+
+ EC_RTDM_GINFO("Initlializing EtherCAT RTDM Interface to Igh EtherCAT Master\n");
+ memset(&ec_rtdm_masterintf[0],0,sizeof(ec_rtdm_masterintf));
+ for (i=0;( (i<EC_RTDM_MAX_MASTERS) && (ret==0) ) ;i++)
+ {
+ // master no to struct
+ ec_rtdm_masterintf[i].masterno = i;
+ // copy from template
+ memcpy(&ec_rtdm_devices[i],&ec_rtdm_device_t,sizeof(ec_rtdm_devices[0]));
+
+ // set device name
+ snprintf(&ec_rtdm_devices[i].device_name[0],RTDM_MAX_DEVNAME_LEN,"%s%d",EC_RTDM_DEV_FILE_NAME,i);
+ // set proc_name
+ ec_rtdm_devices[i].proc_name = &ec_rtdm_devices[i].device_name[0];
+ ec_rtdm_devices[i].driver_name = &ec_rtdm_devices[i].device_name[0];
+ ec_rtdm_devices[i].peripheral_name = &ec_rtdm_devices[i].device_name[0];
+
+ EC_RTDM_GINFO("Registering device %s!\n",ec_rtdm_devices[i].driver_name);
+ ret = rtdm_dev_register(&ec_rtdm_devices[i]);
+
+ }
+ if (ret!=0)
+ {
+ // register m
+ EC_RTDM_GERR("Initialization of EtherCAT RTDM Interface failed\n");
+ }
+ return ret;
+}
+
+/**********************************************************/
+/* CLEANUP DRIVER */
+/**********************************************************/
+void cleanup_module(void)
+{
+ unsigned int i;
+
+ EC_RTDM_GINFO("Cleanup EtherCAT RTDM Interface to Igh EtherCAT Master - Moehwald GmbH\n");
+ for (i=0;i<EC_RTDM_MAX_MASTERS;i++)
+ {
+ if (ec_rtdm_masterintf[i].isattached)
+ {
+ detach_master(&ec_rtdm_masterintf[i]);
+ }
+ EC_RTDM_GINFO("Unregistering device %s!\n",ec_rtdm_devices[i].driver_name);
+ rtdm_dev_unregister(&ec_rtdm_devices[i],1000);
+ }
+}
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("EtherCAT RTDM Interface to Igh EtherCAT Master");