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_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_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_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");

   321

        ec_datagram_print_state(datagram);

   322

        return;

   323

    }

   324

   325

    if (datagram->working_counter != 1) {

   326

        // slave did not put anything in the mailbox yet

   327

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   328

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

   329

                " failed: ");

   330

        ec_datagram_print_wc_error(datagram);

   331

        return;

   332

    }

   333

   334

    if (!ec_slave_mbox_check(datagram)) {

   335

        unsigned long diff_ms =

   336

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

   337

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   338

            ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   339

            EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");

   340

            return;

   341

        }

   342

   343

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

   344

        fsm->retries = EC_FSM_RETRIES;

   345

        return;

   346

    }

   347

   348

    // Fetch response

   349

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

   350

   351

    fsm->retries = EC_FSM_RETRIES;

   352

    fsm->state = ec_fsm_foe_state_ack_read;

   353

}

   354

   355

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

   356

   357

/** Acknowledge a read operation.

   358

 */

   359

void ec_fsm_foe_state_ack_read(

   360

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

   361

        )

   362

{

   363

    ec_datagram_t *datagram = fsm->datagram;

   364

    ec_slave_t *slave = fsm->slave;

   365

    uint8_t *data, mbox_prot;

   366

    uint8_t opCode;

   367

    size_t rec_size;

   368

   369

#ifdef DEBUG_FOE

   370

    printk("ec_fsm_foe_ack_read()\n");

   371

#endif

   372

   373

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   374

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   375

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

   376

        ec_datagram_print_state(datagram);

   377

        return;

   378

    }

   379

   380

    if (datagram->working_counter != 1) {

   381

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   382

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

   383

        ec_datagram_print_wc_error(datagram);

   384

        return;

   385

    }

   386

   387

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

   388

                    &mbox_prot, &rec_size))) {

   389

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   390

        return;

   391

    }

   392

   393

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   394

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   395

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

   396

                mbox_prot);

   397

        return;

   398

    }

   399

   400

    opCode = EC_READ_U8(data);

   401

   402

    if (opCode == EC_FOE_OPCODE_BUSY) {

   403

        // slave not ready

   404

        if (ec_foe_prepare_data_send(fsm)) {

   405

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   406

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

   407

            return;

   408

        }

   409

        fsm->state = ec_fsm_foe_state_data_sent;

   410

        return;

   411

    }

   412

   413

    if (opCode == EC_FOE_OPCODE_ACK) {

   414

        fsm->tx_packet_no++;

   415

        fsm->tx_buffer_offset += fsm->tx_current_size;

   416

   417

        if (fsm->tx_last_packet) {

   418

            fsm->state = ec_fsm_foe_end;

   419

            return;

   420

        }

   421

   422

        if (ec_foe_prepare_data_send(fsm)) {

   423

            ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   424

            return;

   425

        }

   426

        fsm->state = ec_fsm_foe_state_data_sent;

   427

        return;

   428

    }

   429

    ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   430

}

   431

   432

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

   433

   434

/** State: WRQ SENT.

   435

 *

   436

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

   437

 * fragment, if necessary.

   438

 */

   439

void ec_fsm_foe_state_wrq_sent(

   440

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

   441

        )

   442

{

   443

    ec_datagram_t *datagram = fsm->datagram;

   444

    ec_slave_t *slave = fsm->slave;

   445

   446

#ifdef DEBUG_FOE

   447

    printk("ec_foe_state_sent_wrq()\n");

   448

#endif

   449

   450

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   451

        ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   452

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

   453

        ec_datagram_print_state(datagram);

   454

        return;

   455

    }

   456

   457

    if (datagram->working_counter != 1) {

   458

        // slave did not put anything in the mailbox yet

   459

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   460

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

   461

        ec_datagram_print_wc_error(datagram);

   462

        return;

   463

    }

   464

   465

    fsm->jiffies_start = datagram->jiffies_sent;

   466

   467

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

   468

   469

    fsm->retries = EC_FSM_RETRIES;

   470

    fsm->state = ec_fsm_foe_state_ack_check;

   471

}

   472

   473

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

   474

   475

/** State: WRQ SENT.

   476

 *

   477

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

   478

 * fragment, if necessary.

   479

 */

   480

void ec_fsm_foe_state_data_sent(

   481

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

   482

        )

   483

{

   484

    ec_datagram_t *datagram = fsm->datagram;

   485

    ec_slave_t *slave = fsm->slave;

   486

   487

#ifdef DEBUG_FOE

   488

    printk("ec_fsm_foe_state_data_sent()\n");

   489

#endif

   490

   491

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

   492

        ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);

   493

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

   494

        ec_datagram_print_state(datagram);

   495

        return;

   496

    }

   497

   498

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

   499

        ec_foe_set_tx_error(fsm, FOE_WC_ERROR);

   500

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

   501

        ec_datagram_print_wc_error(datagram);

   502

        return;

   503

    }

   504