1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

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

    15

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

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

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

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT slave configuration state machine.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "slave_config.h"

    45

#include "fsm_slave_config.h"

    46

    47

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

    48

    49

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    50

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    51

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    52

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    53

void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *);

    54

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    55

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    56

void ec_fsm_slave_config_state_pdo_assign(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_state_pdo_mapping(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    61

    62

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    68

    69

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    71

    72

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

    73

    74

/** Constructor.

    75

 */

    76

void ec_fsm_slave_config_init(ec_fsm_slave_config_t *fsm, /**< slave state machine */

    77

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

    78

        )

    79

{

    80

    fsm->datagram = datagram;

    81

    82

    // init sub state machines

    83

    ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);

    84

    ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);

    85

    ec_fsm_pdo_assign_init(&fsm->fsm_pdo_assign, &fsm->fsm_coe);

    86

    ec_fsm_pdo_mapping_init(&fsm->fsm_pdo_mapping, &fsm->fsm_coe);

    87

}

    88

    89

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

    90

    91

/** Destructor.

    92

 */

    93

void ec_fsm_slave_config_clear(ec_fsm_slave_config_t *fsm /**< slave state machine */)

    94

{

    95

    // clear sub state machines

    96

    ec_fsm_change_clear(&fsm->fsm_change);

    97

    ec_fsm_coe_clear(&fsm->fsm_coe);

    98

    ec_fsm_pdo_assign_clear(&fsm->fsm_pdo_assign);

    99

    ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_mapping);

   100

}

   101

   102

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

   103

   104

/**

   105

 * Start slave configuration state machine.

   106

 */

   107

   108

void ec_fsm_slave_config_start(ec_fsm_slave_config_t *fsm, /**< slave state machine */

   109

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

   110

        )

   111

{

   112

    fsm->slave = slave;

   113

    fsm->state = ec_fsm_slave_config_state_start;

   114

}

   115

   116

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

   117

   118

/**

   119

   \return false, if state machine has terminated

   120

*/

   121

   122

int ec_fsm_slave_config_running(const ec_fsm_slave_config_t *fsm /**< slave state machine */)

   123

{

   124

    return fsm->state != ec_fsm_slave_config_state_end

   125

        && fsm->state != ec_fsm_slave_config_state_error;

   126

}

   127

   128

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

   129

   130

/**

   131

   Executes the current state of the state machine.

   132

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

   133

   of the state machine is delayed to the next cycle.

   134

   \return false, if state machine has terminated

   135

*/

   136

   137

int ec_fsm_slave_config_exec(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   138

{

   139

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

   140

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

   141

        // datagram was not sent or received yet.

   142

        return ec_fsm_slave_config_running(fsm);

   143

    }

   144

   145

    fsm->state(fsm);

   146

    return ec_fsm_slave_config_running(fsm);

   147

}

   148

   149

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

   150

   151

/**

   152

   \return true, if the state machine terminated gracefully

   153

*/

   154

   155

int ec_fsm_slave_config_success(const ec_fsm_slave_config_t *fsm /**< slave state machine */)

   156

{

   157

    return fsm->state == ec_fsm_slave_config_state_end;

   158

}

   159

   160

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

   161

 * Slave configuration state machine

   162

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

   163

   164

/**

   165

   Slave configuration state: START.

   166

*/

   167

   168

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   169

{

   170

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

   171

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

   172

    }

   173

   174

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

   175

    ec_fsm_change_exec(&fsm->fsm_change);

   176

    fsm->state = ec_fsm_slave_config_state_init;

   177

}

   178

   179

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

   180

   181

/**

   182

   Slave configuration state: INIT.

   183

*/

   184

   185

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   186

{

   187

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

   188

    ec_slave_t *slave = fsm->slave;

   189

    ec_datagram_t *datagram = fsm->datagram;

   190

   191

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

   192

   193

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

   194

        if (!fsm->fsm_change.spontaneous_change)

   195

            slave->error_flag = 1;

   196

        fsm->state = ec_fsm_slave_config_state_error;

   197

        return;

   198

    }

   199

   200

    if (master->debug_level) {

   201

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

   202

    }

   203

   204

    // check and reset CRC fault counters

   205

    //ec_slave_check_crc(slave);

   206

    // TODO: Implement state machine for CRC checking.

   207

   208

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

   209

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   210

        return;

   211

    }

   212

   213

    if (master->debug_level)

   214

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

   215

               slave->ring_position);

   216

   217

    // clear FMMU configurations

   218

    ec_datagram_fpwr(datagram, slave->station_address,

   219

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   220

    memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   221

    fsm->retries = EC_FSM_RETRIES;

   222

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   223

}

   224

   225

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

   226

   227

