1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

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

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

    78

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *);

    79

int ec_foe_prepare_rrq_send(ec_fsm_foe_t *);

    80

int ec_foe_prepare_send_ack(ec_fsm_foe_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_write(ec_fsm_foe_t *);

    86

void ec_fsm_foe_read(ec_fsm_foe_t *);

    87

void ec_fsm_foe_end(ec_fsm_foe_t *);

    88

void ec_fsm_foe_error(ec_fsm_foe_t *);

    89

    90

void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *);

    91

void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *);

    92

    93

void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *);

    94

void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *);

    95

    96

void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *);

    97

    98

void ec_fsm_foe_state_data_check(ec_fsm_foe_t *);

    99

void ec_fsm_foe_state_data_read(ec_fsm_foe_t *);

   100

void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *);

   101

   102

void ec_fsm_foe_write_start(ec_fsm_foe_t *);

   103

void ec_fsm_foe_read_start(ec_fsm_foe_t *);

   104

   105

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

   106

   107

/** Constructor.

   108

 */

   109

void ec_fsm_foe_init(ec_fsm_foe_t *fsm, /**< finite state machine */

   110

                     ec_datagram_t *datagram /**< datagram */

   111

                     )

   112

{

   113

    fsm->state     = NULL;

   114

    fsm->datagram  = datagram;

   115

}

   116

   117

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

   118

   119

/** Destructor.

   120

 */

   121

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

   122

{

   123

}

   124

   125

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

   126

   127

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

   128

 * \return false, if state machine has terminated

   129

 */

   130

int ec_fsm_foe_exec(ec_fsm_foe_t *fsm /**< finite state machine */)

   131

{

   132

    fsm->state(fsm);

   133

   134

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

   135

}

   136

   137

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

   138

   139

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

   140

 * \return non-zero if successful.

   141

 */

   142

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

   143

{

   144

    return fsm->state == ec_fsm_foe_end;

   145

}

   146

   147

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

   148

   149

/** Prepares an FoE transfer.

   150

 */

   151

void ec_fsm_foe_transfer(

   152

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

   153

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

   154

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

   155

        )

   156

{

   157

    fsm->slave = slave;

   158

    fsm->request = request;

   159

    if (request->dir == EC_DIR_OUTPUT) {

   160

        fsm->state = ec_fsm_foe_write;

   161

    }

   162

    else {

   163

        fsm->state = ec_fsm_foe_read;

   164

    }

   165

}

   166

   167

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

   168

   169

/** State: ERROR.

   170

 */

   171

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

   172

{

   173

#ifdef DEBUG_FOE

   174

    printk("ec_fsm_foe_error()\n");

   175

#endif

   176

}

   177

   178

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

   179

   180

/** State: END.

   181

 */

   182

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

   183

{

   184

#ifdef DEBUG_FOE

   185

    printk("ec_fsm_foe_end\n");

   186

#endif

   187

}

   188

   189

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

   190

   191

/** Sends a file or the next fragment.

   192

 */

   193

int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)

   194

{

   195

    size_t remaining_size, current_size;

   196

    uint8_t *data;

   197

   198

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

   199

   200

    if (remaining_size < fsm->slave->configured_tx_mailbox_size

   201

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   202

        current_size = remaining_size;

   203

        fsm->tx_last_packet = 1;

   204

    } else {

   205

        current_size = fsm->slave->configured_tx_mailbox_size

   206

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   207

    }

   208

   209

    data = ec_slave_mbox_prepare_send(fsm->slave,

   210

            fsm->datagram, EC_MBOX_TYPE_FILEACCESS,

   211

            current_size + EC_FOE_HEADER_SIZE);

   212

    if (IS_ERR(data))

   213

        return -1;

   214

   215

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

   216

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

   217

   218

    memcpy(data + EC_FOE_HEADER_SIZE,

   219

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

   220

    fsm->tx_current_size = current_size;

   221

   222

    return 0;

   223

}

   224

   225

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

   226

   227

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

   228

 */

   229

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)

   230

