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

/** Maximum clock difference (in ns) before going to SAFEOP.

    48

 *

    49

 * Wait for DC time difference to drop under this absolute value before

    50

 * requesting SAFEOP.

    51

 */

    52

#define EC_DC_MAX_SYNC_DIFF_NS 10000

    53

    54

/** Maximum time (in ms) to wait for clock discipline.

    55

 */

    56

#define EC_DC_SYNC_WAIT_MS 5000

    57

    58

/** Time offset (in ns), that is added to cyclic start time.

    59

 */

    60

#define EC_DC_START_OFFSET 100000000ULL

    61

    62

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

    63

    64

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);

    68

void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    70

#ifdef EC_SII_ASSIGN

    71

void ec_fsm_slave_config_state_assign_pdi(ec_fsm_slave_config_t *);

    72

#endif

    73

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    74

#ifdef EC_SII_ASSIGN

    75

void ec_fsm_slave_config_state_assign_ethercat(ec_fsm_slave_config_t *);

    76

#endif

    77

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    78

void ec_fsm_slave_config_state_soe_conf_preop(ec_fsm_slave_config_t *);

    79

void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);

    80

void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);

    81

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    82

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    83

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    84

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    85

void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *);

    86

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    87

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    88

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    89

void ec_fsm_slave_config_state_soe_conf_safeop(ec_fsm_slave_config_t *);

    90

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    91

    92

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    93

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    94

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    95

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    96

#ifdef EC_SII_ASSIGN

    97

void ec_fsm_slave_config_enter_assign_pdi(ec_fsm_slave_config_t *);

    98

#endif

    99

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

   100

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

   101

void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *);

   102

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

   103

void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);

   104

void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);

   105

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

   106

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

   107

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

   108

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

   109

void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *);

   110

void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *);

   111

   112

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

   113

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

   114

   115

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

   116

   117

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

   118

   119

/** Constructor.

   120

 */

   121

void ec_fsm_slave_config_init(

   122

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

   123

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

   124

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

   125

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

   126

        ec_fsm_soe_t *fsm_soe, /**< SoE state machine to use. */

   127

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

   128

        )

   129

{

   130

    ec_sdo_request_init(&fsm->request_copy);

   131

    ec_soe_request_init(&fsm->soe_request_copy);

   132

   133

    fsm->datagram = datagram;

   134

    fsm->fsm_change = fsm_change;

   135

    fsm->fsm_coe = fsm_coe;

   136

    fsm->fsm_soe = fsm_soe;

   137

    fsm->fsm_pdo = fsm_pdo;

   138

}

   139

   140

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

   141

   142

/** Destructor.

   143

 */

   144

void ec_fsm_slave_config_clear(

   145

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

   146

        )

   147

{

   148

    ec_sdo_request_clear(&fsm->request_copy);

   149

    ec_soe_request_clear(&fsm->soe_request_copy);

   150

}

   151

   152

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

   153

   154

/** Start slave configuration state machine.

   155

 */

   156

void ec_fsm_slave_config_start(

   157

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

   158

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

   159

        )

   160

{

   161

    fsm->slave = slave;

   162

    fsm->state = ec_fsm_slave_config_state_start;

   163

}

   164

   165

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

   166

   167

/**

   168

 * \return false, if state machine has terminated

   169

 */

   170

int ec_fsm_slave_config_running(

   171

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

   172

        )

   173

{

   174

    return fsm->state != ec_fsm_slave_config_state_end

   175

        && fsm->state != ec_fsm_slave_config_state_error;

   176

}

   177

   178

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

   179

   180

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

   181

 *

   182

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

   183

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

   184

 *

   185

 * \return false, if state machine has terminated

   186

 */

   187

int ec_fsm_slave_config_exec(

   188

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

   189

        )

   190

{

   191

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

   192

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

   193

        // datagram was not sent or received yet.

   194

        return ec_fsm_slave_config_running(fsm);

   195

    }

   196

   197

    fsm->state(fsm);

   198

    return ec_fsm_slave_config_running(fsm);

   199

}

   200

   201

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

   202

   203

/**

   204

 * \return true, if the state machine terminated gracefully

   205

 */

   206

int ec_fsm_slave_config_success(

   207

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

   208

        )

   209

{

   210

    return fsm->state == ec_fsm_slave_config_state_end;

   211

}

   212

   213

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

   214

 * Slave configuration state machine

   215

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

   216

   217

/** Slave configuration state: START.

   218

 */

   219

void ec_fsm_slave_config_state_start(

   220

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

   221

        )

   222

