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

    33

*/

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "mailbox.h"

    39

#include "master.h"

    40

#include "fsm_sii.h"

    41

    42

/** Read/write timeout [ms].

    43

 *

    44

 * Used to calculate timeouts bsed on the jiffies counter.

    45

 *

    46

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

    47

 * a timer interupt frequency of 100 Hz.

    48

 */

    49

#define SII_TIMEOUT 20

    50

    51

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

    52

 */

    53

#define SII_INHIBIT 5

    54

    55

//#define SII_DEBUG

    56

    57

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

    58

    59

void ec_fsm_sii_state_start_reading(ec_fsm_sii_t *);

    60

void ec_fsm_sii_state_read_check(ec_fsm_sii_t *);

    61

void ec_fsm_sii_state_read_fetch(ec_fsm_sii_t *);

    62

void ec_fsm_sii_state_start_writing(ec_fsm_sii_t *);

    63

void ec_fsm_sii_state_write_check(ec_fsm_sii_t *);

    64

void ec_fsm_sii_state_write_check2(ec_fsm_sii_t *);

    65

void ec_fsm_sii_state_end(ec_fsm_sii_t *);

    66

void ec_fsm_sii_state_error(ec_fsm_sii_t *);

    67

    68

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

    69

    70

/**

    71

   Constructor.

    72

*/

    73

    74

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

    75

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

    76

                     )

    77

{

    78

    fsm->state = NULL;

    79

    fsm->datagram = datagram;

    80

}

    81

    82

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

    83

    84

/**

    85

   Destructor.

    86

*/

    87

    88

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

    89

{

    90

}

    91

    92

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

    93

    94

/**

    95

   Initializes the SII read state machine.

    96

*/

    97

    98

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

    99

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

   100

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

   101

                     ec_fsm_sii_addressing_t mode /**< addressing scheme */

   102

                     )

   103

{

   104

    fsm->state = ec_fsm_sii_state_start_reading;

   105

    fsm->slave = slave;

   106

    fsm->word_offset = word_offset;

   107

    fsm->mode = mode;

   108

}

   109

   110

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

   111

   112

/**

   113

   Initializes the SII write state machine.

   114

*/

   115

   116

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

   117

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

   118

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

   119

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

   120

                      ec_fsm_sii_addressing_t mode /**< addressing scheme */

   121

                      )

   122

{

   123

    fsm->state = ec_fsm_sii_state_start_writing;

   124

    fsm->slave = slave;

   125

    fsm->word_offset = word_offset;

   126

    fsm->mode = mode;

   127

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

   128

}

   129

   130

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

   131

   132

/**

   133

   Executes the SII state machine.

   134

   \return false, if the state machine has terminated

   135

*/

   136

   137

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

   138

{

   139

    fsm->state(fsm);

   140

   141

    return fsm->state != ec_fsm_sii_state_end

   142

		&& fsm->state != ec_fsm_sii_state_error;

   143

}

   144

   145

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

   146

   147

/**

   148

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

   149

   \return non-zero if successful.

   150

*/

   151

   152

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

   153

{

   154

    return fsm->state == ec_fsm_sii_state_end;

   155

}

   156

   157

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

   158

 * state functions

   159

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

   160

   161

/**

   162

   SII state: START READING.

   163

   Starts reading the slave information interface.

   164

*/

   165

   166

void ec_fsm_sii_state_start_reading(

   167

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

   168

		)

   169

{

   170

    ec_datagram_t *datagram = fsm->datagram;

   171

   172

    // initiate read operation

   173

    switch (fsm->mode) {

   174

        case EC_FSM_SII_USE_INCREMENT_ADDRESS:

   175

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

   176

            break;

   177

        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:

   178

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

   179

            break;

   180

    }

   181

   182

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

   183

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

   184

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

   185

   186

#ifdef SII_DEBUG

   187

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

   188

	ec_print_data(datagram->data, 4);

   189

#endif

   190

   191

    fsm->retries = EC_FSM_RETRIES;

   192

    fsm->state = ec_fsm_sii_state_read_check;

   193

}

   194

   195

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

   196

   197

/**

   198

   SII state: READ CHECK.

   199

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

   200

*/

   201

   202

void ec_fsm_sii_state_read_check(

   203

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

   204

		)

   205

{

   206

    ec_datagram_t *datagram = fsm->datagram;

   207

   208

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

   209

        return;

   210

   211

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   212

        fsm->state = ec_fsm_sii_state_error;

   213

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

   214

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

   215

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

   216

        return;

   217

    }

   218

   219

    if (datagram->working_counter != 1) {

   220

        fsm->state = ec_fsm_sii_state_error;

   221

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

   222

                fsm->slave->ring_position);

   223

        ec_datagram_print_wc_error(datagram);

   224

        return;

   225

    }

   226

   227

    fsm->jiffies_start = datagram->jiffies_sent;

   228

    fsm->check_once_more = 1;

   229

   230

    // issue check/fetch datagram

   231

    switch (fsm->mode) {

   232

        case EC_FSM_SII_USE_INCREMENT_ADDRESS:

   233

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

   234

            break;

   235

        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:

   236

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

   237

            break;

   238

    }

   239

   240

    ec_datagram_zero(datagram);

   241

    fsm->retries = EC_FSM_RETRIES;

   242

    fsm->state = ec_fsm_sii_state_read_fetch;

   243

}

   244

   245

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

   246

   247

/**

   248

   SII state: READ FETCH.

   249

   Fetches the result of an SII-read datagram.

   250

*/

   251

   252

void ec_fsm_sii_state_read_fetch(

   253

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

   254

		)

   255

