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

    fsm->retries = EC_FSM_RETRIES;

   238

    fsm->state = ec_fsm_sii_state_read_fetch;

   239

}

   240

   241

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

   242

   243

/**

   244

   SII state: READ FETCH.

   245

   Fetches the result of an SII-read datagram.

   246

*/

   247

   248

void ec_fsm_sii_state_read_fetch(

   249

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

   250

		)

   251

{

   252

    ec_datagram_t *datagram = fsm->datagram;

   253

   254

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

   255

        return;

   256

   257

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   258

        fsm->state = ec_fsm_sii_state_error;

   259

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

   260

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

   261

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

   262

        return;

   263

    }

   264

   265

    if (datagram->working_counter != 1) {

   266

        fsm->state = ec_fsm_sii_state_error;

   267

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

   268

                fsm->slave->ring_position);

   269

        ec_datagram_print_wc_error(datagram);

   270

        return;

   271

    }

   272

   273

#ifdef SII_DEBUG

   274

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

   275

	ec_print_data(datagram->data, 10);

   276

#endif

   277

   278

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

   279

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

   280

        fsm->state = ec_fsm_sii_state_error;

   281

        return;

   282

    }

   283

   284

    // check "busy bit"

   285

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

   286

												 // read operation busy

   287

        // still busy... timeout?

   288

        unsigned long diff_ms =

   289

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

   290

        if (diff_ms >= SII_TIMEOUT) {

   291

            if (fsm->check_once_more) {

   292

				fsm->check_once_more = 0;

   293

			} else {

   294

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

   295

                fsm->state = ec_fsm_sii_state_error;

   296

                return;

   297

            }

   298

        }

   299

   300

        // issue check/fetch datagram again

   301

        fsm->retries = EC_FSM_RETRIES;

   302

        return;

   303

    }

   304

   305

    // SII value received.

   306

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

   307

    fsm->state = ec_fsm_sii_state_end;

   308

}

   309

   310

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

   311

   312

/**

   313

   SII state: START WRITING.

   314

   Starts writing a word through the slave information interface.

   315

*/

   316

   317

void ec_fsm_sii_state_start_writing(

   318

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

   319

		)

   320

{

   321

    ec_datagram_t *datagram = fsm->datagram;

   322

   323

    // initiate write operation

   324

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

   325

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

   326

											// + enable write access

   327

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

   328

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

   329

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

   330

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

   331

   332

#ifdef SII_DEBUG

   333

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

   334

	ec_print_data(datagram->data, 8);

   335

#endif

   336

   337

    fsm->retries = EC_FSM_RETRIES;

   338

    fsm->state = ec_fsm_sii_state_write_check;

   339

}

   340

   341

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

   342

   343

/**

   344

   SII state: WRITE CHECK.

   345

*/

   346

   347

void ec_fsm_sii_state_write_check(

   348

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

   349

		)

   350

{

   351

    ec_datagram_t *datagram = fsm->datagram;

   352

   353

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

   354

        return;

   355

   356

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   357

        fsm->state = ec_fsm_sii_state_error;

   358

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

   359

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

   360

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

   361

        return;

   362

    }

   363

   364

    if (datagram->working_counter != 1) {

   365

        fsm->state = ec_fsm_sii_state_error;

   366

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

   367

                fsm->slave->ring_position);

   368

        ec_datagram_print_wc_error(datagram);

   369

        return;

   370

    }

   371

   372

    fsm->jiffies_start = datagram->jiffies_sent;

   373

    fsm->check_once_more = 1;

   374

   375

    // issue check datagram

   376

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

   377

    fsm->retries = EC_FSM_RETRIES;

   378

    fsm->state = ec_fsm_sii_state_write_check2;

   379

}

   380

   381

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

   382

   383

/**

   384

   SII state: WRITE CHECK 2.

   385

*/

   386

   387

void ec_fsm_sii_state_write_check2(

   388

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

   389

		)

   390

{

   391

    ec_datagram_t *datagram = fsm->datagram;

   392

    unsigned long diff_ms;

   393

   394

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

   395

        return;

   396

   397

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   398

        fsm->state = ec_fsm_sii_state_error;

   399

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

   400

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

   401

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

   402

        return;

   403

    }

   404

   405

    if (datagram->working_counter != 1) {

   406

        fsm->state = ec_fsm_sii_state_error;

   407

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

   408

                fsm->slave->ring_position);

   409

        ec_datagram_print_wc_error(datagram);

   410

        return;

   411

    }

   412

   413

#ifdef SII_DEBUG

   414

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

   415

	ec_print_data(datagram->data, 2);

   416

#endif

   417

   418

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

   419

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

   420

        fsm->state = ec_fsm_sii_state_error;

   421

        return;

   422

    }

   423

   424

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

   425

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

   426

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

   427

    if (diff_ms < SII_INHIBIT) {

   428

#ifdef SII_DEBUG

   429

		EC_DBG("too early.\n");

   430

#endif

   431

        // issue check datagram again

   432

        fsm->retries = EC_FSM_RETRIES;

   433

        return;

   434

	}

   435

   436

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

   437

												 // write operation busy bit

   438

        // still busy... timeout?

   439

        if (diff_ms >= SII_TIMEOUT) {

   440

            if (fsm->check_once_more) {

   441

				fsm->check_once_more = 0;

   442

			} else {

   443

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

   444

                fsm->state = ec_fsm_sii_state_error;

   445

                return;

   446

            }

   447

        }

   448

   449

        // issue check datagram again

   450

        fsm->retries = EC_FSM_RETRIES;

   451

        return;

   452

    }

   453

   454

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

   455

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

   456

        fsm->state = ec_fsm_sii_state_error;

   457

        return;

   458

    }

   459

   460

    // success

   461

    fsm->state = ec_fsm_sii_state_end;

   462

}

   463

   464

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

   465

   466

/**

   467

   State: ERROR.

   468

*/

   469

   470

void ec_fsm_sii_state_error(

   471

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

   472

		)

   473

{

   474

}

   475

   476

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

   477

   478

/**

   479

   State: END.

   480

*/

   481

   482

void ec_fsm_sii_state_end(

   483

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

   484

		)

   485

{

   486

}

   487

   488

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