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

/** \file

    31

 *

    32

 * EtherCAT slave configuration state machine.

    33

 */

    34

    35

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

    36

    37

#include <asm/div64.h>

    38

    39

#include "globals.h"

    40

#include "master.h"

    41

#include "mailbox.h"

    42

#include "slave_config.h"

    43

#include "fsm_slave_config.h"

    44

    45

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

    46

    47

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    48

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    49

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    50

void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);

    51

void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *);

    52

void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *);

    53

void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *);

    54

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    55

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    56

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    62

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    67

    68

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    72

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    73

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    74

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    75

void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);

    76

void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);

    77

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    78

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    79

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    80

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    81

    82

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    83

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    84

    85

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    86

    87

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

    88

    89

/** Constructor.

    90

 */

    91

void ec_fsm_slave_config_init(

    92

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

    93

        ec_datagram_t *datagram, /**< datagram structure to use */

    94

        ec_fsm_change_t *fsm_change, /**< State change state machine to use. */

    95

        ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */

    96

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

    97

        )

    98

{

    99

    ec_sdo_request_init(&fsm->request_copy);

   100

   101

    fsm->datagram = datagram;

   102

    fsm->fsm_change = fsm_change;

   103

    fsm->fsm_coe = fsm_coe;

   104

    fsm->fsm_pdo = fsm_pdo;

   105

}

   106

   107

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

   108

   109

/** Destructor.

   110

 */

   111

void ec_fsm_slave_config_clear(

   112

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   113

        )

   114

{

   115

    ec_sdo_request_clear(&fsm->request_copy);

   116

}

   117

   118

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

   119

   120

/** Start slave configuration state machine.

   121

 */

   122

void ec_fsm_slave_config_start(

   123

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

   124

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

   125

        )

   126

{

   127

    fsm->slave = slave;

   128

    fsm->state = ec_fsm_slave_config_state_start;

   129

}

   130

   131

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

   132

   133

/**

   134

 * \return false, if state machine has terminated

   135

 */

   136

int ec_fsm_slave_config_running(

   137

        const ec_fsm_slave_config_t *fsm /**< slave state machine */

   138

        )

   139

{

   140

    return fsm->state != ec_fsm_slave_config_state_end

   141

        && fsm->state != ec_fsm_slave_config_state_error;

   142

}

   143

   144

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

   145

   146

/** Executes the current state of the state machine.

   147

 *

   148

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

   149

 * of the state machine is delayed to the next cycle.

   150

 *

   151

 * \return false, if state machine has terminated

   152

 */

   153

int ec_fsm_slave_config_exec(

   154

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   155

        )

   156

{

   157

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

   158

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

   159

        // datagram was not sent or received yet.

   160

        return ec_fsm_slave_config_running(fsm);

   161

    }

   162

   163

    fsm->state(fsm);

   164

    return ec_fsm_slave_config_running(fsm);

   165

}

   166

   167

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

   168

   169

/**

   170

 * \return true, if the state machine terminated gracefully

   171

 */

   172

int ec_fsm_slave_config_success(

   173

        const ec_fsm_slave_config_t *fsm /**< slave state machine */

   174

        )

   175

{

   176

    return fsm->state == ec_fsm_slave_config_state_end;

   177

}

   178

   179

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

   180

 * Slave configuration state machine

   181

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

   182

   183

/** Slave configuration state: START.

   184

 */

   185

void ec_fsm_slave_config_state_start(

   186

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   187

        )

   188

{

   189

    if (fsm->slave->master->debug_level) {

   190

        EC_DBG("Configuring slave %u...\n", fsm->slave->ring_position);

   191

    }

   192

   193

    ec_fsm_slave_config_enter_init(fsm);

   194

}

   195

   196

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

   197

   198

/** Start state change to INIT.

   199

 */

   200

void ec_fsm_slave_config_enter_init(

   201

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   202

        )

   203

{

   204

    ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);

   205

    ec_fsm_change_exec(fsm->fsm_change);

   206

    fsm->state = ec_fsm_slave_config_state_init;

   207

}

   208

   209

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

   210

   211

/** Slave configuration state: INIT.

   212

 */

   213

void ec_fsm_slave_config_state_init(

   214

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   215

        )

   216

