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

#ifdef EC_SII_ASSIGN

    54

void ec_fsm_slave_scan_state_assign_sii(ec_fsm_slave_scan_t *);

    55

#endif

    56

void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);

    57

void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);

    58

#ifdef EC_REGALIAS

    59

void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *);

    60

#endif

    61

void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);

    62

void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);

    63

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    64

    65

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    66

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    67

    68

void ec_fsm_slave_scan_enter_datalink(ec_fsm_slave_scan_t *);

    69

#ifdef EC_REGALIAS

    70

void ec_fsm_slave_scan_enter_regalias(ec_fsm_slave_scan_t *);

    71

#endif

    72

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    73

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    74

    75

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

    76

    77

/** Constructor.

    78

 */

    79

void ec_fsm_slave_scan_init(

    80

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

    81

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

    82

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    83

                                                  state machine to use. */

    84

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

    85

        )

    86

{

    87

    fsm->datagram = datagram;

    88

    fsm->fsm_slave_config = fsm_slave_config;

    89

    fsm->fsm_pdo = fsm_pdo;

    90

    91

    // init sub state machines

    92

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

    93

}

    94

    95

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

    96

    97

/** Destructor.

    98

 */

    99

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

   100

{

   101

    // clear sub state machines

   102

    ec_fsm_sii_clear(&fsm->fsm_sii);

   103

}

   104

   105

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

   106

   107

/**

   108

 * Start slave scan state machine.

   109

 */

   110

   111

void ec_fsm_slave_scan_start(

   112

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

   113

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

   114

        )

   115

{

   116

    fsm->slave = slave;

   117

    fsm->state = ec_fsm_slave_scan_state_start;

   118

}

   119

   120

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

   121

   122

/**

   123

   \return false, if state machine has terminated

   124

*/

   125

   126

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

   127

{

   128

    return fsm->state != ec_fsm_slave_scan_state_end

   129

        && fsm->state != ec_fsm_slave_scan_state_error;

   130

}

   131

   132

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

   133

   134

/**

   135

   Executes the current state of the state machine.

   136

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

   137

   of the state machine is delayed to the next cycle.

   138

   \return false, if state machine has terminated

   139

*/

   140

   141

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

   142

{

   143

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

   144

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

   145

        // datagram was not sent or received yet.

   146

        return ec_fsm_slave_scan_running(fsm);

   147

    }

   148

   149

    fsm->state(fsm);

   150

    return ec_fsm_slave_scan_running(fsm);

   151

}

   152

   153

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

   154

   155

/**

   156

   \return true, if the state machine terminated gracefully

   157

*/

   158

   159

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

   160

{

   161

    return fsm->state == ec_fsm_slave_scan_state_end;

   162

}

   163

   164

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

   165

 *  slave scan state machine

   166

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

   167

   168

/**

   169

   Slave scan state: START.

   170

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

   171

   slave, according to its ring position.

   172

*/

   173

   174

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

   175

{

   176

    // write station address

   177

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

   178

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

   179

    fsm->retries = EC_FSM_RETRIES;

   180

    fsm->state = ec_fsm_slave_scan_state_address;

   181

}

   182

   183

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

   184

   185

/**

   186

   Slave scan state: ADDRESS.

   187

*/

   188

   189

void ec_fsm_slave_scan_state_address(

   190

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

   191

        )

   192

{

   193

    ec_datagram_t *datagram = fsm->datagram;

   194

   195

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

   196

        return;

   197

   198

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   199

        fsm->state = ec_fsm_slave_scan_state_error;

   200

        EC_SLAVE_ERR(fsm->slave,

   201

                "Failed to receive station address datagram: ");

   202

        ec_datagram_print_state(datagram);

   203

        return;

   204

    }

   205

   206

    if (datagram->working_counter != 1) {

   207

        fsm->slave->error_flag = 1;

   208

        fsm->state = ec_fsm_slave_scan_state_error;

   209

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

   210

        ec_datagram_print_wc_error(datagram);

   211

        return;

   212

    }

   213

   214

    // Read AL state

   215

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

   216

    ec_datagram_zero(datagram);

   217

    fsm->retries = EC_FSM_RETRIES;

   218

    fsm->state = ec_fsm_slave_scan_state_state;

   219

}

   220

   221

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

   222

   223

