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 "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "slave_config.h"

    41

#include "fsm_slave_config.h"

    42

    43

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

    44

    45

void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    46

void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    47

void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    48

void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *);

    49

void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *);

    50

void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *);

    51

void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *);

    52

void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    53

void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *);

    54

void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    55

void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    56

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    57

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    58

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    62

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    63

    64

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    65

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    66

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    68

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    72

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    73

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    74

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    75

    76

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    77

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    78

    79

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    80

    81

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

    82

    83

/** Constructor.

    84

 */

    85

void ec_fsm_slave_config_init(

    86

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

    87

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

    88

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

    89

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

    90

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

    91

        )

    92

{

    93

    ec_sdo_request_init(&fsm->request_copy);

    94

    95

    fsm->datagram = datagram;

    96

    fsm->fsm_change = fsm_change;

    97

    fsm->fsm_coe = fsm_coe;

    98

    fsm->fsm_pdo = fsm_pdo;

    99

}

   100

   101

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

   102

   103

/** Destructor.

   104

 */

   105

void ec_fsm_slave_config_clear(

   106

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

   107

        )

   108

{

   109

    ec_sdo_request_clear(&fsm->request_copy);

   110

}

   111

   112

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

   113

   114

/** Start slave configuration state machine.

   115

 */

   116

void ec_fsm_slave_config_start(

   117

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

   118

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

   119

        )

   120

{

   121

    fsm->slave = slave;

   122

    fsm->state = ec_fsm_slave_config_state_start;

   123

}

   124

   125

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

   126

   127

/**

   128

 * \return false, if state machine has terminated

   129

 */

   130

int ec_fsm_slave_config_running(

   131

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

   132

        )

   133

{

   134

    return fsm->state != ec_fsm_slave_config_state_end

   135

        && fsm->state != ec_fsm_slave_config_state_error;

   136

}

   137

   138

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

   139

   140

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

   141

 *

   142

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

   143

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

   144

 *

   145

 * \return false, if state machine has terminated

   146

 */

   147

int ec_fsm_slave_config_exec(

   148

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

   149

        )

   150

{

   151

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

   152

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

   153

        // datagram was not sent or received yet.

   154

        return ec_fsm_slave_config_running(fsm);

   155

    }

   156

   157

    fsm->state(fsm);

   158

    return ec_fsm_slave_config_running(fsm);

   159

}

   160

   161

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

   162

   163

/**

   164

 * \return true, if the state machine terminated gracefully

   165

 */

   166

int ec_fsm_slave_config_success(

   167

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

   168

        )

   169

{

   170

    return fsm->state == ec_fsm_slave_config_state_end;

   171

}

   172

   173

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

   174

 * Slave configuration state machine

   175

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

   176

   177

/** Slave configuration state: START.

   178

 */

   179

void ec_fsm_slave_config_state_start(

   180

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

   181

        )

   182

{

   183

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

   184

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

   185

    }

   186

   187

    ec_fsm_slave_config_enter_init(fsm);

   188

}

   189

   190

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

   191

   192

/** Start state change to INIT.

   193

 */

   194

void ec_fsm_slave_config_enter_init(

   195

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

   196

        )

   197

{

   198

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

   199

    ec_fsm_change_exec(fsm->fsm_change);

   200

    fsm->state = ec_fsm_slave_config_state_init;

   201

}

   202

   203

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

   204

   205

/** Slave configuration state: INIT.

   206

 */

   207

void ec_fsm_slave_config_state_init(

   208

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

   209

        )

   210