{

   231

    size_t current_size;

   232

    uint8_t *data;

   233

   234

    fsm->tx_buffer_offset = 0;

   235

    fsm->tx_current_size = 0;

   236

    fsm->tx_packet_no = 0;

   237

    fsm->tx_last_packet = 0;

   238

   239

    current_size = fsm->tx_filename_len;

   240

   241

    data = ec_slave_mbox_prepare_send(fsm->slave, fsm->datagram,

   242

            EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);

   243

    if (IS_ERR(data))

   244

        return -1;

   245

   246

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

   247

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

   248

   249

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

   250

   251

    return 0;

   252

}

   253

   254

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

   255

   256

/** Start a write operation.

   257

 */

   258

void ec_fsm_foe_write(

   259

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

   260

        )

   261

{

   262

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

   263

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

   264

    fsm->tx_buffer_offset = 0;

   265

   266

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

   267

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

   268

   269

    fsm->state = ec_fsm_foe_write_start;

   270

}

   271

   272

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

   273

   274

/** Initializes the FoE write state machine.

   275

 */

   276

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

   277

{

   278

    ec_slave_t *slave = fsm->slave;

   279

   280

    fsm->tx_buffer_offset = 0;

   281

    fsm->tx_current_size = 0;

   282

    fsm->tx_packet_no = 0;

   283

    fsm->tx_last_packet = 0;

   284

   285

#ifdef DEBUG_FOE

   286

    printk("ec_fsm_foe_write_start()\n");

   287

#endif

   288

   289

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

   290

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   291

        EC_ERR("Slave %u does not support FoE!\n", slave->ring_position);

   292

        return;

   293

    }

   294

   295

    if (ec_foe_prepare_wrq_send(fsm)) {

   296

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   297

        return;

   298

    }

   299

   300

    fsm->state = ec_fsm_foe_state_wrq_sent;

   301

}

   302

   303

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

   304

   305

/** Check for acknowledge.

   306

 */

   307

void ec_fsm_foe_state_ack_check(

   308

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

   309

        )

   310

{

   311

    ec_datagram_t *datagram = fsm->datagram;

   312

    ec_slave_t *slave = fsm->slave;

   313

   314

#ifdef DEBUG_FOE

   315

    printk("ec_fsm_foe_ack_check()\n");

   316

#endif

   317

   318

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   319

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   320

        EC_ERR("Failed to receive FoE mailbox check datagram for slave %u: ",

   321

               slave->ring_position);

   322

        ec_datagram_print_state(datagram);

   323

        return;

   324

    }

   325

   326

    if (datagram->working_counter != 1) {

   327

        // slave did not put anything in the mailbox yet

   328

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   329

        EC_ERR("Reception of FoE mailbox check datagram failed on slave %u: ",

   330

               slave->ring_position);

   331

        ec_datagram_print_wc_error(datagram);

   332

        return;

   333

    }

   334

   335

    if (!ec_slave_mbox_check(datagram)) {

   336

        unsigned long diff_ms =

   337

            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   338

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   339

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   340

            EC_ERR("Timeout while waiting for ack response "

   341

                    "on slave %u.\n", slave->ring_position);

   342

            return;

   343

        }

   344

   345

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

   346

        fsm->retries = EC_FSM_RETRIES;

   347

        return;

   348

    }

   349

   350

    // Fetch response

   351

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

   352

   353

    fsm->retries = EC_FSM_RETRIES;

   354

    fsm->state = ec_fsm_foe_state_ack_read;

   355

}

   356

   357

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

   358

   359

/** Acknowledge a read operation.

   360

 */

   361

void ec_fsm_foe_state_ack_read(

   362

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

   363

        )

   364

