1

/******************************************************************************

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  ec_rtdm.c	Copyright (C) 2009-2010  Moehwald GmbH B.Benner

     6

 *                            2011       IgH Andreas Stewering-Bone

     7

 *								  

     8

 *								  

     9

 *  This file is part of the IgH EtherCAT master 

    10

 *  

    11

 *  The IgH EtherCAT master is free software; you can

    12

 *  redistribute it and/or modify it under the terms of the GNU Lesser General

    13

 *  Public License as published by the Free Software Foundation; version 2.1

    14

 *  of the License.

    15

 *

    16

 *  The IgH EtherCAT master userspace library is distributed in the hope that

    17

 *  it will be useful, but WITHOUT ANY WARRANTY; without even the implied

    18

 *  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    19

 *  GNU Lesser General Public License for more details.

    20

 *

    21

 *  You should have received a copy of the GNU Lesser General Public License

    22

 *  along with the IgH EtherCAT master userspace library. If not, see

    23

 *  <http://www.gnu.org/licenses/>.

    24

 *  

    25

 *  The license mentioned above concerns the source code only. Using the

    26

 *  EtherCAT technology and brand is only permitted in compliance with the

    27

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    28

 *

    29

 *****************************************************************************/

    30

    31

#include <linux/module.h>

    32

#include <linux/mman.h>

    33

    34

    35

#ifdef ENABLE_XENOMAI

    36

#include <native/task.h>

    37

#include <native/sem.h>

    38

#include <native/mutex.h>

    39

#include <native/timer.h>

    40

#endif

    41

    42

#ifdef ENABLE_RTAI

    43

#include <rtai_sched.h>

    44

#include <rtai_sem.h>

    45

#endif

    46

    47

    48

#include <rtdm/rtdm_driver.h>

    49

    50

#include "../include/ecrt.h"

    51

#include "../include/ec_rtdm.h"

    52

    53

#ifdef ENABLE_XENOMAI

    54

#define my_mutex_create(X,Y)  rt_mutex_create(X, Y)

    55

#define my_mutex_acquire(X,Y) rt_mutex_acquire(X,Y)

    56

#define my_mutex_release(X)   rt_mutex_release(X)

    57

#define my_mutex_delete(X)    rt_mutex_delete(X)

    58

#endif

    59

    60

#ifdef ENABLE_RTAI

    61

#define my_mutex_create(X,Y)  rt_sem_init(X, 1)

    62

#define my_mutex_acquire(X,Y) rt_sem_wait(X)

    63

#define my_mutex_release(X)   rt_sem_signal(X)

    64

#define my_mutex_delete(X)    rt_sem_delete(X)

    65

#define TM_INFINITE

    66

#endif

    67

    68

    69

    70

    71

#define EC_RTDM_MAX_MASTERS 5 /**< Maximum number of masters. */

    72

    73

