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

    51

#define EC_MBOX_TYPE_FILEACCESS 0x04

    52

    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

/**

   108

   Constructor.

   109

*/

   110

   111

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

   112

                     ec_datagram_t *datagram /**< datagram */

   113

                     )

   114

{

   115

    fsm->state     = NULL;

   116

    fsm->datagram  = datagram;

   117

    fsm->rx_errors = 0;

   118

    fsm->tx_errors = 0;

   119

}

   120

   121

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

   122

   123

/**

   124

   Destructor.

   125

*/

   126

   127

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

   128

{

   129

}

   130

   131

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

   132

   133

/**

   134

   Executes the current state of the state machine.

   135

   \return false, if state machine has terminated

   136

*/

   137

   138

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

   139

{

   140

    fsm->state(fsm);

   141

   142

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

   143

}

   144

   145

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

   146

   147

/**

   148

   Returns, if the state machine terminated with success.

   149

   \return non-zero if successful.

   150

*/

   151

   152

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

   153

{

   154

    return fsm->state == ec_fsm_foe_end;

   155

}

   156

   157

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

   158

   161

void ec_fsm_foe_transfer(

   162

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

   163

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

   164

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

   165

        )

   166

{

   167

    fsm->slave = slave;

   168

    fsm->request = request;

   169

    if (request->dir == EC_DIR_OUTPUT) {

   170

        fsm->state = ec_fsm_foe_write;

   171

    }

   172

    else {

   173

        fsm->state = ec_fsm_foe_read;

   174

    }

   175

}

   176

   177

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

   178

   179

/**

   180

   State: ERROR.

   181

*/

   182

   183

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

   184

{

   185

#ifdef DEBUG_FOE

   186

    printk("ec_fsm_foe_error()\n");

   187

#endif

   188

}

   189

   190

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

   191

   192

/**

   193

   State: END.

   194

*/

   195

   196

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

   197

{

   198

#ifdef DEBUG_FOE

   199

    printk("ec_fsm_foe_end\n");

   200

#endif

   201

}

   202

   203

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

   204

/**

   205

   Sends a file or the next fragment.

   206

*/

   207

   208

int ec_foe_prepare_data_send(ec_fsm_foe_t *fsm)

   209

{

   210

    size_t remaining_size, current_size;

   211

    uint8_t *data;

   212

   213

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

   214

   215

    if (remaining_size < fsm->slave->configured_tx_mailbox_size

   216

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   217

        current_size = remaining_size;

   218

        fsm->tx_last_packet = 1;

   219

    } else {

   220

        current_size = fsm->slave->configured_tx_mailbox_size

   221

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   222

    }

   223

   224

    data = ec_slave_mbox_prepare_send(fsm->slave,

   225

            fsm->datagram, EC_MBOX_TYPE_FILEACCESS,

   226

            current_size + EC_FOE_HEADER_SIZE);

   227

    if (IS_ERR(data))

   228

        return -1;

   229

   230

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

   231

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

   232

   233

    memcpy(data + EC_FOE_HEADER_SIZE,

   234

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

   235

    fsm->tx_current_size = current_size;

   236

   237

    return 0;

   238

}

   239

   240

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

   241

/**

   242

   Prepare a write request (WRQ) with filename

   243

*/

   244

   245

int ec_foe_prepare_wrq_send(ec_fsm_foe_t *fsm)

   246

{

   247

    size_t current_size;

   248

    uint8_t *data;

   249

   250

    fsm->tx_buffer_offset = 0;

   251

    fsm->tx_current_size = 0;

   252

    fsm->tx_packet_no = 0;

   253

    fsm->tx_last_packet = 0;

   254

   255

    current_size = fsm->tx_filename_len;

   256

   257

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

   258

            EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);

   259

    if (IS_ERR(data))

   260

        return -1;

   261

   262

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

   263

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

   264

   265

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

   266

   267

    return 0;

   268

}

   269

   270

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

   271

   277

{

   278

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

   279

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

   280

    fsm->tx_buffer_offset = 0;

   281

   282

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

   283

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

   284

   285

    fsm->state = ec_fsm_foe_write_start;

   286

}

   287

   288

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

   292

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

   293

{

   294

    ec_slave_t *slave = fsm->slave;

   295

   296

    fsm->tx_buffer_offset = 0;

   297

    fsm->tx_current_size = 0;

   298

    fsm->tx_packet_no = 0;

   299

    fsm->tx_last_packet = 0;

   300

   301

#ifdef DEBUG_FOE

   302

    printk("ec_fsm_foe_write_start()\n");

   303

#endif

   304

   305

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

   306

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   307

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

   308

        return;

   309

    }

   310

   311

    if (ec_foe_prepare_wrq_send(fsm)) {

   312

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   313

        return;

   314

    }

   315

   316

    fsm->state = ec_fsm_foe_state_wrq_sent;

   317

}

   318

   319

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

   320

   326

