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

#include "fsm_mapping.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_pdo_sync(ec_fsm_slave_t *);

    63

void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);

    64

void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *);

    65

void ec_fsm_slave_conf_state_mapconf(ec_fsm_slave_t *);

    66

void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *);

    67

void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);

    68

    69

void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *);

    70

void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);

    71

void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *);

    72

void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);

    73

void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *);

    74

void ec_fsm_slave_conf_enter_mapconf(ec_fsm_slave_t *);

    75

void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *);

    76

    77

void ec_fsm_slave_state_end(ec_fsm_slave_t *);

    78

void ec_fsm_slave_state_error(ec_fsm_slave_t *);

    79

    80

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

    81

    82

/**

    83

   Constructor.

    84

*/

    85

    86

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

    87

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

    88

        )

    89

{

    90

    fsm->datagram = datagram;

    91

    92

    // init sub state machines

    93

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

    94

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

    95

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

    96

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

    97

}

    98

    99

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

   100

   101

/**

   102

   Destructor.

   103

*/

   104

   105

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

   106

{

   107

    // clear sub state machines

   108

    ec_fsm_sii_clear(&fsm->fsm_sii);

   109

    ec_fsm_change_clear(&fsm->fsm_change);

   110

    ec_fsm_coe_clear(&fsm->fsm_coe);

   111

    ec_fsm_mapping_clear(&fsm->fsm_map);

   112

}

   113

   114

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

   115

   116

/**

   117

 * Start slave scan state machine.

   118

 */

   119

   120

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

   121

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

   122

        )

   123

{

   124

    fsm->slave = slave;

   125

    fsm->state = ec_fsm_slave_scan_state_start;

   126

}

   127

   128

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

   129

   130

/**

   131

 * Start slave configuration state machine.

   132

 */

   133

   134

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

   135

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

   136

        )

   137

{

   138

    fsm->slave = slave;

   139

    fsm->state = ec_fsm_slave_conf_state_start;

   140

}

   141

   142

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

   143

   144

/**

   145

   \return false, if state machine has terminated

   146

*/

   147

   148

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

   149

{

   150

    return fsm->state != ec_fsm_slave_state_end

   151

        && fsm->state != ec_fsm_slave_state_error;

   152

}

   153

   154

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

   155

   156

/**

   157

   Executes the current state of the state machine.

   158

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

   159

   of the state machine is delayed to the next cycle.

   160

   \return false, if state machine has terminated

   161

*/

   162

   163

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

   164

{

   165

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

   166

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

   167

        // datagram was not sent or received yet.

   168

        return ec_fsm_slave_running(fsm);

   169

    }

   170

   171

    fsm->state(fsm);

   172

    return ec_fsm_slave_running(fsm);

   173

}

   174

   175

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

   176

   177

/**

   178

   \return true, if the state machine terminated gracefully

   179

*/

   180

   181

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

   182

{

   183

    return fsm->state == ec_fsm_slave_state_end;

   184

}

   185

   186

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

   187

 *  slave scan state machine

   188

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

   189

   190

/**

   191

   Slave scan state: START.

   192

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

   193

   slave, according to its ring position.

   194

*/

   195

   196

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

   197

{

   198

    // write station address

   199

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

   200

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

   201

    fsm->retries = EC_FSM_RETRIES;

   202

    fsm->state = ec_fsm_slave_scan_state_address;

   203

}

   204

   205

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

   206

   207

/**

   208

   Slave scan state: ADDRESS.

   209

*/

   210

   211

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

   212

