1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2012  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

 * EtherCAT slave (SDO) state machine.

    32

 */

    33

    34

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

    35

    36

#include "globals.h"

    37

#include "master.h"

    38

#include "mailbox.h"

    39

#include "slave_config.h"

    40

    41

#include "fsm_slave.h"

    42

    43

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

    44

    45

void ec_fsm_slave_state_idle(ec_fsm_slave_t *, ec_datagram_t *);

    46

void ec_fsm_slave_state_ready(ec_fsm_slave_t *, ec_datagram_t *);

    47

int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *, ec_datagram_t *);

    48

void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *, ec_datagram_t *);

    49

int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *, ec_datagram_t *);

    50

void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *, ec_datagram_t *);

    51

int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);

    52

void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);

    53

int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);

    54

void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *);

    55

    56

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

    57

    58

/** Constructor.

    59

 */

    60

void ec_fsm_slave_init(

    61

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

    62

        ec_slave_t *slave /**< EtherCAT slave. */

    63

        )

    64

{

    65

    fsm->slave = slave;

    66

    INIT_LIST_HEAD(&fsm->list); // mark as unlisted

    67

    68

    fsm->state = ec_fsm_slave_state_idle;

    69

    fsm->datagram = NULL;

    70

    fsm->sdo_request = NULL;

    71

    fsm->reg_request = NULL;

    72

    fsm->foe_request = NULL;

    73

    fsm->soe_request = NULL;

    74

    75

    // Init sub-state-machines

    76

    ec_fsm_coe_init(&fsm->fsm_coe);

    77

    ec_fsm_foe_init(&fsm->fsm_foe);

    78

    ec_fsm_soe_init(&fsm->fsm_soe);

    79

}

    80

    81

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

    82

    83

/** Destructor.

    84

 */

    85

void ec_fsm_slave_clear(

    86

        ec_fsm_slave_t *fsm /**< Master state machine. */

    87

        )

    88

{

    89

    // signal requests that are currently in operation

    90

    91

    if (fsm->sdo_request) {

    92

        fsm->sdo_request->state = EC_INT_REQUEST_FAILURE;

    93

        wake_up_all(&fsm->slave->master->request_queue);

    94

    }

    95

    96

    if (fsm->reg_request) {

    97

        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;

    98

        wake_up_all(&fsm->slave->master->request_queue);

    99

    }

   100

   101

    if (fsm->foe_request) {

   102

        fsm->foe_request->state = EC_INT_REQUEST_FAILURE;

   103

        wake_up_all(&fsm->slave->master->request_queue);

   104

    }

   105

   106

    if (fsm->soe_request) {

   107

        fsm->soe_request->state = EC_INT_REQUEST_FAILURE;

   108

        wake_up_all(&fsm->slave->master->request_queue);

   109

    }

   110

   111

    // clear sub-state machines

   112

    ec_fsm_coe_clear(&fsm->fsm_coe);

   113

    ec_fsm_foe_clear(&fsm->fsm_foe);

   114

    ec_fsm_soe_clear(&fsm->fsm_soe);

   115

}

   116

   117

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

   118

   119

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

   120

 *

   121

 * \return 1 if \a datagram was used, else 0.

   122

 */

   123

int ec_fsm_slave_exec(

   124

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   125

        ec_datagram_t *datagram /**< New datagram to use. */

   126

        )

   127

{

   128

    int datagram_used;

   129

   130

    fsm->state(fsm, datagram);

   131

   132

    datagram_used = fsm->state != ec_fsm_slave_state_idle &&

   133

        fsm->state != ec_fsm_slave_state_ready;

   134

   135

    if (datagram_used) {

   136

        fsm->datagram = datagram;

   137

    } else {

   138

        fsm->datagram = NULL;

   139

    }

   140

   141

    return datagram_used;

   142

}

   143

   144

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

   145

   146

/** Sets the current state of the state machine to READY

   147

 */

   148