{

   327

    ec_datagram_t *datagram = fsm->datagram;

   328

    ec_slave_t *slave = fsm->slave;

   329

   330

#ifdef DEBUG_FOE

   331

    printk("ec_fsm_foe_ack_check()\n");

   332

#endif

   333

   334

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   335

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   336

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

   337

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

   338

               slave->ring_position, datagram->state);

   339

        return;

   340

    }

   341

   342

    if (datagram->working_counter != 1) {

   343

        // slave did not put anything in the mailbox yet

   344

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   345

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

   346

               slave->ring_position);

   347

        ec_datagram_print_wc_error(datagram);

   348

        return;

   349

    }

   350

   351

    if (!ec_slave_mbox_check(datagram)) {

   352

        unsigned long diff_ms =

   353

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

   354

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   355

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   356

            EC_ERR("Timeout while waiting for ack response "

   357

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

   358

            return;

   359

        }

   360

   361

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

   362

        fsm->retries = EC_FSM_RETRIES;

   363

        return;

   364

    }

   365

   366

    // Fetch response

   367

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

   368

   369

    fsm->retries = EC_FSM_RETRIES;

   370

    fsm->state = ec_fsm_foe_state_ack_read;

   371

}

   372

   373

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

   374

   381

    ec_datagram_t *datagram = fsm->datagram;

   382

    ec_slave_t *slave = fsm->slave;

   383

    uint8_t *data, mbox_prot;

   384

    uint8_t opCode;

   385

    size_t rec_size;

   386

   387

#ifdef DEBUG_FOE

   388

    printk("ec_fsm_foe_ack_read()\n");

   389

#endif

   390

   391

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   392

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   393

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

   394

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

   395

               slave->ring_position, datagram->state);

   396

        return;

   397

    }

   398

   399

    if (datagram->working_counter != 1) {

   400

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   401

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

   402

                slave->ring_position);

   403

        ec_datagram_print_wc_error(datagram);

   404

        return;

   405

    }

   406

   407

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

   408

                    &mbox_prot, &rec_size))) {

   409

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   410

        return;

   411

    }

   412

   413

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   414

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   415

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

   416

        return;

   417

    }

   418

   419

    opCode = EC_READ_U8(data);

   420

   421

    if (opCode == EC_FOE_OPCODE_BUSY) {

   422

        // slave not ready

   423

        if (ec_foe_prepare_data_send(fsm)) {

   424

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   425

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

   426

            return;

   427

        }

   428

        fsm->state = ec_fsm_foe_state_data_sent;

   429

        return;

   430

    }

   431

   432

    if (opCode == EC_FOE_OPCODE_ACK) {

   433

        fsm->tx_packet_no++;

   434

        fsm->tx_buffer_offset += fsm->tx_current_size;

   435

   436

        if (fsm->tx_last_packet) {

   437

            fsm->state = ec_fsm_foe_end;

   438

            return;

   439

        }

   440

   441

        if (ec_foe_prepare_data_send(fsm)) {

   442

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   443

            return;

   444

        }

   445

        fsm->state = ec_fsm_foe_state_data_sent;

   446

        return;

   447

    }

   448

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   449

}

   450

   451

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

   461

{

   462

    ec_datagram_t *datagram = fsm->datagram;

   463

    ec_slave_t *slave = fsm->slave;

   464

   465

#ifdef DEBUG_FOE

   466

    printk("ec_foe_state_sent_wrq()\n");

   467

#endif

   468

   469

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   470

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   471

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

   472

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

   473

                slave->ring_position, datagram->state);

   474

        return;

   475

    }

   476

   477

    if (datagram->working_counter != 1) {

   478

        // slave did not put anything in the mailbox yet

   479

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   480

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

   481

                slave->ring_position);

   482

        ec_datagram_print_wc_error(datagram);

   483

        return;

   484

    }

   485

   486

    fsm->jiffies_start = datagram->jiffies_sent;

   487

   488

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

   489

   490

    fsm->retries = EC_FSM_RETRIES;

   491

    fsm->state = ec_fsm_foe_state_ack_check;

   492

}

   493

   494

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

   504

{

   505

    ec_datagram_t *datagram = fsm->datagram;

   506

    ec_slave_t *slave = fsm->slave;

   507

   508

#ifdef DEBUG_FOE

   509

    printk("ec_fsm_foe_state_data_sent()\n");