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

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

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

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/** \file

    31

 *

    32

 * EtherCAT slave configuration state machine.

    33

 */

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "slave_config.h"

    41

#include "fsm_slave_config.h"

    42

    43

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

    44

    45

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    46

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    47

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    48

void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);

    49

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    50

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    51

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    52

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    53

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    54

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    55

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    56

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_state_dc_assign(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_clear_sync(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    68

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_enter_dc_assign(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    72

    73

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    74

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    75

    76

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    77

    78

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

    79

    80

/** Constructor.

    81

 */

    82

void ec_fsm_slave_config_init(

    83

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

    84

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

    85

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

    86

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

    87

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

    88

        )

    89

{

    90

    ec_sdo_request_init(&fsm->request_copy);

    91

    92

    fsm->datagram = datagram;

    93

    fsm->fsm_change = fsm_change;

    94

    fsm->fsm_coe = fsm_coe;

    95

    fsm->fsm_pdo = fsm_pdo;

    96

}

    97

    98

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

    99

   100

/** Destructor.

   101

 */

   102

void ec_fsm_slave_config_clear(

   103

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

   104

        )

   105

{

   106

    ec_sdo_request_clear(&fsm->request_copy);

   107

}

   108

   109

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

   110

   111

/** Start slave configuration state machine.

   112

 */

   113

void ec_fsm_slave_config_start(

   114

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

   115

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

   116

        )

   117

{

   118

    fsm->slave = slave;

   119

    fsm->state = ec_fsm_slave_config_state_start;

   120

}

   121

   122

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

   123

   124

/**

   125

 * \return false, if state machine has terminated

   126

 */

   127

int ec_fsm_slave_config_running(

   128

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

   129

        )

   130

{

   131

    return fsm->state != ec_fsm_slave_config_state_end

   132

        && fsm->state != ec_fsm_slave_config_state_error;

   133

}

   134

   135

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

   136

   137

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

   138

 *

   139

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

   140

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

   141

 *

   142

 * \return false, if state machine has terminated

   143

 */

   144

int ec_fsm_slave_config_exec(

   145

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

   146

        )

   147

{

   148

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

   149

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

   150

        // datagram was not sent or received yet.

   151

        return ec_fsm_slave_config_running(fsm);

   152

    }

   153

   154

    fsm->state(fsm);

   155

    return ec_fsm_slave_config_running(fsm);

   156

}

   157

   158

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

   159

   160

/**

   161

 * \return true, if the state machine terminated gracefully

   162

 */

   163

int ec_fsm_slave_config_success(

   164

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

   165

        )

   166

{

   167

    return fsm->state == ec_fsm_slave_config_state_end;

   168

}

   169

   170

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

   171

 * Slave configuration state machine

   172

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

   173

   174

/** Slave configuration state: START.

   175

 */

   176

void ec_fsm_slave_config_state_start(

   177

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

   178

        )

   179

{

   180

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

   181

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

   182

    }

   183

   184

    ec_fsm_slave_config_enter_init(fsm);

   185

}

   186

   187

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

   188

   189

/** Start state change to INIT.

   190

 */

   191

void ec_fsm_slave_config_enter_init(

   192

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

   193

        )

   194

{

   195

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

   196

    ec_fsm_change_exec(fsm->fsm_change);

   197

    fsm->state = ec_fsm_slave_config_state_init;

   198

}

   199

   200

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

   201

   202

/** Slave configuration state: INIT.

   203

 */

   204

void ec_fsm_slave_config_state_init(

   205

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

   206

        )

   207

{

   208

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

   209

    ec_slave_t *slave = fsm->slave;

   210

    ec_datagram_t *datagram = fsm->datagram;

   211

   212

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

   213

   214

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

   215

        if (!fsm->fsm_change->spontaneous_change)

   216

            slave->error_flag = 1;

   217

        fsm->state = ec_fsm_slave_config_state_error;

   218

        return;

   219

    }

   220

   221

    if (master->debug_level) {

   222

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

   223

    }

   224

   225

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

   226

        ec_fsm_slave_config_enter_clear_sync(fsm);

   227

        return;

   228

    }

   229

   230

    if (master->debug_level)

   231

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

   232

               slave->ring_position);

   233

   234

    // clear FMMU configurations

   235

    ec_datagram_fpwr(datagram, slave->station_address,

   236

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   237

    ec_datagram_zero(datagram);

   238

    fsm->retries = EC_FSM_RETRIES;

   239

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   240

}

   241

   242

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

   243

   244

