1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   EtherCAT slave state machines.

    33

*/

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "slave_config.h"

    41

    42

#include "fsm_slave_scan.h"

    43

    44

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

    45

    46

void ec_fsm_slave_scan_state_start(ec_fsm_slave_scan_t *);

    47

void ec_fsm_slave_scan_state_address(ec_fsm_slave_scan_t *);

    48

void ec_fsm_slave_scan_state_state(ec_fsm_slave_scan_t *);

    49

void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *);

    50

void ec_fsm_slave_scan_state_dc_cap(ec_fsm_slave_scan_t *);

    51

void ec_fsm_slave_scan_state_dc_times(ec_fsm_slave_scan_t *);

    52

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *);

    53

void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);

    54

void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);

    55

void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);

    56

void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);

    57

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    58

    59

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    60

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    61

    62

void ec_fsm_slave_scan_enter_datalink(ec_fsm_slave_scan_t *);

    63

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    64

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    65

    66

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

    67

    68

/** Constructor.

    69

 */

    70

void ec_fsm_slave_scan_init(

    71

        ec_fsm_slave_scan_t *fsm, /**< Slave scanning state machine. */

    72

        ec_datagram_t *datagram, /**< Datagram to use. */

    73

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    74

                                                  state machine to use. */

    75

        ec_fsm_pdo_t *fsm_pdo /**< PDO configuration machine to use. */

    76

        )

    77

{

    78

    fsm->datagram = datagram;

    79

    fsm->fsm_slave_config = fsm_slave_config;

    80

    fsm->fsm_pdo = fsm_pdo;

    81

    82

    // init sub state machines

    83

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

    84

}

    85

    86

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

    87

    88

/** Destructor.

    89

 */

    90

void ec_fsm_slave_scan_clear(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

    91

{

    92

    // clear sub state machines

    93

    ec_fsm_sii_clear(&fsm->fsm_sii);

    94

}

    95

    96

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

    97

    98

/**

    99

 * Start slave scan state machine.

   100

 */

   101

   102

void ec_fsm_slave_scan_start(

   103

        ec_fsm_slave_scan_t *fsm, /**< slave state machine */

   104

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

   105

        )

   106

{

   107

    fsm->slave = slave;

   108

    fsm->state = ec_fsm_slave_scan_state_start;

   109

}

   110

   111

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

   112

   113

/**

   114

   \return false, if state machine has terminated

   115

*/

   116

   117

int ec_fsm_slave_scan_running(const ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   118

{

   119

    return fsm->state != ec_fsm_slave_scan_state_end

   120

        && fsm->state != ec_fsm_slave_scan_state_error;

   121

}

   122

   123

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

   124

   125

/**

   126

   Executes the current state of the state machine.

   127

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

   128

   of the state machine is delayed to the next cycle.

   129

   \return false, if state machine has terminated

   130

*/

   131

   132

int ec_fsm_slave_scan_exec(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   133

{

   134

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

   135

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

   136

        // datagram was not sent or received yet.

   137

        return ec_fsm_slave_scan_running(fsm);

   138

    }

   139

   140

    fsm->state(fsm);

   141

    return ec_fsm_slave_scan_running(fsm);

   142

}

   143

   144

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

   145

   146

/**

   147

   \return true, if the state machine terminated gracefully

   148

*/

   149

   150

int ec_fsm_slave_scan_success(const ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   151

{

   152

    return fsm->state == ec_fsm_slave_scan_state_end;

   153

}

   154

   155

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

   156

 *  slave scan state machine

   157

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

   158

   159

/**

   160

   Slave scan state: START.

   161

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

   162

   slave, according to its ring position.

   163

*/

   164

   165

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

   166

{

   167

    // write station address

   168

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

   169

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

   170

    fsm->retries = EC_FSM_RETRIES;

   171

    fsm->state = ec_fsm_slave_scan_state_address;

   172

}

   173

   174

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

   175

   176

/**

   177

   Slave scan state: ADDRESS.

   178

*/

   179

   180

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

   181

{

   182

    ec_datagram_t *datagram = fsm->datagram;

   183

   184

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

   185

        return;

   186

   187

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   188

        fsm->state = ec_fsm_slave_scan_state_error;

   189

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

   190

                " (datagram state %u)\n",

   191

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

   192

        return;

   193

    }

   194

   195

    if (datagram->working_counter != 1) {

   196

        fsm->slave->error_flag = 1;

   197

        fsm->state = ec_fsm_slave_scan_state_error;

   198

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

   199

               fsm->slave->ring_position);

   200

        ec_datagram_print_wc_error(datagram);

   201

        return;

   202

    }

   203

   204

    // Read AL state

   205

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

   206

    ec_datagram_zero(datagram);

   207

    fsm->retries = EC_FSM_RETRIES;

   208

    fsm->state = ec_fsm_slave_scan_state_state;

   209

}

   210

   211

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

   212

   213

/**

   214

   Slave scan state: STATE.

   215

*/

   216

   217

void ec_fsm_slave_scan_state_state(

   218

        ec_fsm_slave_scan_t *fsm /**< slave state machine */

   219

        )

   220

{

   221

    ec_datagram_t *datagram = fsm->datagram;

   222

    ec_slave_t *slave = fsm->slave;

   223

   224

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

   225

        return;

   226

   227

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   228

        fsm->state = ec_fsm_slave_scan_state_error;

   229

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

   230

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

   231

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

   232

        return;

   233

    }

   234

   235

    if (datagram->working_counter != 1) {

   236

        fsm->slave->error_flag = 1;

   237

        fsm->state = ec_fsm_slave_scan_state_error;

   238

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

   239

               fsm->slave->ring_position);

   240

        ec_datagram_print_wc_error(datagram);

   241

        return;

   242

    }

   243

   244

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

   245

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   246

        char state_str[EC_STATE_STRING_SIZE];

   247

        ec_state_string(slave->current_state, state_str, 0);

   248

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

   249

                slave->ring_position, state_str);

   250

    }

   251

   252

    // read base data

   253

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

   254

    ec_datagram_zero(datagram);

   255

    fsm->retries = EC_FSM_RETRIES;

   256

    fsm->state = ec_fsm_slave_scan_state_base;

   257

}

   258

   259

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

   260

   261

