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_preop(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_soe_conf_safeop(ec_fsm_slave_config_t *);

    84

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    85

    86

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    87

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    88

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    89

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    90

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    91

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    92

void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *);

    93

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    94

void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);

    95

void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);

    96

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    97

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    98

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    99

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

   100

void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *);

   101

void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *);

   102

   103

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

   104

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

   105

   106

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

   107

   108

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

   109

   110

/** Constructor.

   111

 */

   112

void ec_fsm_slave_config_init(

   113

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

   114

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

   115

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

   116

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

   117

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

   118

        )

   119

{

   120

    ec_sdo_request_init(&fsm->request_copy);

   121

    ec_soe_request_init(&fsm->soe_request_copy);

   122

   123

    fsm->datagram = datagram;

   124

    fsm->fsm_change = fsm_change;

   125

    fsm->fsm_coe = fsm_coe;

   126

    fsm->fsm_pdo = fsm_pdo;

   127

}

   128

   129

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

   130

   131

/** Destructor.

   132

 */

   133

void ec_fsm_slave_config_clear(

   134

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

   135

        )

   136

{

   137

    ec_sdo_request_clear(&fsm->request_copy);

   138

    ec_soe_request_clear(&fsm->soe_request_copy);

   139

}

   140

   141

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

   142

   143

/** Start slave configuration state machine.

   144

 */

   145

void ec_fsm_slave_config_start(

   146

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

   147

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

   148

        )

   149

{

   150

    fsm->slave = slave;

   151

    fsm->state = ec_fsm_slave_config_state_start;

   152

}

   153

   154

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

   155

   156

/**

   157

 * \return false, if state machine has terminated

   158

 */

   159

int ec_fsm_slave_config_running(

   160

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

   161

        )

   162

{

   163

    return fsm->state != ec_fsm_slave_config_state_end

   164

        && fsm->state != ec_fsm_slave_config_state_error;

   165

}

   166

   167

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

   168

   169

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

   170

 *

   171

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

   172

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

   173

 *

   174

 * \return false, if state machine has terminated

   175

 */

   176

int ec_fsm_slave_config_exec(

   177

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

   178

        )

   179

{

   180

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

   181

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

   182

        // datagram was not sent or received yet.

   183

        return ec_fsm_slave_config_running(fsm);

   184

    }

   185

   186

    fsm->state(fsm);

   187

    return ec_fsm_slave_config_running(fsm);

   188

}

   189

   190

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

   191

   192

/**

   193

 * \return true, if the state machine terminated gracefully

   194

 */

   195

int ec_fsm_slave_config_success(

   196

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

   197

        )

   198

{

   199

    return fsm->state == ec_fsm_slave_config_state_end;

   200

}

   201

   202

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

   203

 * Slave configuration state machine

   204

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

   205

   206

/** Slave configuration state: START.

   207

 */

   208

void ec_fsm_slave_config_state_start(

   209

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

   210

        )

   211

{

   212

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

   213

    ec_fsm_slave_config_enter_init(fsm);

   214

}

   215

   216

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

   217

   218

/** Start state change to INIT.

   219

 */

   220

void ec_fsm_slave_config_enter_init(

   221

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

   222

        )

   223

{

   224

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

   225

    ec_fsm_change_exec(fsm->fsm_change);

   226

    fsm->state = ec_fsm_slave_config_state_init;

   227

}

   228

   229

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

   230

   231

/** Slave configuration state: INIT.

   232

 */

   233

void ec_fsm_slave_config_state_init(

   234

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

   235

        )

   236

{

   237

    ec_slave_t *slave = fsm->slave;

   238

    ec_datagram_t *datagram = fsm->datagram;

   239

   240

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

   241

   242

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

   243

        if (!fsm->fsm_change->spontaneous_change)

   244

            slave->error_flag = 1;

   245

        fsm->state = ec_fsm_slave_config_state_error;

   246

        return;

   247

    }

   248

   249

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

   250

   251

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

   252

        ec_fsm_slave_config_enter_clear_sync(fsm);

   253

        return;

   254

    }

   255

   256

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

   257

   258

    // clear FMMU configurations

   259

    ec_datagram_fpwr(datagram, slave->station_address,

   260

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   261

    ec_datagram_zero(datagram);

   262

    fsm->retries = EC_FSM_RETRIES;

   263

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   264

}

   265

   266

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

   267

   268

/** Slave configuration state: CLEAR FMMU.

   269

 */

   270

void ec_fsm_slave_config_state_clear_fmmus(

   271

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

   272

        )

   273

{

   274

    ec_datagram_t *datagram = fsm->datagram;

   275

   276

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

   277

        return;

   278

   279

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   280

        fsm->state = ec_fsm_slave_config_state_error;

   281

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

   282

        return;

   283

    }

   284

   285

    if (datagram->working_counter != 1) {

   286

        fsm->slave->error_flag = 1;

   287

        fsm->state = ec_fsm_slave_config_state_error;

   288

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

   289

        ec_datagram_print_wc_error(datagram);

   290

        return;

   291

    }

   292

   293

    ec_fsm_slave_config_enter_clear_sync(fsm);

   294

}

   295

   296

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

   297

   298

/** Clear the sync manager configurations.

   299

 */

   300

void ec_fsm_slave_config_enter_clear_sync(

   301

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

   302

        )

   303

