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_datagram_t *datagram /**< datagram */

   110

        )

   111

{

   112

    fsm->state = NULL;

   113

    fsm->datagram = datagram;

   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_datagram_t *datagram = fsm->datagram;

   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.\n", request->idn);

   205

   206

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

   207

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

   208

        fsm->state = ec_fsm_soe_error;

   209

        ec_fsm_soe_print_error(fsm);

   210

        return;

   211

    }

   212

   213

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   214

            EC_SOE_SIZE);

   215

    if (IS_ERR(data)) {

   216

        fsm->state = ec_fsm_soe_error;

   217

        ec_fsm_soe_print_error(fsm);

   218

        return;

   219

    }

   220

   221

    EC_WRITE_U8(data, OPCODE_READ_REQUEST);

   222

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

   223

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

   224

   225

    if (master->debug_level) {

   226

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

   227

        ec_print_data(data, EC_SOE_SIZE);

   228

    }

   229

   230

    fsm->request->data_size = 0;

   231

    fsm->request->jiffies_sent = jiffies;

   232

    fsm->retries = EC_FSM_RETRIES;

   233

    fsm->state = ec_fsm_soe_read_request;

   234

}

   235

   236

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

   237

   238

/** SoE state: READ REQUEST.

   239

 */

   240

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

   241

{

   242

    ec_datagram_t *datagram = fsm->datagram;

   243

    ec_slave_t *slave = fsm->slave;

   244

    unsigned long diff_ms;

   245

   246

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

   247

        return; // FIXME: check for response first?

   248

   249

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   250

        fsm->state = ec_fsm_soe_error;

   251

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

   252

        ec_datagram_print_state(datagram);

   253

        ec_fsm_soe_print_error(fsm);

   254

        return;

   255

    }

   256

   257

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

   258

   259

    if (datagram->working_counter != 1) {

   260

        if (!datagram->working_counter) {

   261

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   262

                // no response; send request datagram again

   263

                return;

   264

            }

   265

        }

   266

        fsm->state = ec_fsm_soe_error;

   267

        EC_SLAVE_ERR(slave, "Reception of SoE read request"

   268

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

   269

        ec_datagram_print_wc_error(datagram);

   270

        ec_fsm_soe_print_error(fsm);

   271

        return;

   272

    }

   273

   274

    fsm->jiffies_start = datagram->jiffies_sent;

   275

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

   276

    fsm->retries = EC_FSM_RETRIES;

   277

    fsm->state = ec_fsm_soe_read_check;

   278

}

   279

   280

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

   281

   282

/** CoE state: READ CHECK.

   283

 */

   284

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

   285

{

   286

    ec_datagram_t *datagram = fsm->datagram;

   287

    ec_slave_t *slave = fsm->slave;

   288

   289

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

   290

        return;

   291

   292

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   293

        fsm->state = ec_fsm_soe_error;

   294

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

   295

        ec_datagram_print_state(datagram);

   296

        ec_fsm_soe_print_error(fsm);

   297

        return;

   298

    }

   299

   300

    if (datagram->working_counter != 1) {

   301

        fsm->state = ec_fsm_soe_error;

   302

        EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"

   303

                " datagram failed: ");

   304

        ec_datagram_print_wc_error(datagram);

   305

        ec_fsm_soe_print_error(fsm);

   306

        return;

   307

    }

   308

   309

    if (!ec_slave_mbox_check(datagram)) {

   310

        unsigned long diff_ms =

   311

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

   312

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   313

            fsm->state = ec_fsm_soe_error;

   314

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

   315

                    " read response.\n", diff_ms);

   316

            ec_fsm_soe_print_error(fsm);

   317

            return;

   318

        }

   319

   320

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

   321

        fsm->retries = EC_FSM_RETRIES;

   322

        return;

   323

    }

   324

   325

    // Fetch response

   326

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

   327

    fsm->retries = EC_FSM_RETRIES;

   328

    fsm->state = ec_fsm_soe_read_response;

   329

}

   330

   331

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

   332

   333

/** SoE state: READ RESPONSE.

   334

 */

   335

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

   336

