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

    58

void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    59

void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    60

void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *);

    61

void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *);

    62

void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *);

    63

void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    64

void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    65

    66

void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *);

    67

void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *);

    68

void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *);

    69

void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    70

void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *);

    71

void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    72

void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *);

    73

void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    74

void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    75

void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *);

    76

void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    77

    78

void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    79

void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    80

    81

void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *);

    82

    83

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

    84

    85

/** Constructor.

    86

 */

    87

void ec_fsm_slave_config_init(

    88

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

    89

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

    90

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

    91

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

    92

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

    93

        )

    94

{

    95

    ec_sdo_request_init(&fsm->request_copy);

    96

    97

    fsm->datagram = datagram;

    98

    fsm->fsm_change = fsm_change;

    99

    fsm->fsm_coe = fsm_coe;

   100

    fsm->fsm_pdo = fsm_pdo;

   101

}

   102

   103

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

   104

   105

/** Destructor.

   106

 */

   107

void ec_fsm_slave_config_clear(

   108

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

   109

        )

   110

{

   111

    ec_sdo_request_clear(&fsm->request_copy);

   112

}

   113

   114

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

   115

   116

/** Start slave configuration state machine.

   117

 */

   118

void ec_fsm_slave_config_start(

   119

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

   120

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

   121

        )

   122

{

   123

    fsm->slave = slave;

   124

    fsm->state = ec_fsm_slave_config_state_start;

   125

}

   126

   127

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

   128

   129

/**

   130

 * \return false, if state machine has terminated

   131

 */

   132

int ec_fsm_slave_config_running(

   133

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

   134

        )

   135

{

   136

    return fsm->state != ec_fsm_slave_config_state_end

   137

        && fsm->state != ec_fsm_slave_config_state_error;

   138

}

   139

   140

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

   141

   142

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

   143

 *

   144

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

   145

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

   146

 *

   147

 * \return false, if state machine has terminated

   148

 */

   149

int ec_fsm_slave_config_exec(

   150

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

   151

        )

   152

{

   153

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

   154

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

   155

        // datagram was not sent or received yet.

   156

        return ec_fsm_slave_config_running(fsm);

   157

    }

   158

   159

    fsm->state(fsm);

   160

    return ec_fsm_slave_config_running(fsm);

   161

}

   162

   163

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

   164

   165

/**

   166

 * \return true, if the state machine terminated gracefully

   167

 */

   168

int ec_fsm_slave_config_success(

   169

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

   170

        )

   171

{

   172

    return fsm->state == ec_fsm_slave_config_state_end;

   173

}

   174

   175

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

   176

 * Slave configuration state machine

   177

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

   178

   179

/** Slave configuration state: START.

   180

 */

   181

void ec_fsm_slave_config_state_start(

   182

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

   183

        )

   184

{

   185

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

   186

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

   187

    }

   188

   189

    ec_fsm_slave_config_enter_init(fsm);

   190

}

   191

   192

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

   193

   194

/** Start state change to INIT.

   195

 */

   196

void ec_fsm_slave_config_enter_init(

   197

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

   198

        )

   199

{

   200

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

   201

    ec_fsm_change_exec(fsm->fsm_change);

   202

    fsm->state = ec_fsm_slave_config_state_init;

   203

}

   204

   205

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

   206

   207

/** Slave configuration state: INIT.

   208

 */

   209

void ec_fsm_slave_config_state_init(

   210

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

   211

        )

   212

