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

    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

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT finite state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "fsm.h"

    43

#include "master.h"

    44

    45

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

    46

    47

#define EC_CAT_MEM 0x100

    48

    49

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

    50

    51

void ec_fsm_master_start(ec_fsm_t *);

    52

void ec_fsm_master_wait(ec_fsm_t *);

    53

void ec_fsm_master_slave(ec_fsm_t *);

    54

void ec_fsm_master_calc(ec_fsm_t *);

    55

void ec_fsm_master_finished(ec_fsm_t *);

    56

    57

void ec_fsm_slave_start(ec_fsm_t *);

    58

void ec_fsm_slave_read_base(ec_fsm_t *);

    59

void ec_fsm_slave_read_dl(ec_fsm_t *);

    60

void ec_fsm_slave_prepare_sii(ec_fsm_t *);

    61

void ec_fsm_slave_read_sii(ec_fsm_t *);

    62

void ec_fsm_slave_categories(ec_fsm_t *);

    63

void ec_fsm_slave_category_header(ec_fsm_t *);

    64

void ec_fsm_slave_category_data(ec_fsm_t *);

    65

void ec_fsm_slave_finished(ec_fsm_t *);

    66

    67

void ec_fsm_sii_start_reading(ec_fsm_t *);

    68

void ec_fsm_sii_check(ec_fsm_t *);

    69

void ec_fsm_sii_fetch(ec_fsm_t *);

    70

void ec_fsm_sii_finished(ec_fsm_t *);

    71

void ec_fsm_sii_error(ec_fsm_t *);

    72

    73

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

    74

    75

int ec_fsm_init(ec_fsm_t *fsm, ec_master_t *master)

    76

{

    77

    fsm->master = master;

    78

    fsm->master_state = ec_fsm_master_start;

    79

    fsm->master_slaves_responding = 0;

    80

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

    81

    fsm->slave_cat_data = NULL;

    82

    83

    ec_command_init(&fsm->command);

    84

    if (ec_command_prealloc(&fsm->command, EC_MAX_DATA_SIZE)) {

    85

        EC_ERR("FSM failed to allocate FSM command.\n");

    86

        return -1;

    87

    }

    88

    89

    return 0;

    90

}

    91

    92

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

    93

    94

void ec_fsm_clear(ec_fsm_t *fsm)

    95

{

    96

    if (fsm->slave_cat_data) kfree(fsm->slave_cat_data);

    97

    ec_command_clear(&fsm->command);

    98

}

    99

   100

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

   101

   102

void ec_fsm_reset(ec_fsm_t *fsm)

   103

{

   104

    fsm->master_state = ec_fsm_master_start;

   105

    fsm->master_slaves_responding = 0;

   106

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

   107

   108

    if (fsm->slave_cat_data) {

   109

        kfree(fsm->slave_cat_data);

   110

        fsm->slave_cat_data = NULL;

   111

    }

   112

}

   113

   114

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

   115

   116

void ec_fsm_execute(ec_fsm_t *fsm)

   117

{

   118

    fsm->master_state(fsm);

   119

}

   120

   121

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

   122

   123

int ec_fsm_idle(const ec_fsm_t *fsm)

   124

{

   125

    return (fsm->master_state == ec_fsm_master_start ||

   126

            fsm->master_state == ec_fsm_master_wait ||

   127

            fsm->master_state == ec_fsm_master_finished);

   128

}

   129

   130

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

   131

 *  master state machine

   132

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

   133

   134

/**

   135

   State: Start.

   136

   Starts with getting slave count and slave states.

   137

*/

   138

   139

void ec_fsm_master_start(ec_fsm_t *fsm)

   140

{

   141

    ec_command_brd(&fsm->command, 0x0130, 2);

   142

    ec_master_queue_command(fsm->master, &fsm->command);

   143

   144

    fsm->master_state = ec_fsm_master_wait;

   145

}

   146

   147

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

   148

   149

void ec_fsm_master_wait(ec_fsm_t *fsm)

   150

