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_mailbox_t *mbox /**< mailbox */

   111

                     )

   112

{

   113

    fsm->state = NULL;

   114

    fsm->mbox  = mbox;

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

   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_SLAVE_ERR(slave, "Slave does not support FoE!\n");

   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_mailbox_t *mbox = fsm->mbox;

   312

    ec_datagram_t *datagram = mbox->datagram;

   313

    ec_slave_t *slave = fsm->slave;

   314

   315

#ifdef DEBUG_FOE

   316

    printk("ec_fsm_foe_ack_check()\n");

   317

#endif

   318

   319

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   320

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   321

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

   322

        ec_datagram_print_state(datagram);

   323

        return;

   324

    }

   325

   326

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   327

        // slave did not put anything in the mailbox yet

   328

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   329

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

   330

                " failed: ");

   331

        ec_datagram_print_wc_error(datagram);

   332

        return;

   333

    }

   334

   335

    if (!ec_slave_mbox_check(mbox)) {

   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_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");

   341

            return;

   342

        }

   343

   344

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

   345

        fsm->retries = EC_FSM_RETRIES;

   346

        return;

   347

    }

   348

   349

    // Fetch response

   350

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

   351

   352

    fsm->retries = EC_FSM_RETRIES;

   353

    fsm->state = ec_fsm_foe_state_ack_read;

   354

}

   355

   356

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

   357

   358

/** Acknowledge a read operation.

   359

 */

   360

void ec_fsm_foe_state_ack_read(

   361

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

   362

        )

   363

{

   364

    ec_mailbox_t *mbox = fsm->mbox;

   365

    ec_datagram_t *datagram = mbox->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 (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   376

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   377

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

   378

        ec_datagram_print_state(datagram);

   379

        return;

   380

    }

   381

   382

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   383

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   384

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

   385

        ec_datagram_print_wc_error(datagram);

   386

        return;

   387

    }

   388

   389

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

   390

                    &mbox_prot, &rec_size))) {

   391

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   392

        return;

   393

    }

   394

   395

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   396

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   397

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

   398

                mbox_prot);

   399

        return;

   400

    }

   401

   402

    opCode = EC_READ_U8(data);

   403

   404

    if (opCode == EC_FOE_OPCODE_BUSY) {

   405

        // slave not ready

   406

        if (ec_foe_prepare_data_send(fsm)) {

   407

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   408

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

   409

            return;

   410

        }

   411

        fsm->state = ec_fsm_foe_state_data_sent;

   412

        return;

   413

    }

   414

   415

    if (opCode == EC_FOE_OPCODE_ACK) {

   416

        fsm->tx_packet_no++;

   417

        fsm->tx_buffer_offset += fsm->tx_current_size;

   418

   419

        if (fsm->tx_last_packet) {

   420

            fsm->state = ec_fsm_foe_end;

   421

            return;

   422

        }

   423

   424

        if (ec_foe_prepare_data_send(fsm)) {

   425

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   426

            return;

   427

        }

   428

        fsm->state = ec_fsm_foe_state_data_sent;

   429

        return;

   430

    }

   431

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   432

}

   433

   434

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

   435

   436

/** State: WRQ SENT.

   437

 *

   438

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

   439

 * fragment, if necessary.

   440

 */

   441

void ec_fsm_foe_state_wrq_sent(

   442

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

   443

        )

   444

{

   445

    ec_mailbox_t *mbox = fsm->mbox;

   446

    ec_datagram_t *datagram = mbox->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 (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   454

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   455

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

   456

        ec_datagram_print_state(datagram);

   457

        return;

   458

    }

   459

   460

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   461

        // slave did not put anything in the mailbox yet

   462

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   463

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

   464

        ec_datagram_print_wc_error(datagram);

   465

        return;

   466

    }

   467

   468

    fsm->jiffies_start = datagram->jiffies_sent;

   469

   470

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

   471

   472

    fsm->retries = EC_FSM_RETRIES;

   473

    fsm->state = ec_fsm_foe_state_ack_check;

   474

}

   475

   476

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

   477

   478

/** State: WRQ SENT.

   479

 *

   480

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

   481

 * fragment, if necessary.

   482

 */

   483

void ec_fsm_foe_state_data_sent(

   484

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

   485

        )

   486

{

   487

    ec_mailbox_t *mbox = fsm->mbox;

   488

    ec_datagram_t *datagram = mbox->datagram;

   489

    ec_slave_t *slave = fsm->slave;

   490

   491

#ifdef DEBUG_FOE

   492

    printk("ec_fsm_foe_state_data_sent()\n");

   493

#endif

   494

   495

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   496

        ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);

   497

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

   498

        ec_datagram_print_state(datagram);

   499

        return;

   500

    }

   501