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

 *  This file is used for Prisma RT to interface to EtherCAT devices

     9

 *								  

    10

 *  This file is part of ethercatrtdm interface to IgH EtherCAT master 

    11

 *  

    12

 *  The Moehwald ec_rtdm interface is free software; you can

    13

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

    14

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

    15

 *  of the License.

    16

 *

    17

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

    18

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

    19

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

    20

 *  GNU Lesser General Public License for more details.

    21

 *

    22

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

    23

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

    24

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

    25

 *  

    26

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

    27

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

    28

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    29

 *

    30

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

    31

    32

#include <linux/module.h>

    33

#include <linux/mman.h>

    34

#include <rtdm/rtdm_driver.h>

    35

#include <native/task.h>

    36

#include <native/sem.h>

    37

#include <native/mutex.h>

    38

#include <native/timer.h>

    39

    40

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

    41

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

    42

    43

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

    44

    45

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

    46

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

    47

    48

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

    49

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

    50

    51

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

    52

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

    53

    54

    55

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

    56

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

    57

    58

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

    59

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

    60

    61

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

    62

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

    63

    64

    65

    66

    67

typedef struct _EC_RTDM_DRV_STRUCT {

    68

    unsigned int	    isattached;

    69

    ec_master_t *	    master;

    70

    ec_domain_t *	    domain;				   

    71

    RT_MUTEX                masterlock;

    72

    unsigned int	    sendcnt;

    73

    unsigned int	    reccnt;

    74

    unsigned int	    sendcntlv;

    75

    unsigned int	    reccntlv;

    76

    char                    mutexname[64];

    77

    unsigned int            masterno;

    78

} EC_RTDM_DRV_STRUCT;

    79

    80

    81

static EC_RTDM_DRV_STRUCT ec_rtdm_masterintf[EC_RTDM_MAX_MASTERS];

    82

    83

    84

/* import from ethercat */

    85

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

    86

        );

    87

    88

// driver context struct: used for storing various information

    89

typedef struct _EC_RTDM_DRV_CONTEXT {

    90

    int                       dev_id;

    91

    EC_RTDM_DRV_STRUCT*  pdrvstruc;

    92

} EC_RTDM_DRV_CONTEXT;

    93

    94

    95

    96

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

    97

/*            Utilities                                   */

    98

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

    99

   100

static int _atoi(const char* text)

   101

{

   102

  char  b;

   103

  int wd=-1;

   104

  int nfak=1;

   105

   106

  wd=0;

   107

   108

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

   109

  if (*text=='-') 

   110

    {

   111

      nfak=-1;

   112

	  text++;

   113

    }

   114

  if (*text=='+') 

   115

    {

   116

      text++;

   117

    }

   118

  while (*text!=0)

   119

    {

   120

 	  b = *text;

   121

   122

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

   123

		{

   124

		   b=b-'0';

   125

		   wd=wd*10+b;

   126

		}

   127

	  text++;

   128

	}

   129

  return (nfak*wd);

   130

}

   131

   132

   133

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

   134

/*            DRIVER sendcallback                         */

   135

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

   136

void send_callback(void *cb_data)

   137

{

   138

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   139

   140

    pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;

   141

    if (pdrvstruc->master)

   142

        {

   143

            rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   144

            ecrt_master_send_ext(pdrvstruc->master);

   145

            rt_mutex_release(&pdrvstruc->masterlock);

   146

      }

   147

}

   148

   149

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

   150

   151

void receive_callback(void *cb_data)

   152

{

   153

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   154

   155

    pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;

   156

    if (pdrvstruc->master)

   157

      {

   158

          rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   159

          ecrt_master_receive(pdrvstruc->master);

   160

          rt_mutex_release(&pdrvstruc->masterlock);      

   161

    }

   162

}

   163

   164

void receive_process(EC_RTDM_DRV_STRUCT * pdrvstruc)

   165

{

   166

    if (pdrvstruc->master)

   167

        {

   168

            rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   169

            ecrt_master_receive(pdrvstruc->master);

   170

            ecrt_domain_process(pdrvstruc->domain);

   171

            pdrvstruc->reccnt++;

   172

            rt_mutex_release(&pdrvstruc->masterlock);

   173

      }

   174

}

   175

   176

void send_process(EC_RTDM_DRV_STRUCT * pdrvstruc)

   177

{

   178

  if (pdrvstruc->master)

   179

      {

   180

         rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   181

         ecrt_domain_queue(pdrvstruc->domain);

   182

         ecrt_master_send(pdrvstruc->master);

   183

         pdrvstruc->sendcnt++;

   184

         rt_mutex_release(&pdrvstruc->masterlock);

   185

      }

   186

}  

   187

   188

void detach_master(EC_RTDM_DRV_STRUCT * pdrvstruc)

   189

