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_SLAVE_ERR(fsm->slave,

   194

                "Failed to receive station address datagram: ");

   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_SLAVE_ERR(fsm->slave, "Failed to write station address: ");

   203

        ec_datagram_print_wc_error(datagram);

   204

        return;

   205

    }

   206

   207

    // Read AL state

   208

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

   209

    ec_datagram_zero(datagram);

   210

    fsm->retries = EC_FSM_RETRIES;

   211

    fsm->state = ec_fsm_slave_scan_state_state;

   212

}

   213

   214

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

   215

   216

/**

   217

   Slave scan state: STATE.

   218

*/

   219

   220

void ec_fsm_slave_scan_state_state(

   221

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

   222

        )

   223

{

   224

    ec_datagram_t *datagram = fsm->datagram;

   225

    ec_slave_t *slave = fsm->slave;

   226

   227

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

   228

        return;

   229

   230

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   231

        fsm->state = ec_fsm_slave_scan_state_error;

   232

        EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: ");

   233

        ec_datagram_print_state(datagram);

   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_SLAVE_ERR(slave, "Failed to read AL state: ");

   241

        ec_datagram_print_wc_error(datagram);

   242

        return;

   243

    }

   244

   245

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

   246

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   247

        char state_str[EC_STATE_STRING_SIZE];

   248

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

   249

        EC_SLAVE_WARN(slave, "Slave has state error bit set (%s)!\n",

   250

                state_str);

   251

    }

   252

   253

    // read base data

   254

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

   255

    ec_datagram_zero(datagram);

   256

    fsm->retries = EC_FSM_RETRIES;

   257

    fsm->state = ec_fsm_slave_scan_state_base;

   258

}

   259

   260

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

   261

   262

/** Slave scan state: BASE.

   263

 */

   264

void ec_fsm_slave_scan_state_base(

   265

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

   266

        )

   267

{

   268

    ec_datagram_t *datagram = fsm->datagram;

   269

    ec_slave_t *slave = fsm->slave;

   270

    u8 octet;

   271

    int i;

   272

   273

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

   274

        return;

   275

   276

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   277

        fsm->state = ec_fsm_slave_scan_state_error;

   278

        EC_SLAVE_ERR(slave, "Failed to receive base data datagram: ");

   279

        ec_datagram_print_state(datagram);

   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_SLAVE_ERR(slave, "Failed to read base data: ");

   287

        ec_datagram_print_wc_error(datagram);

   288

        return;

   289

    }

   290

   291

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

   292

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

   293

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

   294

   295

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

   296

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   297

        EC_SLAVE_WARN(slave, "Slave has more FMMUs (%u) than the master can"

   298

                " handle (%u).\n", slave->base_fmmu_count, EC_MAX_FMMUS);

   299

        slave->base_fmmu_count = EC_MAX_FMMUS;

   300

    }

   301

   302

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

   303

    if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) {

   304

        EC_SLAVE_WARN(slave, "Slave provides more sync managers (%u)"

   305

                " than the master can handle (%u).\n",

   306

                slave->base_sync_count, EC_MAX_SYNC_MANAGERS);

   307

        slave->base_sync_count = EC_MAX_SYNC_MANAGERS;

   308

    }

   309

   310

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

   311

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

   312

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

   313

    }

   314

   315

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

   316

    slave->base_fmmu_bit_operation = octet & 0x01;

   317

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

   318

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

   319

   320

    if (slave->base_dc_supported) {

   321

        // read DC capabilities

   322

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

   323

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

   324

        ec_datagram_zero(datagram);

   325

        fsm->retries = EC_FSM_RETRIES;

   326

        fsm->state = ec_fsm_slave_scan_state_dc_cap;

   327

    } else {

   328

        ec_fsm_slave_scan_enter_datalink(fsm);

   329

    }

   330

}

   331

   332

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

   333

   334

/**

   335

   Slave scan state: DC CAPABILITIES.

   336

*/

   337

   338

void ec_fsm_slave_scan_state_dc_cap(

   339

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

   340

        )

   341

