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 sendcallback                         */

   168

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

   169

void send_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

            ecrt_master_send_ext(pdrvstruc->master);

   178

            my_mutex_release(&pdrvstruc->masterlock);

   179

      }

   180

}

   181

   182

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

   183

   184

void receive_callback(void *cb_data)

   185

{

   186

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   187

   188

    pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;

   189

    if (pdrvstruc->master)

   190

      {

   191

          my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   192

          ecrt_master_receive(pdrvstruc->master);

   193

          my_mutex_release(&pdrvstruc->masterlock);      

   194

    }

   195

}

   196

   197

   198

   199

   200

void detach_master(EC_RTDM_DRV_STRUCT * pdrvstruc)

   201

{

   202

   203

  if (pdrvstruc->isattached)

   204

      {

   205

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

   206

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

   207

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

   208

          my_mutex_delete(&pdrvstruc->masterlock);

   209

          pdrvstruc->master = NULL;

   210

          pdrvstruc->isattached=0;

   211

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

   212

      }

   213

}

   214

   215

   216

   217

   218

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

   219

/*            DRIVER OPEN                                 */

   220

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

   221

int ec_rtdm_open_rt(struct rtdm_dev_context    *context,

   222

                 rtdm_user_info_t           *user_info,

   223

                 int                        oflags)

   224

{

   225

    EC_RTDM_DRV_CONTEXT* my_context;

   226

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   227

    const char * p;

   228

    int dev_no;

   229

    unsigned int namelen;

   230

   231

    //int ret;

   232

    int dev_id;

   233

   234

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

   235

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   236

   237

    dev_no = -1;

   238

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

   239

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

   240

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

   241

      {

   242

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

   243

	    {

   244

	       p--;

   245

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

   246

   	    }

   247

	  dev_no=_atoi(p);

   248

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

   249

	    { 

   250

		dev_id    = context->device->device_id;

   251

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

   252

   253

    		my_context->dev_id         = dev_id;

   254

    		my_context->pdrvstruc      = pdrvstruc;

   255

   256

    		// enable interrupt in RTDM

   257

    		return 0;	

   258

	    }	

   259

      }

   260

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

   261

   return -EFAULT;

   262

}

   263

   264

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

   265

/*            DRIVER CLOSE                                */

   266

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

   267

int ec_rtdm_close_rt(struct rtdm_dev_context   *context,

   268

                  rtdm_user_info_t          *user_info)

   269

{

   270

    EC_RTDM_DRV_CONTEXT* my_context;

   271

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   272

   273

    // get the context

   274

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   275

   276

    pdrvstruc =  my_context->pdrvstruc;

   277

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

   278

    detach_master(pdrvstruc);

   279

    return 0;

   280

   281

}

   282

   283

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

   284

/*            DRIVER IOCTL                                */

   285

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

   286

int ec_rtdm_ioctl_rt(struct rtdm_dev_context   *context,

   287

                  rtdm_user_info_t          *user_info,

   288

                  int                       request,

   289

                  void                      *arg)

   290

