1

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

     2

 *

     3

 *  Distributed clocks sample for the IgH EtherCAT master.

     4

 *

     5

 *  $Id$

     6

 *

     7

 *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH

     8

 *

     9

 *  This file is part of the IgH EtherCAT Master.

    10

 *

    11

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    12

 *  modify it under the terms of the GNU General Public License version 2, as

    13

 *  published by the Free Software Foundation.

    14

 *

    15

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    16

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    17

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    18

 *  Public License for more details.

    19

 *

    20

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

    21

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    22

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    23

 *

    24

 *  ---

    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

// Linux

    33

#include <linux/module.h>

    34

#include <linux/err.h>

    35

    36

// RTAI

    37

#include <rtai_sched.h>

    38

#include <rtai_sem.h>

    39

    40

// EtherCAT

    41

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

    42

    43

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

    44

    45

// Module parameters

    46

    47

#define FREQUENCY 1000 // task frequency in Hz

    48

#define INHIBIT_TIME 20

    49

    50

#define TIMERTICKS (1000000000 / FREQUENCY)

    51

    52

#define NUM_DIG_OUT 1

    53

    54

#define PFX "ec_dc_rtai_sample: "

    55

    56

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

    57

    58

// EtherCAT

    59

static ec_master_t *master = NULL;

    60

static ec_master_state_t master_state = {};

    61

spinlock_t master_lock = SPIN_LOCK_UNLOCKED;

    62

    63

static ec_domain_t *domain1 = NULL;

    64

static ec_domain_state_t domain1_state = {};

    65

    66

// RTAI

    67

static RT_TASK task;

    68

static SEM master_sem;

    69

static cycles_t t_last_cycle = 0, t_critical;

    70

    71

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

    72

    73

// process data

    74

static uint8_t *domain1_pd; // process data memory

    75

    76

#define DigOutSlavePos(X) 0, (1 + (X))

    77

#define CounterSlavePos   0, 2

    78

    79

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

    80

#define Beckhoff_EL2008 0x00000002, 0x07d83052

    81

#define IDS_Counter     0x000012ad, 0x05de3052

    82

    83

static int off_dig_out[NUM_DIG_OUT];

    84

static int off_counter_in;

    85

static int off_counter_out;

    86

    87

static unsigned int counter = 0;

    88

static unsigned int blink_counter = 0;

    89

static unsigned int blink = 0;

    90

static u32 counter_value = 0U;

    91

    92

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

    93

    94

static ec_pdo_entry_info_t el2008_channels[] = {

    95

    {0x7000, 1, 1},

    96

    {0x7010, 1, 1},

    97

    {0x7020, 1, 1},

    98

    {0x7030, 1, 1},

    99

    {0x7040, 1, 1},

   100

    {0x7050, 1, 1},

   101

    {0x7060, 1, 1},

   102

    {0x7070, 1, 1}

   103

};

   104

   105

static ec_pdo_info_t el2008_pdos[] = {

   106

    {0x1600, 1, &el2008_channels[0]},

   107

    {0x1601, 1, &el2008_channels[1]},

   108

    {0x1602, 1, &el2008_channels[2]},

   109

    {0x1603, 1, &el2008_channels[3]},

   110

    {0x1604, 1, &el2008_channels[4]},

   111

    {0x1605, 1, &el2008_channels[5]},

   112

    {0x1606, 1, &el2008_channels[6]},

   113

    {0x1607, 1, &el2008_channels[7]}

   114

};

   115

   116

static ec_sync_info_t el2008_syncs[] = {

   117

    {0, EC_DIR_OUTPUT, 8, el2008_pdos},

   118

    {1, EC_DIR_INPUT},

   119

    {0xff}

   120

};

   121

   122

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

   123

   124

void check_domain1_state(void)

   125

{

   126

    ec_domain_state_t ds;

   127

   128

    spin_lock(&master_lock);

   129

    ecrt_domain_state(domain1, &ds);

   130

    spin_unlock(&master_lock);

   131

   132

    if (ds.working_counter != domain1_state.working_counter)

   133

        printk(KERN_INFO PFX "Domain1: WC %u.\n", ds.working_counter);

   134

    if (ds.wc_state != domain1_state.wc_state)

   135

        printk(KERN_INFO PFX "Domain1: State %u.\n", ds.wc_state);

   136

   137

    domain1_state = ds;

   138

}

   139

   140

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

   141

   142

void check_master_state(void)

   143

