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

    45

    46

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

    47

    48

void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);

    49

void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);

    50

void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *);

    51

void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *);

    52

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *);

    53

void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *);

    54

void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *);

    55

    56

void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *);

    57

void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *);

    58

void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *);

    59

void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *);

    60

void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);

    61

void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_t *);

    62

void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);

    63

void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *);

    64

void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *);

    65

void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);

    66

    67

void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *);

    68

void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);

    69

void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *);

    70

void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);

    71

void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *);

    72

void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *);

    73

    74

void ec_fsm_slave_state_end(ec_fsm_slave_t *);

    75

void ec_fsm_slave_state_error(ec_fsm_slave_t *);

    76

    77

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

    78

    79

/**

    80

   Constructor.

    81

*/

    82

    83

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

    84

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

    85

        )

    86

{

    87

    fsm->datagram = datagram;

    88

    89

    // init sub state machines

    90

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

    91

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

    92

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

    93

}

    94

    95

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

    96

    97

/**

    98

   Destructor.

    99

*/

   100

   101

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

   102

{

   103

    // clear sub state machines

   104

    ec_fsm_sii_clear(&fsm->fsm_sii);

   105

    ec_fsm_change_clear(&fsm->fsm_change);

   106

    ec_fsm_coe_clear(&fsm->fsm_coe);

   107

}

   108

   109

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

   110

   111

/**

   112

 * Start slave scan state machine.

   113

 */

   114

   115

void ec_fsm_slave_start_scan(ec_fsm_slave_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_scan_state_start;

   121

}

   122

   123

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

   124

   125

/**

   126

 * Start slave configuration state machine.

   127

 */

   128

   129

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

   130

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

   131

        )

   132

{

   133

    fsm->slave = slave;

   134

    fsm->state = ec_fsm_slave_conf_state_start;

   135

}

   136

   137

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

   138

   139

/**

   140

   \return false, if state machine has terminated

   141

*/

   142

   143

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

   144

{

   145

    return fsm->state != ec_fsm_slave_state_end

   146

        && fsm->state != ec_fsm_slave_state_error;

   147

}

   148

   149

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

   150

   151

/**

   152

   Executes the current state of the state machine.

   153

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

   154

   of the state machine is delayed to the next cycle.

   155

   \return false, if state machine has terminated

   156

*/

   157

   158

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

   159

{

   160

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

   161

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

   162

        // datagram was not sent or received yet.

   163

        return ec_fsm_slave_running(fsm);

   164

    }

   165

   166

    fsm->state(fsm);

   167

    return ec_fsm_slave_running(fsm);

   168

}

   169

   170

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

   171

   172

/**

   173

   \return true, if the state machine terminated gracefully

   174

*/

   175

   176

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

   177

{

   178

    return fsm->state == ec_fsm_slave_state_end;

   179

}

   180

   181

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

   182

 *  slave scan state machine

   183

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

   184

   185

/**

   186

   Slave scan state: START.

   187

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

   188

   slave, according to its ring position.

   189

*/

   190

   191

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

   192

{

   193

    // write station address

   194

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

   195

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

   196

    ec_master_queue_datagram(fsm->slave->master, fsm->datagram);

   197

    fsm->retries = EC_FSM_RETRIES;

   198

    fsm->state = ec_fsm_slave_scan_state_address;

   199

}

   200

   201

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

   202

   203

/**

   204

   Slave scan state: ADDRESS.

   205

*/

   206

   207

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

   208

{

   209

    ec_datagram_t *datagram = fsm->datagram;

   210

   211

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

   212

        ec_master_queue_datagram(fsm->slave->master, fsm->datagram);

   213

        return;

   214

    }

   215

   216

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   217

        fsm->state = ec_fsm_slave_state_error;

   218

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

   219

                " (datagram state %i)\n",

   220

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

   221

        return;

   222

    }

   223

   224

    if (datagram->working_counter != 1) {

   225

        fsm->slave->error_flag = 1;

   226

        fsm->state = ec_fsm_slave_state_error;

   227

        EC_ERR("Failed to write station address - slave %i did not respond.\n",

   228

               fsm->slave->ring_position);

   229

        return;

   230

    }

   231

   232

    // Read AL state

   233

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

   234

    ec_master_queue_datagram(fsm->slave->master, datagram);

   235

    fsm->retries = EC_FSM_RETRIES;

   236

    fsm->state = ec_fsm_slave_scan_state_state;

   237

}

   238

   239

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

   240

   241

/**

   242

   Slave scan state: STATE.

   243

*/

   244

   245

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

   246

{

   247

    ec_datagram_t *datagram = fsm->datagram;

   248

    ec_slave_t *slave = fsm->slave;

   249

   250

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

   251

        ec_master_queue_datagram(fsm->slave->master, datagram);

   252

        return;

   253

    }

   254

   255

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   256

        fsm->state = ec_fsm_slave_state_error;

   257

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

   258

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

   259

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

   260

        return;

   261

    }

   262

   263

    if (datagram->working_counter != 1) {

   264

        fsm->slave->error_flag = 1;

   265

        fsm->state = ec_fsm_slave_state_error;

   266

        EC_ERR("Failed to read AL state - slave %i did not respond.\n",

   267

               fsm->slave->ring_position);

   268

        return;

   269

    }

   270

   271

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

   272

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   273

        char state_str[EC_STATE_STRING_SIZE];

   274

        ec_state_string(slave->current_state, state_str);

   275

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

   276

                slave->ring_position, state_str);

   277

    }

   278

   279

    // read base data

   280

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

   281

    ec_master_queue_datagram(fsm->slave->master, datagram);

   282

    fsm->retries = EC_FSM_RETRIES;

   283

    fsm->state = ec_fsm_slave_scan_state_base;

   284

}

   285

   286

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

   287

   288