/**

   224

   Slave scan state: STATE.

   225

*/

   226

   227

void ec_fsm_slave_scan_state_state(

   228

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

   229

        )

   230

{

   231

    ec_datagram_t *datagram = fsm->datagram;

   232

    ec_slave_t *slave = fsm->slave;

   233

   234

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

   235

        return;

   236

   237

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   238

        fsm->state = ec_fsm_slave_scan_state_error;

   239

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

   240

        ec_datagram_print_state(datagram);

   241

        return;

   242

    }

   243

   244

    if (datagram->working_counter != 1) {

   245

        fsm->slave->error_flag = 1;

   246

        fsm->state = ec_fsm_slave_scan_state_error;

   247

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

   248

        ec_datagram_print_wc_error(datagram);

   249

        return;

   250

    }

   251

   252

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

   253

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   254

        char state_str[EC_STATE_STRING_SIZE];

   255

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

   256

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

   257

                state_str);

   258

    }

   259

   260

    // read base data

   261

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

   262

    ec_datagram_zero(datagram);

   263

    fsm->retries = EC_FSM_RETRIES;

   264

    fsm->state = ec_fsm_slave_scan_state_base;

   265

}

   266

   267

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

   268

   269

/** Slave scan state: BASE.

   270

 */

   271

void ec_fsm_slave_scan_state_base(

   272

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

   273

        )

   274

{

   275

    ec_datagram_t *datagram = fsm->datagram;

   276

    ec_slave_t *slave = fsm->slave;

   277

    u8 octet;

   278

    int i;

   279

   280

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

   281

        return;

   282

   283

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   284

        fsm->state = ec_fsm_slave_scan_state_error;

   285

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

   286

        ec_datagram_print_state(datagram);

   287

        return;

   288

    }

   289

   290

    if (datagram->working_counter != 1) {

   291

        fsm->slave->error_flag = 1;

   292

        fsm->state = ec_fsm_slave_scan_state_error;

   293

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

   294

        ec_datagram_print_wc_error(datagram);

   295

        return;

   296

    }

   297

   298

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

   299

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

   300

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

   301

   302

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

   303

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   304

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

   305

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

   306

        slave->base_fmmu_count = EC_MAX_FMMUS;

   307

    }

   308

   309

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

   310

    if (slave->base_sync_count > EC_MAX_SYNC_MANAGERS) {

   311

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

   312

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

   313

                slave->base_sync_count, EC_MAX_SYNC_MANAGERS);

   314

        slave->base_sync_count = EC_MAX_SYNC_MANAGERS;

   315

    }

   316

   317

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

   318

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

   319

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

   320

    }

   321

   322

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

   323

    slave->base_fmmu_bit_operation = octet & 0x01;

   324

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

   325

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

   326

   327

    if (slave->base_dc_supported) {

   328

        // read DC capabilities

   329

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

   330

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

   331

        ec_datagram_zero(datagram);

   332

        fsm->retries = EC_FSM_RETRIES;

   333

        fsm->state = ec_fsm_slave_scan_state_dc_cap;

   334

    } else {

   335

        ec_fsm_slave_scan_enter_datalink(fsm);

   336

    }

   337

}

   338

   339

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

   340

   341

/**

   342

   Slave scan state: DC CAPABILITIES.

   343

*/

   344

   345

void ec_fsm_slave_scan_state_dc_cap(

   346

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

   347

        )

   348

