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 header size.

    62

 */

    63

#define EC_SOE_HEADER_SIZE (EC_MBOX_HEADER_SIZE + EC_SOE_SIZE)

    64

    65

/** SoE response timeout [ms].

    66

 */

    67

#define EC_SOE_RESPONSE_TIMEOUT 1000

    68

    69

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

    70

    71

void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);

    72

void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);

    73

void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);

    74

void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);

    75

    76

void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);

    77

void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);

    78

void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);

    79

void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);

    80

    81

void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);

    82

void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);

    83

    84

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

    85

    86

extern const ec_code_msg_t soe_error_codes[];

    87

    88

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

    89

    90

/** Outputs an SoE error code.

    91

*/

    92

void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)

    93

{

    94

    const ec_code_msg_t *error_msg;

    95

    96

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

    97

        if (error_msg->code == error_code) {

    98

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

    99

                   error_msg->code, error_msg->message);

   100

            return;

   101

        }

   102

    }

   103

   104

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

   105

}

   106

   107

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

   108

   109

/** Constructor.

   110

 */

   111

void ec_fsm_soe_init(

   112

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

   113

        )

   114

{

   115

    fsm->state = NULL;

   116

    fsm->datagram = NULL;

   117

    fsm->fragment_size = 0;

   118

}

   119

   120

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

   121

   122

/** Destructor.

   123

 */

   124

void ec_fsm_soe_clear(

   125

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

   126

        )

   127

{

   128

}

   129

   130

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

   131

   132

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

   133

 */

   134

void ec_fsm_soe_transfer(

   135

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

   136

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

   137

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

   138

        )

   139

{

   140

    fsm->slave = slave;

   141

    fsm->request = request;

   142

   143

    if (request->dir == EC_DIR_OUTPUT) {

   144

        fsm->state = ec_fsm_soe_write_start;

   145

    } else {

   146

        fsm->state = ec_fsm_soe_read_start;

   147

    }

   148

}

   149

   150

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

   151

   152

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

   153

 *

   154

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

   155

 */

   156

int ec_fsm_soe_exec(

   157

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

   158

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

   159

        )

   160

{

   161

    int datagram_used = 0;

   162

   163

    if (fsm->datagram &&

   164

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

   165

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

   166

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

   167

        // datagram not received yet

   168

        return datagram_used;

   169

    }

   170

   171

    fsm->state(fsm, datagram);

   172

   173

    datagram_used =

   174

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

   175

   176

    if (datagram_used) {

   177

        fsm->datagram = datagram;

   178

    } else {

   179

        fsm->datagram = NULL;

   180

    }

   181

   182

    return datagram_used;

   183

}

   184

   185

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

   186

   187

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

   188

 *

   189

 * \return non-zero if successful.

   190

 */

   191

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

   192

{

   193

    return fsm->state == ec_fsm_soe_end;

   194

}

   195

   196

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

   197

   198

/** Output information about a failed SoE transfer.

   199

 */

   200

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

   201

{

   202

    ec_soe_request_t *request = fsm->request;

   203

   204

    EC_SLAVE_ERR(fsm->slave, "");

   205

   206

    if (request->dir == EC_DIR_OUTPUT) {

   207

        printk("Writing");

   208

    } else {

   209

        printk("Reading");

   210

    }

   211

   212

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

   213

}

   214

   215

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

   216

 * SoE read state machine

   217

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

   218

   219

/** Prepare a read operation.

   220

 *

   221

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

   222

 */

   223

int ec_fsm_soe_prepare_read(

   224

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

   225

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

   226

        )

   227

{

   228

    uint8_t *data;

   229

    ec_slave_t *slave = fsm->slave;

   230

    ec_master_t *master = slave->master;

   231

    ec_soe_request_t *request = fsm->request;

   232

   233

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   234

            EC_SOE_SIZE);

   235

    if (IS_ERR(data)) {

   236

        return PTR_ERR(data);

   237

    }

   238

   239

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

   240

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

   241

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

   242

   243

    if (master->debug_level) {

   244

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

   245

        ec_print_data(data, EC_SOE_SIZE);

   246

    }

   247

   248

    fsm->request->jiffies_sent = jiffies;

   249

    fsm->state = ec_fsm_soe_read_request;

   250

   251

    return 0;

   252

}

   253

   254

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

   255

   256

/** SoE state: READ START.

   257

 */

   258

void ec_fsm_soe_read_start(

   259

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

   260

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

   261

        )

   262

{

   263

    ec_slave_t *slave = fsm->slave;

   264

    ec_soe_request_t *request = fsm->request;

   265

   266

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

   267

            request->drive_no);

   268

   269

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

   270

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

   271

        fsm->state = ec_fsm_soe_error;

   272

        ec_fsm_soe_print_error(fsm);

   273

        return;

   274

    }

   275

   276

    request->data_size = 0;

   277

    fsm->retries = EC_FSM_RETRIES;

   278

   279

    if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   280

        fsm->state = ec_fsm_soe_error;

   281

        ec_fsm_soe_print_error(fsm);

   282

    }

   283

}

   284

   285

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

   286

   287

/** SoE state: READ REQUEST.

   288

 */

   289

void ec_fsm_soe_read_request(

   290

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

   291

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

   292

        )

   293

