1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    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 slave information interface FSM.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "mailbox.h"

    43

#include "master.h"

    44

#include "fsm_sii.h"

    45

    46

/** Read/write timeout [ms].

    47

 *

    48

 * Used to calculate timeouts bsed on the jiffies counter.

    49

 *

    50

 * \attention Must be more than 10 to avoid problems on kernels that run with

    51

 * a timer interupt frequency of 100 Hz.

    52

 */

    53

#define SII_TIMEOUT 20

    54

    55

/** Time before evaluating answer at writing [ms].

    56

 */

    57

#define SII_INHIBIT 5

    58

    59

//#define SII_DEBUG

    60

    61

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

    62

    63

void ec_fsm_sii_state_start_reading(ec_fsm_sii_t *);

    64

void ec_fsm_sii_state_read_check(ec_fsm_sii_t *);

    65

void ec_fsm_sii_state_read_fetch(ec_fsm_sii_t *);

    66

void ec_fsm_sii_state_start_writing(ec_fsm_sii_t *);

    67

void ec_fsm_sii_state_write_check(ec_fsm_sii_t *);

    68

void ec_fsm_sii_state_write_check2(ec_fsm_sii_t *);

    69

void ec_fsm_sii_state_end(ec_fsm_sii_t *);

    70

void ec_fsm_sii_state_error(ec_fsm_sii_t *);

    71

    72

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

    73

    74

/**

    75

   Constructor.

    76

*/

    77

    78

void ec_fsm_sii_init(ec_fsm_sii_t *fsm, /**< finite state machine */

    79

                     ec_datagram_t *datagram /**< datagram structure to use */

    80

                     )

    81

{

    82

    fsm->state = NULL;

    83

    fsm->datagram = datagram;

    84

}

    85

    86

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

    87

    88

/**

    89

   Destructor.

    90

*/

    91

    92

void ec_fsm_sii_clear(ec_fsm_sii_t *fsm /**< finite state machine */)

    93

{

    94

}

    95

    96

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

    97

    98

/**

    99

   Initializes the SII read state machine.

   100

*/

   101

   102

void ec_fsm_sii_read(ec_fsm_sii_t *fsm, /**< finite state machine */

   103

                     ec_slave_t *slave, /**< slave to read from */

   104

                     uint16_t word_offset, /**< offset to read from */

   105

                     ec_fsm_sii_addressing_t mode /**< addressing scheme */

   106

                     )

   107

{

   108

    fsm->state = ec_fsm_sii_state_start_reading;

   109

    fsm->slave = slave;

   110

    fsm->word_offset = word_offset;

   111

    fsm->mode = mode;

   112

}

   113

   114

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

   115

   116

/**

   117

   Initializes the SII write state machine.

   118

*/

   119

   120

void ec_fsm_sii_write(ec_fsm_sii_t *fsm, /**< finite state machine */

   121

                      ec_slave_t *slave, /**< slave to read from */

   122

                      uint16_t word_offset, /**< offset to read from */

   123

                      const uint16_t *value, /**< pointer to 2 bytes of data */

   124

                      ec_fsm_sii_addressing_t mode /**< addressing scheme */

   125

                      )

   126

{

   127

    fsm->state = ec_fsm_sii_state_start_writing;

   128

    fsm->slave = slave;

   129

    fsm->word_offset = word_offset;

   130

    fsm->mode = mode;

   131

    memcpy(fsm->value, value, 2);

   132

}

   133

   134

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

   135

   136

/**

   137

   Executes the SII state machine.

   138

   \return false, if the state machine has terminated

   139

*/

   140

   141

int ec_fsm_sii_exec(ec_fsm_sii_t *fsm /**< finite state machine */)

   142

{

   143

    fsm->state(fsm);

   144

   145

    return fsm->state != ec_fsm_sii_state_end

   146

		&& fsm->state != ec_fsm_sii_state_error;

   147

}

   148

   149

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

   150

   151

/**

   152

   Returns, if the master startup state machine terminated with success.

   153

   \return non-zero if successful.

   154

*/

   155

   156

int ec_fsm_sii_success(ec_fsm_sii_t *fsm /**< Finite state machine */)

   157

{

   158

    return fsm->state == ec_fsm_sii_state_end;

   159

}

   160

   161

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

   162

 * state functions

   163

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

   164

   165

/**

   166

   SII state: START READING.

   167

   Starts reading the slave information interface.

   168

*/

   169

   170

void ec_fsm_sii_state_start_reading(

   171

		ec_fsm_sii_t *fsm /**< finite state machine */

   172

		)

   173

{

   174

    ec_datagram_t *datagram = fsm->datagram;

   175

   176

    // initiate read operation

   177

    switch (fsm->mode) {

   178

        case EC_FSM_SII_USE_INCREMENT_ADDRESS:

   179

            ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4);

   180

            break;

   181

        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:

   182

            ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 4);

   183

            break;

   184

    }

   185

   186

    EC_WRITE_U8 (datagram->data,     0x80); // two address octets

   187

    EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation

   188

    EC_WRITE_U16(datagram->data + 2, fsm->word_offset);

   189

   190

