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 on slave %i: ",

   230

               fsm->slave->ring_position);

   231

        ec_datagram_print_wc_error(datagram);

   232

        return;

   233

    }

   234

   235

    // Read AL state

   236

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

   237

    fsm->retries = EC_FSM_RETRIES;

   238

    fsm->state = ec_fsm_slave_scan_state_state;

   239

}

   240

   241

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

   242

   243

/**

   244

   Slave scan state: STATE.

   245

*/

   246

   247

void ec_fsm_slave_scan_state_state(

   248

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

   249

        )

   250

{

   251

    ec_datagram_t *datagram = fsm->datagram;

   252

    ec_slave_t *slave = fsm->slave;

   253

   254

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

   255

        return;

   256

   257

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   258

        fsm->state = ec_fsm_slave_state_error;

   259

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

   260

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

   261

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

   262

        return;

   263

    }

   264

   265

    if (datagram->working_counter != 1) {

   266

        fsm->slave->error_flag = 1;

   267

        fsm->state = ec_fsm_slave_state_error;

   268

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

   269

               fsm->slave->ring_position);

   270

        ec_datagram_print_wc_error(datagram);

   271

        return;

   272

    }

   273

   274

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

   275

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   276

        char state_str[EC_STATE_STRING_SIZE];

   277

        ec_state_string(slave->current_state, state_str);

   278

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

   279

                slave->ring_position, state_str);

   280

    }

   281

   282

    // read base data

   283

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

   284

    fsm->retries = EC_FSM_RETRIES;

   285

    fsm->state = ec_fsm_slave_scan_state_base;

   286

}

   287

   288

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

   289

   290

/**

   291

   Slave scan state: BASE.

   292

*/

   293

   294

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

   295

{

   296

    ec_datagram_t *datagram = fsm->datagram;

   297

    ec_slave_t *slave = fsm->slave;

   298

   299

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

   300

        return;

   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 from slave %i: ",

   314

               slave->ring_position);

   315

        ec_datagram_print_wc_error(datagram);

   316

        return;

   317

    }

   318

   319

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

   320

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

   321

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

   322

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

   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

    fsm->retries = EC_FSM_RETRIES;

   330

    fsm->state = ec_fsm_slave_scan_state_datalink;

   331

}

   332

   333

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

   334

   335

/**

   336

   Slave scan state: DATALINK.

   337

*/

   338

   339

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

   340

{

   341

    ec_datagram_t *datagram = fsm->datagram;

   342

    ec_slave_t *slave = fsm->slave;

   343

    uint16_t dl_status;

   344

    unsigned int i;

   345

   346

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

   347

        return;

   348

   349

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   350

        fsm->state = ec_fsm_slave_state_error;

   351

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

   352

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

   353

               slave->ring_position, datagram->state);

   354

        return;

   355

    }

   356

   357

    if (datagram->working_counter != 1) {

   358

        fsm->slave->error_flag = 1;

   359

        fsm->state = ec_fsm_slave_state_error;

   360

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

   361

               slave->ring_position);

   362

        ec_datagram_print_wc_error(datagram);

   363

        return;

   364

    }

   365

   366

    dl_status = EC_READ_U16(datagram->data);

   367

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

   368

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

   369

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

   370

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

   371

    }

   372

   373

    // Start fetching EEPROM size

   374

   375

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

   376

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

   377

    fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   378

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

   379

}

   380

   381

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

   382

   383

/**

   384

   Slave scan state: EEPROM SIZE.

   385

*/

   386

   387

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

   388

{

   389

    ec_slave_t *slave = fsm->slave;

   390

    uint16_t cat_type, cat_size;

   391

   392

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

   393

   394

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

   395

        fsm->slave->error_flag = 1;

   396

        fsm->state = ec_fsm_slave_state_error;

   397

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

   398

               slave->ring_position);

   399

        return;

   400

    }

   401

   402

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

   403

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

   404

   405

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

   406

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

   407

        if (next_offset >= EC_MAX_EEPROM_SIZE) {

   408

            EC_WARN("EEPROM size of slave %i exceeds"

   409

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

   410

                    slave->ring_position, EC_MAX_EEPROM_SIZE);

   411

            slave->eeprom_size = EC_FIRST_EEPROM_CATEGORY_OFFSET * 2;

   412

            goto alloc_eeprom;

   413

        }

   414

        fsm->sii_offset = next_offset;

   415

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

   416

                        EC_FSM_SII_NODE);

   417

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

   418

        return;

   419

    }

   420

   421

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

   422

   423

alloc_eeprom:

   424

    if (slave->eeprom_data) {

   425

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

   426

                slave->ring_position);

   427

        kfree(slave->eeprom_data);

   428

    }

   429

   430

    if (!(slave->eeprom_data =

   431

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

   432

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

   433

               slave->eeprom_size, slave->ring_position);

   434

        slave->eeprom_size = 0;

   435

        slave->error_flag = 1;

   436

        fsm->state = ec_fsm_slave_state_error;

   437

        return;

   438

    }

   439

   440

    // Start fetching EEPROM contents

   441

   442

    fsm->state = ec_fsm_slave_scan_state_eeprom_data;

   443

    fsm->sii_offset = 0x0000;

   444

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

   445

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

   446

}

   447

   448

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

   449

   450

/**

   451

   Slave scan state: EEPROM DATA.

   452

*/

   453

   454

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

   455

{

   456

    ec_slave_t *slave = fsm->slave;

   457

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

   458

   459

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

   460

   461

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

   462

        fsm->slave->error_flag = 1;

   463

        fsm->state = ec_fsm_slave_state_error;

   464

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

   465

               slave->ring_position);

   466

        return;

   467

    }

   468

   469

    // 2 words fetched

   470

   471

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

   472

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

   473

               fsm->fsm_sii.value, 4);

   474

    }

   475

    else { // copy the last word

   476

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

   477

               fsm->fsm_sii.value, 2);

   478

    }

   479

   480

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

   481

        // fetch the next 2 words

   482

        fsm->sii_offset += 2;

   483

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

   484

                        EC_FSM_SII_NODE);

   485

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

   486

        return;

   487

    }

   488

   489

    // Evaluate EEPROM contents

   490