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

/** Mailbox type FoE.

    50

 */

    51

#define EC_MBOX_TYPE_FILEACCESS 0x04

    52

    53

/** Size of the FoE header.

    54

 */

    55

#define EC_FOE_HEADER_SIZE 6

    56

// uint8_t  OpCode

    57

// uint8_t  reserved

    58

// uint32_t PacketNo, Password, ErrorCode

    59

    60

//#define DEBUG_FOE

    61

    62

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

    63

    64

/** FoE OpCodes.

    65

 */

    66

enum {

    67

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

    68

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

    69

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

    70

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

    71

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

    72

    EC_FOE_OPCODE_BUSY = 6  /**< Busy. */

    73

} ec_foe_opcode_t;

    74

    75

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

    76

    77

int ec_foe_prepare_data_send(ec_fsm_foe_t *, ec_datagram_t *);

    78

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *, ec_datagram_t *);

    79

int ec_foe_prepare_rrq_send(ec_fsm_foe_t *, ec_datagram_t *);

    80

int ec_foe_prepare_send_ack(ec_fsm_foe_t *, ec_datagram_t *);

    81

    82

void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);

    83

void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);

    84

    85

void ec_fsm_foe_end(ec_fsm_foe_t *, ec_datagram_t *);

    86

void ec_fsm_foe_error(ec_fsm_foe_t *, ec_datagram_t *);

    87

    88

void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *, ec_datagram_t *);

    89

void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *);

    90

    91

void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *);

    92

void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *);

    93

    94

void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *);

    95

    96

void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *);

    97

void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *);

    98

void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);

    99

   100

void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);

   101

void ec_fsm_foe_read_start(ec_fsm_foe_t *, ec_datagram_t *);

   102

   103

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

   104

   105

/** Constructor.

   106

 */

   107

void ec_fsm_foe_init(

   108

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

   109

        )

   110

{

   111

    fsm->state = NULL;

   112

    fsm->datagram = NULL;

   113

}

   114

   115

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

   116

   117

/** Destructor.

   118

 */

   119

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

   120

{

   121

}

   122

   123

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

   124

   125

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

   126

 *

   127

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

   128

 */

   129

int ec_fsm_foe_exec(

   130

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

   131

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

   132

        )

   133

{

   134

    int datagram_used = 0;

   135

   136

    if (fsm->datagram &&

   137

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

   138

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

   139

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

   140

        // datagram not received yet

   141

        return datagram_used;

   142

    }

   143

   144

    fsm->state(fsm, datagram);

   145

   146

    datagram_used =

   147

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

   148

   149

    if (datagram_used) {

   150

        fsm->datagram = datagram;

   151

    } else {

   152

        fsm->datagram = NULL;

   153

    }

   154

   155

    return datagram_used;

   156

}

   157

   158

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

   159

   160

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

   161

 * \return non-zero if successful.

   162

 */

   163

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

   164

{

   165

    return fsm->state == ec_fsm_foe_end;

   166

}

   167

   168

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

   169

   170

/** Prepares an FoE transfer.

   171

 */

   172

void ec_fsm_foe_transfer(

   173

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

   174

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

   175

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

   176

        )

   177

{

   178

    fsm->slave = slave;

   179

    fsm->request = request;

   180

   181

    if (request->dir == EC_DIR_OUTPUT) {

   182

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

   183

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

   184

        fsm->tx_buffer_offset = 0;

   185

   186

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

   187

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

   188

   189

        fsm->state = ec_fsm_foe_write_start;

   190

    }

   191

    else {

   192

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

   193

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

   194

   195

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

   196

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

   197

   198

        fsm->state = ec_fsm_foe_read_start;

   199

    }

   200

}

   201

   202

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

   203

   204

/** State: ERROR.

   205

 */

   206

void ec_fsm_foe_error(

   207

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

   208

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

   209

        )

   210

{

   211

#ifdef DEBUG_FOE

   212

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

   213

#endif

   214

}

   215

   216

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

   217

   218

/** State: END.

   219

 */

   220

void ec_fsm_foe_end(

   221

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

   222

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

   223

        )

   224

{

   225

#ifdef DEBUG_FOE

   226

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

   227

#endif

   228

}

   229

   230

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

   231

   232

/** Sends a file or the next fragment.

   233

 *

   234

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

   235

 */

   236

int ec_foe_prepare_data_send(

   237

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

   238

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

   239

        )

   240

