1

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

     2

 *

     3

 *  $Id$

     4

 *

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

    12

 *

    17

 *

    20

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

    21

 *

    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

#define EC_MBOX_TYPE_FILEACCESS 0x04

    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

enum {

    61

    EC_FOE_OPCODE_RRQ  = 1,

    62

    EC_FOE_OPCODE_WRQ  = 2,

    63

    EC_FOE_OPCODE_DATA = 3,

    64

    EC_FOE_OPCODE_ACK  = 4,

    65

    EC_FOE_OPCODE_ERR  = 5,

    66

    EC_FOE_OPCODE_BUSY = 6

    67

} ec_foe_opcode_t;

    68

    69

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

    70

    71

int ec_foe_prepare_data_send(ec_fsm_foe_t *);

    72

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *);

    73

int ec_foe_prepare_rrq_send(ec_fsm_foe_t *);

    74

int ec_foe_prepare_send_ack(ec_fsm_foe_t *);

    75

    76

void ec_foe_set_tx_error(ec_fsm_foe_t *, uint32_t);

    77

void ec_foe_set_rx_error(ec_fsm_foe_t *, uint32_t);

    78

    79

void ec_fsm_foe_write(ec_fsm_foe_t *);

    80

void ec_fsm_foe_read(ec_fsm_foe_t *);

    81

void ec_fsm_foe_end(ec_fsm_foe_t *);

    82

void ec_fsm_foe_error(ec_fsm_foe_t *);

    83

    84

void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *);

    85

void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *);

    86

    87

void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *);

    88

void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *);

    89

    90

void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *);

    91

    92

void ec_fsm_foe_state_data_check(ec_fsm_foe_t *);

    93

void ec_fsm_foe_state_data_read(ec_fsm_foe_t *);

    94

void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *);

    95

    96

void ec_fsm_foe_write_start(ec_fsm_foe_t *);

    97

void ec_fsm_foe_read_start(ec_fsm_foe_t *);

    98

    99

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

   100

   101

/**

   102

   Constructor.

   103

*/

   104

   105

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

   106

                     ec_datagram_t *datagram /**< datagram */

   107

                     )

   108

{

   109

    fsm->state     = NULL;

   110

    fsm->datagram  = datagram;

   111

    fsm->rx_errors = 0;

   112

    fsm->tx_errors = 0;

   113

}

   114

   115

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

   116

   117

/**

   118

   Destructor.

   119

*/

   120

   121

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

   122

{

   123

}

   124

   125

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

   126

   127

/**

   128

   Executes the current state of the state machine.

   129

   \return false, if state machine has terminated

   130

*/

   131

   132

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

   133

{

   134

    fsm->state(fsm);

   135

   136

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

   137

}

   138

   139

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

   140

   141

/**

   142

   Returns, if the state machine terminated with success.

   143

   \return non-zero if successful.

   144

*/

   145

   146

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

   147

{

   148

    return fsm->state == ec_fsm_foe_end;

   149

}

   150

   151

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

   152

   153

void ec_fsm_foe_transfer(

   154

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

   155

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

   156

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

   157

        )

   158

{

   159

    fsm->slave = slave;

   160

    fsm->request = request;

   161

    if (request->dir == EC_DIR_OUTPUT) {

   162

        fsm->state = ec_fsm_foe_write;

   163

    }

   164

    else {

   165

        fsm->state = ec_fsm_foe_read;

   166

    }

   167

}

   168

   169

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

   170

   171

/**

   172

   State: ERROR.

   173

*/

   174

   175

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

   176

{

   177

#ifdef DEBUG_FOE

   178

    printk("ec_fsm_foe_error()\n");

   179

#endif

   180

}

   181

   182

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

   183

   184

/**

   185

   State: END.

   186

*/

   187

   188

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

   189

{

   190

#ifdef DEBUG_FOE

   191

    printk("ec_fsm_foe_end\n");

   192

#endif

   193

}

   194

   195

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

   196

/**

   197

   Sends a file or the next fragment.

   198

*/

   199

   200