/** Slave scan state: BASE.

   262

 */

   263

void ec_fsm_slave_scan_state_base(

   264

        ec_fsm_slave_scan_t *fsm /**< slave state machine */

   265

        )

   266

{

   267

    ec_datagram_t *datagram = fsm->datagram;

   268

    ec_slave_t *slave = fsm->slave;

   269

    u8 octet;

   270

    int i;

   271

   272

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

   273

        return;

   274

   275

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   276

        fsm->state = ec_fsm_slave_scan_state_error;

   277

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

   278

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

   279

               slave->ring_position, datagram->state);

   280

        return;

   281

    }

   282

   283

    if (datagram->working_counter != 1) {

   284

        fsm->slave->error_flag = 1;

   285

        fsm->state = ec_fsm_slave_scan_state_error;

   286

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

   287

               slave->ring_position);

   288

        ec_datagram_print_wc_error(datagram);

   289

        return;

   290

    }

   291

   292

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

   293

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

   294

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

   295

   296

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

   297

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   298

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

   299

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

   300

                slave->base_fmmu_count, EC_MAX_FMMUS);

   301

        slave->base_fmmu_count = EC_MAX_FMMUS;

   302

    }

   303

   304

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

   305

    if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) {

   306

        EC_WARN("Slave %u provides more sync managers (%u) than the master can"

   307

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

   308

                slave->base_sync_count, EC_MAX_SYNC_MANAGERS);

   309

        slave->base_sync_count = EC_MAX_SYNC_MANAGERS;

   310

    }

   311

   312

    octet = EC_READ_U8(datagram->data + 7);

   313

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

   314

        slave->ports[i].desc = (octet >> (2 * i)) & 0x03;

   315

    }

   316

   317

    octet = EC_READ_U8(datagram->data + 8);

   318

    slave->base_fmmu_bit_operation = octet & 0x01;

   319

    slave->base_dc_supported = (octet >> 2) & 0x01;

   320

    slave->base_dc_range = ((octet >> 3) & 0x01) ? EC_DC_64 : EC_DC_32;

   321

   322

    if (slave->base_dc_supported) {

   323

        // read DC capabilities

   324

        ec_datagram_fprd(datagram, slave->station_address, 0x0910,

   325

                slave->base_dc_range == EC_DC_64 ? 8 : 4);

   326

        ec_datagram_zero(datagram);

   327

        fsm->retries = EC_FSM_RETRIES;

   328

        fsm->state = ec_fsm_slave_scan_state_dc_cap;

   329

    } else {

   330

        ec_fsm_slave_scan_enter_datalink(fsm);

   331

    }

   332

}

   333

   334

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

   335

   336

/**

   337

   Slave scan state: DC CAPABILITIES.

   338

*/

   339

   340

void ec_fsm_slave_scan_state_dc_cap(

   341

        ec_fsm_slave_scan_t *fsm /**< slave state machine */

   342

        )

   343

