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

/** \file

    35

 * EtherCAT Pdo configuration state machine.

    36

 */

    37

    38

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

    39

    40

#include "globals.h"

    41

#include "master.h"

    42

#include "mailbox.h"

    43

#include "slave_config.h"

    44

    45

#include "fsm_pdo.h"

    46

    47

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

    48

    49

void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);

    50

void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);

    51

void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);

    52

void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);

    53

    54

void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);

    55

void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);

    56

    57

void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);

    58

void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);

    59

void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);

    60

void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);

    61

void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);

    62

void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);

    63

void ec_fsm_pdo_conf_state_entries(ec_fsm_pdo_t *);

    64

    65

void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);

    66

void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);

    67

void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);

    68

void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);

    69

void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);

    70

void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);

    71

    72

void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);

    73

void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);

    74

    75

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

    76

    77

/** Constructor.

    78

 */

    79

void ec_fsm_pdo_init(

    80

        ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

    81

        ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */

    82

        )

    83

{

    84

    fsm->fsm_coe = fsm_coe;

    85

    ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);

    86

    ec_pdo_list_init(&fsm->pdos);

    87

    ec_sdo_request_init(&fsm->request);

    88

    ec_pdo_init(&fsm->slave_pdo);

    89

}

    90

    91

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

    92

    93

/** Destructor.

    94

 */

    95

void ec_fsm_pdo_clear(

    96

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

    97

        )

    98

{

    99

    ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);

   100

    ec_pdo_list_clear(&fsm->pdos);

   101

    ec_sdo_request_clear(&fsm->request);

   102

    ec_pdo_clear(&fsm->slave_pdo);

   103

}

   104

   105

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

   106

   107

/** Start reading the Pdo configuration.

   108

 */

   109

void ec_fsm_pdo_start_reading(

   110

        ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

   111

        ec_slave_t *slave /**< slave to configure */

   112

        )

   113

{

   114

    fsm->slave = slave;

   115

    fsm->state = ec_fsm_pdo_read_state_start;

   116

}

   117

   118

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

   119

   120

/** Start writing the Pdo configuration.

   121

 */

   122

void ec_fsm_pdo_start_configuration(

   123

        ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

   124

        ec_slave_t *slave /**< slave to configure */

   125

        )

   126

{

   127

    fsm->slave = slave;

   128

    fsm->state = ec_fsm_pdo_conf_state_start;

   129

}

   130

   131

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

   132

   133

/** Get running state.

   134

 *

   135

 * \return false, if state machine has terminated

   136

 */

   137

int ec_fsm_pdo_running(

   138

        const ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   139

        )

   140

{

   141

    return fsm->state != ec_fsm_pdo_state_end

   142

        && fsm->state != ec_fsm_pdo_state_error;

   143

}

   144

   145

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

   146

   147

/** Executes the current state of the state machine.

   148

 *

   149

 * If the state machine's datagram is not sent or received yet, the execution

   150

 * of the state machine is delayed to the next cycle.

   151

 *

   152

 * \return false, if state machine has terminated

   153

 */

   154

int ec_fsm_pdo_exec(

   155

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   156

        )

   157

{

   158

    fsm->state(fsm);

   159

    return ec_fsm_pdo_running(fsm);

   160

}

   161

   162

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

   163

   164

/** Get execution result.

   165

 *

   166

 * \return true, if the state machine terminated gracefully

   167

 */

   168

int ec_fsm_pdo_success(

   169

        const ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   170

        )

   171

{

   172

    return fsm->state == ec_fsm_pdo_state_end;

   173

}

   174

   175

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

   176

 * Reading state funtions.

   177

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

   178

   179

/** Start reading Pdo assignment.

   180

 */

   181

void ec_fsm_pdo_read_state_start(

   182

        ec_fsm_pdo_t *fsm /**< finite state machine */

   183

        )

   184

{

   185

    // read Pdo assignment for first sync manager not reserved for mailbox

   186

    fsm->sync_index = 1; // next is 2

   187

    ec_fsm_pdo_read_action_next_sync(fsm);

   188

}

   189

   190

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

   191

   192

/** Read Pdo assignment of next sync manager.

   193

 */

   194

void ec_fsm_pdo_read_action_next_sync(

   195

        ec_fsm_pdo_t *fsm /**< Finite state machine */

   196

        )

   197

{

   198

    ec_slave_t *slave = fsm->slave;

   199

   200

    fsm->sync_index++;

   201

   202

    for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {

   203

        if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index)))

   204

            continue;

   205

   206

        if (slave->master->debug_level)

   207

            EC_DBG("Reading Pdo assignment of SM%u.\n", fsm->sync_index);

   208

   209

        ec_pdo_list_clear_pdos(&fsm->pdos);

   210

   211

        ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);

   212

        ecrt_sdo_request_read(&fsm->request);

   213

        fsm->state = ec_fsm_pdo_read_state_pdo_count;

   214

        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

   215

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   216

        return;

   217

    }

   218

   219

    if (slave->master->debug_level)

   220

        EC_DBG("Reading of Pdo configuration finished.\n");

   221

   222

    fsm->state = ec_fsm_pdo_state_end;

   223

}

   224

   225

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

   226

   227