void ec_fsm_slave_set_ready(

   149

        ec_fsm_slave_t *fsm /**< Slave state machine. */

   150

        )

   151

{

   152

    if (fsm->state == ec_fsm_slave_state_idle) {

   153

        EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n");

   154

        fsm->state = ec_fsm_slave_state_ready;

   155

    }

   156

}

   157

   158

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

   159

   160

/** Returns, if the FSM is currently not busy and ready to execute.

   161

 *

   162

 * \return Non-zero if ready.

   163

 */

   164

int ec_fsm_slave_is_ready(

   165

        const ec_fsm_slave_t *fsm /**< Slave state machine. */

   166

        )

   167

{

   168

    return fsm->state == ec_fsm_slave_state_ready;

   169

}

   170

   171

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

   172

 * Slave state machine

   173

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

   174

   175

/** Slave state: IDLE.

   176

 */

   177

void ec_fsm_slave_state_idle(

   178

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   179

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

   180

        )

   181

{

   182

    // do nothing

   183

}

   184

   185

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

   186

   187

/** Slave state: READY.

   188

 */

   189

void ec_fsm_slave_state_ready(

   190

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   191

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

   192

        )

   193

{

   194

    // Check for pending external SDO requests

   195

    if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {

   196

        return;

   197

    }

   198

   199

    // Check for pending external register requests

   200

    if (ec_fsm_slave_action_process_reg(fsm, datagram)) {

   201

        return;

   202

    }

   203

   204

    // Check for pending FoE requests

   205

    if (ec_fsm_slave_action_process_foe(fsm, datagram)) {

   206

        return;

   207

    }

   208

   209

    // Check for pending SoE requests

   210

    if (ec_fsm_slave_action_process_soe(fsm, datagram)) {

   211

        return;

   212

    }

   213

}

   214

   215

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

   216

   217

/** Check for pending SDO requests and process one.

   218

 *

   219

 * \return non-zero, if an SDO request is processed.

   220

 */

   221

int ec_fsm_slave_action_process_sdo(

   222

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   223

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

   224

        )

   225

{

   226

    ec_slave_t *slave = fsm->slave;

   227

    ec_sdo_request_t *request;

   228

   229

    if (list_empty(&slave->sdo_requests)) {

   230

        return 0;

   231

    }

   232

   233

    // take the first request to be processed

   234

    request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list);

   235

    list_del_init(&request->list); // dequeue

   236

   237

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   238

        EC_SLAVE_WARN(slave, "Aborting SDO request,"

   239

                " slave has error flag set.\n");

   240

        request->state = EC_INT_REQUEST_FAILURE;

   241

        wake_up_all(&slave->master->request_queue);

   242

        fsm->state = ec_fsm_slave_state_idle;

   243

        return 1;

   244

    }

   245

   246

    if (slave->current_state == EC_SLAVE_STATE_INIT) {

   247

        EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");

   248

        request->state = EC_INT_REQUEST_FAILURE;

   249

        wake_up_all(&slave->master->request_queue);

   250

        fsm->state = ec_fsm_slave_state_idle;

   251

        return 1;

   252

    }

   253

   254

    fsm->sdo_request = request;

   255

    request->state = EC_INT_REQUEST_BUSY;

   256

   257

    // Found pending SDO request. Execute it!

   258

    EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");

   259

   260

    // Start SDO transfer

   261

    fsm->state = ec_fsm_slave_state_sdo_request;

   262

    ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);

   263

    ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately

   264

    return 1;

   265

}

   266

   267

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

   268

   269

/** Slave state: SDO_REQUEST.

   270

 */

   271

void ec_fsm_slave_state_sdo_request(

   272

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   273

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

   274

        )

   275