/**

   289

   Slave scan state: BASE.

   290

*/

   291

   292

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

   293

{

   294

    ec_datagram_t *datagram = fsm->datagram;

   295

    ec_slave_t *slave = fsm->slave;

   296

   297

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

   298

        ec_master_queue_datagram(fsm->slave->master, datagram);

   299

        return;

   300

    }

   301

   302

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   303

        fsm->state = ec_fsm_slave_state_error;

   304

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

   305

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

   306

               slave->ring_position, datagram->state);

   307

        return;

   308

    }

   309

   310

    if (datagram->working_counter != 1) {

   311

        fsm->slave->error_flag = 1;

   312

        fsm->state = ec_fsm_slave_state_error;

   313

        EC_ERR("Failed to read base data - slave %i did not respond.\n",

   314

               slave->ring_position);

   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

    slave->base_sync_count = EC_READ_U8 (datagram->data + 5);

   323

   324

    if (slave->base_fmmu_count > EC_MAX_FMMUS)

   325

        slave->base_fmmu_count = EC_MAX_FMMUS;

   326

   327

    // read data link status

   328

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

   329

    ec_master_queue_datagram(slave->master, datagram);

   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

        ec_master_queue_datagram(fsm->slave->master, datagram);

   349

        return;

   350

    }

   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 - slave %i did not respond.\n",

   364

               slave->ring_position);

   365

        return;

   366

    }

   367

   368

    dl_status = EC_READ_U16(datagram->data);

   369

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

   370

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

   371

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

   372

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

   373

    }

   374

   375

    // Start fetching EEPROM size

   376

   377

    fsm->sii_offset = 0x0040; // first category header

   378

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

   379

    fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   380

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

   381

}

   382

   383

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

   384

   385

/**

   386

   Slave scan state: EEPROM SIZE.

   387

*/

   388

   389

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

   390

{

   391

    ec_slave_t *slave = fsm->slave;

   392

    uint16_t cat_type, cat_size;

   393

   394

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

   395

   396

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

   397

        fsm->slave->error_flag = 1;

   398

        fsm->state = ec_fsm_slave_state_error;

   399

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

   400

               slave->ring_position);

   401

        return;

   402

    }

   403

   404

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

   405

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

   406

   407

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

   408

        fsm->sii_offset += cat_size + 2;

   409

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

   410

                        EC_FSM_SII_NODE);

   411

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

   412

        return;

   413

    }

   414

   415

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

   416

   417

    if (slave->eeprom_data) {

   418

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

   419

                slave->ring_position);

   420

        kfree(slave->eeprom_data);

   421

    }

   422

   423

    if (!(slave->eeprom_data =

   424

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

   425

        fsm->slave->error_flag = 1;

   426

        fsm->state = ec_fsm_slave_state_error;

   427

        EC_ERR("Failed to allocate EEPROM data on slave %i.\n",

   428

               slave->ring_position);

   429

        return;

   430

    }

   431

   432

    // Start fetching EEPROM contents

   433

   434

    fsm->state = ec_fsm_slave_scan_state_eeprom_data;

   435

    fsm->sii_offset = 0x0000;

   436

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

   437

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

   438

}

   439

   440

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

   441

   442

/**

   443

   Slave scan state: EEPROM DATA.

   444

*/

   445

   446

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

   447

{

   448

    ec_slave_t *slave = fsm->slave;

   449

    uint16_t *cat_word, cat_type, cat_size;

   450

   451

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

   452

   453

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

   454

        fsm->slave->error_flag = 1;

   455

        fsm->state = ec_fsm_slave_state_error;

   456

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

   457

               slave->ring_position);

   458

        return;

   459

    }

   460

   461

    // 2 words fetched

   462

   463

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

   464

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

   465

               fsm->fsm_sii.value, 4);

   466

    }

   467

    else { // copy the last word

   468

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

   469

               fsm->fsm_sii.value, 2);

   470

    }

   471

   472

    if (fsm->sii_offset + 2 < slave->eeprom_size / 2) {

   473

        // fetch the next 2 words

   474

        fsm->sii_offset += 2;

   475

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

   476

                        EC_FSM_SII_NODE);

   477

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

   478

        return;

   479

    }

   480

   481

    // Evaluate EEPROM contents

   482

   483

    slave->sii_alias =

   484

        EC_READ_U16(slave->eeprom_data + 2 * 0x0004);

   485

    slave->sii_vendor_id =

   486

        EC_READ_U32(slave->eeprom_data + 2 * 0x0008);

   487

    slave->sii_product_code =

   488

        EC_READ_U32(slave->eeprom_data + 2 * 0x000A);

   489

    slave->sii_revision_number =

   490

        EC_READ_U32(slave->eeprom_data + 2 * 0x000C);

   491

    slave->sii_serial_number =

   492

        EC_READ_U32(slave->eeprom_data + 2 * 0x000E);