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

#ifdef EC_REGALIAS

    56

void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *);

    57

#endif

    58

void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);

    59

void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);

    60

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    61

    62

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    63

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    64

    65

void ec_fsm_slave_scan_enter_datalink(ec_fsm_slave_scan_t *);

    66

#ifdef EC_REGALIAS

    67

void ec_fsm_slave_scan_enter_regalias(ec_fsm_slave_scan_t *);

    68

#endif

    69

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    70

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    71

    72

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

    73

    74

/** Constructor.

    75

 */

    76

void ec_fsm_slave_scan_init(

    77

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

    78

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

    79

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    80

                                                  state machine to use. */

    81

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

    82

        )

    83

{

    84

    fsm->datagram = datagram;

    85

    fsm->fsm_slave_config = fsm_slave_config;

    86

    fsm->fsm_pdo = fsm_pdo;

    87

    88

    // init sub state machines

    89

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

    90

}

    91

    92

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

    93

    94

/** Destructor.

    95

 */

    96

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

    97

{

    98

    // clear sub state machines

    99

    ec_fsm_sii_clear(&fsm->fsm_sii);

   100

}

   101

   102

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

   103

   104

/**

   105

 * Start slave scan state machine.

   106

 */

   107

   108

void ec_fsm_slave_scan_start(

   109

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

   110

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

   111

        )

   112

{

   113

    fsm->slave = slave;

   114

    fsm->state = ec_fsm_slave_scan_state_start;

   115

}

   116

   117

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

   118

   119

/**

   120

   \return false, if state machine has terminated

   121

*/

   122

   123

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

   124

{

   125

    return fsm->state != ec_fsm_slave_scan_state_end

   126

        && fsm->state != ec_fsm_slave_scan_state_error;

   127

}

   128

   129

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

   130

   131

/**

   132

   Executes the current state of the state machine.

   133

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

   134

   of the state machine is delayed to the next cycle.

   135

   \return false, if state machine has terminated

   136

*/

   137

   138

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

   139

{

   140

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

   141

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

   142

        // datagram was not sent or received yet.

   143

        return ec_fsm_slave_scan_running(fsm);

   144

    }

   145

   146

    fsm->state(fsm);

   147

    return ec_fsm_slave_scan_running(fsm);

   148

}

   149

   150

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

   151

   152

/**

   153

   \return true, if the state machine terminated gracefully

   154

*/

   155

   156

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

   157

{

   158

    return fsm->state == ec_fsm_slave_scan_state_end;

   159

}

   160

   161

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

   162

 *  slave scan state machine

   163

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

   164

   165

/**

   166

   Slave scan state: START.

   167

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

   168

   slave, according to its ring position.

   169

*/

   170

   171

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

   172

{

   173

    // write station address

   174

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

   175

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

   176

    fsm->retries = EC_FSM_RETRIES;

   177

    fsm->state = ec_fsm_slave_scan_state_address;

   178

}

   179

   180

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

   181

   182

/**

   183

   Slave scan state: ADDRESS.

   184

*/

   185

   186

void ec_fsm_slave_scan_state_address(

   187

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

   188

        )

   189

{

   190

    ec_datagram_t *datagram = fsm->datagram;

   191

   192

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

   193

        return;

   194

   195

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   196

        fsm->state = ec_fsm_slave_scan_state_error;

   197

        EC_SLAVE_ERR(fsm->slave,

   198

                "Failed to receive station address datagram: ");

   199

        ec_datagram_print_state(datagram);

   200

        return;

   201

    }

   202

   203

    if (datagram->working_counter != 1) {

   204

        fsm->slave->error_flag = 1;

   205

        fsm->state = ec_fsm_slave_scan_state_error;

   206

        EC_SLAVE_ERR(fsm->slave, "Failed to write station address: ");

   207

        ec_datagram_print_wc_error(datagram);

   208

        return;

   209

    }

   210

   211

    // Read AL state

   212

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

   213

    ec_datagram_zero(datagram);

   214

    fsm->retries = EC_FSM_RETRIES;

   215

    fsm->state = ec_fsm_slave_scan_state_state;

   216

}

   217

   218

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

   219

   220

/**

   221

   Slave scan state: STATE.

   222

*/

   223

   224

void ec_fsm_slave_scan_state_state(

   225

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

   226

        )

   227

{

   228

    ec_datagram_t *datagram = fsm->datagram;

   229

    ec_slave_t *slave = fsm->slave;

   230

   231

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

   232

        return;

   233

   234

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   235

        fsm->state = ec_fsm_slave_scan_state_error;

   236

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

   237

        ec_datagram_print_state(datagram);

   238

        return;

   239

    }

   240

   241

    if (datagram->working_counter != 1) {

   242

        fsm->slave->error_flag = 1;

   243

        fsm->state = ec_fsm_slave_scan_state_error;

   244

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

   245

        ec_datagram_print_wc_error(datagram);

   246

        return;

   247

    }

   248

   249

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

   250

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   251

        char state_str[EC_STATE_STRING_SIZE];

   252

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

   253

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

   254

                state_str);

   255

    }

   256

   257

    // read base data

   258

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

   259

    ec_datagram_zero(datagram);

   260

    fsm->retries = EC_FSM_RETRIES;

   261

    fsm->state = ec_fsm_slave_scan_state_base;

   262

}

   263

   264

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

   265

   266

