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

    57

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    60

    61

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    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_conf(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

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    74

    75

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

    76

    77

/** Constructor.

    78

 */

    79

void ec_fsm_slave_config_init(

    80

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

    81

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

    82

        ec_fsm_change_t *fsm_change, /**< State change state machine to use. */

    83

        ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */

    84

        ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration state machine to use. */

    85

        )

    86

{

    87

    ec_sdo_request_init(&fsm->request_copy);

    88

    89

    fsm->datagram = datagram;

    90

    fsm->fsm_change = fsm_change;

    91

    fsm->fsm_coe = fsm_coe;

    92

    fsm->fsm_pdo = fsm_pdo;

    93

}

    94

    95

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

    96

    97

/** Destructor.

    98

 */

    99

void ec_fsm_slave_config_clear(

   100

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

   101

        )

   102

{

   103

    ec_sdo_request_clear(&fsm->request_copy);

   104

}

   105

   106

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

   107

   108

/** Start slave configuration state machine.

   109

 */

   110

void ec_fsm_slave_config_start(

   111

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

   112

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

   113

        )

   114

{

   115

    fsm->slave = slave;

   116

    fsm->state = ec_fsm_slave_config_state_start;

   117

}

   118

   119

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

   120

   121

/**

   122

 * \return false, if state machine has terminated

   123

 */

   124

int ec_fsm_slave_config_running(

   125

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

   126

        )

   127

{

   128

    return fsm->state != ec_fsm_slave_config_state_end

   129

        && fsm->state != ec_fsm_slave_config_state_error;

   130

}

   131

   132

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

   133

   134

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

   135

 *

   136

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

   137

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

   138

 *

   139

 * \return false, if state machine has terminated

   140

 */

   141

int ec_fsm_slave_config_exec(

   142

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

   143

        )

   144

{

   145

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

   146

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

   147

        // datagram was not sent or received yet.

   148

        return ec_fsm_slave_config_running(fsm);

   149

    }

   150

   151

    fsm->state(fsm);

   152

    return ec_fsm_slave_config_running(fsm);

   153

}

   154

   155

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

   156

   157

/**

   158

 * \return true, if the state machine terminated gracefully

   159

 */

   160

int ec_fsm_slave_config_success(

   161

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

   162

        )

   163

{

   164

    return fsm->state == ec_fsm_slave_config_state_end;

   165

}

   166

   167

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

   168

 * Slave configuration state machine

   169

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

   170

   171

/** Slave configuration state: START.

   172

 */

   173

void ec_fsm_slave_config_state_start(

   174

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

   175

        )

   176

{

   177

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

   178

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

   179

    }

   180

   181

    ec_fsm_slave_config_enter_init(fsm);

   182

}

   183

   184

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

   185

   186

/** Start state change to INIT.

   187

 */

   188

void ec_fsm_slave_config_enter_init(

   189

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

   190

        )

   191

{

   192

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

   193

    ec_fsm_change_exec(fsm->fsm_change);

   194

    fsm->state = ec_fsm_slave_config_state_init;

   195

}

   196

   197

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

   198

   199

/** Slave configuration state: INIT.

   200

 */

   201

void ec_fsm_slave_config_state_init(

   202

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

   203

        )

   204

{

   205

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

   206

    ec_slave_t *slave = fsm->slave;

   207

    ec_datagram_t *datagram = fsm->datagram;

   208

   209

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

   210

   211

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

   212

        if (!fsm->fsm_change->spontaneous_change)

   213

            slave->error_flag = 1;

   214

        fsm->state = ec_fsm_slave_config_state_error;

   215

        return;

   216

    }

   217

   218

    if (master->debug_level) {

   219

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

   220

    }

   221

   222

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

   223

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   224

        return;

   225

    }

   226

   227

    if (master->debug_level)

   228

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

   229

               slave->ring_position);

   230

   231

    // clear FMMU configurations

   232

    ec_datagram_fpwr(datagram, slave->station_address,

   233

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   234

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

   235

    fsm->retries = EC_FSM_RETRIES;

   236

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   237

}

   238

   239

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

   240

   241

/** Slave configuration state: CLEAR FMMU.

   242

 */

   243

void ec_fsm_slave_config_state_clear_fmmus(

   244

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

   245

        )

   246

{

   247

    ec_datagram_t *datagram = fsm->datagram;

   248

   249

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

   250

        return;

   251

   252

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   253

        fsm->state = ec_fsm_slave_config_state_error;

   254

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

   255

               fsm->slave->ring_position);

   256

        return;

   257

    }

   258

   259

    if (datagram->working_counter != 1) {

   260

        fsm->slave->error_flag = 1;

   261

        fsm->state = ec_fsm_slave_config_state_error;

   262

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

   263

               fsm->slave->ring_position);

   264

        ec_datagram_print_wc_error(datagram);

   265

        return;

   266

    }

   267

   268

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   269

}

   270

   271

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

   272

   273

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

   274

 */

   275

void ec_fsm_slave_config_enter_mbox_sync(

   276

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

   277

        )

   278