{

   294

    ec_slave_t *slave = fsm->slave;

   295

    unsigned long diff_ms;

   296

   297

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

   298

        if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   299

            fsm->state = ec_fsm_soe_error;

   300

            ec_fsm_soe_print_error(fsm);

   301

        }

   302

        return;

   303

    }

   304

   305

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

   306

        fsm->state = ec_fsm_soe_error;

   307

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

   308

        ec_datagram_print_state(fsm->datagram);

   309

        ec_fsm_soe_print_error(fsm);

   310

        return;

   311

    }

   312

   313

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

   314

   315

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

   316

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

   317

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   318

                // no response; send request datagram again

   319

                if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   320

                    fsm->state = ec_fsm_soe_error;

   321

                    ec_fsm_soe_print_error(fsm);

   322

                }

   323

                return;

   324

            }

   325

        }

   326

        fsm->state = ec_fsm_soe_error;

   327

        EC_SLAVE_ERR(slave, "Reception of SoE read request"

   328

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

   329

        ec_datagram_print_wc_error(fsm->datagram);

   330

        ec_fsm_soe_print_error(fsm);

   331

        return;

   332

    }

   333

   334

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

   335

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

   336

    fsm->retries = EC_FSM_RETRIES;

   337

    fsm->state = ec_fsm_soe_read_check;

   338

}

   339

   340

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

   341

   342

/** CoE state: READ CHECK.

   343

 */

   344

void ec_fsm_soe_read_check(

   345

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

   346

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

   347

        )

   348

{

   349

    ec_slave_t *slave = fsm->slave;

   350

   351

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

   352

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

   353

        return;

   354

    }

   355

   356

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

   357

        fsm->state = ec_fsm_soe_error;

   358

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

   359

        ec_datagram_print_state(fsm->datagram);

   360

        ec_fsm_soe_print_error(fsm);

   361

        return;

   362

    }

   363

   364

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

   365

        fsm->state = ec_fsm_soe_error;

   366

        EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"

   367

                " datagram failed: ");

   368

        ec_datagram_print_wc_error(fsm->datagram);

   369

        ec_fsm_soe_print_error(fsm);

   370

        return;

   371

    }

   372

   373

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

   374

        unsigned long diff_ms =

   375

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

   376

            1000 / HZ;

   377

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   378

            fsm->state = ec_fsm_soe_error;

   379

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

   380

                    " read response.\n", diff_ms);

   381

            ec_fsm_soe_print_error(fsm);

   382

            return;

   383

        }

   384

   385

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

   386

        fsm->retries = EC_FSM_RETRIES;

   387

        return;

   388

    }

   389

   390

    // Fetch response

   391

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

   392

    fsm->retries = EC_FSM_RETRIES;

   393

    fsm->state = ec_fsm_soe_read_response;

   394

}

   395

   396

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

   397

   398

/** SoE state: READ RESPONSE.

   399

 */

   400

void ec_fsm_soe_read_response(

   401

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

   402

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

   403

        )

   404

{

   405

    ec_slave_t *slave = fsm->slave;

   406

    ec_master_t *master = slave->master;

   407

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

   408

            value_included;

   409

    size_t rec_size, data_size;

   410

    ec_soe_request_t *req = fsm->request;

   411

   412

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

   413

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

   414

        return;

   415

    }

   416

   417

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

   418

        fsm->state = ec_fsm_soe_error;

   419

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

   420

        ec_datagram_print_state(fsm->datagram);

   421

        ec_fsm_soe_print_error(fsm);

   422

        return;

   423

    }

   424

   425

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

   426

        fsm->state = ec_fsm_soe_error;

   427

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

   428

        ec_datagram_print_wc_error(fsm->datagram);

   429

        ec_fsm_soe_print_error(fsm);

   430

        return;

   431

    }

   432

   433

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

   434

    if (IS_ERR(data)) {

   435

        fsm->state = ec_fsm_soe_error;

   436

        ec_fsm_soe_print_error(fsm);

   437

        return;

   438

    }

   439

   440

    if (master->debug_level) {

   441

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

   442

        ec_print_data(data, rec_size);

   443

    }

   444

   445

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   446

        fsm->state = ec_fsm_soe_error;

   447

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

   448

                mbox_prot);

   449

        ec_fsm_soe_print_error(fsm);

   450

        return;

   451

    }

   452

   453

    if (rec_size < EC_SOE_SIZE) {

   454

        fsm->state = ec_fsm_soe_error;

   455

        EC_SLAVE_ERR(slave, "Received currupted SoE read response"

   456

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

   457

        ec_print_data(data, rec_size);

   458

        ec_fsm_soe_print_error(fsm);

   459

        return;

   460

    }

   461

   462

    header = EC_READ_U8(data);

   463

    opcode = header & 0x7;

   464

    incomplete = (header >> 3) & 1;

   465

    error_flag = (header >> 4) & 1;

   466

   467

    if (opcode != OPCODE_READ_RESPONSE) {

   468

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

   469

                opcode);

   470

        ec_print_data(data, rec_size);

   471

        ec_fsm_soe_print_error(fsm);

   472

        fsm->state = ec_fsm_soe_error;

   473

        return;

   474

    }

   475

   476

    if (error_flag) {

   477

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

   478

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

   479

        ec_print_soe_error(slave, req->error_code);

   480

        ec_fsm_soe_print_error(fsm);

   481

        fsm->state = ec_fsm_soe_error;

   482

        return;

   483

    } else {

   484

        req->error_code = 0x0000;

   485

    }

   486

   487

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

   488

    if (!value_included) {

   489

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

   490

        ec_fsm_soe_print_error(fsm);

   491

        fsm->state = ec_fsm_soe_error;

   492

        return;

   493

    }

   494

   495

    data_size = rec_size - EC_SOE_SIZE;

   496

    if (ec_soe_request_append_data(req,

   497

                data + EC_SOE_SIZE, data_size)) {

   498

        fsm->state = ec_fsm_soe_error;

   499

        ec_fsm_soe_print_error(fsm);

   500

        return;

   501