{

   276

    ec_slave_t *slave = fsm->slave;

   277

    ec_sdo_request_t *request = fsm->sdo_request;

   278

   279

    if (ec_fsm_coe_exec(&fsm->fsm_coe, datagram)) {

   280

        return;

   281

    }

   282

   283

    if (!ec_fsm_coe_success(&fsm->fsm_coe)) {

   284

        EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");

   285

        request->state = EC_INT_REQUEST_FAILURE;

   286

        wake_up_all(&slave->master->request_queue);

   287

        fsm->sdo_request = NULL;

   288

        fsm->state = ec_fsm_slave_state_ready;

   289

        return;

   290

    }

   291

   292

    EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n");

   293

   294

    // SDO request finished

   295

    request->state = EC_INT_REQUEST_SUCCESS;

   296

    wake_up_all(&slave->master->request_queue);

   297

    fsm->sdo_request = NULL;

   298

    fsm->state = ec_fsm_slave_state_ready;

   299

}

   300

   301

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

   302

   303

/** Check for pending register requests and process one.

   304

 *

   305

 * \return non-zero, if a register request is processed.

   306

 */

   307

int ec_fsm_slave_action_process_reg(

   308

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   309

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

   310

        )

   311

{

   312

    ec_slave_t *slave = fsm->slave;

   313

    ec_reg_request_t *reg;

   314

   315

    fsm->reg_request = NULL;

   316

   317

    if (slave->config) {

   318

        // search the first internal register request to be processed

   319

        list_for_each_entry(reg, &slave->config->reg_requests, list) {

   320

            if (reg->state == EC_INT_REQUEST_QUEUED) {

   321

                fsm->reg_request = reg;

   322

                break;

   323

            }

   324

        }

   325

    }

   326

   327

    if (!fsm->reg_request && !list_empty(&slave->reg_requests)) {

   328

        // take the first external request to be processed

   329

        fsm->reg_request =

   330

            list_entry(slave->reg_requests.next, ec_reg_request_t, list);

   331

        list_del_init(&fsm->reg_request->list); // dequeue

   332

    }

   333

   334

    if (!fsm->reg_request) { // no register request to process

   335

        return 0;

   336

    }

   337

   338

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   339

        EC_SLAVE_WARN(slave, "Aborting register request,"

   340

                " slave has error flag set.\n");

   341

        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;

   342

        wake_up_all(&slave->master->request_queue);

   343

        fsm->reg_request = NULL;

   344

        fsm->state = ec_fsm_slave_state_idle;

   345

        return 1;

   346

    }

   347

   348

    // Found pending register request. Execute it!

   349

    EC_SLAVE_DBG(slave, 1, "Processing register request...\n");

   350

   351

    fsm->reg_request->state = EC_INT_REQUEST_BUSY;

   352

   353

    // Start register access

   354

    if (fsm->reg_request->dir == EC_DIR_INPUT) {

   355

        ec_datagram_fprd(datagram, slave->station_address,

   356

                fsm->reg_request->address, fsm->reg_request->transfer_size);

   357

        ec_datagram_zero(datagram);

   358

    } else {

   359

        ec_datagram_fpwr(datagram, slave->station_address,

   360

                fsm->reg_request->address, fsm->reg_request->transfer_size);

   361

        memcpy(datagram->data, fsm->reg_request->data,

   362

                fsm->reg_request->transfer_size);

   363

    }

   364

    datagram->device_index = slave->device_index;

   365

    fsm->state = ec_fsm_slave_state_reg_request;

   366

    return 1;

   367

}

   368

   369

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

   370

   371

/** Slave state: Register request.

   372

 */

   373

void ec_fsm_slave_state_reg_request(

   374

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   375

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

   376

        )

   377