{

   190

   191

  if (pdrvstruc->isattached)

   192

      {

   193

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

   194

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

   195

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

   196

          rt_mutex_delete(&pdrvstruc->masterlock);

   197

          pdrvstruc->master = NULL;

   198

          pdrvstruc->isattached=0;

   199

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

   200

      }

   201

}

   202

   203

   204

   205

   206

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

   207

/*            DRIVER OPEN                                 */

   208

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

   209

int ec_rtdm_open_rt(struct rtdm_dev_context    *context,

   210

                 rtdm_user_info_t           *user_info,

   211

                 int                        oflags)

   212

{

   213

    EC_RTDM_DRV_CONTEXT* my_context;

   214

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   215

    const char * p;

   216

    int dev_no;

   217

    unsigned int namelen;

   218

   219

    //int ret;

   220

    int dev_id;

   221

   222

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

   223

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   224

   225

    dev_no = -1;

   226

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

   227

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

   228

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

   229

      {

   230

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

   231

	    {

   232

	       p--;

   233

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

   234

   	    }

   235

	  dev_no=_atoi(p);

   236

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

   237

	    { 

   238

		dev_id    = context->device->device_id;

   239

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

   240

   241

    		my_context->dev_id         = dev_id;

   242

    		my_context->pdrvstruc      = pdrvstruc;

   243

   244

    		// enable interrupt in RTDM

   245

    		return 0;	

   246

	    }	

   247

      }

   248

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

   249

   return -EFAULT;

   250

}

   251

   252

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

   253

/*            DRIVER CLOSE                                */

   254

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

   255

int ec_rtdm_close_rt(struct rtdm_dev_context   *context,

   256

                  rtdm_user_info_t          *user_info)

   257

{

   258

    EC_RTDM_DRV_CONTEXT* my_context;

   259

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   260

   261

    // get the context

   262

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   263

   264

    pdrvstruc =  my_context->pdrvstruc;

   265

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

   266

    detach_master(pdrvstruc);

   267

    return 0;

   268

   269

}

   270

   271

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

   272

/*            DRIVER IOCTL                                */

   273

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

   274

int ec_rtdm_ioctl_rt(struct rtdm_dev_context   *context,

   275

                  rtdm_user_info_t          *user_info,

   276

                  int                       request,

   277

                  void                      *arg)

   278

