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; version 2 of the License.

    12

 *

    13

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

    14

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

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    16

 *  GNU General Public License for more details.

    17

 *

    18

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

    19

 *  along 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

/**

    25

   \file

    26

   EtherCAT finite state machines.

    27

*/

    28

    29

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

    30

    31

#include "globals.h"

    32

#include "fsm.h"

    33

#include "master.h"

    34

    35

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

    36

    37

#define EC_CAT_MEM 0x100

    38

    39

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

    40

    41

void ec_fsm_master_start(ec_fsm_t *);

    42

void ec_fsm_master_wait(ec_fsm_t *);

    43

void ec_fsm_master_slave(ec_fsm_t *);

    44

void ec_fsm_master_calc(ec_fsm_t *);

    45

void ec_fsm_master_finished(ec_fsm_t *);

    46

    47

void ec_fsm_slave_start(ec_fsm_t *);

    48

void ec_fsm_slave_read_base(ec_fsm_t *);

    49

void ec_fsm_slave_read_dl(ec_fsm_t *);

    50

void ec_fsm_slave_prepare_sii(ec_fsm_t *);

    51

void ec_fsm_slave_read_sii(ec_fsm_t *);

    52

void ec_fsm_slave_categories(ec_fsm_t *);

    53

void ec_fsm_slave_category_header(ec_fsm_t *);

    54

void ec_fsm_slave_category_data(ec_fsm_t *);

    55

void ec_fsm_slave_finished(ec_fsm_t *);

    56

    57

void ec_fsm_sii_start_reading(ec_fsm_t *);

    58

void ec_fsm_sii_check(ec_fsm_t *);

    59

void ec_fsm_sii_fetch(ec_fsm_t *);

    60

void ec_fsm_sii_finished(ec_fsm_t *);

    61

void ec_fsm_sii_error(ec_fsm_t *);

    62

    63

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

    64

    65

int ec_fsm_init(ec_fsm_t *fsm, ec_master_t *master)

    66

{

    67

    fsm->master = master;

    68

    fsm->master_state = ec_fsm_master_start;

    69

    fsm->master_slaves_responding = 0;

    70

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

    71

    fsm->slave_cat_data = NULL;

    72

    73

    ec_command_init(&fsm->command);

    74

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

    75

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

    76

        return -1;

    77

    }

    78

    79

    return 0;

    80

}

    81

    82

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

    83

    84

void ec_fsm_clear(ec_fsm_t *fsm)

    85

{

    86

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

    87

    ec_command_clear(&fsm->command);

    88

}

    89

    90

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

    91

    92

void ec_fsm_reset(ec_fsm_t *fsm)

    93

{

    94

    fsm->master_state = ec_fsm_master_start;

    95

    fsm->master_slaves_responding = 0;

    96

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

    97

    98

    if (fsm->slave_cat_data) {

    99

        kfree(fsm->slave_cat_data);

   100

        fsm->slave_cat_data = NULL;

   101

    }

   102

}

   103

   104

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

   105

   106

void ec_fsm_execute(ec_fsm_t *fsm)

   107

{

   108

    fsm->master_state(fsm);

   109

}

   110

   111

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

   112

   113

int ec_fsm_idle(const ec_fsm_t *fsm)

   114

{

   115

    return (fsm->master_state == ec_fsm_master_start ||

   116

            fsm->master_state == ec_fsm_master_wait ||

   117

            fsm->master_state == ec_fsm_master_finished);

   118

}

   119

   120

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

   121

 *  master state machine

   122

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

   123

   124

/**

   125

   State: Start.

   126

   Starts with getting slave count and slave states.

   127

*/

   128

   129

void ec_fsm_master_start(ec_fsm_t *fsm)

   130

{

   131

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

   132

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

   133

   134

    fsm->master_state = ec_fsm_master_wait;

   135

}

   136

   137

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

   138

   139

void ec_fsm_master_wait(ec_fsm_t *fsm)

   140