{

   241

    size_t remaining_size, current_size;

   242

    uint8_t *data;

   243

   244

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

   245

   246

    if (remaining_size < fsm->slave->configured_tx_mailbox_size

   247

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   248

        current_size = remaining_size;

   249

        fsm->tx_last_packet = 1;

   250

    } else {

   251

        current_size = fsm->slave->configured_tx_mailbox_size

   252

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   253

    }

   254

   255

    data = ec_slave_mbox_prepare_send(fsm->slave,

   256

            datagram, EC_MBOX_TYPE_FILEACCESS,

   257

            current_size + EC_FOE_HEADER_SIZE);

   258

    if (IS_ERR(data)) {

   259

        return -1;

   260

    }

   261

   262

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

   263

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

   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_FILEACCESS, 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

   302

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

   303

   304

    return 0;

   305

}

   306

   307

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

   308

   309

/** Initializes the FoE write state machine.

   310

 */

   311

void ec_fsm_foe_write_start(

   312

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

   313

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

   314

        )

   315

{

   316

    ec_slave_t *slave = fsm->slave;

   317

   318

    fsm->tx_buffer_offset = 0;

   319

    fsm->tx_current_size = 0;

   320

    fsm->tx_packet_no = 0;

   321

    fsm->tx_last_packet = 0;

   322

   323

#ifdef DEBUG_FOE

   324

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

   325

#endif

   326

   327

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

   328

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   329

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

   330

        return;

   331

    }

   332

   333

    if (ec_foe_prepare_wrq_send(fsm, datagram)) {

   334

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   335

        return;

   336

    }

   337

   338

    fsm->state = ec_fsm_foe_state_wrq_sent;

   339

}

   340

   341

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

   342

   343

/** Check for acknowledge.

   344

 */

   345

void ec_fsm_foe_state_ack_check(

   346

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

   347

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

   348

        )

   349

{

   350

    ec_slave_t *slave = fsm->slave;

   351

   352

#ifdef DEBUG_FOE

   353

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

   354

#endif

   355

   356

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

   357

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   358

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

   359

        ec_datagram_print_state(fsm->datagram);

   360

        return;

   361

    }

   362

   363

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

   364

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   365

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

   366

                " failed: ");

   367

        ec_datagram_print_wc_error(fsm->datagram);

   368

        return;

   369

    }

   370

   371

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

   372

        // slave did not put anything in the mailbox yet

   373

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

   374

                fsm->jiffies_start) * 1000 / HZ;

   375

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   376

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   377

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

   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

   389

    fsm->retries = EC_FSM_RETRIES;

   390

    fsm->state = ec_fsm_foe_state_ack_read;

   391

}

   392

   393

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

   394

   395

/** Acknowledge a read operation.

   396

 */

   397

void ec_fsm_foe_state_ack_read(

   398

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

   399

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

   400

        )

   401

{

   402

    ec_slave_t *slave = fsm->slave;

   403

    uint8_t *data, mbox_prot;

   404

    uint8_t opCode;

   405

    size_t rec_size;

   406

   407

#ifdef DEBUG_FOE

   408

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

   409

#endif

   410

   411

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

   412

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   413

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

   414

        ec_datagram_print_state(fsm->datagram);

   415

        return;

   416

    }

   417

   418

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

   419

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   420

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

   421

        ec_datagram_print_wc_error(fsm->datagram);

   422

        return;

   423

    }

   424

   425

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

   426

    if (IS_ERR(data)) {

   427

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   428

        return;

   429

    }

   430

   431

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   432

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   433

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

   434

                mbox_prot);

   435

        return;

   436

    }

   437

   438

    opCode = EC_READ_U8(data);

   439

   440

    if (opCode == EC_FOE_OPCODE_BUSY) {

   441

        // slave not ready

   442

        if (ec_foe_prepare_data_send(fsm, datagram)) {

   443

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   444

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

   445

            return;

   446

        }

   447

        fsm->state = ec_fsm_foe_state_data_sent;

   448

        return;

   449

    }

   450

   451

    if (opCode == EC_FOE_OPCODE_ACK) {

   452

        fsm->tx_packet_no++;

   453

        fsm->tx_buffer_offset += fsm->tx_current_size;

   454

   455

        if (fsm->tx_last_packet) {

   456

            fsm->state = ec_fsm_foe_end;

   457

            return;

   458

        }

   459

   460

        if (ec_foe_prepare_data_send(fsm, datagram)) {

   461

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   462

            return;

   463

        }

   464

        fsm->state = ec_fsm_foe_state_data_sent;

   465

        return;

   466

    }

   467

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   468

}

   469

   470

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

   471

   472

/** State: WRQ SENT.

   473

 *

   474

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

   475

 * fragment, if necessary.

   476

 */

   477

void ec_fsm_foe_state_wrq_sent(

   478

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

   479

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

   480

        )

   481

{

   482

    ec_slave_t *slave = fsm->slave;

   483

   484

#ifdef DEBUG_FOE

   485

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

   486

#endif

   487

   488

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

   489

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   490

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

   491

        ec_datagram_print_state(fsm->datagram);

   492

        return;

   493

    }

   494

   495

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

   496

        // slave did not put anything in the mailbox yet

   497

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);