/** Slave configuration state: CLEAR FMMU.

   245

 */

   246

void ec_fsm_slave_config_state_clear_fmmus(

   247

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

   248

        )

   249

{

   250

    ec_datagram_t *datagram = fsm->datagram;

   251

   252

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

   253

        return;

   254

   255

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   256

        fsm->state = ec_fsm_slave_config_state_error;

   257

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

   258

               fsm->slave->ring_position);

   259

        return;

   260

    }

   261

   262

    if (datagram->working_counter != 1) {

   263

        fsm->slave->error_flag = 1;

   264

        fsm->state = ec_fsm_slave_config_state_error;

   265

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

   266

               fsm->slave->ring_position);

   267

        ec_datagram_print_wc_error(datagram);

   268

        return;

   269

    }

   270

   271

    ec_fsm_slave_config_enter_clear_sync(fsm);

   272

}

   273

   274

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

   275

   276

/** Clear the sync manager configurations.

   277

 */

   278

void ec_fsm_slave_config_enter_clear_sync(

   279

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

   280

        )

   281

{

   282

    ec_slave_t *slave = fsm->slave;

   283

    ec_datagram_t *datagram = fsm->datagram;

   284

    size_t sync_size;

   285

   286

    if (!slave->sii.sync_count) {

   287

        // no mailbox protocols supported

   288

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   289

        return;

   290

    }

   291

   292

    if (slave->master->debug_level)

   293

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

   294

                slave->ring_position);

   295

   296

    sync_size = EC_SYNC_PAGE_SIZE * slave->sii.sync_count;

   297

   298

    // clear sync manager configurations

   299

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

   300

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

   301

    fsm->retries = EC_FSM_RETRIES;

   302

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   303

}

   304

   305

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

   306

   307

/** Slave configuration state: CLEAR SYNC.

   308

 */

   309

void ec_fsm_slave_config_state_clear_sync(

   310

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

   311

        )

   312

{

   313

    ec_datagram_t *datagram = fsm->datagram;

   314

   315

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

   316

        return;

   317

   318

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   319

        fsm->state = ec_fsm_slave_config_state_error;

   320

        EC_ERR("Failed receive sync manager clearing datagram"

   321

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

   322

        return;

   323

    }

   324

   325

    if (datagram->working_counter != 1) {

   326

        fsm->slave->error_flag = 1;

   327

        fsm->state = ec_fsm_slave_config_state_error;

   328

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

   329

               fsm->slave->ring_position);

   330

        ec_datagram_print_wc_error(datagram);

   331

        return;

   332

    }

   333

   334

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   335

}

   336

   337

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

   338

   339

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

   340

 */

   341

void ec_fsm_slave_config_enter_mbox_sync(

   342

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

   343

        )

   344

