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

    56

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_state_pdo_sync(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_mapping(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    68

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    69

    70

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    72

    73

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

    74

    75

/** Constructor.

    76

 */

    77

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

    78

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

    79

        )

    80

{

    81

    fsm->datagram = datagram;

    82

    83

    // init sub state machines

    84

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

    85

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

    86

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

    87

    ec_fsm_pdo_config_init(&fsm->fsm_pdo_conf, &fsm->fsm_coe);

    88

}

    89

    90

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

    91

    92

/** Destructor.

    93

 */

    94

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

    95

{

    96

    // clear sub state machines

    97

    ec_fsm_change_clear(&fsm->fsm_change);

    98

    ec_fsm_coe_clear(&fsm->fsm_coe);

    99

    ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_map);

   100

    ec_fsm_pdo_config_clear(&fsm->fsm_pdo_conf);

   101

}

   102

   103

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

   104

   105

/**

   106

 * Start slave configuration state machine.

   107

 */

   108

   109

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

   110

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

   111

        )

   112

{

   113

    fsm->slave = slave;

   114

    fsm->state = ec_fsm_slave_config_state_start;

   115

}

   116

   117

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

   118

   119

/**

   120

   \return false, if state machine has terminated

   121

*/

   122

   123

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

   124

{

   125

    return fsm->state != ec_fsm_slave_config_state_end

   126

        && fsm->state != ec_fsm_slave_config_state_error;

   127

}

   128

   129

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

   130

   131

/**

   132

   Executes the current state of the state machine.

   133

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

   134

   of the state machine is delayed to the next cycle.

   135

   \return false, if state machine has terminated

   136

*/

   137

   138

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

   139

{

   140

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

   141

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

   142

        // datagram was not sent or received yet.

   143

        return ec_fsm_slave_config_running(fsm);

   144

    }

   145

   146

    fsm->state(fsm);

   147

    return ec_fsm_slave_config_running(fsm);

   148

}

   149

   150

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

   151

   152

/**

   153

   \return true, if the state machine terminated gracefully

   154

*/

   155

   156

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

   157

{

   158

    return fsm->state == ec_fsm_slave_config_state_end;

   159

}

   160

   161

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

   162

 * Slave configuration state machine

   163

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

   164

   165

/**

   166

   Slave configuration state: START.

   167

*/

   168

   169

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

   170

{

   171

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

   172

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

   173

    }

   174

   175

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

   176

    ec_fsm_change_exec(&fsm->fsm_change);

   177

    fsm->state = ec_fsm_slave_config_state_init;

   178

}

   179

   180

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

   181

   182

/**

   183

   Slave configuration state: INIT.

   184

*/

   185

   186

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

   187

{

   188

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

   189

    ec_slave_t *slave = fsm->slave;

   190

    ec_datagram_t *datagram = fsm->datagram;

   191

   192

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

   193

   194

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

   195

        if (!fsm->fsm_change.spontaneous_change)

   196

            slave->error_flag = 1;

   197

        fsm->state = ec_fsm_slave_config_state_error;

   198

        return;

   199

    }

   200

   201

    slave->self_configured = 1;

   202

   203

    if (master->debug_level) {

   204

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

   205

    }

   206

   207

    // check and reset CRC fault counters

   208

    //ec_slave_check_crc(slave);

   209

    // TODO: Implement state machine for CRC checking.

   210

   211

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

   212

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   213

        return;

   214

    }

   215

   216

    if (master->debug_level)

   217

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

   218

               slave->ring_position);

   219

   220

    // clear FMMU configurations

   221

    ec_datagram_fpwr(datagram, slave->station_address,

   222

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   223

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

   224

    fsm->retries = EC_FSM_RETRIES;

   225

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   226

}

   227

   228

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

   229

   230

/**

   231

   Slave configuration state: CLEAR FMMU.

   232

*/

   233

   234

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *fsm

   235

                                        /**< slave state machine */)

   236

{

   237

    ec_datagram_t *datagram = fsm->datagram;

   238

   239

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

   240

        return;

   241

   242

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   243

        fsm->state = ec_fsm_slave_config_state_error;

   244

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

   245

               fsm->slave->ring_position);

   246

        return;

   247

    }

   248

   249

    if (datagram->working_counter != 1) {

   250

        fsm->slave->error_flag = 1;

   251

        fsm->state = ec_fsm_slave_config_state_error;

   252

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

   253

               fsm->slave->ring_position);

   254

        ec_datagram_print_wc_error(datagram);

   255

        return;

   256

    }

   257

   258

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   259

}

   260

   261

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

   262

   263

/**

   264

 * Check for mailbox sync managers to be configured.

   265

 */

   266

   267

void ec_fsm_slave_config_enter_mbox_sync(

   268

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

   269

        )

   270