{

   151

    ec_command_t *command = &fsm->command;

   152

    unsigned int first, topology_change, i;

   153

    ec_slave_t *slave;

   154

   155

    if (command->state != EC_CMD_RECEIVED) {

   156

        fsm->master_state = ec_fsm_master_start;

   157

        fsm->master_state(fsm); // execute immediately

   158

        return;

   159

    }

   160

   161

    if (command->working_counter == fsm->master_slaves_responding &&

   162

        command->data[0] == fsm->master_slave_states) {

   163

        fsm->master_state = ec_fsm_master_start;

   164

        fsm->master_state(fsm); // execute immediately

   165

        return;

   166

    }

   167

   168

    topology_change = command->working_counter !=

   169

        fsm->master_slaves_responding;

   170

   171

    fsm->master_slaves_responding = command->working_counter;

   172

    fsm->master_slave_states = command->data[0];

   173

   174

    EC_INFO("FSM: %i slave%s responding (", fsm->master_slaves_responding,

   175

            fsm->master_slaves_responding == 1 ? "" : "s");

   176

   177

    first = 1;

   178

    if (fsm->master_slave_states & EC_SLAVE_STATE_INIT) {

   179

        printk("INIT");

   180

        first = 0;

   181

    }

   182

    if (fsm->master_slave_states & EC_SLAVE_STATE_PREOP) {

   183

        if (!first) printk(", ");

   184

        printk("PREOP");

   185

        first = 0;

   186

    }

   187

    if (fsm->master_slave_states & EC_SLAVE_STATE_SAVEOP) {

   188

        if (!first) printk(", ");

   189

        printk("SAVEOP");

   190

        first = 0;

   191

    }

   192

    if (fsm->master_slave_states & EC_SLAVE_STATE_OP) {

   193

        if (!first) printk(", ");

   194

        printk("OP");

   195

    }

   196

    printk(")\n");

   197

   198

    if (!topology_change || fsm->master->mode == EC_MASTER_MODE_RUNNING) {

   199

        fsm->master_state = ec_fsm_master_start;

   200

        fsm->master_state(fsm); // execute immediately

   201

        return;

   202

    }

   203

   204

    // topology change!

   205

    ec_master_clear_slaves(fsm->master);

   206

   207

    if (!fsm->master_slaves_responding) {

   208

        // no slaves present -> finish state machine.

   209

        fsm->master_state = ec_fsm_master_start;

   210

        fsm->master_state(fsm); // execute immediately

   211

        return;

   212

    }

   213

   214

    // init slaves

   215

    for (i = 0; i < fsm->master_slaves_responding; i++) {

   216

        if (!(slave =

   217

              (ec_slave_t *) kmalloc(sizeof(ec_slave_t), GFP_ATOMIC))) {

   218

            EC_ERR("FSM failed to allocate slave %i!\n", i);

   219

            fsm->master_state = ec_fsm_master_finished;

   220

            return;

   221

        }

   222

   223

        if (ec_slave_init(slave, fsm->master, i, i + 1)) {

   224

            fsm->master_state = ec_fsm_master_finished;

   225

            return;

   226

        }

   227

   228

        if (kobject_add(&slave->kobj)) {

   229

            EC_ERR("FSM failed to add kobject.\n");

   230

            kobject_put(&slave->kobj); // free

   231

            fsm->master_state = ec_fsm_master_finished;

   232

            return;

   233

        }

   234

   235

        list_add_tail(&slave->list, &fsm->master->slaves);

   236

    }

   237

   238

    // begin scanning of slaves

   239

    fsm->slave = list_entry(fsm->master->slaves.next,

   240

                            ec_slave_t, list);

   241

    fsm->slave_state = ec_fsm_slave_start;

   242

   243

    fsm->master_state = ec_fsm_master_slave;

   244

    fsm->master_state(fsm); // execute immediately

   245

}

   246

   247

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

   248

   249

/**

   250

   State: Get Slave.

   251

   Executes the sub-statemachine of a slave.

   252

*/

   253

   254

void ec_fsm_master_slave(ec_fsm_t *fsm)

   255

{

   256

    ec_master_t *master = fsm->master;

   257

   258

    fsm->slave_state(fsm); // execute slave state machine

   259

   260

    if (fsm->slave_state != ec_fsm_slave_finished) return;

   261

   262

    // have all slaves been fetched?

   263

    if (fsm->slave->list.next == &master->slaves) {

   264

        fsm->master_state = ec_fsm_master_calc;

   265

        fsm->master_state(fsm); // execute immediately

   266

        return;

   267

    }

   268

   269

    // process next slave

   270

    fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);

   271

    fsm->slave_state = ec_fsm_slave_start;

   272

    fsm->slave_state(fsm); // execute immediately

   273

}

   274

   275

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

   276

   277

/**

   278

   Free-Run state: Calc.

   279

*/

   280

   281

void ec_fsm_master_calc(ec_fsm_t *fsm)

   282

