1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2008  Olav Zarges, imc Meßsysteme 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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    15

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

 *  You should have received a copy of the GNU General Public License

    20

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT FoE state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "fsm_foe.h"

    45

#include "foe.h"

    46

    47

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

    48

    49

/** Maximum time in ms to wait for responses when reading out the dictionary.

    50

 */

    51

#define EC_FSM_FOE_TIMEOUT 3000

    52

    53

#define EC_MBOX_TYPE_FILEACCESS 0x04

    54

    55

#define myDEBUG

    56

    57

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

    58

    59

int		ec_foe_prepare_data_send( ec_fsm_foe_t * );

    60

int		ec_foe_prepare_wrq_send( ec_fsm_foe_t * );

    61

int		ec_foe_prepare_rrq_send( ec_fsm_foe_t * );

    62

int		ec_foe_prepare_send_ack( ec_fsm_foe_t * );

    63

    64

void	ec_foe_set_tx_error( ec_fsm_foe_t *, uint32_t );

    65

void	ec_foe_set_rx_error( ec_fsm_foe_t *, uint32_t );

    66

    67

void	ec_fsm_foe_write(ec_fsm_foe_t * );

    68

void	ec_fsm_foe_read(ec_fsm_foe_t * );

    69

void 	ec_fsm_foe_end( ec_fsm_foe_t * );

    70

void 	ec_fsm_foe_error( ec_fsm_foe_t * );

    71

    72

void	ec_fsm_foe_state_wrq_sent( ec_fsm_foe_t * );

    73

void	ec_fsm_foe_state_rrq_sent( ec_fsm_foe_t * );

    74

    75

void	ec_fsm_foe_state_ack_check( ec_fsm_foe_t * );

    76

void	ec_fsm_foe_state_ack_read( ec_fsm_foe_t * );

    77

    78

void	ec_fsm_foe_state_data_sent( ec_fsm_foe_t * );

    79

    80

void	ec_fsm_foe_state_data_check( ec_fsm_foe_t * );

    81

void	ec_fsm_foe_state_data_read ( ec_fsm_foe_t * );

    82

void	ec_fsm_foe_state_sent_ack( ec_fsm_foe_t * );

    83

    84

void	ec_fsm_foe_write_start( ec_fsm_foe_t * );

    85

void	ec_fsm_foe_read_start(ec_fsm_foe_t * );

    86

    87

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

    88

    89

/**

    90

   Constructor.

    91

*/

    92

    93

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

    94

                     ec_datagram_t *datagram /**< datagram */

    95

                     )

    96

{

    97

    fsm->state     = NULL;

    98

    fsm->datagram  = datagram;

    99

    fsm->rx_errors = 0;

   100

    fsm->tx_errors = 0;

   101

}

   102

   103

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

   104

   105

/**

   106

   Destructor.

   107

*/

   108

   109

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

   110

{

   111

}

   112

   113

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

   114

   115

/**

   116

   Executes the current state of the state machine.

   117

   \return false, if state machine has terminated

   118

*/

   119

   120

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

   121

{

   122

    fsm->state(fsm);

   123

   124

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

   125

}

   126

   127

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

   128

   129

/**

   130

   Returns, if the state machine terminated with success.

   131

   \return non-zero if successful.

   132

*/

   133

   134

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

   135

{

   136

    return fsm->state == ec_fsm_foe_end;

   137

}

   138

   139

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

   140

   141

void ec_fsm_foe_transfer(

   142

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

   143

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

   144

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

   145

        )

   146

{

   147

    fsm->slave = slave;

   148

    fsm->request = request;

   149

    if (request->dir == EC_DIR_OUTPUT) {

   150

        fsm->state = ec_fsm_foe_write;

   151

    }

   152

    else {

   153

        fsm->state = ec_fsm_foe_read;

   154

    }

   155

}

   156

   157

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

   158

   159

/**

   160

   State: ERROR.

   161

*/

   162

   163

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

   164

{

   165

#ifdef	myDEBUG

   166

	printk("ec_fsm_foe_error()\n");

   167

#endif

   168

}

   169

   170

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

   171

   172

/**

   173

   State: END.

   174

*/

   175

   176

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

   177

{

   178

#ifdef	myDEBUG

   179

    printk("ec_fsm_foe_end\n");

   180

#endif

   181

}

   182

   183

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

   184

   185

#define	EC_MBOX_HEADER_SIZE		6

   186

// uint16_t	Length

   187

// uint16_t	Address

   188

// uint8_t	reserved

   189

// uint8_t	Type:4

   190

// uint8_t	Counter:4

   191

   192

#define	EC_FOE_HEADER_SIZE		6

   193

// uint8_t	OpCode

   194

// uint8_t	reserved

   195

