1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH

     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 SoE state machines.

    33

*/

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "fsm_soe.h"

    41

    42

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

    43

    44

/** Mailbox type for SoE.

    45

 */

    46

#define EC_MBOX_TYPE_SOE 0x05

    47

    48

#define EC_SOE_OPCODE_READ_REQUEST   0x01

    49

#define EC_SOE_OPCODE_READ_RESPONSE  0x02

    50

#define EC_SOE_OPCODE_WRITE_REQUEST  0x03

    51

#define EC_SOE_OPCODE_WRITE_RESPONSE 0x04

    52

    53

#define EC_SOE_READ_REQUEST_SIZE   0x04

    54

#define EC_SOE_READ_RESPONSE_SIZE  0x04

    55

#define EC_SOE_WRITE_REQUEST_SIZE  0x04

    56

#define EC_SOE_WRITE_RESPONSE_SIZE 0x04

    57

    58

#define EC_SOE_RESPONSE_TIMEOUT 1000

    59

    60

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

    61

    62

void ec_fsm_soe_read_start(ec_fsm_soe_t *);

    63

void ec_fsm_soe_read_request(ec_fsm_soe_t *);

    64

void ec_fsm_soe_read_check(ec_fsm_soe_t *);

    65

void ec_fsm_soe_read_response(ec_fsm_soe_t *);

    66

    67

void ec_fsm_soe_write_start(ec_fsm_soe_t *);

    68

void ec_fsm_soe_write_request(ec_fsm_soe_t *);

    69

void ec_fsm_soe_write_check(ec_fsm_soe_t *);

    70

void ec_fsm_soe_write_response(ec_fsm_soe_t *);

    71

    72

void ec_fsm_soe_end(ec_fsm_soe_t *);

    73

void ec_fsm_soe_error(ec_fsm_soe_t *);

    74

    75

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

    76

    77

/** Constructor.

    78

 */

    79

void ec_fsm_soe_init(

    80

		ec_fsm_soe_t *fsm, /**< finite state machine */

    81

		ec_datagram_t *datagram /**< datagram */

    82

		)

    83

{

    84

    fsm->state = NULL;

    85

    fsm->datagram = datagram;

    86

}

    87

    88

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

    89

    90

/** Destructor.

    91

 */

    92

void ec_fsm_soe_clear(

    93

		ec_fsm_soe_t *fsm /**< finite state machine */

    94

		)

    95

{

    96

}

    97

    98

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

    99

   100

/** Starts to transfer an IDN to/from a slave.

   101

 */

   102

void ec_fsm_soe_transfer(

   103

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

   104

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

   105

        ec_soe_request_t *request /**< SoE request. */

   106

        )

   107

{

   108

    fsm->slave = slave;

   109

    fsm->request = request;

   110

    if (request->dir == EC_DIR_OUTPUT) {

   111

        fsm->state = ec_fsm_soe_write_start;

   112

	} else {

   113

        fsm->state = ec_fsm_soe_read_start;

   114

	}

   115

}

   116

   117

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

   118

   119

/**

   120

   Executes the current state of the state machine.

   121

   \return false, if state machine has terminated

   122

*/

   123

   124

int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)

   125

{

   126

    fsm->state(fsm);

   127

   128

    return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;

   129

}

   130

   131

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

   132

   133

/**

   134

   Returns, if the state machine terminated with success.

   135

   \return non-zero if successful.

   136

*/

   137

   138

int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)

   139

{

   140

    return fsm->state == ec_fsm_soe_end;

   141

}

   142

   143

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

   144

 * SoE read state machine

   145

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

   146

   147

/** SoE state: READ START.

   148

 */

   149

void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)

   150