{

   271

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

   272

    ec_slave_t *slave = fsm->slave;

   273

    ec_datagram_t *datagram = fsm->datagram;

   274

    unsigned int i;

   275

   276

    // slave is now in INIT

   277

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

   278

        fsm->state = ec_fsm_slave_config_state_end; // successful

   279

        if (master->debug_level) {

   280

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

   281

                   slave->ring_position);

   282

        }

   283

        return;

   284

    }

   285

   286

    if (!slave->sii.mailbox_protocols) {

   287

        // no mailbox protocols supported

   288

        if (master->debug_level)

   289

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

   290

                    slave->ring_position);

   291

        ec_fsm_slave_config_enter_preop(fsm);

   292

        return;

   293

    }

   294

   295

    if (master->debug_level) {

   296

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

   297

               slave->ring_position);

   298

    }

   299

   300

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

   301

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

   302

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   303

        memset(datagram->data, 0x00,

   304

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   305

   306

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

   307

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

   308

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   309

        }

   310

    } else { // no mailbox sync manager configurations provided

   311

        ec_sync_t sync;

   312

   313

        if (master->debug_level)

   314

            EC_DBG("Slave %u does not provide"

   315

                    " mailbox sync manager configurations.\n",

   316

                    slave->ring_position);

   317

   318

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

   319

                EC_SYNC_PAGE_SIZE * 2);

   320

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

   321

   322

        ec_sync_init(&sync, slave, 0);

   323

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   324

        sync.control_register = 0x26;

   325

        sync.enable = 1;

   326

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

   327

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

   328

   329

        ec_sync_init(&sync, slave, 1);

   330

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   331

        sync.control_register = 0x22;

   332

        sync.enable = 1;

   333

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

   334

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

   335

    }

   336

   337

    fsm->retries = EC_FSM_RETRIES;

   338

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   339

}

   340

   341

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

   342

   343

/**

   344

   Slave configuration state: SYNC.

   345

*/

   346

   347

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

   348

{

   349

    ec_datagram_t *datagram = fsm->datagram;

   350

    ec_slave_t *slave = fsm->slave;

   351

   352

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

   353

        return;

   354

   355

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   356

        fsm->state = ec_fsm_slave_config_state_error;

   357

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

   358

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

   359

               slave->ring_position, datagram->state);

   360

        return;

   361

    }

   362

   363

    if (datagram->working_counter != 1) {

   364

        slave->error_flag = 1;

   365

        fsm->state = ec_fsm_slave_config_state_error;

   366

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

   367

               slave->ring_position);

   368

        ec_datagram_print_wc_error(datagram);

   369

        return;

   370

    }

   371

   372

    ec_fsm_slave_config_enter_preop(fsm);

   373

}

   374

   375

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

   376

   377

/**

   378

 * Request PREOP state.

   379

 */

   380

   381

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

   382

{

   383

    fsm->state = ec_fsm_slave_config_state_preop;

   384

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

   385

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

   386

}

   387

   388

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

   389

   390

/**

   391

   Slave configuration state: PREOP.

   392

*/

   393

   394

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

   395

{

   396

    ec_slave_t *slave = fsm->slave;

   397

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

   398

   399

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

   400

   401

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

   402

        if (!fsm->fsm_change.spontaneous_change)

   403

            slave->error_flag = 1;

   404

        fsm->state = ec_fsm_slave_config_state_error;

   405

        return;

   406

    }

   407

   408

    // slave is now in PREOP

   409

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

   410

   411

    if (master->debug_level) {

   412

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

   413

    }

   414

   415

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

   416

        fsm->state = ec_fsm_slave_config_state_end; // successful

   417

        if (master->debug_level) {

   418

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

   419

                   slave->ring_position);

   420

        }

   421

        return;

   422

    }

   423

   424

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   425

}

   426

   427

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

   428

   429

/**

   430

 * Check for Sdo configurations to be applied.

   431

 */

   432

   433

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

   434

{

   435

    ec_slave_t *slave = fsm->slave;

   436

   437

    if (!slave->config) {

   438

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

   439

        ec_fsm_slave_config_enter_safeop(fsm);

   440

        return;

   441

    }

   442

   443

    // No CoE configuration to be applied?

   444

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

   445

        ec_fsm_slave_config_enter_mapping(fsm);

   446

        return;

   447

    }

   448

   449

    // start Sdo configuration

   450

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   451

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

   452

            ec_sdo_request_t, list);

   453

    ecrt_sdo_request_write(fsm->request);

   454

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

   455

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

   456

}

   457

   458

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

   459

   460

/**

   461

   Slave configuration state: SDO_CONF.

   462

*/

   463

   464

void ec_fsm_slave_config_state_sdo_conf(

   465

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

   466

        )

   467

{

   468

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

   469

   470

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

   471

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

   472

                fsm->slave->ring_position);

   473

        fsm->slave->error_flag = 1;

   474

        fsm->state = ec_fsm_slave_config_state_error;

   475

        return;

   476

    }

   477

   478

    // Another Sdo to configure?

   479

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

   480

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

   481

                list);

   482

        ecrt_sdo_request_write(fsm->request);

   483

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

   484

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

   485

        return;

   486

    }

   487

   488

    // All Sdos are now configured.

   489

    ec_fsm_slave_config_enter_mapping(fsm);

   490

}

   491

   492

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

   493

   494

/**

   495

 * Check for Pdo mappings to be applied.

   496

 */

   497

   498

void ec_fsm_slave_config_enter_mapping(

   499

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

   500

        )

   501

{

   502

    // start configuring Pdo mapping

   503

    fsm->state = ec_fsm_slave_config_state_mapping;

   504

    ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_map, fsm->slave);

   505

    ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map); // execute immediately

   506

}

   507

   508

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

   509

   510

/**

   511

   Slave configuration state: MAPPING.

   512

*/

   513

   514

void ec_fsm_slave_config_state_mapping(

   515

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

   516

        )

   517

{

   518

    if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map)) return;

   519

   520

    if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_map)) {

   521

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

   522

                fsm->slave->ring_position);

   523

        fsm->slave->error_flag = 1;

   524

        fsm->state = ec_fsm_slave_config_state_error;

   525

        return;

   526

    }

   527