{

   223

    EC_SLAVE_DBG(fsm->slave, 1, "Configuring...\n");

   224

    ec_fsm_slave_config_enter_init(fsm);

   225

}

   226

   227

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

   228

   229

/** Start state change to INIT.

   230

 */

   231

void ec_fsm_slave_config_enter_init(

   232

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

   233

        )

   234

{

   235

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

   236

    ec_fsm_change_exec(fsm->fsm_change);

   237

    fsm->state = ec_fsm_slave_config_state_init;

   238

}

   239

   240

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

   241

   242

/** Slave configuration state: INIT.

   243

 */

   244

void ec_fsm_slave_config_state_init(

   245

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

   246

        )

   247

{

   248

    ec_slave_t *slave = fsm->slave;

   249

    ec_datagram_t *datagram = fsm->datagram;

   250

   251

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

   252

   253

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

   254

        if (!fsm->fsm_change->spontaneous_change)

   255

            slave->error_flag = 1;

   256

        fsm->state = ec_fsm_slave_config_state_error;

   257

        return;

   258

    }

   259

   260

    EC_SLAVE_DBG(slave, 1, "Now in INIT.\n");

   261

   262

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

   263

        ec_fsm_slave_config_enter_clear_sync(fsm);

   264

        return;

   265

    }

   266

   267

    EC_SLAVE_DBG(slave, 1, "Clearing FMMU configurations...\n");

   268

   269

    // clear FMMU configurations

   270

    ec_datagram_fpwr(datagram, slave->station_address,

   271

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   272

    ec_datagram_zero(datagram);

   273

    fsm->retries = EC_FSM_RETRIES;

   274

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   275

}

   276

   277

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

   278

   279

/** Slave configuration state: CLEAR FMMU.

   280

 */

   281

void ec_fsm_slave_config_state_clear_fmmus(

   282

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

   283

        )

   284

{

   285

    ec_datagram_t *datagram = fsm->datagram;

   286

   287

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

   288

        return;

   289

   290

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   291

        fsm->state = ec_fsm_slave_config_state_error;

   292

        EC_SLAVE_ERR(fsm->slave, "Failed receive FMMU clearing datagram.\n");

   293

        return;

   294

    }

   295

   296

    if (datagram->working_counter != 1) {

   297

        fsm->slave->error_flag = 1;

   298

        fsm->state = ec_fsm_slave_config_state_error;

   299

        EC_SLAVE_ERR(fsm->slave, "Failed to clear FMMUs: ");

   300

        ec_datagram_print_wc_error(datagram);

   301

        return;

   302

    }

   303

   304

    ec_fsm_slave_config_enter_clear_sync(fsm);

   305

}

   306

   307

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

   308

   309

/** Clear the sync manager configurations.

   310

 */

   311

void ec_fsm_slave_config_enter_clear_sync(

   312

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

   313

        )

   314

{

   315

    ec_slave_t *slave = fsm->slave;

   316

    ec_datagram_t *datagram = fsm->datagram;

   317

    size_t sync_size;

   318

   319

    if (!slave->base_sync_count) {

   320

        // no sync managers

   321

        ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   322

        return;

   323

    }

   324

   325

    EC_SLAVE_DBG(slave, 1, "Clearing sync manager configurations...\n");

   326

   327

    sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count;

   328

   329

    // clear sync manager configurations

   330

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

   331

    ec_datagram_zero(datagram);

   332

    fsm->retries = EC_FSM_RETRIES;

   333

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   334

}

   335

   336

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

   337

   338

/** Slave configuration state: CLEAR SYNC.

   339

 */

   340

void ec_fsm_slave_config_state_clear_sync(

   341

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

   342

        )

   343

{

   344

    ec_datagram_t *datagram = fsm->datagram;

   345

   346

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

   347

        return;

   348

   349

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   350

        fsm->state = ec_fsm_slave_config_state_error;

   351

        EC_SLAVE_ERR(fsm->slave, "Failed receive sync manager"

   352

                " clearing datagram.\n");

   353

        return;

   354

    }

   355

   356

    if (datagram->working_counter != 1) {

   357

        fsm->slave->error_flag = 1;

   358

        fsm->state = ec_fsm_slave_config_state_error;

   359

        EC_SLAVE_ERR(fsm->slave,

   360

                "Failed to clear sync manager configurations: ");

   361

        ec_datagram_print_wc_error(datagram);

   362

        return;

   363

    }

   364

   365

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   366

}

   367

   368

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

   369

   370