{

   279

    EC_RTDM_DRV_CONTEXT* my_context;

   280

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   281

    int ret;

   282

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

   283

    ec_domain_state_t ds;

   284

    ec_master_state_t ms;

   285

    uint64_t app_time;

   286

   287

   288

    ret = 0;

   289

   290

    // get the context

   291

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   292

    pdrvstruc =  my_context->pdrvstruc;

   293

   294

    switch (request) {

   295

    case EC_RTDM_MASTERSTATE:

   296

    {

   297

		if (!pdrvstruc->isattached)

   298

            {

   299

                return -EFAULT;

   300

            }

   301

		if (pdrvstruc->master)

   302

            {

   303

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   304

   305

                ecrt_master_state(pdrvstruc->master, &ms);

   306

   307

                rt_mutex_release(&pdrvstruc->masterlock);

   308

   309

            }

   310

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

   311

            {

   312

                // copy data to user

   313

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

   314

                    {

   315

                        return -EFAULT;

   316

                    }

   317

            }

   318

   319

    }

   320

    break;

   321

    case EC_RTDM_DOMAINSTATE:

   322

    {

   323

		if (!pdrvstruc->isattached)

   324

            {

   325

                return -EFAULT;

   326

            }

   327

	   	if (pdrvstruc->domain)

   328

            {

   329

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   330

   331

                ecrt_domain_state(pdrvstruc->domain, &ds);

   332

   333

                rt_mutex_release(&pdrvstruc->masterlock);

   334

            }

   335

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

   336

            {

   337

                // copy data to user

   338

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

   339

                    {

   340

                        return -EFAULT;

   341

                    }

   342

            }

   343

    }

   344

    break;

   345

   346

    case EC_RTDM_MSTRRECEIVE:

   347

    {	       

   348

        if (pdrvstruc->isattached)

   349

            {

   350

                receive_process(pdrvstruc);

   351

            }

   352

    }

   353

    break;

   354

    case EC_RTDM_MSTRSEND:

   355

    {

   356

   357

        if (pdrvstruc->isattached)

   358

            {

   359

                send_process(pdrvstruc);

   360

            }

   361

    }

   362

    break;

   363

    case EC_RTDM_MASTER_APP_TIME:

   364

    {

   365

		if (!pdrvstruc->isattached)

   366

            {

   367

                return -EFAULT;

   368

            }

   369

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

   370

            {

   371

                // copy data from user

   372

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

   373

                    {

   374

                        return -EFAULT;

   375

                    }

   376

                if (pdrvstruc->master)

   377

                    {

   378

                        rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   379

   380

                        ecrt_master_application_time(pdrvstruc->master, app_time);

   381

   382

                        rt_mutex_release(&pdrvstruc->masterlock);

   383

   384

                    }

   385

            }

   386

    }

   387

    break;

   388

    case EC_RTDM_SYNC_REF_CLOCK:

   389

    {

   390

		if (!pdrvstruc->isattached)

   391

            {

   392

                return -EFAULT;

   393

            }

   394

        if (pdrvstruc->master)

   395

            {

   396

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   397

   398

                ecrt_master_sync_reference_clock(pdrvstruc->master);

   399

   400

                rt_mutex_release(&pdrvstruc->masterlock);

   401

   402

            }

   403

    }

   404

    break;

   405

    case EC_RTDM_SYNC_SLAVE_CLOCK:

   406

    {

   407

		if (!pdrvstruc->isattached)

   408

            {

   409

                return -EFAULT;

   410

            }

   411

        if (pdrvstruc->master)

   412

            {

   413

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   414

   415

                ecrt_master_sync_slave_clocks(pdrvstruc->master);

   416

   417

                rt_mutex_release(&pdrvstruc->masterlock);

   418

   419

            }

   420

    }

   421

    break;

   422

    case EC_RTDM_MSTRATTACH:

   423

    {

   424

        unsigned int mstridx;

   425

   426

        mstridx = 0;

   427

        ret = 0;

   428

   429

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

   430

        if (user_info) 

   431

            {

   432

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

   433

                    {

   434

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

   435

                            {

   436

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

   437

                            }

   438

                        else

   439

                            {

   440

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

   441

                                ret=-EFAULT;

   442

                            }		

   443

                    }

   444

                else

   445

                    { 

   446

                        EC_RTDM_ERR(pdrvstruc->masterno,"user parameter domain missing!\n");

   447

                        ret=-EFAULT;

   448

                    }	

   449

            }

   450

		if (ret!=0) 

   451

            {

   452

                return ret;

   453

            }

   454

   455

		if ( (pdrvstruc->master) && (pdrvstruc->isattached))

   456

            // master is allready attached

   457

            {

   458

                // master is allready attached

   459

                EC_RTDM_ERR(pdrvstruc->masterno,"Master is allready attached!\n");

   460

                ret = -EFAULT;

   461

            }

   462

	    else

   463

            {

   464

                //mstr=ecrt_request_master(0);

   465

                mstridx = pdrvstruc->masterno;

   466

   467

                pdrvstruc->master=ecrt_attach_master(mstridx);

   468

   469

                if (pdrvstruc->master)

   470

                    {

   471

                        // Ok

   472

                        EC_RTDM_INFO(pdrvstruc->masterno,"Master searching for domain!\n");

   473

                        pdrvstruc->domain = ecrt_master_find_domain(pdrvstruc->master,l_ioctlvalue[0]);

   474

                        if (!pdrvstruc->domain)

   475

                            {

   476

                                //

   477

                                EC_RTDM_ERR(pdrvstruc->masterno,"Cannot find domain from index %u!\n",l_ioctlvalue[0]);

   478

                                ret = -EFAULT;

   479

                            }

   480

                        else

   481

                            {

   482

   483

                                // set device name

   484

                                snprintf(&pdrvstruc->mutexname[0],sizeof(pdrvstruc->mutexname)-1,"ETHrtdmLOCK%d",pdrvstruc->masterno);

   485

                                EC_RTDM_INFO(pdrvstruc->masterno,"Creating Master mutex %s!\n",&pdrvstruc->mutexname[0]);

   486

                                rt_mutex_create(&pdrvstruc->masterlock,&pdrvstruc->mutexname[0]);

   487

                                //ecrt_release_master(mstr);

   488

                                ecrt_master_callbacks(pdrvstruc->master, send_callback, receive_callback, pdrvstruc);

   489

                                EC_RTDM_INFO(pdrvstruc->masterno,"MSTR ATTACH done domain=%u!\n",(unsigned int)pdrvstruc->domain);

   490

                                pdrvstruc->isattached=1;

   491

                                ret = 0;

   492

                            }

   493

   494

                    }

   495

                else

   496

                    {

   497

                        EC_RTDM_ERR(pdrvstruc->masterno,"Master attach failed!\n");

   498

                        pdrvstruc->master = NULL;

   499

                        ret = -EFAULT;

   500

                    }

   501

            }

   502

    }

   503

    break;

   504

    default:

   505

        ret = -ENOTTY;

   506

    }

   507

    return ret;

   508

}

   509

   510

   511

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

   512

/*            DRIVER READ                                 */

   513

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

   514

int ec_rtdm_read_rt(struct rtdm_dev_context *context,

   515

                    rtdm_user_info_t *user_info, void *buf, size_t nbyte)

   516

{

   517

    int                     ret;

   518

#if defined(USE_THIS)

   519

    EC_RTDM_DRV_CONTEXT* my_context;

   520

    char                    *out_pos;

   521

    int                     dev_id;

   522

    rtdm_toseq_t            timeout_seq;

   523

    int                     ret;

   524

   525

    out_pos = (char *)buf;

   526

   527

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   528

   529

    // zero bytes requested ? return!

   530