#define EC_RTDM_GINFO(fmt, args...) \

    74

    rtdm_printk(KERN_INFO "EtherCATrtdm: " fmt,  ##args)

    75

    76

#define EC_RTDM_GERR(fmt, args...) \

    77

    rtdm_printk(KERN_ERR "EtherCATrtdm ERROR: " fmt, ##args)

    78

    79

#define EC_RTDM_GWARN(fmt, args...) \

    80

    rtdm_printk(KERN_WARNING "EtherCATrtdm WARNING: " fmt, ##args)

    81

    82

    83

#define EC_RTDM_INFO(devno, fmt, args...) \

    84

    rtdm_printk(KERN_INFO "EtherCATrtdm %u: " fmt, devno, ##args)

    85

    86

#define EC_RTDM_ERR(devno, fmt, args...) \

    87

    rtdm_printk(KERN_ERR "EtherCATrtdm %u ERROR: " fmt, devno, ##args)

    88

    89

#define EC_RTDM_WARN(devno, fmt, args...) \

    90

    rtdm_printk(KERN_WARNING "EtherCATrtdm %u WARNING: " fmt, devno, ##args)

    91

    92

    93

    94

    95

typedef struct _EC_RTDM_DRV_STRUCT {

    96

    unsigned int	    isattached;

    97

    ec_master_t *	    master;

    98

    ec_domain_t *	    domain;	

    99

#ifdef ENABLE_XENOMAI			   

   100

    RT_MUTEX           masterlock;

   101

#endif

   102

#ifdef ENABLE_RTAI

   103

    SEM                masterlock;

   104

#endif

   105

    unsigned int	    sendcnt;

   106

    unsigned int	    reccnt;

   107

    unsigned int	    sendcntlv;

   108

    unsigned int	    reccntlv;

   109

    char                    mutexname[64];

   110

    unsigned int            masterno;

   111

} EC_RTDM_DRV_STRUCT;

   112

   113

   114

static EC_RTDM_DRV_STRUCT ec_rtdm_masterintf[EC_RTDM_MAX_MASTERS];

   115

   116

   117

/* import from ethercat */

   118

ec_master_t *ecrt_attach_master(unsigned int master_index /**< Index of the master to request. */

   119

        );

   120

   121

// driver context struct: used for storing various information

   122

typedef struct _EC_RTDM_DRV_CONTEXT {

   123

    int                       dev_id;

   124

    EC_RTDM_DRV_STRUCT*  pdrvstruc;

   125

} EC_RTDM_DRV_CONTEXT;

   126

   127

   128

   129

/**********************************************************/

   130

/*            Utilities                                   */

   131

/**********************************************************/

   132

   133

static int _atoi(const char* text)

   134

{

   135

  char  b;

   136

  int wd=-1;

   137

  int nfak=1;

   138

   139

  wd=0;

   140

   141

  while ((*text==' ') || (*text=='\t')) text++;

   142

  if (*text=='-') 

   143

    {

   144

      nfak=-1;

   145

	  text++;

   146

    }

   147

  if (*text=='+') 

   148

    {

   149

      text++;

   150

    }

   151

  while (*text!=0)

   152

    {

   153

 	  b = *text;

   154

   155

	  if ( (b>='0') && (b<='9') )

   156

		{

   157

		   b=b-'0';

   158

		   wd=wd*10+b;

   159

		}

   160

	  text++;

   161

	}

   162

  return (nfak*wd);

   163

}

   164

   165

   166

/**********************************************************/

   167

/*            DRIVER lockcallback                         */

   168

/**********************************************************/

   169

void request_lock_callback(void *cb_data)

   170

{

   171

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   172

   173

    pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;

   174

    if (pdrvstruc->master)

   175

        {

   176

            my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   177

        }

   178

}

   179

   180

/*****************************************************************************/

   181

   182

void release_lock_callback(void *cb_data)

   183

{

   184

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   185

   186

    pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;

   187

    if (pdrvstruc->master)

   188

      {

   189

          my_mutex_release(&pdrvstruc->masterlock);      

   190

      }

   191

}

   192

   193

   194

   195

   196

void detach_master(EC_RTDM_DRV_STRUCT * pdrvstruc)

   197

{

   198

   199

  if (pdrvstruc->isattached)

   200

      {

   201

          EC_RTDM_INFO(pdrvstruc->masterno,"reseting callbacks!\n");

   202

          ecrt_master_callbacks(pdrvstruc->master,NULL,NULL,NULL);

   203

          EC_RTDM_INFO(pdrvstruc->masterno,"deleting mutex!\n");

   204

          my_mutex_delete(&pdrvstruc->masterlock);

   205

          pdrvstruc->master = NULL;

   206

          pdrvstruc->isattached=0;

   207

          EC_RTDM_INFO(pdrvstruc->masterno,"master detach done!\n");

   208

      }

   209

}

   210

   211

   212

   213

   214

/**********************************************************/

   215

/*            DRIVER OPEN                                 */

   216

/**********************************************************/

   217

int ec_rtdm_open_rt(struct rtdm_dev_context    *context,

   218

                 rtdm_user_info_t           *user_info,

   219

                 int                        oflags)

   220

{

   221

    EC_RTDM_DRV_CONTEXT* my_context;

   222

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   223

    const char * p;

   224

    int dev_no;

   225

    unsigned int namelen;

   226

   227

    //int ret;

   228

    int dev_id;

   229

   230

    // get the context for our driver - used to store driver info

   231

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   232

   233

    dev_no = -1;

   234

    namelen   = strlen(context->device->driver_name);

   235

    p = &context->device->driver_name[namelen-1];

   236

    if (p!=&context->device->driver_name[0])

   237

      {

   238

	  while ((*p>='0') && (*p<='9')) 

   239

	    {

   240

	       p--;

   241

	       if (p==&context->device->driver_name[0]) break;

   242

   	    }

   243

	  dev_no=_atoi(p);

   244

	  if  ((dev_no!=-1) && (dev_no<EC_RTDM_MAX_MASTERS))

   245

	    { 

   246

		dev_id    = context->device->device_id;

   247

    		pdrvstruc = (EC_RTDM_DRV_STRUCT*)&ec_rtdm_masterintf[dev_no];

   248

   249

    		my_context->dev_id         = dev_id;

   250

    		my_context->pdrvstruc      = pdrvstruc;

   251

   252

    		// enable interrupt in RTDM

   253

    		return 0;	

   254

	    }	

   255

      }

   256

   EC_RTDM_GERR("open - Cannot detect master device no\n");

   257

   return -EFAULT;

   258

}

   259

   260

/**********************************************************/

   261

/*            DRIVER CLOSE                                */

   262

/**********************************************************/

   263

int ec_rtdm_close_rt(struct rtdm_dev_context   *context,

   264

                  rtdm_user_info_t          *user_info)

   265

{

   266

    EC_RTDM_DRV_CONTEXT* my_context;

   267

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   268

   269

    // get the context

   270

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   271

   272

    pdrvstruc =  my_context->pdrvstruc;

   273

    EC_RTDM_INFO(pdrvstruc->masterno,"close called!\n");

   274

    detach_master(pdrvstruc);

   275

    return 0;

   276

   277

}

   278

   279

/**********************************************************/

   280

/*            DRIVER IOCTL                                */

   281

/**********************************************************/

   282

int ec_rtdm_ioctl_rt(struct rtdm_dev_context   *context,

   283

                  rtdm_user_info_t          *user_info,

   284

                  int                       request,

   285

                  void                      *arg)

   286

{

   287

    EC_RTDM_DRV_CONTEXT* my_context;

   288

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   289

    int ret;

   290

    unsigned int l_ioctlvalue[]={0,0,0,0,0,0,0,0};

   291

    ec_domain_state_t ds;

   292

    ec_master_state_t ms;

   293

    uint64_t app_time;

   294

   295

   296

    ret = 0;

   297

   298

    // get the context

   299

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   300

    pdrvstruc =  my_context->pdrvstruc;

   301

   302

    switch (request) {

   303

    case EC_RTDM_MASTERSTATE:

   304

    {

   305

		if (!pdrvstruc->isattached)

   306

            {

   307

                return -EFAULT;

   308

            }

   309

		if (pdrvstruc->master)

   310

            {

   311

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   312

   313

                ecrt_master_state(pdrvstruc->master, &ms);

   314

   315

                my_mutex_release(&pdrvstruc->masterlock);

   316

   317

            }

   318

        if  (rtdm_rw_user_ok(user_info, arg, sizeof(ms)))

   319

            {

   320

                // copy data to user

   321

                if (rtdm_copy_to_user(user_info, arg, &ms,sizeof(ms)))

   322

                    {

   323

                        return -EFAULT;

   324

                    }

   325

            }

   326

   327

    }

   328

    break;

   329

    case EC_RTDM_DOMAINSTATE:

   330

    {

   331

		if (!pdrvstruc->isattached)

   332

            {

   333

                return -EFAULT;

   334

            }

   335

	   	if (pdrvstruc->domain)

   336

            {

   337

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   338

   339

                ecrt_domain_state(pdrvstruc->domain, &ds);

   340

   341

                my_mutex_release(&pdrvstruc->masterlock);

   342

            }

   343

        if  (rtdm_rw_user_ok(user_info, arg, sizeof(ds)))

   344

            {

   345

                // copy data to user

   346

                if (rtdm_copy_to_user(user_info, arg, &ds,sizeof(ds)))

   347

                    {

   348

                        return -EFAULT;

   349

                    }

   350

            }

   351

    }

   352

    break;

   353

    case EC_RTDM_MASTER_RECEIVE:

   354

    {	       

   355

        if (pdrvstruc->isattached)

   356

            {

   357

                if (pdrvstruc->master)

   358

                    {

   359

                        my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   360

                        ecrt_master_receive(pdrvstruc->master);

   361

                        pdrvstruc->reccnt++;

   362

                        my_mutex_release(&pdrvstruc->masterlock);

   363

                    }

   364

            }

   365

    }

   366

    break;

   367

    case EC_RTDM_DOMAIN_PROCESS:

   368

    {	       

   369

        if (pdrvstruc->isattached)

   370

            {

   371

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   372

                ecrt_domain_process(pdrvstruc->domain);

   373

                my_mutex_release(&pdrvstruc->masterlock);

   374

            }

   375

    }

   376

    break;

   377

    case EC_RTDM_MASTER_SEND:

   378

    {

   379

   380

        if (pdrvstruc->isattached)

   381

            {

   382

                if (pdrvstruc->master)

   383

                    {

   384

                        my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   385

                        ecrt_master_send(pdrvstruc->master);

   386

                        pdrvstruc->sendcnt++;

   387

                        my_mutex_release(&pdrvstruc->masterlock);

   388

                    }

   389

            }

   390

    }

   391

    break;

   392

    case EC_RTDM_DOMAIN_QUEQUE:

   393

    {	       

   394

        if (pdrvstruc->isattached)

   395

            {

   396

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   397

                ecrt_domain_queue(pdrvstruc->domain);

   398

                my_mutex_release(&pdrvstruc->masterlock);

   399

            }

   400

    }

   401

    break;

   402

   403

    case EC_RTDM_MASTER_APP_TIME:

   404

    {

   405

		if (!pdrvstruc->isattached)

   406

            {

   407

                rtdm_printk("ERROR : No Master attached\n");

   408

                return -EFAULT;

   409

            }

   410

        if (rtdm_safe_copy_from_user(user_info, &app_time, arg, sizeof(app_time)))

   411

            {

   412

                rtdm_printk("ERROR : can't copy data to driver\n");

   413

                return -EFAULT;

   414

            }

   415

   416

        if (pdrvstruc->master)

   417

            {

   418

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   419

   420

                ecrt_master_application_time(pdrvstruc->master, app_time);

   421

                my_mutex_release(&pdrvstruc->masterlock);

   422

   423

            }

   424

    }

   425

    break;

   426

    case EC_RTDM_SYNC_REF_CLOCK:

   427

    {

   428

		if (!pdrvstruc->isattached)

   429

            {

   430

                return -EFAULT;

   431

            }

   432

        if (pdrvstruc->master)

   433

            {

   434

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   435

   436

                ecrt_master_sync_reference_clock(pdrvstruc->master);

   437

   438

                my_mutex_release(&pdrvstruc->masterlock);

   439

   440

            }

   441

    }

   442

    break;

   443

    case EC_RTDM_SYNC_SLAVE_CLOCK:

   444

    {

   445

		if (!pdrvstruc->isattached)

   446

            {

   447

                return -EFAULT;

   448

            }

   449

        if (pdrvstruc->master)

   450

            {

   451

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   452

   453

                ecrt_master_sync_slave_clocks(pdrvstruc->master);

   454

   455

                my_mutex_release(&pdrvstruc->masterlock);

   456

   457

            }

   458

    }

   459

    break;

   460

    case EC_RTDM_MASTER_SYNC_MONITOR_QUEQUE:

   461

    {

   462

		if (!pdrvstruc->isattached)

   463

            {

   464

                return -EFAULT;

   465

            }

   466

        if (pdrvstruc->master)

   467

            {

   468

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   469

                ecrt_master_sync_monitor_queue(pdrvstruc->master);

   470

                my_mutex_release(&pdrvstruc->masterlock);

   471

            }

   472

    }

   473

    break;

   474

    case EC_RTDM_MASTER_SYNC_MONITOR_PROCESS:

   475

    {

   476

        uint32_t ret;

   477

		if (!pdrvstruc->isattached)

   478

            {

   479

                return -EFAULT;

   480

            }

   481

        if (pdrvstruc->master)

   482

            {

   483

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   484

                ret = ecrt_master_sync_monitor_process(pdrvstruc->master);

   485

                my_mutex_release(&pdrvstruc->masterlock);

   486

                if (rtdm_safe_copy_to_user(user_info, arg, &ret, sizeof(ret)))

   487

                    {

   488

                        EC_RTDM_ERR(pdrvstruc->masterno,"copy to user param failed!\n");

   489

                        ret=-EFAULT;

   490

                    }

   491

            }

   492

    }

   493

    break;

   494

    case EC_RTDM_MSTRATTACH:

   495

    {

   496

        unsigned int mstridx;

   497

   498

        mstridx = 0;

   499

        ret = 0;

   500

   501

        EC_RTDM_INFO(pdrvstruc->masterno,"Master attach start!\n");

   502

        if (user_info) 

   503

            {

   504

                if (rtdm_read_user_ok(user_info, arg, sizeof(unsigned int)))

   505

                    {

   506

                        if (rtdm_copy_from_user(user_info, &l_ioctlvalue[0], arg,sizeof(unsigned int))==0)

   507

                            {

   508

                                pdrvstruc->domain = (ec_domain_t*)l_ioctlvalue[0];

   509

                            }

   510

                        else

   511

                            {

   512

                                EC_RTDM_ERR(pdrvstruc->masterno,"copy user param failed!\n");

   513

                                ret=-EFAULT;