{

   378

    ec_slave_t *slave = fsm->slave;

   379

    ec_reg_request_t *reg = fsm->reg_request;

   380

   381

    if (!reg) {

   382

        // configuration was cleared in the meantime

   383

        fsm->state = ec_fsm_slave_state_ready;

   384

        fsm->reg_request = NULL;

   385

        return;

   386

    }

   387

   388

    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {

   389

        EC_SLAVE_ERR(slave, "Failed to receive register"

   390

                " request datagram: ");

   391

        ec_datagram_print_state(fsm->datagram);

   392

        reg->state = EC_INT_REQUEST_FAILURE;

   393

        wake_up_all(&slave->master->request_queue);

   394

        fsm->reg_request = NULL;

   395

        fsm->state = ec_fsm_slave_state_ready;

   396

        return;

   397

    }

   398

   399

    if (fsm->datagram->working_counter == 1) {

   400

        if (reg->dir == EC_DIR_INPUT) { // read request

   401

            memcpy(reg->data, fsm->datagram->data, reg->transfer_size);

   402

        }

   403

   404

        reg->state = EC_INT_REQUEST_SUCCESS;

   405

        EC_SLAVE_DBG(slave, 1, "Register request successful.\n");

   406

    } else {

   407

        reg->state = EC_INT_REQUEST_FAILURE;

   408

        ec_datagram_print_state(fsm->datagram);

   409

        EC_SLAVE_ERR(slave, "Register request failed"

   410

                " (working counter is %u).\n",

   411

                fsm->datagram->working_counter);

   412

    }

   413

   414

    wake_up_all(&slave->master->request_queue);

   415

    fsm->reg_request = NULL;

   416

    fsm->state = ec_fsm_slave_state_ready;

   417

}

   418

   419

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

   420

   421

/** Check for pending FoE requests and process one.

   422

 *

   423

 * \return non-zero, if an FoE request is processed.

   424

 */

   425

int ec_fsm_slave_action_process_foe(

   426

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   427

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

   428

        )

   429

{

   430

    ec_slave_t *slave = fsm->slave;

   431

    ec_foe_request_t *request;

   432

   433

    if (list_empty(&slave->foe_requests)) {

   434

        return 0;

   435

    }

   436

   437

    // take the first request to be processed

   438

    request = list_entry(slave->foe_requests.next, ec_foe_request_t, list);

   439

    list_del_init(&request->list); // dequeue

   440

   441

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   442

        EC_SLAVE_WARN(slave, "Aborting FoE request,"

   443

                " slave has error flag set.\n");

   444

        request->state = EC_INT_REQUEST_FAILURE;

   445

        wake_up_all(&slave->master->request_queue);

   446

        fsm->state = ec_fsm_slave_state_idle;

   447

        return 1;

   448

    }

   449

   450

    request->state = EC_INT_REQUEST_BUSY;

   451

    fsm->foe_request = request;

   452

   453

    EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");

   454

   455

    fsm->state = ec_fsm_slave_state_foe_request;

   456

    ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);

   457

    ec_fsm_foe_exec(&fsm->fsm_foe, datagram);

   458

    return 1;

   459

}

   460

   461

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

   462

   463

/** Slave state: FOE REQUEST.

   464

 */

   465

void ec_fsm_slave_state_foe_request(

   466

        ec_fsm_slave_t *fsm, /**< Slave state machine. */

   467

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

   468

        )

   469

{

   470

    ec_slave_t *slave = fsm->slave;

   471

    ec_foe_request_t *request = fsm->foe_request;

   472

   473

    if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) {

   474

        return;

   475

    }

   476

   477

    if (!ec_fsm_foe_success(&fsm->fsm_foe)) {

   478

        EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");

   479

        request->state = EC_INT_REQUEST_FAILURE;

   480

        wake_up_all(&slave->master->request_queue);

   481

        fsm->foe_request = NULL;

   482

        fsm->state = ec_fsm_slave_state_ready;

   483

        return;

   484

    }

   485

   486

    // finished transferring FoE

   487

    EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"

   488

            " data.\n", request->data_size);

   489

   490

    request->state = EC_INT_REQUEST_SUCCESS;

   491

    wake_up_all(&slave->master->request_queue);

   492

    fsm->foe_request = NULL;

   493

    fsm->state = ec_fsm_slave_state_ready;

   494

}

   495

   496

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

   497