{

   337

    ec_datagram_t *datagram = fsm->datagram;

   338

    ec_slave_t *slave = fsm->slave;

   339

    ec_master_t *master = slave->master;

   340

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

   341

            value_included;

   342

    size_t rec_size, data_size;

   343

    ec_soe_request_t *req = fsm->request;

   344

   345

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

   346

        return; // FIXME: request again?

   347

   348

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   349

        fsm->state = ec_fsm_soe_error;

   350

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

   351

        ec_datagram_print_state(datagram);

   352

        ec_fsm_soe_print_error(fsm);

   353

        return;

   354

    }

   355

   356

    if (datagram->working_counter != 1) {

   357

        fsm->state = ec_fsm_soe_error;

   358

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

   359

        ec_datagram_print_wc_error(datagram);

   360

        ec_fsm_soe_print_error(fsm);

   361

        return;

   362

    }

   363

   364

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

   365

    if (IS_ERR(data)) {

   366

        fsm->state = ec_fsm_soe_error;

   367

        ec_fsm_soe_print_error(fsm);

   368

        return;

   369

    }

   370

   371

    if (master->debug_level) {

   372

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

   373

        ec_print_data(data, rec_size);

   374

    }

   375

   376

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   377

        fsm->state = ec_fsm_soe_error;

   378

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

   379

                mbox_prot);

   380

        ec_fsm_soe_print_error(fsm);

   381

        return;

   382

    }

   383

   384

    if (rec_size < EC_SOE_SIZE) {

   385

        fsm->state = ec_fsm_soe_error;

   386

        EC_SLAVE_ERR(slave, "Received currupted SoE read response"

   387

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

   388

        ec_print_data(data, rec_size);

   389

        ec_fsm_soe_print_error(fsm);

   390

        return;

   391

    }

   392

   393

    header = EC_READ_U8(data);

   394

    opcode = header & 0x7;

   395

    incomplete = (header >> 3) & 1;

   396

    error_flag = (header >> 4) & 1;

   397

   398

    if (opcode != OPCODE_READ_RESPONSE) {

   399

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

   400

                opcode);

   401

        ec_print_data(data, rec_size);

   402

        ec_fsm_soe_print_error(fsm);

   403

        fsm->state = ec_fsm_soe_error;

   404

        return;

   405

    }

   406

   407

    if (error_flag) {

   408

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

   409

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

   410

        ec_print_soe_error(slave, req->error_code);

   411

        ec_fsm_soe_print_error(fsm);

   412

        fsm->state = ec_fsm_soe_error;

   413

        return;

   414

    } else {

   415

        req->error_code = 0x0000;

   416

    }

   417

   418

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

   419

    if (!value_included) {

   420

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

   421

        ec_fsm_soe_print_error(fsm);

   422

        fsm->state = ec_fsm_soe_error;

   423

        return;

   424

    }

   425

   426

    data_size = rec_size - EC_SOE_SIZE;

   427

    if (ec_soe_request_append_data(req,

   428

                data + EC_SOE_SIZE, data_size)) {

   429

        fsm->state = ec_fsm_soe_error;

   430

        ec_fsm_soe_print_error(fsm);

   431

        return;

   432

    }

   433

   434

    if (incomplete) {

   435

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

   436

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

   437

        fsm->jiffies_start = datagram->jiffies_sent;

   438

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

   439

        fsm->retries = EC_FSM_RETRIES;

   440

        fsm->state = ec_fsm_soe_read_check;

   441

    } else {

   442

        if (master->debug_level) {

   443

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

   444

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

   445

        }

   446

   447

        fsm->state = ec_fsm_soe_end; // success

   448

    }

   449

}

   450

   451

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

   452

 * SoE write state machine

   453

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

   454

   455

/** Write next fragment.

   456

 */

   457

void ec_fsm_soe_write_next_fragment(

   458

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

   459

        )

   460

{

   461

    ec_datagram_t *datagram = fsm->datagram;

   462

    ec_slave_t *slave = fsm->slave;

   463

    ec_master_t *master = slave->master;

   464

    ec_soe_request_t *req = fsm->request;

   465

    uint8_t incomplete, *data;

   466

    size_t header_size, max_fragment_size, remaining_size, fragment_size;

   467

    uint16_t fragments_left;

   468

   469

    header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;

   470

    if (slave->configured_rx_mailbox_size <= header_size) {

   471

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

   472

                slave->configured_rx_mailbox_size);

   473

        fsm->state = ec_fsm_soe_error;

   474

        ec_fsm_soe_print_error(fsm);

   475

        return;

   476

    }

   477

   478

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

   479

    max_fragment_size = slave->configured_rx_mailbox_size - header_size;

   480

    incomplete = remaining_size > max_fragment_size;

   481

    fragment_size = incomplete ? max_fragment_size : remaining_size;

   482

    fragments_left = remaining_size / fragment_size - 1;

   483

    if (remaining_size % fragment_size) {

   484

        fragments_left++;

   485

    }

   486

   487

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   488

            EC_SOE_SIZE + fragment_size);

   489

    if (IS_ERR(data)) {

   490

        fsm->state = ec_fsm_soe_error;

   491

        ec_fsm_soe_print_error(fsm);

   492

        return;

   493

    }

   494

   495

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

   496

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

   497

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

   498

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

   499

    fsm->offset += fragment_size;

   500

   501

    if (master->debug_level) {

   502

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

   503

        ec_print_data(data, EC_SOE_SIZE + fragment_size);

   504

    }

   505

   506

    req->jiffies_sent = jiffies;