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_SLAVE_DBG(fsm->slave, 0, "reading SII data, word %u:\n",

   188

            fsm->word_offset);

   189

    ec_print_data(datagram->data, 4);

   190

#endif

   191

   192

    fsm->retries = EC_FSM_RETRIES;

   193

    fsm->state = ec_fsm_sii_state_read_check;

   194

}

   195

   196

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

   197

   198

/**

   199

   SII state: READ CHECK.

   200

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

   201

*/

   202

   203

void ec_fsm_sii_state_read_check(

   204

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

   205

        )

   206

{

   207

    ec_datagram_t *datagram = fsm->datagram;

   208

   209

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

   210

        return;

   211

   212

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   213

        fsm->state = ec_fsm_sii_state_error;

   214

        EC_SLAVE_ERR(fsm->slave, "Failed to receive SII read datagram: ");

   215

        ec_datagram_print_state(datagram);

   216

        return;

   217

    }

   218

   219

    if (datagram->working_counter != 1) {

   220

        fsm->state = ec_fsm_sii_state_error;

   221

        EC_SLAVE_ERR(fsm->slave, "Reception of SII read datagram failed: ");

   222

        ec_datagram_print_wc_error(datagram);

   223

        return;

   224

    }

   225

   226

    fsm->jiffies_start = datagram->jiffies_sent;

   227

    fsm->check_once_more = 1;

   228

   229

    // issue check/fetch datagram

   230

    switch (fsm->mode) {

   231

        case EC_FSM_SII_USE_INCREMENT_ADDRESS:

   232

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

   233

            break;

   234

        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:

   235

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

   236

            break;

   237

    }

   238

   239

    ec_datagram_zero(datagram);

   240

    fsm->retries = EC_FSM_RETRIES;

   241

    fsm->state = ec_fsm_sii_state_read_fetch;

   242

}

   243

   244

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

   245

   246

/**

   247

   SII state: READ FETCH.

   248

   Fetches the result of an SII-read datagram.

   249

*/

   250

void ec_fsm_sii_state_read_fetch(

   251

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

   252

        )

   253

{

   254

    ec_datagram_t *datagram = fsm->datagram;

   255

   256

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

   257

        return;

   258

   259

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   260

        fsm->state = ec_fsm_sii_state_error;

   261

        EC_SLAVE_ERR(fsm->slave,

   262

                "Failed to receive SII check/fetch datagram: ");

   263

        ec_datagram_print_state(datagram);

   264

        return;

   265

    }

   266

   267

    if (datagram->working_counter != 1) {

   268

        fsm->state = ec_fsm_sii_state_error;

   269

        EC_SLAVE_ERR(fsm->slave,

   270

                "Reception of SII check/fetch datagram failed: ");

   271

        ec_datagram_print_wc_error(datagram);

   272

        return;

   273

    }

   274

   275

#ifdef SII_DEBUG

   276

    EC_SLAVE_DBG(fsm->slave, 0, "checking SII read state:\n");

   277

    ec_print_data(datagram->data, 10);

   278

#endif

   279

   280

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

   281

        EC_SLAVE_ERR(fsm->slave, "Error on last command while"

   282

                " reading from SII word 0x%04x.\n", fsm->word_offset);

   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_SLAVE_ERR(fsm->slave, "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_SLAVE_DBG(fsm->slave, 0, "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_SLAVE_ERR(fsm->slave, "Failed to receive SII write datagram: ");

   362

        ec_datagram_print_state(datagram);

   363

        return;

   364

    }

   365

   366

    if (datagram->working_counter != 1) {

   367

        fsm->state = ec_fsm_sii_state_error;

   368

        EC_SLAVE_ERR(fsm->slave, "Reception of SII write datagram failed: ");

   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_SLAVE_ERR(fsm->slave,

   402

                "Failed to receive SII write check datagram: ");

   403

        ec_datagram_print_state(datagram);

   404

        return;

   405

    }

   406

   407

    if (datagram->working_counter != 1) {

   408

        fsm->state = ec_fsm_sii_state_error;

   409

        EC_SLAVE_ERR(fsm->slave,

   410

                "Reception of SII write check datagram failed: ");

   411

        ec_datagram_print_wc_error(datagram);

   412

        return;

   413

    }

   414

   415

#ifdef SII_DEBUG

   416

    EC_SLAVE_DBG(fsm->slave, 0, "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_SLAVE_ERR(fsm->slave, "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_SLAVE_DBG(fsm->slave, 0, "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_SLAVE_ERR(fsm->slave, "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_SLAVE_ERR(fsm->slave, "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

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