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

int ec_fsm_slave_is_ready(

   163

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

   164

        )

   165

{

   166

    return fsm->state == ec_fsm_slave_state_ready;

   167

}

   168

   169

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

   170

 * Slave state machine

   171

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

   172

   173

/** Slave state: IDLE.

   174

 */

   175

void ec_fsm_slave_state_idle(

   176

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

   177

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

   178

        )

   179

{

   180

    // do nothing

   181

}

   182

   183

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

   184

   185

/** Slave state: READY.

   186

 */

   187

void ec_fsm_slave_state_ready(

   188

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

   189

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

   190

        )

   191

{

   192

    // Check for pending external SDO requests

   193

    if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {

   194

        return;

   195

    }

   196

   197

    // Check for pending external register requests

   198

    if (ec_fsm_slave_action_process_reg(fsm, datagram)) {

   199

        return;

   200

    }

   201

   202

    // Check for pending FoE requests

   203

    if (ec_fsm_slave_action_process_foe(fsm, datagram)) {

   204

        return;

   205

    }

   206

   207

    // Check for pending SoE requests

   208

    if (ec_fsm_slave_action_process_soe(fsm, datagram)) {

   209

        return;

   210

    }

   211

}

   212

   213

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

   214

   215

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

   216

 *

   217

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

   218

 */

   219

int ec_fsm_slave_action_process_sdo(

   220

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

   221

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

   222

        )

   223

{

   224

    ec_slave_t *slave = fsm->slave;

   225

    ec_sdo_request_t *request;

   226

   227

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

   228

        return 0;

   229

    }

   230

   231

    // take the first request to be processed

   232

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

   233

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

   234

   235

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   236

        EC_SLAVE_WARN(slave, "Aborting SDO request,"

   237

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

   238

        request->state = EC_INT_REQUEST_FAILURE;

   239

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

   240

        fsm->state = ec_fsm_slave_state_idle;

   241

        return 1;

   242

    }

   243

   244

    if (slave->current_state == EC_SLAVE_STATE_INIT) {

   245

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

   246

        request->state = EC_INT_REQUEST_FAILURE;

   247

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

   248

        fsm->state = ec_fsm_slave_state_idle;

   249

        return 1;

   250

    }

   251

   252

    fsm->sdo_request = request;

   253

    request->state = EC_INT_REQUEST_BUSY;

   254

   255

    // Found pending SDO request. Execute it!

   256

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

   257

   258

    // Start SDO transfer

   259

    fsm->state = ec_fsm_slave_state_sdo_request;

   260

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

   261

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

   262

    return 1;

   263

}

   264

   265

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

   266

   267

/** Slave state: SDO_REQUEST.

   268

 */

   269

void ec_fsm_slave_state_sdo_request(

   270

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

   271

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

   272

        )

   273

{

   274

    ec_slave_t *slave = fsm->slave;

   275

    ec_sdo_request_t *request = fsm->sdo_request;

   276

   277

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

   278

        return;

   279

    }

   280

   281

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

   282

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

   283

        request->state = EC_INT_REQUEST_FAILURE;

   284

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

   285

        fsm->sdo_request = NULL;

   286

        fsm->state = ec_fsm_slave_state_ready;

   287

        return;

   288

    }

   289

   290

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

   291

   292

    // SDO request finished

   293

    request->state = EC_INT_REQUEST_SUCCESS;

   294

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

   295

    fsm->sdo_request = NULL;

   296

    fsm->state = ec_fsm_slave_state_ready;

   297

}

   298

   299

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

   300

   301

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

   302

 *

   303

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

   304

 */

   305

int ec_fsm_slave_action_process_reg(

   306

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

   307

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

   308

        )

   309

