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

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

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/** \file

    28

 *

    29

 * EtherCAT slave configuration state machine.

    30

 */

    31

    32

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

    33

    34

#include "globals.h"

    35

#include "master.h"

    36

#include "mailbox.h"

    37

#include "slave_config.h"

    38

#include "fsm_slave_config.h"

    39

    40

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

    41

    42

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    43

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    44

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    45

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    46

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    47

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    48

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    49

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    50

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    51

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    52

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    53

    54

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    55

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    56

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    62

    63

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    65

    66

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    67

    68

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

    69

    70

/** Constructor.

    71

 */

    72

void ec_fsm_slave_config_init(

    73

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

    74

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

    75

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

    76

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

    77

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

    78

        )

    79

{

    80

    ec_sdo_request_init(&fsm->request_copy);

    81

    82

    fsm->datagram = datagram;

    83

    fsm->fsm_change = fsm_change;

    84

    fsm->fsm_coe = fsm_coe;

    85

    fsm->fsm_pdo = fsm_pdo;

    86

}

    87

    88

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

    89

    90

/** Destructor.

    91

 */

    92

void ec_fsm_slave_config_clear(

    93

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

    94

        )

    95

{

    96

    ec_sdo_request_clear(&fsm->request_copy);

    97

}

    98

    99

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

   100

   101

/** Start slave configuration state machine.

   102

 */

   103

void ec_fsm_slave_config_start(

   104

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

   105

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

   106

        )

   107

{

   108

    fsm->slave = slave;

   109

    fsm->state = ec_fsm_slave_config_state_start;

   110

}

   111

   112

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

   113

   114

/**

   115

 * \return false, if state machine has terminated

   116

 */

   117

int ec_fsm_slave_config_running(

   118

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

   119

        )

   120

{

   121

    return fsm->state != ec_fsm_slave_config_state_end

   122

        && fsm->state != ec_fsm_slave_config_state_error;

   123

}

   124

   125

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

   126

   127

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

   128

 *

   129

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

   130

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

   131

 *

   132

 * \return false, if state machine has terminated

   133

 */

   134

int ec_fsm_slave_config_exec(

   135

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

   136

        )

   137

{

   138

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

   139

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

   140

        // datagram was not sent or received yet.

   141

        return ec_fsm_slave_config_running(fsm);

   142

    }

   143

   144

    fsm->state(fsm);

   145

    return ec_fsm_slave_config_running(fsm);

   146

}

   147

   148

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

   149

   150

/**

   151

 * \return true, if the state machine terminated gracefully

   152

 */

   153

int ec_fsm_slave_config_success(

   154

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

   155

        )

   156

{

   157

    return fsm->state == ec_fsm_slave_config_state_end;

   158

}

   159

   160

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

   161

 * Slave configuration state machine

   162

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

   163

   164

/** Slave configuration state: START.

   165

 */

   166

void ec_fsm_slave_config_state_start(

   167

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

   168

        )

   169

{

   170

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

   171

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

   172

    }

   173

   174

    ec_fsm_slave_config_enter_init(fsm);

   175

}

   176

   177

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

   178

   179

/** Start state change to INIT.

   180

 */

   181

void ec_fsm_slave_config_enter_init(

   182

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

   183

        )

   184

{

   185

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

   186

    ec_fsm_change_exec(fsm->fsm_change);

   187

    fsm->state = ec_fsm_slave_config_state_init;

   188

}

   189

   190

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

   191

   192

/** Slave configuration state: INIT.

   193

 */

   194

void ec_fsm_slave_config_state_init(

   195

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

   196

        )

   197

{

   198

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

   199

    ec_slave_t *slave = fsm->slave;

   200

    ec_datagram_t *datagram = fsm->datagram;

   201

   202

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

   203

   204

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

   205

        if (!fsm->fsm_change->spontaneous_change)

   206

            slave->error_flag = 1;

   207

        fsm->state = ec_fsm_slave_config_state_error;

   208

        return;

   209

    }

   210

   211

    if (master->debug_level) {

   212

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

   213

    }

   214

   215

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

   216

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   217

        return;

   218

    }

   219

   220

    if (master->debug_level)

   221

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

   222

               slave->ring_position);

   223

   224

    // clear FMMU configurations

   225

    ec_datagram_fpwr(datagram, slave->station_address,

   226

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   227

    ec_datagram_zero(datagram);

   228

    fsm->retries = EC_FSM_RETRIES;

   229

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   230

}

   231

   232

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

   233

   234