{

   349

    ec_datagram_t *datagram = fsm->datagram;

   350

    ec_slave_t *slave = fsm->slave;

   351

   352

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

   353

        return;

   354

   355

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   356

        fsm->state = ec_fsm_slave_scan_state_error;

   357

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

   358

        ec_datagram_print_state(datagram);

   359

        return;

   360

    }

   361

   362

    if (datagram->working_counter == 1) {

   363

        slave->has_dc_system_time = 1;

   364

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

   365

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

   366

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

   367

                "measurement only.\n");

   368

    } else {

   369

        fsm->slave->error_flag = 1;

   370

        fsm->state = ec_fsm_slave_scan_state_error;

   371

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

   372

                "supported: ");

   373

        ec_datagram_print_wc_error(datagram);

   374

        return;

   375

    }

   376

   377

    // read DC port receive times

   378

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

   379

    ec_datagram_zero(datagram);

   380

    fsm->retries = EC_FSM_RETRIES;

   381

    fsm->state = ec_fsm_slave_scan_state_dc_times;

   382

}

   383

   384

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

   385

   386

/**

   387

   Slave scan state: DC TIMES.

   388

*/

   389

   390

void ec_fsm_slave_scan_state_dc_times(

   391

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

   392

        )

   393

{

   394

    ec_datagram_t *datagram = fsm->datagram;

   395

    ec_slave_t *slave = fsm->slave;

   396

    int i;

   397

   398

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

   399

        return;

   400

   401

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   402

        fsm->state = ec_fsm_slave_scan_state_error;

   403

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

   404

        ec_datagram_print_state(datagram);

   405

        return;

   406

    }

   407

   408

    if (datagram->working_counter != 1) {

   409

        fsm->slave->error_flag = 1;

   410

        fsm->state = ec_fsm_slave_scan_state_error;

   411

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

   412

        ec_datagram_print_wc_error(datagram);

   413

        return;

   414

    }

   415

   416

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

   417

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

   418

    }

   419

   420

    ec_fsm_slave_scan_enter_datalink(fsm);

   421

}

   422

   423

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

   424

   425

/**

   426

   Slave scan entry function: DATALINK.

   427

*/

   428

   429

void ec_fsm_slave_scan_enter_datalink(

   430

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

   431

        )

   432

{

   433

    ec_datagram_t *datagram = fsm->datagram;

   434

    ec_slave_t *slave = fsm->slave;

   435

   436

    // read data link status

   437

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

   438

    ec_datagram_zero(datagram);

   439

    fsm->retries = EC_FSM_RETRIES;

   440

    fsm->state = ec_fsm_slave_scan_state_datalink;

   441

}

   442

   443

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

   444

   445

/** Enter slave scan state SII_SIZE.

   446

 */

   447

void ec_fsm_slave_scan_enter_sii_size(

   448

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

   449

        )

   450

{

   451

    // Start fetching SII size

   452

   453

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

   454

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

   455

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   456

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   457

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

   458

}

   459

   460

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

   461

   462

#ifdef EC_SII_ASSIGN

   463

   464

/** Enter slave scan state ASSIGN_SII.

   465

 */

   466

void ec_fsm_slave_scan_enter_assign_sii(

   467

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

   468

        )

   469

{

   470

    ec_datagram_t *datagram = fsm->datagram;

   471

    ec_slave_t *slave = fsm->slave;

   472

   473

    EC_SLAVE_DBG(slave, 1, "Assigning SII access to EtherCAT.\n");

   474

   475

    // assign SII to ECAT

   476

    ec_datagram_fpwr(datagram, slave->station_address, 0x0500, 1);

   477

    EC_WRITE_U8(datagram->data, 0x00); // EtherCAT

   478

    fsm->retries = EC_FSM_RETRIES;

   479

    fsm->state = ec_fsm_slave_scan_state_assign_sii;

   480

}

   481

   482

#endif

   483

   484

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

   485

   486

/**

   487

   Slave scan state: DATALINK.

   488

*/

   489

   490

void ec_fsm_slave_scan_state_datalink(

   491

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

   492

        )

   493

{

   494

    ec_datagram_t *datagram = fsm->datagram;

   495

    ec_slave_t *slave = fsm->slave;

   496

   497

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

   498

        return;

   499

   500

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   501

        fsm->state = ec_fsm_slave_scan_state_error;

   502

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

   503

        ec_datagram_print_state(datagram);

   504

        return;

   505

    }

   506