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

/**

    35

   \file

    36

   EtherCAT slave state machines.

    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.h"

    46

    47

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

    48

    49

void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);

    50

void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);

    51

void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *);

    52

void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *);

    53

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *);

    54

void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *);

    55

void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *);

    56

    57

void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *);

    58

void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *);

    59

void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *);

    60

void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *);

    61

void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);

    62

void ec_fsm_slave_conf_state_sdo_conf(ec_fsm_slave_t *);

    63

void ec_fsm_slave_conf_state_mapping(ec_fsm_slave_t *);

    64

void ec_fsm_slave_conf_state_pdo_conf(ec_fsm_slave_t *);

    65

void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *);

    66

void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);

    67

void ec_fsm_slave_conf_state_safeop(ec_fsm_slave_t *);

    68

void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);

    69

    70

void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *);

    71

void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);

    72

void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *);

    73

void ec_fsm_slave_conf_enter_mapping(ec_fsm_slave_t *);

    74

void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *);

    75

void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);

    76

void ec_fsm_slave_conf_enter_safeop(ec_fsm_slave_t *);

    77

    78

void ec_fsm_slave_state_end(ec_fsm_slave_t *);

    79

void ec_fsm_slave_state_error(ec_fsm_slave_t *);

    80

    81

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

    82

    83

/** Constructor.

    84

 */

    85

void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */

    86

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

    87

        )

    88

{

    89

    fsm->datagram = datagram;

    90

    91

    // init sub state machines

    92

    ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);

    93

    ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);

    94

    ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);

    95

    ec_fsm_pdo_mapping_init(&fsm->fsm_pdo_map, &fsm->fsm_coe);

    96

    ec_fsm_pdo_config_init(&fsm->fsm_pdo_conf, &fsm->fsm_coe);

    97

}

    98

    99

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

   100

   101

/** Destructor.

   102

 */

   103

void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */)

   104

{

   105

    // clear sub state machines

   106

    ec_fsm_sii_clear(&fsm->fsm_sii);

   107

    ec_fsm_change_clear(&fsm->fsm_change);

   108

    ec_fsm_coe_clear(&fsm->fsm_coe);

   109

    ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_map);

   110

    ec_fsm_pdo_config_clear(&fsm->fsm_pdo_conf);

   111

}

   112

   113

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

   114

   115

/**

   116

 * Start slave scan state machine.

   117

 */

   118

   119

void ec_fsm_slave_start_scan(ec_fsm_slave_t *fsm, /**< slave state machine */

   120

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

   121

        )

   122

{

   123

    fsm->slave = slave;

   124

    fsm->state = ec_fsm_slave_scan_state_start;

   125

}

   126

   127

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

   128

   129

/**

   130

 * Start slave configuration state machine.

   131

 */

   132

   133

void ec_fsm_slave_start_conf(ec_fsm_slave_t *fsm, /**< slave state machine */

   134

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

   135

        )

   136

{

   137

    fsm->slave = slave;

   138

    fsm->state = ec_fsm_slave_conf_state_start;

   139

}

   140

   141

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

   142

   143

/**

   144

   \return false, if state machine has terminated

   145

*/

   146

   147

int ec_fsm_slave_running(const ec_fsm_slave_t *fsm /**< slave state machine */)

   148

{

   149

    return fsm->state != ec_fsm_slave_state_end

   150

        && fsm->state != ec_fsm_slave_state_error;

   151

}

   152

   153

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

   154

   155

/**

   156

   Executes the current state of the state machine.

   157

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

   158

   of the state machine is delayed to the next cycle.

   159

   \return false, if state machine has terminated

   160

*/

   161

   162

int ec_fsm_slave_exec(ec_fsm_slave_t *fsm /**< slave state machine */)

   163

{

   164

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

   165

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

   166

        // datagram was not sent or received yet.

   167

        return ec_fsm_slave_running(fsm);

   168

    }

   169

   170

    fsm->state(fsm);

   171

    return ec_fsm_slave_running(fsm);

   172

}

   173

   174

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

   175

   176

/**

   177

   \return true, if the state machine terminated gracefully

   178

*/

   179

   180

int ec_fsm_slave_success(const ec_fsm_slave_t *fsm /**< slave state machine */)

   181

{

   182

    return fsm->state == ec_fsm_slave_state_end;

   183

}

   184

   185

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

   186

 *  slave scan state machine

   187

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

   188

   189

/**

   190

   Slave scan state: START.

   191

   First state of the slave state machine. Writes the station address to the

   192

   slave, according to its ring position.

   193

*/

   194

   195

void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *fsm /**< slave state machine */)

   196

{

   197

    // write station address

   198

    ec_datagram_apwr(fsm->datagram, fsm->slave->ring_position, 0x0010, 2);

   199

    EC_WRITE_U16(fsm->datagram->data, fsm->slave->station_address);

   200

    fsm->retries = EC_FSM_RETRIES;

   201

    fsm->state = ec_fsm_slave_scan_state_address;

   202

}

   203

   204

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

   205

   206

