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

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/**

    28

   \file

    29

   EtherCAT slave information interface FSM.

    30

*/

    31

    32

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

    33

    34

#include "globals.h"

    35

#include "mailbox.h"

    36

#include "master.h"

    37

#include "fsm_sii.h"

    38

    39

/** Read/write timeout [ms].

    40

 *

    41

 * Used to calculate timeouts bsed on the jiffies counter.

    42

 *

    43

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

    44

 * a timer interupt frequency of 100 Hz.

    45

 */

    46

#define SII_TIMEOUT 20

    47

    48

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

    49

 */

    50

#define SII_INHIBIT 5

    51

    52

//#define SII_DEBUG

    53

    54

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

    55

    56

void ec_fsm_sii_state_start_reading(ec_fsm_sii_t *);

    57

void ec_fsm_sii_state_read_check(ec_fsm_sii_t *);

    58

void ec_fsm_sii_state_read_fetch(ec_fsm_sii_t *);

    59

void ec_fsm_sii_state_start_writing(ec_fsm_sii_t *);

    60

void ec_fsm_sii_state_write_check(ec_fsm_sii_t *);

    61

void ec_fsm_sii_state_write_check2(ec_fsm_sii_t *);

    62

void ec_fsm_sii_state_end(ec_fsm_sii_t *);

    63

void ec_fsm_sii_state_error(ec_fsm_sii_t *);

    64

    65

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

    66

    67

/**

    68

   Constructor.

    69

*/

    70

    71

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

    72

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

    73

                     )

    74

{

    75

    fsm->state = NULL;

    76

    fsm->datagram = datagram;

    77

}

    78

    79

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

    80

    81

/**

    82

   Destructor.

    83

*/

    84

    85

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

    86

{

    87

}

    88

    89

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

    90

    91

/**

    92

   Initializes the SII read state machine.

    93

*/

    94

    95

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

    96

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

    97

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

    98

                     ec_fsm_sii_addressing_t mode /**< addressing scheme */

    99

                     )

   100

{

   101

    fsm->state = ec_fsm_sii_state_start_reading;

   102

    fsm->slave = slave;

   103

    fsm->word_offset = word_offset;

   104

    fsm->mode = mode;

   105

}

   106

   107

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

   108

   109

/**

   110

   Initializes the SII write state machine.

   111

*/

   112

   113

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

   114

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

   115

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

   116

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

   117

                      ec_fsm_sii_addressing_t mode /**< addressing scheme */

   118

                      )

   119

{

   120

    fsm->state = ec_fsm_sii_state_start_writing;

   121

    fsm->slave = slave;

   122

    fsm->word_offset = word_offset;

   123

    fsm->mode = mode;

   124

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

   125

}

   126

   127

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

   128

   129

/**

   130

   Executes the SII state machine.

   131

   \return false, if the state machine has terminated

   132

*/

   133

   134

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

   135

{

   136

    fsm->state(fsm);

   137

   138

    return fsm->state != ec_fsm_sii_state_end

   139

		&& fsm->state != ec_fsm_sii_state_error;

   140

}

   141

   142

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

   143

   144

/**

   145

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

   146

   \return non-zero if successful.

   147

*/

   148

   149

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

   150

{

   151

    return fsm->state == ec_fsm_sii_state_end;

   152

}

   153

   154

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

   155

 * state functions

   156

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

   157

   158

/**

   159

   SII state: START READING.

   160

   Starts reading the slave information interface.

   161

*/

   162

   163

void ec_fsm_sii_state_start_reading(

   164

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

   165

		)

   166

