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

    56

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    57

    58

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    62

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    67

    68

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    70

    71

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    72

    73

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

    74

    75

/** Constructor.

    76

 */

    77

void ec_fsm_slave_config_init(

    78

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

    79

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

    80

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

    81

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

    82

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

    83

        )

    84

{

    85

    ec_sdo_request_init(&fsm->request_copy);

    86

    87

    fsm->datagram = datagram;

    88

    fsm->fsm_change = fsm_change;

    89

    fsm->fsm_coe = fsm_coe;

    90

    fsm->fsm_pdo = fsm_pdo;

    91

}

    92

    93

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

    94

    95

/** Destructor.

    96

 */

    97

void ec_fsm_slave_config_clear(

    98

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

    99

        )

   100

{

   101

    ec_sdo_request_clear(&fsm->request_copy);

   102

}

   103

   104

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

   105

   106

/** Start slave configuration state machine.

   107

 */

   108

void ec_fsm_slave_config_start(

   109

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

   110

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

   111

        )

   112

{

   113

    fsm->slave = slave;

   114

    fsm->state = ec_fsm_slave_config_state_start;

   115

}

   116

   117

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

   118

   119

/**

   120

 * \return false, if state machine has terminated

   121

 */

   122

int ec_fsm_slave_config_running(

   123

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

   124

        )

   125

{

   126

    return fsm->state != ec_fsm_slave_config_state_end

   127

        && fsm->state != ec_fsm_slave_config_state_error;

   128

}

   129

   130

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

   131

   132

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

   133

 *

   134

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

   135

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

   136

 *

   137

 * \return false, if state machine has terminated

   138

 */

   139

int ec_fsm_slave_config_exec(

   140

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

   141

        )

   142

{

   143

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

   144

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

   145

        // datagram was not sent or received yet.

   146

        return ec_fsm_slave_config_running(fsm);

   147

    }

   148

   149

    fsm->state(fsm);

   150

    return ec_fsm_slave_config_running(fsm);

   151

}

   152

   153

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

   154

   155

/**

   156

 * \return true, if the state machine terminated gracefully

   157

 */

   158

int ec_fsm_slave_config_success(

   159

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

   160

        )

   161

{

   162

    return fsm->state == ec_fsm_slave_config_state_end;

   163

}

   164

   165

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

   166

 * Slave configuration state machine

   167

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

   168

   169

/** Slave configuration state: START.

   170

 */

   171

void ec_fsm_slave_config_state_start(

   172

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

   173

        )

   174

{

   175

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

   176

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

   177

    }

   178

   179

    ec_fsm_slave_config_enter_init(fsm);

   180

}

   181

   182

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

   183

   184

/** Start state change to INIT.

   185

 */

   186

void ec_fsm_slave_config_enter_init(

   187

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

   188

        )

   189

{

   190

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

   191

    ec_fsm_change_exec(fsm->fsm_change);

   192

    fsm->state = ec_fsm_slave_config_state_init;

   193

}

   194

   195

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

   196

   197

/** Slave configuration state: INIT.

   198

 */

   199

void ec_fsm_slave_config_state_init(

   200

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

   201

        )

   202

{

   203

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

   204

    ec_slave_t *slave = fsm->slave;

   205

    ec_datagram_t *datagram = fsm->datagram;

   206

   207

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

   208

   209

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

   210

        if (!fsm->fsm_change->spontaneous_change)

   211

            slave->error_flag = 1;

   212

        fsm->state = ec_fsm_slave_config_state_error;

   213

        return;

   214

    }

   215

   216

    if (master->debug_level) {

   217

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

   218

    }

   219

   220

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

   221

        ec_fsm_slave_config_enter_clear_sync(fsm);

   222

        return;

   223

    }

   224

   225

    if (master->debug_level)

   226

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

   227

               slave->ring_position);

   228

   229

    // clear FMMU configurations

   230

    ec_datagram_fpwr(datagram, slave->station_address,

   231

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   232

    ec_datagram_zero(datagram);

   233

    fsm->retries = EC_FSM_RETRIES;

   234

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   235

}

   236

   237

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

   238

   239

