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

    45

#include "slave_config.h"

    46

#include "fsm_slave.h"

    47

    48

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

    49

    50

void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);

    51

void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);

    52

void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *);

    53

void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *);

    54

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *);

    55

void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *);

    56

void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *);

    57

    58

void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *);

    59

void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *);

    60

void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *);

    61

void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *);

    62

void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);

    63

void ec_fsm_slave_conf_state_sdo_conf(ec_fsm_slave_t *);

    64

void ec_fsm_slave_conf_state_mapping(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_saveop(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_pdo_sync(ec_fsm_slave_t *);

    73

void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);

    74

void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *);

    75

void ec_fsm_slave_conf_enter_mapping(ec_fsm_slave_t *);

    76

void ec_fsm_slave_conf_enter_saveop(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

/**

    84

   Constructor.

    85

*/

    86

    87

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

    88

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

    89

        )

    90

{

    91

    fsm->datagram = datagram;

    92

    93

    // init sub state machines

    94

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

    95

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

    96

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

    97

    ec_fsm_mapping_init(&fsm->fsm_map, &fsm->fsm_coe);

    98

}

    99

   100

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

   101

   102

/**

   103

   Destructor.

   104

*/

   105

   106

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

   107

{

   108

    // clear sub state machines

   109

    ec_fsm_sii_clear(&fsm->fsm_sii);

   110

    ec_fsm_change_clear(&fsm->fsm_change);

   111

    ec_fsm_coe_clear(&fsm->fsm_coe);

   112

    ec_fsm_mapping_clear(&fsm->fsm_map);

   113

}

   114

   115

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

   116

   117

/**

   118

 * Start slave scan state machine.

   119

 */

   120

   121

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

   122

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

   123

        )

   124

{

   125

    fsm->slave = slave;

   126

    fsm->state = ec_fsm_slave_scan_state_start;

   127

}

   128

   129

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

   130

   131

/**

   132

 * Start slave configuration state machine.

   133

 */

   134

   135

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

   136

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

   137

        )

   138

{

   139

    fsm->slave = slave;

   140

    fsm->state = ec_fsm_slave_conf_state_start;

   141

}

   142

   143

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

   144

   145

/**

   146

   \return false, if state machine has terminated

   147

*/

   148

   149

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

   150

{

   151

    return fsm->state != ec_fsm_slave_state_end

   152

        && fsm->state != ec_fsm_slave_state_error;

   153

}

   154

   155

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

   156

   157

/**

   158

   Executes the current state of the state machine.

   159

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

   160

   of the state machine is delayed to the next cycle.

   161

   \return false, if state machine has terminated

   162

*/

   163

   164

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

   165

{

   166

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

   167

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

   168

        // datagram was not sent or received yet.

   169

        return ec_fsm_slave_running(fsm);

   170

    }

   171

   172

    fsm->state(fsm);

   173

    return ec_fsm_slave_running(fsm);

   174

}

   175

   176

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

   177

   178

/**

   179

   \return true, if the state machine terminated gracefully

   180

*/

   181

   182

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

   183

{

   184

    return fsm->state == ec_fsm_slave_state_end;

   185

}

   186

   187

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

   188

 *  slave scan state machine

   189

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

   190

   191

/**

   192

   Slave scan state: START.

   193

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

   194

   slave, according to its ring position.

   195

*/

   196

   197

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

   198

{

   199

    // write station address

   200

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

   201

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

   202

    fsm->retries = EC_FSM_RETRIES;

   203

    fsm->state = ec_fsm_slave_scan_state_address;

   204

}

   205

   206

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

   207

   208

/**

   209

   Slave scan state: ADDRESS.

   210

*/

   211

   212

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

   213

{

   214

    ec_datagram_t *datagram = fsm->datagram;

   215

   216

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

   217

        return;

   218

   219

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   220

        fsm->state = ec_fsm_slave_state_error;

   221

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

   222

                " (datagram state %i)\n",

   223

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

   224

        return;

   225

    }

   226

   227

    if (datagram->working_counter != 1) {

   228

        fsm->slave->error_flag = 1;

   229

        fsm->state = ec_fsm_slave_state_error;

   230

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

   231

               fsm->slave->ring_position);

   232

        ec_datagram_print_wc_error(datagram);

   233

        return;

   234

    }

   235

   236

    // Read AL state

   237

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

   238

    fsm->retries = EC_FSM_RETRIES;

   239

    fsm->state = ec_fsm_slave_scan_state_state;

   240

}

   241

   242

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

   243

   244

/**

   245

   Slave scan state: STATE.

   246

*/

   247

   248

void ec_fsm_slave_scan_state_state(

   249

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

   250

        )

   251

{

   252

    ec_datagram_t *datagram = fsm->datagram;

   253

    ec_slave_t *slave = fsm->slave;

   254

   255

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

   256

        return;

   257

   258

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   259

        fsm->state = ec_fsm_slave_state_error;

   260

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

   261

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

   262

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

   263

        return;

   264

    }

   265

   266

    if (datagram->working_counter != 1) {

   267

        fsm->slave->error_flag = 1;

   268

        fsm->state = ec_fsm_slave_state_error;

   269

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

   270

               fsm->slave->ring_position);

   271

        ec_datagram_print_wc_error(datagram);

   272

        return;

   273

    }

   274

   275

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

   276

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   277

        char state_str[EC_STATE_STRING_SIZE];

   278

        ec_state_string(slave->current_state, state_str);

   279

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

   280

                slave->ring_position, state_str);

   281

    }

   282

   283

    // read base data

   284

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

   285

    fsm->retries = EC_FSM_RETRIES;

   286

    fsm->state = ec_fsm_slave_scan_state_base;

   287

}

   288

   289

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

   290

   291

