1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

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

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

     9

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

    10

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

    11

 *  published by the Free Software Foundation.

    12

 *

    13

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

    14

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

    15

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

    16

 *  Public License for more details.

    17

 *

    18

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

    19

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

    20

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

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/** \file

    31

 *

    32

 * EtherCAT slave configuration state machine.

    33

 */

    34

    35

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

    36

    37

#include <asm/div64.h>

    38

    39

#include "globals.h"

    40

#include "master.h"

    41

#include "mailbox.h"

    42

#include "slave_config.h"

    43

#include "fsm_slave_config.h"

    44

    45

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

    46

    47

/** Time difference [ns] to tolerate without setting a new system time offset.

    48

 */

    49

#define EC_SYSTEM_TIME_TOLERANCE_NS 100000000

    50

    51

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

    52

    53

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    54

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    55

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    56

void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    62

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    68

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    72

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    73

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    74

    75

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    76

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    77

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    78

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    79

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    80

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    81

void ec_fsm_slave_config_enter_soe_conf(ec_fsm_slave_config_t *);

    82

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    83

void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);

    84

void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);

    85

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    86

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    87

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    88

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    89

    90

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    91

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    92

    93

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    94

    95

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

    96

    97

/** Constructor.

    98

 */

    99

void ec_fsm_slave_config_init(

   100

        ec_fsm_slave_config_t *fsm, /**< slave state machine */

   101

        ec_datagram_t *datagram, /**< datagram structure to use */

   102

        ec_fsm_change_t *fsm_change, /**< State change state machine to use. */

   103

        ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */

   104

        ec_fsm_pdo_t *fsm_pdo /**< PDO configuration state machine to use. */

   105

        )

   106

{

   107

    ec_sdo_request_init(&fsm->request_copy);

   108

    ec_soe_request_init(&fsm->soe_request_copy);

   109

   110

    fsm->datagram = datagram;

   111

    fsm->fsm_change = fsm_change;

   112

    fsm->fsm_coe = fsm_coe;

   113

    fsm->fsm_pdo = fsm_pdo;

   114

}

   115

   116

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

   117

   118

/** Destructor.

   119

 */

   120

void ec_fsm_slave_config_clear(

   121

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   122

        )

   123

{

   124

    ec_sdo_request_clear(&fsm->request_copy);

   125

    ec_soe_request_clear(&fsm->soe_request_copy);

   126

}

   127

   128

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

   129

   130

/** Start slave configuration state machine.

   131

 */

   132

void ec_fsm_slave_config_start(

   133

        ec_fsm_slave_config_t *fsm, /**< slave state machine */

   134

        ec_slave_t *slave /**< slave to configure */

   135

        )

   136

{

   137

    fsm->slave = slave;

   138

    fsm->state = ec_fsm_slave_config_state_start;

   139

}

   140

   141

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

   142

   143

/**

   144

 * \return false, if state machine has terminated

   145

 */

   146

int ec_fsm_slave_config_running(

   147

        const ec_fsm_slave_config_t *fsm /**< slave state machine */

   148

        )

   149

{

   150

    return fsm->state != ec_fsm_slave_config_state_end

   151

        && fsm->state != ec_fsm_slave_config_state_error;

   152

}

   153

   154

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

   155

   156

/** Executes the current state of the state machine.

   157

 *

   158

 * If the state machine's datagram is not sent or received yet, the execution

   159

 * of the state machine is delayed to the next cycle.

   160

 *

   161

 * \return false, if state machine has terminated

   162

 */

   163

int ec_fsm_slave_config_exec(

   164

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   165

        )

   166

{

   167

    if (fsm->datagram->state == EC_DATAGRAM_SENT

   168

        || fsm->datagram->state == EC_DATAGRAM_QUEUED) {

   169

        // datagram was not sent or received yet.

   170

        return ec_fsm_slave_config_running(fsm);

   171

    }

   172

   173

    fsm->state(fsm);

   174

    return ec_fsm_slave_config_running(fsm);

   175

}

   176

   177

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

   178

   179

/**

   180

 * \return true, if the state machine terminated gracefully

   181

 */

   182

int ec_fsm_slave_config_success(

   183

        const ec_fsm_slave_config_t *fsm /**< slave state machine */

   184

        )

   185

{

   186

    return fsm->state == ec_fsm_slave_config_state_end;

   187

}

   188

   189

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

   190

 * Slave configuration state machine

   191

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

   192

   193

/** Slave configuration state: START.

   194

 */

   195

void ec_fsm_slave_config_state_start(

   196

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   197

        )

   198

{

   199

    if (fsm->slave->master->debug_level) {

   200

        EC_DBG("Configuring slave %u...\n", fsm->slave->ring_position);

   201

    }

   202

   203

    ec_fsm_slave_config_enter_init(fsm);

   204

}

   205

   206

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

   207

   208

/** Start state change to INIT.

   209

 */

   210

void ec_fsm_slave_config_enter_init(

   211

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   212

        )

   213