{

   256

    ec_datagram_t *datagram = fsm->datagram;

   257

   258

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

   259

        return;

   260

   261

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   262

        fsm->state = ec_fsm_sii_state_error;

   263

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

   264

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

   265

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

   266

        return;

   267

    }

   268

   269

    if (datagram->working_counter != 1) {

   270

        fsm->state = ec_fsm_sii_state_error;

   271

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

   272

                fsm->slave->ring_position);

   273

        ec_datagram_print_wc_error(datagram);

   274

        return;

   275

    }

   276

   277

#ifdef SII_DEBUG

   278

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

   279

	ec_print_data(datagram->data, 10);

   280

#endif

   281

   282

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

   283

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

   284

        fsm->state = ec_fsm_sii_state_error;

   285

        return;

   286

    }

   287

   288

    // check "busy bit"

   289

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

   290

												 // read operation busy

   291

        // still busy... timeout?

   292

        unsigned long diff_ms =

   293

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

   294

        if (diff_ms >= SII_TIMEOUT) {

   295

            if (fsm->check_once_more) {

   296

				fsm->check_once_more = 0;

   297

			} else {

   298

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

   299

                fsm->state = ec_fsm_sii_state_error;

   300

                return;

   301

            }

   302

        }

   303

   304

        // issue check/fetch datagram again

   305

        fsm->retries = EC_FSM_RETRIES;

   306

        return;

   307

    }

   308

   309

    // SII value received.

   310

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

   311

    fsm->state = ec_fsm_sii_state_end;

   312

}

   313

   314

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

   315

   316

/**

   317

   SII state: START WRITING.

   318

   Starts writing a word through the slave information interface.

   319

*/

   320

   321

void ec_fsm_sii_state_start_writing(

   322

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

   323

		)

   324

{

   325

    ec_datagram_t *datagram = fsm->datagram;

   326

   327

    // initiate write operation

   328

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

   329

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

   330

											// + enable write access

   331

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

   332

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

   333

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

   334

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

   335

   336

#ifdef SII_DEBUG

   337

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

   338

	ec_print_data(datagram->data, 8);

   339

#endif

   340

   341

    fsm->retries = EC_FSM_RETRIES;

   342

    fsm->state = ec_fsm_sii_state_write_check;

   343

}

   344

   345

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

   346

   347

/**

   348

   SII state: WRITE CHECK.

   349

*/

   350

   351

void ec_fsm_sii_state_write_check(

   352

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

   353

		)

   354

{

   355

    ec_datagram_t *datagram = fsm->datagram;

   356

   357

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

   358

        return;

   359

   360

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   361

        fsm->state = ec_fsm_sii_state_error;

   362

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

   363

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

   364

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

   365

        return;

   366

    }

   367

   368

    if (datagram->working_counter != 1) {

   369

        fsm->state = ec_fsm_sii_state_error;

   370

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

   371

                fsm->slave->ring_position);

   372

        ec_datagram_print_wc_error(datagram);

   373

        return;

   374

    }

   375

   376

    fsm->jiffies_start = datagram->jiffies_sent;

   377

    fsm->check_once_more = 1;

   378

   379

    // issue check datagram

   380

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

   381

    ec_datagram_zero(datagram);

   382

    fsm->retries = EC_FSM_RETRIES;

   383

    fsm->state = ec_fsm_sii_state_write_check2;

   384

}

   385

   386

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

   387

   388

/**

   389

   SII state: WRITE CHECK 2.

   390

*/

   391

   392

void ec_fsm_sii_state_write_check2(

   393

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

   394

		)

   395

{

   396

    ec_datagram_t *datagram = fsm->datagram;

   397

    unsigned long diff_ms;

   398

   399

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

   400

        return;

   401

   402

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   403

        fsm->state = ec_fsm_sii_state_error;

   404

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

   405

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

   406

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

   407

        return;

   408

    }

   409

   410

    if (datagram->working_counter != 1) {

   411

        fsm->state = ec_fsm_sii_state_error;

   412

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

   413

                fsm->slave->ring_position);

   414

        ec_datagram_print_wc_error(datagram);

   415

        return;

   416

    }

   417

   418

#ifdef SII_DEBUG

   419

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

   420

	ec_print_data(datagram->data, 2);

   421

#endif

   422

   423

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

   424

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

   425

        fsm->state = ec_fsm_sii_state_error;

   426

        return;

   427

    }

   428

   429

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

   430

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

   431

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

   432

    if (diff_ms < SII_INHIBIT) {

   433

#ifdef SII_DEBUG

   434

		EC_DBG("too early.\n");

   435

#endif

   436

        // issue check datagram again

   437

        fsm->retries = EC_FSM_RETRIES;

   438

        return;

   439

	}

   440

   441

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

   442

												 // write operation busy bit

   443

        // still busy... timeout?

   444

        if (diff_ms >= SII_TIMEOUT) {

   445

            if (fsm->check_once_more) {

   446

				fsm->check_once_more = 0;

   447

			} else {

   448

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

   449

                fsm->state = ec_fsm_sii_state_error;

   450

                return;

   451

            }

   452

        }

   453

   454

        // issue check datagram again

   455

        fsm->retries = EC_FSM_RETRIES;

   456

        return;

   457

    }

   458

   459

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

   460

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

   461

        fsm->state = ec_fsm_sii_state_error;

   462

        return;

   463

    }

   464

   465

    // success

   466

    fsm->state = ec_fsm_sii_state_end;

   467

}

   468

   469

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

   470

   471

/**

   472

   State: ERROR.

   473

*/

   474

   475

void ec_fsm_sii_state_error(

   476

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

   477

		)

   478

{

   479

}

   480

   481

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

   482

   483

/**

   484

   State: END.

   485

*/

   486

   487

void ec_fsm_sii_state_end(

   488

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

   489

		)

   490

{

   491

}

   492

   493

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