{

   167

    ec_datagram_t *datagram = fsm->datagram;

   168

   169

    // initiate read operation

   170

    switch (fsm->mode) {

   171

        case EC_FSM_SII_USE_INCREMENT_ADDRESS:

   172

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

   173

            break;

   174

        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:

   175

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

   176

            break;

   177

    }

   178

   179

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

   180

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

   181

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

   182

   183

#ifdef SII_DEBUG

   184

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

   185

	ec_print_data(datagram->data, 4);

   186

#endif

   187

   188

    fsm->retries = EC_FSM_RETRIES;

   189

    fsm->state = ec_fsm_sii_state_read_check;

   190

}

   191

   192

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

   193

   194

/**

   195

   SII state: READ CHECK.

   196

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

   197

*/

   198

   199

void ec_fsm_sii_state_read_check(

   200

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

   201

		)

   202

{

   203

    ec_datagram_t *datagram = fsm->datagram;

   204

   205

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

   206

        return;

   207

   208

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   209

        fsm->state = ec_fsm_sii_state_error;

   210

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

   211

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

   212

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

   213

        return;

   214

    }

   215

   216

    if (datagram->working_counter != 1) {

   217

        fsm->state = ec_fsm_sii_state_error;

   218

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

   219

                fsm->slave->ring_position);

   220

        ec_datagram_print_wc_error(datagram);

   221

        return;

   222

    }

   223

   224

    fsm->jiffies_start = datagram->jiffies_sent;

   225

    fsm->check_once_more = 1;

   226

   227

    // issue check/fetch datagram

   228

    switch (fsm->mode) {

   229

        case EC_FSM_SII_USE_INCREMENT_ADDRESS:

   230

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

   231

            break;

   232

        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:

   233

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

   234

            break;

   235

    }

   236

   237

    ec_datagram_zero(datagram);

   238

    fsm->retries = EC_FSM_RETRIES;

   239

    fsm->state = ec_fsm_sii_state_read_fetch;

   240

}

   241

   242

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

   243

   244

/**

   245

   SII state: READ FETCH.

   246

   Fetches the result of an SII-read datagram.

   247

*/

   248

   249

void ec_fsm_sii_state_read_fetch(

   250

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

   251

		)

   252

{

   253

    ec_datagram_t *datagram = fsm->datagram;

   254

   255

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

   256

        return;

   257

   258

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   259

        fsm->state = ec_fsm_sii_state_error;

   260

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

   261

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

   262

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

   263

        return;

   264

    }

   265

   266

    if (datagram->working_counter != 1) {

   267

        fsm->state = ec_fsm_sii_state_error;

   268

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

   269

                fsm->slave->ring_position);

   270

        ec_datagram_print_wc_error(datagram);

   271

        return;

   272

    }

   273

   274

#ifdef SII_DEBUG

   275

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

   276

	ec_print_data(datagram->data, 10);

   277

#endif

   278

   279

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

   280

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

   281

        fsm->state = ec_fsm_sii_state_error;

   282

        return;

   283

    }

   284

   285

    // check "busy bit"

   286

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

   287

												 // read operation busy

   288

        // still busy... timeout?

   289

        unsigned long diff_ms =

   290

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

   291

        if (diff_ms >= SII_TIMEOUT) {

   292

            if (fsm->check_once_more) {

   293

				fsm->check_once_more = 0;

   294

			} else {

   295

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

   296

                fsm->state = ec_fsm_sii_state_error;

   297

                return;

   298

            }

   299

        }

   300

   301

        // issue check/fetch datagram again

   302

        fsm->retries = EC_FSM_RETRIES;

   303

        return;

   304

    }

   305

   306

    // SII value received.

   307

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

   308

    fsm->state = ec_fsm_sii_state_end;

   309

}

   310

   311

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

   312

   313

/**

   314

   SII state: START WRITING.

   315

   Starts writing a word through the slave information interface.

   316

*/

   317

   318

void ec_fsm_sii_state_start_writing(

   319

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

   320

		)

   321

