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

    62

static ec_domain_t *domain1 = NULL;

    63

static ec_domain_state_t domain1_state = {};

    64

    65

// RTAI

    66

static RT_TASK task;

    67

static SEM master_sem;

    68

static cycles_t t_last_cycle = 0, t_critical;

    69

    70

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

    71

    72

// process data

    73

static uint8_t *domain1_pd; // process data memory

    74

    75

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

    76

#define CounterSlavePos   0, 2

    77

    78

#define Beckhoff_EK1100 0x00000002, 0x044c2c52

    79

#define Beckhoff_EL2008 0x00000002, 0x07d83052

    80

#define IDS_Counter     0x000012ad, 0x05de3052

    81

    82

static int off_dig_out[NUM_DIG_OUT];

    83

static int off_counter_in;

    84

static int off_counter_out;

    85

    86

static unsigned int counter = 0;

    87

static unsigned int blink_counter = 0;

    88

static unsigned int blink = 0;

    89

static u32 counter_value = 0U;

    90

    91

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

    92

    93

static ec_pdo_entry_info_t el2008_channels[] = {

    94

    {0x7000, 1, 1},

    95

    {0x7010, 1, 1},

    96

    {0x7020, 1, 1},

    97

    {0x7030, 1, 1},

    98

    {0x7040, 1, 1},

    99

    {0x7050, 1, 1},

   100

    {0x7060, 1, 1},

   101

    {0x7070, 1, 1}

   102

};

   103

   104

static ec_pdo_info_t el2008_pdos[] = {

   105

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

   106

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

   107

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

   108

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

   109

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

   110

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

   111

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

   112

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

   113

};

   114

   115

static ec_sync_info_t el2008_syncs[] = {

   116

    {0, EC_DIR_OUTPUT, 8, el2008_pdos},

   117

    {1, EC_DIR_INPUT},

   118

    {0xff}

   119

};

   120

   121

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

   122

   123

void check_domain1_state(void)

   124

{

   125

    ec_domain_state_t ds;

   126

   128

    ecrt_domain_state(domain1, &ds);

   130

   131

    if (ds.working_counter != domain1_state.working_counter)

   132

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

   133

    if (ds.wc_state != domain1_state.wc_state)

   134

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

   135

   136

    domain1_state = ds;

   137

}

   138

   139

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

   140

   141

void check_master_state(void)

   142

{

   143

    ec_master_state_t ms;

   144

   146

    ecrt_master_state(master, &ms);

   148

   149

    if (ms.slaves_responding != master_state.slaves_responding)

   150

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

   151

    if (ms.al_states != master_state.al_states)

   152

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

   153

    if (ms.link_up != master_state.link_up)

   154

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

   155

   156

    master_state = ms;

   157

}

   158

   159

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

   160

   161

void run(long data)

   162

{

   163

    int i;

   164

	struct timeval tv;

   165

    unsigned int sync_ref_counter = 0;

   166

   167

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

   168

   169

    while (1) {

   170

        t_last_cycle = get_cycles();

   171

   172

        // receive process data

   173

        rt_sem_wait(&master_sem);

   174

        ecrt_master_receive(master);

   175

        ecrt_domain_process(domain1);

   176

        rt_sem_signal(&master_sem);

   177

   178

        // check process data state (optional)

   179

        check_domain1_state();

   180

   181

        if (counter) {

   182

            counter--;

   183

        } else {

   184

			u32 c;

   185

   186

            counter = FREQUENCY;

   187

   188

            // check for master state (optional)

   189

            check_master_state();

   190

   191

			c = EC_READ_U32(domain1_pd + off_counter_in);

   192

			if (counter_value != c) {

   193

				counter_value = c;

   194

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

   195

			}

   196

   197

        }

   198

   199

        if (blink_counter) {

   200

            blink_counter--;

   201

        } else {

   202

            blink_counter = 9;

   203

   204

            // calculate new process data

   205

            blink = !blink;

   206

        }

   207

   208

        // write process data

   209

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

   210

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

   211

        }

   212

   213

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

   214

   215

        rt_sem_wait(&master_sem);

   216

   217

        tv.tv_usec += 1000;

   218

        if (tv.tv_usec >= 1000000)  {

   219

            tv.tv_usec -= 1000000;

   220

            tv.tv_sec++;

   221

        }

   222

        ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));

   223

   224

        if (sync_ref_counter) {

   225

            sync_ref_counter--;

   226

        } else {

   227

            sync_ref_counter = 9;

   228

            ecrt_master_sync_reference_clock(master);

   229

        }

   230

		ecrt_master_sync_slave_clocks(master);

   231

        ecrt_domain_queue(domain1);

   232

        ecrt_master_send(master);

   233

        rt_sem_signal(&master_sem);

   234

   235

        rt_task_wait_period();

   236

    }

   237

}

   238

   239

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

   240

   242

