1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2008  Olav Zarges, imc Messsysteme GmbH

     6

 *                2013  Florian Pose <fp@igh-essen.com>

     7

 *

     8

 *  This file is part of the IgH EtherCAT Master.

     9

 *

    10

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    11

 *  modify it under the terms of the GNU General Public License version 2, as

    12

 *  published by the Free Software Foundation.

    13

 *

    14

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    15

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    17

 *  Public License for more details.

    18

 *

    19

 *  You should have received a copy of the GNU General Public License along

    20

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    21

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22

 *

    23

 *  ---

    24

 *

    25

 *  The license mentioned above concerns the source code only. Using the

    26

 *  EtherCAT technology and brand is only permitted in compliance with the

    27

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    28

 *

    29

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

    30

    31

/** \file

    32

 * EtherCAT FoE state machines.

    33

 */

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "fsm_foe.h"

    41

#include "foe.h"

    42

    43

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

    44

    45

/** Maximum time in jiffies to wait for responses when reading out the

    46

 * dictionary.

    47

 */

    48

#define EC_FSM_FOE_TIMEOUT_JIFFIES (3 * HZ)

    49

    50

/** Size of the FoE header.

    51

 */

    52

#define EC_FOE_HEADER_SIZE 6

    53

// uint8_t  OpCode

    54

// uint8_t  reserved

    55

// uint32_t PacketNo, Password, ErrorCode

    56

    57

//#define DEBUG_FOE

    58

    59

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

    60

    61

/** FoE OpCodes.

    62

 */

    63

enum {

    64

    EC_FOE_OPCODE_RRQ  = 1, /**< Read request. */

    65

    EC_FOE_OPCODE_WRQ  = 2, /**< Write request. */

    66

    EC_FOE_OPCODE_DATA = 3, /**< Data. */

    67

    EC_FOE_OPCODE_ACK  = 4, /**< Acknowledge. */

    68

    EC_FOE_OPCODE_ERR  = 5, /**< Error. */

    69

    EC_FOE_OPCODE_BUSY = 6  /**< Busy. */

    70

};

    71

    72

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

    73

    74

int ec_foe_prepare_data_send(ec_fsm_foe_t *, ec_datagram_t *);

    75

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *, ec_datagram_t *);

    76

int ec_foe_prepare_rrq_send(ec_fsm_foe_t *, ec_datagram_t *);

    77

int ec_foe_prepare_send_ack(ec_fsm_foe_t *, ec_datagram_t *);

    78

    79

void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);

    80

void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);

    81

    82

void ec_fsm_foe_end(ec_fsm_foe_t *, ec_datagram_t *);

    83

void ec_fsm_foe_error(ec_fsm_foe_t *, ec_datagram_t *);

    84

    85

void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *, ec_datagram_t *);

    86

void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *);

    87

    88

void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *);

    89

void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *);

    90

    91

void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *);

    92

    93

void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *);

    94

void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *);

    95

void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);

    96

    97

void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);

    98

void ec_fsm_foe_read_start(ec_fsm_foe_t *, ec_datagram_t *);

    99

   100

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

   101

   102

/** Constructor.

   103

 */

   104

void ec_fsm_foe_init(

   105

        ec_fsm_foe_t *fsm /**< finite state machine */

   106

        )

   107

{

   108

    fsm->state = NULL;

   109

    fsm->datagram = NULL;

   110

}

   111

   112

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

   113

   114

/** Destructor.

   115

 */

   116

void ec_fsm_foe_clear(ec_fsm_foe_t *fsm /**< finite state machine */)

   117

{

   118

}

   119

   120

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

   121

   122

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

   123

 *

   124

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

   125

 */

   126

int ec_fsm_foe_exec(

   127

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

   128

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

   129

        )

   130

{

   131

    int datagram_used = 0;

   132

   133

    if (fsm->datagram &&

   134

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

   135

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

   136

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

   137

        // datagram not received yet

   138

        return datagram_used;

   139

    }

   140

   141

    fsm->state(fsm, datagram);

   142

   143

    datagram_used =

   144

        fsm->state != ec_fsm_foe_end && fsm->state != ec_fsm_foe_error;

   145

   146

    if (datagram_used) {

   147

        fsm->datagram = datagram;

   148

    } else {

   149

        fsm->datagram = NULL;

   150

    }

   151

   152

    return datagram_used;

   153

}

   154

   155

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

   156

   157

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

   158

 * \return non-zero if successful.

   159

 */

   160

int ec_fsm_foe_success(const ec_fsm_foe_t *fsm /**< Finite state machine */)

   161

{

   162

    return fsm->state == ec_fsm_foe_end;

   163

}

   164

   165

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

   166

   167

/** Prepares an FoE transfer.

   168

 */

   169

void ec_fsm_foe_transfer(

   170

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

   171

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

   172

        ec_foe_request_t *request /**< Sdo request. */

   173

        )

   174

