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

 *								  

    10

 *  

    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

#include <rtdm/rtdm_driver.h>

    34

#include <native/task.h>

    35

#include <native/sem.h>

    36

#include <native/mutex.h>

    37

#include <native/timer.h>

    38

    39

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

    40

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

    41

    42

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

    43

    44

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

    46

    47

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

    49

    50

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

    52

    53

    54

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

    56

    57

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

    59

    60

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

    62

    63

    64

    65

    66

typedef struct _EC_RTDM_DRV_STRUCT {

    67

    unsigned int	    isattached;

    68

    ec_master_t *	    master;

    69

    ec_domain_t *	    domain;				   

    70

    RT_MUTEX                masterlock;

    71

    unsigned int	    sendcnt;

    72

    unsigned int	    reccnt;

    73

    unsigned int	    sendcntlv;

    74

    unsigned int	    reccntlv;

    75

    char                    mutexname[64];

    76

    unsigned int            masterno;

    77

} EC_RTDM_DRV_STRUCT;

    78

    79

    80

static EC_RTDM_DRV_STRUCT ec_rtdm_masterintf[EC_RTDM_MAX_MASTERS];

    81

    82

    83

/* import from ethercat */

    84

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

    85

        );

    86

    87

// driver context struct: used for storing various information

    88

typedef struct _EC_RTDM_DRV_CONTEXT {

    89

    int                       dev_id;

    90

    EC_RTDM_DRV_STRUCT*  pdrvstruc;

    91

} EC_RTDM_DRV_CONTEXT;

    92

    93

    94

    95

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

    96

/*            Utilities                                   */

    97

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

    98

    99

static int _atoi(const char* text)

   100

{

   101

  char  b;

   102

  int wd=-1;

   103

  int nfak=1;

   104

   105

  wd=0;

   106

   107

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

   108

  if (*text=='-') 

   109

    {

   110

      nfak=-1;

   111

	  text++;

   112

    }

   113

  if (*text=='+') 

   114

    {

   115

      text++;

   116

    }

   117

  while (*text!=0)

   118

    {

   119

 	  b = *text;

   120

   121

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

   122

		{

   123

		   b=b-'0';

   124

		   wd=wd*10+b;

   125

		}

   126

	  text++;

   127

	}

   128

  return (nfak*wd);

   129

}

   130

   131

   132

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

   133

/*            DRIVER sendcallback                         */

   134

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

   135

void send_callback(void *cb_data)

   136

{

   137

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   138

   139

    pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;

   140

    if (pdrvstruc->master)

   141

        {

   142

            rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   143

            ecrt_master_send_ext(pdrvstruc->master);

   144

            rt_mutex_release(&pdrvstruc->masterlock);

   145

      }

   146

}

   147

   148

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

   149

   150

void receive_callback(void *cb_data)

   151

{

   152

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   153

   154

    pdrvstruc = (EC_RTDM_DRV_STRUCT*)cb_data;

   155

    if (pdrvstruc->master)

   156

      {

   157

          rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   158

          ecrt_master_receive(pdrvstruc->master);

   159

          rt_mutex_release(&pdrvstruc->masterlock);      

   160

    }

   161

}

   162

   165

   166

void detach_master(EC_RTDM_DRV_STRUCT * pdrvstruc)

   167

{

   168

   169

  if (pdrvstruc->isattached)

   170

      {

   171

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

   172

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

   173

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

   174

          rt_mutex_delete(&pdrvstruc->masterlock);

   175

          pdrvstruc->master = NULL;

   176

          pdrvstruc->isattached=0;

   177

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

   178

      }

   179

}

   180

   181

   182

   183

   184

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

   185

/*            DRIVER OPEN                                 */

   186

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

   187

int ec_rtdm_open_rt(struct rtdm_dev_context    *context,

   188

                 rtdm_user_info_t           *user_info,

   189

                 int                        oflags)

   190

{

   191

    EC_RTDM_DRV_CONTEXT* my_context;

   192

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   193

    const char * p;

   194

    int dev_no;

   195

    unsigned int namelen;

   196

   197

    //int ret;

   198

    int dev_id;

   199

   200

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

   201

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   202

   203

    dev_no = -1;

   204

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

   205

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

   206

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

   207

      {

   208

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

   209

	    {

   210

	       p--;

   211

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

   212

   	    }

   213

	  dev_no=_atoi(p);

   214

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

   215

	    { 

   216

		dev_id    = context->device->device_id;

   217

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

   218

   219

    		my_context->dev_id         = dev_id;

   220

    		my_context->pdrvstruc      = pdrvstruc;

   221

   222

    		// enable interrupt in RTDM

   223

    		return 0;	

   224

	    }	

   225

      }

   226

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

   227

   return -EFAULT;

   228

}

   229

   230

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

   231

/*            DRIVER CLOSE                                */

   232

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

   233

int ec_rtdm_close_rt(struct rtdm_dev_context   *context,

   234

                  rtdm_user_info_t          *user_info)

   235

