1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/** \file

    28

 *

    29

 * EtherCAT slave configuration state machine.

    30

 */

    31

    32

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

    33

    34

#include "globals.h"

    35

#include "master.h"

    36

#include "mailbox.h"

    37

#include "slave_config.h"

    38

#include "fsm_slave_config.h"

    39

    40

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

    41

    42

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    43

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    44

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    45

void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);

    46

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    47

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    48

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    49

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    50

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    51

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    52

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    53

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    54

    55

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    56

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    62

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    64

    65

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    67

    68

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    69

    70

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

    71

    72

/** Constructor.

    73

 */

    74

void ec_fsm_slave_config_init(

    75

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

    76

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

    77

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

    78

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

    79

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

    80

        )

    81

{

    82

    ec_sdo_request_init(&fsm->request_copy);

    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

    ec_sdo_request_clear(&fsm->request_copy);

    99

}

   100

   101

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

   102

   103

/** Start slave configuration state machine.

   104

 */

   105

void ec_fsm_slave_config_start(

   106

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

   107

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

   108

        )

   109

{

   110

    fsm->slave = slave;

   111

    fsm->state = ec_fsm_slave_config_state_start;

   112

}

   113

   114

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

   115

   116

/**

   117

 * \return false, if state machine has terminated

   118

 */

   119

int ec_fsm_slave_config_running(

   120

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

   121

        )

   122

{

   123

    return fsm->state != ec_fsm_slave_config_state_end

   124

        && fsm->state != ec_fsm_slave_config_state_error;

   125

}

   126

   127

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

   128

   129

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

   130

 *

   131

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

   132

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

   133

 *

   134

 * \return false, if state machine has terminated

   135

 */

   136

int ec_fsm_slave_config_exec(

   137

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

   138

        )

   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

int ec_fsm_slave_config_success(

   156

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

   157

        )

   158

{

   159

    return fsm->state == ec_fsm_slave_config_state_end;

   160

}

   161

   162

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

   163

 * Slave configuration state machine

   164

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

   165

   166

/** Slave configuration state: START.

   167

 */

   168

void ec_fsm_slave_config_state_start(

   169

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

   170

        )

   171

{

   172

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

   173

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

   174

    }

   175

   176

    ec_fsm_slave_config_enter_init(fsm);

   177

}

   178

   179

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

   180

   181

/** Start state change to INIT.

   182

 */

   183

void ec_fsm_slave_config_enter_init(

   184

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

   185

        )

   186

{

   187

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

   188

    ec_fsm_change_exec(fsm->fsm_change);

   189

    fsm->state = ec_fsm_slave_config_state_init;

   190

}

   191

   192

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

   193

   194

/** Slave configuration state: INIT.

   195

 */

   196

void ec_fsm_slave_config_state_init(

   197

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

   198

        )

   199

{

   200

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

   201

    ec_slave_t *slave = fsm->slave;

   202

    ec_datagram_t *datagram = fsm->datagram;

   203

   204

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

   205

   206

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

   207

        if (!fsm->fsm_change->spontaneous_change)

   208

            slave->error_flag = 1;

   209

        fsm->state = ec_fsm_slave_config_state_error;

   210

        return;

   211

    }

   212

   213

    if (master->debug_level) {

   214

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

   215

    }

   216

   217

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

   218

        ec_fsm_slave_config_enter_clear_sync(fsm);

   219

        return;

   220

    }

   221

   222

    if (master->debug_level)

   223

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

   224

               slave->ring_position);

   225

   226

    // clear FMMU configurations

   227

    ec_datagram_fpwr(datagram, slave->station_address,

   228

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   229

    ec_datagram_zero(datagram);

   230

    fsm->retries = EC_FSM_RETRIES;

   231

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   232

}

   233

   234

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

   235

   236

/** Slave configuration state: CLEAR FMMU.

   237

 */

   238

void ec_fsm_slave_config_state_clear_fmmus(

   239

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

   240

        )

   241

