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 *, ec_datagram_t *);

    68

void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);

    69

void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);

    70

void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);

    71

    72

void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);

    73

void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);

    74

void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);

    75

void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);

    76

    77

void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);

    78

void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_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

        )

   110

{

   111

    fsm->state = NULL;

   112

    fsm->datagram = NULL;

   113

    fsm->fragment_size = 0;

   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

   139

    if (request->dir == EC_DIR_OUTPUT) {

   140

        fsm->state = ec_fsm_soe_write_start;

   141

    } else {

   142

        fsm->state = ec_fsm_soe_read_start;

   143

    }

   144

}

   145

   146

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

   147

   148

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

   149

 *

   150

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

   151

 */

   152

int ec_fsm_soe_exec(

   153

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

   154

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

   155

        )

   156

{

   157

    int datagram_used = 0;

   158

   159

    if (fsm->datagram &&

   160

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

   161

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

   162

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

   163

        // datagram not received yet

   164

        return datagram_used;

   165

    }

   166

   167

    fsm->state(fsm, datagram);

   168

   169

    datagram_used =

   170

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

   171

   172

    if (datagram_used) {

   173

        fsm->datagram = datagram;

   174

    } else {

   175

        fsm->datagram = NULL;

   176

    }

   177

   178

    return datagram_used;

   179

}

   180

   181

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

   182

   183

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

   184

 *

   185

 * \return non-zero if successful.

   186

 */

   187

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

   188

{

   189

    return fsm->state == ec_fsm_soe_end;

   190

}

   191

   192

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

   193

   194

/** Output information about a failed SoE transfer.

   195

 */

   196

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

   197

{

   198

    ec_soe_request_t *request = fsm->request;

   199

   200

    EC_SLAVE_ERR(fsm->slave, "");

   201

   202

    if (request->dir == EC_DIR_OUTPUT) {

   203

        printk("Writing");

   204

    } else {

   205

        printk("Reading");

   206

    }

   207

   208

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

   209

}

   210

   211

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

   212

 * SoE read state machine

   213

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

   214

   215

/** Prepare a read operation.

   216

 *

   217

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

   218

 */

   219

int ec_fsm_soe_prepare_read(

   220

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

   221

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

   222

        )

   223

{

   224

    uint8_t *data;

   225

    ec_slave_t *slave = fsm->slave;

   226

    ec_master_t *master = slave->master;

   227

    ec_soe_request_t *request = fsm->request;

   228

   229

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   230

            EC_SOE_SIZE);

   231

    if (IS_ERR(data)) {

   232

        return PTR_ERR(data);

   233

    }

   234

   235

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

   236

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

   237

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

   238

   239

    if (master->debug_level) {

   240

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

   241

        ec_print_data(data, EC_SOE_SIZE);

   242

    }

   243

   244

    fsm->request->jiffies_sent = jiffies;

   245

    fsm->state = ec_fsm_soe_read_request;

   246

   247

    return 0;

   248

}

   249

   250

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

   251

   252

/** SoE state: READ START.

   253

 */

   254

void ec_fsm_soe_read_start(

   255

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

   256

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

   257

        )

   258

{

   259

    ec_slave_t *slave = fsm->slave;

   260

    ec_soe_request_t *request = fsm->request;

   261

   262

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

   263

            request->drive_no);

   264

   265

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

   266

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

   267

        fsm->state = ec_fsm_soe_error;

   268

        ec_fsm_soe_print_error(fsm);

   269

        return;

   270

    }

   271

   272

    request->data_size = 0;

   273

    fsm->retries = EC_FSM_RETRIES;

   274

   275

    if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   276

        fsm->state = ec_fsm_soe_error;

   277

        ec_fsm_soe_print_error(fsm);

   278

    }

   279

}

   280

   281

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

   282

   283

/** SoE state: READ REQUEST.

   284

 */

   285

void ec_fsm_soe_read_request(

   286

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

   287

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

   288

        )

   289

{

   290

    ec_slave_t *slave = fsm->slave;

   291

    unsigned long diff_ms;

   292

   293

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

   294

        if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   295

            fsm->state = ec_fsm_soe_error;

   296

            ec_fsm_soe_print_error(fsm);

   297

        }

   298

        return;

   299

    }

   300

   301

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

   302

        fsm->state = ec_fsm_soe_error;

   303

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

   304

        ec_datagram_print_state(fsm->datagram);

   305

        ec_fsm_soe_print_error(fsm);

   306

        return;

   307

    }

   308

   309

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

   310

   311

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

   312

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

   313

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   314

                // no response; send request datagram again

   315

                if (ec_fsm_soe_prepare_read(fsm, datagram)) {

   316

                    fsm->state = ec_fsm_soe_error;

   317

                    ec_fsm_soe_print_error(fsm);

   318

                }

   319

                return;

   320

            }

   321

        }

   322

        fsm->state = ec_fsm_soe_error;

   323

        EC_SLAVE_ERR(slave, "Reception of SoE read request"

   324

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

   325

        ec_datagram_print_wc_error(fsm->datagram);

   326

        ec_fsm_soe_print_error(fsm);

   327

        return;

   328

    }

   329

   330

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

   331

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

   332

    fsm->retries = EC_FSM_RETRIES;

   333

    fsm->state = ec_fsm_soe_read_check;

   334

}

   335

   336

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

   337

   338