{

   151

    ec_datagram_t *datagram = fsm->datagram;

   152

    ec_slave_t *slave = fsm->slave;

   153

    ec_master_t *master = slave->master;

   154

    ec_soe_request_t *request = fsm->request;

   155

    uint8_t *data;

   156

   157

    if (master->debug_level)

   158

        EC_DBG("Reading IDN 0x%04X from slave %u.\n",

   159

               request->idn, slave->ring_position);

   160

   161

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

   162

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

   163

        fsm->state = ec_fsm_soe_error;

   164

        return;

   165

    }

   166

   167

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   168

			EC_SOE_READ_REQUEST_SIZE);

   169

    if (IS_ERR(data)) {

   170

        fsm->state = ec_fsm_soe_error;

   171

        return;

   172

    }

   173

   174

    EC_WRITE_U8(data, EC_SOE_OPCODE_READ_REQUEST);

   175

    EC_WRITE_U8(data + 1, 1 << 6); // request value

   176

    EC_WRITE_U16(data + 2, request->idn);

   177

   178

    if (master->debug_level) {

   179

        EC_DBG("SCC read request:\n");

   180

        ec_print_data(data, EC_SOE_READ_REQUEST_SIZE);

   181

    }

   182

   183

    fsm->request->jiffies_sent = jiffies;

   184

    fsm->retries = EC_FSM_RETRIES;

   185

    fsm->state = ec_fsm_soe_read_request;

   186

}

   187

   188

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

   189

   190

/** SoE state: READ REQUEST.

   191

 */

   192

void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)

   193

{

   194

    ec_datagram_t *datagram = fsm->datagram;

   195

    ec_slave_t *slave = fsm->slave;

   196

    unsigned long diff_ms;

   197

   198

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   199

        return; // FIXME: check for response first?

   200

   201

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   202

        fsm->state = ec_fsm_soe_error;

   203

        EC_ERR("Failed to receive SoE read request for slave %u: ",

   204

               slave->ring_position);

   205

        ec_datagram_print_state(datagram);

   206

        return;

   207

    }

   208

   209

    diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

   210

   211

    if (datagram->working_counter != 1) {

   212

        if (!datagram->working_counter) {

   213

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   214

                // no response; send request datagram again

   215

                return;

   216

            }

   217

        }

   218

        fsm->state = ec_fsm_soe_error;

   219

        EC_ERR("Reception of SoE read request for IDN 0x%04x failed"

   220

                " after %u ms on slave %u: ",

   221

                fsm->request->idn, (u32) diff_ms,

   222

                fsm->slave->ring_position);

   223

        ec_datagram_print_wc_error(datagram);

   224

        return;

   225

    }

   226

   227

    fsm->jiffies_start = datagram->jiffies_sent;

   228

   229

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

   230

    fsm->retries = EC_FSM_RETRIES;

   231

    fsm->state = ec_fsm_soe_read_check;

   232

}

   233

   234

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

   235

   236

/** CoE state: READ CHECK.

   237

 */

   238

void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)

   239

{

   240

    ec_datagram_t *datagram = fsm->datagram;

   241

    ec_slave_t *slave = fsm->slave;

   242

   243

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   244

        return;

   245

   246

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   247

        fsm->state = ec_fsm_soe_error;

   248

        EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ",

   249

               slave->ring_position);

   250

        ec_datagram_print_state(datagram);

   251

        return;

   252

    }

   253

   254

    if (datagram->working_counter != 1) {

   255

        fsm->state = ec_fsm_soe_error;

   256

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

   257

				slave->ring_position);

   258

        ec_datagram_print_wc_error(datagram);

   259

        return;

   260

    }

   261

   262

    if (!ec_slave_mbox_check(datagram)) {

   263

        unsigned long diff_ms =

   264

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

   265

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   266

            fsm->state = ec_fsm_soe_error;

   267

            EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x"

   268

                    " read response on slave %u.\n", (u32) diff_ms,

   269

                    fsm->request->idn, slave->ring_position);

   270

            return;

   271

        }

   272

   273

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

   274

        fsm->retries = EC_FSM_RETRIES;

   275

        return;

   276

    }

   277

   278

    // Fetch response

   279

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

   280

    fsm->retries = EC_FSM_RETRIES;

   281

    fsm->state = ec_fsm_soe_read_response;

   282

}

   283

   284

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

   285

   286