{

   310

    ec_slave_t *slave = fsm->slave;

   311

    ec_reg_request_t *reg;

   312

   313

    fsm->reg_request = NULL;

   314

   315

    if (slave->config) {

   316

        // search the first internal register request to be processed

   317

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

   318

            if (reg->state == EC_INT_REQUEST_QUEUED) {

   319

                fsm->reg_request = reg;

   320

                break;

   321

            }

   322

        }

   323

    }

   324

   325

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

   326

        // take the first external request to be processed

   327

        fsm->reg_request =

   328

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

   329

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

   330

    }

   331

   332

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

   333

        return 0;

   334

    }

   335

   336

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   337

        EC_SLAVE_WARN(slave, "Aborting register request,"

   338

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

   339

        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;

   340

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

   341

        fsm->reg_request = NULL;

   342

        fsm->state = ec_fsm_slave_state_idle;

   343

        return 1;

   344

    }

   345

   346

    // Found pending register request. Execute it!

   347

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

   348

   349

    fsm->reg_request->state = EC_INT_REQUEST_BUSY;

   350

   351

    // Start register access

   352

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

   353

        ec_datagram_fprd(datagram, slave->station_address,

   354

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

   355

        ec_datagram_zero(datagram);

   356

    } else {

   357

        ec_datagram_fpwr(datagram, slave->station_address,

   358

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

   359

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

   360

                fsm->reg_request->transfer_size);

   361

    }

   362

    datagram->device_index = slave->device_index;

   363

    fsm->state = ec_fsm_slave_state_reg_request;

   364

    return 1;

   365

}

   366

   367

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

   368

   369

/** Slave state: Register request.

   370

 */

   371

void ec_fsm_slave_state_reg_request(

   372

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

   373

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

   374

        )

   375

{

   376

    ec_slave_t *slave = fsm->slave;

   377

    ec_reg_request_t *reg = fsm->reg_request;

   378

   379

    if (!reg) {

   380

        // configuration was cleared in the meantime

   381

        fsm->state = ec_fsm_slave_state_ready;

   382

        fsm->reg_request = NULL;

   383

        return;

   384

    }

   385

   386

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

   387

        EC_SLAVE_ERR(slave, "Failed to receive register"

   388

                " request datagram: ");

   389

        ec_datagram_print_state(fsm->datagram);

   390

        reg->state = EC_INT_REQUEST_FAILURE;

   391

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

   392

        fsm->reg_request = NULL;

   393

        fsm->state = ec_fsm_slave_state_ready;

   394

        return;

   395

    }

   396

   397

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

   398

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

   399

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

   400

        }

   401

   402

        reg->state = EC_INT_REQUEST_SUCCESS;

   403

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

   404

    } else {

   405

        reg->state = EC_INT_REQUEST_FAILURE;

   406

        ec_datagram_print_state(fsm->datagram);

   407

        EC_SLAVE_ERR(slave, "Register request failed"

   408

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

   409

                fsm->datagram->working_counter);

   410

    }

   411

   412

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

   413

    fsm->reg_request = NULL;

   414

    fsm->state = ec_fsm_slave_state_ready;

   415

}

   416

   417

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

   418

   419

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

   420

 *

   421

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

   422

 */

   423

int ec_fsm_slave_action_process_foe(

   424

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

   425

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

   426

        )

   427

{

   428

    ec_slave_t *slave = fsm->slave;

   429

    ec_foe_request_t *request;

   430

   431

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

   432

        return 0;

   433

    }

   434

   435

    // take the first request to be processed

   436

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

   437

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

   438

   439

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   440

        EC_SLAVE_WARN(slave, "Aborting FoE request,"

   441

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

   442

        request->state = EC_INT_REQUEST_FAILURE;

   443

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

   444

        fsm->state = ec_fsm_slave_state_idle;

   445

        return 1;

   446

    }

   447

   448

    request->state = EC_INT_REQUEST_BUSY;

   449

    fsm->foe_request = request;

   450

   451

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

   452

   453

    fsm->state = ec_fsm_slave_state_foe_request;

   454

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

   455

    ec_fsm_foe_exec(&fsm->fsm_foe, datagram);

   456

    return 1;

   457

}

   458

   459

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

   460

   461

/** Slave state: FOE REQUEST.

   462

 */

   463

void ec_fsm_slave_state_foe_request(

   464

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

   465

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

   466

        )

   467

{

   468

    ec_slave_t *slave = fsm->slave;

   469

    ec_foe_request_t *request = fsm->foe_request;

   470

   471

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

   472

        return;

   473

    }

   474

   475

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

   476

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

   477

        request->state = EC_INT_REQUEST_FAILURE;

   478

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

   479

        fsm->foe_request = NULL;

   480

        fsm->state = ec_fsm_slave_state_ready;

   481

        return;

   482

    }

   483

   484

    // finished transferring FoE

   485

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

   486

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

   487

   488

    request->state = EC_INT_REQUEST_SUCCESS;

   489

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

   490

    fsm->foe_request = NULL;

   491

    fsm->state = ec_fsm_slave_state_ready;

   492

}

   493