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 5000

    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

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    72

void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *);

    73

void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);

    74

void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);

    75

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    76

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    77

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    78

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    79

void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *);

    80

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    81

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    82

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    83

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    84

    85

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    86

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    87

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    88

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    89

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    90

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    91

void ec_fsm_slave_config_enter_soe_conf(ec_fsm_slave_config_t *);

    92

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    93

void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);

    94

void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);

    95

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    96

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    97

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    98

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    99

   100

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

   101

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

   102

   103

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

   104

   105

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

   106

   107

/** Constructor.

   108

 */

   109

void ec_fsm_slave_config_init(

   110

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

   111

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

   112

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

   113

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

   114

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

   115

        )

   116

{

   117

    ec_sdo_request_init(&fsm->request_copy);

   118

    ec_soe_request_init(&fsm->soe_request_copy);

   119

   120

    fsm->datagram = datagram;

   121

    fsm->fsm_change = fsm_change;

   122

    fsm->fsm_coe = fsm_coe;

   123

    fsm->fsm_pdo = fsm_pdo;

   124

}

   125

   126

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

   127

   128

/** Destructor.

   129

 */

   130

void ec_fsm_slave_config_clear(

   131

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

   132

        )

   133

{

   134

    ec_sdo_request_clear(&fsm->request_copy);

   135

    ec_soe_request_clear(&fsm->soe_request_copy);

   136

}

   137

   138

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

   139

   140

/** Start slave configuration state machine.

   141

 */

   142

void ec_fsm_slave_config_start(

   143

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

   144

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

   145

        )

   146

{

   147

    fsm->slave = slave;

   148

    fsm->state = ec_fsm_slave_config_state_start;

   149

}

   150

   151

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

   152

   153

/**

   154

 * \return false, if state machine has terminated

   155

 */

   156

int ec_fsm_slave_config_running(

   157

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

   158

        )

   159

{

   160

    return fsm->state != ec_fsm_slave_config_state_end

   161

        && fsm->state != ec_fsm_slave_config_state_error;

   162

}

   163

   164

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

   165

   166

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

   167

 *

   168

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

   169

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

   170

 *

   171

 * \return false, if state machine has terminated

   172

 */

   173

int ec_fsm_slave_config_exec(

   174

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

   175

        )

   176

{

   177

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

   178

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

   179

        // datagram was not sent or received yet.

   180

        return ec_fsm_slave_config_running(fsm);

   181

    }

   182

   183

    fsm->state(fsm);

   184

    return ec_fsm_slave_config_running(fsm);

   185

}

   186

   187

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

   188

   189

/**

   190

 * \return true, if the state machine terminated gracefully

   191

 */

   192

int ec_fsm_slave_config_success(

   193

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

   194

        )

   195

{

   196

    return fsm->state == ec_fsm_slave_config_state_end;

   197

}

   198

   199

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

   200

 * Slave configuration state machine

   201

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

   202

   203

/** Slave configuration state: START.

   204

 */

   205

void ec_fsm_slave_config_state_start(

   206

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

   207

        )

   208

{

   209

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

   210

    ec_fsm_slave_config_enter_init(fsm);

   211

}

   212

   213

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

   214

   215

/** Start state change to INIT.

   216

 */

   217

void ec_fsm_slave_config_enter_init(

   218

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

   219

        )

   220

{

   221

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

   222

    ec_fsm_change_exec(fsm->fsm_change);

   223

    fsm->state = ec_fsm_slave_config_state_init;

   224

}

   225

   226

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

   227

   228

/** Slave configuration state: INIT.

   229

 */

   230

void ec_fsm_slave_config_state_init(

   231

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

   232

        )

   233

{

   234

    ec_slave_t *slave = fsm->slave;

   235

    ec_datagram_t *datagram = fsm->datagram;

   236

   237

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

   238

   239

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

   240

        if (!fsm->fsm_change->spontaneous_change)

   241

            slave->error_flag = 1;

   242

        fsm->state = ec_fsm_slave_config_state_error;

   243

        return;

   244

    }

   245

   246

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

   247

   248

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

   249

        ec_fsm_slave_config_enter_clear_sync(fsm);

   250

        return;

   251

    }

   252

   253

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

   254

   255

    // clear FMMU configurations

   256

    ec_datagram_fpwr(datagram, slave->station_address,

   257

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   258

    ec_datagram_zero(datagram);

   259

    fsm->retries = EC_FSM_RETRIES;

   260

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   261

}

   262

   263

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

   264

   265

/** Slave configuration state: CLEAR FMMU.

   266

 */

   267

void ec_fsm_slave_config_state_clear_fmmus(

   268

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

   269

        )

   270

{

   271

    ec_datagram_t *datagram = fsm->datagram;

   272

   273

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

   274

        return;

   275

   276

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   277

        fsm->state = ec_fsm_slave_config_state_error;

   278

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

   279

        return;

   280

    }

   281

   282

    if (datagram->working_counter != 1) {

   283

        fsm->slave->error_flag = 1;

   284

        fsm->state = ec_fsm_slave_config_state_error;

   285

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

   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

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

   312

   313

    sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count;

   314

   315

    // clear sync manager configurations

   316

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

   317

    ec_datagram_zero(datagram);

   318

    fsm->retries = EC_FSM_RETRIES;

   319

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   320

}

   321

   322

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

   323

   324