// uint32_t	PacketNo, Password, ErrorCode

   196

   197

enum {

   198

	EC_FOE_OPCODE_RRQ  = 1,

   199

	EC_FOE_OPCODE_WRQ  = 2,

   200

	EC_FOE_OPCODE_DATA = 3,

   201

	EC_FOE_OPCODE_ACK  = 4,

   202

	EC_FOE_OPCODE_ERR  = 5,

   203

	EC_FOE_OPCODE_BUSY = 6

   204

} ec_foe_opcode_t;

   205

   206

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

   207

/**

   208

   Sends a file or the next fragment.

   209

*/

   210

   211

int ec_foe_prepare_data_send( ec_fsm_foe_t *fsm ) {

   212

    size_t       remaining_size, current_size;

   213

    uint8_t*     data;

   214

   215

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

   216

   217

    if (remaining_size < fsm->slave->sii.std_tx_mailbox_size

   218

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   219

        current_size = remaining_size;

   220

        fsm->tx_last_packet = 1;

   221

    } else {

   222

        current_size = fsm->slave->sii.std_tx_mailbox_size

   223

            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   224

    }

   225

   226

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

   227

    		EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE)))

   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, fsm->tx_buffer + fsm->tx_buffer_offset, current_size);

   234

   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

    size_t current_size;

   247

    uint8_t *data;

   248

   249

    fsm->tx_buffer_offset = 0;

   250

    fsm->tx_current_size = 0;

   251

    fsm->tx_packet_no = 0;

   252

	fsm->tx_last_packet = 0;

   253

   254

    current_size = fsm->tx_filename_len;

   255

   256

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

   257

    		EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE)))

   258

        return -1;

   259

   260

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

   261

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

   262

   263

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

   264

   265

    return 0;

   266

}

   267

   268

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

   269

   270

char		tx_buffer[0x1000];

   271

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

   272

{

   273

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

   274

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

   275

    fsm->tx_buffer_offset = 0;

   276

   277

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

   278

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

   279

   280

	fsm->state = ec_fsm_foe_write_start;

   281

   282

#ifdef use_ext_buffer

   283

	{

   284

		int i;

   285

		fsm->tx_data = tx_buffer;

   286

		for (i=0 ; i<sizeof(tx_buffer) ; i++) {

   287

			tx_buffer[i] = (uint8_t)(i);

   288

		}

   289

		fsm->tx_data_len = sizeof(tx_buffer);

   290

	}

   291

#endif

   292

}

   293

   294

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

   295

/**

   296

   Initializes the SII write state machine.

   297

*/

   298

   299

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

   300

{

   301

    ec_slave_t *slave = fsm->slave;

   302

   303

    fsm->tx_buffer_offset = 0;

   304

    fsm->tx_current_size = 0;

   305

    fsm->tx_packet_no = 0;

   306

	fsm->tx_last_packet = 0;

   307

   308

#ifdef	myDEBUG

   309

    printk("ec_fsm_foe_write_start()\n");

   310

#endif

   311

   312

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

   313

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   314

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

   315

        return;

   316

    }

   317

   318

    if (ec_foe_prepare_wrq_send(fsm)) {

   319

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   320

        return;

   321

    }

   322

   323

    fsm->state = ec_fsm_foe_state_wrq_sent;

   324

}

   325

   326

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

   327

   328

void ec_fsm_foe_state_ack_check( ec_fsm_foe_t *fsm ) {

   329

    ec_datagram_t *datagram = fsm->datagram;

   330

    ec_slave_t *slave = fsm->slave;

   331

   332

#ifdef	myDEBUG

   333

//	printk("ec_fsm_foe_ack_check()\n");

   334

#endif

   335

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   336

    	ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   337

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

   338

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

   339

               slave->ring_position, datagram->state);

   340

        return;

   341

    }

   342

   343

    if (datagram->working_counter != 1) {

   344

    	// slave hat noch nichts in die Mailbox getan

   345

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   346

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

   347

               slave->ring_position);

   348

        ec_datagram_print_wc_error(datagram);

   349

	    return;

   350

	}

   351

   352

    if (!ec_slave_mbox_check(datagram)) {

   353

        unsigned long diff_ms =

   354

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

   355

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   356

        	ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   357

            EC_ERR("Timeout while waiting for ack response "

   358

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

   359

            return;

   360

        }

   361

//        EC_ERR("WAIT!!!!!!!!!!!!!\n");

   362

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

   363

        fsm->retries = EC_FSM_RETRIES;

   364

        return;

   365

    }

   366

   367

    // Fetch response

   368

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

   369

   370

    fsm->retries = EC_FSM_RETRIES;

   371

    fsm->state = ec_fsm_foe_state_ack_read;

   372

}

   373

   374

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

   375

   376