{

   236

    EC_RTDM_DRV_CONTEXT* my_context;

   237

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   238

   239

    // get the context

   240

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   241

   242

    pdrvstruc =  my_context->pdrvstruc;

   243

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

   244

    detach_master(pdrvstruc);

   245

    return 0;

   246

   247

}

   248

   249

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

   250

/*            DRIVER IOCTL                                */

   251

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

   252

int ec_rtdm_ioctl_rt(struct rtdm_dev_context   *context,

   253

                  rtdm_user_info_t          *user_info,

   254

                  int                       request,

   255

                  void                      *arg)

   256

{

   257

    EC_RTDM_DRV_CONTEXT* my_context;

   258

    EC_RTDM_DRV_STRUCT * pdrvstruc;

   259

    int ret;

   260

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

   261

    ec_domain_state_t ds;

   262

    ec_master_state_t ms;

   263

    uint64_t app_time;

   264

   265

   266

    ret = 0;

   267

   268

    // get the context

   269

    my_context = (EC_RTDM_DRV_CONTEXT*)context->dev_private;

   270

    pdrvstruc =  my_context->pdrvstruc;

   271

   272

    switch (request) {

   273

    case EC_RTDM_MASTERSTATE:

   274

    {

   275

		if (!pdrvstruc->isattached)

   276

            {

   277

                return -EFAULT;

   278

            }

   279

		if (pdrvstruc->master)

   280

            {

   281

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   282

   283

                ecrt_master_state(pdrvstruc->master, &ms);

   284

   285

                rt_mutex_release(&pdrvstruc->masterlock);

   286

   287

            }

   288

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

   289

            {

   290

                // copy data to user

   291

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

   292

                    {

   293

                        return -EFAULT;

   294

                    }

   295

            }

   296

   297

    }

   298

    break;

   299

    case EC_RTDM_DOMAINSTATE:

   300

    {

   301

		if (!pdrvstruc->isattached)

   302

            {

   303

                return -EFAULT;

   304

            }

   305

	   	if (pdrvstruc->domain)

   306

            {

   307

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   308

   309

                ecrt_domain_state(pdrvstruc->domain, &ds);

   310

   311

                rt_mutex_release(&pdrvstruc->masterlock);

   312

            }

   313

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

   314

            {

   315

                // copy data to user

   316

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

   317

                    {

   318

                        return -EFAULT;

   319

                    }

   320

            }

   321

    }

   322

    break;

   324

    {	       

   325

        if (pdrvstruc->isattached)

   326

            {

   327

                if (pdrvstruc->master)

   328

                    {

   329

                        rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   332

                        rt_mutex_release(&pdrvstruc->masterlock);

   333

                    }

   334

            }

   335

    }

   336

    break;

   396

    case EC_RTDM_SYNC_REF_CLOCK:

   397

    {

   398

		if (!pdrvstruc->isattached)

   399

            {

   400

                return -EFAULT;

   401

            }

   402

        if (pdrvstruc->master)

   403

            {

   404

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   405

   406

                ecrt_master_sync_reference_clock(pdrvstruc->master);

   407

   408

                rt_mutex_release(&pdrvstruc->masterlock);

   409

   410

            }

   411

    }

   412

    break;

   413

    case EC_RTDM_SYNC_SLAVE_CLOCK:

   414

    {

   415

		if (!pdrvstruc->isattached)

   416

            {

   417

                return -EFAULT;

   418

            }

   419

        if (pdrvstruc->master)

   420

            {

   421

                rt_mutex_acquire(&pdrvstruc->masterlock,TM_INFINITE);

   422

   423

                ecrt_master_sync_slave_clocks(pdrvstruc->master);

   424

   425

                rt_mutex_release(&pdrvstruc->masterlock);

   426

   427

            }

   428

    }

   429

    break;

   464

    case EC_RTDM_MSTRATTACH:

   465

    {

   466

        unsigned int mstridx;

   467

   468

        mstridx = 0;

   469

        ret = 0;

   470

   471

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

   472

        if (user_info) 

   473

            {

   474

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

   475

                    {

   476

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

   477

                            {

   478

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

   479

                            }

   480

                        else

   481

                            {

   482

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

   483

                                ret=-EFAULT;

   484

                            }		

   485

                    }

   486

                else

   487

                    { 

   488

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

   489

                        ret=-EFAULT;

   490

                    }	

   491

            }

   492

		if (ret!=0) 

   493

            {

   494

                return ret;

   495

            }

   496

   497

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

   498

            // master is allready attached

   499

            {

   500

                // master is allready attached

   501

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

   502

                ret = -EFAULT;

   503

            }

   504

	    else

   505

            {

   506

                //mstr=ecrt_request_master(0);

   507

                mstridx = pdrvstruc->masterno;

   508

   509

                pdrvstruc->master=ecrt_attach_master(mstridx);

   510

   511

                if (pdrvstruc->master)

   512

                    {

   513

                        // Ok

   514

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

   515

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

   516

                        if (!pdrvstruc->domain)

   517

                            {

   518

                                //

   519

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

   520

                                ret = -EFAULT;

   521

                            }