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_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

   224

        if (sync_ref_counter) {

   225

            sync_ref_counter--;

   226

        } else {

   227

            sync_ref_counter = 9;

   228

#if 0

   229

            printk(KERN_INFO PFX "ref: %u %u %llu\n",

   230

                    (u32) tv.tv_sec, (u32) tv.tv_usec, EC_TIMEVAL2NANO(&tv));

   231

#endif

   232

            ecrt_master_sync_reference_clock(master, EC_TIMEVAL2NANO(&tv));

   233

        }

   234

		ecrt_master_sync_slave_clocks(master);

   235

        ecrt_domain_queue(domain1);

   236

        ecrt_master_send(master);

   237

        rt_sem_signal(&master_sem);

   238

   239

        rt_task_wait_period();

   240

    }

   241

}

   242

   243

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

   244

   245

int request_lock(void *data)

   246

{

   247

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

   248

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

   249

   250

    // allow access

   251

    rt_sem_wait(&master_sem);

   252

    return 0;

   253

}

   254

   255

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

   256

   257

void release_lock(void *data)

   258

{

   259

    rt_sem_signal(&master_sem);

   260

}

   261

   262

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

   263

   264

int __init init_mod(void)

   265

{

   266

    int ret = -1, i;

   267

    RTIME tick_period, requested_ticks, now;

   268

    ec_slave_config_t *sc;

   269

   270

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

   271

   272

    rt_sem_init(&master_sem, 1);

   273

   274

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

   275

   276

    master = ecrt_request_master(0);

   277

    if (!master) {

   278

        ret = -EBUSY; 

   279

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

   280

        goto out_return;

   281

    }

   282

   283

    ecrt_master_callbacks(master, request_lock, release_lock, NULL);

   284

   285

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

   286

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

   287

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

   288

        goto out_release_master;

   289

    }

   290

   291

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

   292

   293

    // create configuration for reference clock FIXME

   294

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

   295

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

   296

		goto out_release_master;

   297

	}

   298

   299

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

   300

        if (!(sc = ecrt_master_slave_config(master,

   301

                        DigOutSlavePos(i), Beckhoff_EL2008))) {

   302

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

   303

            goto out_release_master;

   304

        }

   305

   306

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

   307

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

   308

            goto out_release_master;

   309

        }

   310

   311

        off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,

   312

                0x7000, 1, domain1, NULL);

   313

   314

        if (off_dig_out[i] < 0)

   315

            goto out_release_master;

   316

    }

   317

   318

	if (!(sc = ecrt_master_slave_config(master,

   319

					CounterSlavePos, IDS_Counter))) {

   320

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

   321

		goto out_release_master;

   322

	}

   323

	off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,

   324

			0x6020, 0x11, domain1, NULL);

   325

	if (off_counter_in < 0)

   326

		goto out_release_master;

   327

	off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,

   328

			0x7020, 1, domain1, NULL);

   329

	if (off_counter_out < 0)

   330

		goto out_release_master;

   331

   332

#if 1

   333

    // configure SYNC signals for this slave

   334

	ecrt_slave_config_dc_assign_activate(sc, 0x0700);

   335

	ecrt_slave_config_dc_sync_cycle_times(sc, 1000000, 0);

   336

	ecrt_slave_config_dc_sync_shift_times(sc,  440000, 0);

   337

#endif

   338

   339

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

   340

    if (ecrt_master_activate(master)) {

   341

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

   342

        goto out_release_master;

   343

    }

   344

   345

    // Get internal process data for domain

   346

    domain1_pd = ecrt_domain_data(domain1);

   347

   348

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

   349

    requested_ticks = nano2count(TIMERTICKS);

   350

    tick_period = start_rt_timer(requested_ticks);

   351

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

   352

           (int) tick_period, (int) requested_ticks);

   353

   354

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

   355

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

   356

        goto out_stop_timer;

   357

    }

   358

   359

    now = rt_get_time();

   360

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

   361

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

   362

        goto out_stop_task;

   363

    }

   364

   365

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

   366

    return 0;

   367

   368

 out_stop_task:

   369

    rt_task_delete(&task);

   370

 out_stop_timer:

   371

    stop_rt_timer();

   372

 out_release_master:

   373

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

   374

    ecrt_release_master(master);

   375

 out_return:

   376

    rt_sem_delete(&master_sem);

   377

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

   378

    return ret;

   379

}

   380

   381

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

   382

   383

void __exit cleanup_mod(void)

   384

{

   385

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

   386

   387

    rt_task_delete(&task);

   388

    stop_rt_timer();

   389

    ecrt_release_master(master);

   390

    rt_sem_delete(&master_sem);

   391

   392

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

   393

}

   394

   395

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

   396

   397

MODULE_LICENSE("GPL");

   398

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

   399

MODULE_DESCRIPTION("EtherCAT distributed clocks sample module");

   400

   401

module_init(init_mod);

   402

module_exit(cleanup_mod);

   403

   404

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