{

   345

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

   346

    ec_slave_t *slave = fsm->slave;

   347

    ec_datagram_t *datagram = fsm->datagram;

   348

    unsigned int i;

   349

   350

    // slave is now in INIT

   351

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

   352

        fsm->state = ec_fsm_slave_config_state_end; // successful

   353

        if (master->debug_level) {

   354

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

   355

                   slave->ring_position);

   356

        }

   357

        return;

   358

    }

   359

   360

    if (!slave->sii.mailbox_protocols) {

   361

        // no mailbox protocols supported

   362

        if (master->debug_level)

   363

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

   364

                    slave->ring_position);

   365

        ec_fsm_slave_config_enter_boot_preop(fsm);

   366

        return;

   367

    }

   368

   369

    if (master->debug_level) {

   370

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

   371

               slave->ring_position);

   372

    }

   373

   374

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   375

        ec_sync_t sync;

   376

   377

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

   378

                EC_SYNC_PAGE_SIZE * 2);

   379

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

   380

   381

        ec_sync_init(&sync, slave);

   382

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   383

        sync.control_register = 0x26;

   384

        sync.enable = 1;

   385

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

   386

                EC_DIR_INVALID, // use default direction

   387

                datagram->data);

   388

        slave->configured_rx_mailbox_offset =

   389

            slave->sii.boot_rx_mailbox_offset;

   390

        slave->configured_rx_mailbox_size =

   391

            slave->sii.boot_rx_mailbox_size;

   392

   393

        ec_sync_init(&sync, slave);

   394

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   395

        sync.control_register = 0x22;

   396

        sync.enable = 1;

   397

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

   398

                EC_DIR_INVALID, // use default direction

   399

                datagram->data + EC_SYNC_PAGE_SIZE);

   400

        slave->configured_tx_mailbox_offset =

   401

            slave->sii.boot_tx_mailbox_offset;

   402

        slave->configured_tx_mailbox_size =

   403

            slave->sii.boot_tx_mailbox_size;

   404

   405

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

   406

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

   407

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   408

        ec_datagram_zero(datagram);

   409

   410

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

   411

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

   412

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

   413

                    EC_DIR_INVALID, // use default direction

   414

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   415

        }

   416

   417

        slave->configured_rx_mailbox_offset =

   418

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

   419

        slave->configured_rx_mailbox_size =

   420

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

   421

        slave->configured_tx_mailbox_offset =

   422

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

   423

        slave->configured_tx_mailbox_size =

   424

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

   425

    } else { // no mailbox sync manager configurations provided

   426

        ec_sync_t sync;

   427

   428

        if (master->debug_level)

   429

            EC_DBG("Slave %u does not provide"

   430

                    " mailbox sync manager configurations.\n",

   431

                    slave->ring_position);

   432

   433

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

   434

                EC_SYNC_PAGE_SIZE * 2);

   435

        ec_datagram_zero(datagram);

   436

   437

        ec_sync_init(&sync, slave);

   438

        sync.physical_start_address = slave->sii.std_rx_mailbox_offset;

   439

        sync.control_register = 0x26;

   440

        sync.enable = 1;

   441

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

   442

                EC_DIR_INVALID, // use default direction

   443

                datagram->data);

   444

        slave->configured_rx_mailbox_offset =

   445

            slave->sii.std_rx_mailbox_offset;

   446

        slave->configured_rx_mailbox_size =

   447

            slave->sii.std_rx_mailbox_size;

   448

   449

        ec_sync_init(&sync, slave);

   450

        sync.physical_start_address = slave->sii.std_tx_mailbox_offset;

   451

        sync.control_register = 0x22;

   452

        sync.enable = 1;

   453

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

   454

                EC_DIR_INVALID, // use default direction

   455

                datagram->data + EC_SYNC_PAGE_SIZE);

   456

        slave->configured_tx_mailbox_offset =

   457

            slave->sii.boot_tx_mailbox_offset;

   458

        slave->configured_tx_mailbox_size =

   459

            slave->sii.boot_tx_mailbox_size;

   460

    }

   461

   462

    fsm->take_time = 1;

   463

   464

    fsm->retries = EC_FSM_RETRIES;

   465

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   466

}

   467

   468

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

   469

   470

/** Slave configuration state: SYNC.

   471

 *

   472

 * \todo Timeout for response.

   473

 */

   474

void ec_fsm_slave_config_state_mbox_sync(

   475

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

   476

        )

   477

{

   478

    ec_datagram_t *datagram = fsm->datagram;

   479

    ec_slave_t *slave = fsm->slave;

   480

   481

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

   482

        return;

   483

   484

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   485

        fsm->state = ec_fsm_slave_config_state_error;

   486

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

   487

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

   488

               slave->ring_position, datagram->state);

   489

        return;

   490

    }

   491

   492

    if (fsm->take_time) {

   493

        fsm->take_time = 0;

   494

        fsm->jiffies_start = datagram->jiffies_sent;

   495

    }

   496

   497

    /* Because the sync manager configurations are cleared during the last

   498

     * cycle, some slaves do not immediately respond to the mailbox sync

   499

     * manager configuration datagram. Therefore, resend the datagram for

   500

     * a certain time, if the slave does not respond.

   501

     */

   502

    if (datagram->working_counter == 0) {

   503

        unsigned long diff = datagram->jiffies_received - fsm->jiffies_start;