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

#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

/** 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

} ec_foe_opcode_t;

    70

    71

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

    72

    73

int ec_foe_prepare_data_send(ec_fsm_foe_t *);

    74

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *);

    75

int ec_foe_prepare_rrq_send(ec_fsm_foe_t *);

    76

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

    82

void ec_fsm_foe_read(ec_fsm_foe_t *);

    83

void ec_fsm_foe_end(ec_fsm_foe_t *);

    84

void ec_fsm_foe_error(ec_fsm_foe_t *);

    85

    86

void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *);

    87

void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *);

    88

    89

void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *);

    90

void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *);

    91

    92

void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *);

    93

    94

void ec_fsm_foe_state_data_check(ec_fsm_foe_t *);

    95

void ec_fsm_foe_state_data_read(ec_fsm_foe_t *);

    96

void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *);

    97

    98

void ec_fsm_foe_write_start(ec_fsm_foe_t *);

    99

void ec_fsm_foe_read_start(ec_fsm_foe_t *);

   100

   101

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

   102

   103

/**

   104

   Constructor.

   105

*/

   106

   107

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

   108

                     ec_datagram_t *datagram /**< datagram */

   109

                     )

   110

{

   111

    fsm->state     = NULL;

   112

    fsm->datagram  = datagram;

   113

    fsm->rx_errors = 0;

   114

    fsm->tx_errors = 0;

   115

}

   116

   117

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

   118

   119

/**

   120

   Destructor.

   121

*/

   122

   123

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

   124

{

   125

}

   126

   127

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

   128

   129

/**

   130

   Executes the current state of the state machine.

   131

   \return false, if state machine has terminated

   132

*/

   133

   134

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

   135

{

   136

    fsm->state(fsm);

   137

   138

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

   139

}

   140

   141

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

   142

   143

/**

   144

   Returns, if the state machine terminated with success.

   145

   \return non-zero if successful.

   146

*/

   147

   148

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

   149

{

   150

    return fsm->state == ec_fsm_foe_end;

   151

}

   152

   153

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

   154

   155

void ec_fsm_foe_transfer(

   156

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

   157

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

   158

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

   159

        )

   160

{

   161

    fsm->slave = slave;

   162

    fsm->request = request;

   163

    if (request->dir == EC_DIR_OUTPUT) {

   164

        fsm->state = ec_fsm_foe_write;

   165

    }

   166

    else {

   167

        fsm->state = ec_fsm_foe_read;

   168

    }

   169

}

   170

   171

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

   172

   173

/**

   174

   State: ERROR.

   175

*/

   176

   177

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

   178

{

   179

#ifdef DEBUG_FOE

   180

    printk("ec_fsm_foe_error()\n");

   181

#endif

   182

}

   183

   184

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

   185

   186

/**

   187

   State: END.

   188

*/

   189

   190

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

   191

{

   192

#ifdef DEBUG_FOE

   193

    printk("ec_fsm_foe_end\n");

   194

#endif

   195

}

   196

   197

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

   198

/**

   199

   Sends a file or the next fragment.

   200

*/

   201

   202

int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)

   203

{

   204

    size_t remaining_size, current_size;

   205

    uint8_t *data;

   206

   207

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

   208

   209

    if (remaining_size < fsm->slave->configured_tx_mailbox_size

   210

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   211

        current_size = remaining_size;

   212

        fsm->tx_last_packet = 1;

   213

    } else {

   214

        current_size = fsm->slave->configured_tx_mailbox_size

   215

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   216

    }

   217

   218

    data = ec_slave_mbox_prepare_send(fsm->slave,

   219

            fsm->datagram, EC_MBOX_TYPE_FILEACCESS,

   220

            current_size + EC_FOE_HEADER_SIZE);

   221

    if (IS_ERR(data))

   222

        return -1;

   223

   224

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

   225

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

   226

   227

    memcpy(data + EC_FOE_HEADER_SIZE,

   228

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

   229

    fsm->tx_current_size = current_size;

   230

   231

    return 0;

   232

}

   233

   234

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

   235

/**

   236

   Prepare a write request (WRQ) with filename

   237

*/

   238

   239

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)

   240

{

   241

    size_t current_size;

   242

    uint8_t *data;

   243

   244

    fsm->tx_buffer_offset = 0;

   245

    fsm->tx_current_size = 0;

   246

    fsm->tx_packet_no = 0;

   247

    fsm->tx_last_packet = 0;

   248

   249

    current_size = fsm->tx_filename_len;

   250

   251

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

   252

            EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);

   253

    if (IS_ERR(data))

   254

        return -1;

   255

   256

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

   257

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

   258

   259

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

   260

   261

    return 0;

   262

}

   263

   264

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

   265

   266

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

   267