{

   365

    ec_datagram_t *datagram = fsm->datagram;

   366

    ec_slave_t *slave = fsm->slave;

   367

    uint8_t *data, mbox_prot;

   368

    uint8_t opCode;

   369

    size_t rec_size;

   370

   371

#ifdef DEBUG_FOE

   372

    printk("ec_fsm_foe_ack_read()\n");

   373

#endif

   374

   375

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   376

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   377

        EC_ERR("Failed to receive FoE ack response datagram for"

   378

               " slave %u: ", slave->ring_position);

   379

        ec_datagram_print_state(datagram);

   380

        return;

   381

    }

   382

   383

    if (datagram->working_counter != 1) {

   384

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   385

        EC_ERR("Reception of FoE ack response failed on slave %u: ",

   386

                slave->ring_position);

   387

        ec_datagram_print_wc_error(datagram);

   388

        return;

   389

    }

   390

   391

    if (!(data = ec_slave_mbox_fetch(fsm->slave, datagram,

   392

                    &mbox_prot, &rec_size))) {

   393

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   394

        return;

   395

    }

   396

   397

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   398

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   399

        EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   400

        return;

   401

    }

   402

   403

    opCode = EC_READ_U8(data);

   404

   405

    if (opCode == EC_FOE_OPCODE_BUSY) {

   406

        // slave not ready

   407

        if (ec_foe_prepare_data_send(fsm)) {

   408

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   409

            EC_ERR("Slave is busy.\n");

   410

            return;

   411

        }

   412

        fsm->state = ec_fsm_foe_state_data_sent;

   413

        return;

   414

    }

   415

   416

    if (opCode == EC_FOE_OPCODE_ACK) {

   417

        fsm->tx_packet_no++;

   418

        fsm->tx_buffer_offset += fsm->tx_current_size;

   419

   420

        if (fsm->tx_last_packet) {

   421

            fsm->state = ec_fsm_foe_end;

   422

            return;

   423

        }

   424

   425

        if (ec_foe_prepare_data_send(fsm)) {

   426

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   427

            return;

   428

        }

   429

        fsm->state = ec_fsm_foe_state_data_sent;

   430

        return;

   431

    }

   432

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   433

}

   434

   435

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

   436

   437

/** State: WRQ SENT.

   438

 *

   439

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

   440

 * fragment, if necessary.

   441

 */

   442

void ec_fsm_foe_state_wrq_sent(

   443

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

   444

        )

   445

{

   446

    ec_datagram_t *datagram = fsm->datagram;

   447

    ec_slave_t *slave = fsm->slave;

   448

   449

#ifdef DEBUG_FOE

   450

    printk("ec_foe_state_sent_wrq()\n");

   451

#endif

   452

   453

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   454

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   455

        EC_ERR("Failed to send FoE WRQ for slave %u: ",

   456

                slave->ring_position);

   457

        ec_datagram_print_state(datagram);

   458

        return;

   459

    }

   460

   461

    if (datagram->working_counter != 1) {

   462

        // slave did not put anything in the mailbox yet

   463

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   464

        EC_ERR("Reception of FoE WRQ failed on slave %u: ",

   465

                slave->ring_position);

   466

        ec_datagram_print_wc_error(datagram);

   467

        return;

   468

    }

   469

   470

    fsm->jiffies_start = datagram->jiffies_sent;

   471

   472

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

   473

   474

    fsm->retries = EC_FSM_RETRIES;

   475

    fsm->state = ec_fsm_foe_state_ack_check;

   476

}

   477

   478

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

   479

   480

/** State: WRQ SENT.

   481

 *

   482

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

   483

 * fragment, if necessary.

   484

 */

   485

void ec_fsm_foe_state_data_sent(

   486

        ec_fsm_foe_t *fsm /**< Foe statemachine. */

   487

        )

   488

{

   489

    ec_datagram_t *datagram = fsm->datagram;

   490

    ec_slave_t *slave = fsm->slave;

   491

   492

#ifdef DEBUG_FOE

   493

    printk("ec_fsm_foe_state_data_sent()\n");

   494

#endif

   495

   496

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

   497

        ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);

   498

        EC_ERR("Failed to receive FoE ack response datagram for"

   499

               " slave %u: ", slave->ring_position);

   500

        ec_datagram_print_state(datagram);

   501

        return;

   502

    }

   503

   504

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

   505

        ec_foe_set_tx_error(fsm, FOE_WC_ERROR);