/** Clear the DC assignment.

   371

 */

   372

void ec_fsm_slave_config_enter_dc_clear_assign(

   373

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

   374

        )

   375

{

   376

    ec_slave_t *slave = fsm->slave;

   377

    ec_datagram_t *datagram = fsm->datagram;

   378

   379

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

   380

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   381

        return;

   382

    }

   383

   384

    EC_SLAVE_DBG(slave, 1, "Clearing DC assignment...\n");

   385

   386

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

   387

    ec_datagram_zero(datagram);

   388

    fsm->retries = EC_FSM_RETRIES;

   389

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   390

}

   391

   392

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

   393

   394

/** Slave configuration state: CLEAR DC ASSIGN.

   395

 */

   396

void ec_fsm_slave_config_state_dc_clear_assign(

   397

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

   398

        )

   399

{

   400

    ec_datagram_t *datagram = fsm->datagram;

   401

   402

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

   403

        return;

   404

   405

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   406

        fsm->state = ec_fsm_slave_config_state_error;

   407

        EC_SLAVE_ERR(fsm->slave, "Failed receive DC assignment"

   408

                " clearing datagram.\n");

   409

        return;

   410

    }

   411

   412

    if (datagram->working_counter != 1) {

   413

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

   414

        EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: ");

   415

        ec_datagram_print_wc_error(datagram);

   416

    }

   417

   418

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   419

}

   420

   421

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

   422

   423

/** Check for mailbox sync managers to be configured.

   424

 */

   425

void ec_fsm_slave_config_enter_mbox_sync(

   426

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

   427

        )

   428

{

   429

    ec_slave_t *slave = fsm->slave;

   430

    ec_datagram_t *datagram = fsm->datagram;

   431

    unsigned int i;

   432

   433

    // slave is now in INIT

   434

    if (slave->current_state == slave->requested_state) {

   435

        fsm->state = ec_fsm_slave_config_state_end; // successful

   436

        EC_SLAVE_DBG(slave, 1, "Finished configuration.\n");

   437

        return;

   438

    }

   439

   440

    if (!slave->sii.mailbox_protocols) {

   441

        // no mailbox protocols supported

   442

        EC_SLAVE_DBG(slave, 1, "Slave does not support"

   443

                " mailbox communication.\n");

   444

#ifdef EC_SII_ASSIGN

   445

        ec_fsm_slave_config_enter_assign_pdi(fsm);

   446

#else

   447

        ec_fsm_slave_config_enter_boot_preop(fsm);

   448

#endif

   449

        return;

   450

    }

   451

   452

    EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n");

   453

   454

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   455

        ec_sync_t sync;

   456

   457

        ec_datagram_fpwr(datagram, slave->station_address, 0x0800,

   458

                EC_SYNC_PAGE_SIZE * 2);

   459

        ec_datagram_zero(datagram);

   460

   461

        ec_sync_init(&sync, slave);

   462

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   463

        sync.control_register = 0x26;

   464

        sync.enable = 1;

   465

        ec_sync_page(&sync, 0, slave->sii.boot_rx_mailbox_size,

   466

                EC_DIR_INVALID, // use default direction

   467

                0, // no PDO xfer

   468

                datagram->data);

   469

        slave->configured_rx_mailbox_offset =

   470

            slave->sii.boot_rx_mailbox_offset;

   471

        slave->configured_rx_mailbox_size =

   472

            slave->sii.boot_rx_mailbox_size;

   473

   474

        ec_sync_init(&sync, slave);

   475

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   476

        sync.control_register = 0x22;

   477

        sync.enable = 1;

   478

        ec_sync_page(&sync, 1, slave->sii.boot_tx_mailbox_size,

   479

                EC_DIR_INVALID, // use default direction

   480

                0, // no PDO xfer

   481

                datagram->data + EC_SYNC_PAGE_SIZE);

   482

        slave->configured_tx_mailbox_offset =

   483

            slave->sii.boot_tx_mailbox_offset;

   484

        slave->configured_tx_mailbox_size =

   485

            slave->sii.boot_tx_mailbox_size;

   486

   487

    } else if (slave->sii.sync_count >= 2) { // mailbox configuration provided

   488

        ec_datagram_fpwr(datagram, slave->station_address, 0x0800,

   489

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   490

        ec_datagram_zero(datagram);

   491

   492

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

   493

            ec_sync_page(&slave->sii.syncs[i], i,

   494

                    slave->sii.syncs[i].default_length,

   495

                    NULL, // use default sync manager configuration

   496

                    0, // no PDO xfer

   497