/** Count assigned Pdos.

   228

 */

   229

void ec_fsm_pdo_read_state_pdo_count(

   230

        ec_fsm_pdo_t *fsm /**< finite state machine */

   231

        )

   232

{

   233

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   234

   235

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   236

        EC_ERR("Failed to read number of assigned Pdos for SM%u.\n",

   237

                fsm->sync_index);

   238

        fsm->state = ec_fsm_pdo_state_error;

   239

        return;

   240

    }

   241

   242

    if (fsm->request.data_size != sizeof(uint8_t)) {

   243

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   244

                "from slave %u.\n", fsm->request.data_size,

   245

                fsm->request.index, fsm->request.subindex,

   246

                fsm->slave->ring_position);

   247

        fsm->state = ec_fsm_pdo_state_error;

   248

        return;

   249

    }

   250

    fsm->pdo_count = EC_READ_U8(fsm->request.data);

   251

   252

    if (fsm->slave->master->debug_level)

   253

        EC_DBG("%u Pdos assigned.\n", fsm->pdo_count);

   254

   255

    // read first Pdo

   256

    fsm->pdo_pos = 1;

   257

    ec_fsm_pdo_read_action_next_pdo(fsm);

   258

}

   259

   260

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

   261

   262

/** Read next Pdo.

   263

 */

   264

void ec_fsm_pdo_read_action_next_pdo(

   265

        ec_fsm_pdo_t *fsm /**< finite state machine */

   266

        )

   267

{

   268

    if (fsm->pdo_pos <= fsm->pdo_count) {

   269

        ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index,

   270

                fsm->pdo_pos);

   271

        ecrt_sdo_request_read(&fsm->request);

   272

        fsm->state = ec_fsm_pdo_read_state_pdo;

   273

        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

   274

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   275

        return;

   276

    }

   277

   278

    // finished reading Pdo configuration

   279

   280

    if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {

   281

        fsm->state = ec_fsm_pdo_state_error;

   282

        return;

   283

    }

   284

   285

    fsm->sync->assign_source = EC_ASSIGN_COE;

   286

    ec_pdo_list_clear_pdos(&fsm->pdos);

   287

   288

    // next sync manager

   289

    ec_fsm_pdo_read_action_next_sync(fsm);

   290

}

   291

   292

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

   293

   294

/** Fetch Pdo information.

   295

 */

   296

void ec_fsm_pdo_read_state_pdo(

   297

        ec_fsm_pdo_t *fsm /**< finite state machine */

   298

        )

   299

{

   300

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   301

   302

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   303

        EC_ERR("Failed to read index of assigned Pdo %u from SM%u.\n",

   304

                fsm->pdo_pos, fsm->sync_index);

   305

        fsm->state = ec_fsm_pdo_state_error;

   306

        return;

   307

    }

   308

   309

    if (fsm->request.data_size != sizeof(uint16_t)) {

   310

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   311

                "from slave %u.\n", fsm->request.data_size,

   312

                fsm->request.index, fsm->request.subindex,

   313

                fsm->slave->ring_position);

   314

        fsm->state = ec_fsm_pdo_state_error;

   315

        return;

   316

    }

   317

   318

    if (!(fsm->pdo = (ec_pdo_t *)

   319

                kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   320

        EC_ERR("Failed to allocate Pdo.\n");

   321

        fsm->state = ec_fsm_pdo_state_error;

   322

        return;

   323

    }

   324

   325

    ec_pdo_init(fsm->pdo);

   326

    fsm->pdo->index = EC_READ_U16(fsm->request.data);

   327

    fsm->pdo->sync_index = fsm->sync_index;

   328

   329

    if (fsm->slave->master->debug_level)

   330

        EC_DBG("Pdo 0x%04X.\n", fsm->pdo->index);

   331

   332

    list_add_tail(&fsm->pdo->list, &fsm->pdos.list);

   333

   334

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   335

    ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);

   336

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

   337

}

   338

   339

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

   340

   341

/** Fetch Pdo information.

   342

 */

   343

void ec_fsm_pdo_read_state_pdo_entries(

   344

        ec_fsm_pdo_t *fsm /**< finite state machine */

   345

        )

   346

{

   347

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   348

        return;

   349

   350

    if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {

   351

        EC_ERR("Failed to read mapped Pdo entries for Pdo 0x%04X.\n",

   352

                fsm->pdo->index);

   353

        fsm->state = ec_fsm_pdo_state_error;

   354

        return;

   355

    }

   356

   357

    // next Pdo

   358

    fsm->pdo_pos++;

   359

    ec_fsm_pdo_read_action_next_pdo(fsm);

   360

}

   361

   362

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

   363

 * Writing state functions.

   364

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

   365

   366

