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

/** SoE operations

    49

 */

    50

enum ec_soe_opcodes {

    51

    OPCODE_READ_REQUEST   = 0x01, /**< Read request. */

    52

    OPCODE_READ_RESPONSE  = 0x02, /**< Read response. */

    53

    OPCODE_WRITE_REQUEST  = 0x03, /**< Write request. */

    54

    OPCODE_WRITE_RESPONSE = 0x04  /**< Write response. */

    55

};

    56

    57

/** Size of all SoE headers.

    58

 */

    59

#define EC_SOE_SIZE 0x04

    60

    61

/** SoE response timeout [ms].

    62

 */

    63

#define EC_SOE_RESPONSE_TIMEOUT 1000

    64

    65

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

    66

    67

void ec_fsm_soe_read_start(ec_fsm_soe_t *);

    68

void ec_fsm_soe_read_request(ec_fsm_soe_t *);

    69

void ec_fsm_soe_read_check(ec_fsm_soe_t *);

    70

void ec_fsm_soe_read_response(ec_fsm_soe_t *);

    71

    72

void ec_fsm_soe_write_start(ec_fsm_soe_t *);

    73

void ec_fsm_soe_write_request(ec_fsm_soe_t *);

    74

void ec_fsm_soe_write_check(ec_fsm_soe_t *);

    75

void ec_fsm_soe_write_response(ec_fsm_soe_t *);

    76

    77

void ec_fsm_soe_end(ec_fsm_soe_t *);

    78

void ec_fsm_soe_error(ec_fsm_soe_t *);

    79

    80

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

    81

    82

extern const ec_code_msg_t soe_error_codes[];

    83

    84

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

    85

    86

/** Outputs an SoE error code.

    87

*/

    88

void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)

    89

{

    90

    const ec_code_msg_t *error_msg;

    91

    92

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

    93

        if (error_msg->code == error_code) {

    94

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

    95

                   error_msg->code, error_msg->message);

    96

            return;

    97

        }

    98

    }

    99

   100

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

   101

}

   102

   103

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

   104

   105

/** Constructor.

   106

 */

   107

void ec_fsm_soe_init(

   108

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

   109

        ec_mailbox_t *mbox /**< mailbox */

   110

        )

   111

{

   112

    fsm->state = NULL;

   113

    fsm->mbox = mbox;

   114

}

   115

   116

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

   117

   118

/** Destructor.

   119

 */

   120

void ec_fsm_soe_clear(

   121

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

   122

        )

   123

{

   124

}

   125

   126

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

   127

   128

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

   129

 */

   130

void ec_fsm_soe_transfer(

   131

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

   132

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

   133

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

   134

        )

   135

{

   136

    fsm->slave = slave;

   137

    fsm->request = request;

   138

    if (request->dir == EC_DIR_OUTPUT) {

   139

        fsm->state = ec_fsm_soe_write_start;

   140

    } else {

   141

        fsm->state = ec_fsm_soe_read_start;

   142

    }

   143

}

   144

   145

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

   146

   147

/**

   148

   Executes the current state of the state machine.

   149

   \return false, if state machine has terminated

   150

*/

   151

   152

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

   153

{

   154

    fsm->state(fsm);

   155

   156

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

   157

}

   158

   159

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

   160

   161

/**

   162

   Returns, if the state machine terminated with success.

   163

   \return non-zero if successful.

   164

*/

   165

   166

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

   167

{

   168

    return fsm->state == ec_fsm_soe_end;

   169

}

   170

   171

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

   172

   173

/** Output information about a failed SoE transfer.

   174

 */

   175

void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)

   176

{

   177

    ec_soe_request_t *request = fsm->request;

   178

   179

    EC_SLAVE_ERR(fsm->slave, "");

   180

   181

    if (request->dir == EC_DIR_OUTPUT) {

   182

        printk("Writing");

   183

    } else {

   184

        printk("Reading");

   185

    }

   186

   187

    printk(" IDN 0x%04X failed.\n", request->idn);

   188

}

   189

   190

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

   191

 * SoE read state machine

   192

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

   193

   194

/** SoE state: READ START.

   195

 */

   196

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

   197

{

   198

    ec_mailbox_t *mbox = fsm->mbox;

   199

    ec_slave_t *slave = fsm->slave;

   200

    ec_master_t *master = slave->master;

   201

    ec_soe_request_t *request = fsm->request;

   202

    uint8_t *data;

   203

   204

    EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,

   205

            request->drive_no);

   206

   207

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

   208

        EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");

   209

        fsm->state = ec_fsm_soe_error;

   210

        ec_fsm_soe_print_error(fsm);

   211

        return;

   212

    }

   213

   214

    data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,

   215

            EC_SOE_SIZE);

   216

    if (IS_ERR(data)) {

   217

        fsm->state = ec_fsm_soe_error;

   218

        ec_fsm_soe_print_error(fsm);

   219

        return;

   220

    }

   221

   222

    EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);

   223

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

   224

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

   225

   226

    if (master->debug_level) {

   227

        EC_SLAVE_DBG(slave, 0, "SCC read request:\n");

   228

        ec_print_data(data, EC_SOE_SIZE);

   229

    }

   230

   231

    fsm->request->data_size = 0;

   232

    fsm->request->jiffies_sent = jiffies;

   233

    fsm->retries = EC_FSM_RETRIES;

   234

    fsm->state = ec_fsm_soe_read_request;

   235

}

   236

   237

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

   238

   239