{

   243

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

   261

}

   262

   263

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

   264

   265

int __init init_mod(void)

   266

{

   267

    int ret = -1, i;

   268

    RTIME tick_period, requested_ticks, now;

   269

    ec_slave_config_t *sc;

   270

   271

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

   272

   273

    rt_sem_init(&master_sem, 1);

   274

   275

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

   276

   277

    master = ecrt_request_master(0);

   278

    if (!master) {

   279

        ret = -EBUSY; 

   280

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

   281

        goto out_return;

   282

    }

   283

   285

   286

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

   287

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

   288

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

   289

        goto out_release_master;

   290

    }

   291

   292

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

   293

   294

    // create configuration for reference clock FIXME

   295

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

   296

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

   297

		goto out_release_master;

   298

	}

   299

   300

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

   301

        if (!(sc = ecrt_master_slave_config(master,

   302

                        DigOutSlavePos(i), Beckhoff_EL2008))) {

   303

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

   304

            goto out_release_master;

   305

        }

   306

   307

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

   308

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

   309

            goto out_release_master;

   310

        }

   311

   312

        off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,

   313

                0x7000, 1, domain1, NULL);

   314

   315

        if (off_dig_out[i] < 0)

   316

            goto out_release_master;

   317

    }

   318

   319

	if (!(sc = ecrt_master_slave_config(master,

   320

					CounterSlavePos, IDS_Counter))) {

   321

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

   322

		goto out_release_master;

   323

	}

   324

	off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,

   325

			0x6020, 0x11, domain1, NULL);

   326

	if (off_counter_in < 0)

   327

		goto out_release_master;

   328

	off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,

   329

			0x7020, 1, domain1, NULL);

   330

	if (off_counter_out < 0)

   331

		goto out_release_master;

   332

   333

    // configure SYNC signals for this slave

   334

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

   335

   336

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

   337

    if (ecrt_master_activate(master)) {

   338

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

   339

        goto out_release_master;

   340

    }

   341

   342

    // Get internal process data for domain

   343

    domain1_pd = ecrt_domain_data(domain1);

   344

   345

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

   346

    requested_ticks = nano2count(TIMERTICKS);

   347

    tick_period = start_rt_timer(requested_ticks);

   348

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

   349

           (int) tick_period, (int) requested_ticks);

   350

   351

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

   352

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

   353

        goto out_stop_timer;

   354

    }

   355

   356

    now = rt_get_time();

   357

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

   358

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

   359

        goto out_stop_task;

   360

    }

   361

   362

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

   363

    return 0;

   364

   365

 out_stop_task:

   366

    rt_task_delete(&task);

   367

 out_stop_timer:

   368

    stop_rt_timer();

   369

 out_release_master:

   370

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

   371

    ecrt_release_master(master);

   372

 out_return:

   373

    rt_sem_delete(&master_sem);

   374

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

   375

    return ret;

   376

}

   377

   378

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

   379

   380

void __exit cleanup_mod(void)

   381

{

   382

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

   383

   384

    rt_task_delete(&task);

   385

    stop_rt_timer();

   386

    ecrt_release_master(master);

   387

    rt_sem_delete(&master_sem);

   388

   389

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

   390

}

   391

   392

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

   393

   394

MODULE_LICENSE("GPL");

   395

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

   396

MODULE_DESCRIPTION("EtherCAT distributed clocks sample module");

   397

   398

module_init(init_mod);

   399

module_exit(cleanup_mod);

   400

   401

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