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

    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(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_ERR("SoE error 0x%04X: \"%s\".\n",

    95

                   error_msg->code, error_msg->message);

    96

            return;

    97

        }

    98

    }

    99

   100

    EC_ERR("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_ERR("");

   180

   181

    if (request->dir == EC_DIR_OUTPUT) {

   182

        printk("Writing");

   183

    } else {

   184

        printk("Reading");

   185

    }

   186

   187

    printk(" IDN 0x%04X failed on slave %u.\n",

   188

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

   189

}

   190

   191

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

   192

 * SoE read state machine

   193

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

   194

   195

/** SoE state: READ START.

   196

 */

   197

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

   198

{

   199

    ec_datagram_t *datagram = fsm->datagram;

   200

    ec_slave_t *slave = fsm->slave;

   201

    ec_master_t *master = slave->master;

   202

    ec_soe_request_t *request = fsm->request;

   203

    uint8_t *data;

   204

   205

    if (master->debug_level)

   206

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

   207

               request->idn, slave->ring_position);

   208

   209

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

   210

        EC_ERR("Slave does not support SoE!\n");

   211

        fsm->state = ec_fsm_soe_error;

   212

        ec_fsm_soe_print_error(fsm);

   213

        return;

   214

    }

   215

   216

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   218

    if (IS_ERR(data)) {

   219

        fsm->state = ec_fsm_soe_error;

   220

        ec_fsm_soe_print_error(fsm);

   221

        return;

   222

    }

   223

   225

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

   226

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

   227

   228

    if (master->debug_level) {

   229

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

   231

    }

   232

   233

    fsm->request->data_size = 0;

   234

    fsm->request->jiffies_sent = jiffies;

   235

    fsm->retries = EC_FSM_RETRIES;

   236

    fsm->state = ec_fsm_soe_read_request;

   237

}

   238

   239

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

   240

   241

/** SoE state: READ REQUEST.

   242

 */

   243

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

   244

{

   245

    ec_datagram_t *datagram = fsm->datagram;

   246

    ec_slave_t *slave = fsm->slave;

   247

    unsigned long diff_ms;

   248

   249

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

   250

        return; // FIXME: check for response first?

   251

   252

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   253

        fsm->state = ec_fsm_soe_error;

   254

        EC_ERR("Failed to receive SoE read request: ");

   255

        ec_datagram_print_state(datagram);

   256

        ec_fsm_soe_print_error(fsm);

   257

        return;

   258

    }

   259

   260

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

   261

   262

    if (datagram->working_counter != 1) {

   263

        if (!datagram->working_counter) {

   264

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   265

                // no response; send request datagram again

   266

                return;

   267

            }

   268

        }

   269

        fsm->state = ec_fsm_soe_error;

   270

        EC_ERR("Reception of SoE read request failed after %u ms: ",

   271

                (u32) diff_ms);

   272

        ec_datagram_print_wc_error(datagram);

   273

        ec_fsm_soe_print_error(fsm);

   274

        return;

   275

    }

   276

   277

    fsm->jiffies_start = datagram->jiffies_sent;

   278

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

   279

    fsm->retries = EC_FSM_RETRIES;

   280

    fsm->state = ec_fsm_soe_read_check;

   281

}

   282

   283

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

   284

   285

/** CoE state: READ CHECK.

   286

 */

   287

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

   288

{

   289

    ec_datagram_t *datagram = fsm->datagram;

   290

    ec_slave_t *slave = fsm->slave;

   291

   292

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

   293

        return;

   294

   295

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   296

        fsm->state = ec_fsm_soe_error;

   297

        EC_ERR("Failed to receive SoE mailbox check datagram: ");

   298

        ec_datagram_print_state(datagram);

   299

        ec_fsm_soe_print_error(fsm);

   300

        return;

   301

    }

   302

   303

    if (datagram->working_counter != 1) {

   304

        fsm->state = ec_fsm_soe_error;

   305

        EC_ERR("Reception of SoE mailbox check datagram failed: ");

   306

        ec_datagram_print_wc_error(datagram);

   307

        ec_fsm_soe_print_error(fsm);

   308

        return;

   309

    }

   310

   311

    if (!ec_slave_mbox_check(datagram)) {

   312

        unsigned long diff_ms =

   313

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

   314

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   315

            fsm->state = ec_fsm_soe_error;

   316

            EC_ERR("Timeout after %u ms while waiting for read response.\n",

   317

                    (u32) diff_ms);

   318

            ec_fsm_soe_print_error(fsm);

   319

            return;

   320

        }

   321

   322

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

   323

        fsm->retries = EC_FSM_RETRIES;

   324

        return;

   325

    }

   326

   327

    // Fetch response

   328

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

   329

    fsm->retries = EC_FSM_RETRIES;

   330

    fsm->state = ec_fsm_soe_read_response;

   331

}

   332

   333

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

   334

   335

