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 *);

    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_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

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

    72

    73

/** Constructor.

    74

 */

    75

void ec_fsm_slave_config_init(

    76

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

    81

        )

    82

{

    83

    fsm->datagram = datagram;

    87

}

    88

    89

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

    90

    91

/** Destructor.

    92

 */

    93

void ec_fsm_slave_config_clear(

    94

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

    95

        )

    96

{

    97

}

    98

    99

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

   100

   101

/** Start slave configuration state machine.

   102

 */

   103

void ec_fsm_slave_config_start(

   104

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

   105

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

   106

        )

   107

{

   108

    fsm->slave = slave;

   109

    fsm->state = ec_fsm_slave_config_state_start;

   110

}

   111

   112

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

   113

   114

/**

   115

 * \return false, if state machine has terminated

   116

 */

   117

int ec_fsm_slave_config_running(

   118

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

   119

        )

   120

{

   121

    return fsm->state != ec_fsm_slave_config_state_end

   122

        && fsm->state != ec_fsm_slave_config_state_error;

   123

}

   124

   125

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

   126

   127

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

   128

 *

   129

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

   130

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

   131

 *

   132

 * \return false, if state machine has terminated

   133

 */

   134

int ec_fsm_slave_config_exec(

   135

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

   136

        )

   137

{

   138

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

   139

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

   140

        // datagram was not sent or received yet.

   141

        return ec_fsm_slave_config_running(fsm);

   142

    }

   143

   144

    fsm->state(fsm);

   145

    return ec_fsm_slave_config_running(fsm);

   146

}

   147

   148

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

   149

   150

/**

   151

 * \return true, if the state machine terminated gracefully

   152

 */

   153

int ec_fsm_slave_config_success(

   154

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

   155

        )

   156

{

   157

    return fsm->state == ec_fsm_slave_config_state_end;

   158

}

   159

   160

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

   161

 * Slave configuration state machine

   162

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

   163

   164

/** Slave configuration state: START.

   165

 */

   166

void ec_fsm_slave_config_state_start(

   167

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

   168

        )

   169

{

   170

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

   171

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

   172

    }

   173

   174

    // configuration will be done immediately; therefore reset the

   175

    // force flag

   176

    fsm->slave->force_config = 0;

   177

   180

    fsm->state = ec_fsm_slave_config_state_init;

   181

}

   182

   183

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

   184

   185

/** Slave configuration state: INIT.

   186

 */

   187

void ec_fsm_slave_config_state_init(

   188

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

   189

        )

   190

{

   191

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

   192

    ec_slave_t *slave = fsm->slave;

   193

    ec_datagram_t *datagram = fsm->datagram;

   194

   199

            slave->error_flag = 1;

   200

        fsm->state = ec_fsm_slave_config_state_error;

   201

        return;

   202

    }

   203

   204

    if (master->debug_level) {

   205

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

   206

    }

   207

   208

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

   209

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   210

        return;

   211

    }

   212

   213

    if (master->debug_level)

   214

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

   215

               slave->ring_position);

   216

   217

    // clear FMMU configurations

   218

    ec_datagram_fpwr(datagram, slave->station_address,

   219

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   220

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

   221

    fsm->retries = EC_FSM_RETRIES;

   222

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   223

}

   224

   225

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

   226

   227

/** Slave configuration state: CLEAR FMMU.

   228

 */

   229

void ec_fsm_slave_config_state_clear_fmmus(

   230

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

   231

        )

   232

{

   233

    ec_datagram_t *datagram = fsm->datagram;

   234

   235

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

   236

        return;

   237

   238

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   239

        fsm->state = ec_fsm_slave_config_state_error;

   240

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

   241

               fsm->slave->ring_position);

   242

        return;

   243

    }

   244

   245

    if (datagram->working_counter != 1) {

   246

        fsm->slave->error_flag = 1;

   247

        fsm->state = ec_fsm_slave_config_state_error;

   248

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

   249

               fsm->slave->ring_position);

   250

        ec_datagram_print_wc_error(datagram);

   251

        return;

   252

    }

   253

   254

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   255

}

   256

   257

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

   258

   259

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

   260

 */

   261

void ec_fsm_slave_config_enter_mbox_sync(

   262

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

   263

        )

   264

{

   265

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

   266

    ec_slave_t *slave = fsm->slave;

   267

    ec_datagram_t *datagram = fsm->datagram;

   268

    unsigned int i;

   269

   270

    // slave is now in INIT

   271

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

   272

        fsm->state = ec_fsm_slave_config_state_end; // successful

   273

        if (master->debug_level) {

   274

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

   275

                   slave->ring_position);

   276

        }

   277

        return;

   278

    }

   279

   280

    if (!slave->sii.mailbox_protocols) {

   281

        // no mailbox protocols supported

   282

        if (master->debug_level)

   283

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

   284

                    slave->ring_position);

   285

        ec_fsm_slave_config_enter_preop(fsm);

   286

        return;

   287

    }

   288

   289

    if (master->debug_level) {

   290

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

   291

               slave->ring_position);

   292

    }

   293

   294

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

   295

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

   296

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   297

        memset(datagram->data, 0x00,

   298

                EC_SYNC_PAGE_SIZE * slave->sii.sync_count);

   299

   300

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

   301

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

   302

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

   303

                    EC_DIR_INVALID, // use default direction

   304

                    datagram->data + EC_SYNC_PAGE_SIZE * i);

   305

        }

   306

    } else { // no mailbox sync manager configurations provided

   307

        ec_sync_t sync;

   308

   309

        if (master->debug_level)

   310

            EC_DBG("Slave %u does not provide"

   311

                    " mailbox sync manager configurations.\n",

   312

                    slave->ring_position);

   313

   314

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

   315

                EC_SYNC_PAGE_SIZE * 2);

   316

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

   317

   318

        ec_sync_init(&sync, slave);

   319

        sync.physical_start_address = slave->sii.rx_mailbox_offset;

   320

        sync.control_register = 0x26;

   321

        sync.enable = 1;

   322

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

   323

                EC_DIR_INVALID, // use default direction

   324

                datagram->data);

   325

   326

        ec_sync_init(&sync, slave);

   327

        sync.physical_start_address = slave->sii.tx_mailbox_offset;

   328

        sync.control_register = 0x22;

   329

        sync.enable = 1;

   330

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

   331

                EC_DIR_INVALID, // use default direction

   332

                datagram->data + EC_SYNC_PAGE_SIZE);

   333

    }

   334

   335

    fsm->retries = EC_FSM_RETRIES;

   336

    fsm->state = ec_fsm_slave_config_state_mbox_sync;

   337

}

   338

   339

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

   340

   341