/**

   228

   Slave configuration state: CLEAR FMMU.

   229

*/

   230

   231

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *fsm

   232

                                        /**< slave state machine */)

   233

{

   234

    ec_datagram_t *datagram = fsm->datagram;

   235

   236

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

   237

        return;

   238

   239

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   240

        fsm->state = ec_fsm_slave_config_state_error;

   241

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

   242

               fsm->slave->ring_position);

   243

        return;

   244

    }

   245

   246

    if (datagram->working_counter != 1) {

   247

        fsm->slave->error_flag = 1;

   248

        fsm->state = ec_fsm_slave_config_state_error;

   249

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

   250

               fsm->slave->ring_position);

   251

        ec_datagram_print_wc_error(datagram);

   252

        return;

   253

    }

   254

   255

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   256

}

   257

   258

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

   259

   260

/**

   261

 * Check for mailbox sync managers to be configured.

   262

 */

   263

   264

void ec_fsm_slave_config_enter_mbox_sync(

   265

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

   266

        )

   267

{

   268

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

   269

    ec_slave_t *slave = fsm->slave;

   270

    ec_datagram_t *datagram = fsm->datagram;

   271

    unsigned int i;

   272

   273

    // slave is now in INIT

   274

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

   275

        fsm->state = ec_fsm_slave_config_state_end; // successful

   276

        if (master->debug_level) {

   277

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

   278

                   slave->ring_position);

   279

        }

   280

        return;

   281

    }

   282

   283

    if (!slave->sii.mailbox_protocols) {

   284

        // no mailbox protocols supported

   285

        if (master->debug_level)

   286

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

   287

                    slave->ring_position);

   288

        ec_fsm_slave_config_enter_preop(fsm);

   289

        return;

   290

    }

   291

   292

    if (master->debug_level) {

   293

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

   294

               slave->ring_position);

   295

    }

   296

   297

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

   298

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

   299

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   300

        memset(datagram->data, 0x00,

   301

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   302

   303

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

   304

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

   305

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

   306

                    EC_DIR_INVALID, // use default direction

   307

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   308

        }

   309

    } else { // no mailbox sync manager configurations provided

   310

        ec_sync_t sync;

   311

   312

        if (master->debug_level)

   313

            EC_DBG("Slave %u does not provide"

   314

                    " mailbox sync manager configurations.\n",

   315

                    slave->ring_position);

   316

   317

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

   318

                EC_SYNC_PAGE_SIZE * 2);

   319

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

   320

   321

        ec_sync_init(&sync, slave);

   322

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   323

        sync.control_register = 0x26;

   324

        sync.enable = 1;

   325

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

   326

                EC_DIR_INVALID, // use default direction

   327

                datagram->data);

   328

   329

        ec_sync_init(&sync, slave);

   330

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   331

        sync.control_register = 0x22;

   332

        sync.enable = 1;

   333

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

   334

                EC_DIR_INVALID, // use default direction

   335

                datagram->data + EC_SYNC_PAGE_SIZE);

   336

    }

   337

   338

    fsm->retries = EC_FSM_RETRIES;

   339

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   340

}

   341

   342

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

   343

   344

/**

   345

   Slave configuration state: SYNC.

   346

*/

   347

   348

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   349

{

   350

    ec_datagram_t *datagram = fsm->datagram;

   351

    ec_slave_t *slave = fsm->slave;

   352

   353

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

   354

        return;

   355

   356

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   357

        fsm->state = ec_fsm_slave_config_state_error;

   358

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

   359

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

   360

               slave->ring_position, datagram->state);

   361

        return;

   362

    }

   363

   364

    if (datagram->working_counter != 1) {

   365

        slave->error_flag = 1;

   366

        fsm->state = ec_fsm_slave_config_state_error;

   367

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

   368

               slave->ring_position);

   369

        ec_datagram_print_wc_error(datagram);

   370

        return;

   371

    }

   372

   373

    ec_fsm_slave_config_enter_preop(fsm);

   374

}

   375

   376

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

   377

   378

