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

    62

void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_enter_pdo_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

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

    80

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

    81

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

    82

        )

    83

{

    84

    fsm->datagram = datagram;

    85

    fsm->fsm_change = fsm_change;

    86

    fsm->fsm_coe = fsm_coe;

    87

    fsm->fsm_pdo = fsm_pdo;

    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

}

    99

   100

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

   101

   102

/** Start slave configuration state machine.

   103

 */

   104

void ec_fsm_slave_config_start(

   105

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

   106

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

   107

        )

   108

{

   109

    fsm->slave = slave;

   110

    fsm->state = ec_fsm_slave_config_state_start;

   111

}

   112

   113

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

   114

   115

/**

   116

 * \return false, if state machine has terminated

   117

 */

   118

int ec_fsm_slave_config_running(

   119

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

   120

        )

   121

{

   122

    return fsm->state != ec_fsm_slave_config_state_end

   123

        && fsm->state != ec_fsm_slave_config_state_error;

   124

}

   125

   126

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

   127

   128

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

   129

 *

   130

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

   131

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

   132

 *

   133

 * \return false, if state machine has terminated

   134

 */

   135

int ec_fsm_slave_config_exec(

   136

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

   137

        )

   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

int ec_fsm_slave_config_success(

   155

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

   156

        )

   157

{

   158

    return fsm->state == ec_fsm_slave_config_state_end;

   159

}

   160

   161

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

   162

 * Slave configuration state machine

   163

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

   164

   165

/** Slave configuration state: START.

   166

 */

   167

void ec_fsm_slave_config_state_start(

   168

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

   169

        )

   170

{

   171

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

   172

        EC_DBG("Configuring slave %u...\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

/** Slave configuration state: INIT.

   183

 */

   184

void ec_fsm_slave_config_state_init(

   185

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

   186

        )

   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

    if (master->debug_level) {

   202

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

   203

    }

   204

   205

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

   206

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   207

        return;

   208

    }

   209

   210

    if (master->debug_level)

   211

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

   212

               slave->ring_position);

   213

   214

    // clear FMMU configurations

   215

    ec_datagram_fpwr(datagram, slave->station_address,

   216

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   217

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

   218

    fsm->retries = EC_FSM_RETRIES;

   219

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   220

}

   221

   222

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

   223

   224

/** Slave configuration state: CLEAR FMMU.

   225

 */

   226

void ec_fsm_slave_config_state_clear_fmmus(

   227

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

   228

        )

   229

{

   230

    ec_datagram_t *datagram = fsm->datagram;

   231

   232

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

   233

        return;

   234

   235

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   236

        fsm->state = ec_fsm_slave_config_state_error;

   237

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

   238

               fsm->slave->ring_position);

   239

        return;

   240

    }

   241

   242

    if (datagram->working_counter != 1) {

   243

        fsm->slave->error_flag = 1;

   244

        fsm->state = ec_fsm_slave_config_state_error;

   245

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

   246

               fsm->slave->ring_position);

   247

        ec_datagram_print_wc_error(datagram);

   248

        return;

   249

    }

   250

   251

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   252

}

   253

   254

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

   255

   256

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

   257

 */

   258

void ec_fsm_slave_config_enter_mbox_sync(

   259

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

   260

        )

   261

{

   262

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

   263

    ec_slave_t *slave = fsm->slave;

   264

    ec_datagram_t *datagram = fsm->datagram;

   265

    unsigned int i;

   266

   267

    // slave is now in INIT

   268

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

   269

        fsm->state = ec_fsm_slave_config_state_end; // successful

   270

        if (master->debug_level) {

   271

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

   272

                   slave->ring_position);

   273

        }

   274

        return;

   275

    }

   276

   277

    if (!slave->sii.mailbox_protocols) {

   278

        // no mailbox protocols supported

   279

        if (master->debug_level)

   280

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

   281

                    slave->ring_position);

   282

        ec_fsm_slave_config_enter_preop(fsm);

   283

        return;

   284

    }

   285

   286

    if (master->debug_level) {

   287

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

   288

               slave->ring_position);

   289

    }

   290

   291

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

   292

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

   293

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   294

        memset(datagram->data, 0x00,

   295

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   296

   297

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

   298

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

   299

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

   300

                    EC_DIR_INVALID, // use default direction

   301

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   302

        }

   303

    } else { // no mailbox sync manager configurations provided

   304

        ec_sync_t sync;

   305

   306

        if (master->debug_level)

   307

            EC_DBG("Slave %u does not provide"

   308

                    " mailbox sync manager configurations.\n",

   309

                    slave->ring_position);

   310

   311

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

   312

                EC_SYNC_PAGE_SIZE * 2);

   313

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

   314

   315

        ec_sync_init(&sync, slave);

   316

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   317

        sync.control_register = 0x26;

   318

        sync.enable = 1;

   319

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

   320

                EC_DIR_INVALID, // use default direction

   321

                datagram->data);

   322

   323

        ec_sync_init(&sync, slave);

   324

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   325

        sync.control_register = 0x22;

   326

        sync.enable = 1;

   327

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

   328

                EC_DIR_INVALID, // use default direction

   329

                datagram->data + EC_SYNC_PAGE_SIZE);

   330

    }

   331

   332

    fsm->retries = EC_FSM_RETRIES;

   333

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   334

}

   335

   336

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

   337

   338

