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

/** \file

    35

 *

    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(

    77

        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_assign_init(&fsm->fsm_pdo_assign, &fsm->fsm_coe);

    87

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

    88

}

    89

    90

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

    91

    92

/** Destructor.

    93

 */

    94

void ec_fsm_slave_config_clear(

    95

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

    96

        )

    97

{

    98

    // clear sub state machines

    99

    ec_fsm_change_clear(&fsm->fsm_change);

   100

    ec_fsm_coe_clear(&fsm->fsm_coe);

   101

    ec_fsm_pdo_assign_clear(&fsm->fsm_pdo_assign);

   102

    ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_mapping);

   103

}

   104

   105

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

   106

   107

/** Start slave configuration state machine.

   108

 */

   109

void ec_fsm_slave_config_start(

   110

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

   111

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

   112

        )

   113

{

   114

    fsm->slave = slave;

   115

    fsm->state = ec_fsm_slave_config_state_start;

   116

}

   117

   118

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

   119

   120

/**

   121

 * \return false, if state machine has terminated

   122

 */

   123

int ec_fsm_slave_config_running(

   124

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

   125

        )

   126

{

   127

    return fsm->state != ec_fsm_slave_config_state_end

   128

        && fsm->state != ec_fsm_slave_config_state_error;

   129

}

   130

   131

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

   132

   133

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

   134

 *

   135

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

   136

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

   137

 *

   138

 * \return false, if state machine has terminated

   139

 */

   140

int ec_fsm_slave_config_exec(

   141

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

   142

        )

   143

{

   144

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

   145

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

   146

        // datagram was not sent or received yet.

   147

        return ec_fsm_slave_config_running(fsm);

   148

    }

   149

   150

    fsm->state(fsm);

   151

    return ec_fsm_slave_config_running(fsm);

   152

}

   153

   154

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

   155

   156

/**

   157

 * \return true, if the state machine terminated gracefully

   158

 */

   159

int ec_fsm_slave_config_success(

   160

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

   161

        )

   162

{

   163

    return fsm->state == ec_fsm_slave_config_state_end;

   164

}

   165

   166

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

   167

 * Slave configuration state machine

   168

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

   169

   170

/** Slave configuration state: START.

   171

 */

   172

void ec_fsm_slave_config_state_start(

   173

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

   174

        )

   175

{

   176

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

   177

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

   178

    }

   179

   180

    // configuration will be done immediately; therefore reset the

   181

    // force flag

   182

    fsm->slave->force_config = 0;

   183

   184

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

   185

    ec_fsm_change_exec(&fsm->fsm_change);

   186

    fsm->state = ec_fsm_slave_config_state_init;

   187

}

   188

   189

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

   190

   191

/** Slave configuration state: INIT.

   192

 */

   193

void ec_fsm_slave_config_state_init(

   194

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

   195

        )

   196

{

   197

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

   198

    ec_slave_t *slave = fsm->slave;

   199

    ec_datagram_t *datagram = fsm->datagram;

   200

   201

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

   202

   203

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

   204

        if (!fsm->fsm_change.spontaneous_change)

   205

            slave->error_flag = 1;

   206

        fsm->state = ec_fsm_slave_config_state_error;

   207

        return;

   208

    }

   209

   210

    if (master->debug_level) {

   211

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

   212

    }

   213

   214

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

   215

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   216

        return;

   217

    }

   218

   219

    if (master->debug_level)

   220

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

   221

               slave->ring_position);

   222

   223

    // clear FMMU configurations

   224

    ec_datagram_fpwr(datagram, slave->station_address,

   225

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   226

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

   227

    fsm->retries = EC_FSM_RETRIES;

   228

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   229

}

   230

   231

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

   232

   233

/** Slave configuration state: CLEAR FMMU.

   234

 */

   235

void ec_fsm_slave_config_state_clear_fmmus(

   236

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

   237

        )

   238

{

   239

    ec_datagram_t *datagram = fsm->datagram;

   240

   241

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

   242

        return;

   243

   244

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   245

        fsm->state = ec_fsm_slave_config_state_error;

   246

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

   247

               fsm->slave->ring_position);

   248

        return;

   249

    }

   250

   251

    if (datagram->working_counter != 1) {

   252

        fsm->slave->error_flag = 1;

   253

        fsm->state = ec_fsm_slave_config_state_error;

   254

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

   255

               fsm->slave->ring_position);

   256

        ec_datagram_print_wc_error(datagram);

   257

        return;

   258

    }

   259

   260

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   261

}

   262

   263

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

   264

   265

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

   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 %u.\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 %u.\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_page(&slave->sii.syncs[i], i,

   308

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

   309

                    EC_DIR_INVALID, // use default direction

   310

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   311

        }

   312

    } else { // no mailbox sync manager configurations provided

   313

        ec_sync_t sync;

   314

   315

        if (master->debug_level)

   316

            EC_DBG("Slave %u does not provide"

   317

                    " mailbox sync manager configurations.\n",

   318

                    slave->ring_position);

   319

   320

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

   321

                EC_SYNC_PAGE_SIZE * 2);

   322

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

   323

   324

        ec_sync_init(&sync, slave);

   325

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   326

        sync.control_register = 0x26;

   327

        sync.enable = 1;

   328

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

   329

                EC_DIR_INVALID, // use default direction

   330

                datagram->data);

   331

   332

        ec_sync_init(&sync, slave);

   333

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   334

        sync.control_register = 0x22;

   335

        sync.enable = 1;

   336

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

   337

                EC_DIR_INVALID, // use default direction

   338

                datagram->data + EC_SYNC_PAGE_SIZE);

   339

    }

   340

   341

    fsm->retries = EC_FSM_RETRIES;

   342

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   343

}

   344

   345

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

   346

   347

