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

        rt_sem_signal(&master_sem);

   233

        ecrt_master_send(master);

   234

   235

        rt_task_wait_period();

   236

    }

   237

}

   238

   239

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

   240

   241

void request_lock_callback(void *cb_data)

   242

{

   243

    ec_master_t *m = (ec_master_t *) cb_data;

   244

    rt_sem_wait(&master_sem);

   245

}

   246

   247

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

   248

   249

void release_lock_callback(void *cb_data)

   250

{

   251

    ec_master_t *m = (ec_master_t *) cb_data;

   252

    rt_sem_signal(&master_sem);

   253

}

   254

   255

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

   256

   257

int __init init_mod(void)

   258

{

   259

    int ret = -1, i;

   260

    RTIME tick_period, requested_ticks, now;

   261

    ec_slave_config_t *sc;

   262

   263

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

   264

   265

    rt_sem_init(&master_sem, 1);

   266

   267

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

   268

   269

    master = ecrt_request_master(0);

   270

    if (!master) {

   271

        ret = -EBUSY; 

   272

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

   273

        goto out_return;

   274

    }

   275

   276

    ecrt_master_callbacks(master, request_lock_callback, release_lock_callback, master);

   277

   278

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

   279

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

   280

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

   281

        goto out_release_master;

   282

    }

   283

   284

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

   285

   286

    // create configuration for reference clock FIXME

   287

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

   288

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

   289

        goto out_release_master;

   290

    }

   291

   292

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

   293

        if (!(sc = ecrt_master_slave_config(master,

   294

                        DigOutSlavePos(i), Beckhoff_EL2008))) {

   295

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

   296

            goto out_release_master;

   297

        }

   298

   299

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

   300

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

   301

            goto out_release_master;

   302

        }

   303

   304

        off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,

   305

                0x7000, 1, domain1, NULL);

   306

   307

        if (off_dig_out[i] < 0)

   308

            goto out_release_master;

   309

    }

   310

   311

    if (!(sc = ecrt_master_slave_config(master,

   312

                    CounterSlavePos, IDS_Counter))) {

   313

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

   314

        goto out_release_master;

   315

    }

   316

    off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,

   317

            0x6020, 0x11, domain1, NULL);

   318

    if (off_counter_in < 0)

   319

        goto out_release_master;

   320

    off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,

   321

            0x7020, 1, domain1, NULL);

   322

    if (off_counter_out < 0)

   323

        goto out_release_master;

   324

   325

    // configure SYNC signals for this slave

   326

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

   327

   328

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

   329

    if (ecrt_master_activate(master)) {

   330

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

   331

        goto out_release_master;

   332

    }

   333

   334

    // Get internal process data for domain

   335

    domain1_pd = ecrt_domain_data(domain1);

   336

   337

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

   338

    requested_ticks = nano2count(TIMERTICKS);

   339

    tick_period = start_rt_timer(requested_ticks);

   340

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

   341

           (int) tick_period, (int) requested_ticks);

   342

   343

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

   344

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

   345

        goto out_stop_timer;

   346

    }

   347

   348

    now = rt_get_time();

   349

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

   350

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

   351

        goto out_stop_task;

   352

    }

   353

   354

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

   355

    return 0;

   356

   357

 out_stop_task:

   358

    rt_task_delete(&task);

   359

 out_stop_timer:

   360

    stop_rt_timer();

   361

 out_release_master:

   362

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

   363

    ecrt_release_master(master);

   364

 out_return:

   365

    rt_sem_delete(&master_sem);

   366

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

   367

    return ret;

   368

}

   369

   370

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

   371

   372

void __exit cleanup_mod(void)

   373

{

   374

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

   375

   376

    rt_task_delete(&task);

   377

    stop_rt_timer();

   378

    ecrt_release_master(master);

   379

    rt_sem_delete(&master_sem);

   380

   381

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

   382

}

   383

   384

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

   385

   386

MODULE_LICENSE("GPL");

   387

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

   388

MODULE_DESCRIPTION("EtherCAT distributed clocks sample module");

   389

   390

module_init(init_mod);

   391

module_exit(cleanup_mod);

   392

   393

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