{

   141

    ec_command_t *command = &fsm->command;

   142

    unsigned int first, topology_change, i;

   143

    ec_slave_t *slave;

   144

   145

    if (command->state != EC_CMD_RECEIVED) {

   146

        fsm->master_state = ec_fsm_master_start;

   147

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

   148

        return;

   149

    }

   150

   151

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

   152

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

   153

        fsm->master_state = ec_fsm_master_start;

   154

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

   155

        return;

   156

    }

   157

   158

    topology_change = command->working_counter !=

   159

        fsm->master_slaves_responding;

   160

   161

    fsm->master_slaves_responding = command->working_counter;

   162

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

   163

   164

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

   165

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

   166

   167

    first = 1;

   168

    if (fsm->master_slave_states & EC_SLAVE_STATE_INIT) {

   169

        printk("INIT");

   170

        first = 0;

   171

    }

   172

    if (fsm->master_slave_states & EC_SLAVE_STATE_PREOP) {

   173

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

   174

        printk("PREOP");

   175

        first = 0;

   176

    }

   177

    if (fsm->master_slave_states & EC_SLAVE_STATE_SAVEOP) {

   178

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

   179

        printk("SAVEOP");

   180

        first = 0;

   181

    }

   182

    if (fsm->master_slave_states & EC_SLAVE_STATE_OP) {

   183

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

   184

        printk("OP");

   185

    }

   186

    printk(")\n");

   187

   188

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

   189

        fsm->master_state = ec_fsm_master_start;

   190

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

   191

        return;

   192

    }

   193

   194

    // topology change!

   195

    ec_master_clear_slaves(fsm->master);

   196

   197

    if (!fsm->master_slaves_responding) {

   198

        // no slaves present -> finish state machine.

   199

        fsm->master_state = ec_fsm_master_start;

   200

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

   201

        return;

   202

    }

   203

   204

    // init slaves

   205

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

   206

        if (!(slave =

   207

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

   208

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

   209

            fsm->master_state = ec_fsm_master_finished;

   210

            return;

   211

        }

   212

   213

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

   214

            fsm->master_state = ec_fsm_master_finished;

   215

            return;

   216

        }

   217

   218

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

   219

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

   220

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

   221

            fsm->master_state = ec_fsm_master_finished;

   222

            return;

   223

        }

   224

   225

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

   226

    }

   227

   228

    // begin scanning of slaves

   229

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

   230

                            ec_slave_t, list);

   231

    fsm->slave_state = ec_fsm_slave_start;

   232

   233

    fsm->master_state = ec_fsm_master_slave;

   234

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

   235

}

   236

   237

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

   238

   239

/**

   240

   State: Get Slave.

   241

   Executes the sub-statemachine of a slave.

   242

*/

   243

   244

void ec_fsm_master_slave(ec_fsm_t *fsm)

   245

{

   246

    ec_master_t *master = fsm->master;

   247

   248

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

   249

   250

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

   251

   252

    // have all slaves been fetched?

   253

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

   254

        fsm->master_state = ec_fsm_master_calc;

   255

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

   256

        return;

   257

    }

   258

   259

    // process next slave

   260

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

   261

    fsm->slave_state = ec_fsm_slave_start;

   262

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

   263

}

   264

   265

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

   266

   267

/**

   268

   Free-Run state: Calc.

   269

*/

   270

   271

void ec_fsm_master_calc(ec_fsm_t *fsm)

   272

{

   273

    uint16_t coupler_index, coupler_subindex;

   274

    uint16_t reverse_coupler_index, current_coupler_index;

   275

    ec_slave_t *slave;

   276

    ec_slave_ident_t *ident;

   277

    ec_master_t *master = fsm->master;

   278

   279

    coupler_index = 0;

   280

    reverse_coupler_index = 0xFFFF;

   281

    current_coupler_index = 0x3FFF;

   282

    coupler_subindex = 0;

   283

   284

    // for every slave on the bus

   285

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

   286

    {

   287

        // search for identification in "database"

   288

        ident = slave_idents;

   289

        while (ident->type) {

   290

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

   291

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

   292

                slave->type = ident->type;

   293

                break;

   294

            }

   295

            ident++;

   296

        }

   297

   298

        if (!slave->type) {

   299

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

   300

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

   301

                    slave->sii_product_code, slave->ring_position);

   302

        }

   303

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

   304

            if (slave->sii_alias)

   305

                current_coupler_index = reverse_coupler_index--;

   306

            else

   307

                current_coupler_index = coupler_index++;

   308

            coupler_subindex = 0;

   309

        }

   310

   311

        slave->coupler_index = current_coupler_index;

   312

        slave->coupler_subindex = coupler_subindex;

   313

        coupler_subindex++;

   314

    }

   315

   316

    fsm->master_state = ec_fsm_master_start;

   317

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

   318

}

   319

   320

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

   321

   322

/**

   323

   Free-Run state: Finished.

   324

   End state of the state machine. Does nothing.

   325

*/

   326

   327

void ec_fsm_master_finished(ec_fsm_t *fsm)

   328

{

   329

}

   330

   331

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

   332

 *  slave state machine

   333

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

   334

   335

/**

   336

   Slave state: Start.

   337

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

   338

   slave, according to its ring position.

   339

*/

   340

   341

void ec_fsm_slave_start(ec_fsm_t *fsm)

   342

{

   343

    ec_command_t *command = &fsm->command;

   344

   345

    // write station address

   346

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

   347

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

   348

    ec_master_queue_command(fsm->master, command);

   349

    fsm->slave_state = ec_fsm_slave_read_base;

   350

}

   351

   352

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

   353

   354

/**

   355

   Slave state: Read base.

   356

*/

   357

   358

void ec_fsm_slave_read_base(ec_fsm_t *fsm)

   359

