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_regalias(ec_fsm_slave_scan_t *);

    56

void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);

    57

void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);

    58

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    59

    60

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    61

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    62

    63

void ec_fsm_slave_scan_enter_datalink(ec_fsm_slave_scan_t *);

    64

void ec_fsm_slave_scan_enter_regalias(ec_fsm_slave_scan_t *);

    65

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    66

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    67

    68

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

    69

    70

/** Constructor.

    71

 */

    72

void ec_fsm_slave_scan_init(

    73

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

    74

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

    75

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    76

                                                  state machine to use. */

    77

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

    78

        )

    79

{

    80

    fsm->datagram = datagram;

    81

    fsm->fsm_slave_config = fsm_slave_config;

    82

    fsm->fsm_pdo = fsm_pdo;

    83

    84

    // init sub state machines

    85

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

    86

}

    87

    88

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

    89

    90

/** Destructor.

    91

 */

    92

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

    93

{

    94

    // clear sub state machines

    95

    ec_fsm_sii_clear(&fsm->fsm_sii);

    96

}

    97

    98

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

    99

   100

/**

   101

 * Start slave scan state machine.

   102

 */

   103

   104

void ec_fsm_slave_scan_start(

   105

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

   106

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

   107

        )

   108

{

   109

    fsm->slave = slave;

   110

    fsm->state = ec_fsm_slave_scan_state_start;

   111

}

   112

   113

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

   114

   115

/**

   116

   \return false, if state machine has terminated

   117

*/

   118

   119

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

   120

{

   121

    return fsm->state != ec_fsm_slave_scan_state_end

   122

        && fsm->state != ec_fsm_slave_scan_state_error;

   123

}

   124

   125

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

   126

   127

/**

   128

   Executes the current state of the state machine.

   129

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

   130

   of the state machine is delayed to the next cycle.

   131

   \return false, if state machine has terminated

   132

*/

   133

   134

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

   135

{

   136

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

   137

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

   138

        // datagram was not sent or received yet.

   139

        return ec_fsm_slave_scan_running(fsm);

   140

    }

   141

   142

    fsm->state(fsm);

   143

    return ec_fsm_slave_scan_running(fsm);

   144

}

   145

   146

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

   147

   148

/**

   149

   \return true, if the state machine terminated gracefully

   150

*/

   151

   152

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

   153

{

   154

    return fsm->state == ec_fsm_slave_scan_state_end;

   155

}

   156

   157

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

   158

 *  slave scan state machine

   159

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

   160

   161

/**

   162

   Slave scan state: START.

   163

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

   164

   slave, according to its ring position.

   165

*/

   166

   167

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

   168

{

   169

    // write station address

   170

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

   171

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

   172

    fsm->retries = EC_FSM_RETRIES;

   173

    fsm->state = ec_fsm_slave_scan_state_address;

   174

}

   175

   176

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

   177

   178

/**

   179

   Slave scan state: ADDRESS.

   180

*/

   181

   182

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

   183

{

   184

    ec_datagram_t *datagram = fsm->datagram;

   185

   186

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

   187

        return;

   188

   189

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   190

        fsm->state = ec_fsm_slave_scan_state_error;

   191

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

   192

                " (datagram state %u)\n",

   193

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

   194

        return;

   195

    }

   196

   197

    if (datagram->working_counter != 1) {

   198

        fsm->slave->error_flag = 1;

   199

        fsm->state = ec_fsm_slave_scan_state_error;

   200

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

   201

               fsm->slave->ring_position);

   202

        ec_datagram_print_wc_error(datagram);

   203

        return;

   204

    }

   205

   206

    // Read AL state

   207

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

   208

    ec_datagram_zero(datagram);

   209

    fsm->retries = EC_FSM_RETRIES;

   210

    fsm->state = ec_fsm_slave_scan_state_state;

   211

}

   212

   213

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

   214

   215

/**

   216

   Slave scan state: STATE.

   217

*/

   218

   219

void ec_fsm_slave_scan_state_state(

   220

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

   221

        )

   222

{

   223

    ec_datagram_t *datagram = fsm->datagram;

   224

    ec_slave_t *slave = fsm->slave;

   225

   226

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

   227

        return;

   228

   229

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   230

        fsm->state = ec_fsm_slave_scan_state_error;

   231

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

   232

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

   233

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

   234

        return;

   235

    }

   236

   237

    if (datagram->working_counter != 1) {

   238

        fsm->slave->error_flag = 1;

   239

        fsm->state = ec_fsm_slave_scan_state_error;

   240

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

   241

               fsm->slave->ring_position);

   242

        ec_datagram_print_wc_error(datagram);

   243

        return;

   244

    }

   245

   246

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

   247

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   248

        char state_str[EC_STATE_STRING_SIZE];

   249

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

   250

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

   251

                slave->ring_position, state_str);

   252

    }

   253

   254

    // read base data

   255

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

   256

    ec_datagram_zero(datagram);

   257

    fsm->retries = EC_FSM_RETRIES;

   258

    fsm->state = ec_fsm_slave_scan_state_base;

   259

}

   260

   261

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

   262

   263

/** Slave scan state: BASE.

   264

 */

   265

void ec_fsm_slave_scan_state_base(

   266

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

   267

        )

   268

