1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    15

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

 *  You should have received a copy of the GNU General Public License

    20

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT slave information interface FSM.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "mailbox.h"

    43

#include "master.h"

    44

#include "fsm_sii.h"

    45

    46

/**

    47

 * Read/Write timeout. [ms]

    48

 */

    49

#define EEPROM_TIMEOUT 10

    50

    51

/**

    52

 * Time before evaluating answer at writing. [ms]

    53

 */

    54

#define EEPROM_INHIBIT  5

    55

    56

//#define SII_DEBUG

    57

    58

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

    59

    60

void ec_fsm_sii_state_start_reading(ec_fsm_sii_t *);

    61

void ec_fsm_sii_state_read_check(ec_fsm_sii_t *);

    62

void ec_fsm_sii_state_read_fetch(ec_fsm_sii_t *);

    63

void ec_fsm_sii_state_start_writing(ec_fsm_sii_t *);

    64

void ec_fsm_sii_state_write_check(ec_fsm_sii_t *);

    65

void ec_fsm_sii_state_write_check2(ec_fsm_sii_t *);

    66

void ec_fsm_sii_state_end(ec_fsm_sii_t *);

    67

void ec_fsm_sii_state_error(ec_fsm_sii_t *);

    68

    69

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

    70

    71

/**

    72

   Constructor.

    73

*/

    74

    75

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

    76

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

    77

                     )

    78

{

    79

    fsm->state = NULL;

    80

    fsm->datagram = datagram;

    81

}

    82

    83

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

    84

    85

/**

    86

   Destructor.

    87

*/

    88

    89

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

    90

{

    91

}

    92

    93

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

    94

    95

/**

    96

   Initializes the SII read state machine.

    97

*/

    98

    99

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

   100

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

   101

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

   102

                     ec_fsm_sii_addressing_t mode /**< addressing scheme */

   103

                     )

   104

{

   105

    fsm->state = ec_fsm_sii_state_start_reading;

   106

    fsm->slave = slave;

   107

    fsm->word_offset = word_offset;

   108

    fsm->mode = mode;

   109

}

   110

   111

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

   112

   113

/**

   114

   Initializes the SII write state machine.

   115

*/

   116

   117

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

   118

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

   119

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

   120

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

   121

                      ec_fsm_sii_addressing_t mode /**< addressing scheme */

   122

                      )

   123

{

   124

    fsm->state = ec_fsm_sii_state_start_writing;

   125

    fsm->slave = slave;

   126

    fsm->word_offset = word_offset;

   127

    fsm->mode = mode;

   128

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

   129

}

   130

   131

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

   132

   133

/**

   134

   Executes the SII state machine.

   135

   \return false, if the state machine has terminated

   136

*/

   137

   138

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

   139

{

   140

    fsm->state(fsm);

   141

   142

    return fsm->state != ec_fsm_sii_state_end

   143

		&& fsm->state != ec_fsm_sii_state_error;

   144

}

   145

   146

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

   147

   148

/**

   149

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

   150

   \return non-zero if successful.

   151

*/

   152

   153

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

   154

{

   155

    return fsm->state == ec_fsm_sii_state_end;

   156

}

   157

   158

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

   159

 * state functions

   160

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

   161

   162

/**

   163

   SII state: START READING.

   164

   Starts reading the slave information interface.

   165

*/

   166

   167

void ec_fsm_sii_state_start_reading(

   168

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

   169

		)

   170

{

   171

    ec_datagram_t *datagram = fsm->datagram;

   172

   173

    // initiate read operation

   174

    switch (fsm->mode) {

   175

        case EC_FSM_SII_POSITION:

   176

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

   177

            break;

   178

        case EC_FSM_SII_NODE:

   179

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

   180

            break;

   181

    }

   182

   183

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

   184

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

   185

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

   186

   187

#ifdef SII_DEBUG

   188

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

   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_ERR("Failed to receive SII read datagram from slave %i"

   215

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

   216

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

   217

        return;

   218

    }

   219

   220

    if (datagram->working_counter != 1) {

   221

        fsm->state = ec_fsm_sii_state_error;

   222

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

   223

                fsm->slave->ring_position);

   224

        ec_datagram_print_wc_error(datagram);

   225

        return;

   226

    }

   227

   228

    fsm->cycles_start = datagram->cycles_sent;

   229

    fsm->check_once_more = 1;

   230

   231

    // issue check/fetch datagram

   232

    switch (fsm->mode) {

   233

        case EC_FSM_SII_POSITION:

   234

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

   235

            break;

   236

        case EC_FSM_SII_NODE:

   237

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

   238

            break;

   239

    }

   240

   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 %i"

   264

                " (datagram state %i).\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 %i: ",

   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

        if (datagram->cycles_received

   293

            - fsm->cycles_start >= (cycles_t) EEPROM_TIMEOUT * cpu_khz) {

   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_npwr(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 %i"

   362

                " (datagram state %i).\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 %i: ",

   370

                fsm->slave->ring_position);

   371

        ec_datagram_print_wc_error(datagram);

   372

        return;

   373

    }

   374

   375

    fsm->cycles_start = datagram->cycles_sent;

   376

    fsm->check_once_more = 1;

   377

   378

    // issue check datagram

   379

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

   380

    fsm->retries = EC_FSM_RETRIES;

   381

    fsm->state = ec_fsm_sii_state_write_check2;

   382

}

   383

   384

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

   385

   386

/**

   387

   SII state: WRITE CHECK 2.

   388

*/

   389

   390

void ec_fsm_sii_state_write_check2(

   391

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

   392

		)

   393

{

   394

    ec_datagram_t *datagram = fsm->datagram;

   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 %i"

   402

                " (datagram state %i).\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 %i: ",

   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

	if (datagram->cycles_received - fsm->cycles_start

   429

			< (cycles_t) EEPROM_INHIBIT * cpu_khz) {

   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 (datagram->cycles_received

   442

            - fsm->cycles_start >= (cycles_t) EEPROM_TIMEOUT * cpu_khz) {

   443

            if (fsm->check_once_more) {

   444

				fsm->check_once_more = 0;

   445

			} else {

   446

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

   447

                fsm->state = ec_fsm_sii_state_error;

   448

                return;

   449

            }

   450

        }

   451

   452

        // issue check datagram again

   453

        fsm->retries = EC_FSM_RETRIES;

   454

        return;

   455

    }

   456

   457

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

   458

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

   459

        fsm->state = ec_fsm_sii_state_error;

   460

        return;

   461

    }

   462

   463

    // success

   464

    fsm->state = ec_fsm_sii_state_end;

   465

}

   466

   467

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

   468

   469

/**

   470

   State: ERROR.

   471

*/

   472

   473

void ec_fsm_sii_state_error(

   474

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

   475

		)

   476

{

   477

}

   478

   479

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

   480

   481

/**

   482

   State: END.

   483

*/

   484

   485

void ec_fsm_sii_state_end(

   486

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

   487

		)

   488

{

   489

}

   490

   491

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