/** SoE state: READ REQUEST.

   240

 */

   241

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

   242

{

   243

    ec_mailbox_t *mbox = fsm->mbox;

   244

    ec_slave_t *slave = fsm->slave;

   245

    unsigned long diff_ms;

   246

   247

    if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)

   248

        return; // FIXME: check for response first?

   249

   250

    if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {

   251

        fsm->state = ec_fsm_soe_error;

   252

        EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");

   253

        ec_datagram_print_state(mbox->datagram);

   254

        ec_fsm_soe_print_error(fsm);

   255

        return;

   256

    }

   257

   258

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

   259

   260

    if (!ec_mbox_is_datagram_wc(mbox, 1)) {

   261

        if (ec_mbox_is_datagram_wc(mbox, 0)) {

   262

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   263

                // no response; send request datagram again

   264

                return;

   265

            }

   266

        }

   267

        fsm->state = ec_fsm_soe_error;

   268

        EC_SLAVE_ERR(slave, "Reception of SoE read request"

   269

                " failed after %lu ms: ", diff_ms);

   270

        ec_datagram_print_wc_error(mbox->datagram);

   271

        ec_fsm_soe_print_error(fsm);

   272

        return;

   273

    }

   274

   275

    fsm->jiffies_start = mbox->datagram->jiffies_sent;

   276

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

   277

    fsm->retries = EC_FSM_RETRIES;

   278

    fsm->state = ec_fsm_soe_read_check;

   279

}

   280

   281

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

   282

   283

/** CoE state: READ CHECK.

   284

 */

   285

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

   286

{

   287

    ec_mailbox_t *mbox = fsm->mbox;

   288

    ec_slave_t *slave = fsm->slave;

   289

   290

    if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)

   291

        return;

   292

   293

    if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {

   294

        fsm->state = ec_fsm_soe_error;

   295

        EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");

   296

        ec_datagram_print_state(mbox->datagram);

   297

        ec_fsm_soe_print_error(fsm);

   298

        return;

   299

    }

   300

   301

    if (!ec_mbox_is_datagram_wc(mbox, 1)) {

   302

        fsm->state = ec_fsm_soe_error;

   303

        EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"

   304

                " datagram failed: ");

   305

        ec_datagram_print_wc_error(mbox->datagram);

   306

        ec_fsm_soe_print_error(fsm);

   307

        return;

   308

    }

   309

   310

    if (!ec_slave_mbox_check(mbox)) {

   311

        unsigned long diff_ms =

   312

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

   313

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   314

            fsm->state = ec_fsm_soe_error;

   315

            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"

   316

                    " read response.\n", diff_ms);

   317

            ec_fsm_soe_print_error(fsm);

   318

            return;

   319

        }

   320

   321

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

   322

        fsm->retries = EC_FSM_RETRIES;

   323

        return;

   324

    }

   325

   326

    // Fetch response

   327

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

   328

    fsm->retries = EC_FSM_RETRIES;

   329

    fsm->state = ec_fsm_soe_read_response;

   330

}

   331

   332

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

   333

   334

/** SoE state: READ RESPONSE.

   335

 */

   336

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

   337