{

   304

    ec_slave_t *slave = fsm->slave;

   305

    ec_datagram_t *datagram = fsm->datagram;

   306

    size_t sync_size;

   307

   308

    if (!slave->base_sync_count) {

   309

        // no sync managers

   310

        ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   311

        return;

   312

    }

   313

   314

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

   315

   316

    sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count;

   317

   318

    // clear sync manager configurations

   319

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

   320

    ec_datagram_zero(datagram);

   321

    fsm->retries = EC_FSM_RETRIES;

   322

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   323

}

   324

   325

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

   326

   327

/** Slave configuration state: CLEAR SYNC.

   328

 */

   329

void ec_fsm_slave_config_state_clear_sync(

   330

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

   331

        )

   332

{

   333

    ec_datagram_t *datagram = fsm->datagram;

   334

   335

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

   336

        return;

   337

   338

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   339

        fsm->state = ec_fsm_slave_config_state_error;

   340

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

   341

                " clearing datagram.\n");

   342

        return;

   343

    }

   344

   345

    if (datagram->working_counter != 1) {

   346

        fsm->slave->error_flag = 1;

   347

        fsm->state = ec_fsm_slave_config_state_error;

   348

        EC_SLAVE_ERR(fsm->slave,

   349

                "Failed to clear sync manager configurations: ");

   350

        ec_datagram_print_wc_error(datagram);

   351

        return;

   352

    }

   353

   354

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   355

}

   356

   357

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

   358

   359

/** Clear the DC assignment.

   360

 */

   361

void ec_fsm_slave_config_enter_dc_clear_assign(

   362

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

   363

        )

   364

{

   365

    ec_slave_t *slave = fsm->slave;

   366

    ec_datagram_t *datagram = fsm->datagram;

   367

   368

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

   369

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   370

        return;

   371

    }

   372

   373

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

   374

   375

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

   376

    ec_datagram_zero(datagram);

   377

    fsm->retries = EC_FSM_RETRIES;

   378

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   379

}

   380

   381

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

   382

   383

/** Slave configuration state: CLEAR DC ASSIGN.

   384

 */

   385

void ec_fsm_slave_config_state_dc_clear_assign(

   386

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

   387

        )

   388

{

   389

    ec_datagram_t *datagram = fsm->datagram;

   390

   391

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

   392

        return;

   393

   394

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   395

        fsm->state = ec_fsm_slave_config_state_error;

   396

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

   397

                " clearing datagram.\n");

   398

        return;

   399

    }

   400

   401

    if (datagram->working_counter != 1) {

   402

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

   403

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

   404

        ec_datagram_print_wc_error(datagram);

   405

    }

   406

   407

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   408

}

   409

   410

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

   411

   412

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

   413

 */

   414

void ec_fsm_slave_config_enter_mbox_sync(

   415

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

   416

        )

   417

{

   418

    ec_slave_t *slave = fsm->slave;

   419

    ec_datagram_t *datagram = fsm->datagram;

   420

    unsigned int i;

   421

   422

    // slave is now in INIT

   423

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

   424

        fsm->state = ec_fsm_slave_config_state_end; // successful

   425

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

   426

        return;

   427

    }

   428

   429

    if (!slave->sii.mailbox_protocols) {

   430

        // no mailbox protocols supported

   431

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

   432

                " mailbox communication.\n");

   433

        ec_fsm_slave_config_enter_boot_preop(fsm);

   434

        return;

   435

    }

   436

   437

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

   438

   439

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   440

        ec_sync_t sync;

   441

   442

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

   443

                EC_SYNC_PAGE_SIZE * 2);

   444

        ec_datagram_zero(datagram);

   445

   446

        ec_sync_init(&sync, slave);

   447

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   448

        sync.control_register = 0x26;

   449

        sync.enable = 1;

   450

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

   451

                EC_DIR_INVALID, // use default direction

   452

                datagram->data);

   453

        slave->configured_rx_mailbox_offset =

   454

            slave->sii.boot_rx_mailbox_offset;

   455

        slave->configured_rx_mailbox_size =

   456

            slave->sii.boot_rx_mailbox_size;

   457

   458

        ec_sync_init(&sync, slave);

   459

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   460

        sync.control_register = 0x22;

   461

        sync.enable = 1;

   462

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

   463

                EC_DIR_INVALID, // use default direction

   464

                datagram->data + EC_SYNC_PAGE_SIZE);

   465

        slave->configured_tx_mailbox_offset =

   466

            slave->sii.boot_tx_mailbox_offset;

   467

        slave->configured_tx_mailbox_size =

   468

            slave->sii.boot_tx_mailbox_size;

   469

   470

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

   471

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

   472

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   473

        ec_datagram_zero(datagram);

   474

   475

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

   476

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

   477

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

   478

                    NULL, // use default sync manager configuration

   479

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   480

        }

   481

   482

        slave->configured_rx_mailbox_offset =

   483

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

   484

        slave->configured_rx_mailbox_size =

   485

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

   486

        slave->configured_tx_mailbox_offset =

   487

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

   488

        slave->configured_tx_mailbox_size =

   489

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

   490

    } else { // no mailbox sync manager configurations provided

   491

        ec_sync_t sync;

   492

   493

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

   494

                " mailbox sync manager configurations.\n");

   495

   496

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

   497

                EC_SYNC_PAGE_SIZE * 2);

   498

        ec_datagram_zero(datagram);