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

/**

    31

   \file

    32

   EtherCAT SoE state machines.

    33

*/

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "fsm_soe.h"

    41

    42

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

    43

    44

/** Mailbox type for SoE.

    45

 */

    46

#define EC_MBOX_TYPE_SOE 0x05

    47

    48

#define EC_SOE_OPCODE_READ_REQUEST   0x01

    49

#define EC_SOE_OPCODE_READ_RESPONSE  0x02

    50

#define EC_SOE_OPCODE_WRITE_REQUEST  0x03

    51

#define EC_SOE_OPCODE_WRITE_RESPONSE 0x04

    52

    53

#define EC_SOE_READ_REQUEST_SIZE   0x04

    54

#define EC_SOE_READ_RESPONSE_SIZE  0x04

    55

#define EC_SOE_WRITE_REQUEST_SIZE  0x04

    56

#define EC_SOE_WRITE_RESPONSE_SIZE 0x04

    57

    58

#define EC_SOE_RESPONSE_TIMEOUT 1000

    59

    60

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

    61

    62

void ec_fsm_soe_read_start(ec_fsm_soe_t *);

    63

void ec_fsm_soe_read_request(ec_fsm_soe_t *);

    64

void ec_fsm_soe_read_check(ec_fsm_soe_t *);

    65

void ec_fsm_soe_read_response(ec_fsm_soe_t *);

    66

    67

void ec_fsm_soe_write_start(ec_fsm_soe_t *);

    68

void ec_fsm_soe_write_request(ec_fsm_soe_t *);

    69

void ec_fsm_soe_write_check(ec_fsm_soe_t *);

    70

void ec_fsm_soe_write_response(ec_fsm_soe_t *);

    71

    72

void ec_fsm_soe_end(ec_fsm_soe_t *);

    73

void ec_fsm_soe_error(ec_fsm_soe_t *);

    74

    75

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

    76

    77

extern const ec_code_msg_t soe_error_codes[];

    78

    79

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

    80

    81

/** Outputs an SoE error code.

    82

*/

    83

void ec_print_soe_error(uint16_t error_code)

    84

{

    85

    const ec_code_msg_t *error_msg;

    86

    87

    for (error_msg = soe_error_codes; error_msg->code; error_msg++) {

    88

        if (error_msg->code == error_code) {

    89

            EC_ERR("SoE error 0x%04X: \"%s\".\n",

    90

                   error_msg->code, error_msg->message);

    91

            return;

    92

        }

    93

    }

    94

    95

    EC_ERR("Unknown SoE error 0x%04X.\n", error_code);

    96

}

    97

    98

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

    99

   100

/** Constructor.

   101

 */

   102

void ec_fsm_soe_init(

   103

        ec_fsm_soe_t *fsm, /**< finite state machine */

   104

        ec_datagram_t *datagram /**< datagram */

   105

        )

   106

{

   107

    fsm->state = NULL;

   108

    fsm->datagram = datagram;

   109

}

   110

   111

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

   112

   113

/** Destructor.

   114

 */

   115

void ec_fsm_soe_clear(

   116

        ec_fsm_soe_t *fsm /**< finite state machine */

   117

        )

   118

{

   119

}

   120

   121

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

   122

   123

/** Starts to transfer an IDN to/from a slave.

   124

 */

   125

void ec_fsm_soe_transfer(

   126

        ec_fsm_soe_t *fsm, /**< State machine. */

   127

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

   128

        ec_soe_request_t *request /**< SoE request. */

   129

        )

   130

{

   131

    fsm->slave = slave;

   132

    fsm->request = request;

   133

    if (request->dir == EC_DIR_OUTPUT) {

   134

        fsm->state = ec_fsm_soe_write_start;

   135

    } else {

   136

        fsm->state = ec_fsm_soe_read_start;

   137

    }

   138

}

   139

   140

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

   141

   142

/**

   143

   Executes the current state of the state machine.

   144

   \return false, if state machine has terminated

   145

*/

   146

   147

int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)

   148

{

   149

    fsm->state(fsm);

   150

   151

    return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;

   152

}

   153

   154

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

   155

   156