{

   242

    ec_datagram_t *datagram = fsm->datagram;

   243

   244

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

   245

        return;

   246

   247

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   248

        fsm->state = ec_fsm_slave_config_state_error;

   249

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

   250

               fsm->slave->ring_position);

   251

        return;

   252

    }

   253

   254

    if (datagram->working_counter != 1) {

   255

        fsm->slave->error_flag = 1;

   256

        fsm->state = ec_fsm_slave_config_state_error;

   257

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

   258

               fsm->slave->ring_position);

   259

        ec_datagram_print_wc_error(datagram);

   260

        return;

   261

    }

   262

   263

    ec_fsm_slave_config_enter_clear_sync(fsm);

   264

}

   265

   266

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

   267

   268

/** Clear the sync manager configurations.

   269

 */

   270

void ec_fsm_slave_config_enter_clear_sync(

   271

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

   272

        )

   273

{

   274

    ec_slave_t *slave = fsm->slave;

   275

    ec_datagram_t *datagram = fsm->datagram;

   276

    size_t sync_size;

   277

   278

    if (!slave->sii.sync_count) {

   279

        // no mailbox protocols supported

   280

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   281

        return;

   282

    }

   283

   284

    sync_size = EC_SYNC_PAGE_SIZE * slave->sii.sync_count;

   285

   286

    // clear sync manager configurations

   287

    ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size);

   288

    memset(datagram->data, 0x00, sync_size);

   289

    fsm->retries = EC_FSM_RETRIES;

   290

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   291

}

   292

   293

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

   294

   295

/** Slave configuration state: CLEAR SYNC.

   296

 */

   297

void ec_fsm_slave_config_state_clear_sync(

   298

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

   299

        )

   300

{

   301

    ec_datagram_t *datagram = fsm->datagram;

   302

   303

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

   304

        return;

   305

   306

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   307

        fsm->state = ec_fsm_slave_config_state_error;

   308

        EC_ERR("Failed receive sync manager clearing datagram"

   309

                " for slave %u.\n", fsm->slave->ring_position);

   310

        return;

   311

    }

   312

   313

    if (datagram->working_counter != 1) {

   314

        fsm->slave->error_flag = 1;

   315

        fsm->state = ec_fsm_slave_config_state_error;

   316

        EC_ERR("Failed to clear sync manager configurations of slave %u: ",

   317

               fsm->slave->ring_position);

   318

        ec_datagram_print_wc_error(datagram);

   319

        return;

   320

    }

   321

   322

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   323

}

   324

   325

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

   326

   327

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

   328

 */

   329

void ec_fsm_slave_config_enter_mbox_sync(

   330

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

   331

        )

   332