/** Start Pdo configuration.

   367

 */

   368

void ec_fsm_pdo_conf_state_start(

   369

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   370

        )

   371

{

   372

    if (!fsm->slave->config) {

   373

        fsm->state = ec_fsm_pdo_state_end;

   374

        return;

   375

    }

   376

   377

    fsm->sync_index = 0xff; // next is zero

   378

    ec_fsm_pdo_conf_action_next_sync(fsm);

   379

}

   380

   381

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

   382

   383

/** Assign next Pdo.

   384

 */

   385

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   386

        const ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

   387

        const struct list_head *list /**< current Pdo list item */

   388

        )

   389

{

   390

    list = list->next; 

   391

    if (list == &fsm->pdos.list)

   392

        return NULL; // no next Pdo

   393

    return list_entry(list, ec_pdo_t, list);

   394

}

   395

   396

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

   397

   398

/** Get the next sync manager for a pdo configuration.

   399

 */

   400

void ec_fsm_pdo_conf_action_next_sync(

   401

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   402

        )

   403

{

   404

    fsm->sync_index++;

   405

   406

    for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {

   407

        if (ec_pdo_list_copy(&fsm->pdos,

   408

                    &fsm->slave->config->sync_configs[fsm->sync_index].pdos)) {

   409

            fsm->state = ec_fsm_pdo_state_error;

   410

            return;

   411

        }

   412

   413

        if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {

   414

            if (!list_empty(&fsm->pdos.list))

   415

                EC_WARN("Pdos configured for SM%u, but slave %u does not "

   416

                        "provide the sync manager information!\n",

   417

                        fsm->sync_index, fsm->slave->ring_position);

   418

            continue;

   419

        }

   420

   421

        // get first configured Pdo

   422

        if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {

   423

            // no pdos configured

   424

            ec_fsm_pdo_conf_action_check_assignment(fsm);

   425

            return;

   426

        }

   427

   428

        ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   429

        return;

   430

    }

   431

   432

    fsm->state = ec_fsm_pdo_state_end;

   433

}

   434

   435

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

   436

   437

/**

   438

 */

   439

void ec_fsm_pdo_conf_action_pdo_mapping(

   440

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   441

        )

   442

{

   443

    const ec_pdo_t *assigned_pdo;

   444

   445

    fsm->slave_pdo.index = fsm->pdo->index;

   446

   447

    if ((assigned_pdo = ec_slave_find_pdo(fsm->slave, fsm->pdo->index))) {

   448

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   449

    } else { // configured Pdo is not assigned and thus unknown

   450

        ec_pdo_clear_entries(&fsm->slave_pdo);

   451

    }

   452

   453

    if (list_empty(&fsm->slave_pdo.entries)) {

   454

   455

        if (fsm->slave->master->debug_level)

   456

            EC_DBG("Reading mapping of Pdo 0x%04X.\n",

   457

                    fsm->pdo->index);

   458

   459

        // pdo mapping is unknown; start loading it

   460

        ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,

   461

                &fsm->slave_pdo);

   462

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   463

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

   464

        return;

   465

    }

   466

   467

    // pdo mapping is known, check if it most be re-configured

   468

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   469

}

   470

   471

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

   472

   473

/**

   474

 */

   475

void ec_fsm_pdo_conf_state_read_mapping(

   476

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   477

        )

   478

{

   479

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   480

        return;

   481

   482

    if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))

   483

        EC_WARN("Failed to read mapped Pdo entries for Pdo 0x%04X.\n",

   484

                fsm->pdo->index);

   485

   486

    // check if the mapping must be re-configured

   487

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   488

}

   489

   490

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

   491

   492

/**

   493

 */

   494

void ec_fsm_pdo_conf_action_check_mapping(

   495

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   496

        )

   497

{

   498

    if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {

   499

        if (fsm->slave->master->debug_level)

   500

            EC_DBG("Mapping of Pdo 0x%04X is already configured correctly.\n",

   501

                    fsm->pdo->index);

   502

        ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);

   503

        return;

   504

    }

   505

   506

    if (fsm->slave->master->debug_level) {

   507

        // TODO display diff

   508

        EC_DBG("Changing mapping of Pdo 0x%04X.\n", fsm->pdo->index);

   509

    }

   510

   511

    ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,

   512

            fsm->pdo);

   513

    fsm->state = ec_fsm_pdo_conf_state_mapping;

   514

    fsm->state(fsm); // execure immediately

   515

}

   516

   517

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

   518

   519

/**

   520

 */

   521

void ec_fsm_pdo_conf_state_mapping(

   522

        ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   523

        )

   524

{

   525

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   526

        return;

   527

   528

    if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))

   529

        EC_WARN("Failed to configure mapping of Pdo 0x%04X.\n",