{

   214

    ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);

   215

    ec_fsm_change_exec(fsm->fsm_change);

   216

    fsm->state = ec_fsm_slave_config_state_init;

   217

}

   218

   219

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

   220

   221

/** Slave configuration state: INIT.

   222

 */

   223

void ec_fsm_slave_config_state_init(

   224

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   225

        )

   226

{

   227

    ec_master_t *master = fsm->slave->master;

   228

    ec_slave_t *slave = fsm->slave;

   229

    ec_datagram_t *datagram = fsm->datagram;

   230

   231

    if (ec_fsm_change_exec(fsm->fsm_change)) return;

   232

   233

    if (!ec_fsm_change_success(fsm->fsm_change)) {

   234

        if (!fsm->fsm_change->spontaneous_change)

   235

            slave->error_flag = 1;

   236

        fsm->state = ec_fsm_slave_config_state_error;

   237

        return;

   238

    }

   239

   240

    if (master->debug_level) {

   241

        EC_DBG("Slave %u is now in INIT.\n", slave->ring_position);

   242

    }

   243

   244

    if (!slave->base_fmmu_count) { // skip FMMU configuration

   245

        ec_fsm_slave_config_enter_clear_sync(fsm);

   246

        return;

   247

    }

   248

   249

    if (master->debug_level)

   250

        EC_DBG("Clearing FMMU configurations of slave %u...\n",

   251

               slave->ring_position);

   252

   253

    // clear FMMU configurations

   254

    ec_datagram_fpwr(datagram, slave->station_address,

   255

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   256

    ec_datagram_zero(datagram);

   257

    fsm->retries = EC_FSM_RETRIES;

   258

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   259

}

   260

   261

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

   262

   263

/** Slave configuration state: CLEAR FMMU.

   264

 */

   265

void ec_fsm_slave_config_state_clear_fmmus(

   266

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   267

        )

   268

{

   269

    ec_datagram_t *datagram = fsm->datagram;

   270

   271

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   272

        return;

   273

   274

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   275

        fsm->state = ec_fsm_slave_config_state_error;

   276

        EC_ERR("Failed receive FMMU clearing datagram for slave %u.\n",

   277

               fsm->slave->ring_position);

   278

        return;

   279

    }

   280

   281

    if (datagram->working_counter != 1) {

   282

        fsm->slave->error_flag = 1;

   283

        fsm->state = ec_fsm_slave_config_state_error;

   284

        EC_ERR("Failed to clear FMMUs of slave %u: ",

   285

               fsm->slave->ring_position);

   286

        ec_datagram_print_wc_error(datagram);

   287

        return;

   288

    }

   289

   290

    ec_fsm_slave_config_enter_clear_sync(fsm);

   291

}

   292

   293

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

   294

   295

/** Clear the sync manager configurations.

   296

 */

   297

void ec_fsm_slave_config_enter_clear_sync(

   298

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   299

        )

   300

{

   301

    ec_slave_t *slave = fsm->slave;

   302

    ec_datagram_t *datagram = fsm->datagram;

   303

    size_t sync_size;

   304

   305

    if (!slave->base_sync_count) {

   306

        // no sync managers

   307

        ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   308

        return;

   309

    }

   310

   311

    if (slave->master->debug_level)

   312

        EC_DBG("Clearing sync manager configurations of slave %u...\n",

   313

                slave->ring_position);

   314

   315

    sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count;

   316

   317

    // clear sync manager configurations

   318

    ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size);

   319

    memset(datagram->data, 0x00, sync_size);

   320

    fsm->retries = EC_FSM_RETRIES;

   321

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   322

}

   323

   324

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

   325

   326

/** Slave configuration state: CLEAR SYNC.

   327

 */

   328

void ec_fsm_slave_config_state_clear_sync(

   329

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   330

        )

   331

{

   332

    ec_datagram_t *datagram = fsm->datagram;

   333

   334

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   335

        return;

   336

   337

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   338

        fsm->state = ec_fsm_slave_config_state_error;

   339

        EC_ERR("Failed receive sync manager clearing datagram"

   340

                " for slave %u.\n", fsm->slave->ring_position);

   341

        return;

   342

    }

   343

   344

    if (datagram->working_counter != 1) {

   345

        fsm->slave->error_flag = 1;

   346

        fsm->state = ec_fsm_slave_config_state_error;

   347

        EC_ERR("Failed to clear sync manager configurations of slave %u: ",

   348

               fsm->slave->ring_position);

   349

        ec_datagram_print_wc_error(datagram);

   350

        return;

   351

    }

   352

   353

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   354

}

   355

   356

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

   357

   358

/** Clear the DC assignment.

   359

 */

   360

void ec_fsm_slave_config_enter_dc_clear_assign(

   361

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   362

        )

   363