{

   360

    ec_command_t *command = &fsm->command;

   361

   362

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

   363

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

   364

               fsm->slave->ring_position);

   365

        fsm->slave_state = ec_fsm_slave_finished;

   366

        return;

   367

    }

   368

   369

    // read base data

   370

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

   371

    ec_master_queue_command(fsm->master, command);

   372

    fsm->slave_state = ec_fsm_slave_read_dl;

   373

}

   374

   375

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

   376

   377

/**

   378

   Slave state: Read DL.

   379

*/

   380

   381

void ec_fsm_slave_read_dl(ec_fsm_t *fsm)

   382

{

   383

    ec_command_t *command = &fsm->command;

   384

    ec_slave_t *slave = fsm->slave;

   385

   386

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

   387

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

   388

               slave->ring_position);

   389

        fsm->slave_state = ec_fsm_slave_finished;

   390

        return;

   391

    }

   392

   393

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

   394

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

   395

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

   396

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

   397

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

   398

   399

    if (slave->base_fmmu_count > EC_MAX_FMMUS)

   400

        slave->base_fmmu_count = EC_MAX_FMMUS;

   401

   402

    // read data link status

   403

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

   404

    ec_master_queue_command(slave->master, command);

   405

    fsm->slave_state = ec_fsm_slave_prepare_sii;

   406

}

   407

   408

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

   409

   410

/**

   411

   Slave state: Prepare SII.

   412

*/

   413

   414

void ec_fsm_slave_prepare_sii(ec_fsm_t *fsm)

   415

{

   416

    ec_command_t *command = &fsm->command;

   417

    ec_slave_t *slave = fsm->slave;

   418

    uint16_t dl_status;

   419

    unsigned int i;

   420

   421

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

   422

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

   423

               slave->ring_position);

   424

        fsm->slave_state = ec_fsm_slave_finished;

   425

        return;

   426

    }

   427

   428

    dl_status = EC_READ_U16(command->data);

   429

   430

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

   431

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

   432

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

   433

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

   434

    }

   435

   436

    fsm->sii_offset = 0x0004;

   437

    fsm->sii_state = ec_fsm_sii_start_reading;

   438

    fsm->slave_sii_num = 0;

   439

    fsm->slave_state = ec_fsm_slave_read_sii;

   440

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

   441

}

   442

   443

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

   444

   445

/**

   446

   Slave state: Read SII.

   447

*/

   448

   449

void ec_fsm_slave_read_sii(ec_fsm_t *fsm)

   450

{

   451

    ec_slave_t *slave = fsm->slave;

   452

   453

    // execute SII state machine

   454

    fsm->sii_state(fsm);

   455

   456

    if (fsm->sii_state == ec_fsm_sii_error) {

   457

        fsm->slave_state = ec_fsm_slave_finished;

   458

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

   459

               fsm->sii_offset, slave->ring_position);

   460

        return;

   461

    }

   462

   463

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

   464

   465

    switch (fsm->slave_sii_num) {

   466

        case 0:

   467

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

   468

            fsm->sii_offset = 0x0008;

   469

            break;

   470

        case 1:

   471

            slave->sii_vendor_id = fsm->sii_result;

   472

            fsm->sii_offset = 0x000A;

   473

            break;

   474

        case 2:

   475

            slave->sii_product_code = fsm->sii_result;

   476

            fsm->sii_offset = 0x000C;

   477

            break;

   478

        case 3:

   479

            slave->sii_revision_number = fsm->sii_result;

   480

            fsm->sii_offset = 0x000E;

   481

            break;

   482

        case 4:

   483

            slave->sii_serial_number = fsm->sii_result;

   484

            fsm->sii_offset = 0x0018;

   485

            break;

   486

        case 5:

   487

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

   488

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

   489

            fsm->sii_offset = 0x001A;

   490

            break;

   491

        case 6:

   492

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

   493

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

   494

            fsm->sii_offset = 0x001C;

   495

            break;

   496

        case 7:

   497

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

   498

            fsm->slave_state = ec_fsm_slave_categories;

   499

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

   500

            return;

   501

    }

   502

   503

    fsm->slave_sii_num++;

   504

    fsm->sii_state = ec_fsm_sii_start_reading;

   505

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

   506

}

   507

   508

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

   509

   510

/**

   511

   Slave state: Categories.

   512

*/

   513

   514

void ec_fsm_slave_categories(ec_fsm_t *fsm)

   515

{

   516

    fsm->slave_cat_offset = 0x0040;

   517

   518

    if (fsm->slave_cat_data) {

   519

        EC_INFO("FSM freeing old category data on slave %i...\n",

   520

                fsm->slave->ring_position);

   521

        kfree(fsm->slave_cat_data);

   522

    }

   523

   524

    if (!(fsm->slave_cat_data = (uint8_t *) kmalloc(EC_CAT_MEM, GFP_ATOMIC))) {

   525

        EC_ERR("FSM Failed to allocate category data.\n");

   526

        fsm->slave_state = ec_fsm_slave_finished;

   527

        return;

   528

    }

   529

   530