/**

   379

 * Request PREOP state.

   380

 */

   381

   382

void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   383

{

   384

    fsm->state = ec_fsm_slave_config_state_preop;

   385

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

   386

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

   387

}

   388

   389

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

   390

   391

/**

   392

   Slave configuration state: PREOP.

   393

*/

   394

   395

void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   396

{

   397

    ec_slave_t *slave = fsm->slave;

   398

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

   399

   400

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

   401

   402

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

   403

        if (!fsm->fsm_change.spontaneous_change)

   404

            slave->error_flag = 1;

   405

        fsm->state = ec_fsm_slave_config_state_error;

   406

        return;

   407

    }

   408

   409

    // slave is now in PREOP

   410

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

   411

   412

    if (master->debug_level) {

   413

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

   414

    }

   415

   416

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

   417

        fsm->state = ec_fsm_slave_config_state_end; // successful

   418

        if (master->debug_level) {

   419

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

   420

                   slave->ring_position);

   421

        }

   422

        return;

   423

    }

   424

   425

    if (!slave->config) {

   426

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

   427

        ec_fsm_slave_config_enter_safeop(fsm);

   428

        return;

   429

    }

   430

   431

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   432

}

   433

   434

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

   435

   436

/**

   437

 * Check for Sdo configurations to be applied.

   438

 */

   439

   440

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   441

{

   442

    ec_slave_t *slave = fsm->slave;

   443

   444

    // No CoE configuration to be applied?

   445

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

   446

        ec_fsm_slave_config_enter_pdo_sync(fsm);

   447

        return;

   448

    }

   449

   450

    // start Sdo configuration

   451

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   452

    fsm->request = list_entry(fsm->slave->config->sdo_configs.next,

   453

            ec_sdo_request_t, list);

   454

    ecrt_sdo_request_write(fsm->request);

   455

    ec_fsm_coe_transfer(&fsm->fsm_coe, fsm->slave, fsm->request);

   456

    ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   457

}

   458

   459

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

   460

   461

/**

   462

   Slave configuration state: SDO_CONF.

   463

*/

   464

   465

void ec_fsm_slave_config_state_sdo_conf(

   466

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

   467

        )

   468

{

   469

    if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;

   470

   471

    if (!ec_fsm_coe_success(&fsm->fsm_coe)) {

   472

        EC_ERR("Sdo configuration failed for slave %u.\n",

   473

                fsm->slave->ring_position);

   474

        fsm->slave->error_flag = 1;

   475

        fsm->state = ec_fsm_slave_config_state_error;

   476

        return;

   477

    }

   478

   479

    // Another Sdo to configure?

   480

    if (fsm->request->list.next != &fsm->slave->config->sdo_configs) {

   481

        fsm->request = list_entry(fsm->request->list.next, ec_sdo_request_t,

   482

                list);

   483

        ecrt_sdo_request_write(fsm->request);

   484

        ec_fsm_coe_transfer(&fsm->fsm_coe, fsm->slave, fsm->request);

   485

        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   486

        return;

   487

    }

   488

   489

    // All Sdos are now configured.

   490

    ec_fsm_slave_config_enter_pdo_sync(fsm);

   491

}

   492

   493

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

   494

   495

/**

   496

 * Check for Pdo sync managers to be configured.

   497

 */

   498

   499

void ec_fsm_slave_config_enter_pdo_sync(

   500

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

   501

        )

   502

{

   503

    ec_slave_t *slave = fsm->slave;

   504

    ec_datagram_t *datagram = fsm->datagram;

   505

    unsigned int i, offset, num_pdo_syncs;

   506

    uint8_t sync_index;

   507

    const ec_sync_t *sync;

   508

    const ec_sync_config_t *sync_config;

   509

    uint16_t size;

   510

   511

    if (slave->sii.mailbox_protocols) {

   512

        offset = 2; // slave has mailboxes

   513

    } else {

   514

        offset = 0;

   515

    }

   516

   517

    if (slave->sii.sync_count <= offset) {

   518

        // no Pdo sync managers to configure

   519

        ec_fsm_slave_config_enter_safeop(fsm);