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

/** SoE operations

    45

 */

    46

enum ec_soe_opcodes {

    47

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

    48

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

    49

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

    50

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

    51

};

    52

    53

/** Size of all SoE headers.

    54

 */

    55

#define EC_SOE_SIZE 0x04

    56

    57

/** SoE response timeout [ms].

    58

 */

    59

#define EC_SOE_RESPONSE_TIMEOUT 1000

    60

    61

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

    62

    63

void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);

    64

void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);

    65

void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);

    66

void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);

    67

    68

void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);

    69

void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);

    70

void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);

    71

void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);

    72

    73

void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);

    74

void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);

    75

    76

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

    77

    78

extern const ec_code_msg_t soe_error_codes[];

    79

    80

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

    81

    82

/** Outputs an SoE error code.

    83

*/

    84

void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)

    85

{

    86

    const ec_code_msg_t *error_msg;

    87

    88

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

    89

        if (error_msg->code == error_code) {

    90

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

    91

                   error_msg->code, error_msg->message);

    92

            return;

    93

        }

    94

    }

    95

    96

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

    97

}

    98

    99

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

   100

   101

/** Constructor.

   102

 */

   103

void ec_fsm_soe_init(

   104

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

   105

        )

   106

{

   107

    fsm->state = NULL;

   108

    fsm->datagram = NULL;

   109

    fsm->fragment_size = 0;

   110

}

   111

   112

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

   113

   114

/** Destructor.

   115

 */

   116

void ec_fsm_soe_clear(

   117

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

   118

        )

   119

{

   120

}

   121

   122

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

   123

   124

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

   125

 */

   126

void ec_fsm_soe_transfer(

   127

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

   128

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

   129

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

   130

        )

   131

{

   132

    fsm->slave = slave;

   133

    fsm->request = request;

   134

   135

    if (request->dir == EC_DIR_OUTPUT) {

   136

        fsm->state = ec_fsm_soe_write_start;

   137

    } else {

   138

        fsm->state = ec_fsm_soe_read_start;

   139

    }

   140

}

   141

   142

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

   143

   144

/** Executes the current state of the state machine.

   145

 *

   146

 * \return 1 if the datagram was used, else 0.

   147

 */

   148

int ec_fsm_soe_exec(

   149

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

   150

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

   151

        )

   152

{

   153

    int datagram_used = 0;

   154

   155

    if (fsm->datagram &&

   156

            (fsm->datagram->state == EC_DATAGRAM_INIT ||

   157

             fsm->datagram->state == EC_DATAGRAM_QUEUED ||

   158

             fsm->datagram->state == EC_DATAGRAM_SENT)) {

   159

        // datagram not received yet

   160

        return datagram_used;

   161

    }

   162

   163

    fsm->state(fsm, datagram);

   164

   165

    datagram_used =

   166

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

   167

   168

    if (datagram_used) {

   169

        fsm->datagram = datagram;

   170

    } else {

   171

        fsm->datagram = NULL;

   172

    }

   173

   174

    return datagram_used;

   175

}

   176

   177

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

   178

   179

/** Returns, if the state machine terminated with success.

   180

 *

   181

 * \return non-zero if successful.

   182

 */

   183

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

   184

{

   185

    return fsm->state == ec_fsm_soe_end;

   186

}

   187

   188

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

   189

   190

/** Output information about a failed SoE transfer.

   191

 */

   192

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

   193

{

   194

    ec_soe_request_t *request = fsm->request;

   195

   196

    EC_SLAVE_ERR(fsm->slave, "");

   197

   198

    if (request->dir == EC_DIR_OUTPUT) {

   199

        printk("Writing");

   200

    } else {

   201

        printk("Reading");

   202

    }

   203

   204

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

   205

}

   206

   207

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

   208

 * SoE read state machine

   209

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

   210

   211

/** Prepare a read operation.

   212

 *

   213

 * \return 0 on success, otherwise a negative error code.

   214

 */

   215

int ec_fsm_soe_prepare_read(

   216

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

   217

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

   218

        )

   219