/**

   207

   Slave scan state: ADDRESS.

   208

*/

   209

   210

void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *fsm /**< slave state machine */)

   211

{

   212

    ec_datagram_t *datagram = fsm->datagram;

   213

   214

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

   215

        return;

   216

   217

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   218

        fsm->state = ec_fsm_slave_state_error;

   219

        EC_ERR("Failed to receive station address datagram for slave %i"

   220

                " (datagram state %i)\n",

   221

                fsm->slave->ring_position, datagram->state);

   222

        return;

   223

    }

   224

   225

    if (datagram->working_counter != 1) {

   226

        fsm->slave->error_flag = 1;

   227

        fsm->state = ec_fsm_slave_state_error;

   228

        EC_ERR("Failed to write station address on slave %i: ",

   229

               fsm->slave->ring_position);

   230

        ec_datagram_print_wc_error(datagram);

   231

        return;

   232

    }

   233

   234

    // Read AL state

   235

    ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0130, 2);

   236

    fsm->retries = EC_FSM_RETRIES;

   237

    fsm->state = ec_fsm_slave_scan_state_state;

   238

}

   239

   240

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

   241

   242

/**

   243

   Slave scan state: STATE.

   244

*/

   245

   246

void ec_fsm_slave_scan_state_state(

   247

        ec_fsm_slave_t *fsm /**< slave state machine */

   248

        )

   249

{

   250

    ec_datagram_t *datagram = fsm->datagram;

   251

    ec_slave_t *slave = fsm->slave;

   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_state_error;

   258

        EC_ERR("Failed to receive AL state datagram from slave %i"

   259

                " (datagram state %i).\n",

   260

               fsm->slave->ring_position, datagram->state);

   261

        return;

   262

    }

   263

   264

    if (datagram->working_counter != 1) {

   265

        fsm->slave->error_flag = 1;

   266

        fsm->state = ec_fsm_slave_state_error;

   267

        EC_ERR("Failed to read AL state of slave %i: ",

   268

               fsm->slave->ring_position);

   269

        ec_datagram_print_wc_error(datagram);

   270

        return;

   271

    }

   272

   273

    slave->current_state = EC_READ_U8(datagram->data);

   274

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   275

        char state_str[EC_STATE_STRING_SIZE];

   276

        ec_state_string(slave->current_state, state_str);

   277

        EC_WARN("Slave %i has state error bit set (%s)!\n",

   278

                slave->ring_position, state_str);

   279

    }

   280

   281

    // read base data

   282

    ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0000, 6);

   283

    fsm->retries = EC_FSM_RETRIES;

   284

    fsm->state = ec_fsm_slave_scan_state_base;

   285

}

   286

   287

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

   288

   289

/**

   290

   Slave scan state: BASE.

   291

*/

   292

   293

void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *fsm /**< slave state machine */)

   294

{

   295

    ec_datagram_t *datagram = fsm->datagram;

   296

    ec_slave_t *slave = fsm->slave;

   297

   298

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

   299

        return;

   300

   301

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   302

        fsm->state = ec_fsm_slave_state_error;

   303

        EC_ERR("Failed to receive base data datagram for slave %i"

   304

                " (datagram state %i).\n",

   305

               slave->ring_position, datagram->state);

   306

        return;

   307

    }

   308

   309

    if (datagram->working_counter != 1) {

   310

        fsm->slave->error_flag = 1;

   311

        fsm->state = ec_fsm_slave_state_error;

   312

        EC_ERR("Failed to read base data from slave %i: ",

   313

               slave->ring_position);

   314

        ec_datagram_print_wc_error(datagram);

   315

        return;

   316

    }

   317

   318

    slave->base_type       = EC_READ_U8 (datagram->data);

   319

    slave->base_revision   = EC_READ_U8 (datagram->data + 1);

   320

    slave->base_build      = EC_READ_U16(datagram->data + 2);

   321

    slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4);

   322

   323

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   324

        EC_WARN("Slave %u has more FMMUs (%u) than the master can"

   325

                " handle (%u).\n", slave->ring_position,

   326

                slave->base_fmmu_count, EC_MAX_FMMUS);

   327

        slave->base_fmmu_count = EC_MAX_FMMUS;

   328

    }

   329

   330

    // read data link status

   331

    ec_datagram_fprd(datagram, slave->station_address, 0x0110, 2);

   332

    fsm->retries = EC_FSM_RETRIES;

   333

    fsm->state = ec_fsm_slave_scan_state_datalink;

   334

}

   335

   336

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

   337

   338

/**

   339

   Slave scan state: DATALINK.

   340

*/

   341

   342

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *fsm /**< slave state machine */)

   343

