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_soe_conf(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    62

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    68

    69

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    72

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    73

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    74

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    75

void ec_fsm_slave_config_enter_soe_conf(ec_fsm_slave_config_t *);

    76

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    77

void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *);

    78

void ec_fsm_slave_config_enter_watchdog(ec_fsm_slave_config_t *);

    79

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    80

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    81

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    82

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    83

    84

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    85

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    86

    87

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    88

    89

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

    90

    91

/** Constructor.

    92

 */

    93

void ec_fsm_slave_config_init(

    94

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

    95

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

    96

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

    97

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

    98

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

    99

        )

   100

{

   101

    ec_sdo_request_init(&fsm->request_copy);

   102

   103

    fsm->datagram = datagram;

   104

    fsm->fsm_change = fsm_change;

   105

    fsm->fsm_coe = fsm_coe;

   106

    fsm->fsm_pdo = fsm_pdo;

   107

}

   108

   109

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

   110

   111

/** Destructor.

   112

 */

   113

void ec_fsm_slave_config_clear(

   114

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

   115

        )

   116

{

   117

    ec_sdo_request_clear(&fsm->request_copy);

   118

    ec_soe_request_clear(&fsm->soe_request_copy);

   119

}

   120

   121

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

   122

   123

/** Start slave configuration state machine.

   124

 */

   125

void ec_fsm_slave_config_start(

   126

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

   127

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

   128

        )

   129

{

   130

    fsm->slave = slave;

   131

    fsm->state = ec_fsm_slave_config_state_start;

   132

}

   133

   134

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

   135

   136

/**

   137

 * \return false, if state machine has terminated

   138

 */

   139

int ec_fsm_slave_config_running(

   140

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

   141

        )

   142

{

   143

    return fsm->state != ec_fsm_slave_config_state_end

   144

        && fsm->state != ec_fsm_slave_config_state_error;

   145

}

   146

   147

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

   148

   149

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

   150

 *

   151

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

   152

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

   153

 *

   154

 * \return false, if state machine has terminated

   155

 */

   156

int ec_fsm_slave_config_exec(

   157

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

   158

        )

   159

{

   160

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

   161

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

   162

        // datagram was not sent or received yet.

   163

        return ec_fsm_slave_config_running(fsm);

   164

    }

   165

   166

    fsm->state(fsm);

   167

    return ec_fsm_slave_config_running(fsm);

   168

}

   169

   170

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

   171

   172

/**

   173

 * \return true, if the state machine terminated gracefully

   174

 */

   175

int ec_fsm_slave_config_success(

   176

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

   177

        )

   178

{

   179

    return fsm->state == ec_fsm_slave_config_state_end;

   180

}

   181

   182

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

   183

 * Slave configuration state machine

   184

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

   185

   186

/** Slave configuration state: START.

   187

 */

   188

void ec_fsm_slave_config_state_start(

   189

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

   190

        )

   191

{

   192

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

   193

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

   194

    }

   195

   196

    ec_fsm_slave_config_enter_init(fsm);

   197

}

   198

   199

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

   200

   201

/** Start state change to INIT.

   202

 */

   203

void ec_fsm_slave_config_enter_init(

   204

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

   205

        )

   206

{

   207

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

   208

    ec_fsm_change_exec(fsm->fsm_change);

   209

    fsm->state = ec_fsm_slave_config_state_init;

   210

}

   211

   212

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

   213

   214

/** Slave configuration state: INIT.

   215

 */

   216

void ec_fsm_slave_config_state_init(

   217

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

   218

        )

   219

{

   220

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

   221

    ec_slave_t *slave = fsm->slave;

   222

    ec_datagram_t *datagram = fsm->datagram;

   223

   224

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

   225

   226

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

   227

        if (!fsm->fsm_change->spontaneous_change)

   228

            slave->error_flag = 1;

   229

        fsm->state = ec_fsm_slave_config_state_error;

   230

        return;

   231

    }

   232

   233

    if (master->debug_level) {

   234

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

   235

    }

   236

   237

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

   238

        ec_fsm_slave_config_enter_clear_sync(fsm);

   239

        return;

   240

    }

   241

   242

    if (master->debug_level)

   243

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

   244

               slave->ring_position);

   245

   246

    // clear FMMU configurations

   247

    ec_datagram_fpwr(datagram, slave->station_address,

   248

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   249

    ec_datagram_zero(datagram);

   250

    fsm->retries = EC_FSM_RETRIES;

   251

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   252

}

   253

   254

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

   255

   256

/** Slave configuration state: CLEAR FMMU.

   257

 */

   258

void ec_fsm_slave_config_state_clear_fmmus(

   259

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

   260

        )

   261

{

   262

    ec_datagram_t *datagram = fsm->datagram;

   263

   264

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

   265

        return;

   266

   267

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   268

        fsm->state = ec_fsm_slave_config_state_error;

   269

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

   270

               fsm->slave->ring_position);

   271

        return;

   272

    }

   273

   274

    if (datagram->working_counter != 1) {

   275

        fsm->slave->error_flag = 1;

   276

        fsm->state = ec_fsm_slave_config_state_error;

   277

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

   278

               fsm->slave->ring_position);

   279

        ec_datagram_print_wc_error(datagram);

   280

        return;

   281

    }

   282

   283

    ec_fsm_slave_config_enter_clear_sync(fsm);

   284

}

   285

   286

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

   287

   288

/** Clear the sync manager configurations.

   289

 */

   290

void ec_fsm_slave_config_enter_clear_sync(

   291

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

   292

        )

   293