/**

   292

   Slave scan state: BASE.

   293

*/

   294

   295

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

   296

{

   297

    ec_datagram_t *datagram = fsm->datagram;

   298

    ec_slave_t *slave = fsm->slave;

   299

   300

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

   301

        return;

   302

   303

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   304

        fsm->state = ec_fsm_slave_state_error;

   305

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

   306

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

   307

               slave->ring_position, datagram->state);

   308

        return;

   309

    }

   310

   311

    if (datagram->working_counter != 1) {

   312

        fsm->slave->error_flag = 1;

   313

        fsm->state = ec_fsm_slave_state_error;

   314

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

   315

               slave->ring_position);

   316

        ec_datagram_print_wc_error(datagram);

   317

        return;

   318

    }

   319

   320

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

   321

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

   322

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

   323

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

   324

   325

    if (slave->base_fmmu_count > EC_MAX_FMMUS)

   326

        slave->base_fmmu_count = EC_MAX_FMMUS;

   327

   328

    // read data link status

   329

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

   330

    fsm->retries = EC_FSM_RETRIES;

   331

    fsm->state = ec_fsm_slave_scan_state_datalink;

   332

}

   333

   334

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

   335

   336

/**

   337

   Slave scan state: DATALINK.

   338

*/

   339

   340

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

   341

{

   342

    ec_datagram_t *datagram = fsm->datagram;

   343

    ec_slave_t *slave = fsm->slave;

   344

    uint16_t dl_status;

   345

    unsigned int i;

   346

   347

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

   348

        return;

   349

   350

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   351

        fsm->state = ec_fsm_slave_state_error;

   352

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

   353

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

   354

               slave->ring_position, datagram->state);

   355

        return;

   356

    }

   357

   358

    if (datagram->working_counter != 1) {

   359

        fsm->slave->error_flag = 1;

   360

        fsm->state = ec_fsm_slave_state_error;

   361

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

   362

               slave->ring_position);

   363

        ec_datagram_print_wc_error(datagram);

   364

        return;

   365

    }

   366

   367

    dl_status = EC_READ_U16(datagram->data);

   368

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

   369

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

   370

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

   371

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

   372

    }

   373

   374

    // Start fetching EEPROM size

   375

   376

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

   377

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

   378

    fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   379

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

   380

}

   381

   382

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

   383

   384

/**

   385

   Slave scan state: EEPROM SIZE.

   386

*/

   387

   388

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

   389

{

   390

    ec_slave_t *slave = fsm->slave;

   391

    uint16_t cat_type, cat_size;

   392

   393

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

   394

   395

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

   396

        fsm->slave->error_flag = 1;

   397

        fsm->state = ec_fsm_slave_state_error;

   398

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

   399

               slave->ring_position);

   400

        return;

   401

    }

   402

   403

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

   404

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

   405

   406

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

   407

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

   408

        if (next_offset >= EC_MAX_EEPROM_SIZE) {

   409

            EC_WARN("EEPROM size of slave %i exceeds"

   410

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

   411

                    slave->ring_position, EC_MAX_EEPROM_SIZE);

   412

            // cut off category data...

   413

            slave->eeprom_size = EC_FIRST_EEPROM_CATEGORY_OFFSET * 2;

   414

            goto alloc_eeprom;

   415

        }

   416

        fsm->sii_offset = next_offset;

   417

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

   418

                        EC_FSM_SII_NODE);

   419

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

   420

        return;

   421

    }

   422

   423

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

   424

   425

alloc_eeprom:

   426

    if (slave->eeprom_data) {

   427

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

   428

                slave->ring_position);

   429

        kfree(slave->eeprom_data);

   430

    }

   431

   432

    if (!(slave->eeprom_data =

   433

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

   434

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

   435

               slave->eeprom_size, slave->ring_position);

   436

        slave->eeprom_size = 0;

   437

        slave->error_flag = 1;

   438

        fsm->state = ec_fsm_slave_state_error;

   439

        return;

   440

    }

   441

   442

    // Start fetching EEPROM contents

   443

   444

    fsm->state = ec_fsm_slave_scan_state_eeprom_data;

   445

    fsm->sii_offset = 0x0000;

   446

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

   447

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

   448

}

   449

   450

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

   451

   452

/**

   453

   Slave scan state: EEPROM DATA.

   454

*/

   455

   456

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

   457

{

   458

    ec_slave_t *slave = fsm->slave;

   459

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

   460

   461

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

   462

   463

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

   464

        fsm->slave->error_flag = 1;

   465

        fsm->state = ec_fsm_slave_state_error;

   466

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

   467

               slave->ring_position);

   468

        return;

   469

    }

   470

   471

    // 2 words fetched

   472

   473

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

   474

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

   475

               fsm->fsm_sii.value, 4);

   476

    }

   477

    else { // copy the last word

   478

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

   479

               fsm->fsm_sii.value, 2);

   480

    }

   481

   482

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

   483

        // fetch the next 2 words

   484

        fsm->sii_offset += 2;

   485

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

   486

                        EC_FSM_SII_NODE);

   487

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

   488

        return;

   489

    }

   490