int ec_foe_prepare_data_send( ec_fsm_foe_t *fsm ) {

   201

    size_t       remaining_size, current_size;

   202

    uint8_t*     data;

   203

   204

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

   205

   206

    if (remaining_size < fsm->slave->configured_tx_mailbox_size

   207

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   208

        current_size = remaining_size;

   209

        fsm->tx_last_packet = 1;

   210

    } else {

   211

        current_size = fsm->slave->configured_tx_mailbox_size

   212

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   213

    }

   214

   215

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

   216

                    EC_MBOX_TYPE_FILEACCESS,

   217

                    current_size + EC_FOE_HEADER_SIZE)))

   218

        return -1;

   219

   220

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

   221

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

   222

   223

    memcpy(data + EC_FOE_HEADER_SIZE,

   224

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

   225

    fsm->tx_current_size = current_size;

   226

   227

    return 0;

   228

}

   229

   230

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

   231

/**

   232

   Prepare a write request (WRQ) with filename

   233

*/

   234

   235

int ec_foe_prepare_wrq_send( ec_fsm_foe_t *fsm ) {

   236

    size_t current_size;

   237

    uint8_t *data;

   238

   239

    fsm->tx_buffer_offset = 0;

   240

    fsm->tx_current_size = 0;

   241

    fsm->tx_packet_no = 0;

   242

    fsm->tx_last_packet = 0;

   243

   244

    current_size = fsm->tx_filename_len;

   245

   246

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

   247

                    EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE)))

   248

        return -1;

   249

   250

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

   251

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

   252

   253

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

   254

   255

    return 0;

   256

}

   257

   258

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

   259

   260

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

   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 SII write state machine.

   275

*/

   276

   277

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

   278

{

   279

    ec_slave_t *slave = fsm->slave;

   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

#ifdef DEBUG_FOE

   287

    printk("ec_fsm_foe_write_start()\n");

   288

#endif

   289

   290

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

   291

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   292

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

   293

        return;

   294

    }

   295

   296

    if (ec_foe_prepare_wrq_send(fsm)) {

   297

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   298

        return;

   299

    }

   300

   301

    fsm->state = ec_fsm_foe_state_wrq_sent;

   302

}

   303

   304

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

   305

   306

void ec_fsm_foe_state_ack_check( ec_fsm_foe_t *fsm ) {

   307

    ec_datagram_t *datagram = fsm->datagram;

   308

    ec_slave_t *slave = fsm->slave;

   309

   310

#ifdef DEBUG_FOE

   311

    printk("ec_fsm_foe_ack_check()\n");

   312

#endif

   313

   314

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   315

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   316

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

   317

                " (datagram state %u).\n",

   318

               slave->ring_position, datagram->state);

   319

        return;

   320

    }

   321

   322

    if (datagram->working_counter != 1) {

   323

        // slave did not put anything in the mailbox yet

   324

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   325

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

   326

               slave->ring_position);

   327

        ec_datagram_print_wc_error(datagram);

   328

        return;

   329

    }

   330

   331

    if (!ec_slave_mbox_check(datagram)) {

   332

        unsigned long diff_ms =

   333

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

   334

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   335

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   336

            EC_ERR("Timeout while waiting for ack response "

   337

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

   338

            return;

   339

        }

   340

   341

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

   342

        fsm->retries = EC_FSM_RETRIES;

   343

        return;

   344

    }

   345

   346

    // Fetch response

   347

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

   348

   349

    fsm->retries = EC_FSM_RETRIES;

   350

    fsm->state = ec_fsm_foe_state_ack_read;

   351

}

   352

   353

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

   354

   355