{

   338

    ec_mailbox_t *mbox = fsm->mbox;

   339

    ec_slave_t *slave = fsm->slave;

   340

    ec_master_t *master = slave->master;

   341

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

   342

            value_included;

   343

    size_t rec_size, data_size;

   344

    ec_soe_request_t *req = fsm->request;

   345

   346

    if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--)

   347

        return; // FIXME: request again?

   348

   349

    if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {

   350

        fsm->state = ec_fsm_soe_error;

   351

        EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");

   352

        ec_datagram_print_state(mbox->datagram);

   353

        ec_fsm_soe_print_error(fsm);

   354

        return;

   355

    }

   356

   357

    if (!ec_mbox_is_datagram_wc(mbox, 1)) {

   358

        fsm->state = ec_fsm_soe_error;

   359

        EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");

   360

        ec_datagram_print_wc_error(mbox->datagram);

   361

        ec_fsm_soe_print_error(fsm);

   362

        return;

   363

    }

   364

   365

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

   366

    if (IS_ERR(data)) {

   367

        fsm->state = ec_fsm_soe_error;

   368

        ec_fsm_soe_print_error(fsm);

   369

        return;

   370

    }

   371

   372

    if (master->debug_level) {

   373

        EC_SLAVE_DBG(slave, 0, "SCC read response:\n");

   374

        ec_print_data(data, rec_size);

   375

    }

   376

   377

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   378

        fsm->state = ec_fsm_soe_error;

   379

        EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",

   380

                mbox_prot);

   381

        ec_fsm_soe_print_error(fsm);

   382

        return;

   383

    }

   384

   385

    if (rec_size < EC_SOE_SIZE) {

   386

        fsm->state = ec_fsm_soe_error;

   387

        EC_SLAVE_ERR(slave, "Received currupted SoE read response"

   388

                " (%zu bytes)!\n", rec_size);

   389

        ec_print_data(data, rec_size);

   390

        ec_fsm_soe_print_error(fsm);

   391

        return;

   392

    }

   393

   394

    header = EC_READ_U8(data);

   395

    opcode = header & 0x7;

   396

    incomplete = (header >> 3) & 1;

   397

    error_flag = (header >> 4) & 1;

   398

   399

    if (opcode != OPCODE_READ_RESPONSE) {

   400

        EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n",

   401

                opcode);

   402

        ec_print_data(data, rec_size);

   403

        ec_fsm_soe_print_error(fsm);

   404

        fsm->state = ec_fsm_soe_error;

   405

        return;

   406

    }

   407

   408

    if (error_flag) {

   409

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

   410

        EC_SLAVE_ERR(slave, "Received error response:\n");

   411

        ec_print_soe_error(slave, req->error_code);

   412

        ec_fsm_soe_print_error(fsm);

   413

        fsm->state = ec_fsm_soe_error;

   414

        return;

   415

    } else {

   416

        req->error_code = 0x0000;

   417

    }

   418

   419

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

   420

    if (!value_included) {

   421

        EC_SLAVE_ERR(slave, "No value included!\n");

   422

        ec_fsm_soe_print_error(fsm);

   423

        fsm->state = ec_fsm_soe_error;

   424

        return;

   425

    }

   426

   427

    data_size = rec_size - EC_SOE_SIZE;

   428

    if (ec_soe_request_append_data(req,

   429

                data + EC_SOE_SIZE, data_size)) {

   430

        fsm->state = ec_fsm_soe_error;

   431

        ec_fsm_soe_print_error(fsm);

   432

        return;

   433

    }

   434

   435

    if (incomplete) {

   436

        EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"

   437

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

   438

        fsm->jiffies_start = mbox->datagram->jiffies_sent;

   439

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

   440

        fsm->retries = EC_FSM_RETRIES;

   441

        fsm->state = ec_fsm_soe_read_check;

   442

    } else {

   443

        if (master->debug_level) {

   444

            EC_SLAVE_DBG(slave, 0, "IDN data:\n");

   445

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

   446

        }

   447

   448

        fsm->state = ec_fsm_soe_end; // success

   449

    }

   450

}

   451

   452

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

   453

 * SoE write state machine

   454

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

   455

   456

/** Write next fragment.

   457

 */

   458

void ec_fsm_soe_write_next_fragment(

   459

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

   460

        )

   461

{

   462

    ec_mailbox_t *mbox = fsm->mbox;

   463

    ec_slave_t *slave = fsm->slave;

   464

    ec_master_t *master = slave->master;

   465

    ec_soe_request_t *req = fsm->request;

   466

    uint8_t incomplete, *data;

   467

    size_t header_size, max_fragment_size, remaining_size, fragment_size;

   468

    uint16_t fragments_left;

   469

   470

    header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;

   471

    if (slave->configured_rx_mailbox_size <= header_size) {

   472

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

   473

                slave->configured_rx_mailbox_size);

   474

        fsm->state = ec_fsm_soe_error;

   475

        ec_fsm_soe_print_error(fsm);

   476

        return;

   477

    }

   478

   479

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

   480

    max_fragment_size = slave->configured_rx_mailbox_size - header_size;

   481

    incomplete = remaining_size > max_fragment_size;

   482

    fragment_size = incomplete ? max_fragment_size : remaining_size;

   483

    fragments_left = remaining_size / fragment_size - 1;

   484

    if (remaining_size % fragment_size) {

   485

        fragments_left++;

   486

    }

   487

   488

    data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,

   489

            EC_SOE_SIZE + fragment_size);

   490

    if (IS_ERR(data)) {

   491

        fsm->state = ec_fsm_soe_error;

   492

        ec_fsm_soe_print_error(fsm);

   493

        return;

   494

    }

   495

   496

    EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 |

   497

            (req->drive_no & 0x07) << 5);

   498

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

   499

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

   500

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

   501

    fsm->offset += fragment_size;