{

   322

    ec_datagram_t *datagram = fsm->datagram;

   323

   324

    // initiate write operation

   325

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

   326

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

   327

											// + enable write access

   328

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

   329

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

   330

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

   331

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

   332

   333

#ifdef SII_DEBUG

   334

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

   335

	ec_print_data(datagram->data, 8);

   336

#endif

   337

   338

    fsm->retries = EC_FSM_RETRIES;

   339

    fsm->state = ec_fsm_sii_state_write_check;

   340

}

   341

   342

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

   343

   344

/**

   345

   SII state: WRITE CHECK.

   346

*/

   347

   348

void ec_fsm_sii_state_write_check(

   349

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

   350

		)

   351

{

   352

    ec_datagram_t *datagram = fsm->datagram;

   353

   354

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

   355

        return;

   356

   357

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   358

        fsm->state = ec_fsm_sii_state_error;

   359

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

   360

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

   361

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

   362

        return;

   363

    }

   364

   365

    if (datagram->working_counter != 1) {

   366

        fsm->state = ec_fsm_sii_state_error;

   367

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

   368

                fsm->slave->ring_position);

   369

        ec_datagram_print_wc_error(datagram);

   370

        return;

   371

    }

   372

   373

    fsm->jiffies_start = datagram->jiffies_sent;

   374

    fsm->check_once_more = 1;

   375

   376

    // issue check datagram

   377

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

   378

    ec_datagram_zero(datagram);

   379

    fsm->retries = EC_FSM_RETRIES;

   380

    fsm->state = ec_fsm_sii_state_write_check2;

   381

}

   382

   383

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

   384

   385

/**

   386

   SII state: WRITE CHECK 2.

   387

*/

   388

   389

void ec_fsm_sii_state_write_check2(

   390

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

   391

		)

   392

{

   393

    ec_datagram_t *datagram = fsm->datagram;

   394

    unsigned long diff_ms;

   395

   396

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

   397

        return;

   398

   399

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   400

        fsm->state = ec_fsm_sii_state_error;

   401

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

   402

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

   403

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

   404

        return;

   405

    }

   406

   407

    if (datagram->working_counter != 1) {

   408

        fsm->state = ec_fsm_sii_state_error;

   409

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

   410

                fsm->slave->ring_position);

   411

        ec_datagram_print_wc_error(datagram);

   412

        return;

   413

    }

   414

   415

#ifdef SII_DEBUG

   416

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

   417

	ec_print_data(datagram->data, 2);

   418

#endif

   419

   420

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

   421

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

   422

        fsm->state = ec_fsm_sii_state_error;

   423

        return;

   424

    }

   425

   426

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

   427

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

   428

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

   429

    if (diff_ms < SII_INHIBIT) {

   430

#ifdef SII_DEBUG

   431

		EC_DBG("too early.\n");

   432

#endif

   433

        // issue check datagram again

   434

        fsm->retries = EC_FSM_RETRIES;

   435

        return;

   436

	}

   437

   438

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

   439

												 // write operation busy bit

   440

        // still busy... timeout?

   441

        if (diff_ms >= SII_TIMEOUT) {

   442

            if (fsm->check_once_more) {

   443

				fsm->check_once_more = 0;

   444

			} else {

   445

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

   446

                fsm->state = ec_fsm_sii_state_error;

   447

                return;

   448

            }

   449

        }

   450

   451

        // issue check datagram again

   452

        fsm->retries = EC_FSM_RETRIES;

   453

        return;

   454

    }

   455

   456

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

   457

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

   458

        fsm->state = ec_fsm_sii_state_error;

   459

        return;

   460

    }

   461

   462

    // success

   463

    fsm->state = ec_fsm_sii_state_end;

   464

}

   465

   466

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

   467

   468

/**

   469

   State: ERROR.

   470

*/

   471

   472

void ec_fsm_sii_state_error(

   473

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

   474

		)

   475

{

   476

}

   477

   478

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

   479

   480

/**

   481

   State: END.

   482

*/

   483

   484

void ec_fsm_sii_state_end(

   485

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

   486

		)

   487

{

   488

}

   489

   490

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