{

   294

    ec_slave_t *slave = fsm->slave;

   295

    ec_datagram_t *datagram = fsm->datagram;

   296

    size_t sync_size;

   297

   298

    if (!slave->base_sync_count) {

   299

        // no sync managers

   300

        ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   301

        return;

   302

    }

   303

   304

    if (slave->master->debug_level)

   305

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

   306

                slave->ring_position);

   307

   308

    sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count;

   309

   310

    // clear sync manager configurations

   311

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

   312

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

   313

    fsm->retries = EC_FSM_RETRIES;

   314

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   315

}

   316

   317

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

   318

   319

/** Slave configuration state: CLEAR SYNC.

   320

 */

   321

void ec_fsm_slave_config_state_clear_sync(

   322

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

   323

        )

   324

{

   325

    ec_datagram_t *datagram = fsm->datagram;

   326

   327

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

   328

        return;

   329

   330

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   331

        fsm->state = ec_fsm_slave_config_state_error;

   332

        EC_ERR("Failed receive sync manager clearing datagram"

   333

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

   334

        return;

   335

    }

   336

   337

    if (datagram->working_counter != 1) {

   338

        fsm->slave->error_flag = 1;

   339

        fsm->state = ec_fsm_slave_config_state_error;

   340

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

   341

               fsm->slave->ring_position);

   342

        ec_datagram_print_wc_error(datagram);

   343

        return;

   344

    }

   345

   346

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   347

}

   348

   349

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

   350

   351

/** Clear the DC assignment.

   352

 */

   353

void ec_fsm_slave_config_enter_dc_clear_assign(

   354

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

   355

        )

   356

{

   357

    ec_slave_t *slave = fsm->slave;

   358

    ec_datagram_t *datagram = fsm->datagram;

   359

   360

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

   361

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   362

        return;

   363

    }

   364

   365

    if (slave->master->debug_level)

   366

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

   367

                slave->ring_position);

   368

   369

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

   370

    ec_datagram_zero(datagram);

   371

    fsm->retries = EC_FSM_RETRIES;

   372

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   373

}

   374

   375

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

   376

   377

/** Slave configuration state: CLEAR DC ASSIGN.

   378

 */

   379

void ec_fsm_slave_config_state_dc_clear_assign(

   380

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

   381

        )

   382

{

   383

    ec_datagram_t *datagram = fsm->datagram;

   384

   385

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

   386

        return;

   387

   388

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   389

        fsm->state = ec_fsm_slave_config_state_error;

   390

        EC_ERR("Failed receive DC assignment clearing datagram"

   391

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

   392

        return;

   393

    }

   394

   395

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

   396

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

   397

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

   398

               fsm->slave->ring_position);

   399

        ec_datagram_print_wc_error(datagram);

   400

    }

   401

   402

    // read DC system time and system time offset

   403

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

   404

    fsm->retries = EC_FSM_RETRIES;

   405

    fsm->state = ec_fsm_slave_config_state_dc_read_offset;

   406

}

   407

   408

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

   409

   410

/** Slave configuration state: DC READ OFFSET.

   411

 */

   412

void ec_fsm_slave_config_state_dc_read_offset(

   413

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

   414

        )

   415

{

   416

    ec_datagram_t *datagram = fsm->datagram;

   417

    ec_slave_t *slave = fsm->slave;

   418

    u64 system_time, old_offset, new_offset;

   419

   420

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

   421

        return;

   422

   423

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   424

        fsm->state = ec_fsm_slave_config_state_error;

   425

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

   426

                slave->ring_position);

   427

        ec_datagram_print_state(datagram);

   428

        return;

   429

    }

   430

   431

    if (datagram->working_counter != 1) {

   432

        slave->error_flag = 1;

   433

        fsm->state = ec_fsm_slave_config_state_error;

   434

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

   435

                slave->ring_position);

   436

        ec_datagram_print_wc_error(datagram);

   437

        return;

   438

    }

   439

   440

    system_time = EC_READ_U64(datagram->data);

   441

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

   442

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

   443

   444

    if (slave->master->debug_level)

   445

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

   446

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

   447

                slave->ring_position, system_time, old_offset,

   448

                slave->master->app_time, new_offset);

   449

   450

    // set DC system time offset and transmission delay

   451

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

   452

    EC_WRITE_U64(datagram->data, new_offset);

   453

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

   454

    fsm->retries = EC_FSM_RETRIES;

   455

    fsm->state = ec_fsm_slave_config_state_dc_write_offset;

   456

}

   457

   458

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

   459

   460

/** Slave configuration state: DC WRITE OFFSET.

   461

 */

   462

void ec_fsm_slave_config_state_dc_write_offset(

   463

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

   464

        )

   465

{

   466

    ec_datagram_t *datagram = fsm->datagram;

   467

    ec_slave_t *slave = fsm->slave;

   468

   469

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

   470

        return;

   471

   472

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   473

        fsm->state = ec_fsm_slave_config_state_error;

   474

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

   475

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

   476

        ec_datagram_print_state(datagram);

   477

        return;

   478

    }

   479

   480

    if (datagram->working_counter != 1) {

   481

        slave->error_flag = 1;

   482

        fsm->state = ec_fsm_slave_config_state_error;

   483

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

   484

                slave->ring_position);

   485

        ec_datagram_print_wc_error(datagram);

   486

        return;

   487

    }

   488

   489

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   490

}

   491

   492

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

   493

   494

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

   495

 */

   496

void ec_fsm_slave_config_enter_mbox_sync(

   497

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

   498

        )

   499

{