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_config(&slave->sii.syncs[i], slave->sii.syncs[i].length,

   305

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   306

        }

   307

    } else { // no mailbox sync manager configurations provided

   308

        ec_sync_t sync;

   309

   310

        if (master->debug_level)

   311

            EC_DBG("Slave %u does not provide"

   312

                    " mailbox sync manager configurations.\n",

   313

                    slave->ring_position);

   314

   315

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

   316

                EC_SYNC_PAGE_SIZE * 2);

   317

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

   318

   319

        ec_sync_init(&sync, slave, 0);

   320

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   321

        sync.control_register = 0x26;

   322

        sync.enable = 1;

   323

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

   324

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

   325

   326

        ec_sync_init(&sync, slave, 1);

   327

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   328

        sync.control_register = 0x22;

   329

        sync.enable = 1;

   330

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

   331

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

   332

    }

   333

   334

    fsm->retries = EC_FSM_RETRIES;

   335

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   336

}

   337

   338

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

   339

   340

/**

   341

   Slave configuration state: SYNC.

   342

*/

   343

   344

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

   345

{

   346

    ec_datagram_t *datagram = fsm->datagram;

   347

    ec_slave_t *slave = fsm->slave;

   348

   349

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

   350

        return;

   351

   352

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   353

        fsm->state = ec_fsm_slave_config_state_error;

   354

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

   355

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

   356

               slave->ring_position, datagram->state);

   357

        return;

   358

    }

   359

   360

    if (datagram->working_counter != 1) {

   361

        slave->error_flag = 1;

   362

        fsm->state = ec_fsm_slave_config_state_error;

   363

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

   364

               slave->ring_position);

   365

        ec_datagram_print_wc_error(datagram);

   366

        return;

   367

    }

   368

   369

    ec_fsm_slave_config_enter_preop(fsm);

   370

}

   371

   372

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

   373

   374

/**

   375

 * Request PREOP state.

   376

 */

   377

   378

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

   379

{

   380

    fsm->state = ec_fsm_slave_config_state_preop;

   381

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

   382

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

   383

}

   384

   385

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

   386

   387

/**

   388

   Slave configuration state: PREOP.

   389

*/

   390

   391

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

   392

{

   393

    ec_slave_t *slave = fsm->slave;

   394

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

   395

   396

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

   397

   398

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

   399

        if (!fsm->fsm_change.spontaneous_change)

   400

            slave->error_flag = 1;

   401

        fsm->state = ec_fsm_slave_config_state_error;

   402

        return;

   403

    }

   404

   405

    // slave is now in PREOP

   406

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

   407

   408

    if (master->debug_level) {

   409

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

   410

    }

   411

   412

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

   413

        fsm->state = ec_fsm_slave_config_state_end; // successful

   414

        if (master->debug_level) {

   415

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

   416

                   slave->ring_position);

   417

        }

   418

        return;

   419

    }

   420

   421

    if (!slave->config) {

   422

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

   423

        ec_fsm_slave_config_enter_safeop(fsm);

   424

        return;

   425

    }

   426

   427

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   428

}

   429

   430

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

   431

   432

/**

   433

 * Check for Sdo configurations to be applied.

   434

 */

   435

   436

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

   437

{

   438

    ec_slave_t *slave = fsm->slave;

   439

   440

    // No CoE configuration to be applied?

   441

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

   442

        ec_fsm_slave_config_enter_pdo_sync(fsm);

   443

        return;

   444

    }

   445

   446

    // start Sdo configuration

   447

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   448

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

   449

            ec_sdo_request_t, list);

   450

    ecrt_sdo_request_write(fsm->request);

   451

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

   452

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

   453

}

   454

   455

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

   456

   457

/**

   458

   Slave configuration state: SDO_CONF.

   459

*/

   460

   461

void ec_fsm_slave_config_state_sdo_conf(

   462

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

   463

        )

   464

{

   465

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

   466

   467

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

   468

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

   469

                fsm->slave->ring_position);

   470

        fsm->slave->error_flag = 1;

   471

        fsm->state = ec_fsm_slave_config_state_error;

   472

        return;

   473

    }

   474

   475

    // Another Sdo to configure?

   476

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

   477

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

   478

                list);

   479

        ecrt_sdo_request_write(fsm->request);

   480

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

   481

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

   482

        return;

   483

    }

   484

   485

    // All Sdos are now configured.

   486

    ec_fsm_slave_config_enter_pdo_sync(fsm);

   487

}

   488

   489

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

   490

   491

/**

   492

 * Check for Pdo sync managers to be configured.

   493

 */

   494

   495

void ec_fsm_slave_config_enter_pdo_sync(

   496

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

   497

        )

   498

{

   499

    ec_slave_t *slave = fsm->slave;

   500

    ec_datagram_t *datagram = fsm->datagram;

   501

    unsigned int i, offset, num_pdo_syncs;

   502

    const ec_sync_t *sync;

   503

    ec_direction_t dir;

   504

    uint16_t size;

   505

   506

    if (slave->sii.mailbox_protocols) {

   507

        offset = 2; // slave has mailboxes

   508

    } else {

   509

        offset = 0;

   510

    }

   511

   512

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

   513

        // no Pdo sync managers to configure

   514

        ec_fsm_slave_config_enter_safeop(fsm);

   515

        return;

   516

    }

   517

   518

    num_pdo_syncs = slave->sii.sync_count - offset;

   519

   520