{

   175

    fsm->slave = slave;

   176

    fsm->request = request;

   177

   178

    if (request->dir == EC_DIR_OUTPUT) {

   179

        fsm->tx_buffer = fsm->request->buffer;

   180

        fsm->tx_buffer_size = fsm->request->data_size;

   181

        fsm->tx_buffer_offset = 0;

   182

   183

        fsm->tx_filename = fsm->request->file_name;

   184

        fsm->tx_filename_len = strlen(fsm->tx_filename);

   185

   186

        fsm->state = ec_fsm_foe_write_start;

   187

    }

   188

    else {

   189

        fsm->rx_buffer = fsm->request->buffer;

   190

        fsm->rx_buffer_size = fsm->request->buffer_size;

   191

   192

        fsm->rx_filename = fsm->request->file_name;

   193

        fsm->rx_filename_len = strlen(fsm->rx_filename);

   194

   195

        fsm->state = ec_fsm_foe_read_start;

   196

    }

   197

}

   198

   199

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

   200

   201

/** State: ERROR.

   202

 */

   203

void ec_fsm_foe_error(

   204

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

   205

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

   206

        )

   207

{

   208

#ifdef DEBUG_FOE

   209

    EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);

   210

#endif

   211

}

   212

   213

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

   214

   215

/** State: END.

   216

 */

   217

void ec_fsm_foe_end(

   218

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

   219

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

   220

        )

   221

{

   222

#ifdef DEBUG_FOE

   223

    EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);

   224

#endif

   225

}

   226

   227

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

   228

   229

/** Sends a file or the next fragment.

   230

 *

   231

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

   232

 */

   233

int ec_foe_prepare_data_send(

   234

        ec_fsm_foe_t *fsm, /**< Finite state machine. */

   235

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

   236

        )

   237

{

   238

    size_t remaining_size, current_size;

   239

    uint8_t *data;

   240

   241

    remaining_size = fsm->tx_buffer_size - fsm->tx_buffer_offset;

   242

   243

    if (remaining_size < fsm->slave->configured_tx_mailbox_size

   244

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   245

        current_size = remaining_size;

   246

        fsm->tx_last_packet = 1;

   247

    } else {

   248

        current_size = fsm->slave->configured_tx_mailbox_size

   249

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   250

    }

   251

   252

    data = ec_slave_mbox_prepare_send(fsm->slave,

   253

            datagram, EC_MBOX_TYPE_FOE, current_size + EC_FOE_HEADER_SIZE);

   254

    if (IS_ERR(data)) {

   255

        return -1;

   256

    }

   257

   258

    EC_WRITE_U16(data, EC_FOE_OPCODE_DATA);    // OpCode = DataBlock req.

   259

    EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password

   260

#ifdef DEBUG_FOE

   261

    EC_SLAVE_DBG(fsm->slave, 0, "sending opcode %u packet %u\n",

   262

            EC_FOE_OPCODE_DATA, fsm->tx_packet_no);

   263

#endif

   264

   265

    memcpy(data + EC_FOE_HEADER_SIZE,

   266

            fsm->tx_buffer + fsm->tx_buffer_offset, current_size);

   267

    fsm->tx_current_size = current_size;

   268

   269

    return 0;

   270

}

   271

   272

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

   273

   274

/** Prepare a write request (WRQ) with filename

   275

 *

   276

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

   277

 */

   278

int ec_foe_prepare_wrq_send(

   279

        ec_fsm_foe_t *fsm, /**< Finite state machine. */

   280

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

   281

        )

   282

{

   283

    size_t current_size;

   284

    uint8_t *data;

   285

   286

    fsm->tx_buffer_offset = 0;

   287

    fsm->tx_current_size = 0;

   288

    fsm->tx_packet_no = 0;

   289

    fsm->tx_last_packet = 0;

   290

   291

    current_size = fsm->tx_filename_len;

   292

   293

    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,

   294

            EC_MBOX_TYPE_FOE, current_size + EC_FOE_HEADER_SIZE);

   295

    if (IS_ERR(data)) {

   296

        return -1;

   297

    }

   298

   299

    EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request

   300

    EC_WRITE_U32( data + 2, fsm->tx_packet_no );

   301

#ifdef DEBUG_FOE

   302

    EC_SLAVE_DBG(fsm->slave, 0, "sending opcode %u packet %u\n",

   303

            EC_FOE_OPCODE_WRQ, fsm->tx_packet_no);

   304

#endif

   305

   306

    memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_filename, current_size);

   307

   308

    return 0;

   309

}

   310

   311

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

   312

   313

/** Initializes the FoE write state machine.

   314

 */

   315

void ec_fsm_foe_write_start(

   316

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

   317

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

   318

        )

   319

{

   320

    ec_slave_t *slave = fsm->slave;

   321

   322

    fsm->tx_buffer_offset = 0;

   323

    fsm->tx_current_size = 0;

   324

    fsm->tx_packet_no = 0;

   325

    fsm->tx_last_packet = 0;

   326

   327

#ifdef DEBUG_FOE

   328

    EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);

   329