#ifdef SII_DEBUG

   191

	EC_DBG("reading SII data:\n");

   192

	ec_print_data(datagram->data, 4);

   193

#endif

   194

   195

    fsm->retries = EC_FSM_RETRIES;

   196

    fsm->state = ec_fsm_sii_state_read_check;

   197

}

   198

   199

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

   200

   201

/**

   202

   SII state: READ CHECK.

   203

   Checks, if the SII-read-datagram has been sent and issues a fetch datagram.

   204

*/

   205

   206

void ec_fsm_sii_state_read_check(

   207

		ec_fsm_sii_t *fsm /**< finite state machine */

   208

		)

   209

{

   210

    ec_datagram_t *datagram = fsm->datagram;

   211

   212

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

   213

        return;

   214

   215

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   216

        fsm->state = ec_fsm_sii_state_error;

   217

        EC_ERR("Failed to receive SII read datagram from slave %u"

   218

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

   219

               fsm->slave->ring_position, datagram->state);

   220

        return;

   221

    }

   222

   223

    if (datagram->working_counter != 1) {

   224

        fsm->state = ec_fsm_sii_state_error;

   225

        EC_ERR("Reception of SII read datagram failed on slave %u: ",

   226

                fsm->slave->ring_position);

   227

        ec_datagram_print_wc_error(datagram);

   228

        return;

   229

    }

   230

   231

    fsm->jiffies_start = datagram->jiffies_sent;

   232

    fsm->check_once_more = 1;

   233

   234

    // issue check/fetch datagram

   235

    switch (fsm->mode) {

   236

        case EC_FSM_SII_USE_INCREMENT_ADDRESS:

   237

            ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10);

   238

            break;

   239

        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:

   240

            ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 10);

   241

            break;

   242

    }

   243

   245

    fsm->retries = EC_FSM_RETRIES;

   246

    fsm->state = ec_fsm_sii_state_read_fetch;

   247

}

   248

   249

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

   250

   251

/**

   252

   SII state: READ FETCH.

   253

   Fetches the result of an SII-read datagram.

   254

*/

   255

   256

void ec_fsm_sii_state_read_fetch(

   257

		ec_fsm_sii_t *fsm /**< finite state machine */

   258

		)

   259

{

   260

    ec_datagram_t *datagram = fsm->datagram;

   261

   262

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

   263

        return;

   264

   265

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   266

        fsm->state = ec_fsm_sii_state_error;

   267

        EC_ERR("Failed to receive SII check/fetch datagram from slave %u"

   268

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

   269

               fsm->slave->ring_position, datagram->state);

   270

        return;

   271

    }

   272

   273

    if (datagram->working_counter != 1) {

   274

        fsm->state = ec_fsm_sii_state_error;

   275

        EC_ERR("Reception of SII check/fetch datagram failed on slave %u: ",

   276

                fsm->slave->ring_position);

   277

        ec_datagram_print_wc_error(datagram);

   278

        return;

   279

    }

   280

   281

#ifdef SII_DEBUG

   282

	EC_DBG("checking SII read state:\n");

   283

	ec_print_data(datagram->data, 10);

   284

#endif

   285

   286

    if (EC_READ_U8(datagram->data + 1) & 0x20) {

   287

        EC_ERR("SII: Error on last SII command!\n");

   288

        fsm->state = ec_fsm_sii_state_error;

   289

        return;

   290

    }

   291

   292

    // check "busy bit"

   293

    if (EC_READ_U8(datagram->data + 1) & 0x81) { // busy bit or

   294

												 // read operation busy

   295

        // still busy... timeout?

   296

        unsigned long diff_ms =

   297

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

   298

        if (diff_ms >= SII_TIMEOUT) {

   299

            if (fsm->check_once_more) {

   300

				fsm->check_once_more = 0;

   301

			} else {

   302

                EC_ERR("SII: Read timeout.\n");

   303

                fsm->state = ec_fsm_sii_state_error;

   304

                return;

   305

            }

   306

        }

   307

   308

        // issue check/fetch datagram again

   309

        fsm->retries = EC_FSM_RETRIES;

   310

        return;

   311

    }

   312

   313

    // SII value received.

   314

    memcpy(fsm->value, datagram->data + 6, 4);

   315

    fsm->state = ec_fsm_sii_state_end;

   316

}

   317

   318

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

   319

   320

/**

   321

   SII state: START WRITING.

   322

   Starts writing a word through the slave information interface.

   323

*/

   324

   325

void ec_fsm_sii_state_start_writing(

   326

		ec_fsm_sii_t *fsm /**< finite state machine */

   327

		)

   328

{

   329

    ec_datagram_t *datagram = fsm->datagram;

   330

   331

    // initiate write operation

   332

    ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 8);

   333

    EC_WRITE_U8 (datagram->data,     0x81); // two address octets

   334

											// + enable write access

   335

    EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation

   336

    EC_WRITE_U16(datagram->data + 2, fsm->word_offset);

   337

	memset(datagram->data + 4, 0x00, 2);

   338

    memcpy(datagram->data + 6, fsm->value, 2);

   339

   340

