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

    56

void ec_fsm_slave_config_state_dc_offset(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    62

    63

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    68

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_enter_dc_read(ec_fsm_slave_config_t *);

    72

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    73

    74

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    75

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    76

    77

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    78

    79

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

    80

    81

/** Constructor.

    82

 */

    83

void ec_fsm_slave_config_init(

    84

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

    85

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

    86

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

    87

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

    88

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

    89

        )

    90

{

    91

    ec_sdo_request_init(&fsm->request_copy);

    92

    93

    fsm->datagram = datagram;

    94

    fsm->fsm_change = fsm_change;

    95

    fsm->fsm_coe = fsm_coe;

    96

    fsm->fsm_pdo = fsm_pdo;

    97

}

    98

    99

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

   100

   101

/** Destructor.

   102

 */

   103

void ec_fsm_slave_config_clear(

   104

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

   105

        )

   106

{

   107

    ec_sdo_request_clear(&fsm->request_copy);

   108

}

   109

   110

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

   111

   112

/** Start slave configuration state machine.

   113

 */

   114

void ec_fsm_slave_config_start(

   115

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

   116

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

   117

        )

   118

{

   119

    fsm->slave = slave;

   120

    fsm->state = ec_fsm_slave_config_state_start;

   121

}

   122

   123

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

   124

   125

/**

   126

 * \return false, if state machine has terminated

   127

 */

   128

int ec_fsm_slave_config_running(

   129

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

   130

        )

   131

{

   132

    return fsm->state != ec_fsm_slave_config_state_end

   133

        && fsm->state != ec_fsm_slave_config_state_error;

   134

}

   135

   136

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

   137

   138

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

   139

 *

   140

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

   141

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

   142

 *

   143

 * \return false, if state machine has terminated

   144

 */

   145

int ec_fsm_slave_config_exec(

   146

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

   147

        )

   148

{

   149

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

   150

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

   151

        // datagram was not sent or received yet.

   152

        return ec_fsm_slave_config_running(fsm);

   153

    }

   154

   155

    fsm->state(fsm);

   156

    return ec_fsm_slave_config_running(fsm);

   157

}

   158

   159

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

   160

   161

/**

   162

 * \return true, if the state machine terminated gracefully

   163

 */

   164

int ec_fsm_slave_config_success(

   165

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

   166

        )

   167

{

   168

    return fsm->state == ec_fsm_slave_config_state_end;

   169

}

   170

   171

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

   172

 * Slave configuration state machine

   173

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

   174

   175

/** Slave configuration state: START.

   176

 */

   177

void ec_fsm_slave_config_state_start(

   178

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

   179

        )

   180

{

   181

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

   182

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

   183

    }

   184

   185

    ec_fsm_slave_config_enter_init(fsm);

   186

}

   187

   188

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

   189

   190

/** Start state change to INIT.

   191

 */

   192

void ec_fsm_slave_config_enter_init(

   193

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

   194

        )

   195

{

   196

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

   197

    ec_fsm_change_exec(fsm->fsm_change);

   198

    fsm->state = ec_fsm_slave_config_state_init;

   199

}

   200

   201

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

   202

   203

/** Slave configuration state: INIT.

   204

 */

   205

void ec_fsm_slave_config_state_init(

   206

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

   207

        )

   208

{

   209

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

   210

    ec_slave_t *slave = fsm->slave;

   211

    ec_datagram_t *datagram = fsm->datagram;

   212

   213

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

   214

   215

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

   216

        if (!fsm->fsm_change->spontaneous_change)

   217

            slave->error_flag = 1;

   218

        fsm->state = ec_fsm_slave_config_state_error;

   219

        return;

   220

    }

   221

   222

    if (master->debug_level) {

   223

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

   224

    }

   225

   226

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

   227

        ec_fsm_slave_config_enter_clear_sync(fsm);

   228

        return;

   229

    }

   230

   231

    if (master->debug_level)

   232

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

   233

               slave->ring_position);

   234

   235

    // clear FMMU configurations

   236

    ec_datagram_fpwr(datagram, slave->station_address,

   237

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   238

    ec_datagram_zero(datagram);

   239

    fsm->retries = EC_FSM_RETRIES;

   240

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   241

}

   242

   243

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

   244

   245