#endif

   330

   331

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

   332

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   333

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

   334

        return;

   335

    }

   336

   337

    if (ec_foe_prepare_wrq_send(fsm, datagram)) {

   338

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   339

        return;

   340

    }

   341

   342

    fsm->state = ec_fsm_foe_state_wrq_sent;

   343

}

   344

   345

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

   346

   347

/** Check for acknowledge.

   348

 */

   349

void ec_fsm_foe_state_ack_check(

   350

        ec_fsm_foe_t *fsm, /**< FoE statemachine. */

   351

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

   352

        )

   353

{

   354

    ec_slave_t *slave = fsm->slave;

   355

   356

#ifdef DEBUG_FOE

   357

    EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);

   358

#endif

   359

   360

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

   361

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   362

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

   363

        ec_datagram_print_state(fsm->datagram);

   364

        return;

   365

    }

   366

   367

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

   368

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   369

        EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"

   370

                " failed: ");

   371

        ec_datagram_print_wc_error(fsm->datagram);

   372

        return;

   373

    }

   374

   375

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

   376

        // slave did not put anything in the mailbox yet

   377

        if (time_after(fsm->datagram->jiffies_received,

   378

                    fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) {

   379

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   380

            EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");

   381

            return;

   382

        }

   383

   384

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

   385

        fsm->retries = EC_FSM_RETRIES;

   386

        return;

   387

    }

   388

   389

    // Fetch response

   390

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

   391

   392

    fsm->retries = EC_FSM_RETRIES;

   393

    fsm->state = ec_fsm_foe_state_ack_read;

   394

}

   395

   396

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

   397

   398

/** Acknowledge a read operation.

   399

 */

   400

void ec_fsm_foe_state_ack_read(

   401

        ec_fsm_foe_t *fsm, /**< FoE statemachine. */

   402

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

   403

        )

   404

{

   405

    ec_slave_t *slave = fsm->slave;

   406

    uint8_t *data, mbox_prot;

   407

    uint8_t opCode;

   408

    size_t rec_size;

   409

   410

#ifdef DEBUG_FOE

   411

    EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);

   412

#endif

   413

   414

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

   415

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   416

        EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");

   417

        ec_datagram_print_state(fsm->datagram);

   418

        return;

   419

    }

   420

   421

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

   422

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   423

        EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");

   424

        ec_datagram_print_wc_error(fsm->datagram);

   425

        return;

   426

    }

   427

   428

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

   429

    if (IS_ERR(data)) {

   430

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   431

        return;

   432

    }

   433

   434

    if (mbox_prot != EC_MBOX_TYPE_FOE) {

   435

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   436

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

   437

                mbox_prot);

   438

        return;

   439

    }

   440

   441

    opCode = EC_READ_U8(data);

   442

#ifdef DEBUG_FOE

   443

    EC_SLAVE_DBG(fsm->slave, 0, "received opcode %u\n", opCode);

   444

#endif

   445

   446

    if (opCode == EC_FOE_OPCODE_BUSY) {

   447

        // slave not ready

   448

        if (ec_foe_prepare_data_send(fsm, datagram)) {

   449

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   450

            EC_SLAVE_ERR(slave, "Slave is busy.\n");

   451

            return;

   452

        }

   453

        fsm->state = ec_fsm_foe_state_data_sent;

   454

        return;

   455

    }

   456

   457

    if (opCode == EC_FOE_OPCODE_ERR) {

   458

        fsm->request->error_code = EC_READ_U32(data + 2);

   459

        EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n",

   460

                fsm->request->error_code);

   461

        if (rec_size > 6 && data[6]) {

   462

            uint8_t text[256];

   463

            strncpy(text, data + 6, min(rec_size - 6, sizeof(text)));

   464

            text[sizeof(text)-1] = 0;

   465

            EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text);

   466

        }

   467

        ec_foe_set_tx_error(fsm, FOE_OPCODE_ERROR);

   468

        return;

   469

    }

   470

   471

    if (opCode == EC_FOE_OPCODE_ACK) {

   472

        fsm->tx_packet_no++;

   473

        fsm->tx_buffer_offset += fsm->tx_current_size;

   474

   475

        if (fsm->tx_last_packet) {

   476

            fsm->state = ec_fsm_foe_end;

   477

            return;

   478

        }

   479

   480

        if (ec_foe_prepare_data_send(fsm, datagram)) {

   481

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   482

            return;

   483

        }

   484

        fsm->state = ec_fsm_foe_state_data_sent;

   485

        return;

   486

    }

   487

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   488

}

   489

   490

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

   491

   492

/** State: WRQ SENT.

   493

 *

   494

 * Checks is the previous transmit datagram succeded and sends the next

   495

 * fragment, if necessary.

   496

 */

   497

void ec_fsm_foe_state_wrq_sent(