{

   213

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

   214

    ec_slave_t *slave = fsm->slave;

   215

    ec_datagram_t *datagram = fsm->datagram;

   216

   217

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

   218

   219

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

   220

        if (!fsm->fsm_change->spontaneous_change)

   221

            slave->error_flag = 1;

   222

        fsm->state = ec_fsm_slave_config_state_error;

   223

        return;

   224

    }

   225

   226

    if (master->debug_level) {

   227

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

   228

    }

   229

   230

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

   231

        ec_fsm_slave_config_enter_clear_sync(fsm);

   232

        return;

   233

    }

   234

   235

    if (master->debug_level)

   236

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

   237

               slave->ring_position);

   238

   239

    // clear FMMU configurations

   240

    ec_datagram_fpwr(datagram, slave->station_address,

   241

            0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   242

    ec_datagram_zero(datagram);

   243

    fsm->retries = EC_FSM_RETRIES;

   244

    fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   245

}

   246

   247

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

   248

   249

/** Slave configuration state: CLEAR FMMU.

   250

 */

   251

void ec_fsm_slave_config_state_clear_fmmus(

   252

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

   253

        )

   254

{

   255

    ec_datagram_t *datagram = fsm->datagram;

   256

   257

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

   258

        return;

   259

   260

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   261

        fsm->state = ec_fsm_slave_config_state_error;

   262

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

   263

               fsm->slave->ring_position);

   264

        return;

   265

    }

   266

   267

    if (datagram->working_counter != 1) {

   268

        fsm->slave->error_flag = 1;

   269

        fsm->state = ec_fsm_slave_config_state_error;

   270

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

   271

               fsm->slave->ring_position);

   272

        ec_datagram_print_wc_error(datagram);

   273

        return;

   274

    }

   275

   276

    ec_fsm_slave_config_enter_clear_sync(fsm);

   277

}

   278

   279

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

   280

   281

/** Clear the sync manager configurations.

   282

 */

   283

void ec_fsm_slave_config_enter_clear_sync(

   284

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

   285

        )

   286

{

   287

    ec_slave_t *slave = fsm->slave;

   288

    ec_datagram_t *datagram = fsm->datagram;

   289

    size_t sync_size;

   290

   291

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

   292

        // no sync managers

   293

        ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   294

        return;

   295

    }

   296

   297

    if (slave->master->debug_level)

   298

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

   299

                slave->ring_position);

   300

   301

    sync_size = EC_SYNC_PAGE_SIZE * slave->sii.sync_count;

   302

   303

    // clear sync manager configurations

   304

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

   305

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

   306

    fsm->retries = EC_FSM_RETRIES;

   307

    fsm->state = ec_fsm_slave_config_state_clear_sync;

   308

}

   309

   310

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

   311

   312

/** Slave configuration state: CLEAR SYNC.

   313

 */

   314

void ec_fsm_slave_config_state_clear_sync(

   315

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

   316

        )

   317

{

   318

    ec_datagram_t *datagram = fsm->datagram;

   319

   320

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

   321

        return;

   322

   323

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   324

        fsm->state = ec_fsm_slave_config_state_error;

   325

        EC_ERR("Failed receive sync manager clearing datagram"

   326

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

   327

        return;

   328

    }

   329

   330

    if (datagram->working_counter != 1) {

   331

        fsm->slave->error_flag = 1;

   332

        fsm->state = ec_fsm_slave_config_state_error;

   333

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

   334

               fsm->slave->ring_position);

   335

        ec_datagram_print_wc_error(datagram);

   336

        return;

   337

    }

   338

   339

    ec_fsm_slave_config_enter_dc_clear_assign(fsm);

   340

}

   341

   342

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

   343

   344

/** Clear the DC assignment.

   345

 */

   346

void ec_fsm_slave_config_enter_dc_clear_assign(

   347

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

   348

        )

   349

{

   350

    ec_slave_t *slave = fsm->slave;

   351

    ec_datagram_t *datagram = fsm->datagram;

   352

   353

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

   354

        ec_fsm_slave_config_enter_mbox_sync(fsm);

   355

        return;

   356

    }

   357

   358

    if (slave->master->debug_level)

   359

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

   360

                slave->ring_position);

   361

   362

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

   363

    ec_datagram_zero(datagram);

   364

    fsm->retries = EC_FSM_RETRIES;

   365

    fsm->state = ec_fsm_slave_config_state_dc_clear_assign;

   366

}

   367

   368

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

   369

   370