{

   144

    ec_master_state_t ms;

   145

   146

    spin_lock(&master_lock);

   147

    ecrt_master_state(master, &ms);

   148

    spin_unlock(&master_lock);

   149

   150

    if (ms.slaves_responding != master_state.slaves_responding)

   151

        printk(KERN_INFO PFX "%u slave(s).\n", ms.slaves_responding);

   152

    if (ms.al_states != master_state.al_states)

   153

        printk(KERN_INFO PFX "AL states: 0x%02X.\n", ms.al_states);

   154

    if (ms.link_up != master_state.link_up)

   155

        printk(KERN_INFO PFX "Link is %s.\n", ms.link_up ? "up" : "down");

   156

   157

    master_state = ms;

   158

}

   159

   160

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

   161

   162

void run(long data)

   163

{

   164

    int i;

   165

	struct timeval tv;

   166

    unsigned int sync_ref_counter = 0;

   167

   168

    count2timeval(nano2count(rt_get_real_time_ns()), &tv);

   169

   170

    while (1) {

   171

        t_last_cycle = get_cycles();

   172

   173

        // receive process data

   174

        rt_sem_wait(&master_sem);

   175

        ecrt_master_receive(master);

   176

        ecrt_domain_process(domain1);

   177

        rt_sem_signal(&master_sem);

   178

   179

        // check process data state (optional)

   180

        check_domain1_state();

   181

   182

        if (counter) {

   183

            counter--;

   184

        } else {

   185

			u32 c;

   186

   187

            counter = FREQUENCY;

   188

   189

            // check for master state (optional)

   190

            check_master_state();

   191

   192

			c = EC_READ_U32(domain1_pd + off_counter_in);

   193

			if (counter_value != c) {

   194

				counter_value = c;

   195

				printk(KERN_INFO PFX "counter=%u\n", counter_value);

   196

			}

   197

   198

        }

   199

   200

        if (blink_counter) {

   201

            blink_counter--;

   202

        } else {

   203

            blink_counter = 9;

   204

   205

            // calculate new process data

   206

            blink = !blink;

   207

        }

   208

   209

        // write process data

   210

        for (i = 0; i < NUM_DIG_OUT; i++) {

   211

            EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);

   212

        }

   213

   214

		EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);

   215

   216

        rt_sem_wait(&master_sem);

   217

   218

        tv.tv_usec += 1000;

   219

        if (tv.tv_usec >= 1000000)  {

   220

            tv.tv_usec -= 1000000;

   221

            tv.tv_sec++;

   222

        }

   223

        ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));

   224

   225

        if (sync_ref_counter) {

   226

            sync_ref_counter--;

   227

        } else {

   228

            sync_ref_counter = 9;

   229

            ecrt_master_sync_reference_clock(master);

   230

        }

   231

		ecrt_master_sync_slave_clocks(master);

   232

        ecrt_domain_queue(domain1);

   233

        ecrt_master_send(master);

   234

        rt_sem_signal(&master_sem);

   235

   236

        rt_task_wait_period();

   237

    }

   238

}

   239

   240

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

   241

   242

int request_lock(void *data)

   243

{

   244

    // too close to the next real time cycle: deny access...

   245

    if (get_cycles() - t_last_cycle > t_critical) return -1;

   246

   247

    // allow access

   248

    rt_sem_wait(&master_sem);

   249

    return 0;

   250

}

   251

   252

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

   253

   254

void release_lock(void *data)

   255

{

   256

    rt_sem_signal(&master_sem);

   257

}

   258

   259

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

   260

   261

int __init init_mod(void)

   262