/** Slave configuration state: CLEAR FMMU.

   246

 */

   247

void ec_fsm_slave_config_state_clear_fmmus(

   248

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

   249

        )

   250

{

   251

    ec_datagram_t *datagram = fsm->datagram;

   252

   253

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

   254

        return;

   255

   256

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   257

        fsm->state = ec_fsm_slave_config_state_error;

   258

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

   259

               fsm->slave->ring_position);

   260

        return;

   261

    }

   262

   263

    if (datagram->working_counter != 1) {

   264

        fsm->slave->error_flag = 1;

   265

        fsm->state = ec_fsm_slave_config_state_error;

   266

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

   267

               fsm->slave->ring_position);

   268

        ec_datagram_print_wc_error(datagram);

   269

        return;

   270

    }

   271

   272

    ec_fsm_slave_config_enter_clear_sync(fsm);

   273

}

   274

   275

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

   276

   277

/** Clear the sync manager configurations.

   278

 */

   279

void ec_fsm_slave_config_enter_clear_sync(

   280

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

   281

        )

   282

{

   283

    ec_slave_t *slave = fsm->slave;

   284

    ec_datagram_t *datagram = fsm->datagram;

   285

    size_t sync_size;

   286

   289

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   290

        return;

   291

    }

   292

   293

    if (slave->master->debug_level)

   294

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

   295

                slave->ring_position);

   296

   297

    sync_size = EC_SYNC_PAGE_SIZE * slave->sii.sync_count;

   298

   299

    // clear sync manager configurations

   300

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

   301

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

   302

    fsm->retries = EC_FSM_RETRIES;

   303

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   304

}

   305

   306

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

   307

   308

/** Slave configuration state: CLEAR SYNC.

   309

 */

   310

void ec_fsm_slave_config_state_clear_sync(

   311

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

   312

        )

   313

{

   314

    ec_datagram_t *datagram = fsm->datagram;

   315

   316

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

   317

        return;

   318

   319

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   320

        fsm->state = ec_fsm_slave_config_state_error;

   321

        EC_ERR("Failed receive sync manager clearing datagram"

   322

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

   323

        return;

   324

    }

   325

   326

    if (datagram->working_counter != 1) {

   327

        fsm->slave->error_flag = 1;

   328

        fsm->state = ec_fsm_slave_config_state_error;

   329

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

   330

               fsm->slave->ring_position);

   331

        ec_datagram_print_wc_error(datagram);

   332

        return;

   333

    }

   334

   335

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   336

}

   337

   338

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

   339

   340

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

   341

 */

   342

void ec_fsm_slave_config_enter_mbox_sync(

   343

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

   344

        )

   345

