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 %i...\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

    slave->self_configured = 1;

   201

   202

    if (master->debug_level) {

   203

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

   204

    }

   205

   206

    // check and reset CRC fault counters

   207

    //ec_slave_check_crc(slave);

   208

    // TODO: Implement state machine for CRC checking.

   209

   210

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

   211

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   212

        return;

   213

    }

   214

   215

    if (master->debug_level)

   216

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

   217

               slave->ring_position);

   218

   219

    // clear FMMU configurations

   220

    ec_datagram_fpwr(datagram, slave->station_address,

   221

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   222

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

   223

    fsm->retries = EC_FSM_RETRIES;

   224

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   225

}

   226

   227

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

   228

   229

/**

   230

   Slave configuration state: CLEAR FMMU.

   231

*/

   232

   233

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *fsm

   234

                                        /**< slave state machine */)

   235

{

   236

    ec_datagram_t *datagram = fsm->datagram;

   237

   238

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

   239

        return;

   240

   241

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   242

        fsm->state = ec_fsm_slave_config_state_error;

   243

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

   244

               fsm->slave->ring_position);

   245

        return;

   246

    }

   247

   248

    if (datagram->working_counter != 1) {

   249

        fsm->slave->error_flag = 1;

   250

        fsm->state = ec_fsm_slave_config_state_error;

   251

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

   252

               fsm->slave->ring_position);

   253

        ec_datagram_print_wc_error(datagram);

   254

        return;

   255

    }

   256

   257

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   258

}

   259

   260

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

   261

   262

/**

   263

 * Check for mailbox sync managers to be configured.

   264

 */

   265

   266

void ec_fsm_slave_config_enter_mbox_sync(

   267

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

   268

        )

   269

{

   270

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

   271

    ec_slave_t *slave = fsm->slave;

   272

    ec_datagram_t *datagram = fsm->datagram;

   273

    unsigned int i;

   274

   275

    // slave is now in INIT

   276

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

   277

        fsm->state = ec_fsm_slave_config_state_end; // successful

   278

        if (master->debug_level) {

   279

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

   280

                   slave->ring_position);

   281

        }

   282

        return;

   283

    }

   284

   285

    if (!slave->sii.mailbox_protocols) {

   286

        // no mailbox protocols supported

   287

        if (master->debug_level)

   288

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

   289

                    slave->ring_position);

   290

        ec_fsm_slave_config_enter_preop(fsm);

   291

        return;

   292

    }

   293

   294

    if (master->debug_level) {

   295

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

   296

               slave->ring_position);

   297

    }

   298

   299

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

   300

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

   301

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   302

        memset(datagram->data, 0x00,

   303

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   304

   305

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

   306

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

   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, 0);

   322

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   323

        sync.control_register = 0x26;

   324

        sync.enable = 1;

   325

        ec_sync_config(&sync, slave->sii.rx_mailbox_size,

   326

                datagram->data + EC_SYNC_PAGE_SIZE * sync.index);

   327

   328

        ec_sync_init(&sync, slave, 1);

   329

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   330

        sync.control_register = 0x22;

   331

        sync.enable = 1;

   332

        ec_sync_config(&sync, slave->sii.tx_mailbox_size,

   333

                datagram->data + EC_SYNC_PAGE_SIZE * sync.index);

   334

    }

   335

   336

    fsm->retries = EC_FSM_RETRIES;

   337

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   338

}

   339

   340

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

   341

   342

/**

   343

   Slave configuration state: SYNC.

   344

*/

   345

   346

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

   347

{

   348

    ec_datagram_t *datagram = fsm->datagram;

   349

    ec_slave_t *slave = fsm->slave;

   350

   351

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

   352

        return;

   353

   354

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   355

        fsm->state = ec_fsm_slave_config_state_error;

   356

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

   357

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

   358

               slave->ring_position, datagram->state);

   359

        return;

   360

    }

   361

   362

    if (datagram->working_counter != 1) {

   363

        slave->error_flag = 1;

   364

        fsm->state = ec_fsm_slave_config_state_error;

   365

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

   366

               slave->ring_position);

   367

        ec_datagram_print_wc_error(datagram);

   368

        return;

   369

    }

   370

   371

    ec_fsm_slave_config_enter_preop(fsm);

   372

}

   373

   374

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

   375

   376