{

   213

    ec_datagram_t *datagram = fsm->datagram;

   214

   215

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

   216

        return;

   217

   218

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   219

        fsm->state = ec_fsm_slave_state_error;

   220

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

   221

                " (datagram state %i)\n",

   222

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

   223

        return;

   224

    }

   225

   226

    if (datagram->working_counter != 1) {

   227

        fsm->slave->error_flag = 1;

   228

        fsm->state = ec_fsm_slave_state_error;

   229

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

   230

               fsm->slave->ring_position);

   231

        return;

   232

    }

   233

   234

    // Read AL state

   235

    ec_datagram_nprd(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(ec_fsm_slave_t *fsm /**< slave state machine */)

   247

{

   248

    ec_datagram_t *datagram = fsm->datagram;

   249

    ec_slave_t *slave = fsm->slave;

   250

   251

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

   252

        return;

   253

   254

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   255

        fsm->state = ec_fsm_slave_state_error;

   256

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

   257

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

   258

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

   259

        return;

   260

    }

   261

   262

    if (datagram->working_counter != 1) {

   263

        fsm->slave->error_flag = 1;

   264

        fsm->state = ec_fsm_slave_state_error;

   265

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

   266

               fsm->slave->ring_position);

   267

        return;

   268

    }

   269

   270

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

   271

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   272

        char state_str[EC_STATE_STRING_SIZE];

   273

        ec_state_string(slave->current_state, state_str);

   274

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

   275

                slave->ring_position, state_str);

   276

    }

   277

   278

    // read base data

   279

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

   280

    fsm->retries = EC_FSM_RETRIES;

   281

    fsm->state = ec_fsm_slave_scan_state_base;

   282

}

   283

   284

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

   285

   286

/**

   287

   Slave scan state: BASE.

   288

*/

   289

   290

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

   291

{

   292

    ec_datagram_t *datagram = fsm->datagram;

   293

    ec_slave_t *slave = fsm->slave;

   294

   295

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

   296

        return;

   297

   298

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   299

        fsm->state = ec_fsm_slave_state_error;

   300

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

   301

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

   302

               slave->ring_position, datagram->state);

   303

        return;

   304

    }

   305

   306

    if (datagram->working_counter != 1) {

   307

        fsm->slave->error_flag = 1;

   308

        fsm->state = ec_fsm_slave_state_error;

   309

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

   310

               slave->ring_position);

   311

        return;

   312

    }

   313

   314

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

   315

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

   316

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

   317

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

   318

   319

    if (slave->base_fmmu_count > EC_MAX_FMMUS)

   320

        slave->base_fmmu_count = EC_MAX_FMMUS;

   321

   322

    // read data link status

   323

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

   324

    fsm->retries = EC_FSM_RETRIES;

   325

    fsm->state = ec_fsm_slave_scan_state_datalink;

   326

}

   327

   328

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

   329

   330

/**

   331

   Slave scan state: DATALINK.

   332

*/

   333

   334

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

   335

{

   336

    ec_datagram_t *datagram = fsm->datagram;

   337

    ec_slave_t *slave = fsm->slave;

   338

    uint16_t dl_status;

   339

    unsigned int i;

   340

   341

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

   342

        return;

   343

   344

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   345

        fsm->state = ec_fsm_slave_state_error;

   346

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

   347

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

   348

               slave->ring_position, datagram->state);

   349

        return;

   350

    }

   351

   352

    if (datagram->working_counter != 1) {

   353

        fsm->slave->error_flag = 1;

   354

        fsm->state = ec_fsm_slave_state_error;

   355

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

   356

               slave->ring_position);

   357

        return;

   358

    }

   359

   360

    dl_status = EC_READ_U16(datagram->data);

   361

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

   362

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

   363

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

   364

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

   365

    }

   366

   367

    // Start fetching EEPROM size

   368

   369

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

   370

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

   371

    fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   372

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

   373

}

   374

   375

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

   376

   377

/**

   378

   Slave scan state: EEPROM SIZE.

   379

*/

   380

   381

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

   382

{

   383

    ec_slave_t *slave = fsm->slave;

   384

    uint16_t cat_type, cat_size;

   385

   386

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

   387

   388

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

   389

        fsm->slave->error_flag = 1;

   390

        fsm->state = ec_fsm_slave_state_error;

   391

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

   392

               slave->ring_position);

   393

        return;

   394

    }

   395

   396

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

   397

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

   398

   399

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

   400

        fsm->sii_offset += cat_size + 2;

   401

        if (fsm->sii_offset >= EC_MAX_EEPROM_SIZE) {

   402

            EC_WARN("EEPROM size of slave %i exceeds"

   403

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

   404

                    slave->ring_position, EC_MAX_EEPROM_SIZE);

   405

            slave->eeprom_size = EC_FIRST_EEPROM_CATEGORY_OFFSET * 2;

   406

            goto alloc_eeprom;

   407

        }

   408

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

   409

                        EC_FSM_SII_NODE);

   410

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

   411

        return;

   412

    }

   413

   414

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

   415

   416

alloc_eeprom:

   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 =