{

   364

    ec_slave_t *slave = fsm->slave;

   365

    ec_datagram_t *datagram = fsm->datagram;

   366

   367

    if (!slave->base_dc_supported || !slave->has_dc_system_time) {

   368

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   369

        return;

   370

    }

   371

   372

    if (slave->master->debug_level)

   373

        EC_DBG("Clearing DC assignment of slave %u...\n",

   374

                slave->ring_position);

   375

   376

    ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);

   377

    ec_datagram_zero(datagram);

   378

    fsm->retries = EC_FSM_RETRIES;

   379

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   380

}

   381

   382

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

   383

   384

/** Slave configuration state: CLEAR DC ASSIGN.

   385

 */

   386

void ec_fsm_slave_config_state_dc_clear_assign(

   387

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   388

        )

   389

{

   390

    ec_datagram_t *datagram = fsm->datagram;

   391

   392

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   393

        return;

   394

   395

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   396

        fsm->state = ec_fsm_slave_config_state_error;

   397

        EC_ERR("Failed receive DC assignment clearing datagram"

   398

                " for slave %u.\n", fsm->slave->ring_position);

   399

        return;

   400

    }

   401

   402

    if (fsm->slave->master->debug_level && datagram->working_counter != 1) {

   403

        // clearing the DC assignment does not succeed on simple slaves

   404

        EC_DBG("Failed to clear DC assignment of slave %u: ",

   405

               fsm->slave->ring_position);

   406

        ec_datagram_print_wc_error(datagram);

   407

    }

   408

   409

    // read DC system time (0x0910, 64 bit)

   410

    //                         gap (64 bit)

   411

    //     and time offset (0x0920, 64 bit)

   412

    ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24);

   413

    fsm->retries = EC_FSM_RETRIES;

   414

    fsm->state = ec_fsm_slave_config_state_dc_read_offset;

   415

}

   416

   417

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

   418

   419

#define ABS(X) ((X) >= 0 ? (X) : -(X))

   420

   421

/** Configure 32 bit time offset.

   422

 */

   423

u64 ec_fsm_slave_config_dc_offset32(

   424

        ec_fsm_slave_config_t *fsm, /**< slave state machine */

   425

        u64 system_time, /**< System time register. */

   426

        u64 old_offset, /**< Time offset register. */

   427

        unsigned long jiffies_since_read /**< Jiffies for correction. */

   428

        )

   429

{

   430

    ec_slave_t *slave = fsm->slave;

   431

    u32 correction, system_time32, old_offset32, new_offset;

   432

    s32 time_diff;

   433

   434

    system_time32 = (u32) system_time;

   435

    old_offset32 = (u32) old_offset;

   436

   437

    // correct read system time by elapsed time since read operation

   438

    correction = jiffies_since_read * 1000 / HZ * 1000000;

   439

    system_time32 += correction;

   440

    time_diff = (u32) slave->master->app_time - system_time32;

   441

   442

    if (slave->master->debug_level)

   443

        EC_DBG("Slave %u: system_time=%u (corrected with %u),"

   444

                " app_time=%u, diff=%i\n",

   445

                slave->ring_position, system_time32, correction,

   446

                (u32) slave->master->app_time, time_diff);

   447

   448

    if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {

   449

        new_offset = time_diff + old_offset32;

   450

        if (slave->master->debug_level)

   451

            EC_DBG("Slave %u: Setting time offset to %u (was %u)\n",

   452

                    slave->ring_position, new_offset, old_offset32);

   453

        return (u64) new_offset;

   454

    } else {

   455

        if (slave->master->debug_level)

   456

            EC_DBG("Slave %u: Not touching time offset.\n",

   457

                    slave->ring_position);

   458

        return old_offset;

   459

    }

   460

}

   461

   462

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

   463

   464

/** Configure 64 bit time offset.

   465

 */

   466

u64 ec_fsm_slave_config_dc_offset64(

   467

        ec_fsm_slave_config_t *fsm, /**< slave state machine */

   468

        u64 system_time, /**< System time register. */

   469

        u64 old_offset, /**< Time offset register. */

   470

        unsigned long jiffies_since_read /**< Jiffies for correction. */

   471

        )

   472

{

   473

    ec_slave_t *slave = fsm->slave;

   474

    u64 new_offset, correction;

   475

    s64 time_diff;

   476

   477

    // correct read system time by elapsed time since read operation

   478

    correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000;

   479

    system_time += correction;

   480

    time_diff = fsm->slave->master->app_time - system_time;

   481

   482

    if (slave->master->debug_level)

   483

        EC_DBG("Slave %u: system_time=%llu (corrected with %llu),"

   484

                " app_time=%llu, diff=%lli\n",

   485

                slave->ring_position, system_time, correction,

   486

                slave->master->app_time, time_diff);

   487

   488

   489

    if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {

   490

        new_offset = time_diff + old_offset;

   491

        if (slave->master->debug_level)

   492

            EC_DBG("Slave %u: Setting time offset to %llu (was %llu)\n",

   493

                    slave->ring_position, new_offset, old_offset);

   494

    } else {

   495

        new_offset = old_offset;

   496

        if (slave->master->debug_level)

   497

            EC_DBG("Slave %u: Not touching time offset.\n",

   498

                    slave->ring_position);

   499

    }