/** Slave configuration state: SYNC.

   339

 */

   340

void ec_fsm_slave_config_state_mbox_sync(

   341

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

   342

        )

   343

{

   344

    ec_datagram_t *datagram = fsm->datagram;

   345

    ec_slave_t *slave = fsm->slave;

   346

   347

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

   348

        return;

   349

   350

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   351

        fsm->state = ec_fsm_slave_config_state_error;

   352

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

   353

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

   354

               slave->ring_position, datagram->state);

   355

        return;

   356

    }

   357

   358

    if (datagram->working_counter != 1) {

   359

        slave->error_flag = 1;

   360

        fsm->state = ec_fsm_slave_config_state_error;

   361

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

   362

               slave->ring_position);

   363

        ec_datagram_print_wc_error(datagram);

   364

        return;

   365

    }

   366

   367

    ec_fsm_slave_config_enter_preop(fsm);

   368

}

   369

   370

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

   371

   372

/** Request PREOP state.

   373

 */

   374

void ec_fsm_slave_config_enter_preop(

   375

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

   376

        )

   377

{

   378

    fsm->state = ec_fsm_slave_config_state_preop;

   379

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

   380

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

   381

}

   382

   383

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

   384

   385

/** Slave configuration state: PREOP.

   386

 */

   387

void ec_fsm_slave_config_state_preop(

   388

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

   389

        )

   390

{

   391

    ec_slave_t *slave = fsm->slave;

   392

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

   393

   394

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

   395

   396

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

   397

        if (!fsm->fsm_change->spontaneous_change)

   398

            slave->error_flag = 1;

   399

        fsm->state = ec_fsm_slave_config_state_error;

   400

        return;

   401

    }

   402

   403

    // slave is now in PREOP

   404

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

   405

   406

    if (master->debug_level) {

   407

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

   408

    }

   409

   410

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

   411

        fsm->state = ec_fsm_slave_config_state_end; // successful

   412

        if (master->debug_level) {

   413

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

   414

                   slave->ring_position);

   415

        }

   416

        return;

   417

    }

   418

   419

    if (!slave->config) {

   420

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

   421

        ec_fsm_slave_config_enter_safeop(fsm);

   422

        return;

   423

    }

   424

   425

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   426

}

   427

   428

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

   429

   430

/** Check for Sdo configurations to be applied.

   431

 */

   432

void ec_fsm_slave_config_enter_sdo_conf(

   433

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

   434

        )

   435

{

   436

    ec_slave_t *slave = fsm->slave;

   437

   438

    // No CoE configuration to be applied?

   439

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

   440

        ec_fsm_slave_config_enter_pdo_conf(fsm);

   441

        return;

   442

    }

   443

   444

    // start Sdo configuration

   445

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   446

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

   447

            ec_sdo_request_t, list);

   448

    ecrt_sdo_request_write(fsm->request);

   449

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

   450

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

   451

}

   452

   453

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

   454

   455

/** Slave configuration state: SDO_CONF.

   456

 */

   457

void ec_fsm_slave_config_state_sdo_conf(

   458

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

   459

        )

   460

{

   461

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

   462

   463

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

   464

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

   465

                fsm->slave->ring_position);

   466

        fsm->slave->error_flag = 1;

   467

        fsm->state = ec_fsm_slave_config_state_error;

   468

        return;

   469

    }

   470

   471

    // Another Sdo to configure?

   472

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

   473

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

   474

                list);

   475

        ecrt_sdo_request_write(fsm->request);

   476

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

   477

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

   478

        return;

   479

    }

   480

   481

    // All Sdos are now configured.

   482

    ec_fsm_slave_config_enter_pdo_conf(fsm);

   483

}

   484

   485

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

   486

   487

/** PDO_CONF entry function.

   488

 */

   489

void ec_fsm_slave_config_enter_pdo_conf(

   490

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

   491

        )

   492

{

   493

    // Start configuring Pdos

   494

    ec_fsm_pdo_start_configuration(fsm->fsm_pdo, fsm->slave);

   495

    fsm->state = ec_fsm_slave_config_state_pdo_conf;

   496

    fsm->state(fsm); // execute immediately

   497

}

   498

   499

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

   500

   501

/** Slave configuration state: PDO_CONF.

   502

 */

   503

void ec_fsm_slave_config_state_pdo_conf(

   504

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

   505

        )

   506

{

   507

    if (ec_fsm_pdo_exec(fsm->fsm_pdo))

   508

        return;

   509

   510

    if (!ec_fsm_pdo_success(fsm->fsm_pdo)) {