{

   333

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

   334

    ec_slave_t *slave = fsm->slave;

   335

    ec_datagram_t *datagram = fsm->datagram;

   336

    unsigned int i;

   337

   338

    // slave is now in INIT

   339

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

   340

        fsm->state = ec_fsm_slave_config_state_end; // successful

   341

        if (master->debug_level) {

   342

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

   343

                   slave->ring_position);

   344

        }

   345

        return;

   346

    }

   347

   348

    if (!slave->sii.mailbox_protocols) {

   349

        // no mailbox protocols supported

   350

        if (master->debug_level)

   351

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

   352

                    slave->ring_position);

   353

        ec_fsm_slave_config_enter_boot_preop(fsm);

   354

        return;

   355

    }

   356

   357

    if (master->debug_level) {

   358

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

   359

               slave->ring_position);

   360

    }

   361

   362

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   363

        ec_sync_t sync;

   364

   365

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

   366

                EC_SYNC_PAGE_SIZE * 2);

   367

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

   368

   369

        ec_sync_init(&sync, slave);

   370

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   371

        sync.control_register = 0x26;

   372

        sync.enable = 1;

   373

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

   374

                EC_DIR_INVALID, // use default direction

   375

                datagram->data);

   376

        slave->configured_rx_mailbox_offset =

   377

            slave->sii.boot_rx_mailbox_offset;

   378

        slave->configured_rx_mailbox_size =

   379

            slave->sii.boot_rx_mailbox_size;

   380

   381

        ec_sync_init(&sync, slave);

   382

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   383

        sync.control_register = 0x22;

   384

        sync.enable = 1;

   385

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

   386

                EC_DIR_INVALID, // use default direction

   387

                datagram->data + EC_SYNC_PAGE_SIZE);

   388

        slave->configured_tx_mailbox_offset =

   389

            slave->sii.boot_tx_mailbox_offset;

   390

        slave->configured_tx_mailbox_size =

   391

            slave->sii.boot_tx_mailbox_size;

   392

   393

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

   394

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

   395

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   396

        ec_datagram_zero(datagram);

   397

   398

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

   399

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

   400

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

   401

                    EC_DIR_INVALID, // use default direction

   402

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   403

        }

   404

   405

        slave->configured_rx_mailbox_offset =

   406

            slave->sii.syncs[0].physical_start_address;

   407

        slave->configured_rx_mailbox_size =

   408

            slave->sii.syncs[0].default_length;

   409

        slave->configured_tx_mailbox_offset =

   410

            slave->sii.syncs[1].physical_start_address;

   411

        slave->configured_tx_mailbox_size =

   412

            slave->sii.syncs[1].default_length;

   413

    } else { // no mailbox sync manager configurations provided

   414

        ec_sync_t sync;

   415

   416

        if (master->debug_level)

   417

            EC_DBG("Slave %u does not provide"

   418

                    " mailbox sync manager configurations.\n",

   419

                    slave->ring_position);

   420

   421

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

   422

                EC_SYNC_PAGE_SIZE * 2);

   423

        ec_datagram_zero(datagram);

   424

   425

        ec_sync_init(&sync, slave);

   426

        sync.physical_start_address = slave->sii.std_rx_mailbox_offset;

   427

        sync.control_register = 0x26;

   428

        sync.enable = 1;

   429

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

   430

                EC_DIR_INVALID, // use default direction

   431

                datagram->data);

   432

        slave->configured_rx_mailbox_offset =

   433

            slave->sii.std_rx_mailbox_offset;

   434

        slave->configured_rx_mailbox_size =

   435

            slave->sii.std_rx_mailbox_size;

   436

   437

        ec_sync_init(&sync, slave);

   438

        sync.physical_start_address = slave->sii.std_tx_mailbox_offset;

   439

        sync.control_register = 0x22;

   440

        sync.enable = 1;

   441

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

   442

                EC_DIR_INVALID, // use default direction

   443

                datagram->data + EC_SYNC_PAGE_SIZE);

   444

        slave->configured_tx_mailbox_offset =

   445

            slave->sii.boot_tx_mailbox_offset;

   446

        slave->configured_tx_mailbox_size =

   447

            slave->sii.boot_tx_mailbox_size;

   448

    }

   449

   450

    fsm->retries = EC_FSM_RETRIES;

   451

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   452

}

   453

   454

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

   455

   456

/** Slave configuration state: SYNC.

   457

 */

   458

void ec_fsm_slave_config_state_mbox_sync(

   459

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

   460

        )

   461

{

   462

    ec_datagram_t *datagram = fsm->datagram;

   463

    ec_slave_t *slave = fsm->slave;

   464

   465

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

   466

        return;

   467

   468

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   469

        fsm->state = ec_fsm_slave_config_state_error;

   470

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

   471

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

   472

               slave->ring_position, datagram->state);

   473

        return;

   474

    }

   475

   476

    if (datagram->working_counter != 1) {

   477

        slave->error_flag = 1;

   478

        fsm->state = ec_fsm_slave_config_state_error;

   479

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

   480

               slave->ring_position);

   481

        ec_datagram_print_wc_error(datagram);

   482

        return;

   483

    }

   484

   485

    ec_fsm_slave_config_enter_boot_preop(fsm);

   486

}

   487

   488

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

   489

   490

/** Request PREOP state.

   491

 */

   492

void ec_fsm_slave_config_enter_boot_preop(

   493

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

   494

        )

   495

{

   496

    fsm->state = ec_fsm_slave_config_state_boot_preop;

   497

   498

    if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) {

   499

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

   500

    } else { // BOOT

   501

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

   502

    }

   503

   504

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