void ec_fsm_foe_state_ack_read( ec_fsm_foe_t *fsm ) {

   356

   357

    ec_datagram_t *datagram = fsm->datagram;

   358

    ec_slave_t *slave = fsm->slave;

   359

    uint8_t *data, mbox_prot;

   360

    uint8_t opCode;

   361

    size_t rec_size;

   362

   363

#ifdef DEBUG_FOE

   364

    printk("ec_fsm_foe_ack_read()\n");

   365

#endif

   366

   367

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   368

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   369

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

   370

               " slave %u (datagram state %u).\n",

   371

               slave->ring_position, datagram->state);

   372

        return;

   373

    }

   374

   375

    if (datagram->working_counter != 1) {

   376

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   377

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

   378

                slave->ring_position);

   379

        ec_datagram_print_wc_error(datagram);

   380

        return;

   381

    }

   382

   383

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

   384

                    &mbox_prot, &rec_size))) {

   385

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   386

        return;

   387

    }

   388

   389

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   390

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   391

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

   392

        return;

   393

    }

   394

   395

    opCode = EC_READ_U8(data);

   396

   397

    if (opCode == EC_FOE_OPCODE_BUSY) {

   398

        // slave not ready

   399

        if (ec_foe_prepare_data_send(fsm)) {

   400

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   401

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

   402

            return;

   403

        }

   404

        fsm->state = ec_fsm_foe_state_data_sent;

   405

        return;

   406

    }

   407

   408

    if (opCode == EC_FOE_OPCODE_ACK) {

   409

        fsm->tx_packet_no++;

   410

        fsm->tx_buffer_offset += fsm->tx_current_size;

   411

   412

        if (fsm->tx_last_packet) {

   413

            fsm->state = ec_fsm_foe_end;

   414

            return;

   415

        }

   416

   417

        if (ec_foe_prepare_data_send(fsm)) {

   418

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   419

            return;

   420

        }

   421

        fsm->state = ec_fsm_foe_state_data_sent;

   422

        return;

   423

    }

   424

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   425

}

   426

   427

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

   428

/**

   429

   State: WRQ SENT.

   430

   Checks is the previous transmit datagram succeded and sends the next

   431

   fragment, if necessary.

   432

*/

   433

   434

void ec_fsm_foe_state_wrq_sent( ec_fsm_foe_t *fsm ) {

   435

    ec_datagram_t *datagram = fsm->datagram;

   436

    ec_slave_t *slave = fsm->slave;

   437

   438

#ifdef DEBUG_FOE

   439

    printk("ec_foe_state_sent_wrq()\n");

   440

#endif

   441

   442

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   443

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   444

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

   445

                " (datagram state %u).\n",

   446

                slave->ring_position, datagram->state);

   447

        return;

   448

    }

   449

   450

    if (datagram->working_counter != 1) {

   451

        // slave did not put anything in the mailbox yet

   452

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   453

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

   454

                slave->ring_position);

   455

        ec_datagram_print_wc_error(datagram);

   456

        return;

   457

    }

   458

   459

    fsm->jiffies_start = datagram->jiffies_sent;

   460

   461

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

   462

   463

    fsm->retries = EC_FSM_RETRIES;

   464

    fsm->state = ec_fsm_foe_state_ack_check;

   465

}

   466

   467

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

   468

/**

   469

   State: WRQ SENT.

   470

   Checks is the previous transmit datagram succeded and sends the next

   471

   fragment, if necessary.

   472

*/

   473

   474

void ec_fsm_foe_state_data_sent( ec_fsm_foe_t *fsm ) {

   475

    ec_datagram_t *datagram = fsm->datagram;

   476

    ec_slave_t *slave = fsm->slave;

   477

   478

#ifdef DEBUG_FOE

   479

    printk("ec_fsm_foe_state_data_sent()\n");

   480

#endif

   481

   482

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

   483

        ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);

   484

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

   485

               " slave %u (datagram state %u).\n",

   486

               slave->ring_position, datagram->state);

   487

        return;

   488

    }

   489

   490

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

   491

        ec_foe_set_tx_error(fsm, FOE_WC_ERROR);

   492

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

   493

                slave->ring_position);

   494

        ec_datagram_print_wc_error(datagram);

   495

        return;

   496

    }

   497

   498

    ec_slave_mbox_prepare_check(fsm->slave, fsm->datagram);

   499

    fsm->jiffies_start = jiffies;

   500

    fsm->retries = EC_FSM_RETRIES;

   501

    fsm->state = ec_fsm_foe_state_ack_check;

   502

}

   503

   504

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

   505

/**

   506

   Prepare a read request (RRQ) with filename

   507

*/

   508

   509

int ec_foe_prepare_rrq_send( ec_fsm_foe_t *fsm ) {

   510

    size_t current_size;

   511

    uint8_t *data;

   512

   513

    current_size = fsm->rx_filename_len;

   514