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 ms to wait for responses when reading out the dictionary.

    46

 */

    47

#define EC_FSM_FOE_TIMEOUT 3000

    48

    49

/** Size of the FoE header.

    50

 */

    51

#define EC_FOE_HEADER_SIZE 6

    52

// uint8_t  OpCode

    53

// uint8_t  reserved

    54

// uint32_t PacketNo, Password, ErrorCode

    55

    56

//#define DEBUG_FOE

    57

    58

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

    59

    60

/** FoE OpCodes.

    61

 */

    62

enum {

    63

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

    64

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

    65

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

    66

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

    67

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

    68

    EC_FOE_OPCODE_BUSY = 6  /**< Busy. */

    69

};

    70

    71

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

    72

    73

int ec_foe_prepare_data_send(ec_fsm_foe_t *, ec_datagram_t *);

    74

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *, ec_datagram_t *);

    75

int ec_foe_prepare_rrq_send(ec_fsm_foe_t *, ec_datagram_t *);

    76

int ec_foe_prepare_send_ack(ec_fsm_foe_t *, ec_datagram_t *);

    77

    78

void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);

    79

void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);

    80

    81

void ec_fsm_foe_end(ec_fsm_foe_t *, ec_datagram_t *);

    82

void ec_fsm_foe_error(ec_fsm_foe_t *, ec_datagram_t *);

    83

    84

void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *, ec_datagram_t *);

    85

void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *);

    86

    87

void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *);

    88

void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *);

    89

    90

void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *);

    91

    92

void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *);

    93

void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *);

    94

void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);

    95

    96

void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);

    97

void ec_fsm_foe_read_start(ec_fsm_foe_t *, ec_datagram_t *);

    98

    99

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

   100

   101

/** Constructor.

   102

 */

   103

void ec_fsm_foe_init(

   104

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

   105

        )

   106

{

   107

    fsm->state = NULL;

   108

    fsm->datagram = NULL;

   109

}

   110

   111

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

   112

   113

/** Destructor.

   114

 */

   115

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

   116

{

   117

}

   118

   119

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

   120

   121

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

   122

 *

   123

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

   124

 */

   125

int ec_fsm_foe_exec(

   126

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

   127

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

   128

        )

   129

{

   130

    int datagram_used = 0;

   131

   132

    if (fsm->datagram &&

   133

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

   134

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

   135

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

   136

        // datagram not received yet

   137

        return datagram_used;

   138

    }

   139

   140

    fsm->state(fsm, datagram);

   141

   142

    datagram_used =

   143

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

   144

   145

    if (datagram_used) {

   146

        fsm->datagram = datagram;

   147

    } else {

   148

        fsm->datagram = NULL;

   149

    }

   150

   151

    return datagram_used;

   152

}

   153

   154

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

   155

   156

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

   157

 * \return non-zero if successful.

   158

 */

   159

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

   160

{

   161

    return fsm->state == ec_fsm_foe_end;

   162

}

   163

   164

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

   165

   166

/** Prepares an FoE transfer.

   167

 */

   168

void ec_fsm_foe_transfer(

   169

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

   170

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

   171

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

   172

        )

   173

{

   174

    fsm->slave = slave;

   175

    fsm->request = request;

   176

   177

    if (request->dir == EC_DIR_OUTPUT) {

   178

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

   179

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

   180

        fsm->tx_buffer_offset = 0;

   181

   182

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

   183

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

   184

   185

        fsm->state = ec_fsm_foe_write_start;

   186

    }

   187

    else {

   188

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

   189

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

   190

   191

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

   192

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

   193

   194

        fsm->state = ec_fsm_foe_read_start;

   195

    }

   196

}

   197

   198

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

   199

   200

/** State: ERROR.

   201

 */

   202

void ec_fsm_foe_error(

   203

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

   204

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

   205

        )

   206

{

   207

#ifdef DEBUG_FOE

   208

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

   209

#endif

   210

}

   211

   212

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

   213

   214

/** State: END.

   215

 */

   216

void ec_fsm_foe_end(

   217

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

   218

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

   219

        )

   220

{

   221

#ifdef DEBUG_FOE

   222

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

   223

#endif

   224

}

   225

   226

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

   227

   228

/** Sends a file or the next fragment.

   229

 *

   230

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

   231

 */

   232

int ec_foe_prepare_data_send(

   233

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

   234

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

   235

        )

   236

{

   237

    size_t remaining_size, current_size;

   238

    uint8_t *data;

   239

   240

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

   241

   242

    if (remaining_size < fsm->slave->configured_tx_mailbox_size

   243

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   244

        current_size = remaining_size;

   245

        fsm->tx_last_packet = 1;

   246

    } else {

   247

        current_size = fsm->slave->configured_tx_mailbox_size

   248

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   249

    }

   250

   251

    data = ec_slave_mbox_prepare_send(fsm->slave,

   252

            datagram, EC_MBOX_TYPE_FOE, current_size + EC_FOE_HEADER_SIZE);

   253

    if (IS_ERR(data)) {

   254

        return -1;

   255

    }

   256

   257

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

   258

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

   259

   260

    memcpy(data + EC_FOE_HEADER_SIZE,

   261

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

   262

    fsm->tx_current_size = current_size;

   263

   264

    return 0;

   265

}

   266

   267

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

   268

   269

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

   270

 *

   271

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

   272

 */

   273

int ec_foe_prepare_wrq_send(

   274

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

   275

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

   276

        )

   277

{

   278

    size_t current_size;

   279

    uint8_t *data;

   280

   281

    fsm->tx_buffer_offset = 0;

   282

    fsm->tx_current_size = 0;

   283

    fsm->tx_packet_no = 0;

   284

    fsm->tx_last_packet = 0;

   285

   286

    current_size = fsm->tx_filename_len;

   287

   288

    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,

   289

            EC_MBOX_TYPE_FOE, current_size + EC_FOE_HEADER_SIZE);

   290

    if (IS_ERR(data)) {

   291

        return -1;

   292

    }

   293

   294

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

   295

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

   296

   297

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

   298

   299

    return 0;

   300

}

   301

   302

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

   303

   304

