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

   244

    fsm->retries = EC_FSM_RETRIES;

   245

    fsm->state = ec_fsm_sii_state_read_fetch;

   246

}

   247

   248

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

   249

   250

/**

   251

   SII state: READ FETCH.

   252

   Fetches the result of an SII-read datagram.

   253

*/

   254

   255

void ec_fsm_sii_state_read_fetch(

   256

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

   257

		)

   258

{

   259

    ec_datagram_t *datagram = fsm->datagram;

   260

   261

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

   262

        return;

   263

   264

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   265

        fsm->state = ec_fsm_sii_state_error;

   266

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

   267

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

   268

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

   269

        return;

   270

    }

   271

   272

    if (datagram->working_counter != 1) {

   273

        fsm->state = ec_fsm_sii_state_error;

   274

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

   275

                fsm->slave->ring_position);

   276

        ec_datagram_print_wc_error(datagram);

   277

        return;

   278

    }

   279

   280

#ifdef SII_DEBUG

   281

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

   282

	ec_print_data(datagram->data, 10);

   283

#endif

   284

   285

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

   286

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

   287

        fsm->state = ec_fsm_sii_state_error;

   288

        return;

   289

    }

   290

   291

    // check "busy bit"

   292

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

   293

												 // read operation busy

   294

        // still busy... timeout?

   295

        unsigned long diff_ms =

   296

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

   297

        if (diff_ms >= SII_TIMEOUT) {

   298

            if (fsm->check_once_more) {

   299

				fsm->check_once_more = 0;

   300

			} else {

   301

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

   302

                fsm->state = ec_fsm_sii_state_error;

   303

                return;

   304

            }

   305

        }

   306

   307

        // issue check/fetch datagram again

   308

        fsm->retries = EC_FSM_RETRIES;

   309

        return;

   310

    }

   311

   312

    // SII value received.

   313

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

   314

    fsm->state = ec_fsm_sii_state_end;

   315

}

   316

   317

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

   318

   319

/**

   320

   SII state: START WRITING.

   321

   Starts writing a word through the slave information interface.

   322

*/

   323

   324

void ec_fsm_sii_state_start_writing(

   325

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

   326

		)

   327

{

   328

    ec_datagram_t *datagram = fsm->datagram;

   329

   330

    // initiate write operation

   331

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

   332

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

   333

											// + enable write access

   334

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

   335

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

   336

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

   337

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

   338

   339

#ifdef SII_DEBUG

   340

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

   341

	ec_print_data(datagram->data, 8);

   342

#endif

   343

   344

    fsm->retries = EC_FSM_RETRIES;

   345

    fsm->state = ec_fsm_sii_state_write_check;

   346

}

   347

   348

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

   349

   350

/**

   351

   SII state: WRITE CHECK.

   352

*/

   353

   354

void ec_fsm_sii_state_write_check(

   355

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

   356

		)

   357

{

   358

    ec_datagram_t *datagram = fsm->datagram;

   359

   360

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

   361

        return;

   362

   363

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   364

        fsm->state = ec_fsm_sii_state_error;

   365

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

   366

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

   367

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

   368

        return;

   369

    }

   370

   371

    if (datagram->working_counter != 1) {

   372

        fsm->state = ec_fsm_sii_state_error;

   373

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

   374

                fsm->slave->ring_position);

   375

        ec_datagram_print_wc_error(datagram);

   376

        return;

   377

    }

   378

   379

    fsm->jiffies_start = datagram->jiffies_sent;

   380

    fsm->check_once_more = 1;

   381

   382

    // issue check datagram

   383

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

   384

    fsm->retries = EC_FSM_RETRIES;

   385

    fsm->state = ec_fsm_sii_state_write_check2;

   386

}

   387

   388

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

   389

   390

/**

   391

   SII state: WRITE CHECK 2.

   392

*/

   393

   394

void ec_fsm_sii_state_write_check2(

   395

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

   396

		)

   397

{

   398

    ec_datagram_t *datagram = fsm->datagram;

   399

    unsigned long diff_ms;

   400

   401

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

   402

        return;

   403

   404

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   405

        fsm->state = ec_fsm_sii_state_error;

   406

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

   407

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

   408

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

   409

        return;

   410

    }

   411

   412

    if (datagram->working_counter != 1) {

   413

        fsm->state = ec_fsm_sii_state_error;

   414

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

   415

                fsm->slave->ring_position);

   416

        ec_datagram_print_wc_error(datagram);

   417

        return;

   418

    }

   419

   420

#ifdef SII_DEBUG

   421

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

   422

	ec_print_data(datagram->data, 2);

   423

#endif

   424

   425

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

   426

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

   427

        fsm->state = ec_fsm_sii_state_error;

   428

        return;

   429

    }

   430

   431

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

   432

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

   433

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

   434

    if (diff_ms < SII_INHIBIT) {

   435

#ifdef SII_DEBUG

   436

		EC_DBG("too early.\n");

   437

#endif

   438

        // issue check datagram again

   439

        fsm->retries = EC_FSM_RETRIES;

   440

        return;

   441

	}

   442

   443

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

   444

												 // write operation busy bit

   445

        // still busy... timeout?

   446

        if (diff_ms >= SII_TIMEOUT) {

   447

            if (fsm->check_once_more) {

   448

				fsm->check_once_more = 0;

   449

			} else {

   450

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

   451

                fsm->state = ec_fsm_sii_state_error;

   452

                return;

   453

            }

   454

        }

   455

   456

        // issue check datagram again

   457

        fsm->retries = EC_FSM_RETRIES;

   458

        return;

   459

    }

   460

   461

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

   462

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

   463

        fsm->state = ec_fsm_sii_state_error;

   464

        return;

   465

    }

   466

   467

    // success

   468

    fsm->state = ec_fsm_sii_state_end;

   469

}

   470

   471

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

   472

   473

/**

   474

   State: ERROR.

   475

*/

   476

   477

void ec_fsm_sii_state_error(

   478

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

   479

		)

   480

{

   481

}

   482

   483

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

   484

   485

/**

   486

   State: END.

   487

*/

   488

   489

void ec_fsm_sii_state_end(

   490

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

   491

		)

   492

{

   493

}

   494

   495

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