{

   279

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

   280

    ec_slave_t *slave = fsm->slave;

   281

    ec_datagram_t *datagram = fsm->datagram;

   282

    unsigned int i;

   283

   284

    // slave is now in INIT

   285

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

   286

        fsm->state = ec_fsm_slave_config_state_end; // successful

   287

        if (master->debug_level) {

   288

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

   289

                   slave->ring_position);

   290

        }

   291

        return;

   292

    }

   293

   294

    if (!slave->sii.mailbox_protocols) {

   295

        // no mailbox protocols supported

   296

        if (master->debug_level)

   297

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

   298

                    slave->ring_position);

   299

        ec_fsm_slave_config_enter_preop(fsm);

   300

        return;

   301

    }

   302

   303

    if (master->debug_level) {

   304

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

   305

               slave->ring_position);

   306

    }

   307

   308

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

   309

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

   310

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   311

        memset(datagram->data, 0x00,

   312

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   313

   314

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

   315

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

   316

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

   317

                    EC_DIR_INVALID, // use default direction

   318

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   319

        }

   320

    } else { // no mailbox sync manager configurations provided

   321

        ec_sync_t sync;

   322

   323

        if (master->debug_level)

   324

            EC_DBG("Slave %u does not provide"

   325

                    " mailbox sync manager configurations.\n",

   326

                    slave->ring_position);

   327

   328

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

   329

                EC_SYNC_PAGE_SIZE * 2);

   330

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

   331

   332

        ec_sync_init(&sync, slave);

   333

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   334

        sync.control_register = 0x26;

   335

        sync.enable = 1;

   336

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

   337

                EC_DIR_INVALID, // use default direction

   338

                datagram->data);

   339

   340

        ec_sync_init(&sync, slave);

   341

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   342

        sync.control_register = 0x22;

   343

        sync.enable = 1;

   344

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

   345

                EC_DIR_INVALID, // use default direction

   346

                datagram->data + EC_SYNC_PAGE_SIZE);

   347

    }

   348

   349

    fsm->retries = EC_FSM_RETRIES;

   350

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   351

}

   352

   353

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

   354

   355

/** Slave configuration state: SYNC.

   356

 */

   357

void ec_fsm_slave_config_state_mbox_sync(

   358

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

   359

        )

   360

{

   361

    ec_datagram_t *datagram = fsm->datagram;

   362

    ec_slave_t *slave = fsm->slave;

   363

   364

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

   365

        return;

   366

   367

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   368

        fsm->state = ec_fsm_slave_config_state_error;

   369

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

   370

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

   371

               slave->ring_position, datagram->state);

   372

        return;

   373

    }

   374

   375

    if (datagram->working_counter != 1) {

   376

        slave->error_flag = 1;

   377

        fsm->state = ec_fsm_slave_config_state_error;

   378

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

   379

               slave->ring_position);

   380

        ec_datagram_print_wc_error(datagram);

   381

        return;

   382

    }

   383

   384

    ec_fsm_slave_config_enter_preop(fsm);

   385

}

   386

   387

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

   388

   389

/** Request PREOP state.

   390

 */

   391

void ec_fsm_slave_config_enter_preop(

   392

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

   393

        )

   394

{

   395

    fsm->state = ec_fsm_slave_config_state_preop;

   396

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

   397

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

   398

}

   399

   400

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

   401

   402

/** Slave configuration state: PREOP.

   403

 */

   404

void ec_fsm_slave_config_state_preop(

   405

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

   406

        )

   407

{

   408

    ec_slave_t *slave = fsm->slave;

   409

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

   410

   411

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

   412

   413

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

   414

        if (!fsm->fsm_change->spontaneous_change)

   415

            slave->error_flag = 1;

   416

        fsm->state = ec_fsm_slave_config_state_error;

   417

        return;

   418

    }

   419

   420

    // slave is now in PREOP

   421

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

   422

   423

    if (master->debug_level) {

   424

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

   425

    }

   426

   427

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

   428

        fsm->state = ec_fsm_slave_config_state_end; // successful

   429

        if (master->debug_level) {

   430

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

   431

                   slave->ring_position);

   432

        }

   433

        return;

   434

    }

   435

   436

    if (!slave->config) {

   437

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

   438

        ec_fsm_slave_config_enter_safeop(fsm);

   439

        return;

   440

    }

   441

   442

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   443

}

   444

   445

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

   446

   447

/** Check for Sdo configurations to be applied.

   448

 */

   449

void ec_fsm_slave_config_enter_sdo_conf(

   450

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

   451

        )

   452

{

   453

    ec_slave_t *slave = fsm->slave;

   454

   455

    // No CoE configuration to be applied?

   456

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

   457

        ec_fsm_slave_config_enter_pdo_conf(fsm);

   458

        return;

   459

    }

   460

   461

    // start Sdo configuration

   462

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   463

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

   464

            ec_sdo_request_t, list);

   465

    ec_sdo_request_copy(&fsm->request_copy, fsm->request);

   466

    ecrt_sdo_request_write(&fsm->request_copy);

   467

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

   468

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

   469

}

   470

   471

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

   472

   473

/** Slave configuration state: SDO_CONF.

   474

 */

   475

void ec_fsm_slave_config_state_sdo_conf(

   476

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

   477

        )

   478

{

   479

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

   480

   481

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

   482

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

   483

                fsm->slave->ring_position);

   484

        fsm->slave->error_flag = 1;

   485

        fsm->state = ec_fsm_slave_config_state_error;

   486

        return;

   487

    }

   488

   489

    if (!fsm->slave->config) { // config removed in the meantime

   490

        ec_fsm_slave_config_reconfigure(fsm);

   491

        return;

   492

    }

   493

   494

    // Another Sdo to configure?

   495

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

   496

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

   497

                ec_sdo_request_t, list);

   498

        ec_sdo_request_copy(&fsm->request_copy, fsm->request);

   499

        ecrt_sdo_request_write(&fsm->request_copy);

   500

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

   501

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

   502

        return;

   503

    }

   504

   505

    // All Sdos are now configured.

   506

    ec_fsm_slave_config_enter_pdo_conf(fsm);

   507

}

   508

   509

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

   510