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

void ec_fsm_sii_state_read_fetch(

   252

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

   253

        )

   254

{

   255

    ec_datagram_t *datagram = fsm->datagram;

   256

   257

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

   258

        return;

   259

   260

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   261

        fsm->state = ec_fsm_sii_state_error;

   262

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

   263

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

   264

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

   265

        return;

   266

    }

   267

   268

    if (datagram->working_counter != 1) {

   269

        fsm->state = ec_fsm_sii_state_error;

   270

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

   271

                fsm->slave->ring_position);

   272

        ec_datagram_print_wc_error(datagram);

   273

        return;

   274

    }

   275

   276

#ifdef SII_DEBUG

   277

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

   278

    ec_print_data(datagram->data, 10);

   279

#endif

   280

   281

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

   282

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

   283

        fsm->state = ec_fsm_sii_state_error;

   284

        return;

   285

    }

   286

   287

    // check "busy bit"

   288

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

   289

                                                    read operation busy */

   290

        // still busy... timeout?

   291

        unsigned long diff_ms =

   292

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

   293

        if (diff_ms >= SII_TIMEOUT) {

   294

            if (fsm->check_once_more) {

   295

                fsm->check_once_more = 0;

   296

            } else {

   297

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

   298

                fsm->state = ec_fsm_sii_state_error;

   299

                return;

   300

            }

   301

        }

   302

   303

        // issue check/fetch datagram again

   304

        fsm->retries = EC_FSM_RETRIES;

   305

        return;

   306

    }

   307

   308

    // SII value received.

   309

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

   310

    fsm->state = ec_fsm_sii_state_end;

   311

}

   312

   313

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

   314

   315

/**

   316

   SII state: START WRITING.

   317

   Starts writing a word through the slave information interface.

   318

*/

   319

   320

void ec_fsm_sii_state_start_writing(

   321

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

   322

        )

   323

{

   324

    ec_datagram_t *datagram = fsm->datagram;

   325

   326

    // initiate write operation

   327

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

   328

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

   329

                                               + enable write access */

   330

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

   331

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

   332

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

   333

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

   334

   335

#ifdef SII_DEBUG

   336

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

   337

    ec_print_data(datagram->data, 8);

   338

#endif

   339

   340

    fsm->retries = EC_FSM_RETRIES;

   341

    fsm->state = ec_fsm_sii_state_write_check;

   342

}

   343

   344

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

   345

   346

/**

   347

   SII state: WRITE CHECK.

   348

*/

   349

   350

void ec_fsm_sii_state_write_check(

   351

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

   352

        )

   353

{

   354

    ec_datagram_t *datagram = fsm->datagram;

   355

   356

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

   357

        return;

   358

   359

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   360

        fsm->state = ec_fsm_sii_state_error;

   361

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

   362

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

   363

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

   364

        return;

   365

    }

   366

   367

    if (datagram->working_counter != 1) {

   368

        fsm->state = ec_fsm_sii_state_error;

   369

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

   370

                fsm->slave->ring_position);

   371

        ec_datagram_print_wc_error(datagram);

   372

        return;

   373

    }

   374

   375

    fsm->jiffies_start = datagram->jiffies_sent;

   376

    fsm->check_once_more = 1;

   377

   378

    // issue check datagram

   379

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

   380

    ec_datagram_zero(datagram);

   381

    fsm->retries = EC_FSM_RETRIES;

   382

    fsm->state = ec_fsm_sii_state_write_check2;

   383

}

   384

   385

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

   386

   387

/**

   388

   SII state: WRITE CHECK 2.

   389

*/

   390

   391

void ec_fsm_sii_state_write_check2(

   392

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

   393

        )

   394

{

   395

    ec_datagram_t *datagram = fsm->datagram;

   396

    unsigned long diff_ms;

   397

   398

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

   399

        return;

   400

   401

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   402

        fsm->state = ec_fsm_sii_state_error;

   403

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

   404

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

   405

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

   406

        return;

   407

    }

   408

   409

    if (datagram->working_counter != 1) {

   410

        fsm->state = ec_fsm_sii_state_error;

   411

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

   412

                fsm->slave->ring_position);

   413

        ec_datagram_print_wc_error(datagram);

   414

        return;

   415

    }

   416

   417

#ifdef SII_DEBUG

   418

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

   419

    ec_print_data(datagram->data, 2);

   420

#endif

   421

   422

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

   423

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

   424

        fsm->state = ec_fsm_sii_state_error;

   425

        return;

   426

    }

   427

   428

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

   429

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

   430

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

   431

    if (diff_ms < SII_INHIBIT) {

   432

#ifdef SII_DEBUG

   433

        EC_DBG("too early.\n");

   434

#endif

   435

        // issue check datagram again

   436

        fsm->retries = EC_FSM_RETRIES;

   437

        return;

   438

    }

   439

   440

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

   441

                                                    write operation busy bit */

   442

        // still busy... timeout?

   443

        if (diff_ms >= SII_TIMEOUT) {

   444

            if (fsm->check_once_more) {

   445

                fsm->check_once_more = 0;

   446

            } else {

   447

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

   448

                fsm->state = ec_fsm_sii_state_error;

   449

                return;

   450

            }

   451

        }

   452

   453

        // issue check datagram again

   454

        fsm->retries = EC_FSM_RETRIES;

   455

        return;

   456

    }

   457

   458

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

   459

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

   460

        fsm->state = ec_fsm_sii_state_error;

   461

        return;

   462

    }

   463

   464

    // success

   465

    fsm->state = ec_fsm_sii_state_end;

   466

}

   467

   468

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

   469

   470

/**

   471

   State: ERROR.

   472

*/

   473

   474

void ec_fsm_sii_state_error(

   475

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

   476

        )

   477

{

   478

}

   479

   480

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

   481

   482

/**

   483

   State: END.

   484

*/

   485

   486

void ec_fsm_sii_state_end(

   487

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

   488

        )

   489

{

   490

}

   491

   492

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