/** Initializes the FoE write state machine.

   305

 */

   306

void ec_fsm_foe_write_start(

   307

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

   308

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

   309

        )

   310

{

   311

    ec_slave_t *slave = fsm->slave;

   312

   313

    fsm->tx_buffer_offset = 0;

   314

    fsm->tx_current_size = 0;

   315

    fsm->tx_packet_no = 0;

   316

    fsm->tx_last_packet = 0;

   317

   318

#ifdef DEBUG_FOE

   319

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

   320

#endif

   321

   322

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

   323

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   324

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

   325

        return;

   326

    }

   327

   328

    if (ec_foe_prepare_wrq_send(fsm, datagram)) {

   329

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   330

        return;

   331

    }

   332

   333

    fsm->state = ec_fsm_foe_state_wrq_sent;

   334

}

   335

   336

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

   337

   338

/** Check for acknowledge.

   339

 */

   340

void ec_fsm_foe_state_ack_check(

   341

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

   342

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

   343

        )

   344

{

   345

    ec_slave_t *slave = fsm->slave;

   346

   347

#ifdef DEBUG_FOE

   348

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

   349

#endif

   350

   351

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

   352

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   353

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

   354

        ec_datagram_print_state(fsm->datagram);

   355

        return;

   356

    }

   357

   358

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

   359

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   360

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

   361

                " failed: ");

   362

        ec_datagram_print_wc_error(fsm->datagram);

   363

        return;

   364

    }

   365

   366

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

   367

        // slave did not put anything in the mailbox yet

   368

        unsigned long diff_ms = (fsm->datagram->jiffies_received -

   369

                fsm->jiffies_start) * 1000 / HZ;

   370

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   371

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   372

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

   373

            return;

   374

        }

   375

   376

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

   377

        fsm->retries = EC_FSM_RETRIES;

   378

        return;

   379

    }

   380

   381

    // Fetch response

   382

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

   383

   384

    fsm->retries = EC_FSM_RETRIES;

   385

    fsm->state = ec_fsm_foe_state_ack_read;

   386

}

   387

   388

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

   389

   390

/** Acknowledge a read operation.

   391

 */

   392

void ec_fsm_foe_state_ack_read(

   393

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

   394

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

   395

        )

   396

{

   397

    ec_slave_t *slave = fsm->slave;

   398

    uint8_t *data, mbox_prot;

   399

    uint8_t opCode;

   400

    size_t rec_size;

   401

   402

#ifdef DEBUG_FOE

   403

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

   404

#endif

   405

   406

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

   407

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   408

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

   409

        ec_datagram_print_state(fsm->datagram);

   410

        return;

   411

    }

   412

   413

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

   414

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   415

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

   416

        ec_datagram_print_wc_error(fsm->datagram);

   417

        return;

   418

    }

   419

   420

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

   421

    if (IS_ERR(data)) {

   422

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   423

        return;

   424

    }

   425

   426

    if (mbox_prot != EC_MBOX_TYPE_FOE) {

   427

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   428

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

   429

                mbox_prot);

   430

        return;

   431

    }

   432

   433

    opCode = EC_READ_U8(data);

   434

   435

    if (opCode == EC_FOE_OPCODE_BUSY) {

   436

        // slave not ready

   437

        if (ec_foe_prepare_data_send(fsm, datagram)) {

   438

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   439

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

   440

            return;

   441

        }

   442

        fsm->state = ec_fsm_foe_state_data_sent;

   443

        return;

   444

    }

   445

   446

    if (opCode == EC_FOE_OPCODE_ACK) {

   447

        fsm->tx_packet_no++;

   448

        fsm->tx_buffer_offset += fsm->tx_current_size;

   449

   450

        if (fsm->tx_last_packet) {

   451

            fsm->state = ec_fsm_foe_end;

   452

            return;

   453

        }

   454

   455

        if (ec_foe_prepare_data_send(fsm, datagram)) {

   456

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   457

            return;

   458

        }

   459

        fsm->state = ec_fsm_foe_state_data_sent;

   460

        return;

   461

    }

   462

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   463

}

   464

   465

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

   466

   467

/** State: WRQ SENT.

   468

 *

   469

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

   470

 * fragment, if necessary.

   471

 */

   472

void ec_fsm_foe_state_wrq_sent(

   473

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

   474

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

   475

        )

   476

{

   477

    ec_slave_t *slave = fsm->slave;

   478

   479

#ifdef DEBUG_FOE

   480

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

   481

#endif

   482

   483

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

   484

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   485

        EC_SLAVE_ERR(slave, "Failed to send FoE WRQ: ");

   486

        ec_datagram_print_state(fsm->datagram);

   487

        return;

   488

    }

   489

   490

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

   491

        // slave did not put anything in the mailbox yet

   492

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   493

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

   494

        ec_datagram_print_wc_error(fsm->datagram);

   495

        return;

   496

    }

   497