/** CoE state: READ CHECK.

   339

 */

   340

void ec_fsm_soe_read_check(

   341

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

   342

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

   343

        )

   344

{

   345

    ec_slave_t *slave = fsm->slave;

   346

   347

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

   348

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

   349

        return;

   350

    }

   351

   352

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

   353

        fsm->state = ec_fsm_soe_error;

   354

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

   355

        ec_datagram_print_state(fsm->datagram);

   356

        ec_fsm_soe_print_error(fsm);

   357

        return;

   358

    }

   359

   360

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

   361

        fsm->state = ec_fsm_soe_error;

   362

        EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"

   363

                " datagram failed: ");

   364

        ec_datagram_print_wc_error(fsm->datagram);

   365

        ec_fsm_soe_print_error(fsm);

   366

        return;

   367

    }

   368

   369

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

   370

        unsigned long diff_ms =

   371

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

   372

            1000 / HZ;

   373

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   374

            fsm->state = ec_fsm_soe_error;

   375

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

   376

                    " read response.\n", diff_ms);

   377

            ec_fsm_soe_print_error(fsm);

   378

            return;

   379

        }

   380

   381

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

   382

        fsm->retries = EC_FSM_RETRIES;

   383

        return;

   384

    }

   385

   386

    // Fetch response

   387

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

   388

    fsm->retries = EC_FSM_RETRIES;

   389

    fsm->state = ec_fsm_soe_read_response;

   390

}

   391

   392

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

   393

   394

/** SoE state: READ RESPONSE.

   395

 */

   396

void ec_fsm_soe_read_response(

   397

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

   398

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

   399

        )

   400

{

   401

    ec_slave_t *slave = fsm->slave;

   402

    ec_master_t *master = slave->master;

   403

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

   404

            value_included;

   405

    size_t rec_size, data_size;

   406

    ec_soe_request_t *req = fsm->request;

   407

   408

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

   409

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

   410

        return;

   411

    }

   412

   413

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

   414

        fsm->state = ec_fsm_soe_error;

   415

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

   416

        ec_datagram_print_state(fsm->datagram);

   417

        ec_fsm_soe_print_error(fsm);

   418

        return;

   419

    }

   420

   421

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

   422

        fsm->state = ec_fsm_soe_error;

   423

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

   424

        ec_datagram_print_wc_error(fsm->datagram);

   425

        ec_fsm_soe_print_error(fsm);

   426

        return;

   427

    }

   428

   429

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

   430

    if (IS_ERR(data)) {

   431

        fsm->state = ec_fsm_soe_error;

   432

        ec_fsm_soe_print_error(fsm);

   433

        return;

   434

    }

   435

   436

    if (master->debug_level) {

   437

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

   438

        ec_print_data(data, rec_size);

   439

    }

   440

   441

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   442

        fsm->state = ec_fsm_soe_error;

   443

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

   444

                mbox_prot);

   445

        ec_fsm_soe_print_error(fsm);

   446

        return;

   447

    }

   448

   449

    if (rec_size < EC_SOE_SIZE) {

   450

        fsm->state = ec_fsm_soe_error;

   451

        EC_SLAVE_ERR(slave, "Received currupted SoE read response"

   452

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

   453

        ec_print_data(data, rec_size);

   454

        ec_fsm_soe_print_error(fsm);

   455

        return;

   456

    }

   457

   458

    header = EC_READ_U8(data);

   459

    opcode = header & 0x7;

   460

    incomplete = (header >> 3) & 1;

   461

    error_flag = (header >> 4) & 1;

   462

   463

    if (opcode != OPCODE_READ_RESPONSE) {

   464

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

   465

                opcode);

   466

        ec_print_data(data, rec_size);

   467

        ec_fsm_soe_print_error(fsm);

   468

        fsm->state = ec_fsm_soe_error;

   469

        return;

   470

    }

   471

   472

    if (error_flag) {

   473

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

   474

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

   475

        ec_print_soe_error(slave, req->error_code);

   476

        ec_fsm_soe_print_error(fsm);

   477

        fsm->state = ec_fsm_soe_error;

   478

        return;

   479

    } else {

   480

        req->error_code = 0x0000;

   481

    }

   482

   483

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

   484

    if (!value_included) {

   485

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

   486

        ec_fsm_soe_print_error(fsm);

   487

        fsm->state = ec_fsm_soe_error;

   488

        return;

   489

    }

   490

   491

    data_size = rec_size - EC_SOE_SIZE;

   492

    if (ec_soe_request_append_data(req,

   493

                data + EC_SOE_SIZE, data_size)) {

   494

        fsm->state = ec_fsm_soe_error;

   495

        ec_fsm_soe_print_error(fsm);

   496

        return;

   497

    }

   498

   499

    if (incomplete) {

   500

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