{

   291

    EC_RTDM_DRV_CONTEXT* my_context;

   292

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   293

    int ret;

   294

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

   295

    ec_domain_state_t ds;

   296

    ec_master_state_t ms;

   297

    uint64_t app_time;

   298

   299

   300

    ret = 0;

   301

   302

    // get the context

   303

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   304

    pdrvstruc =  my_context->pdrvstruc;

   305

   306

    switch (request) {

   307

    case EC_RTDM_MASTERSTATE:

   308

    {

   309

		if (!pdrvstruc->isattached)

   310

            {

   311

                return -EFAULT;

   312

            }

   313

		if (pdrvstruc->master)

   314

            {

   315

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   316

   317

                ecrt_master_state(pdrvstruc->master, &ms);

   318

   319

                my_mutex_release(&pdrvstruc->masterlock);

   320

   321

            }

   322

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

   323

            {

   324

                // copy data to user

   325

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

   326

                    {

   327

                        return -EFAULT;

   328

                    }

   329

            }

   330

   331

    }

   332

    break;

   333

    case EC_RTDM_DOMAINSTATE:

   334

    {

   335

		if (!pdrvstruc->isattached)

   336

            {

   337

                return -EFAULT;

   338

            }

   339

	   	if (pdrvstruc->domain)

   340

            {

   341

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   342

   343

                ecrt_domain_state(pdrvstruc->domain, &ds);

   344

   345

                my_mutex_release(&pdrvstruc->masterlock);

   346

            }

   347

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

   348

            {

   349

                // copy data to user

   350

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

   351

                    {

   352

                        return -EFAULT;

   353

                    }

   354

            }

   355

    }

   356

    break;

   357

    case EC_RTDM_MASTER_RECEIVE:

   358

    {	       

   359

        if (pdrvstruc->isattached)

   360

            {

   361

                if (pdrvstruc->master)

   362

                    {

   363

                        my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   364

                        ecrt_master_receive(pdrvstruc->master);

   365

                        pdrvstruc->reccnt++;

   366

                        my_mutex_release(&pdrvstruc->masterlock);

   367

                    }

   368

            }

   369

    }

   370

    break;

   371

    case EC_RTDM_DOMAIN_PROCESS:

   372

    {	       

   373

        if (pdrvstruc->isattached)

   374

            {

   375

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   376

                ecrt_domain_process(pdrvstruc->domain);

   377

                my_mutex_release(&pdrvstruc->masterlock);

   378

            }

   379

    }

   380

    break;

   381

    case EC_RTDM_MASTER_SEND:

   382

    {

   383

   384

        if (pdrvstruc->isattached)

   385

            {

   386

                if (pdrvstruc->master)

   387

                    {

   388

                        my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   389

                        ecrt_master_send(pdrvstruc->master);

   390

                        pdrvstruc->sendcnt++;

   391

                        my_mutex_release(&pdrvstruc->masterlock);

   392

                    }

   393

            }

   394

    }

   395

    break;

   396

    case EC_RTDM_DOMAIN_QUEQUE:

   397

    {	       

   398

        if (pdrvstruc->isattached)

   399

            {

   400

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   401

                ecrt_domain_queue(pdrvstruc->domain);

   402

                my_mutex_release(&pdrvstruc->masterlock);

   403

            }

   404

    }

   405

    break;

   406

   407

    case EC_RTDM_MASTER_APP_TIME:

   408

    {

   409

		if (!pdrvstruc->isattached)

   410

            {

   411

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

   412

                return -EFAULT;

   413

            }

   414

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

   415

            {

   416

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

   417

                return -EFAULT;

   418

            }

   419

   420

        if (pdrvstruc->master)

   421

            {

   422

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   423

   424

                ecrt_master_application_time(pdrvstruc->master, app_time);

   425

                my_mutex_release(&pdrvstruc->masterlock);

   426

   427

            }

   428

    }

   429

    break;

   430

    case EC_RTDM_SYNC_REF_CLOCK:

   431

    {

   432

		if (!pdrvstruc->isattached)

   433

            {

   434

                return -EFAULT;

   435

            }

   436

        if (pdrvstruc->master)

   437

            {

   438

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   439

   440

                ecrt_master_sync_reference_clock(pdrvstruc->master);

   441

   442

                my_mutex_release(&pdrvstruc->masterlock);

   443

   444

            }

   445

    }

   446

    break;

   447

    case EC_RTDM_SYNC_SLAVE_CLOCK:

   448

    {

   449

		if (!pdrvstruc->isattached)

   450

            {

   451

                return -EFAULT;

   452

            }

   453

        if (pdrvstruc->master)

   454

            {

   455

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   456

   457

                ecrt_master_sync_slave_clocks(pdrvstruc->master);

   458

   459

                my_mutex_release(&pdrvstruc->masterlock);

   460

   461

            }

   462

    }

   463

    break;

   464

    case EC_RTDM_MASTER_SYNC_MONITOR_QUEQUE:

   465

    {

   466

		if (!pdrvstruc->isattached)

   467

            {

   468

                return -EFAULT;

   469

            }

   470

        if (pdrvstruc->master)

   471

            {

   472

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   473

                ecrt_master_sync_monitor_queue(pdrvstruc->master);

   474

                my_mutex_release(&pdrvstruc->masterlock);

   475

            }

   476

    }

   477

    break;

   478

    case EC_RTDM_MASTER_SYNC_MONITOR_PROCESS:

   479

    {

   480

        uint32_t ret;

   481

		if (!pdrvstruc->isattached)

   482

            {

   483

                return -EFAULT;

   484

            }

   485

        if (pdrvstruc->master)

   486

            {

   487

                my_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   488

                ret = ecrt_master_sync_monitor_process(pdrvstruc->master);

   489

                my_mutex_release(&pdrvstruc->masterlock);

   490

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

   491

                    {

   492

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

   493

                        ret=-EFAULT;

   494

                    }

   495

            }

   496

    }

   497

    break;

   498

    case EC_RTDM_MSTRATTACH:

   499

    {

   500

        unsigned int mstridx;

   501

   502

        mstridx = 0;

   503

        ret = 0;

   504

   505

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

   506

        if (user_info) 

   507

            {

   508

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

   509

                    {

   510

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

   511

                            {

   512

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

   513

                            }