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(

   183

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

   184

        )

   185

{

   186

    ec_datagram_t *datagram = fsm->datagram;

   187

   188

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

   189

        return;

   190

   191

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   192

        fsm->state = ec_fsm_slave_scan_state_error;

   193

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

   194

                fsm->slave->ring_position);

   195

        ec_datagram_print_state(datagram);

   196

        return;

   197

    }

   198

   199

    if (datagram->working_counter != 1) {

   200

        fsm->slave->error_flag = 1;

   201

        fsm->state = ec_fsm_slave_scan_state_error;

   202

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

   203

               fsm->slave->ring_position);

   204

        ec_datagram_print_wc_error(datagram);

   205

        return;

   206

    }

   207

   208

    // Read AL state

   209

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

   210

    ec_datagram_zero(datagram);

   211

    fsm->retries = EC_FSM_RETRIES;

   212

    fsm->state = ec_fsm_slave_scan_state_state;

   213

}

   214

   215

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

   216

   217

/**

   218

   Slave scan state: STATE.

   219

*/

   220

   221

void ec_fsm_slave_scan_state_state(

   222

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

   223

        )

   224

{

   225

    ec_datagram_t *datagram = fsm->datagram;

   226

    ec_slave_t *slave = fsm->slave;

   227

   228

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

   229

        return;

   230

   231

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   232

        fsm->state = ec_fsm_slave_scan_state_error;

   233

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

   234

               fsm->slave->ring_position);

   235

        ec_datagram_print_state(datagram);

   236

        return;

   237

    }

   238

   239

    if (datagram->working_counter != 1) {

   240

        fsm->slave->error_flag = 1;

   241

        fsm->state = ec_fsm_slave_scan_state_error;

   242

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

   243

               fsm->slave->ring_position);

   244

        ec_datagram_print_wc_error(datagram);

   245

        return;

   246

    }

   247

   248

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

   249

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   250

        char state_str[EC_STATE_STRING_SIZE];

   251

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

   252

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

   253

                slave->ring_position, state_str);

   254

    }

   255

   256

    // read base data

   257

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

   258

    ec_datagram_zero(datagram);

   259

    fsm->retries = EC_FSM_RETRIES;

   260

    fsm->state = ec_fsm_slave_scan_state_base;

   261

}

   262

   263

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

   264

   265

/** Slave scan state: BASE.

   266

 */

   267

void ec_fsm_slave_scan_state_base(

   268

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

   269

        )

   270

{

   271

    ec_datagram_t *datagram = fsm->datagram;

   272

    ec_slave_t *slave = fsm->slave;

   273

    u8 octet;

   274

    int i;

   275

   276

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

   277

        return;

   278

   279

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   280

        fsm->state = ec_fsm_slave_scan_state_error;

   281

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

   282

               slave->ring_position);

   283

        ec_datagram_print_state(datagram);

   284

        return;

   285

    }

   286

   287

    if (datagram->working_counter != 1) {

   288

        fsm->slave->error_flag = 1;

   289

        fsm->state = ec_fsm_slave_scan_state_error;

   290

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

   291

               slave->ring_position);

   292

        ec_datagram_print_wc_error(datagram);

   293

        return;

   294

    }

   295

   296

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

   297

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

   298

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

   299

   300

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

   301

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   302

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

   303

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

   304

                slave->base_fmmu_count, EC_MAX_FMMUS);

   305

        slave->base_fmmu_count = EC_MAX_FMMUS;

   306

    }

   307

   308

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

   309

    if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) {

   310

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

   311

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

   312

                slave->base_sync_count, EC_MAX_SYNC_MANAGERS);

   313

        slave->base_sync_count = EC_MAX_SYNC_MANAGERS;

   314

    }

   315

   316

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

   317

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

   318

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

   319

    }

   320

   321

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

   322

    slave->base_fmmu_bit_operation = octet & 0x01;

   323

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

   324

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

   325

   326

    if (slave->base_dc_supported) {

   327

        // read DC capabilities

   328

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

   329

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

   330

        ec_datagram_zero(datagram);

   331

        fsm->retries = EC_FSM_RETRIES;

   332

        fsm->state = ec_fsm_slave_scan_state_dc_cap;

   333

    } else {

   334

        ec_fsm_slave_scan_enter_datalink(fsm);

   335

    }

   336

}

   337

   338

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

   339

   340

/**

   341

   Slave scan state: DC CAPABILITIES.

   342

*/

   343

   344