/** Slave configuration state: CLEAR DC ASSIGN.

   371

 */

   372

void ec_fsm_slave_config_state_dc_clear_assign(

   373

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

   374

        )

   375

{

   376

    ec_datagram_t *datagram = fsm->datagram;

   377

   378

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

   379

        return;

   380

   381

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   382

        fsm->state = ec_fsm_slave_config_state_error;

   383

        EC_ERR("Failed receive DC assignment clearing datagram"

   384

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

   385

        return;

   386

    }

   387

   388

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

   389

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

   390

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

   391

               fsm->slave->ring_position);

   392

        ec_datagram_print_wc_error(datagram);

   393

    }

   394

   395

    // read DC system time and system time offset

   396

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

   397

    fsm->retries = EC_FSM_RETRIES;

   398

    fsm->state = ec_fsm_slave_config_state_dc_read_offset;

   399

}

   400

   401

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

   402

   403

/** Slave configuration state: DC READ OFFSET.

   404

 */

   405

void ec_fsm_slave_config_state_dc_read_offset(

   406

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

   407

        )

   408

{

   409

    ec_datagram_t *datagram = fsm->datagram;

   410

    ec_slave_t *slave = fsm->slave;

   411

    u64 system_time, old_offset, new_offset;

   412

   413

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

   414

        return;

   415

   416

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   417

        fsm->state = ec_fsm_slave_config_state_error;

   418

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

   419

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

   420

                slave->ring_position, datagram->state);

   421

        return;

   422

    }

   423

   424

    if (datagram->working_counter != 1) {

   425

        slave->error_flag = 1;

   426

        fsm->state = ec_fsm_slave_config_state_error;

   427

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

   428

                slave->ring_position);

   429

        ec_datagram_print_wc_error(datagram);

   430

        return;

   431

    }

   432

   433

    system_time = EC_READ_U64(datagram->data);

   434

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

   435

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

   436

   437

    if (slave->master->debug_level)

   438

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

   439

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

   440

                slave->ring_position, system_time, old_offset,

   441

                slave->master->app_time, new_offset);

   442

   443

    // set DC system time offset and transmission delay

   444

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

   445

    EC_WRITE_U64(datagram->data, new_offset);

   446

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

   447

    fsm->retries = EC_FSM_RETRIES;

   448

    fsm->state = ec_fsm_slave_config_state_dc_write_offset;

   449

}

   450

   451

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

   452

   453

/** Slave configuration state: DC WRITE OFFSET.

   454

 */

   455

void ec_fsm_slave_config_state_dc_write_offset(

   456

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

   457

        )

   458

{

   459

    ec_datagram_t *datagram = fsm->datagram;

   460

    ec_slave_t *slave = fsm->slave;

   461

   462

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

   463

        return;

   464

   465

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   466

        fsm->state = ec_fsm_slave_config_state_error;

   467

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

   468

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

   469

                slave->ring_position, datagram->state);

   470

        return;

   471

    }

   472

   473

    if (datagram->working_counter != 1) {

   474

        slave->error_flag = 1;

   475

        fsm->state = ec_fsm_slave_config_state_error;

   476

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

   477

                slave->ring_position);

   478

        ec_datagram_print_wc_error(datagram);

   479

        return;

   480

    }

   481

   482

    ec_fsm_slave_config_enter_mbox_sync(fsm);

   483

}

   484

   485

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

   486

   487

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

   488

 */

   489

void ec_fsm_slave_config_enter_mbox_sync(

   490

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

   491

        )

   492

{

   493

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

   494

    ec_slave_t *slave = fsm->slave;

   495

    ec_datagram_t *datagram = fsm->datagram;

   496

    unsigned int i;

   497

   498

    // slave is now in INIT

   499

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

   500

        fsm->state = ec_fsm_slave_config_state_end; // successful

   501