/** Slave configuration state: CLEAR SYNC.

   325

 */

   326

void ec_fsm_slave_config_state_clear_sync(

   327

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

   328

        )

   329

{

   330

    ec_datagram_t *datagram = fsm->datagram;

   331

   332

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

   333

        return;

   334

   335

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   336

        fsm->state = ec_fsm_slave_config_state_error;

   337

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

   338

                " clearing datagram.\n");

   339

        return;

   340

    }

   341

   342

    if (datagram->working_counter != 1) {

   343

        fsm->slave->error_flag = 1;

   344

        fsm->state = ec_fsm_slave_config_state_error;

   345

        EC_SLAVE_ERR(fsm->slave,

   346

                "Failed to clear sync manager configurations: ");

   347

        ec_datagram_print_wc_error(datagram);

   348

        return;

   349

    }

   350

   351

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   352

}

   353

   354

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

   355

   356

/** Clear the DC assignment.

   357

 */

   358

void ec_fsm_slave_config_enter_dc_clear_assign(

   359

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

   360

        )

   361

{

   362

    ec_slave_t *slave = fsm->slave;

   363

    ec_datagram_t *datagram = fsm->datagram;

   364

   365

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

   366

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   367

        return;

   368

    }

   369

   370

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

   371

   372

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

   373

    ec_datagram_zero(datagram);

   374

    fsm->retries = EC_FSM_RETRIES;

   375

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   376

}

   377

   378

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

   379

   380

/** Slave configuration state: CLEAR DC ASSIGN.

   381

 */

   382

void ec_fsm_slave_config_state_dc_clear_assign(

   383

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

   384

        )

   385

{

   386

    ec_datagram_t *datagram = fsm->datagram;

   387

   388

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

   389

        return;

   390

   391

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   392

        fsm->state = ec_fsm_slave_config_state_error;

   393

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

   394

                " clearing datagram.\n");

   395

        return;

   396

    }

   397

   398

    if (datagram->working_counter != 1) {

   399

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

   400

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

   401

        ec_datagram_print_wc_error(datagram);

   402

    }

   403

   404

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   405

}

   406

   407

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

   408

   409

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

   410

 */

   411

void ec_fsm_slave_config_enter_mbox_sync(

   412

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

   413

        )

   414

{

   415

    ec_slave_t *slave = fsm->slave;

   416

    ec_datagram_t *datagram = fsm->datagram;

   417

    unsigned int i;

   418

   419

    // slave is now in INIT

   420

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

   421

        fsm->state = ec_fsm_slave_config_state_end; // successful

   422

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

   423

        return;

   424

    }

   425

   426

    if (!slave->sii.mailbox_protocols) {

   427

        // no mailbox protocols supported

   428

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

   429

                " mailbox communication.\n");

   430

        ec_fsm_slave_config_enter_boot_preop(fsm);

   431

        return;

   432

    }

   433

   434

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

   435

   436

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   437

        ec_sync_t sync;

   438

   439

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

   440

                EC_SYNC_PAGE_SIZE * 2);

   441

        ec_datagram_zero(datagram);

   442

   443

        ec_sync_init(&sync, slave);

   444

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   445

        sync.control_register = 0x26;

   446

        sync.enable = 1;

   447

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

   448

                EC_DIR_INVALID, // use default direction

   449

                datagram->data);

   450

        slave->configured_rx_mailbox_offset =

   451

            slave->sii.boot_rx_mailbox_offset;

   452

        slave->configured_rx_mailbox_size =

   453

            slave->sii.boot_rx_mailbox_size;

   454

   455

        ec_sync_init(&sync, slave);

   456

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   457

        sync.control_register = 0x22;

   458

        sync.enable = 1;

   459

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

   460

                EC_DIR_INVALID, // use default direction

   461

                datagram->data + EC_SYNC_PAGE_SIZE);

   462

        slave->configured_tx_mailbox_offset =

   463

            slave->sii.boot_tx_mailbox_offset;

   464

        slave->configured_tx_mailbox_size =

   465

            slave->sii.boot_tx_mailbox_size;

   466

   467

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

   468

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

   469

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   470

        ec_datagram_zero(datagram);

   471

   472

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

   473

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

   474

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

   475

                    NULL, // use default sync manager configuration

   476

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   477

        }

   478

   479

        slave->configured_rx_mailbox_offset =

   480

            slave->sii.syncs[0].physical_start_address;

   481

        slave->configured_rx_mailbox_size =

   482

            slave->sii.syncs[0].default_length;

   483

        slave->configured_tx_mailbox_offset =

   484

            slave->sii.syncs[1].physical_start_address;

   485

        slave->configured_tx_mailbox_size =

   486

            slave->sii.syncs[1].default_length;

   487

    } else { // no mailbox sync manager configurations provided

   488

        ec_sync_t sync;

   489

   490

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

   491

                " mailbox sync manager configurations.\n");

   492

   493

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

   494

                EC_SYNC_PAGE_SIZE * 2);

   495

        ec_datagram_zero(datagram);

   496

   497

        ec_sync_init(&sync, slave);

   498

        sync.physical_start_address = slave->sii.std_rx_mailbox_offset;