{

   269

    ec_datagram_t *datagram = fsm->datagram;

   270

    ec_slave_t *slave = fsm->slave;

   271

    u8 octet;

   272

    int i;

   273

   274

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

   275

        return;

   276

   277

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   278

        fsm->state = ec_fsm_slave_scan_state_error;

   279

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

   280

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

   281

               slave->ring_position, datagram->state);

   282

        return;

   283

    }

   284

   285

    if (datagram->working_counter != 1) {

   286

        fsm->slave->error_flag = 1;

   287

        fsm->state = ec_fsm_slave_scan_state_error;

   288

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

   289

               slave->ring_position);

   290

        ec_datagram_print_wc_error(datagram);

   291

        return;

   292

    }

   293

   294

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

   295

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

   296

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

   297

   298

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

   299

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   300

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

   301

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

   302

                slave->base_fmmu_count, EC_MAX_FMMUS);

   303

        slave->base_fmmu_count = EC_MAX_FMMUS;

   304

    }

   305

   306

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

   307

    if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) {

   308

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

   309

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

   310

                slave->base_sync_count, EC_MAX_SYNC_MANAGERS);

   311

        slave->base_sync_count = EC_MAX_SYNC_MANAGERS;

   312

    }

   313

   314

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

   315

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

   316

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

   317

    }

   318

   319

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

   320

    slave->base_fmmu_bit_operation = octet & 0x01;

   321

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

   322

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

   323

   324

    if (slave->base_dc_supported) {

   325

        // read DC capabilities

   326

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

   327

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

   328

        ec_datagram_zero(datagram);

   329

        fsm->retries = EC_FSM_RETRIES;

   330

        fsm->state = ec_fsm_slave_scan_state_dc_cap;

   331

    } else {

   332

        ec_fsm_slave_scan_enter_datalink(fsm);

   333

    }

   334

}

   335

   336

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

   337

   338

/**

   339

   Slave scan state: DC CAPABILITIES.

   340

*/

   341

   342

void ec_fsm_slave_scan_state_dc_cap(

   343

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

   344

        )

   345

{

   346

    ec_datagram_t *datagram = fsm->datagram;

   347

    ec_slave_t *slave = fsm->slave;

   348

   349

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

   350

        return;

   351

   352

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   353

        fsm->state = ec_fsm_slave_scan_state_error;

   354

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

   355

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

   356

               slave->ring_position, datagram->state);

   357

        return;

   358

    }

   359

   360

    if (datagram->working_counter == 1) {

   361

        slave->has_dc_system_time = 1;

   362

        if (slave->master->debug_level) {

   363

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

   364

                    slave->ring_position);

   365

        }

   366

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

   367

        if (slave->master->debug_level) {

   368

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

   369

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

   370

        }

   371

    } else {

   372

        fsm->slave->error_flag = 1;

   373

        fsm->state = ec_fsm_slave_scan_state_error;

   374

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

   375

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

   376

        ec_datagram_print_wc_error(datagram);

   377

        return;

   378

    }

   379

   380

    // read DC port receive times

   381

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

   382

    ec_datagram_zero(datagram);

   383

    fsm->retries = EC_FSM_RETRIES;

   384

    fsm->state = ec_fsm_slave_scan_state_dc_times;

   385

}

   386

   387

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

   388

   389

/**

   390

   Slave scan state: DC TIMES.

   391

*/

   392

   393

void ec_fsm_slave_scan_state_dc_times(

   394

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

   395

        )

   396

{

   397

    ec_datagram_t *datagram = fsm->datagram;

   398

    ec_slave_t *slave = fsm->slave;

   399

    int i;

   400

   401

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

   402

        return;

   403

   404

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   405

        fsm->state = ec_fsm_slave_scan_state_error;

   406

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

   407

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

   408

               slave->ring_position, datagram->state);

   409

        return;

   410

    }

   411

   412

    if (datagram->working_counter != 1) {

   413

        fsm->slave->error_flag = 1;

   414

        fsm->state = ec_fsm_slave_scan_state_error;

   415

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

   416

                slave->ring_position);

   417

        ec_datagram_print_wc_error(datagram);

   418

        return;

   419

    }

   420

   421

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

   422

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

   423

    }

   424

   425

    ec_fsm_slave_scan_enter_datalink(fsm);

   426

}

   427

   428

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

   429

   430

/**

   431

   Slave scan entry function: DATALINK.

   432

*/

   433

   434

void ec_fsm_slave_scan_enter_datalink(

   435

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

   436

        )

   437

{

   438

    ec_datagram_t *datagram = fsm->datagram;

   439

    ec_slave_t *slave = fsm->slave;

   440

   441

    // read data link status

   442

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

   443

    ec_datagram_zero(datagram);

   444

    fsm->retries = EC_FSM_RETRIES;

   445

    fsm->state = ec_fsm_slave_scan_state_datalink;

   446

}

   447

   448

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

   449

   450

/**

   451

   Slave scan state: DATALINK.

   452

*/

   453

   454

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

   455

{

   456

    ec_datagram_t *datagram = fsm->datagram;

   457

    ec_slave_t *slave = fsm->slave;

   458

    uint16_t dl_status;

   459

    unsigned int i;

   460

   461

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

   462

        return;

   463

   464

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   465

        fsm->state = ec_fsm_slave_scan_state_error;

   466

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

   467

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

   468

               slave->ring_position, datagram->state);

   469

        return;

   470

    }

   471

   472

    if (datagram->working_counter != 1) {

   473

        fsm->slave->error_flag = 1;

   474

        fsm->state = ec_fsm_slave_scan_state_error;

   475

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

   476

               slave->ring_position);

   477

        ec_datagram_print_wc_error(datagram);

   478

        return;

   479

    }

   480

   481

    dl_status = EC_READ_U16(datagram->data);

   482

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

   483

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

   484

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

   485

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

   486

    }

   487

   488

    // Start fetching SII size

   489

   490

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

   491

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

   492

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   493

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   494

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

   495

}

   496

   497

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

   498

   499

/**

   500

   Slave scan state: SII SIZE.

   501

*/

   502

   503

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

   504

{

   505

    ec_slave_t *slave = fsm->slave;

   506

    uint16_t cat_type, cat_size;

   507

   508

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

   509

   510

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