/** Slave configuration state: CLEAR FMMU.

   240

 */

   241

void ec_fsm_slave_config_state_clear_fmmus(

   242

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

   243

        )

   244

{

   245

    ec_datagram_t *datagram = fsm->datagram;

   246

   247

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

   248

        return;

   249

   250

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   251

        fsm->state = ec_fsm_slave_config_state_error;

   252

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

   253

               fsm->slave->ring_position);

   254

        return;

   255

    }

   256

   257

    if (datagram->working_counter != 1) {

   258

        fsm->slave->error_flag = 1;

   259

        fsm->state = ec_fsm_slave_config_state_error;

   260

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

   261

               fsm->slave->ring_position);

   262

        ec_datagram_print_wc_error(datagram);

   263

        return;

   264

    }

   265

   266

    ec_fsm_slave_config_enter_clear_sync(fsm);

   267

}

   268

   269

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

   270

   271

/** Clear the sync manager configurations.

   272

 */

   273

void ec_fsm_slave_config_enter_clear_sync(

   274

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

   275

        )

   276

{

   277

    ec_slave_t *slave = fsm->slave;

   278

    ec_datagram_t *datagram = fsm->datagram;

   279

    size_t sync_size;

   280

   281

    if (!slave->sii.sync_count) {

   282

        // no mailbox protocols supported

   283

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   284

        return;

   285

    }

   286

   287

    if (slave->master->debug_level)

   288

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

   289

                slave->ring_position);

   290

   291

    sync_size = EC_SYNC_PAGE_SIZE * slave->sii.sync_count;

   292

   293

    // clear sync manager configurations

   294

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

   295

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

   296

    fsm->retries = EC_FSM_RETRIES;

   297

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   298

}

   299

   300

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

   301

   302

/** Slave configuration state: CLEAR SYNC.

   303

 */

   304

void ec_fsm_slave_config_state_clear_sync(

   305

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

   306

        )

   307

{

   308

    ec_datagram_t *datagram = fsm->datagram;

   309

   310

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

   311

        return;

   312

   313

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   314

        fsm->state = ec_fsm_slave_config_state_error;

   315

        EC_ERR("Failed receive sync manager clearing datagram"

   316

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

   317

        return;

   318

    }

   319

   320

    if (datagram->working_counter != 1) {

   321

        fsm->slave->error_flag = 1;

   322

        fsm->state = ec_fsm_slave_config_state_error;

   323

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

   324

               fsm->slave->ring_position);

   325

        ec_datagram_print_wc_error(datagram);

   326

        return;

   327

    }

   328

   329

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   330

}

   331

   332

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

   333

   334

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

   335

 */

   336

void ec_fsm_slave_config_enter_mbox_sync(

   337

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

   338

        )

   339