{

   220

    uint8_t *data;

   221

    ec_slave_t *slave = fsm->slave;

   222

    ec_master_t *master = slave->master;

   223

    ec_soe_request_t *request = fsm->request;

   224

   225

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   226

            EC_SOE_SIZE);

   227

    if (IS_ERR(data)) {

   228

        return PTR_ERR(data);

   229

    }

   230

   231

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

   232

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

   233

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

   234

   235

    if (master->debug_level) {

   236

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

   237

        ec_print_data(data, EC_SOE_SIZE);

   238

    }

   239

   240

    fsm->request->jiffies_sent = jiffies;

   241

    fsm->state = ec_fsm_soe_read_request;

   242

   243

    return 0;

   244

}

   245

   246

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

   247

   248

/** SoE state: READ START.

   249

 */

   250

void ec_fsm_soe_read_start(

   251

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

   252

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

   253

        )

   254

{

   255

    ec_slave_t *slave = fsm->slave;

   256

    ec_soe_request_t *request = fsm->request;

   257

   258

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

   259

            request->drive_no);

   260

   261

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

   262

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

   263

        fsm->state = ec_fsm_soe_error;

   264

        ec_fsm_soe_print_error(fsm);

   265

        return;

   266

    }

   267

   268

    request->data_size = 0;

   269

    fsm->retries = EC_FSM_RETRIES;

   270

   271

    if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   272

        fsm->state = ec_fsm_soe_error;

   273

        ec_fsm_soe_print_error(fsm);

   274

    }

   275

}

   276

   277

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

   278

   279

/** SoE state: READ REQUEST.

   280

 */

   281

void ec_fsm_soe_read_request(

   282

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

   283

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

   284

        )

   285

{

   286

    ec_slave_t *slave = fsm->slave;

   287

    unsigned long diff_ms;

   288

   289

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

   290

        if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   291

            fsm->state = ec_fsm_soe_error;

   292

            ec_fsm_soe_print_error(fsm);

   293

        }

   294

        return;

   295

    }

   296

   297

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

   298

        fsm->state = ec_fsm_soe_error;

   299

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

   300

        ec_datagram_print_state(fsm->datagram);

   301

        ec_fsm_soe_print_error(fsm);

   302

        return;

   303

    }

   304

   305

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

   306

   307

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

   308

        if (!fsm->datagram->working_counter) {

   309

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   310

                // no response; send request datagram again

   311

                if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   312

                    fsm->state = ec_fsm_soe_error;

   313

                    ec_fsm_soe_print_error(fsm);

   314

                }

   315

                return;

   316

            }

   317

        }

   318

        fsm->state = ec_fsm_soe_error;

   319

        EC_SLAVE_ERR(slave, "Reception of SoE read request"

   320

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

   321

        ec_datagram_print_wc_error(fsm->datagram);

   322

        ec_fsm_soe_print_error(fsm);

   323

        return;

   324

    }

   325

   326

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

   327

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

   328

    fsm->retries = EC_FSM_RETRIES;

   329

    fsm->state = ec_fsm_soe_read_check;

   330

}

   331

   332

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

   333

   334

/** CoE state: READ CHECK.

   335

 */

   336

void ec_fsm_soe_read_check(

   337

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

   338

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

   339

        )

   340

{

   341

    ec_slave_t *slave = fsm->slave;

   342

   343

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

   344

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

   345

        return;

   346

    }

   347

   348

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

   349

        fsm->state = ec_fsm_soe_error;

   350

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

   351

        ec_datagram_print_state(fsm->datagram);

   352

        ec_fsm_soe_print_error(fsm);

   353

        return;

   354

    }

   355

   356

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

   357

        fsm->state = ec_fsm_soe_error;

   358

        EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"

   359

                " datagram failed: ");

   360

        ec_datagram_print_wc_error(fsm->datagram);

   361

        ec_fsm_soe_print_error(fsm);

   362

        return;

   363

    }

   364

   365

    if (!ec_slave_mbox_check(fsm->datagram)) {

   366

        unsigned long diff_ms =

   367

            (fsm->datagram->jiffies_received - fsm->jiffies_start) *

   368

            1000 / HZ;

   369

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   370

            fsm->state = ec_fsm_soe_error;

   371

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

   372

                    " read response.\n", diff_ms);

   373

            ec_fsm_soe_print_error(fsm);

   374

            return;

   375

        }

   376

   377

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

   378

        fsm->retries = EC_FSM_RETRIES;

   379

        return;

   380

    }

   381

   382

    // Fetch response

   383

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

   384

    fsm->retries = EC_FSM_RETRIES;

   385

    fsm->state = ec_fsm_soe_read_response;

   386

}

   387

   388

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

   389

   390