#ifdef SII_DEBUG

   341

	EC_DBG("writing SII data:\n");

   342

	ec_print_data(datagram->data, 8);

   343

#endif

   344

   345

    fsm->retries = EC_FSM_RETRIES;

   346

    fsm->state = ec_fsm_sii_state_write_check;

   347

}

   348

   349

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

   350

   351

/**

   352

   SII state: WRITE CHECK.

   353

*/

   354

   355

void ec_fsm_sii_state_write_check(

   356

		ec_fsm_sii_t *fsm /**< finite state machine */

   357

		)

   358

{

   359

    ec_datagram_t *datagram = fsm->datagram;

   360

   361

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

   362

        return;

   363

   364

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   365

        fsm->state = ec_fsm_sii_state_error;

   366

        EC_ERR("Failed to receive SII write datagram for slave %u"

   367

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

   368

               fsm->slave->ring_position, datagram->state);

   369

        return;

   370

    }

   371

   372

    if (datagram->working_counter != 1) {

   373

        fsm->state = ec_fsm_sii_state_error;

   374

        EC_ERR("Reception of SII write datagram failed on slave %u: ",

   375

                fsm->slave->ring_position);

   376

        ec_datagram_print_wc_error(datagram);

   377

        return;

   378

    }

   379

   380

    fsm->jiffies_start = datagram->jiffies_sent;

   381

    fsm->check_once_more = 1;

   382

   383

    // issue check datagram

   384

    ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 2);

   386

    fsm->retries = EC_FSM_RETRIES;

   387

    fsm->state = ec_fsm_sii_state_write_check2;

   388

}

   389

   390

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

   391

   392

/**

   393

   SII state: WRITE CHECK 2.

   394

*/

   395

   396

void ec_fsm_sii_state_write_check2(

   397

		ec_fsm_sii_t *fsm /**< finite state machine */

   398

		)

   399

{

   400

    ec_datagram_t *datagram = fsm->datagram;

   401

    unsigned long diff_ms;

   402

   403

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

   404

        return;

   405

   406

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   407

        fsm->state = ec_fsm_sii_state_error;

   408

        EC_ERR("Failed to receive SII write check datagram from slave %u"

   409

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

   410

               fsm->slave->ring_position, datagram->state);

   411

        return;

   412

    }

   413

   414

    if (datagram->working_counter != 1) {

   415

        fsm->state = ec_fsm_sii_state_error;

   416

        EC_ERR("Reception of SII write check datagram failed on slave %u: ",

   417

                fsm->slave->ring_position);

   418

        ec_datagram_print_wc_error(datagram);

   419

        return;

   420

    }

   421

   422

#ifdef SII_DEBUG

   423

	EC_DBG("checking SII write state:\n");

   424

	ec_print_data(datagram->data, 2);

   425

#endif

   426

   427

    if (EC_READ_U8(datagram->data + 1) & 0x20) {

   428

        EC_ERR("SII: Error on last SII command!\n");

   429

        fsm->state = ec_fsm_sii_state_error;

   430

        return;

   431

    }

   432

   433

	/* FIXME: some slaves never answer with the busy flag set...

   434

	 * wait a few ms for the write operation to complete. */

   435

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

   436

    if (diff_ms < SII_INHIBIT) {

   437

#ifdef SII_DEBUG

   438

		EC_DBG("too early.\n");

   439

#endif

   440

        // issue check datagram again

   441

        fsm->retries = EC_FSM_RETRIES;

   442

        return;

   443

	}

   444

   445

    if (EC_READ_U8(datagram->data + 1) & 0x82) { // busy bit or

   446

												 // write operation busy bit

   447

        // still busy... timeout?

   448

        if (diff_ms >= SII_TIMEOUT) {

   449

            if (fsm->check_once_more) {

   450

				fsm->check_once_more = 0;

   451

			} else {

   452

                EC_ERR("SII: Write timeout.\n");

   453

                fsm->state = ec_fsm_sii_state_error;

   454

                return;

   455

            }

   456

        }

   457

   458

        // issue check datagram again

   459

        fsm->retries = EC_FSM_RETRIES;

   460

        return;

   461

    }

   462

   463

    if (EC_READ_U8(datagram->data + 1) & 0x40) {

   464

        EC_ERR("SII: Write operation failed!\n");

   465

        fsm->state = ec_fsm_sii_state_error;

   466

        return;

   467

    }

   468

   469

    // success

   470

    fsm->state = ec_fsm_sii_state_end;

   471

}

   472

   473

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

   474

   475

/**

   476

   State: ERROR.

   477

*/

   478

   479

void ec_fsm_sii_state_error(

   480

		ec_fsm_sii_t *fsm /**< finite state machine */

   481

		)

   482

{

   483

}

   484

   485

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

   486

   487

/**

   488

   State: END.

   489

*/

   490

   491

void ec_fsm_sii_state_end(

   492

		ec_fsm_sii_t *fsm /**< finite state machine */

   493

		)

   494

{

   495

}

   496

   497

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