/**

   377

 * Request PREOP state.

   378

 */

   379

   380

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

   381

{

   382

    fsm->state = ec_fsm_slave_config_state_preop;

   383

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

   384

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

   385

}

   386

   387

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

   388

   389

/**

   390

   Slave configuration state: PREOP.

   391

*/

   392

   393

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

   394

{

   395

    ec_slave_t *slave = fsm->slave;

   396

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

   397

   398

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

   399

   400

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

   401

        if (!fsm->fsm_change.spontaneous_change)

   402

            slave->error_flag = 1;

   403

        fsm->state = ec_fsm_slave_config_state_error;

   404

        return;

   405

    }

   406

   407

    // slave is now in PREOP

   408

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

   409

   410

    if (master->debug_level) {

   411

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

   412

    }

   413

   414

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

   415

        fsm->state = ec_fsm_slave_config_state_end; // successful

   416

        if (master->debug_level) {

   417

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

   418

                   slave->ring_position);

   419

        }

   420

        return;

   421

    }

   422

   423

    if (!slave->config) {

   424

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

   425

        ec_fsm_slave_config_enter_safeop(fsm);

   426

        return;

   427

    }

   428

   429

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   430

}

   431

   432

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

   433

   434

/**

   435

 * Check for Sdo configurations to be applied.

   436

 */

   437

   438

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

   439

{

   440

    ec_slave_t *slave = fsm->slave;

   441

   442

    // No CoE configuration to be applied?

   443

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

   444

        ec_fsm_slave_config_enter_pdo_sync(fsm);

   445

        return;

   446

    }

   447

   448

    // start Sdo configuration

   449

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   450

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

   451

            ec_sdo_request_t, list);

   452

    ecrt_sdo_request_write(fsm->request);

   453

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

   454

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

   455

}

   456

   457

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

   458

   459

/**

   460

   Slave configuration state: SDO_CONF.

   461

*/

   462

   463

void ec_fsm_slave_config_state_sdo_conf(

   464

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

   465

        )

   466

{

   467

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

   468

   469

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

   470

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

   471

                fsm->slave->ring_position);

   472

        fsm->slave->error_flag = 1;

   473

        fsm->state = ec_fsm_slave_config_state_error;

   474

        return;

   475

    }

   476

   477

    // Another Sdo to configure?

   478

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

   479

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

   480

                list);

   481

        ecrt_sdo_request_write(fsm->request);

   482

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

   483

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

   484

        return;

   485

    }

   486

   487

    // All Sdos are now configured.

   488

    ec_fsm_slave_config_enter_pdo_sync(fsm);

   489

}

   490

   491

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

   492

   493

/**

   494

 * Check for Pdo sync managers to be configured.

   495

 */

   496

   497

void ec_fsm_slave_config_enter_pdo_sync(

   498

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

   499

        )

   500

{

   501

    ec_slave_t *slave = fsm->slave;

   502

    ec_datagram_t *datagram = fsm->datagram;

   503

    unsigned int i, offset, num_pdo_syncs;

   504

    const ec_sync_t *sync;

   505

    ec_direction_t dir;

   506

    uint16_t size;

   507

   508

    if (slave->sii.mailbox_protocols) {

   509

        offset = 2; // slave has mailboxes

   510

    } else {

   511

        offset = 0;

   512

    }

   513

   514

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

   515

        // no Pdo sync managers to configure

   516

        ec_fsm_slave_config_enter_safeop(fsm);

   517

        return;

   518

    }

   519

   520

    num_pdo_syncs = slave->sii.sync_count - offset;

   521

   522