{

   211

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

   212

    ec_slave_t *slave = fsm->slave;

   213

    ec_datagram_t *datagram = fsm->datagram;

   214

   215

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

   216

   217

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

   218

        if (!fsm->fsm_change->spontaneous_change)

   219

            slave->error_flag = 1;

   220

        fsm->state = ec_fsm_slave_config_state_error;

   221

        return;

   222

    }

   223

   224

    if (master->debug_level) {

   225

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

   226

    }

   227

   228

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

   229

        ec_fsm_slave_config_enter_clear_sync(fsm);

   230

        return;

   231

    }

   232

   233

    if (master->debug_level)

   234

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

   235

               slave->ring_position);

   236

   237

    // clear FMMU configurations

   238

    ec_datagram_fpwr(datagram, slave->station_address,

   239

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   240

    ec_datagram_zero(datagram);

   241

    fsm->retries = EC_FSM_RETRIES;

   242

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   243

}

   244

   245

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

   246

   247

/** Slave configuration state: CLEAR FMMU.

   248

 */

   249

void ec_fsm_slave_config_state_clear_fmmus(

   250

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

   251

        )

   252

{

   253

    ec_datagram_t *datagram = fsm->datagram;

   254

   255

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

   256

        return;

   257

   258

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   259

        fsm->state = ec_fsm_slave_config_state_error;

   260

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

   261

               fsm->slave->ring_position);

   262

        return;

   263

    }

   264

   265

    if (datagram->working_counter != 1) {

   266

        fsm->slave->error_flag = 1;

   267

        fsm->state = ec_fsm_slave_config_state_error;

   268

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

   269

               fsm->slave->ring_position);

   270

        ec_datagram_print_wc_error(datagram);

   271

        return;

   272

    }

   273

   274

    ec_fsm_slave_config_enter_clear_sync(fsm);

   275

}

   276

   277

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

   278

   279

/** Clear the sync manager configurations.

   280

 */

   281

void ec_fsm_slave_config_enter_clear_sync(

   282

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

   283

        )

   284

{

   285

    ec_slave_t *slave = fsm->slave;

   286

    ec_datagram_t *datagram = fsm->datagram;

   287

    size_t sync_size;

   288

   289

    if (!slave->sii.sync_count) { // FIXME use base_sync_count?

   290

        // no sync managers

   291

        ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   292

        return;

   293

    }

   294

   295

    if (slave->master->debug_level)

   296

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

   297

                slave->ring_position);

   298

   299

    sync_size = EC_SYNC_PAGE_SIZE * slave->sii.sync_count;

   300

   301

    // clear sync manager configurations

   302

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

   303

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

   304

    fsm->retries = EC_FSM_RETRIES;

   305

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   306

}

   307

   308

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

   309

   310

/** Slave configuration state: CLEAR SYNC.

   311

 */

   312

void ec_fsm_slave_config_state_clear_sync(

   313

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

   314

        )

   315

{

   316

    ec_datagram_t *datagram = fsm->datagram;

   317

   318

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

   319

        return;

   320

   321

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   322

        fsm->state = ec_fsm_slave_config_state_error;

   323

        EC_ERR("Failed receive sync manager clearing datagram"

   324

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

   325

        return;

   326

    }

   327

   328

    if (datagram->working_counter != 1) {

   329

        fsm->slave->error_flag = 1;

   330

        fsm->state = ec_fsm_slave_config_state_error;

   331

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

   332

               fsm->slave->ring_position);

   333

        ec_datagram_print_wc_error(datagram);

   334

        return;

   335

    }

   336

   337

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   338

}

   339

   340

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

   341

   342

/** Clear the DC assignment.

   343

 */

   344

void ec_fsm_slave_config_enter_dc_clear_assign(

   345

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

   346

        )

   347

{

   348

    ec_slave_t *slave = fsm->slave;

   349

    ec_datagram_t *datagram = fsm->datagram;

   350

   351

    if (!slave->base_dc_supported) {

   352

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   353

        return;

   354

    }

   355

   356

    if (slave->master->debug_level)

   357

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

   358

                slave->ring_position);

   359

   360

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

   361

    ec_datagram_zero(datagram);

   362

    fsm->retries = EC_FSM_RETRIES;

   363

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   364

}

   365

   366

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

   367

   368

/** Slave configuration state: CLEAR DC ASSIGN.

   369

 */

   370