/** SoE state: READ RESPONSE.

   391

 */

   392

void ec_fsm_soe_read_response(

   393

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

   394

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

   395

        )

   396

{

   397

    ec_slave_t *slave = fsm->slave;

   398

    ec_master_t *master = slave->master;

   399

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

   400

            value_included;

   401

    size_t rec_size, data_size;

   402

    ec_soe_request_t *req = fsm->request;

   403

   404

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

   405

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

   406

        return;

   407

    }

   408

   409

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

   410

        fsm->state = ec_fsm_soe_error;

   411

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

   412

        ec_datagram_print_state(fsm->datagram);

   413

        ec_fsm_soe_print_error(fsm);

   414

        return;

   415

    }

   416

   417

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

   418

        fsm->state = ec_fsm_soe_error;

   419

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

   420

        ec_datagram_print_wc_error(fsm->datagram);

   421

        ec_fsm_soe_print_error(fsm);

   422

        return;

   423

    }

   424

   425

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

   426

    if (IS_ERR(data)) {

   427

        fsm->state = ec_fsm_soe_error;

   428

        ec_fsm_soe_print_error(fsm);

   429

        return;

   430

    }

   431

   432

    if (master->debug_level) {

   433

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

   434

        ec_print_data(data, rec_size);

   435

    }

   436

   437

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   438

        fsm->state = ec_fsm_soe_error;

   439

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

   440

                mbox_prot);

   441

        ec_fsm_soe_print_error(fsm);

   442

        return;

   443

    }

   444

   445

    if (rec_size < EC_SOE_SIZE) {

   446

        fsm->state = ec_fsm_soe_error;

   447

        EC_SLAVE_ERR(slave, "Received currupted SoE read response"

   448

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

   449

        ec_print_data(data, rec_size);

   450

        ec_fsm_soe_print_error(fsm);

   451

        return;

   452

    }

   453

   454

    header = EC_READ_U8(data);

   455

    opcode = header & 0x7;

   456

    incomplete = (header >> 3) & 1;

   457

    error_flag = (header >> 4) & 1;

   458

   459

    if (opcode != OPCODE_READ_RESPONSE) {

   460

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

   461

                opcode);

   462

        ec_print_data(data, rec_size);

   463

        ec_fsm_soe_print_error(fsm);

   464

        fsm->state = ec_fsm_soe_error;

   465

        return;

   466

    }

   467

   468

    if (error_flag) {

   469

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

   470

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

   471

        ec_print_soe_error(slave, req->error_code);

   472

        ec_fsm_soe_print_error(fsm);

   473

        fsm->state = ec_fsm_soe_error;

   474

        return;

   475

    } else {

   476

        req->error_code = 0x0000;

   477

    }

   478

   479

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

   480

    if (!value_included) {

   481

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

   482

        ec_fsm_soe_print_error(fsm);

   483

        fsm->state = ec_fsm_soe_error;

   484

        return;

   485

    }

   486

   487

    data_size = rec_size - EC_SOE_SIZE;

   488

    if (ec_soe_request_append_data(req,

   489

                data + EC_SOE_SIZE, data_size)) {

   490

        fsm->state = ec_fsm_soe_error;

   491

        ec_fsm_soe_print_error(fsm);

   492

        return;

   493

    }

   494

   495

    if (incomplete) {

   496

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

   497

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

   498

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

   499

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

   500