/** SoE state: READ RESPONSE.

   287

 */

   288

void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)

   289

{

   290

    ec_datagram_t *datagram = fsm->datagram;

   291

    ec_slave_t *slave = fsm->slave;

   292

    ec_master_t *master = slave->master;

   293

    uint8_t *data, mbox_prot, opcode, error_flag, value_included;

   294

    size_t rec_size, data_size;

   295

    ec_soe_request_t *req = fsm->request;

   296

   297

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   298

        return; // FIXME: request again?

   299

   300

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   301

        fsm->state = ec_fsm_soe_error;

   302

        EC_ERR("Failed to receive SoE read response datagram for"

   303

               " slave %u: ", slave->ring_position);

   304

        ec_datagram_print_state(datagram);

   305

        return;

   306

    }

   307

   308

    if (datagram->working_counter != 1) {

   309

        fsm->state = ec_fsm_soe_error;

   310

        EC_ERR("Reception of SoE read response failed on slave %u: ",

   311

                slave->ring_position);

   312

        ec_datagram_print_wc_error(datagram);

   313

        return;

   314

    }

   315

   316

    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);

   317

    if (IS_ERR(data)) {

   318

        fsm->state = ec_fsm_soe_error;

   319

        return;

   320

    }

   321

   322

    if (master->debug_level) {

   323

        EC_DBG("SCC read response:\n");

   324

        ec_print_data(data, rec_size);

   325

    }

   326

   327

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   328

        fsm->state = ec_fsm_soe_error;

   329

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

   330

        return;

   331

    }

   332

   333

    if (rec_size < EC_SOE_READ_RESPONSE_SIZE) {

   334

        fsm->state = ec_fsm_soe_error;

   335

        EC_ERR("Received currupted SoE read response (%zu bytes)!\n",

   336

				rec_size);

   337

        ec_print_data(data, rec_size);

   338

        return;

   339

    }

   340

   341

	opcode = EC_READ_U8(data) & 0x7;

   342

    if (opcode != EC_SOE_OPCODE_READ_RESPONSE) {

   343

        EC_ERR("Received no read response (opcode %x).\n", opcode);

   344

        ec_print_data(data, rec_size);

   345

        fsm->state = ec_fsm_soe_error;

   346

        return;

   347

    }

   348

   349

	error_flag = (EC_READ_U8(data) >> 4) & 1;

   350

	if (error_flag) {

   351

		req->error_code = EC_READ_U16(data + rec_size - 2);

   352

		EC_ERR("Received error response: 0x%04x.\n",

   353

				req->error_code);

   354

        fsm->state = ec_fsm_soe_error;

   355

        return;

   356

	} else {

   357

		req->error_code = 0x0000;

   358

	}

   359

   360

	value_included = (EC_READ_U8(data + 1) >> 6) & 1;

   361

	if (!value_included) {

   362

		EC_ERR("No value included!\n");

   363

		fsm->state = ec_fsm_soe_error;

   364

		return;

   365

	}

   366

   367

	data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE;

   368

	if (ec_soe_request_copy_data(req,

   369

				data + EC_SOE_READ_RESPONSE_SIZE, data_size)) {

   370

		fsm->state = ec_fsm_soe_error;

   371

		return;

   372

	}

   373

   374

	if (master->debug_level) {

   375

		EC_DBG("IDN data:\n");

   376

		ec_print_data(req->data, req->data_size);

   377

	}

   378

   379

    fsm->state = ec_fsm_soe_end; // success

   380

}

   381

   382

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

   383

 * SoE write state machine

   384

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

   385

   386

/** SoE state: WRITE START.

   387

 */

   388

void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)

   389