/** SoE state: READ RESPONSE.

   336

 */

   337

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

   338

{

   339

    ec_datagram_t *datagram = fsm->datagram;

   340

    ec_slave_t *slave = fsm->slave;

   341

    ec_master_t *master = slave->master;

   342

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

   343

            value_included;

   344

    size_t rec_size, data_size;

   345

    ec_soe_request_t *req = fsm->request;

   346

   347

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

   348

        return; // FIXME: request again?

   349

   350

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   351

        fsm->state = ec_fsm_soe_error;

   352

        EC_ERR("Failed to receive SoE read response datagram: ");

   353

        ec_datagram_print_state(datagram);

   354

        ec_fsm_soe_print_error(fsm);

   355

        return;

   356

    }

   357

   358

    if (datagram->working_counter != 1) {

   359

        fsm->state = ec_fsm_soe_error;

   360

        EC_ERR("Reception of SoE read response failed: ");

   361

        ec_datagram_print_wc_error(datagram);

   362

        ec_fsm_soe_print_error(fsm);

   363

        return;

   364

    }

   365

   366

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

   367

    if (IS_ERR(data)) {

   368

        fsm->state = ec_fsm_soe_error;

   369

        ec_fsm_soe_print_error(fsm);

   370

        return;

   371

    }

   372

   373

    if (master->debug_level) {

   374

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

   375

        ec_print_data(data, rec_size);

   376

    }

   377

   378

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   379

        fsm->state = ec_fsm_soe_error;

   380

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

   381

        ec_fsm_soe_print_error(fsm);

   382

        return;

   383

    }

   384

   386

        fsm->state = ec_fsm_soe_error;

   387

        EC_ERR("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

   400

        EC_ERR("Received no read response (opcode %x).\n", 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_ERR("Received error response:\n");

   410

        ec_print_soe_error(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_ERR("No value included!\n");

   421

        ec_fsm_soe_print_error(fsm);

   422

        fsm->state = ec_fsm_soe_error;

   423

        return;

   424

    }

   425

   427

    if (ec_soe_request_append_data(req,

   429

        fsm->state = ec_fsm_soe_error;

   430

        ec_fsm_soe_print_error(fsm);

   431

        return;

   432

    }

   433

   434

    if (incomplete) {

   435

        if (master->debug_level) {

   436

            EC_DBG("SoE data incomplete. Waiting for fragment"

   437

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

   438

        }

   439

        fsm->jiffies_start = datagram->jiffies_sent;

   440

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

   441

        fsm->retries = EC_FSM_RETRIES;

   442

        fsm->state = ec_fsm_soe_read_check;

   443

    } else {

   444

        if (master->debug_level) {

   445

            EC_DBG("IDN data:\n");

   446

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

   447

        }

   448

   449

        fsm->state = ec_fsm_soe_end; // success

   450

    }

   451

}

   452

   453

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

   454

 * SoE write state machine

   455

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

   456

   457

/** Write next fragment.

   458

 */

   459

void ec_fsm_soe_write_next_fragment(

   460

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

   461

        )

   462

{

   463

    ec_datagram_t *datagram = fsm->datagram;

   464

    ec_slave_t *slave = fsm->slave;

   465

    ec_master_t *master = slave->master;

   466

    ec_soe_request_t *req = fsm->request;

   467

    uint8_t incomplete, *data;

   468

    size_t header_size, max_fragment_size, remaining_size, fragment_size;

   469

    uint16_t fragments_left;

   470

   472

    if (slave->configured_rx_mailbox_size <= header_size) {

   473

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

   474

                slave->configured_rx_mailbox_size);

   475

        fsm->state = ec_fsm_soe_error;

   476

        ec_fsm_soe_print_error(fsm);

   477

        return;

   478

    }

   479

   480

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

   481

    max_fragment_size = slave->configured_rx_mailbox_size - header_size;

   482

    incomplete = remaining_size > max_fragment_size;

   483

    fragment_size = incomplete ? max_fragment_size : remaining_size;

   484

    fragments_left = remaining_size / fragment_size - 1;

   485

    if (remaining_size % fragment_size) {

   486

        fragments_left++;

   487

    }

   488

   489

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   491

    if (IS_ERR(data)) {

   492

        fsm->state = ec_fsm_soe_error;

   493

        ec_fsm_soe_print_error(fsm);

   494

        return;

   495

    }

   496

   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;

   502

   503

    if (master->debug_level) {

   504

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

   506

    }

   507

   508

    req->jiffies_sent = jiffies;