{

   283

    uint16_t coupler_index, coupler_subindex;

   284

    uint16_t reverse_coupler_index, current_coupler_index;

   285

    ec_slave_t *slave;

   286

    ec_slave_ident_t *ident;

   287

    ec_master_t *master = fsm->master;

   288

   289

    coupler_index = 0;

   290

    reverse_coupler_index = 0xFFFF;

   291

    current_coupler_index = 0x3FFF;

   292

    coupler_subindex = 0;

   293

   294

    // for every slave on the bus

   295

    list_for_each_entry(slave, &master->slaves, list)

   296

    {

   297

        // search for identification in "database"

   298

        ident = slave_idents;

   299

        while (ident->type) {

   300

            if (unlikely(ident->vendor_id == slave->sii_vendor_id

   301

                         && ident->product_code == slave->sii_product_code)) {

   302

                slave->type = ident->type;

   303

                break;

   304

            }

   305

            ident++;

   306

        }

   307

   308

        if (!slave->type) {

   309

            EC_WARN("FSM: Unknown slave device (vendor 0x%08X, code 0x%08X) at"

   310

                    " position %i.\n", slave->sii_vendor_id,

   311

                    slave->sii_product_code, slave->ring_position);

   312

        }

   313

        else if (slave->type->special == EC_TYPE_BUS_COUPLER) {

   314

            if (slave->sii_alias)

   315

                current_coupler_index = reverse_coupler_index--;

   316

            else

   317

                current_coupler_index = coupler_index++;

   318

            coupler_subindex = 0;

   319

        }

   320

   321

        slave->coupler_index = current_coupler_index;

   322

        slave->coupler_subindex = coupler_subindex;

   323

        coupler_subindex++;

   324

    }

   325

   326

    fsm->master_state = ec_fsm_master_start;

   327

    fsm->master_state(fsm); // execute immediately

   328

}

   329

   330

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

   331

   332

/**

   333

   Free-Run state: Finished.

   334

   End state of the state machine. Does nothing.

   335

*/

   336

   337

void ec_fsm_master_finished(ec_fsm_t *fsm)

   338

{

   339

}

   340

   341

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

   342

 *  slave state machine

   343

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

   344

   345

/**

   346

   Slave state: Start.

   347

   First state of the slave state machine. Writes the station address to the

   348

   slave, according to its ring position.

   349

*/

   350

   351

void ec_fsm_slave_start(ec_fsm_t *fsm)

   352

{

   353

    ec_command_t *command = &fsm->command;

   354

   355

    // write station address

   356

    ec_command_apwr(command, fsm->slave->ring_position, 0x0010, 2);

   357

    EC_WRITE_U16(command->data, fsm->slave->station_address);

   358

    ec_master_queue_command(fsm->master, command);

   359

    fsm->slave_state = ec_fsm_slave_read_base;

   360

}

   361

   362

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

   363

   364

/**

   365

   Slave state: Read base.

   366

*/

   367

   368

void ec_fsm_slave_read_base(ec_fsm_t *fsm)

   369

{

   370

    ec_command_t *command = &fsm->command;

   371

   372

    if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {

   373

        EC_ERR("FSM failed to write station address of slave %i.\n",

   374

               fsm->slave->ring_position);

   375

        fsm->slave_state = ec_fsm_slave_finished;

   376

        return;

   377

    }

   378

   379

    // read base data

   380

    ec_command_nprd(command, fsm->slave->station_address, 0x0000, 6);

   381

    ec_master_queue_command(fsm->master, command);

   382

    fsm->slave_state = ec_fsm_slave_read_dl;

   383

}

   384

   385

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

   386

   387

/**

   388

   Slave state: Read DL.

   389

*/

   390

   391

void ec_fsm_slave_read_dl(ec_fsm_t *fsm)

   392

{

   393

    ec_command_t *command = &fsm->command;

   394

    ec_slave_t *slave = fsm->slave;

   395

   396

    if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {

   397

        EC_ERR("FSM failed to read base data of slave %i.\n",

   398

               slave->ring_position);

   399

        fsm->slave_state = ec_fsm_slave_finished;

   400

        return;

   401

    }

   402

   403

    slave->base_type       = EC_READ_U8 (command->data);

   404

    slave->base_revision   = EC_READ_U8 (command->data + 1);

   405

    slave->base_build      = EC_READ_U16(command->data + 2);

   406

    slave->base_fmmu_count = EC_READ_U8 (command->data + 4);

   407

    slave->base_sync_count = EC_READ_U8 (command->data + 5);

   408

   409

    if (slave->base_fmmu_count > EC_MAX_FMMUS)

   410

        slave->base_fmmu_count = EC_MAX_FMMUS;

   411

   412

    // read data link status

   413

    ec_command_nprd(command, slave->station_address, 0x0110, 2);

   414

    ec_master_queue_command(slave->master, command);

   415

    fsm->slave_state = ec_fsm_slave_prepare_sii;

   416

}

   417

   418

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

   419

   420