/**

   157

   Returns, if the state machine terminated with success.

   158

   \return non-zero if successful.

   159

*/

   160

   161

int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)

   162

{

   163

    return fsm->state == ec_fsm_soe_end;

   164

}

   165

   166

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

   167

 * SoE read state machine

   168

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

   169

   170

/** SoE state: READ START.

   171

 */

   172

void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)

   173

{

   174

    ec_datagram_t *datagram = fsm->datagram;

   175

    ec_slave_t *slave = fsm->slave;

   176

    ec_master_t *master = slave->master;

   177

    ec_soe_request_t *request = fsm->request;

   178

    uint8_t *data;

   179

   180

    if (master->debug_level)

   181

        EC_DBG("Reading IDN 0x%04X from slave %u.\n",

   182

               request->idn, slave->ring_position);

   183

   184

    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {

   185

        EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);

   186

        fsm->state = ec_fsm_soe_error;

   187

        return;

   188

    }

   189

   190

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   191

            EC_SOE_READ_REQUEST_SIZE);

   192

    if (IS_ERR(data)) {

   193

        fsm->state = ec_fsm_soe_error;

   194

        return;

   195

    }

   196

   197

    EC_WRITE_U8(data, EC_SOE_OPCODE_READ_REQUEST);

   198

    EC_WRITE_U8(data + 1, 1 << 6); // request value

   199

    EC_WRITE_U16(data + 2, request->idn);

   200

   201

    if (master->debug_level) {

   202

        EC_DBG("SCC read request:\n");

   203

        ec_print_data(data, EC_SOE_READ_REQUEST_SIZE);

   204

    }

   205

   206

    fsm->request->data_size = 0;

   207

    fsm->request->jiffies_sent = jiffies;

   208

    fsm->retries = EC_FSM_RETRIES;

   209

    fsm->state = ec_fsm_soe_read_request;

   210

}

   211

   212

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

   213

   214

/** SoE state: READ REQUEST.

   215

 */

   216

void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)

   217

{

   218

    ec_datagram_t *datagram = fsm->datagram;

   219

    ec_slave_t *slave = fsm->slave;

   220

    unsigned long diff_ms;

   221

   222

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

   223

        return; // FIXME: check for response first?

   224

   225

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   226

        fsm->state = ec_fsm_soe_error;

   227

        EC_ERR("Failed to receive SoE read request for slave %u: ",

   228

               slave->ring_position);

   229

        ec_datagram_print_state(datagram);

   230

        return;

   231

    }

   232

   233

    diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

   234

   235

    if (datagram->working_counter != 1) {

   236

        if (!datagram->working_counter) {

   237

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   238

                // no response; send request datagram again

   239

                return;

   240

            }

   241

        }

   242

        fsm->state = ec_fsm_soe_error;

   243

        EC_ERR("Reception of SoE read request for IDN 0x%04x failed"

   244

                " after %u ms on slave %u: ",

   245

                fsm->request->idn, (u32) diff_ms,

   246

                fsm->slave->ring_position);

   247

        ec_datagram_print_wc_error(datagram);

   248

        return;

   249

    }

   250

   251

    fsm->jiffies_start = datagram->jiffies_sent;

   252

    ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   253

    fsm->retries = EC_FSM_RETRIES;

   254

    fsm->state = ec_fsm_soe_read_check;

   255

}

   256

   257

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

   258

   259

/** CoE state: READ CHECK.

   260

 */

   261

void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)

   262

