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

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

    56

    57

int		ec_foe_prepare_data_send( ec_fsm_foe_t * );

    58

int		ec_foe_prepare_wrq_send( ec_fsm_foe_t * );

    59

int		ec_foe_prepare_rrq_send( ec_fsm_foe_t * );

    60

int		ec_foe_prepare_send_ack( ec_fsm_foe_t * );

    61

    62

void	ec_foe_set_tx_error( ec_fsm_foe_t *, uint32_t );

    63

void	ec_foe_set_rx_error( ec_fsm_foe_t *, uint32_t );

    64

    65

void	ec_fsm_foe_write(ec_fsm_foe_t * );

    66

void	ec_fsm_foe_read(ec_fsm_foe_t * );

    67

void 	ec_fsm_foe_end( ec_fsm_foe_t * );

    68

void 	ec_fsm_foe_error( ec_fsm_foe_t * );

    69

    70

void	ec_fsm_foe_state_wrq_sent( ec_fsm_foe_t * );

    71

void	ec_fsm_foe_state_rrq_sent( ec_fsm_foe_t * );

    72

    73

void	ec_fsm_foe_state_ack_check( ec_fsm_foe_t * );

    74

void	ec_fsm_foe_state_ack_read( ec_fsm_foe_t * );

    75

    76

void	ec_fsm_foe_state_data_sent( ec_fsm_foe_t * );

    77

    78

void	ec_fsm_foe_state_data_check( ec_fsm_foe_t * );

    79

void	ec_fsm_foe_state_data_read ( ec_fsm_foe_t * );

    80

void	ec_fsm_foe_state_sent_ack( ec_fsm_foe_t * );

    81

    82

void	ec_fsm_foe_write_start( ec_fsm_foe_t * );

    83

void	ec_fsm_foe_read_start(ec_fsm_foe_t * );

    84

    85

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

    86

    87

/**

    88

   Constructor.

    89

*/

    90

    91

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

    92

                     ec_datagram_t *datagram /**< datagram */

    93

                     )

    94

{

    95

    fsm->state     = NULL;

    96

    fsm->datagram  = datagram;

    97

    fsm->rx_errors = 0;

    98

    fsm->tx_errors = 0;

    99

}

   100

   101

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

   102

   103

/**

   104

   Destructor.

   105

*/

   106

   107

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

   108

{

   109

}

   110

   111

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

   112

   113

/**

   114

   Executes the current state of the state machine.

   115

   \return false, if state machine has terminated

   116

*/

   117

   118

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

   119

{

   120

    fsm->state(fsm);

   121

   122

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

   123

}

   124

   125

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

   126

   127

/**

   128

   Returns, if the state machine terminated with success.

   129

   \return non-zero if successful.

   130

*/

   131

   132

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

   133

{

   134

    return fsm->state == ec_fsm_foe_end;

   135

}

   136

   137

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

   138

   139

void ec_fsm_foe_transfer(

   140

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

   141

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

   142

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

   143

        )

   144

{

   145

    fsm->slave = slave;

   146

    fsm->request = request;

   147

    if (request->dir == EC_DIR_OUTPUT) {

   148

        fsm->state = ec_fsm_foe_write;

   149

    }

   150

    else {

   151

        fsm->state = ec_fsm_foe_read;

   152

    }

   153

}

   154

   155

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

   156

   157

/**

   158

   State: ERROR.

   159

*/

   160

   161

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

   162

{

   163

#ifdef	myDEBUG

   164

	printk("ec_fsm_foe_error()\n");

   165

#endif

   166

}

   167

   168

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

   169

   170

/**

   171

   State: END.

   172

*/

   173

   174

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

   175

{

   176

#ifdef	myDEBUG

   177

    printk("ec_fsm_foe_end\n");

   178

#endif

   179

}

   180

   181

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

   182

   183

#define	EC_MBOX_HEADER_SIZE		6

   184

// uint16_t	Length

   185

// uint16_t	Address

   186

// uint8_t	reserved

   187

// uint8_t	Type:4

   188

// uint8_t	Counter:4

   189

   190

#define	EC_FOE_HEADER_SIZE		6

   191

// uint8_t	OpMode

   192

// uint8_t	reserved

   193

// uint32_t	PacketNo, Password, ErrorCode

   194

   195

enum {

   196

	EC_FOE_OPMODE_RRQ  = 1,

   197

	EC_FOE_OPMODE_WRQ  = 2,

   198

	EC_FOE_OPMODE_DATA = 3,

   199

	EC_FOE_OPMODE_ACK  = 4,

   200

	EC_FOE_OPMODE_ERR  = 5,

   201