{

   268

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

   269

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

   270

    fsm->tx_buffer_offset = 0;

   271

   272

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

   273

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

   274

   275

    fsm->state = ec_fsm_foe_write_start;

   276

}

   277

   278

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

   279

/**

   280

   Initializes the SII write state machine.

   281

*/

   282

   283

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

   284

{

   285

    ec_slave_t *slave = fsm->slave;

   286

   287

    fsm->tx_buffer_offset = 0;

   288

    fsm->tx_current_size = 0;

   289

    fsm->tx_packet_no = 0;

   290

    fsm->tx_last_packet = 0;

   291

   292

#ifdef DEBUG_FOE

   293

    printk("ec_fsm_foe_write_start()\n");

   294

#endif

   295

   296

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

   297

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   298

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

   299

        return;

   300

    }

   301

   302

    if (ec_foe_prepare_wrq_send(fsm)) {

   303

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   304

        return;

   305

    }

   306

   307

    fsm->state = ec_fsm_foe_state_wrq_sent;

   308

}

   309

   310

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

   311

   312

void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *fsm)

   313

{

   314

    ec_datagram_t *datagram = fsm->datagram;

   315

    ec_slave_t *slave = fsm->slave;

   316

   317

#ifdef DEBUG_FOE

   318

    printk("ec_fsm_foe_ack_check()\n");

   319

#endif

   320

   321

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   322

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   323

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

   324

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

   325

               slave->ring_position, datagram->state);

   326

        return;

   327

    }

   328

   329

    if (datagram->working_counter != 1) {

   330

        // slave did not put anything in the mailbox yet

   331

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   332

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

   333

               slave->ring_position);

   334

        ec_datagram_print_wc_error(datagram);

   335

        return;

   336

    }

   337

   338

    if (!ec_slave_mbox_check(datagram)) {

   339

        unsigned long diff_ms =

   340

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

   341

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   342

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   343

            EC_ERR("Timeout while waiting for ack response "

   344

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

   345

            return;

   346

        }

   347

   348

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

   349

        fsm->retries = EC_FSM_RETRIES;

   350

        return;

   351

    }

   352

   353

    // Fetch response

   354

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

   355

   356

    fsm->retries = EC_FSM_RETRIES;

   357

    fsm->state = ec_fsm_foe_state_ack_read;

   358

}

   359

   360

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

   361

   362

void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *fsm)

   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 (datagram state %u).\n",

   379

               slave->ring_position, datagram->state);

   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

   Checks is the previous transmit datagram succeded and sends the next

   439

   fragment, if necessary.

   440

*/

   441

   442

void ec_fsm_foe_state_wrq_sent(ec_fsm_foe_t *fsm)

   443

{

   444

    ec_datagram_t *datagram = fsm->datagram;

   445

    ec_slave_t *slave = fsm->slave;

   446

   447

#ifdef DEBUG_FOE

   448

    printk("ec_foe_state_sent_wrq()\n");

   449

#endif

   450

   451

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   452

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   453

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

   454

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

   455

                slave->ring_position, datagram->state);

   456

        return;

   457

    }

   458

   459

    if (datagram->working_counter != 1) {

   460

        // slave did not put anything in the mailbox yet

   461

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   462

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

   463

                slave->ring_position);

   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, datagram); // 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

   Checks is the previous transmit datagram succeded and sends the next

   480

   fragment, if necessary.

   481

*/

   482

   483

void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *fsm)

   484

{

   485

    ec_datagram_t *datagram = fsm->datagram;

   486

    ec_slave_t *slave = fsm->slave;

   487

   488

#ifdef DEBUG_FOE

   489

    printk("ec_fsm_foe_state_data_sent()\n");

   490

#endif

   491

   492

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

   493

        ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);

   494

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

   495

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

   496

               slave->ring_position, datagram->state);

   497

        return;

   498

    }

   499

   500

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

   501

        ec_foe_set_tx_error(fsm, FOE_WC_ERROR);

   502

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

   503

                slave->ring_position);

   504

        ec_datagram_print_wc_error(datagram);

   505

        return;

   506

    }

   507

   508

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

   509

    fsm->jiffies_start = jiffies;

   510

    fsm->retries = EC_FSM_RETRIES;

   511

    fsm->state = ec_fsm_foe_state_ack_check;

   512

}