{

   342

    ec_datagram_t *datagram = fsm->datagram;

   343

    ec_slave_t *slave = fsm->slave;

   344

   345

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

   346

        return;

   347

   348

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   349

        fsm->state = ec_fsm_slave_scan_state_error;

   350

        EC_SLAVE_ERR(slave, "Failed to receive system time datagram: ");

   351

        ec_datagram_print_state(datagram);

   352

        return;

   353

    }

   354

   355

    if (datagram->working_counter == 1) {

   356

        slave->has_dc_system_time = 1;

   357

        EC_SLAVE_DBG(slave, 1, "Slave has the System Time register.\n");

   358

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

   359

        EC_SLAVE_DBG(slave, 1, "Slave has no System Time register; delay "

   360

                "measurement only.\n");

   361

    } else {

   362

        fsm->slave->error_flag = 1;

   363

        fsm->state = ec_fsm_slave_scan_state_error;

   364

        EC_SLAVE_ERR(slave, "Failed to determine, if system time register is "

   365

                "supported: ");

   366

        ec_datagram_print_wc_error(datagram);

   367

        return;

   368

    }

   369

   370

    // read DC port receive times

   371

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

   372

    ec_datagram_zero(datagram);

   373

    fsm->retries = EC_FSM_RETRIES;

   374

    fsm->state = ec_fsm_slave_scan_state_dc_times;

   375

}

   376

   377

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

   378

   379

/**

   380

   Slave scan state: DC TIMES.

   381

*/

   382

   383

void ec_fsm_slave_scan_state_dc_times(

   384

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

   385

        )

   386

{

   387

    ec_datagram_t *datagram = fsm->datagram;

   388

    ec_slave_t *slave = fsm->slave;

   389

    int i;

   390

   391

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

   392

        return;

   393

   394

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   395

        fsm->state = ec_fsm_slave_scan_state_error;

   396

        EC_SLAVE_ERR(slave, "Failed to receive system time datagram: ");

   397

        ec_datagram_print_state(datagram);

   398

        return;

   399

    }

   400

   401

    if (datagram->working_counter != 1) {

   402

        fsm->slave->error_flag = 1;

   403

        fsm->state = ec_fsm_slave_scan_state_error;

   404

        EC_SLAVE_ERR(slave, "Failed to get DC receive times: ");

   405

        ec_datagram_print_wc_error(datagram);

   406

        return;

   407

    }

   408

   409

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

   410

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

   411

    }

   412

   413

    ec_fsm_slave_scan_enter_datalink(fsm);

   414

}

   415

   416

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

   417

   418

/**

   419

   Slave scan entry function: DATALINK.

   420

*/

   421

   422

void ec_fsm_slave_scan_enter_datalink(

   423

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

   424

        )

   425

{

   426

    ec_datagram_t *datagram = fsm->datagram;

   427

    ec_slave_t *slave = fsm->slave;

   428

   429

    // read data link status

   430

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

   431

    ec_datagram_zero(datagram);

   432

    fsm->retries = EC_FSM_RETRIES;

   433

    fsm->state = ec_fsm_slave_scan_state_datalink;

   434

}

   435

   436

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

   437

   438

/**

   439

   Slave scan state: DATALINK.

   440

*/

   441

   442

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

   443

{

   444

    ec_datagram_t *datagram = fsm->datagram;

   445

    ec_slave_t *slave = fsm->slave;

   446

    uint16_t dl_status;

   447

    unsigned int i;

   448

   449

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

   450

        return;

   451

   452

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   453

        fsm->state = ec_fsm_slave_scan_state_error;

   454

        EC_SLAVE_ERR(slave, "Failed to receive DL status datagram: ");

   455

        ec_datagram_print_state(datagram);

   456

        return;

   457

    }

   458

   459

    if (datagram->working_counter != 1) {

   460

        fsm->slave->error_flag = 1;

   461

        fsm->state = ec_fsm_slave_scan_state_error;

   462

        EC_SLAVE_ERR(slave, "Failed to read DL status: ");

   463

        ec_datagram_print_wc_error(datagram);

   464

        return;

   465

    }

   466

   467

    dl_status = EC_READ_U16(datagram->data);

   468

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

   469

        slave->ports[i].link.link_up =

   470

            dl_status & (1 << (4 + i)) ? 1 : 0;

   471

        slave->ports[i].link.loop_closed =

   472

            dl_status & (1 << (8 + i * 2)) ? 1 : 0;

   473

        slave->ports[i].link.signal_detected =

   474

            dl_status & (1 << (9 + i * 2)) ? 1 : 0;

   475

    }

   476

   477

    // Start fetching SII size

   478

   479

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

   480

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

   481

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   482

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   483

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

   484

}

   485

   486

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

   487

   488

/**

   489

   Slave scan state: SII SIZE.

   490

*/

   491

   492

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

   493

{

   494

    ec_slave_t *slave = fsm->slave;

   495

    uint16_t cat_type, cat_size;

   496

   497

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

   498

   499

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

   500

        fsm->slave->error_flag = 1;

   501

        fsm->state = ec_fsm_slave_scan_state_error;

   502

        EC_SLAVE_ERR(slave, "Failed to read SII size.\n");

   503

        return;

   504

    }

   505

   506

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

   507

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

   508