/** Slave configuration state: SYNC.

   342

 */

   343

void ec_fsm_slave_config_state_mbox_sync(

   344

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

   345

        )

   346

{

   347

    ec_datagram_t *datagram = fsm->datagram;

   348

    ec_slave_t *slave = fsm->slave;

   349

   350

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

   351

        return;

   352

   353

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   354

        fsm->state = ec_fsm_slave_config_state_error;

   355

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

   356

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

   357

               slave->ring_position, datagram->state);

   358

        return;

   359

    }

   360

   361

    if (datagram->working_counter != 1) {

   362

        slave->error_flag = 1;

   363

        fsm->state = ec_fsm_slave_config_state_error;

   364

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

   365

               slave->ring_position);

   366

        ec_datagram_print_wc_error(datagram);

   367

        return;

   368

    }

   369

   370

    ec_fsm_slave_config_enter_preop(fsm);

   371

}

   372

   373

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

   374

   375

/** Request PREOP state.

   376

 */

   377

void ec_fsm_slave_config_enter_preop(

   378

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

   379

        )

   380

{

   381

    fsm->state = ec_fsm_slave_config_state_preop;

   384

}

   385

   386

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

   387

   388

/** Slave configuration state: PREOP.

   389

 */

   390

void ec_fsm_slave_config_state_preop(

   391

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

   392

        )

   393

{

   394

    ec_slave_t *slave = fsm->slave;

   395

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

   396

   401

            slave->error_flag = 1;

   402

        fsm->state = ec_fsm_slave_config_state_error;

   403

        return;

   404

    }

   405

   406

    // slave is now in PREOP

   407

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

   408

   409

    if (master->debug_level) {

   410

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

   411

    }

   412

   413

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

   414

        fsm->state = ec_fsm_slave_config_state_end; // successful

   415

        if (master->debug_level) {

   416

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

   417

                   slave->ring_position);

   418

        }

   419

        return;

   420

    }

   421

   422

    if (!slave->config) {

   423

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

   424

        ec_fsm_slave_config_enter_safeop(fsm);

   425

        return;

   426

    }

   427

   428

    ec_fsm_slave_config_enter_sdo_conf(fsm);

   429

}

   430

   431

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

   432

   433

/** Check for Sdo configurations to be applied.

   434

 */

   435

void ec_fsm_slave_config_enter_sdo_conf(

   436

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

   437

        )

   438

{

   439

    ec_slave_t *slave = fsm->slave;

   440

   441

    // No CoE configuration to be applied?

   442

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

   443

        ec_fsm_slave_config_enter_pdo_sync(fsm);

   444

        return;

   445

    }

   446

   447

    // start Sdo configuration

   448

    fsm->state = ec_fsm_slave_config_state_sdo_conf;

   449

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

   450

            ec_sdo_request_t, list);

   451

    ecrt_sdo_request_write(fsm->request);

   454

}

   455

   456

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

   457

   458

/** Slave configuration state: SDO_CONF.

   459

 */

   460

void ec_fsm_slave_config_state_sdo_conf(

   461

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

   462

        )

   463

{

   467

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

   468

                fsm->slave->ring_position);

   469

        fsm->slave->error_flag = 1;

   470

        fsm->state = ec_fsm_slave_config_state_error;

   471

        return;

   472

    }

   473

   474

    // Another Sdo to configure?

   475

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

   476

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

   477

                list);

   478

        ecrt_sdo_request_write(fsm->request);

   481

        return;

   482

    }

   483

   484

    // All Sdos are now configured.

   485

    ec_fsm_slave_config_enter_pdo_sync(fsm);

   486

}

   487

   488

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

   489

   490

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

   491

 */

   492

void ec_fsm_slave_config_enter_pdo_sync(

   493

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

   494

        )

   495

{

   496

    ec_slave_t *slave = fsm->slave;

   497

    ec_datagram_t *datagram = fsm->datagram;

   498

    unsigned int i, offset, num_pdo_syncs;

   499

    uint8_t sync_index;

   500

    const ec_sync_t *sync;

   501

    const ec_sync_config_t *sync_config;

   502

    uint16_t size;

   503

   504

    if (slave->sii.mailbox_protocols) {

   505

        offset = 2; // slave has mailboxes

   506

    } else {

   507

        offset = 0;

   508

    }

   509

   510

    if (slave->sii.sync_count <= offset) {