/** Slave configuration state: CLEAR FMMU.

   235

 */

   236

void ec_fsm_slave_config_state_clear_fmmus(

   237

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

   238

        )

   239

{

   240

    ec_datagram_t *datagram = fsm->datagram;

   241

   242

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

   243

        return;

   244

   245

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   246

        fsm->state = ec_fsm_slave_config_state_error;

   247

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

   248

               fsm->slave->ring_position);

   249

        return;

   250

    }

   251

   252

    if (datagram->working_counter != 1) {

   253

        fsm->slave->error_flag = 1;

   254

        fsm->state = ec_fsm_slave_config_state_error;

   255

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

   256

               fsm->slave->ring_position);

   257

        ec_datagram_print_wc_error(datagram);

   258

        return;

   259

    }

   260

   261

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   262

}

   263

   264

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

   265

   266

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

   267

 */

   268

void ec_fsm_slave_config_enter_mbox_sync(

   269

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

   270

        )

   271

{

   272

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

   273

    ec_slave_t *slave = fsm->slave;

   274

    ec_datagram_t *datagram = fsm->datagram;

   275

    unsigned int i;

   276

   277

    // slave is now in INIT

   278

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

   279

        fsm->state = ec_fsm_slave_config_state_end; // successful

   280

        if (master->debug_level) {

   281

            EC_DBG("Finished configuration of slave %u.\n",

   282

                   slave->ring_position);

   283

        }

   284

        return;

   285

    }

   286

   287

    if (!slave->sii.mailbox_protocols) {

   288

        // no mailbox protocols supported

   289

        if (master->debug_level)

   290

            EC_DBG("Slave %u does not support mailbox communication.\n",

   291

                    slave->ring_position);

   292

        ec_fsm_slave_config_enter_boot_preop(fsm);

   293

        return;

   294

    }

   295

   296

    if (master->debug_level) {

   297

        EC_DBG("Configuring mailbox sync managers of slave %u.\n",

   298

               slave->ring_position);

   299

    }

   300

   301

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   302

        ec_sync_t sync;

   303

   304

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

   305

                EC_SYNC_PAGE_SIZE * 2);

   306

        memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * 2);

   307

   308

        ec_sync_init(&sync, slave);

   309

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   310

        sync.control_register = 0x26;

   311

        sync.enable = 1;

   312

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

   313

                EC_DIR_INVALID, // use default direction

   314

                datagram->data);

   315

        slave->configured_rx_mailbox_offset =

   316

            slave->sii.boot_rx_mailbox_offset;

   317

        slave->configured_rx_mailbox_size =

   318

            slave->sii.boot_rx_mailbox_size;

   319

   320

        ec_sync_init(&sync, slave);

   321

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   322

        sync.control_register = 0x22;

   323

        sync.enable = 1;

   324

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

   325

                EC_DIR_INVALID, // use default direction

   326

                datagram->data + EC_SYNC_PAGE_SIZE);

   327

        slave->configured_tx_mailbox_offset =

   328

            slave->sii.boot_tx_mailbox_offset;

   329

        slave->configured_tx_mailbox_size =

   330

            slave->sii.boot_tx_mailbox_size;

   331

   332

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

   333

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

   334

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   335

        ec_datagram_zero(datagram);

   336

   337

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

   338

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

   339

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

   340

                    EC_DIR_INVALID, // use default direction

   341

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   342

        }

   343

   344

        slave->configured_rx_mailbox_offset =

   345

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

   346

        slave->configured_rx_mailbox_size =

   347

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

   348

        slave->configured_tx_mailbox_offset =

   349

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

   350

        slave->configured_tx_mailbox_size =

   351

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

   352

    } else { // no mailbox sync manager configurations provided

   353

        ec_sync_t sync;

   354

   355

        if (master->debug_level)

   356

            EC_DBG("Slave %u does not provide"

   357

                    " mailbox sync manager configurations.\n",

   358

                    slave->ring_position);

   359

   360

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

   361

                EC_SYNC_PAGE_SIZE * 2);

   362

        ec_datagram_zero(datagram);

   363

   364

        ec_sync_init(&sync, slave);

   365

        sync.physical_start_address = slave->sii.std_rx_mailbox_offset;

   366

        sync.control_register = 0x26;

   367

        sync.enable = 1;

   368

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

   369

                EC_DIR_INVALID, // use default direction

   370

                datagram->data);

   371

        slave->configured_rx_mailbox_offset =

   372

            slave->sii.std_rx_mailbox_offset;

   373

        slave->configured_rx_mailbox_size =

   374

            slave->sii.std_rx_mailbox_size;

   375

   376

        ec_sync_init(&sync, slave);

   377

        sync.physical_start_address = slave->sii.std_tx_mailbox_offset;

   378

        sync.control_register = 0x22;

   379

        sync.enable = 1;

   380

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

   381

                EC_DIR_INVALID, // use default direction

   382

                datagram->data + EC_SYNC_PAGE_SIZE);

   383

        slave->configured_tx_mailbox_offset =

   384

            slave->sii.boot_tx_mailbox_offset;

   385

        slave->configured_tx_mailbox_size =

   386

            slave->sii.boot_tx_mailbox_size;

   387

    }

   388

   389

    fsm->retries = EC_FSM_RETRIES;

   390

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   391

}

   392

   393

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

   394

   395