{

   263

    ec_datagram_t *datagram = fsm->datagram;

   264

    ec_slave_t *slave = fsm->slave;

   265

   266

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

   267

        return;

   268

   269

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   270

        fsm->state = ec_fsm_soe_error;

   271

        EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ",

   272

               slave->ring_position);

   273

        ec_datagram_print_state(datagram);

   274

        return;

   275

    }

   276

   277

    if (datagram->working_counter != 1) {

   278

        fsm->state = ec_fsm_soe_error;

   279

        EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ",

   280

                slave->ring_position);

   281

        ec_datagram_print_wc_error(datagram);

   282

        return;

   283

    }

   284

   285

    if (!ec_slave_mbox_check(datagram)) {

   286

        unsigned long diff_ms =

   287

            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   288

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   289

            fsm->state = ec_fsm_soe_error;

   290

            EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x"

   291

                    " read response on slave %u.\n", (u32) diff_ms,

   292

                    fsm->request->idn, slave->ring_position);

   293

            return;

   294

        }

   295

   296

        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   297

        fsm->retries = EC_FSM_RETRIES;

   298

        return;

   299

    }

   300

   301

    // Fetch response

   302

    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

   303

    fsm->retries = EC_FSM_RETRIES;

   304

    fsm->state = ec_fsm_soe_read_response;

   305

}

   306

   307

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

   308

   309

/** SoE state: READ RESPONSE.

   310

 */

   311

void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)

   312

{

   313

    ec_datagram_t *datagram = fsm->datagram;

   314

    ec_slave_t *slave = fsm->slave;

   315

    ec_master_t *master = slave->master;

   316

    uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,

   317

            value_included;

   318

    size_t rec_size, data_size;

   319

    ec_soe_request_t *req = fsm->request;

   320

   321

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

   322

        return; // FIXME: request again?

   323

   324

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   325

        fsm->state = ec_fsm_soe_error;

   326

        EC_ERR("Failed to receive SoE read response datagram for"

   327

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

   328

        ec_datagram_print_state(datagram);

   329

        return;

   330

    }

   331

   332

    if (datagram->working_counter != 1) {

   333

        fsm->state = ec_fsm_soe_error;

   334

        EC_ERR("Reception of SoE read response failed on slave %u: ",

   335

                slave->ring_position);

   336

        ec_datagram_print_wc_error(datagram);

   337

        return;

   338

    }

   339

   340

    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);

   341

    if (IS_ERR(data)) {

   342

        fsm->state = ec_fsm_soe_error;

   343

        return;

   344

    }

   345

   346

    if (master->debug_level) {

   347

        EC_DBG("SCC read response:\n");

   348

        ec_print_data(data, rec_size);

   349

    }

   350

   351

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   352

        fsm->state = ec_fsm_soe_error;

   353

        EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   354

        return;

   355

    }

   356

   357

    if (rec_size < EC_SOE_READ_RESPONSE_SIZE) {

   358

        fsm->state = ec_fsm_soe_error;

   359

        EC_ERR("Received currupted SoE read response (%zu bytes)!\n",

   360

                rec_size);

   361

        ec_print_data(data, rec_size);

   362

        return;

   363

    }

   364

   365

    header = EC_READ_U8(data);

   366

    opcode = header & 0x7;

   367

    incomplete = (header >> 3) & 1;

   368

    error_flag = (header >> 4) & 1;

   369

   370

    if (opcode != EC_SOE_OPCODE_READ_RESPONSE) {

   371

        EC_ERR("Received no read response (opcode %x).\n", opcode);

   372

        ec_print_data(data, rec_size);

   373

        fsm->state = ec_fsm_soe_error;

   374

        return;

   375

    }

   376

   377

    if (error_flag) {

   378

        req->error_code = EC_READ_U16(data + rec_size - 2);

   379

        EC_ERR("Received error response:\n");

   380

        ec_print_soe_error(req->error_code);

   381

        fsm->state = ec_fsm_soe_error;

   382

        return;

   383

    } else {

   384

        req->error_code = 0x0000;

   385

    }

   386

   387

    value_included = (EC_READ_U8(data + 1) >> 6) & 1;

   388

    if (!value_included) {

   389

        EC_ERR("No value included!\n");

   390

        fsm->state = ec_fsm_soe_error;

   391

        return;

   392

    }

   393

   394

    data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE;

   395

    if (ec_soe_request_append_data(req,

   396

                data + EC_SOE_READ_RESPONSE_SIZE, data_size)) {

   397

        fsm->state = ec_fsm_soe_error;

   398

        return;

   399

    }

   400

   401

    if (incomplete) {

   402

        if (master->debug_level) {

   403

            EC_DBG("SoE data incomplete. Waiting for fragment"

   404

                    " at offset %zu.\n", req->data_size);

   405

        }

   406

        fsm->jiffies_start = datagram->jiffies_sent;

   407

        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   408

        fsm->retries = EC_FSM_RETRIES;

   409

        fsm->state = ec_fsm_soe_read_check;

   410

    } else {

   411

        if (master->debug_level) {

   412

            EC_DBG("IDN data:\n");

   413

            ec_print_data(req->data, req->data_size);

   414

        }

   415

   416

        fsm->state = ec_fsm_soe_end; // success

   417

    }

   418

}

   419

   420

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

   421

 * SoE write state machine

   422

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

   423

   424