void ec_fsm_foe_state_ack_read( ec_fsm_foe_t *fsm ) {

   377

   378

    ec_datagram_t *datagram = fsm->datagram;

   379

    ec_slave_t *slave = fsm->slave;

   380

    uint8_t *data, mbox_prot;

   381

    uint8_t opCode;

   382

    size_t rec_size;

   383

   384

#ifdef	myDEBUG

   385

	printk("ec_fsm_foe_ack_read()\n");

   386

#endif

   387

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   388

    	ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   389

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

   390

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

   391

               slave->ring_position, datagram->state);

   392

        return;

   393

    }

   394

   395

    if (datagram->working_counter != 1) {

   396

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   397

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

   398

                slave->ring_position);

   399

        ec_datagram_print_wc_error(datagram);

   400

        return;

   401

    }

   402

   403

	if (!(data = ec_slave_mbox_fetch(fsm->slave, datagram, &mbox_prot, &rec_size))) {

   404

    	ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   405

        return;

   406

    }

   407

   408

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   409

    	ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   410

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

   411

        return;

   412

    }

   413

   414

    opCode = EC_READ_U8(data);

   415

   416

    if (opCode == EC_FOE_OPCODE_BUSY) {

   417

    	// slave ist noch nicht bereit

   418

        if (ec_foe_prepare_data_send(fsm)) {

   419

        	ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   420

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

   421

        	return;

   422

        }

   423

        fsm->state = ec_fsm_foe_state_data_sent;

   424

        return;

   425

    }

   426

   427

    if (opCode == EC_FOE_OPCODE_ACK) {

   428

        fsm->tx_packet_no++;

   429

        fsm->tx_buffer_offset += fsm->tx_current_size;

   430

   431

        if (fsm->tx_last_packet) {

   432

        	fsm->state = ec_fsm_foe_end;

   433

        	return;

   434

        }

   435

   436

        if (ec_foe_prepare_data_send(fsm)) {

   437

        	ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   438

	    	return;

   439

	    }

   440

        fsm->state = ec_fsm_foe_state_data_sent;

   441

    	return;

   442

    }

   443

	ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   444

}

   445

   446

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

   447

/**

   448

   State: WRQ SENT.

   449

   Checks is the previous transmit datagram succeded and sends the next

   450

   fragment, if necessary.

   451

*/

   452

   453

void ec_fsm_foe_state_wrq_sent( ec_fsm_foe_t *fsm ) {

   454

    ec_datagram_t *datagram = fsm->datagram;

   455

    ec_slave_t *slave = fsm->slave;

   456

   457

#ifdef	myDEBUG

   458

	printk("ec_foe_state_sent_wrq()\n");

   459

#endif

   460

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   461

    	ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   462

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

   463

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

   464

                slave->ring_position, datagram->state);

   465

        return;

   466

    }

   467

   468

    if (datagram->working_counter != 1) {

   469

    	// slave hat noch nichts in die Mailbox getan

   470

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   471

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

   472

                slave->ring_position);

   473

        ec_datagram_print_wc_error(datagram);

   474

    	return;

   475

    }

   476

   477

    fsm->jiffies_start = datagram->jiffies_sent;

   478

   479

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

   480

   481

    fsm->retries = EC_FSM_RETRIES;

   482

    fsm->state = ec_fsm_foe_state_ack_check;

   483

}

   484

   485

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

   486

/**

   487

   State: WRQ SENT.

   488

   Checks is the previous transmit datagram succeded and sends the next

   489

   fragment, if necessary.

   490

*/

   491

   492

void ec_fsm_foe_state_data_sent( ec_fsm_foe_t *fsm ) {

   493

    ec_datagram_t *datagram = fsm->datagram;

   494

    ec_slave_t *slave = fsm->slave;

   495

   496

#ifdef	myDEBUG

   497

	printk("ec_fsm_foe_state_data_sent()\n");

   498

#endif

   499

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

   500

    	ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);

   501

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

   502

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

   503

               slave->ring_position, datagram->state);

   504

        return;

   505

    }

   506

   507

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

   508

	    ec_foe_set_tx_error(fsm, FOE_WC_ERROR);

   509

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

   510

                slave->ring_position);

   511

        ec_datagram_print_wc_error(datagram);

   512

        return;

   513

    }

   514

   515

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

   516

    fsm->jiffies_start = jiffies;

   517

    fsm->retries = EC_FSM_RETRIES;

   518

    fsm->state = ec_fsm_foe_state_ack_check;

   519

}

   520

   521

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

   522

/**

   523

   Prepare a read request (RRQ) with filename

   524

*/

   525

   526

int ec_foe_prepare_rrq_send( ec_fsm_foe_t *fsm ) {