{

   217

    ec_master_t *master = fsm->slave->master;

   218

    ec_slave_t *slave = fsm->slave;

   219

    ec_datagram_t *datagram = fsm->datagram;

   220

   221

    if (ec_fsm_change_exec(fsm->fsm_change)) return;

   222

   223

    if (!ec_fsm_change_success(fsm->fsm_change)) {

   224

        if (!fsm->fsm_change->spontaneous_change)

   225

            slave->error_flag = 1;

   226

        fsm->state = ec_fsm_slave_config_state_error;

   227

        return;

   228

    }

   229

   230

    if (master->debug_level) {

   231

        EC_DBG("Slave %u is now in INIT.\n", slave->ring_position);

   232

    }

   233

   234

    if (!slave->base_fmmu_count) { // skip FMMU configuration

   235

        ec_fsm_slave_config_enter_clear_sync(fsm);

   236

        return;

   237

    }

   238

   239

    if (master->debug_level)

   240

        EC_DBG("Clearing FMMU configurations of slave %u...\n",

   241

               slave->ring_position);

   242

   243

    // clear FMMU configurations

   244

    ec_datagram_fpwr(datagram, slave->station_address,

   245

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   246

    ec_datagram_zero(datagram);

   247

    fsm->retries = EC_FSM_RETRIES;

   248

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   249

}

   250

   251

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

   252

   253

/** Slave configuration state: CLEAR FMMU.

   254

 */

   255

void ec_fsm_slave_config_state_clear_fmmus(

   256

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   257

        )

   258

{

   259

    ec_datagram_t *datagram = fsm->datagram;

   260

   261

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

   262

        return;

   263

   264

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   265

        fsm->state = ec_fsm_slave_config_state_error;

   266

        EC_ERR("Failed receive FMMU clearing datagram for slave %u.\n",

   267

               fsm->slave->ring_position);

   268

        return;

   269

    }

   270

   271

    if (datagram->working_counter != 1) {

   272

        fsm->slave->error_flag = 1;

   273

        fsm->state = ec_fsm_slave_config_state_error;

   274

        EC_ERR("Failed to clear FMMUs of slave %u: ",

   275

               fsm->slave->ring_position);

   276

        ec_datagram_print_wc_error(datagram);

   277

        return;

   278

    }

   279

   280

    ec_fsm_slave_config_enter_clear_sync(fsm);

   281

}

   282

   283

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

   284

   285

/** Clear the sync manager configurations.

   286

 */

   287

void ec_fsm_slave_config_enter_clear_sync(

   288

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   289

        )

   290

{

   291

    ec_slave_t *slave = fsm->slave;

   292

    ec_datagram_t *datagram = fsm->datagram;

   293

    size_t sync_size;

   294

   295

    if (!slave->base_sync_count) {

   296

        // no sync managers

   297

        ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   298

        return;

   299

    }

   300

   301

    if (slave->master->debug_level)

   302

        EC_DBG("Clearing sync manager configurations of slave %u...\n",

   303

                slave->ring_position);

   304

   305

    sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count;

   306

   307

    // clear sync manager configurations

   308

    ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size);

   309

    memset(datagram->data, 0x00, sync_size);

   310

    fsm->retries = EC_FSM_RETRIES;

   311

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   312

}

   313

   314

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

   315

   316

/** Slave configuration state: CLEAR SYNC.

   317

 */

   318

void ec_fsm_slave_config_state_clear_sync(

   319

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   320

        )

   321

{

   322

    ec_datagram_t *datagram = fsm->datagram;

   323

   324

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

   325

        return;

   326

   327

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   328

        fsm->state = ec_fsm_slave_config_state_error;

   329

        EC_ERR("Failed receive sync manager clearing datagram"

   330

                " for slave %u.\n", fsm->slave->ring_position);

   331

        return;

   332

    }

   333

   334

    if (datagram->working_counter != 1) {

   335

        fsm->slave->error_flag = 1;

   336

        fsm->state = ec_fsm_slave_config_state_error;

   337

        EC_ERR("Failed to clear sync manager configurations of slave %u: ",

   338

               fsm->slave->ring_position);

   339

        ec_datagram_print_wc_error(datagram);

   340

        return;

   341

    }

   342

   343

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   344

}

   345

   346

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

   347

   348

/** Clear the DC assignment.

   349

 */

   350

void ec_fsm_slave_config_enter_dc_clear_assign(

   351

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   352

        )

   353

{

   354

    ec_slave_t *slave = fsm->slave;

   355

    ec_datagram_t *datagram = fsm->datagram;

   356

   357

    if (!slave->base_dc_supported || !slave->has_dc_system_time) {

   358

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   359

        return;

   360

    }

   361

   362

    if (slave->master->debug_level)

   363

        EC_DBG("Clearing DC assignment of slave %u...\n",

   364

                slave->ring_position);

   365

   366

    ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2);

   367

    ec_datagram_zero(datagram);

   368

    fsm->retries = EC_FSM_RETRIES;

   369

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   370

}

   371

   372

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

   373

   374