void ec_fsm_slave_scan_state_dc_cap(

   345

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

   346

        )

   347

{

   348

    ec_datagram_t *datagram = fsm->datagram;

   349

    ec_slave_t *slave = fsm->slave;

   350

   351

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

   352

        return;

   353

   354

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   355

        fsm->state = ec_fsm_slave_scan_state_error;

   356

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

   357

               slave->ring_position);

   358

        ec_datagram_print_state(datagram);

   359

        return;

   360

    }

   361

   362

    if (datagram->working_counter == 1) {

   363

        slave->has_dc_system_time = 1;

   364

        if (slave->master->debug_level) {

   365

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

   366

                    slave->ring_position);

   367

        }

   368

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

   369

        if (slave->master->debug_level) {

   370

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

   371

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

   372

        }

   373

    } else {

   374

        fsm->slave->error_flag = 1;

   375

        fsm->state = ec_fsm_slave_scan_state_error;

   376

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

   377

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

   378

        ec_datagram_print_wc_error(datagram);

   379

        return;

   380

    }

   381

   382

    // read DC port receive times

   383

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

   384

    ec_datagram_zero(datagram);

   385

    fsm->retries = EC_FSM_RETRIES;

   386

    fsm->state = ec_fsm_slave_scan_state_dc_times;

   387

}

   388

   389

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

   390

   391

/**

   392

   Slave scan state: DC TIMES.

   393

*/

   394

   395

void ec_fsm_slave_scan_state_dc_times(

   396

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

   397

        )

   398

{

   399

    ec_datagram_t *datagram = fsm->datagram;

   400

    ec_slave_t *slave = fsm->slave;

   401

    int i;

   402

   403

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

   404

        return;

   405

   406

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   407

        fsm->state = ec_fsm_slave_scan_state_error;

   408

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

   409

               slave->ring_position);

   410

        ec_datagram_print_state(datagram);

   411

        return;

   412

    }

   413

   414

    if (datagram->working_counter != 1) {

   415

        fsm->slave->error_flag = 1;

   416

        fsm->state = ec_fsm_slave_scan_state_error;

   417

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

   418

                slave->ring_position);

   419

        ec_datagram_print_wc_error(datagram);

   420

        return;

   421

    }

   422

   423

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

   424

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

   425

    }

   426

   427

    ec_fsm_slave_scan_enter_datalink(fsm);

   428

}

   429

   430

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

   431

   432

/**

   433

   Slave scan entry function: DATALINK.

   434

*/

   435

   436

void ec_fsm_slave_scan_enter_datalink(

   437

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

   438

        )

   439

{

   440

    ec_datagram_t *datagram = fsm->datagram;

   441

    ec_slave_t *slave = fsm->slave;

   442

   443

    // read data link status

   444

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

   445

    ec_datagram_zero(datagram);

   446

    fsm->retries = EC_FSM_RETRIES;

   447

    fsm->state = ec_fsm_slave_scan_state_datalink;

   448

}

   449

   450

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

   451

   452

/**

   453

   Slave scan state: DATALINK.

   454

*/

   455

   456

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

   457

{

   458

    ec_datagram_t *datagram = fsm->datagram;

   459

    ec_slave_t *slave = fsm->slave;

   460

    uint16_t dl_status;

   461

    unsigned int i;

   462

   463

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

   464

        return;

   465

   466

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   467

        fsm->state = ec_fsm_slave_scan_state_error;

   468

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

   469

               slave->ring_position);

   470

        ec_datagram_print_state(datagram);

   471

        return;

   472

    }

   473

   474

    if (datagram->working_counter != 1) {

   475

        fsm->slave->error_flag = 1;

   476

        fsm->state = ec_fsm_slave_scan_state_error;

   477

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

   478

               slave->ring_position);

   479

        ec_datagram_print_wc_error(datagram);

   480

        return;

   481

    }

   482

   483

    dl_status = EC_READ_U16(datagram->data);

   484

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

   485

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

   486

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

   487

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

   488

    }

   489

   490

    // Start fetching SII size

   491

   492

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

   493

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

   494

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   495

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   496

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

   497

}

   498

   499

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

   500

   501

/**

   502

   Slave scan state: SII SIZE.

   503

*/

   504

   505

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

   506

{

   507

    ec_slave_t *slave = fsm->slave;

   508

    uint16_t cat_type, cat_size;

   509