/** Write next fragment.

   425

 */

   426

void ec_fsm_soe_write_next_fragment(

   427

        ec_fsm_soe_t *fsm /**< finite state machine */

   428

        )

   429

{

   430

    ec_datagram_t *datagram = fsm->datagram;

   431

    ec_slave_t *slave = fsm->slave;

   432

    ec_master_t *master = slave->master;

   433

    ec_soe_request_t *req = fsm->request;

   434

    uint8_t incomplete, *data;

   435

    size_t header_size, max_fragment_size, remaining_size, fragment_size;

   436

    uint16_t fragments_left;

   437

   438

    header_size = EC_MBOX_HEADER_SIZE + EC_SOE_WRITE_REQUEST_SIZE;

   439

    if (slave->configured_rx_mailbox_size <= header_size) {

   440

        EC_ERR("Mailbox size (%u) too small for SoE write.\n",

   441

                slave->configured_rx_mailbox_size);

   442

        fsm->state = ec_fsm_soe_error;

   443

        return;

   444

    }

   445

   446

    remaining_size = req->data_size - fsm->offset;

   447

    max_fragment_size = slave->configured_rx_mailbox_size - header_size;

   448

    incomplete = remaining_size > max_fragment_size;

   449

    fragment_size = incomplete ? max_fragment_size : remaining_size;

   450

    fragments_left = remaining_size / fragment_size - 1;

   451

    if (remaining_size % fragment_size) {

   452

        fragments_left++;

   453

    }

   454

   455

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   456

            EC_SOE_WRITE_REQUEST_SIZE + fragment_size);

   457

    if (IS_ERR(data)) {

   458

        fsm->state = ec_fsm_soe_error;

   459

        return;

   460

    }

   461

   462

    EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST | incomplete << 3);

   463

    EC_WRITE_U8(data + 1, 1 << 6); // only value included

   464

    EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);

   465

    memcpy(data + 4, req->data + fsm->offset, fragment_size);

   466

    fsm->offset += fragment_size;

   467

   468

    if (master->debug_level) {

   469

        EC_DBG("SCC write request:\n");

   470

        ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + fragment_size);

   471

    }

   472

   473

    req->jiffies_sent = jiffies;

   474

    fsm->retries = EC_FSM_RETRIES;

   475

    fsm->state = ec_fsm_soe_write_request;

   476

}

   477

   478

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

   479

   480

/** SoE state: WRITE START.

   481

 */

   482

void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)

   483

{

   484

    ec_slave_t *slave = fsm->slave;

   485

    ec_master_t *master = slave->master;

   486

    ec_soe_request_t *req = fsm->request;

   487

   488

    if (master->debug_level)

   489

        EC_DBG("Writing IDN 0x%04X (%zu byte) to slave %u.\n",

   490

               req->idn, req->data_size, slave->ring_position);

   491

   492

    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {

   493

        EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);

   494

        fsm->state = ec_fsm_soe_error;

   495

        return;

   496

    }

   497

   498

    fsm->offset = 0;

   499

    ec_fsm_soe_write_next_fragment(fsm);

   500

}

   501

   502

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

   503

   504

/** SoE state: WRITE REQUEST.

   505

 */

   506

void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)

   507

{

   508

    ec_datagram_t *datagram = fsm->datagram;

   509

    ec_slave_t *slave = fsm->slave;

   510

    unsigned long diff_ms;

   511

   512

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

   513

        return; // FIXME: check for response first?