/** Slave configuration state: SYNC.

   396

 */

   397

void ec_fsm_slave_config_state_mbox_sync(

   398

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

   399

        )

   400

{

   401

    ec_datagram_t *datagram = fsm->datagram;

   402

    ec_slave_t *slave = fsm->slave;

   403

   404

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

   405

        return;

   406

   407

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   408

        fsm->state = ec_fsm_slave_config_state_error;

   409

        EC_ERR("Failed to receive sync manager configuration datagram for"

   410

               " slave %u (datagram state %u).\n",

   411

               slave->ring_position, datagram->state);

   412

        return;

   413

    }

   414

   415

    if (datagram->working_counter != 1) {

   416

        slave->error_flag = 1;

   417

        fsm->state = ec_fsm_slave_config_state_error;

   418

        EC_ERR("Failed to set sync managers of slave %u: ",

   419

               slave->ring_position);

   420

        ec_datagram_print_wc_error(datagram);

   421

        return;

   422

    }

   423

   424

    ec_fsm_slave_config_enter_boot_preop(fsm);

   425

}

   426

   427

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

   428

   429

/** Request PREOP state.

   430

 */

   431

void ec_fsm_slave_config_enter_boot_preop(

   432

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

   433

        )

   434

{

   435

    fsm->state = ec_fsm_slave_config_state_boot_preop;

   436

   437

    if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) {

   438

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

   439

    } else { // BOOT

   440

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

   441

    }

   442

   443

    ec_fsm_change_exec(fsm->fsm_change); // execute immediately

   444

}

   445

   446

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

   447

   448

/** Slave configuration state: BOOT/PREOP.

   449

 */

   450

void ec_fsm_slave_config_state_boot_preop(

   451

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

   452

        )

   453

{

   454

    ec_slave_t *slave = fsm->slave;

   455

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

   456

   457

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

   458

   459

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

   460

        if (!fsm->fsm_change->spontaneous_change)

   461

            slave->error_flag = 1;

   462

        fsm->state = ec_fsm_slave_config_state_error;

   463

        return;

   464

    }

   465

   466

    // slave is now in BOOT/PREOP

   467

    slave->jiffies_preop = fsm->datagram->jiffies_received;

   468

   469

    if (master->debug_level) {

   470

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

   471

                slave->requested_state != EC_SLAVE_STATE_BOOT

   472

                ? "PREOP" : "BOOT");

   473

    }

   474

   475

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

   476

        fsm->state = ec_fsm_slave_config_state_end; // successful

   477

        if (master->debug_level) {

   478

            EC_DBG("Finished configuration of slave %u.\n",

   479

                   slave->ring_position);

   480

        }

   481

        return;

   482

    }

   483

   484

    if (!slave->config) {

   485

        EC_DBG("Slave %u is not configured.\n", slave->ring_position);

   486

        ec_fsm_slave_config_enter_safeop(fsm);

   487

        return;

   488

    }

   489

   490

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   491

}

   492

   493

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

   494

   495

/** Check for SDO configurations to be applied.

   496

 */

   497

void ec_fsm_slave_config_enter_sdo_conf(

   498

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

   499

        )

   500

{

   501

    ec_slave_t *slave = fsm->slave;

   502

   503

    // No CoE configuration to be applied?

   504

    if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration

   505

        ec_fsm_slave_config_enter_pdo_conf(fsm);