{

   344

    ec_datagram_t *datagram = fsm->datagram;

   345

    ec_slave_t *slave = fsm->slave;

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

   352

        EC_ERR("Failed to receive system time datagram for slave %u"

   353

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

   354

               slave->ring_position, datagram->state);

   355

        return;

   356

    }

   357

   358

    if (datagram->working_counter == 1) {

   359

        slave->has_dc_system_time = 1;

   360

        if (slave->master->debug_level) {

   361

            EC_DBG("Slave %u has the System Time register.\n",

   362

                    slave->ring_position);

   363

        }

   364

    } else if (datagram->working_counter == 0) {

   365

        if (slave->master->debug_level) {

   366

            EC_DBG("Slave %u has no System Time register; delay "

   367

                    "measurement only.\n", slave->ring_position);

   368

        }

   369

    } else {

   370

        fsm->slave->error_flag = 1;

   371

        fsm->state = ec_fsm_slave_scan_state_error;

   372

        EC_ERR("Failed to determine, if system time register is "

   373

                "supported by slave %u: ", slave->ring_position);

   374

        ec_datagram_print_wc_error(datagram);

   375

        return;

   376

    }

   377

   378

    // read DC port receive times

   379

    ec_datagram_fprd(datagram, slave->station_address, 0x0900, 16);

   380

    ec_datagram_zero(datagram);

   381

    fsm->retries = EC_FSM_RETRIES;

   382

    fsm->state = ec_fsm_slave_scan_state_dc_times;

   383

}

   384

   385

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

   386

   387

/**

   388

   Slave scan state: DC TIMES.

   389

*/

   390

   391

void ec_fsm_slave_scan_state_dc_times(

   392

        ec_fsm_slave_scan_t *fsm /**< slave state machine */

   393

        )

   394

{

   395

    ec_datagram_t *datagram = fsm->datagram;

   396

    ec_slave_t *slave = fsm->slave;

   397

    int i;

   398

   399

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

   400

        return;

   401

   402

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   403

        fsm->state = ec_fsm_slave_scan_state_error;

   404

        EC_ERR("Failed to receive system time datagram for slave %u"

   405

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

   406

               slave->ring_position, datagram->state);

   407

        return;

   408

    }

   409

   410

    if (datagram->working_counter != 1) {

   411

        fsm->slave->error_flag = 1;

   412

        fsm->state = ec_fsm_slave_scan_state_error;

   413

        EC_ERR("Failed to get DC receive times of slave %u: ",

   414

                slave->ring_position);

   415

        ec_datagram_print_wc_error(datagram);

   416

        return;

   417

    }

   418

   419

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

   420

        slave->ports[i].receive_time = EC_READ_U32(datagram->data + 4 * i);

   421

    }

   422

   423

    ec_fsm_slave_scan_enter_datalink(fsm);

   424

}

   425

   426

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

   427

   428

/**

   429

   Slave scan entry function: DATALINK.

   430

*/

   431

   432

void ec_fsm_slave_scan_enter_datalink(

   433

        ec_fsm_slave_scan_t *fsm /**< slave state machine */

   434

        )

   435

{

   436

    ec_datagram_t *datagram = fsm->datagram;

   437

    ec_slave_t *slave = fsm->slave;

   438

   439

    // read data link status

   440

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

   441

    ec_datagram_zero(datagram);

   442

    fsm->retries = EC_FSM_RETRIES;

   443

    fsm->state = ec_fsm_slave_scan_state_datalink;

   444

}

   445

   446

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

   447

   448

/**

   449

   Slave scan state: DATALINK.

   450

*/

   451

   452

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

   453

{

   454

    ec_datagram_t *datagram = fsm->datagram;

   455

    ec_slave_t *slave = fsm->slave;

   456

    uint16_t dl_status;

   457

    unsigned int i;

   458

   459

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

   460

        return;

   461

   462

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   463

        fsm->state = ec_fsm_slave_scan_state_error;

   464

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

   465

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

   466

               slave->ring_position, datagram->state);

   467

        return;

   468

    }

   469

   470

    if (datagram->working_counter != 1) {

   471

        fsm->slave->error_flag = 1;

   472

        fsm->state = ec_fsm_slave_scan_state_error;

   473

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

   474

               slave->ring_position);

   475

        ec_datagram_print_wc_error(datagram);

   476

        return;

   477

    }

   478

   479

    dl_status = EC_READ_U16(datagram->data);

   480

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

   481

        slave->ports[i].link.link_up = dl_status & (1 << (4 + i)) ? 1 : 0;

   482

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

   483

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

   484

    }

   485

   486

    // Start fetching SII size

   487

   488

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

   489

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

   490

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   491

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   492

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

   493

}

   494

   495

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

   496

   497

/**

   498

   Slave scan state: SII SIZE.

   499

*/

   500

   501

void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   502

{

   503

    ec_slave_t *slave = fsm->slave;

   504

    uint16_t cat_type, cat_size;

   505

   506

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

   507

   508

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

   509

        fsm->slave->error_flag = 1;

   510

        fsm->state = ec_fsm_slave_scan_state_error;

   511