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

   127

    rt_sem_wait(&master_sem);

   128

    ecrt_domain_state(domain1, &ds);

   129

    rt_sem_signal(&master_sem);

   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

   145

    rt_sem_wait(&master_sem);

   146

    ecrt_master_state(master, &ms);

   147

    rt_sem_signal(&master_sem);

   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

   241

void send_callback(void *cb_data)

   242

{

   243

    ec_master_t *m = (ec_master_t *) cb_data;

   244

   245

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

   246

    if (get_cycles() - t_last_cycle <= t_critical) {

   247

        rt_sem_wait(&master_sem);

   248

        ecrt_master_send_ext(m);

   249

        rt_sem_signal(&master_sem);

   250

    }

   251

}

   252

   253

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

   254

   255

void receive_callback(void *cb_data)

   256

{

   257

    ec_master_t *m = (ec_master_t *) cb_data;

   258

   259

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

   260

    if (get_cycles() - t_last_cycle <= t_critical) {

   261

        rt_sem_wait(&master_sem);

   262

        ecrt_master_receive(m);

   263

        rt_sem_signal(&master_sem);

   264

    }

   265

}

   266

   267

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

   268

   269

int __init init_mod(void)

   270

{

   271

    int ret = -1, i;

   272

    RTIME tick_period, requested_ticks, now;

   273

    ec_slave_config_t *sc;

   274

   275

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

   276

   277

    rt_sem_init(&master_sem, 1);

   278

   279

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

   280

   281

    master = ecrt_request_master(0);

   282

    if (!master) {

   283

        ret = -EBUSY; 

   284

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

   285

        goto out_return;

   286

    }

   287

   288

    ecrt_master_callbacks(master, send_callback, receive_callback, master);

   289

   290

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

   291

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

   292

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

   293

        goto out_release_master;

   294

    }

   295

   296

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

   297

   298

    // create configuration for reference clock FIXME

   299

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

   300

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

   301

        goto out_release_master;

   302

    }

   303

   304

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

   305

        if (!(sc = ecrt_master_slave_config(master,

   306

                        DigOutSlavePos(i), Beckhoff_EL2008))) {

   307

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

   308

            goto out_release_master;

   309

        }

   310

   311

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

   312

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

   313

            goto out_release_master;

   314

        }

   315

   316

        off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,

   317

                0x7000, 1, domain1, NULL);

   318

   319

        if (off_dig_out[i] < 0)

   320

            goto out_release_master;

   321

    }

   322

   323

    if (!(sc = ecrt_master_slave_config(master,

   324

                    CounterSlavePos, IDS_Counter))) {

   325

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

   326

        goto out_release_master;

   327

    }

   328

    off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,

   329

            0x6020, 0x11, domain1, NULL);

   330

    if (off_counter_in < 0)

   331

        goto out_release_master;

   332

    off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,

   333

            0x7020, 1, domain1, NULL);

   334

    if (off_counter_out < 0)

   335

        goto out_release_master;

   336

   337

    // configure SYNC signals for this slave

   338

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

   339

   340

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

   341

    if (ecrt_master_activate(master)) {

   342

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

   343

        goto out_release_master;

   344

    }

   345

   346

    // Get internal process data for domain

   347

    domain1_pd = ecrt_domain_data(domain1);

   348

   349

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

   350

    requested_ticks = nano2count(TIMERTICKS);

   351

    tick_period = start_rt_timer(requested_ticks);

   352

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

   353

           (int) tick_period, (int) requested_ticks);

   354

   355

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

   356

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

   357

        goto out_stop_timer;

   358

    }

   359

   360

    now = rt_get_time();

   361

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

   362

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

   363

        goto out_stop_task;

   364

    }

   365

   366

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

   367

    return 0;

   368

   369

 out_stop_task:

   370

    rt_task_delete(&task);

   371

 out_stop_timer:

   372

    stop_rt_timer();

   373

 out_release_master:

   374

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

   375

    ecrt_release_master(master);

   376

 out_return:

   377

    rt_sem_delete(&master_sem);

   378

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

   379

    return ret;

   380

}

   381

   382

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

   383

   384

void __exit cleanup_mod(void)

   385

{

   386

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

   387

   388

    rt_task_delete(&task);

   389

    stop_rt_timer();

   390

    ecrt_release_master(master);

   391

    rt_sem_delete(&master_sem);

   392

   393

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

   394

}

   395

   396

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

   397

   398

MODULE_LICENSE("GPL");

   399

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

   400

MODULE_DESCRIPTION("EtherCAT distributed clocks sample module");

   401

   402

module_init(init_mod);

   403

module_exit(cleanup_mod);

   404

   405

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