{

   344

    ec_datagram_t *datagram = fsm->datagram;

   345

    ec_slave_t *slave = fsm->slave;

   346

    uint16_t dl_status;

   347

    unsigned int i;

   348

   349

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

   350

        return;

   351

   352

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   353

        fsm->state = ec_fsm_slave_state_error;

   354

        EC_ERR("Failed to receive DL status datagram from slave %i"

   355

                " (datagram state %i).\n",

   356

               slave->ring_position, datagram->state);

   357

        return;

   358

    }

   359

   360

    if (datagram->working_counter != 1) {

   361

        fsm->slave->error_flag = 1;

   362

        fsm->state = ec_fsm_slave_state_error;

   363

        EC_ERR("Failed to read DL status from slave %i: ",

   364

               slave->ring_position);

   365

        ec_datagram_print_wc_error(datagram);

   366

        return;

   367

    }

   368

   369

    dl_status = EC_READ_U16(datagram->data);

   370

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

   371

        slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0;

   372

        slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;

   373

        slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;

   374

    }

   375

   376

    // Start fetching EEPROM size

   377

   378

    fsm->sii_offset = EC_FIRST_EEPROM_CATEGORY_OFFSET; // first category header

   379

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   380

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   381

    fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   382

    fsm->state(fsm); // execute state immediately

   383

}

   384

   385

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

   386

   387

/**

   388

   Slave scan state: EEPROM SIZE.

   389

*/

   390

   391

void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *fsm /**< slave state machine */)

   392

{

   393

    ec_slave_t *slave = fsm->slave;

   394

    uint16_t cat_type, cat_size;

   395

   396

    if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   397

   398

    if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   399

        fsm->slave->error_flag = 1;

   400

        fsm->state = ec_fsm_slave_state_error;

   401

        EC_ERR("Failed to read EEPROM size of slave %i.\n",

   402

               slave->ring_position);

   403

        return;

   404

    }

   405

   406

    cat_type = EC_READ_U16(fsm->fsm_sii.value);

   407

    cat_size = EC_READ_U16(fsm->fsm_sii.value + 2);

   408

   409

    if (cat_type != 0xFFFF) { // not the last category

   410

        off_t next_offset = 2UL + fsm->sii_offset + cat_size;

   411

        if (next_offset >= EC_MAX_EEPROM_SIZE) {

   412

            EC_WARN("EEPROM size of slave %i exceeds"

   413

                    " %u words (0xffff limiter missing?).\n",

   414

                    slave->ring_position, EC_MAX_EEPROM_SIZE);

   415

            // cut off category data...

   416

            slave->eeprom_size = EC_FIRST_EEPROM_CATEGORY_OFFSET * 2;

   417

            goto alloc_eeprom;

   418

        }

   419

        fsm->sii_offset = next_offset;

   420

        ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   421

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   422

        ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   423

        return;

   424

    }

   425

   426

    slave->eeprom_size = (fsm->sii_offset + 1) * 2;

   427

   428

alloc_eeprom:

   429

    if (slave->eeprom_data) {

   430

        EC_WARN("Freeing old EEPROM data on slave %i...\n",

   431

                slave->ring_position);

   432

        kfree(slave->eeprom_data);

   433

    }

   434

   435

    if (!(slave->eeprom_data =

   436

                (uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) {

   437

        EC_ERR("Failed to allocate %u bytes of EEPROM data for slave %u.\n",

   438

               slave->eeprom_size, slave->ring_position);

   439

        slave->eeprom_size = 0;

   440

        slave->error_flag = 1;

   441

        fsm->state = ec_fsm_slave_state_error;

   442

        return;

   443

    }

   444

   445

    // Start fetching EEPROM contents

   446

   447

    fsm->state = ec_fsm_slave_scan_state_eeprom_data;

   448

    fsm->sii_offset = 0x0000;

   449

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   450

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   451

    ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   452

}

   453

   454

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

   455

   456

/**

   457

   Slave scan state: EEPROM DATA.

   458

*/

   459

   460

void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *fsm /**< slave state machine */)

   461

{

   462

    ec_slave_t *slave = fsm->slave;

   463

    uint16_t *cat_word, cat_type, cat_size, eeprom_word_size = slave->eeprom_size / 2;

   464

   465

    if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   466

   467

    if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   468

        fsm->slave->error_flag = 1;

   469

        fsm->state = ec_fsm_slave_state_error;

   470

        EC_ERR("Failed to fetch EEPROM contents of slave %i.\n",

   471

               slave->ring_position);

   472

        return;

   473

    }

   474

   475

    // 2 words fetched

   476

   477

    if (fsm->sii_offset + 2 <= eeprom_word_size) { // 2 words fit

   478

        memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   479

               fsm->fsm_sii.value, 4);

   480

    }

   481

    else { // copy the last word

   482

        memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   483

               fsm->fsm_sii.value, 2);

   484

    }

   485

   486

    if (fsm->sii_offset + 2 < eeprom_word_size) {

   487

        // fetch the next 2 words

   488

        fsm->sii_offset += 2;

   489

        ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,