{

   263

    int ret = -1, i;

   264

    RTIME tick_period, requested_ticks, now;

   265

    ec_slave_config_t *sc;

   266

   267

    printk(KERN_INFO PFX "Starting...\n");

   268

   269

    rt_sem_init(&master_sem, 1);

   270

   271

    t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;

   272

   273

    master = ecrt_request_master(0);

   274

    if (!master) {

   275

        ret = -EBUSY; 

   276

        printk(KERN_ERR PFX "Requesting master 0 failed!\n");

   277

        goto out_return;

   278

    }

   279

   280

    ecrt_master_callbacks(master, request_lock, release_lock, NULL);

   281

   282

    printk(KERN_INFO PFX "Registering domain...\n");

   283

    if (!(domain1 = ecrt_master_create_domain(master))) {

   284

        printk(KERN_ERR PFX "Domain creation failed!\n");

   285

        goto out_release_master;

   286

    }

   287

   288

    printk(KERN_INFO PFX "Configuring PDOs...\n");

   289

   290

    // create configuration for reference clock FIXME

   291

	if (!(sc = ecrt_master_slave_config(master, 0, 0, Beckhoff_EK1100))) {

   292

		printk(KERN_ERR PFX "Failed to get slave configuration.\n");

   293

		goto out_release_master;

   294

	}

   295

   296

    for (i = 0; i < NUM_DIG_OUT; i++) {

   297

        if (!(sc = ecrt_master_slave_config(master,

   298

                        DigOutSlavePos(i), Beckhoff_EL2008))) {

   299

            printk(KERN_ERR PFX "Failed to get slave configuration.\n");

   300

            goto out_release_master;

   301

        }

   302

   303

        if (ecrt_slave_config_pdos(sc, EC_END, el2008_syncs)) {

   304

            printk(KERN_ERR PFX "Failed to configure PDOs.\n");

   305

            goto out_release_master;

   306

        }

   307

   308

        off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,

   309

                0x7000, 1, domain1, NULL);

   310

   311

        if (off_dig_out[i] < 0)

   312

            goto out_release_master;

   313

    }

   314

   315

	if (!(sc = ecrt_master_slave_config(master,

   316

					CounterSlavePos, IDS_Counter))) {

   317

		printk(KERN_ERR PFX "Failed to get slave configuration.\n");

   318

		goto out_release_master;

   319

	}

   320

	off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,

   321

			0x6020, 0x11, domain1, NULL);

   322

	if (off_counter_in < 0)

   323

		goto out_release_master;

   324

	off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,

   325

			0x7020, 1, domain1, NULL);

   326

	if (off_counter_out < 0)

   327

		goto out_release_master;

   328

   329

    // configure SYNC signals for this slave

   330

	ecrt_slave_config_dc(sc, 0x0700, 1000000, 440000, 0, 0);

   331

   332

    printk(KERN_INFO PFX "Activating master...\n");

   333

    if (ecrt_master_activate(master)) {

   334

        printk(KERN_ERR PFX "Failed to activate master!\n");

   335

        goto out_release_master;

   336

    }

   337

   338

    // Get internal process data for domain

   339

    domain1_pd = ecrt_domain_data(domain1);

   340

   341

    printk(KERN_INFO PFX "Starting cyclic sample thread...\n");

   342

    requested_ticks = nano2count(TIMERTICKS);

   343

    tick_period = start_rt_timer(requested_ticks);

   344

    printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n",

   345

           (int) tick_period, (int) requested_ticks);

   346

   347

    if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) {

   348

        printk(KERN_ERR PFX "Failed to init RTAI task!\n");

   349

        goto out_stop_timer;

   350

    }

   351

   352

    now = rt_get_time();

   353

    if (rt_task_make_periodic(&task, now + tick_period, tick_period)) {

   354

        printk(KERN_ERR PFX "Failed to run RTAI task!\n");

   355

        goto out_stop_task;

   356

    }

   357

   358

    printk(KERN_INFO PFX "Initialized.\n");

   359

    return 0;

   360

   361

 out_stop_task:

   362

    rt_task_delete(&task);

   363

 out_stop_timer:

   364

    stop_rt_timer();

   365

 out_release_master:

   366

    printk(KERN_ERR PFX "Releasing master...\n");

   367

    ecrt_release_master(master);

   368

 out_return:

   369

    rt_sem_delete(&master_sem);

   370

    printk(KERN_ERR PFX "Failed to load. Aborting.\n");

   371

    return ret;

   372

}

   373

   374

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

   375

   376

void __exit cleanup_mod(void)

   377

{

   378

    printk(KERN_INFO PFX "Stopping...\n");

   379

   380

    rt_task_delete(&task);

   381

    stop_rt_timer();

   382

    ecrt_release_master(master);

   383

    rt_sem_delete(&master_sem);

   384

   385

    printk(KERN_INFO PFX "Unloading.\n");

   386

}

   387

   388

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

   389

   390

MODULE_LICENSE("GPL");

   391

MODULE_AUTHOR("Florian Pose <fp@igh-essen.com>");

   392

MODULE_DESCRIPTION("EtherCAT distributed clocks sample module");

   393

   394

module_init(init_mod);

   395

module_exit(cleanup_mod);

   396

   397

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