ab@2054: /******************************************************************************
ab@2054:  *
ab@2054:  *  $Id$
ab@2054:  *
ab@2054:  *  ec_rtdm.c	Copyright (C) 2009-2010  Moehwald GmbH B.Benner
ab@2054:  *                            2011       IgH Andreas Stewering-Bone
ab@2054:  *								  
ab@2054:  *								  
ab@2055:  *  This file is part of the IgH EtherCAT master 
ab@2054:  *  
ab@2055:  *  The IgH EtherCAT master is free software; you can
ab@2054:  *  redistribute it and/or modify it under the terms of the GNU Lesser General
ab@2054:  *  Public License as published by the Free Software Foundation; version 2.1
ab@2054:  *  of the License.
ab@2054:  *
ab@2054:  *  The IgH EtherCAT master userspace library is distributed in the hope that
ab@2054:  *  it will be useful, but WITHOUT ANY WARRANTY; without even the implied
ab@2054:  *  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
ab@2054:  *  GNU Lesser General Public License for more details.
ab@2054:  *
ab@2054:  *  You should have received a copy of the GNU Lesser General Public License
ab@2054:  *  along with the IgH EtherCAT master userspace library. If not, see
ab@2054:  *  <http://www.gnu.org/licenses/>.
ab@2054:  *  
ab@2054:  *  The license mentioned above concerns the source code only. Using the
ab@2054:  *  EtherCAT technology and brand is only permitted in compliance with the
ab@2054:  *  industrial property and similar rights of Beckhoff Automation GmbH.
ab@2054:  *
ab@2054:  *****************************************************************************/
ab@2054: 
ab@2054: #include <linux/module.h>
ab@2054: #include <linux/mman.h>
ab@2058: 
ab@2058: 
ab@2058: #ifdef ENABLE_XENOMAI
ab@2054: #include <native/task.h>
ab@2054: #include <native/sem.h>
ab@2054: #include <native/mutex.h>
ab@2054: #include <native/timer.h>
ab@2058: #endif
ab@2058: 
ab@2058: #ifdef ENABLE_RTAI
ab@2058: #include <rtai_sched.h>
ab@2058: #include <rtai_sem.h>
ab@2058: #endif
ab@2058: 
ab@2058: 
ab@2058: #include <rtdm/rtdm_driver.h>
ab@2054: 
ab@2054: #include "../include/ecrt.h"
ab@2054: #include "../include/ec_rtdm.h"
ab@2054: 
ab@2058: #ifdef ENABLE_XENOMAI
ab@2058: #define my_mutex_create(X,Y)  rt_mutex_create(X, Y)
ab@2058: #define my_mutex_acquire(X,Y) rt_mutex_acquire(X,Y)
ab@2058: #define my_mutex_release(X)   rt_mutex_release(X)
ab@2058: #define my_mutex_delete(X)    rt_mutex_delete(X)
ab@2058: #endif
ab@2058: 
ab@2058: #ifdef ENABLE_RTAI
ab@2058: #define my_mutex_create(X,Y)  rt_sem_init(X, 1)
ab@2058: #define my_mutex_acquire(X,Y) rt_sem_wait(X)
ab@2058: #define my_mutex_release(X)   rt_sem_signal(X)
ab@2058: #define my_mutex_delete(X)    rt_sem_delete(X)
ab@2058: #define TM_INFINITE
ab@2058: #endif
ab@2058: 
ab@2058: 
ab@2058: 
ab@2058: 
ab@2054: #define EC_RTDM_MAX_MASTERS 5 /**< Maximum number of masters. */
ab@2054: 
ab@2054: #define EC_RTDM_GINFO(fmt, args...) \
ab@2055:     rtdm_printk(KERN_INFO "EtherCATrtdm: " fmt,  ##args)
ab@2054: 
ab@2054: #define EC_RTDM_GERR(fmt, args...) \
ab@2055:     rtdm_printk(KERN_ERR "EtherCATrtdm ERROR: " fmt, ##args)
ab@2054: 
ab@2054: #define EC_RTDM_GWARN(fmt, args...) \
ab@2055:     rtdm_printk(KERN_WARNING "EtherCATrtdm WARNING: " fmt, ##args)
ab@2054: 
ab@2054: 
ab@2054: #define EC_RTDM_INFO(devno, fmt, args...) \
ab@2055:     rtdm_printk(KERN_INFO "EtherCATrtdm %u: " fmt, devno, ##args)
ab@2054: 
ab@2054: #define EC_RTDM_ERR(devno, fmt, args...) \
ab@2055:     rtdm_printk(KERN_ERR "EtherCATrtdm %u ERROR: " fmt, devno, ##args)
ab@2054: 
ab@2054: #define EC_RTDM_WARN(devno, fmt, args...) \
ab@2055:     rtdm_printk(KERN_WARNING "EtherCATrtdm %u WARNING: " fmt, devno, ##args)
ab@2054: 
ab@2054: 
ab@2054: 
ab@2054: 
ab@2054: typedef struct _EC_RTDM_DRV_STRUCT {
ab@2054:     unsigned int	    isattached;
ab@2054:     ec_master_t *	    master;
ab@2058:     ec_domain_t *	    domain;	
ab@2058: #ifdef ENABLE_XENOMAI			   
ab@2058:     RT_MUTEX           masterlock;
ab@2058: #endif
ab@2058: #ifdef ENABLE_RTAI
ab@2058:     SEM                masterlock;
ab@2058: #endif
ab@2054:     unsigned int	    sendcnt;
ab@2054:     unsigned int	    reccnt;
ab@2054:     unsigned int	    sendcntlv;
ab@2054:     unsigned int	    reccntlv;
ab@2054:     char                    mutexname[64];
ab@2054:     unsigned int            masterno;
ab@2054: } EC_RTDM_DRV_STRUCT;
ab@2054: 
ab@2054: 
ab@2054: static EC_RTDM_DRV_STRUCT ec_rtdm_masterintf[EC_RTDM_MAX_MASTERS];
ab@2054: 
ab@2054: 
ab@2054: /* import from ethercat */
ab@2054: ec_master_t *ecrt_attach_master(unsigned int master_index /**< Index of the master to request. */
ab@2054:         );
ab@2054: 
ab@2054: // driver context struct: used for storing various information
ab@2054: typedef struct _EC_RTDM_DRV_CONTEXT {
ab@2054:     int                       dev_id;
ab@2054:     EC_RTDM_DRV_STRUCT*  pdrvstruc;
ab@2054: } EC_RTDM_DRV_CONTEXT;
ab@2054: 
ab@2054: 
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            Utilities                                   */
ab@2054: /**********************************************************/
ab@2054: 
ab@2054: static int _atoi(const char* text)
ab@2054: {
ab@2054:   char  b;
ab@2054:   int wd=-1;
ab@2054:   int nfak=1;
ab@2054: 
ab@2054:   wd=0;
ab@2054: 
ab@2054:   while ((*text==' ') || (*text=='\t')) text++;
ab@2054:   if (*text=='-') 
ab@2054:     {
ab@2054:       nfak=-1;
ab@2054: 	  text++;
ab@2054:     }
ab@2054:   if (*text=='+') 
ab@2054:     {
ab@2054:       text++;
ab@2054:     }
ab@2054:   while (*text!=0)
ab@2054:     {
ab@2054:  	  b = *text;
ab@2054: 
ab@2054: 	  if ( (b>='0') && (b<='9') )
ab@2054: 		{
ab@2054: 		   b=b-'0';
ab@2054: 		   wd=wd*10+b;
ab@2054: 		}
ab@2054: 	  text++;
ab@2054: 	}
ab@2054:   return (nfak*wd);
ab@2054: }
ab@2054: 
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            DRIVER sendcallback                         */
ab@2054: /**********************************************************/
ab@2054: void send_callback(void *cb_data)
ab@2054: {
ab@2054:     EC_RTDM_DRV_STRUCT * pdrvstruc;
ab@2054:     
ab@2054:     pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;
ab@2054:     if (pdrvstruc->master)
ab@2054:         {
ab@2058:             my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2054:             ecrt_master_send_ext(pdrvstruc->master);
ab@2058:             my_mutex_release(&pdrvstruc->masterlock);
ab@2054:       }
ab@2054: }
ab@2054: 
ab@2054: /*****************************************************************************/
ab@2054: 
ab@2054: void receive_callback(void *cb_data)
ab@2054: {
ab@2054:     EC_RTDM_DRV_STRUCT * pdrvstruc;
ab@2054: 
ab@2054:     pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;
ab@2054:     if (pdrvstruc->master)
ab@2054:       {
ab@2058:           my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2054:           ecrt_master_receive(pdrvstruc->master);
ab@2058:           my_mutex_release(&pdrvstruc->masterlock);      
ab@2054:     }
ab@2054: }
ab@2054: 
ab@2055: 
ab@2055: 
ab@2054: 
ab@2054: void detach_master(EC_RTDM_DRV_STRUCT * pdrvstruc)
ab@2054: {
ab@2054: 
ab@2054:   if (pdrvstruc->isattached)
ab@2054:       {
ab@2054:           EC_RTDM_INFO(pdrvstruc->masterno,"reseting callbacks!\n");
ab@2054:           ecrt_master_callbacks(pdrvstruc->master,NULL,NULL,NULL);
ab@2054:           EC_RTDM_INFO(pdrvstruc->masterno,"deleting mutex!\n");
ab@2058:           my_mutex_delete(&pdrvstruc->masterlock);
ab@2054:           pdrvstruc->master = NULL;
ab@2054:           pdrvstruc->isattached=0;
ab@2054:           EC_RTDM_INFO(pdrvstruc->masterno,"master detach done!\n");
ab@2054:       }
ab@2054: }
ab@2054: 
ab@2054: 
ab@2054: 
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            DRIVER OPEN                                 */
ab@2054: /**********************************************************/
ab@2054: int ec_rtdm_open_rt(struct rtdm_dev_context    *context,
ab@2054:                  rtdm_user_info_t           *user_info,
ab@2054:                  int                        oflags)
ab@2054: {
ab@2054:     EC_RTDM_DRV_CONTEXT* my_context;
ab@2054:     EC_RTDM_DRV_STRUCT * pdrvstruc;
ab@2054:     const char * p;
ab@2054:     int dev_no;
ab@2054:     unsigned int namelen;
ab@2054: 
ab@2054:     //int ret;
ab@2054:     int dev_id;
ab@2054: 
ab@2054:     // get the context for our driver - used to store driver info
ab@2054:     my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
ab@2054: 
ab@2054:     dev_no = -1;
ab@2054:     namelen   = strlen(context->device->driver_name);
ab@2054:     p = &context->device->driver_name[namelen-1];
ab@2054:     if (p!=&context->device->driver_name[0])
ab@2054:       {
ab@2054: 	  while ((*p>='0') && (*p<='9')) 
ab@2054: 	    {
ab@2054: 	       p--;
ab@2054: 	       if (p==&context->device->driver_name[0]) break;
ab@2054:    	    }
ab@2054: 	  dev_no=_atoi(p);
ab@2054: 	  if  ((dev_no!=-1) && (dev_no<EC_RTDM_MAX_MASTERS))
ab@2054: 	    { 
ab@2054: 		dev_id    = context->device->device_id;
ab@2054:     		pdrvstruc = (EC_RTDM_DRV_STRUCT*)&ec_rtdm_masterintf[dev_no];
ab@2054: 
ab@2054:     		my_context->dev_id         = dev_id;
ab@2054:     		my_context->pdrvstruc      = pdrvstruc;
ab@2054: 		 
ab@2054:     		// enable interrupt in RTDM
ab@2054:     		return 0;	
ab@2054: 	    }	
ab@2054:       }
ab@2054:    EC_RTDM_GERR("open - Cannot detect master device no\n");
ab@2054:    return -EFAULT;
ab@2054: }
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            DRIVER CLOSE                                */
ab@2054: /**********************************************************/
ab@2054: int ec_rtdm_close_rt(struct rtdm_dev_context   *context,
ab@2054:                   rtdm_user_info_t          *user_info)
ab@2054: {
ab@2054:     EC_RTDM_DRV_CONTEXT* my_context;
ab@2054:     EC_RTDM_DRV_STRUCT * pdrvstruc;
ab@2054: 
ab@2054:     // get the context
ab@2054:     my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
ab@2054: 
ab@2054:     pdrvstruc =  my_context->pdrvstruc;
ab@2054:     EC_RTDM_INFO(pdrvstruc->masterno,"close called!\n");
ab@2054:     detach_master(pdrvstruc);
ab@2054:     return 0;
ab@2054: 	
ab@2054: }
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            DRIVER IOCTL                                */
ab@2054: /**********************************************************/
ab@2054: int ec_rtdm_ioctl_rt(struct rtdm_dev_context   *context,
ab@2054:                   rtdm_user_info_t          *user_info,
ab@2054:                   int                       request,
ab@2054:                   void                      *arg)
ab@2054: {
ab@2054:     EC_RTDM_DRV_CONTEXT* my_context;
ab@2054:     EC_RTDM_DRV_STRUCT * pdrvstruc;
ab@2054:     int ret;
ab@2054:     unsigned int l_ioctlvalue[]={0,0,0,0,0,0,0,0};
ab@2054:     ec_domain_state_t ds;
ab@2054:     ec_master_state_t ms;
ab@2054:     uint64_t app_time;
ab@2054: 
ab@2054: 
ab@2054:     ret = 0;
ab@2054: 
ab@2054:     // get the context
ab@2054:     my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
ab@2054:     pdrvstruc =  my_context->pdrvstruc;
ab@2054:     
ab@2054:     switch (request) {
ab@2054:     case EC_RTDM_MASTERSTATE:
ab@2054:     {
ab@2054: 		if (!pdrvstruc->isattached)
ab@2054:             {
ab@2054:                 return -EFAULT;
ab@2054:             }
ab@2054: 		if (pdrvstruc->master)
ab@2054:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2054:                 
ab@2054:                 ecrt_master_state(pdrvstruc->master, &ms);
ab@2054:                 
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2054:                 
ab@2054:             }
ab@2054:         if  (rtdm_rw_user_ok(user_info, arg, sizeof(ms)))
ab@2054:             {
ab@2054:                 // copy data to user
ab@2054:                 if (rtdm_copy_to_user(user_info, arg, &ms,sizeof(ms)))
ab@2054:                     {
ab@2054:                         return -EFAULT;
ab@2054:                     }
ab@2054:             }
ab@2054:         
ab@2054:     }
ab@2054:     break;
ab@2054:     case EC_RTDM_DOMAINSTATE:
ab@2054:     {
ab@2054: 		if (!pdrvstruc->isattached)
ab@2054:             {
ab@2054:                 return -EFAULT;
ab@2054:             }
ab@2054: 	   	if (pdrvstruc->domain)
ab@2054:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2054:                 
ab@2054:                 ecrt_domain_state(pdrvstruc->domain, &ds);
ab@2054:                 
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2054:             }
ab@2054:         if  (rtdm_rw_user_ok(user_info, arg, sizeof(ds)))
ab@2054:             {
ab@2054:                 // copy data to user
ab@2054:                 if (rtdm_copy_to_user(user_info, arg, &ds,sizeof(ds)))
ab@2054:                     {
ab@2054:                         return -EFAULT;
ab@2054:                     }
ab@2054:             }
ab@2054:     }
ab@2054:     break;
ab@2055:     case EC_RTDM_MASTER_RECEIVE:
ab@2054:     {	       
ab@2054:         if (pdrvstruc->isattached)
ab@2054:             {
ab@2054:                 if (pdrvstruc->master)
ab@2054:                     {
ab@2058:                         my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2055:                         ecrt_master_receive(pdrvstruc->master);
ab@2055:                         pdrvstruc->reccnt++;
ab@2058:                         my_mutex_release(&pdrvstruc->masterlock);
ab@2054:                     }
ab@2054:             }
ab@2054:     }
ab@2054:     break;
ab@2055:     case EC_RTDM_DOMAIN_PROCESS:
ab@2055:     {	       
ab@2055:         if (pdrvstruc->isattached)
ab@2055:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2055:                 ecrt_domain_process(pdrvstruc->domain);
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2055:             }
ab@2055:     }
ab@2055:     break;
ab@2055:     case EC_RTDM_MASTER_SEND:
ab@2055:     {
ab@2055:         
ab@2055:         if (pdrvstruc->isattached)
ab@2055:             {
ab@2055:                 if (pdrvstruc->master)
ab@2055:                     {
ab@2058:                         my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2055:                         ecrt_master_send(pdrvstruc->master);
ab@2055:                         pdrvstruc->sendcnt++;
ab@2058:                         my_mutex_release(&pdrvstruc->masterlock);
ab@2055:                     }
ab@2055:             }
ab@2055:     }
ab@2055:     break;
ab@2055:     case EC_RTDM_DOMAIN_QUEQUE:
ab@2055:     {	       
ab@2055:         if (pdrvstruc->isattached)
ab@2055:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2055:                 ecrt_domain_queue(pdrvstruc->domain);
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2055:             }
ab@2055:     }
ab@2055:     break;
ab@2055: 
ab@2055:     case EC_RTDM_MASTER_APP_TIME:
ab@2055:     {
ab@2055: 		if (!pdrvstruc->isattached)
ab@2055:             {
ab@2055:                 rtdm_printk("ERROR : No Master attached\n");
ab@2055:                 return -EFAULT;
ab@2055:             }
ab@2055:         if (rtdm_safe_copy_from_user(user_info, &app_time, arg, sizeof(app_time)))
ab@2055:             {
ab@2055:                 rtdm_printk("ERROR : can't copy data to driver\n");
ab@2055:                 return -EFAULT;
ab@2055:             }
ab@2055:             
ab@2055:         if (pdrvstruc->master)
ab@2055:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2055:                 
ab@2055:                 ecrt_master_application_time(pdrvstruc->master, app_time);
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2055:                 
ab@2055:             }
ab@2055:     }
ab@2055:     break;
ab@2054:     case EC_RTDM_SYNC_REF_CLOCK:
ab@2054:     {
ab@2054: 		if (!pdrvstruc->isattached)
ab@2054:             {
ab@2054:                 return -EFAULT;
ab@2054:             }
ab@2054:         if (pdrvstruc->master)
ab@2054:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2054:                 
ab@2054:                 ecrt_master_sync_reference_clock(pdrvstruc->master);
ab@2054:                 
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2054:                 
ab@2054:             }
ab@2054:     }
ab@2054:     break;
ab@2054:     case EC_RTDM_SYNC_SLAVE_CLOCK:
ab@2054:     {
ab@2054: 		if (!pdrvstruc->isattached)
ab@2054:             {
ab@2054:                 return -EFAULT;
ab@2054:             }
ab@2054:         if (pdrvstruc->master)
ab@2054:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2054:                 
ab@2054:                 ecrt_master_sync_slave_clocks(pdrvstruc->master);
ab@2054:                 
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2054:                 
ab@2054:             }
ab@2054:     }
ab@2054:     break;
ab@2055:     case EC_RTDM_MASTER_SYNC_MONITOR_QUEQUE:
ab@2055:     {
ab@2055: 		if (!pdrvstruc->isattached)
ab@2055:             {
ab@2055:                 return -EFAULT;
ab@2055:             }
ab@2055:         if (pdrvstruc->master)
ab@2055:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2055:                 ecrt_master_sync_monitor_queue(pdrvstruc->master);
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2055:             }
ab@2055:     }
ab@2055:     break;
ab@2055:     case EC_RTDM_MASTER_SYNC_MONITOR_PROCESS:
ab@2055:     {
ab@2055:         uint32_t ret;
ab@2055: 		if (!pdrvstruc->isattached)
ab@2055:             {
ab@2055:                 return -EFAULT;
ab@2055:             }
ab@2055:         if (pdrvstruc->master)
ab@2055:             {
ab@2058:                 my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);
ab@2055:                 ret = ecrt_master_sync_monitor_process(pdrvstruc->master);
ab@2058:                 my_mutex_release(&pdrvstruc->masterlock);
ab@2055:                 if (rtdm_safe_copy_to_user(user_info, arg, &ret, sizeof(ret)))
ab@2055:                     {
ab@2055:                         EC_RTDM_ERR(pdrvstruc->masterno,"copy to user param failed!\n");
ab@2055:                         ret=-EFAULT;
ab@2055:                     }
ab@2055:             }
ab@2055:     }
ab@2055:     break;
ab@2054:     case EC_RTDM_MSTRATTACH:
ab@2054:     {
ab@2054:         unsigned int mstridx;
ab@2054:         
ab@2054:         mstridx = 0;
ab@2054:         ret = 0;
ab@2054:         
ab@2054:         EC_RTDM_INFO(pdrvstruc->masterno,"Master attach start!\n");
ab@2054:         if (user_info) 
ab@2054:             {
ab@2054:                 if (rtdm_read_user_ok(user_info, arg, sizeof(unsigned int)))
ab@2054:                     {
ab@2054:                         if (rtdm_copy_from_user(user_info, &l_ioctlvalue[0], arg,sizeof(unsigned int))==0)
ab@2054:                             {
ab@2054:                                 pdrvstruc->domain = (ec_domain_t*)l_ioctlvalue[0];
ab@2054:                             }
ab@2054:                         else
ab@2054:                             {
ab@2054:                                 EC_RTDM_ERR(pdrvstruc->masterno,"copy user param failed!\n");
ab@2054:                                 ret=-EFAULT;
ab@2054:                             }		
ab@2054:                     }
ab@2054:                 else
ab@2054:                     { 
ab@2054:                         EC_RTDM_ERR(pdrvstruc->masterno,"user parameter domain missing!\n");
ab@2054:                         ret=-EFAULT;
ab@2054:                     }	
ab@2054:             }
ab@2054: 		if (ret!=0) 
ab@2054:             {
ab@2054:                 return ret;
ab@2054:             }
ab@2054:         
ab@2054: 		if ( (pdrvstruc->master) && (pdrvstruc->isattached))
ab@2054:             // master is allready attached
ab@2054:             {
ab@2054:                 // master is allready attached
ab@2054:                 EC_RTDM_ERR(pdrvstruc->masterno,"Master is allready attached!\n");
ab@2054:                 ret = -EFAULT;
ab@2054:             }
ab@2054: 	    else
ab@2054:             {
ab@2054:                 //mstr=ecrt_request_master(0);
ab@2054:                 mstridx = pdrvstruc->masterno;
ab@2054: 	        	
ab@2054:                 pdrvstruc->master=ecrt_attach_master(mstridx);
ab@2054:                 
ab@2054:                 if (pdrvstruc->master)
ab@2054:                     {
ab@2054:                         // Ok
ab@2054:                         EC_RTDM_INFO(pdrvstruc->masterno,"Master searching for domain!\n");
ab@2054:                         pdrvstruc->domain = ecrt_master_find_domain(pdrvstruc->master,l_ioctlvalue[0]);
ab@2054:                         if (!pdrvstruc->domain)
ab@2054:                             {
ab@2054:                                 //
ab@2054:                                 EC_RTDM_ERR(pdrvstruc->masterno,"Cannot find domain from index %u!\n",l_ioctlvalue[0]);
ab@2054:                                 ret = -EFAULT;
ab@2054:                             }
ab@2054:                         else
ab@2054:                             {
ab@2054:                                 
ab@2054:                                 // set device name
ab@2054:                                 snprintf(&pdrvstruc->mutexname[0],sizeof(pdrvstruc->mutexname)-1,"ETHrtdmLOCK%d",pdrvstruc->masterno);
ab@2054:                                 EC_RTDM_INFO(pdrvstruc->masterno,"Creating Master mutex %s!\n",&pdrvstruc->mutexname[0]);
ab@2058:                                 my_mutex_create(&pdrvstruc->masterlock,&pdrvstruc->mutexname[0]);
ab@2054:                                 //ecrt_release_master(mstr);
ab@2054:                                 ecrt_master_callbacks(pdrvstruc->master, send_callback, receive_callback, pdrvstruc);
ab@2054:                                 EC_RTDM_INFO(pdrvstruc->masterno,"MSTR ATTACH done domain=%u!\n",(unsigned int)pdrvstruc->domain);
ab@2054:                                 pdrvstruc->isattached=1;
ab@2054:                                 ret = 0;
ab@2054:                             }
ab@2054:                         
ab@2054:                     }
ab@2054:                 else
ab@2054:                     {
ab@2054:                         EC_RTDM_ERR(pdrvstruc->masterno,"Master attach failed!\n");
ab@2054:                         pdrvstruc->master = NULL;
ab@2054:                         ret = -EFAULT;
ab@2054:                     }
ab@2054:             }
ab@2054:     }
ab@2054:     break;
ab@2054:     default:
ab@2054:         ret = -ENOTTY;
ab@2054:     }
ab@2054:     return ret;
ab@2054: }
ab@2054: 
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            DRIVER READ                                 */
ab@2054: /**********************************************************/
ab@2054: int ec_rtdm_read_rt(struct rtdm_dev_context *context,
ab@2054:                     rtdm_user_info_t *user_info, void *buf, size_t nbyte)
ab@2054: {
ab@2054:     int                     ret;
ab@2054: #if defined(USE_THIS)
ab@2054:     EC_RTDM_DRV_CONTEXT* my_context;
ab@2054:     char                    *out_pos;
ab@2054:     int                     dev_id;
ab@2054:     rtdm_toseq_t            timeout_seq;
ab@2054:     int                     ret;
ab@2054: 
ab@2054:     out_pos = (char *)buf;
ab@2054:     
ab@2054:     my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
ab@2054:     
ab@2054:     // zero bytes requested ? return!
ab@2054:     if (nbyte == 0)
ab@2054:         return 0;
ab@2054: 
ab@2054:     // check if R/W actions to user-space are allowed
ab@2054:     if (user_info && !rtdm_rw_user_ok(user_info, buf, nbyte))
ab@2054:         return -EFAULT;
ab@2054: 
ab@2054:     dev_id = my_context->dev_id;
ab@2054: 
ab@2054:     // in case we need to check if reading is allowed (locking)
ab@2054: /*    if (test_and_set_bit(0, &ctx->in_lock))
ab@2054:         return -EBUSY;
ab@2054: */
ab@2054: /*  // if we need to do some stuff with preemption disabled:
ab@2054:     rtdm_lock_get_irqsave(&ctx->lock, lock_ctx);
ab@2054:     // stuff here
ab@2054:     rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);
ab@2054: */
ab@2054: 
ab@2054:     // wait: if ctx->timeout = 0, it will block infintely until
ab@2054:     //       rtdm_event_signal(&ctx->irq_event); is called from our
ab@2054:     //       interrupt routine
ab@2054:     //ret = rtdm_event_timedwait(&ctx->irq_event, ctx->timeout, &timeout_seq);
ab@2054: 
ab@2054:     // now write the requested stuff to user-space
ab@2054:     if (rtdm_copy_to_user(user_info, out_pos,
ab@2054:                           dummy_buffer, BUFSIZE) != 0) {
ab@2054:         ret = -EFAULT;
ab@2054:     } else {
ab@2054:         ret = BUFSIZE;
ab@2054:     }
ab@2054: #else
ab@2054:     ret = -EFAULT;
ab@2054: #endif
ab@2054:     return ret;
ab@2054: }
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            DRIVER WRITE                                */
ab@2054: /**********************************************************/
ab@2054: int ec_rtdm_write_rt(struct rtdm_dev_context *context,
ab@2054:                    rtdm_user_info_t *user_info,
ab@2054:                    const void *buf, size_t nbyte)
ab@2054: {
ab@2054:     int                     ret;
ab@2054: 
ab@2054: #if defined(USE_THIS)
ab@2054:     int                     dev_id;
ab@2054:     char                    *in_pos = (char *)buf;
ab@2054: 
ab@2054:     EC_RTDM_DRV_CONTEXT* my_context;
ab@2054:     
ab@2054: 
ab@2054:     my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;
ab@2054:     
ab@2054: 
ab@2054:     if (nbyte == 0)
ab@2054:         return 0;
ab@2054:     if (user_info && !rtdm_read_user_ok(user_info, buf, nbyte))
ab@2054:         return -EFAULT;
ab@2054: 
ab@2054:     dev_id = my_context->dev_id;
ab@2054: 
ab@2054:     if (rtdm_copy_from_user(user_info, dummy_buffer,
ab@2054:                              in_pos, BUFSIZE) != 0) {
ab@2054:         ret = -EFAULT;
ab@2054:     } else {
ab@2054:        ret = BUFSIZE;
ab@2054:     }
ab@2054: #else
ab@2054:     ret = -EFAULT;
ab@2054: #endif
ab@2054:     // used when it is atomic
ab@2054: //   rtdm_mutex_unlock(&ctx->out_lock);
ab@2054:     return ret;
ab@2054: }
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            DRIVER OPERATIONS                           */
ab@2054: /**********************************************************/
ab@2054: 
ab@2054: // Template
ab@2054: 
ab@2054: static struct rtdm_device ec_rtdm_device_t = {
ab@2054:     struct_version:     RTDM_DEVICE_STRUCT_VER,
ab@2054: 
ab@2054:     device_flags:       RTDM_NAMED_DEVICE,
ab@2054:     context_size:   	sizeof(EC_RTDM_DRV_CONTEXT),
ab@2054:     device_name:        EC_RTDM_DEV_FILE_NAME,
ab@2054: 
ab@2054: /* open and close functions are not real-time safe due kmalloc
ab@2054:    and kfree. If you do not use kmalloc and kfree, and you made
ab@2054:    sure that there is no syscall in the open/close handler, you
ab@2054:    can declare the open_rt and close_rt handler.
ab@2054: */
ab@2054:     open_rt:            NULL,
ab@2054:     open_nrt:           ec_rtdm_open_rt,
ab@2054: 
ab@2054:     ops: {
ab@2054:         close_rt:       NULL,
ab@2054:         close_nrt:      ec_rtdm_close_rt,
ab@2054: 
ab@2054:         ioctl_rt:       ec_rtdm_ioctl_rt,
ab@2054:         ioctl_nrt:      ec_rtdm_ioctl_rt, // rtdm_mmap_to_user is not RT safe
ab@2054: 
ab@2054:         read_rt:        ec_rtdm_read_rt,
ab@2054:         read_nrt:       NULL,
ab@2054: 
ab@2054:         write_rt:       ec_rtdm_write_rt,
ab@2054:         write_nrt:      NULL,
ab@2054: 
ab@2054:         recvmsg_rt:     NULL,
ab@2054:         recvmsg_nrt:    NULL,
ab@2054: 
ab@2054:         sendmsg_rt:     NULL,
ab@2054:         sendmsg_nrt:    NULL,
ab@2054:     },
ab@2054: 
ab@2054:     device_class:       RTDM_CLASS_EXPERIMENTAL,
ab@2054:     device_sub_class:   222,
ab@2054:     driver_name:        EC_RTDM_DEV_FILE_NAME,
ab@2054:     driver_version:     RTDM_DRIVER_VER(1,0,1),
ab@2054:     peripheral_name:    EC_RTDM_DEV_FILE_NAME,
ab@2055:     provider_name:      "EtherLab Community",
ab@2054: //    proc_name:          ethcatrtdm_device.device_name,
ab@2054: };
ab@2054: 
ab@2054: 
ab@2054: static struct rtdm_device ec_rtdm_devices[EC_RTDM_MAX_MASTERS];
ab@2054: 
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            INIT DRIVER                                 */
ab@2054: /**********************************************************/
ab@2054: int init_module(void)
ab@2054: {
ab@2054: 	unsigned int i;
ab@2054:     int ret;
ab@2054: 
ab@2054:     ret = 0; 	
ab@2054:     
ab@2054:     EC_RTDM_GINFO("Initlializing EtherCAT RTDM Interface to Igh EtherCAT Master\n");
ab@2054:     memset(&ec_rtdm_masterintf[0],0,sizeof(ec_rtdm_masterintf));
ab@2054:     for (i=0;( (i<EC_RTDM_MAX_MASTERS) && (ret==0) ) ;i++)
ab@2054:       {	 
ab@2054:     	// master no to struct
ab@2054:     	ec_rtdm_masterintf[i].masterno = i;
ab@2054:         // copy from template
ab@2054:         memcpy(&ec_rtdm_devices[i],&ec_rtdm_device_t,sizeof(ec_rtdm_devices[0]));
ab@2054: 
ab@2054:         // set device name
ab@2054:         snprintf(&ec_rtdm_devices[i].device_name[0],RTDM_MAX_DEVNAME_LEN,"%s%d",EC_RTDM_DEV_FILE_NAME,i);
ab@2054:         // set proc_name
ab@2054: 	    ec_rtdm_devices[i].proc_name = &ec_rtdm_devices[i].device_name[0];
ab@2054: 	    ec_rtdm_devices[i].driver_name = &ec_rtdm_devices[i].device_name[0];
ab@2054: 		ec_rtdm_devices[i].peripheral_name = &ec_rtdm_devices[i].device_name[0];
ab@2054: 	
ab@2054: 		EC_RTDM_GINFO("Registering device %s!\n",ec_rtdm_devices[i].driver_name);
ab@2054: 		ret = rtdm_dev_register(&ec_rtdm_devices[i]);
ab@2054: 
ab@2054:       }	
ab@2054:     if (ret!=0)
ab@2054:       {	
ab@2054:     	// register m
ab@2054:         EC_RTDM_GERR("Initialization of EtherCAT RTDM Interface failed\n");
ab@2054:       }		
ab@2054:     return ret;
ab@2054: }
ab@2054: 
ab@2054: /**********************************************************/
ab@2054: /*            CLEANUP DRIVER                              */
ab@2054: /**********************************************************/
ab@2054: void cleanup_module(void)
ab@2054: {
ab@2054:     unsigned int i;
ab@2054: 
ab@2055:     EC_RTDM_GINFO("Cleanup EtherCAT RTDM Interface \n");
ab@2054:     for (i=0;i<EC_RTDM_MAX_MASTERS;i++)
ab@2054:       {
ab@2054:          if (ec_rtdm_masterintf[i].isattached)
ab@2054:            {
ab@2054: 	          detach_master(&ec_rtdm_masterintf[i]);
ab@2054: 	       }
ab@2054:  		 EC_RTDM_GINFO("Unregistering device %s!\n",ec_rtdm_devices[i].driver_name);
ab@2054:          rtdm_dev_unregister(&ec_rtdm_devices[i],1000);
ab@2054:     }
ab@2054: }
ab@2054: 
ab@2054: MODULE_LICENSE("GPL");
ab@2055: MODULE_DESCRIPTION("EtherCAT RTDM Interface");