/** Slave configuration state: SYNC.

   348

 */

   349

void ec_fsm_slave_config_state_mbox_sync(

   350

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

   351

        )

   352

{

   353

    ec_datagram_t *datagram = fsm->datagram;

   354

    ec_slave_t *slave = fsm->slave;

   355

   356

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

   357

        return;

   358

   359

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   360

        fsm->state = ec_fsm_slave_config_state_error;

   361

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

   362

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

   363

               slave->ring_position, datagram->state);

   364

        return;

   365

    }

   366

   367

    if (datagram->working_counter != 1) {

   368

        slave->error_flag = 1;

   369

        fsm->state = ec_fsm_slave_config_state_error;

   370

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

   371

               slave->ring_position);

   372

        ec_datagram_print_wc_error(datagram);

   373

        return;

   374

    }

   375

   376

    ec_fsm_slave_config_enter_preop(fsm);

   377

}

   378

   379

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

   380

   381

/** Request PREOP state.

   382

 */

   383

void ec_fsm_slave_config_enter_preop(

   384

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

   385

        )

   386

{

   387

    fsm->state = ec_fsm_slave_config_state_preop;

   388

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

   389

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

   390

}

   391

   392

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

   393

   394

/** Slave configuration state: PREOP.

   395

 */

   396

void ec_fsm_slave_config_state_preop(

   397

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

   398

        )

   399

{

   400

    ec_slave_t *slave = fsm->slave;

   401

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

   402

   403

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

   404

   405

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

   406

        if (!fsm->fsm_change.spontaneous_change)

   407

            slave->error_flag = 1;

   408

        fsm->state = ec_fsm_slave_config_state_error;

   409

        return;

   410

    }

   411

   412

    // slave is now in PREOP

   413

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

   414

   415

    if (master->debug_level) {

   416

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

   417

    }

   418

   419

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

   420

        fsm->state = ec_fsm_slave_config_state_end; // successful

   421

        if (master->debug_level) {

   422

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

   423

                   slave->ring_position);

   424

        }

   425

        return;

   426

    }

   427

   428

    if (!slave->config) {

   429

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

   430

        ec_fsm_slave_config_enter_safeop(fsm);

   431

        return;

   432

    }

   433

   434

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   435

}

   436

   437

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

   438

   439

/** Check for Sdo configurations to be applied.

   440

 */

   441

void ec_fsm_slave_config_enter_sdo_conf(

   442

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

   443

        )

   444

{

   445

    ec_slave_t *slave = fsm->slave;

   446

   447

    // No CoE configuration to be applied?

   448

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

   449

        ec_fsm_slave_config_enter_pdo_sync(fsm);

   450

        return;

   451

    }

   452

   453

    // start Sdo configuration

   454

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   455

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

   456

            ec_sdo_request_t, list);

   457

    ecrt_sdo_request_write(fsm->request);

   458

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

   459

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

   460

}

   461

   462

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

   463

   464

/** Slave configuration state: SDO_CONF.

   465

 */

   466

void ec_fsm_slave_config_state_sdo_conf(

   467

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

   468

        )

   469

{

   470

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

   471

   472

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

   473

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

   474

                fsm->slave->ring_position);

   475

        fsm->slave->error_flag = 1;

   476

        fsm->state = ec_fsm_slave_config_state_error;

   477

        return;

   478

    }

   479

   480

    // Another Sdo to configure?

   481

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

   482

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

   483

                list);

   484

        ecrt_sdo_request_write(fsm->request);

   485

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

   486

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

   487

        return;

   488

    }

   489

   490

    // All Sdos are now configured.

   491

    ec_fsm_slave_config_enter_pdo_sync(fsm);

   492

}

   493

   494

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

   495

   496

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

   497

 */

   498

void ec_fsm_slave_config_enter_pdo_sync(

   499

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

   500

        )

   501

{

   502

    ec_slave_t *slave = fsm->slave;

   503

    ec_datagram_t *datagram = fsm->datagram;

   504

    unsigned int i, offset, num_pdo_syncs;

   505

    uint8_t sync_index;

   506

    const ec_sync_t *sync;

   507

    const ec_sync_config_t *sync_config;

   508

    uint16_t size;

   509

   510

    if (slave->sii.mailbox_protocols) {

   511

        offset = 2; // slave has mailboxes

   512

    } else {