void ec_fsm_slave_config_state_dc_clear_assign(

   371

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

   372

        )

   373

{

   374

    ec_datagram_t *datagram = fsm->datagram;

   375

   376

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

   377

        return;

   378

   379

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   380

        fsm->state = ec_fsm_slave_config_state_error;

   381

        EC_ERR("Failed receive DC assignment clearing datagram"

   382

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

   383

        return;

   384

    }

   385

   386

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

   387

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

   388

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

   389

               fsm->slave->ring_position);

   390

        ec_datagram_print_wc_error(datagram);

   391

    }

   392

   393

    // read DC system time and system time offset

   394

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

   395

    fsm->retries = EC_FSM_RETRIES;

   396

    fsm->state = ec_fsm_slave_config_state_dc_read_offset;

   397

}

   398

   399

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

   400

   401

/** Slave configuration state: DC READ OFFSET.

   402

 */

   403

void ec_fsm_slave_config_state_dc_read_offset(

   404

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

   405

        )

   406

{

   407

    ec_datagram_t *datagram = fsm->datagram;

   408

    ec_slave_t *slave = fsm->slave;

   409

    u64 system_time, old_offset, new_offset;

   410

   411

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

   412

        return;

   413

   414

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   415

        fsm->state = ec_fsm_slave_config_state_error;

   416

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

   417

                " (datagram state %u).\n",

   418

                slave->ring_position, datagram->state);

   419

        return;

   420

    }

   421

   422

    if (datagram->working_counter != 1) {

   423

        slave->error_flag = 1;

   424

        fsm->state = ec_fsm_slave_config_state_error;

   425

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

   426

                slave->ring_position);

   427

        ec_datagram_print_wc_error(datagram);

   428

        return;

   429

    }

   430

   431

    system_time = EC_READ_U64(datagram->data);

   432

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

   433

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

   434

   435

    if (slave->master->debug_level)

   436

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

   437

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

   438

                slave->ring_position, system_time, old_offset,

   439

                slave->master->app_time, new_offset);

   440

   441

    // set DC system time offset

   442

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

   443

    EC_WRITE_U64(datagram->data, new_offset);

   444

    fsm->retries = EC_FSM_RETRIES;

   445

    fsm->state = ec_fsm_slave_config_state_dc_write_offset;

   446

}

   447

   448

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

   449

   450

/** Slave configuration state: DC WRITE OFFSET.

   451

 */

   452

void ec_fsm_slave_config_state_dc_write_offset(

   453

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

   454

        )

   455

{

   456

    ec_datagram_t *datagram = fsm->datagram;

   457

    ec_slave_t *slave = fsm->slave;

   458

   459

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

   460

        return;

   461

   462

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   463

        fsm->state = ec_fsm_slave_config_state_error;

   464

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

   465

                " (datagram state %u).\n",

   466

                slave->ring_position, datagram->state);

   467

        return;

   468

    }

   469

   470

    if (datagram->working_counter != 1) {

   471

        slave->error_flag = 1;

   472

        fsm->state = ec_fsm_slave_config_state_error;

   473

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

   474

                slave->ring_position);

   475

        ec_datagram_print_wc_error(datagram);

   476

        return;

   477

    }

   478

   479

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   480

}

   481

   482

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

   483

   484

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

   485

 */

   486

void ec_fsm_slave_config_enter_mbox_sync(

   487

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

   488

        )

   489

{

   490

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

   491

    ec_slave_t *slave = fsm->slave;

   492

    ec_datagram_t *datagram = fsm->datagram;

   493

    unsigned int i;

   494

   495

    // slave is now in INIT

   496

    if (slave->current_state == slave->requested_state) {

   497

        fsm->state = ec_fsm_slave_config_state_end; // successful

   498

        if (master->debug_level) {

   499

            EC_DBG("Finished configuration of slave %u.\n",

   500

                   slave->ring_position);

   501

        }