{

   390

    ec_datagram_t *datagram = fsm->datagram;

   391

    ec_slave_t *slave = fsm->slave;

   392

    ec_master_t *master = slave->master;

   393

    ec_soe_request_t *request = fsm->request;

   394

    uint8_t *data;

   395

   396

    if (master->debug_level)

   397

        EC_DBG("Writing IDN 0x%04X to slave %u.\n",

   398

               request->idn, slave->ring_position);

   399

   400

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

   401

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

   402

        fsm->state = ec_fsm_soe_error;

   403

        return;

   404

    }

   405

   406

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   407

			EC_SOE_WRITE_REQUEST_SIZE + request->data_size);

   408

    if (IS_ERR(data)) {

   409

        fsm->state = ec_fsm_soe_error;

   410

        return;

   411

    }

   412

   413

    EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST);

   414

    EC_WRITE_U8(data + 1, 1 << 6); // only value included

   415

    EC_WRITE_U16(data + 2, request->idn);

   416

	memcpy(data + 4, request->data, request->data_size);

   417

   418

    if (master->debug_level) {

   419

        EC_DBG("SCC write request:\n");

   420

        ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + request->data_size);

   421

    }

   422

   423

    fsm->request->jiffies_sent = jiffies;

   424

    fsm->retries = EC_FSM_RETRIES;

   425

    fsm->state = ec_fsm_soe_write_request;

   426

}

   427

   428

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

   429

   430

/** SoE state: WRITE REQUEST.

   431

 */

   432

void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)

   433

{

   434

    ec_datagram_t *datagram = fsm->datagram;

   435

    ec_slave_t *slave = fsm->slave;

   436

    unsigned long diff_ms;

   437

   438

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   439

        return; // FIXME: check for response first?

   440

   441

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   442

        fsm->state = ec_fsm_soe_error;

   443

        EC_ERR("Failed to receive SoE write request for slave %u: ",

   444

               slave->ring_position);

   445

        ec_datagram_print_state(datagram);

   446

        return;

   447

    }

   448

   449

    diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

   450

   451

    if (datagram->working_counter != 1) {

   452

        if (!datagram->working_counter) {

   453

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   454

                // no response; send request datagram again

   455

                return;

   456

            }

   457

        }

   458

        fsm->state = ec_fsm_soe_error;

   459

        EC_ERR("Reception of SoE write request for IDN 0x%04x failed"

   460

                " after %u ms on slave %u: ",

   461

                fsm->request->idn, (u32) diff_ms,

   462

                fsm->slave->ring_position);

   463

        ec_datagram_print_wc_error(datagram);

   464

        return;

   465

    }

   466

   467

    fsm->jiffies_start = datagram->jiffies_sent;

   468

   469

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

   470

    fsm->retries = EC_FSM_RETRIES;

   471

    fsm->state = ec_fsm_soe_write_check;

   472

}

   473

   474

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

   475

   476

/** CoE state: WRITE CHECK.

   477

 */

   478

void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)

   479

{

   480

    ec_datagram_t *datagram = fsm->datagram;

   481

    ec_slave_t *slave = fsm->slave;

   482

   483

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   484

        return;

   485

   486

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   487

        fsm->state = ec_fsm_soe_error;

   488

        EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ",

   489

               slave->ring_position);

   490

        ec_datagram_print_state(datagram);

   491

        return;

   492

    }

   493

   494

    if (datagram->working_counter != 1) {

   495

        fsm->state = ec_fsm_soe_error;

   496

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

   497

				slave->ring_position);

   498

        ec_datagram_print_wc_error(datagram);

   499

        return;

   500

    }

   501

   502

    if (!ec_slave_mbox_check(datagram)) {

   503

        unsigned long diff_ms =

   504

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

   505

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   506

            fsm->state = ec_fsm_soe_error;

   507

            EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x"

   508

                    " write response on slave %u.\n", (u32) diff_ms,

   509

                    fsm->request->idn, slave->ring_position);

   510

            return;

   511

        }

   512

   513

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

   514

        fsm->retries = EC_FSM_RETRIES;

   515

        return;

   516

    }

   517

   518

    // Fetch response

   519