{

   346

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

   347

    ec_slave_t *slave = fsm->slave;

   348

    ec_datagram_t *datagram = fsm->datagram;

   349

    unsigned int i;

   350

   351

    // slave is now in INIT

   352

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

   353

        fsm->state = ec_fsm_slave_config_state_end; // successful

   354

        if (master->debug_level) {

   355

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

   356

                   slave->ring_position);

   357

        }

   358

        return;

   359

    }

   360

   361

    if (!slave->sii.mailbox_protocols) {

   362

        // no mailbox protocols supported

   363

        if (master->debug_level)

   364

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

   365

                    slave->ring_position);

   366

        ec_fsm_slave_config_enter_boot_preop(fsm);

   367

        return;

   368

    }

   369

   370

    if (master->debug_level) {

   371

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

   372

               slave->ring_position);

   373

    }

   374

   375

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   376

        ec_sync_t sync;

   377

   378

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

   379

                EC_SYNC_PAGE_SIZE * 2);

   380

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

   381

   382

        ec_sync_init(&sync, slave);

   383

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   384

        sync.control_register = 0x26;

   385

        sync.enable = 1;

   386

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

   387

                EC_DIR_INVALID, // use default direction

   388

                datagram->data);

   389

        slave->configured_rx_mailbox_offset =

   390

            slave->sii.boot_rx_mailbox_offset;

   391

        slave->configured_rx_mailbox_size =

   392

            slave->sii.boot_rx_mailbox_size;

   393

   394

        ec_sync_init(&sync, slave);

   395

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   396

        sync.control_register = 0x22;

   397

        sync.enable = 1;

   398

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

   399

                EC_DIR_INVALID, // use default direction

   400

                datagram->data + EC_SYNC_PAGE_SIZE);

   401

        slave->configured_tx_mailbox_offset =

   402

            slave->sii.boot_tx_mailbox_offset;

   403

        slave->configured_tx_mailbox_size =

   404

            slave->sii.boot_tx_mailbox_size;

   405

   406

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

   407

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

   408

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   409

        ec_datagram_zero(datagram);

   410

   411

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

   412

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

   413

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

   414

                    EC_DIR_INVALID, // use default direction

   415

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   416

        }

   417

   418

        slave->configured_rx_mailbox_offset =

   419

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

   420

        slave->configured_rx_mailbox_size =

   421

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

   422

        slave->configured_tx_mailbox_offset =

   423

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

   424

        slave->configured_tx_mailbox_size =

   425

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

   426

    } else { // no mailbox sync manager configurations provided

   427

        ec_sync_t sync;

   428

   429

        if (master->debug_level)

   430

            EC_DBG("Slave %u does not provide"

   431

                    " mailbox sync manager configurations.\n",

   432

                    slave->ring_position);

   433

   434

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

   435

                EC_SYNC_PAGE_SIZE * 2);

   436

        ec_datagram_zero(datagram);

   437

   438

        ec_sync_init(&sync, slave);

   439

        sync.physical_start_address = slave->sii.std_rx_mailbox_offset;

   440

        sync.control_register = 0x26;

   441

        sync.enable = 1;

   442

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

   443

                EC_DIR_INVALID, // use default direction

   444

                datagram->data);

   445

        slave->configured_rx_mailbox_offset =

   446

            slave->sii.std_rx_mailbox_offset;

   447

        slave->configured_rx_mailbox_size =

   448

            slave->sii.std_rx_mailbox_size;

   449

   450

        ec_sync_init(&sync, slave);

   451

        sync.physical_start_address = slave->sii.std_tx_mailbox_offset;

   452

        sync.control_register = 0x22;

   453

        sync.enable = 1;

   454

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

   455

                EC_DIR_INVALID, // use default direction

   456

                datagram->data + EC_SYNC_PAGE_SIZE);

   457

        slave->configured_tx_mailbox_offset =

   458

            slave->sii.boot_tx_mailbox_offset;

   459

        slave->configured_tx_mailbox_size =

   460

            slave->sii.boot_tx_mailbox_size;

   461

    }

   462

   463

    fsm->take_time = 1;

   464

   465

    fsm->retries = EC_FSM_RETRIES;

   466

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   467

}

   468

   469

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

   470

   471

/** Slave configuration state: SYNC.

   472

 *

   473

 * \todo Timeout for response.

   474

 */

   475

void ec_fsm_slave_config_state_mbox_sync(

   476

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

   477

        )

   478

{

   479

    ec_datagram_t *datagram = fsm->datagram;

   480

    ec_slave_t *slave = fsm->slave;

   481

   482

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

   483

        return;

   484

   485

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   486

        fsm->state = ec_fsm_slave_config_state_error;

   487

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

   488

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

   489

               slave->ring_position, datagram->state);

   490

        return;

   491

    }

   492

   493

    if (fsm->take_time) {

   494

        fsm->take_time = 0;

   495

        fsm->jiffies_start = datagram->jiffies_sent;

   496

    }

   497

   498

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

   499

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

   500

     * manager configuration datagram. Therefore, resend the datagram for

   501

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

   502

     */

   503

    if (datagram->working_counter == 0) {