/**

   421

   Slave state: Prepare SII.

   422

*/

   423

   424

void ec_fsm_slave_prepare_sii(ec_fsm_t *fsm)

   425

{

   426

    ec_command_t *command = &fsm->command;

   427

    ec_slave_t *slave = fsm->slave;

   428

    uint16_t dl_status;

   429

    unsigned int i;

   430

   431

    if (command->state != EC_CMD_RECEIVED || command->working_counter != 1) {

   432

        EC_ERR("FSM failed to read DL status of slave %i.\n",

   433

               slave->ring_position);

   434

        fsm->slave_state = ec_fsm_slave_finished;

   435

        return;

   436

    }

   437

   438

    dl_status = EC_READ_U16(command->data);

   439

   440

    for (i = 0; i < 4; i++) {

   441

        slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0;

   442

        slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;

   443

        slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;

   444

    }

   445

   446

    fsm->sii_offset = 0x0004;

   447

    fsm->sii_state = ec_fsm_sii_start_reading;

   448

    fsm->slave_sii_num = 0;

   449

    fsm->slave_state = ec_fsm_slave_read_sii;

   450

    fsm->slave_state(fsm); // execute state immediately

   451

}

   452

   453

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

   454

   455

/**

   456

   Slave state: Read SII.

   457

*/

   458

   459

void ec_fsm_slave_read_sii(ec_fsm_t *fsm)

   460

{

   461

    ec_slave_t *slave = fsm->slave;

   462

   463

    // execute SII state machine

   464

    fsm->sii_state(fsm);

   465

   466

    if (fsm->sii_state == ec_fsm_sii_error) {

   467

        fsm->slave_state = ec_fsm_slave_finished;

   468

        EC_ERR("FSM failed to read SII data at 0x%04X on slave %i.\n",

   469

               fsm->sii_offset, slave->ring_position);

   470

        return;

   471

    }

   472

   473

    if (fsm->sii_state != ec_fsm_sii_finished) return;

   474

   475

    switch (fsm->slave_sii_num) {

   476

        case 0:

   477

            slave->sii_alias = fsm->sii_result & 0xFFFF;

   478

            fsm->sii_offset = 0x0008;

   479

            break;

   480

        case 1:

   481

            slave->sii_vendor_id = fsm->sii_result;

   482

            fsm->sii_offset = 0x000A;

   483

            break;

   484

        case 2:

   485

            slave->sii_product_code = fsm->sii_result;

   486

            fsm->sii_offset = 0x000C;

   487

            break;

   488

        case 3:

   489

            slave->sii_revision_number = fsm->sii_result;

   490

            fsm->sii_offset = 0x000E;

   491

            break;

   492

        case 4:

   493

            slave->sii_serial_number = fsm->sii_result;

   494

            fsm->sii_offset = 0x0018;

   495

            break;

   496

        case 5:

   497

            slave->sii_rx_mailbox_offset = fsm->sii_result & 0xFFFF;

   498

            slave->sii_rx_mailbox_size = fsm->sii_result >> 16;

   499

            fsm->sii_offset = 0x001A;

   500

            break;

   501

        case 6:

   502

            slave->sii_tx_mailbox_offset = fsm->sii_result & 0xFFFF;

   503

            slave->sii_tx_mailbox_size = fsm->sii_result >> 16;

   504

            fsm->sii_offset = 0x001C;

   505

            break;

   506

        case 7:

   507

            slave->sii_mailbox_protocols = fsm->sii_result & 0xFFFF;

   508

            fsm->slave_state = ec_fsm_slave_categories;

   509

            fsm->slave_state(fsm); // execute state immediately

   510

            return;

   511

    }

   512

   513

    fsm->slave_sii_num++;

   514

    fsm->sii_state = ec_fsm_sii_start_reading;

   515

    fsm->slave_state(fsm); // execute state immediately

   516

}

   517

   518

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

   519

   520

/**

   521

   Slave state: Categories.

   522

*/

   523

   524

void ec_fsm_slave_categories(ec_fsm_t *fsm)

   525

{

   526

    fsm->slave_cat_offset = 0x0040;

   527

   528

    if (fsm->slave_cat_data) {

   529