	EC_FOE_OPMODE_BUSY = 6

   202

} ec_foe_opmode_t;

   203

   204

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

   205

/**

   206

   Sends a file or the next fragment.

   207

*/

   208

   209

int ec_foe_prepare_data_send( ec_fsm_foe_t *fsm ) {

   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->sii.tx_mailbox_size - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {

   216

        current_size = remaining_size;

   217

        fsm->tx_last_packet = 1;

   218

    }

   219

    else {

   220

        current_size = fsm->slave->sii.tx_mailbox_size - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;

   221

    }

   222

   223

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

   224

    		EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE)))

   225

        return -1;

   226

   227

    EC_WRITE_U8 ( data, EC_FOE_OPMODE_DATA ); 		// OpMode = DataBlock req.

   228

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

   229

   230

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

   231

   232

    fsm->tx_current_size = current_size;

   233

   234

    return 0;

   235

}

   236

   237

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

   238

/**

   239

   Prepare a write request (WRQ) with filename

   240

*/

   241

   242

int ec_foe_prepare_wrq_send( ec_fsm_foe_t *fsm ) {

   243

    size_t current_size;

   244

    uint8_t *data;

   245

   246

    fsm->tx_buffer_offset = 0;

   247

    fsm->tx_current_size = 0;

   248

    fsm->tx_packet_no = 0;

   249

	fsm->tx_last_packet = 0;

   250

   251

    current_size = fsm->tx_filename_len;

   252

   253

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

   254

    		EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE)))

   255

        return -1;

   256

   257

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

   258

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

   259

   260

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

   261

   262

    return 0;

   263

}

   264

   265

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

   266

   267

char		tx_buffer[0x1000];

   268

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

   269

{

   270

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

   271

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

   272

    fsm->tx_buffer_offset = 0;

   273

   274

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

   275

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

   276

   277

	fsm->state = ec_fsm_foe_write_start;

   278

   279

#ifdef use_ext_buffer

   280

	{

   281

		int i;

   282

		fsm->tx_data = tx_buffer;

   283

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

   284

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

   285

		}

   286

		fsm->tx_data_len = sizeof(tx_buffer);

   287

	}

   288

#endif

   289

}

   290

   291

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

   292

/**

   293

   Initializes the SII write state machine.

   294

*/

   295

   296

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

   297

{

   298

    ec_slave_t *slave = fsm->slave;

   299

   300

    fsm->tx_buffer_offset = 0;

   301

    fsm->tx_current_size = 0;

   302

    fsm->tx_packet_no = 0;

   303

	fsm->tx_last_packet = 0;

   304

   305

#ifdef	myDEBUG

   306

    printk("ec_fsm_foe_write_start()\n");

   307

#endif

   308

   309

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

   310

        ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   311

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

   312

        return;

   313

    }

   314

   315

    if (ec_foe_prepare_wrq_send(fsm)) {

   316

        ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   317

        return;

   318

    }

   319

   320

    fsm->state = ec_fsm_foe_state_wrq_sent;

   321

}

   322

   323

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

   324

   325

void ec_fsm_foe_state_ack_check( ec_fsm_foe_t *fsm ) {

   326

    ec_datagram_t *datagram = fsm->datagram;

   327

    ec_slave_t *slave = fsm->slave;

   328

   329

#ifdef	myDEBUG

   330

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

   331

#endif

   332

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   333

    	ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   334

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

   335

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

   336

               slave->ring_position, datagram->state);

   337

        return;

   338

    }

   339

   340

    if (datagram->working_counter != 1) {

   341

    	// slave hat noch nichts in die Mailbox getan

   342

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   343

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

   344

               slave->ring_position);

   345

        ec_datagram_print_wc_error(datagram);

   346

	    return;

   347

	}

   348

   349

    if (!ec_slave_mbox_check(datagram)) {

   350

        unsigned long diff_ms =

   351

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

   352

        if (diff_ms >= EC_FSM_FOE_TIMEOUT) {

   353

        	ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);

   354

            EC_ERR("Timeout while waiting for ack response "

   355

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

   356

            return;

   357

        }

   358

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

   359

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

   360

        fsm->retries = EC_FSM_RETRIES;

   361

        return;

   362

    }

   363

   364

    // Fetch response

   365

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

   366

   367

    fsm->retries = EC_FSM_RETRIES;

   368

    fsm->state = ec_fsm_foe_state_ack_read;

   369

}

   370

   371

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

   372

   373