{

   340

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

   341

    ec_slave_t *slave = fsm->slave;

   342

    ec_datagram_t *datagram = fsm->datagram;

   343

    unsigned int i;

   344

   345

    // slave is now in INIT

   346

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

   347

        fsm->state = ec_fsm_slave_config_state_end; // successful

   348

        if (master->debug_level) {

   349

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

   350

                   slave->ring_position);

   351

        }

   352

        return;

   353

    }

   354

   355

    if (!slave->sii.mailbox_protocols) {

   356

        // no mailbox protocols supported

   357

        if (master->debug_level)

   358

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

   359

                    slave->ring_position);

   360

        ec_fsm_slave_config_enter_boot_preop(fsm);

   361

        return;

   362

    }

   363

   364

    if (master->debug_level) {

   365

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

   366

               slave->ring_position);

   367

    }

   368

   369

    if (slave->requested_state == EC_SLAVE_STATE_BOOT) {

   370

        ec_sync_t sync;

   371

   372

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

   373

                EC_SYNC_PAGE_SIZE * 2);

   374

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

   375

   376

        ec_sync_init(&sync, slave);

   377

        sync.physical_start_address = slave->sii.boot_rx_mailbox_offset;

   378

        sync.control_register = 0x26;

   379

        sync.enable = 1;

   380

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

   381

                EC_DIR_INVALID, // use default direction

   382

                datagram->data);

   383

        slave->configured_rx_mailbox_offset =

   384

            slave->sii.boot_rx_mailbox_offset;

   385

        slave->configured_rx_mailbox_size =

   386

            slave->sii.boot_rx_mailbox_size;

   387

   388

        ec_sync_init(&sync, slave);

   389

        sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;

   390

        sync.control_register = 0x22;

   391

        sync.enable = 1;

   392

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

   393

                EC_DIR_INVALID, // use default direction

   394

                datagram->data + EC_SYNC_PAGE_SIZE);

   395

        slave->configured_tx_mailbox_offset =

   396

            slave->sii.boot_tx_mailbox_offset;

   397

        slave->configured_tx_mailbox_size =

   398

            slave->sii.boot_tx_mailbox_size;

   399

   400

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

   401

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

   402

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   403

        ec_datagram_zero(datagram);

   404

   405

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

   406

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

   407

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

   408

                    EC_DIR_INVALID, // use default direction

   409

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   410

        }

   411

   412

        slave->configured_rx_mailbox_offset =

   413

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

   414

        slave->configured_rx_mailbox_size =

   415

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

   416

        slave->configured_tx_mailbox_offset =

   417

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

   418

        slave->configured_tx_mailbox_size =

   419

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

   420

    } else { // no mailbox sync manager configurations provided

   421

        ec_sync_t sync;

   422

   423

        if (master->debug_level)

   424

            EC_DBG("Slave %u does not provide"

   425

                    " mailbox sync manager configurations.\n",

   426

                    slave->ring_position);

   427

   428

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

   429

                EC_SYNC_PAGE_SIZE * 2);

   430

        ec_datagram_zero(datagram);

   431

   432

        ec_sync_init(&sync, slave);

   433

        sync.physical_start_address = slave->sii.std_rx_mailbox_offset;

   434

        sync.control_register = 0x26;

   435

        sync.enable = 1;

   436

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

   437

                EC_DIR_INVALID, // use default direction

   438

                datagram->data);

   439

        slave->configured_rx_mailbox_offset =

   440

            slave->sii.std_rx_mailbox_offset;

   441

        slave->configured_rx_mailbox_size =

   442

            slave->sii.std_rx_mailbox_size;

   443

   444

        ec_sync_init(&sync, slave);

   445

        sync.physical_start_address = slave->sii.std_tx_mailbox_offset;

   446

        sync.control_register = 0x22;

   447

        sync.enable = 1;

   448

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

   449

                EC_DIR_INVALID, // use default direction

   450

                datagram->data + EC_SYNC_PAGE_SIZE);

   451

        slave->configured_tx_mailbox_offset =

   452

            slave->sii.boot_tx_mailbox_offset;

   453

        slave->configured_tx_mailbox_size =

   454

            slave->sii.boot_tx_mailbox_size;

   455

    }

   456

   457

    fsm->take_time = 1;

   458

   459

    fsm->retries = EC_FSM_RETRIES;

   460

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   461

}

   462

   463

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

   464

   465

/** Slave configuration state: SYNC.

   466

 *

   467

 * \todo Timeout for response.

   468

 */

   469

void ec_fsm_slave_config_state_mbox_sync(

   470

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

   471

        )

   472

{

   473

    ec_datagram_t *datagram = fsm->datagram;

   474

    ec_slave_t *slave = fsm->slave;

   475

   476

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

   477

        return;

   478

   479

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   480

        fsm->state = ec_fsm_slave_config_state_error;

   481

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

   482

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

   483

               slave->ring_position, datagram->state);

   484

        return;

   485

    }

   486

   487

    if (fsm->take_time) {

   488

        fsm->take_time = 0;

   489

        fsm->jiffies_start = datagram->jiffies_sent;

   490

    }

   491

   492

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

   493

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

   494

     * manager configuration datagram. Therefore, resend the datagram for

   495

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

   496

     */

   497

    if (datagram->working_counter == 0) {

   498

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

   499

   500

        if (diff >= HZ) {

   501

            slave->error_flag = 1;

   502

            fsm->state = ec_fsm_slave_config_state_error;

   503

            EC_ERR("Timeout while configuring mailbox sync managers of"