/** Slave configuration state: CLEAR DC ASSIGN.

   375

 */

   376

void ec_fsm_slave_config_state_dc_clear_assign(

   377

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   378

        )

   379

{

   380

    ec_datagram_t *datagram = fsm->datagram;

   381

   382

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

   383

        return;

   384

   385

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   386

        fsm->state = ec_fsm_slave_config_state_error;

   387

        EC_ERR("Failed receive DC assignment clearing datagram"

   388

                " for slave %u.\n", fsm->slave->ring_position);

   389

        return;

   390

    }

   391

   392

    if (fsm->slave->master->debug_level && datagram->working_counter != 1) {

   393

        // clearing the DC assignment does not succeed on simple slaves

   394

        EC_DBG("Failed to clear DC assignment of slave %u: ",

   395

               fsm->slave->ring_position);

   396

        ec_datagram_print_wc_error(datagram);

   397

    }

   398

   399

    // read DC system time and system time offset

   400

    ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24);

   401

    fsm->retries = EC_FSM_RETRIES;

   402

    fsm->state = ec_fsm_slave_config_state_dc_read_offset;

   403

}

   404

   405

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

   406

   407

/** Slave configuration state: DC READ OFFSET.

   408

 */

   409

void ec_fsm_slave_config_state_dc_read_offset(

   410

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   411

        )

   412

{

   413

    ec_datagram_t *datagram = fsm->datagram;

   414

    ec_slave_t *slave = fsm->slave;

   415

    u64 system_time, old_offset, new_offset;

   416

   417

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

   418

        return;

   419

   420

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   421

        fsm->state = ec_fsm_slave_config_state_error;

   422

        EC_ERR("Failed to receive DC times datagram for slave %u: ",

   423

                slave->ring_position);

   424

        ec_datagram_print_state(datagram);

   425

        return;

   426

    }

   427

   428

    if (datagram->working_counter != 1) {

   429

        slave->error_flag = 1;

   430

        fsm->state = ec_fsm_slave_config_state_error;

   431

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

   432

                slave->ring_position);

   433

        ec_datagram_print_wc_error(datagram);

   434

        return;

   435

    }

   436

   437

    system_time = EC_READ_U64(datagram->data);

   438

    old_offset = EC_READ_U64(datagram->data + 16);

   439

    new_offset = slave->master->app_time - system_time + old_offset;

   440

   441

    if (slave->master->debug_level)

   442

        EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, "

   443

                "app_time=%llu, new_offset=%llu\n",

   444

                slave->ring_position, system_time, old_offset,

   445

                slave->master->app_time, new_offset);

   446

   447

    // set DC system time offset and transmission delay

   448

    ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12);

   449

    EC_WRITE_U64(datagram->data, new_offset);

   450

    EC_WRITE_U32(datagram->data + 8, slave->transmission_delay);

   451

    fsm->retries = EC_FSM_RETRIES;

   452

    fsm->state = ec_fsm_slave_config_state_dc_write_offset;

   453

}

   454

   455

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

   456

   457

/** Slave configuration state: DC WRITE OFFSET.

   458

 */

   459

void ec_fsm_slave_config_state_dc_write_offset(

   460

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   461

        )

   462

{

   463

    ec_datagram_t *datagram = fsm->datagram;

   464

    ec_slave_t *slave = fsm->slave;

   465

   466

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

   467

        return;

   468

   469

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   470

        fsm->state = ec_fsm_slave_config_state_error;

   471

        EC_ERR("Failed to receive DC system time offset datagram for"

   472

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

   473

        ec_datagram_print_state(datagram);

   474

        return;

   475

    }

   476

   477

    if (datagram->working_counter != 1) {

   478

        slave->error_flag = 1;

   479

        fsm->state = ec_fsm_slave_config_state_error;

   480

        EC_ERR("Failed to set DC system time offset of slave %u: ",

   481

                slave->ring_position);

   482

        ec_datagram_print_wc_error(datagram);

   483

        return;

   484

    }

   485

   486

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   487

}

   488

   489

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

   490

   491

/** Check for mailbox sync managers to be configured.

   492

 */

   493

void ec_fsm_slave_config_enter_mbox_sync(

   494

        ec_fsm_slave_config_t *fsm /**< slave state machine */

   495

        )

   496

{

   497

    ec_master_t *master = fsm->slave->master;

   498

    ec_slave_t *slave = fsm->slave;

   499

    ec_datagram_t *datagram = fsm->datagram;

   500

    unsigned int i;