void ec_fsm_foe_state_ack_read( ec_fsm_foe_t *fsm ) {

   374

   375

    ec_datagram_t *datagram = fsm->datagram;

   376

    ec_slave_t *slave = fsm->slave;

   377

    uint8_t *data, mbox_prot;

   378

    uint16_t opMode;

   379

    size_t rec_size;

   380

   381

#ifdef	myDEBUG

   382

	printk("ec_fsm_foe_ack_read()\n");

   383

#endif

   384

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   385

    	ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   386

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

   387

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

   388

               slave->ring_position, datagram->state);

   389

        return;

   390

    }

   391

   392

    if (datagram->working_counter != 1) {

   393

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   394

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

   395

                slave->ring_position);

   396

        ec_datagram_print_wc_error(datagram);

   397

        return;

   398

    }

   399

   400

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

   401

    	ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   402

        return;

   403

    }

   404

   405

    if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE

   406

    	ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);

   407

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

   408

        return;

   409

    }

   410

   411

    opMode = EC_READ_U16(data);

   412

   413

    if ( opMode == EC_FOE_OPMODE_BUSY ) {

   414

    	// slave ist noch nicht bereit

   415

        if (ec_foe_prepare_data_send(fsm)) {

   416

        	ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   417

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

   418

        	return;

   419

        }

   420

        fsm->state = ec_fsm_foe_state_data_sent;

   421

        return;

   422

    }

   423

   424

    if ( opMode == EC_FOE_OPMODE_ACK ) {

   425

        fsm->tx_packet_no++;

   426

        fsm->tx_buffer_offset += fsm->tx_current_size;

   427

   428

        if (fsm->tx_last_packet) {

   429

        	fsm->state = ec_fsm_foe_end;

   430

        	return;

   431

        }

   432

   433

        if (ec_foe_prepare_data_send(fsm)) {

   434

        	ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);

   435

	    	return;

   436

	    }

   437

        fsm->state = ec_fsm_foe_state_data_sent;

   438

    	return;

   439

    }

   440

	ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);

   441

}

   442

   443

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

   444

/**

   445

   State: WRQ SENT.

   446

   Checks is the previous transmit datagram succeded and sends the next

   447

   fragment, if necessary.

   448

*/

   449

   450

void ec_fsm_foe_state_wrq_sent( ec_fsm_foe_t *fsm ) {

   451

    ec_datagram_t *datagram = fsm->datagram;

   452

    ec_slave_t *slave = fsm->slave;

   453

   454

#ifdef	myDEBUG

   455

	printk("ec_foe_state_sent_wrq()\n");

   456

#endif

   457

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   458

    	ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);

   459

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

   460

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

   461

                slave->ring_position, datagram->state);

   462

        return;

   463

    }

   464

   465

    if (datagram->working_counter != 1) {

   466

    	// slave hat noch nichts in die Mailbox getan

   467

        ec_foe_set_rx_error(fsm, FOE_WC_ERROR);

   468

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

   469

                slave->ring_position);

   470

        ec_datagram_print_wc_error(datagram);

   471

    	return;

   472

    }

   473

   474

    fsm->jiffies_start = datagram->jiffies_sent;

   475

   476

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

   477

   478

    fsm->retries = EC_FSM_RETRIES;

   479

    fsm->state = ec_fsm_foe_state_ack_check;

   480

}

   481

   482

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

   483

/**

   484

   State: WRQ SENT.

   485

   Checks is the previous transmit datagram succeded and sends the next

   486

   fragment, if necessary.

   487

*/

   488

   489

void ec_fsm_foe_state_data_sent( ec_fsm_foe_t *fsm ) {

   490

    ec_datagram_t *datagram = fsm->datagram;

   491

    ec_slave_t *slave = fsm->slave;

   492

   493

#ifdef	myDEBUG

   494

	printk("ec_fsm_foe_state_data_sent()\n");

   495

#endif

   496

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

   497

    	ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);

   498

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

   499

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

   500

               slave->ring_position, datagram->state);

   501

        return;

   502

    }

   503

   504

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

   505

	    ec_foe_set_tx_error(fsm, FOE_WC_ERROR);

   506

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

   507

                slave->ring_position);

   508

        ec_datagram_print_wc_error(datagram);

   509

        return;

   510

    }

   511

   512

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

   513

    fsm->jiffies_start = jiffies;

   514

    fsm->retries = EC_FSM_RETRIES;

   515

    fsm->state = ec_fsm_foe_state_ack_check;

   516

}

   517

   518

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

   519

/**

   520

   Prepare a read request (RRQ) with filename

   521

*/

   522

   523

int ec_foe_prepare_rrq_send( ec_fsm_foe_t *fsm ) {

   524

    size_t current_size;

   525

    uint8_t *data;

   526

   527

    current_size = fsm->rx_filename_len;

   528

   529

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

   530