/** Slave scan state: BASE.

   267

 */

   268

void ec_fsm_slave_scan_state_base(

   269

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

   270

        )

   271

{

   272

    ec_datagram_t *datagram = fsm->datagram;

   273

    ec_slave_t *slave = fsm->slave;

   274

    u8 octet;

   275

    int i;

   276

   277

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

   278

        return;

   279

   280

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   281

        fsm->state = ec_fsm_slave_scan_state_error;

   282

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

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

   291

        ec_datagram_print_wc_error(datagram);

   292

        return;

   293

    }

   294

   295

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

   296

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

   297

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

   298

   299

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

   300

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   301

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

   302

                " handle (%u).\n", 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_SLAVE_WARN(slave, "Slave provides more sync managers (%u)"

   309

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

   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_SLAVE_ERR(slave, "Failed to receive system time datagram: ");

   355

        ec_datagram_print_state(datagram);

   356

        return;

   357

    }

   358

   359

    if (datagram->working_counter == 1) {

   360

        slave->has_dc_system_time = 1;

   361

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

   362

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

   363

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

   364

                "measurement only.\n");

   365

    } else {

   366

        fsm->slave->error_flag = 1;

   367

        fsm->state = ec_fsm_slave_scan_state_error;

   368

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

   369

                "supported: ");

   370

        ec_datagram_print_wc_error(datagram);

   371

        return;

   372

    }

   373

   374

    // read DC port receive times

   375

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

   376

    ec_datagram_zero(datagram);

   377

    fsm->retries = EC_FSM_RETRIES;

   378

    fsm->state = ec_fsm_slave_scan_state_dc_times;

   379

}

   380

   381

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

   382

   383

/**

   384

   Slave scan state: DC TIMES.

   385

*/

   386

   387

void ec_fsm_slave_scan_state_dc_times(

   388

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

   389

        )

   390

{

   391

    ec_datagram_t *datagram = fsm->datagram;

   392

    ec_slave_t *slave = fsm->slave;

   393

    int i;

   394

   395

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

   396

        return;

   397

   398

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   399

        fsm->state = ec_fsm_slave_scan_state_error;

   400

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

   401

        ec_datagram_print_state(datagram);

   402

        return;

   403

    }

   404

   405

    if (datagram->working_counter != 1) {

   406

        fsm->slave->error_flag = 1;

   407

        fsm->state = ec_fsm_slave_scan_state_error;

   408

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

   409

        ec_datagram_print_wc_error(datagram);

   410

        return;

   411

    }

   412

   413

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

   414

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

   415

    }

   416

   417

    ec_fsm_slave_scan_enter_datalink(fsm);

   418

}

   419

   420

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

   421

   422

/**

   423

   Slave scan entry function: DATALINK.

   424

*/

   425

   426

void ec_fsm_slave_scan_enter_datalink(

   427

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

   428

        )

   429

{

   430

    ec_datagram_t *datagram = fsm->datagram;

   431

    ec_slave_t *slave = fsm->slave;

   432

   433

    // read data link status

   434

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

   435

    ec_datagram_zero(datagram);

   436

    fsm->retries = EC_FSM_RETRIES;

   437

    fsm->state = ec_fsm_slave_scan_state_datalink;

   438

}

   439

   440

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

   441

   442

/**

   443

   Slave scan state: DATALINK.

   444

*/

   445

   446

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

   447

{

   448

    ec_datagram_t *datagram = fsm->datagram;

   449

    ec_slave_t *slave = fsm->slave;

   450

    uint16_t dl_status;

   451

    unsigned int i;

   452

   453

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

   454

        return;

   455

   456

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   457

        fsm->state = ec_fsm_slave_scan_state_error;

   458

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

   459

        ec_datagram_print_state(datagram);

   460

        return;

   461

    }

   462

   463

    if (datagram->working_counter != 1) {

   464

        fsm->slave->error_flag = 1;

   465

        fsm->state = ec_fsm_slave_scan_state_error;

   466

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

   467

        ec_datagram_print_wc_error(datagram);

   468

        return;

   469

    }

   470

   471

    dl_status = EC_READ_U16(datagram->data);

   472

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

   473

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

   474

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

   475

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

   476

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

   477

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

   478

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

   479

    }

   480

   481

    // Start fetching SII size

   482

   483

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

   484

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

   485

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   486

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   487

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

   488

}

   489

   490

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

   491

   492

/**

   493

   Slave scan state: SII SIZE.

   494

*/

   495

   496

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

   497

{

   498

    ec_slave_t *slave = fsm->slave;

   499

    uint16_t cat_type, cat_size;

   500

   501

    if (ec_fsm_sii_exec(&fsm->fsm_sii))

   502

        return;

   503

   504

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

   505

        fsm->slave->error_flag = 1;

   506

        fsm->state = ec_fsm_slave_scan_state_error;

   507

        EC_SLAVE_ERR(slave, "Failed to determine SII content size:"