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

    64

void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);

    65

void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);

    66

void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);

    67

void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);

    68

void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);

    69

void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);

    70

    71

void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);

    72

void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);

    73

    74

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

    75

    76

/** Constructor.

    77

 */

    78

void ec_fsm_pdo_init(

    79

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

    80

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

    81

        )

    82

{

    83

    fsm->fsm_coe = fsm_coe;

    84

    ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);

    85

    ec_pdo_list_init(&fsm->pdos);

    86

    ec_sdo_request_init(&fsm->request);

    87

    ec_pdo_init(&fsm->slave_pdo);

    88

}

    89

    90

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

    91

    92

/** Destructor.

    93

 */

    94

void ec_fsm_pdo_clear(

    95

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

    96

        )

    97

{

    98

    ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);

    99

    ec_pdo_list_clear(&fsm->pdos);

   100

    ec_sdo_request_clear(&fsm->request);

   101

    ec_pdo_clear(&fsm->slave_pdo);

   102

}

   103

   104

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

   105

   106

/** Start reading the Pdo configuration.

   107

 */

   108

void ec_fsm_pdo_start_reading(

   109

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

   110

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

   111

        )

   112

{

   113

    fsm->slave = slave;

   114

    fsm->state = ec_fsm_pdo_read_state_start;

   115

}

   116

   117

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

   118

   119

/** Start writing the Pdo configuration.

   120

 */

   121

void ec_fsm_pdo_start_configuration(

   122

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

   123

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

   124

        )

   125

{

   126

    fsm->slave = slave;

   127

    fsm->state = ec_fsm_pdo_conf_state_start;

   128

}

   129

   130

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

   131

   132

/** Get running state.

   133

 *

   134

 * \return false, if state machine has terminated

   135

 */

   136

int ec_fsm_pdo_running(

   137

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

   138

        )

   139

{

   140

    return fsm->state != ec_fsm_pdo_state_end

   141

        && fsm->state != ec_fsm_pdo_state_error;

   142

}

   143

   144

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

   145

   146

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

   147

 *

   148

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

   149

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

   150

 *

   151

 * \return false, if state machine has terminated

   152

 */

   153

int ec_fsm_pdo_exec(

   154

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

   155

        )

   156

{

   157

    fsm->state(fsm);

   158

    return ec_fsm_pdo_running(fsm);

   159

}

   160

   161

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

   162

   163

/** Get execution result.

   164

 *

   165

 * \return true, if the state machine terminated gracefully

   166

 */

   167

int ec_fsm_pdo_success(

   168

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

   169

        )

   170

{

   171

    return fsm->state == ec_fsm_pdo_state_end;

   172

}

   173

   174

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

   175

 * Reading state funtions.

   176

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

   177

   178

/** Start reading Pdo assignment.

   179

 */

   180

void ec_fsm_pdo_read_state_start(

   181

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

   182

        )

   183

{

   184

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

   185

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

   186

    ec_fsm_pdo_read_action_next_sync(fsm);

   187

}

   188

   189

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

   190

   191

/** Read Pdo assignment of next sync manager.

   192

 */

   193

void ec_fsm_pdo_read_action_next_sync(

   194

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

   195

        )

   196

{

   197

    ec_slave_t *slave = fsm->slave;

   198

   199

    fsm->sync_index++;

   200

   201

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

   202

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

   203

            continue;

   204

   205

        if (slave->master->debug_level)

   206

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

   207

   208

        ec_pdo_list_clear_pdos(&fsm->pdos);

   209

   210

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

   211

        ecrt_sdo_request_read(&fsm->request);

   212

        fsm->state = ec_fsm_pdo_read_state_pdo_count;

   213

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

   214

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

   215

        return;

   216

    }

   217

   218

    if (slave->master->debug_level)

   219

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

   220

   221

    fsm->state = ec_fsm_pdo_state_end;

   222

}

   223

   224

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

   225

   226

/** Count assigned Pdos.

   227

 */

   228

void ec_fsm_pdo_read_state_pdo_count(

   229

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

   230

        )

   231

{

   232

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

   233

   234

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

   235

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

   236

                fsm->sync_index);

   237

        fsm->state = ec_fsm_pdo_state_error;

   238

        return;

   239

    }

   240

   241

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

   242

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

   243

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

   244

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

   245

                fsm->slave->ring_position);

   246

        fsm->state = ec_fsm_pdo_state_error;

   247

        return;

   248

    }

   249

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

   250

   251

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

   252

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

   253

   254

    // read first Pdo

   255

    fsm->pdo_pos = 1;

   256

    ec_fsm_pdo_read_action_next_pdo(fsm);

   257

}

   258

   259

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

   260

   261

/** Read next Pdo.

   262

 */

   263

void ec_fsm_pdo_read_action_next_pdo(

   264

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

   265

        )

   266

{

   267

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

   268

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

   269

                fsm->pdo_pos);

   270

        ecrt_sdo_request_read(&fsm->request);

   271

        fsm->state = ec_fsm_pdo_read_state_pdo;

   272

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

   273

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

   274

        return;

   275

    }

   276

   277

    // finished reading Pdo configuration

   278

   279

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

   280

        fsm->state = ec_fsm_pdo_state_error;

   281

        return;

   282

    }

   283

   284

    ec_pdo_list_clear_pdos(&fsm->pdos);

   285

   286

    // next sync manager

   287

    ec_fsm_pdo_read_action_next_sync(fsm);

   288

}

   289

   290

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

   291

   292

/** Fetch Pdo information.

   293

 */

   294

void ec_fsm_pdo_read_state_pdo(

   295

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

   296

        )

   297

{

   298

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

   299

   300

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

   301

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

   302

                fsm->pdo_pos, fsm->sync_index);

   303

        fsm->state = ec_fsm_pdo_state_error;

   304

        return;

   305

    }

   306

   307

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

   308

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

   309

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

   310

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

   311

                fsm->slave->ring_position);

   312

        fsm->state = ec_fsm_pdo_state_error;

   313

        return;

   314

    }

   315

   316

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

   317

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

   318

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

   319

        fsm->state = ec_fsm_pdo_state_error;

   320

        return;

   321

    }

   322

   323

    ec_pdo_init(fsm->pdo);

   324

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

   325

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

   326

   327

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

   328

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

   329

   330

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

   331

   332

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   333

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

   334

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

   335

}

   336

   337

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

   338

   339

/** Fetch Pdo information.

   340

 */

   341

void ec_fsm_pdo_read_state_pdo_entries(

   342

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

   343

        )

   344

{

   345

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   346

        return;

   347

   348

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

   349

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

   350

                fsm->pdo->index);

   351

        fsm->state = ec_fsm_pdo_state_error;

   352

        return;

   353

    }

   354

   355

    // next Pdo

   356

    fsm->pdo_pos++;

   357

    ec_fsm_pdo_read_action_next_pdo(fsm);

   358

}

   359

   360

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

   361

 * Writing state functions.

   362

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

   363

   364

/** Start Pdo configuration.

   365

 */

   366

void ec_fsm_pdo_conf_state_start(

   367

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

   368

        )

   369

{

   370

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

   371

        fsm->state = ec_fsm_pdo_state_end;

   372

        return;

   373

    }

   374

   375

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

   376

    ec_fsm_pdo_conf_action_next_sync(fsm);

   377

}

   378

   379

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

   380

   381

/** Assign next Pdo.

   382

 */

   383

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   384

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

   385

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

   386

        )

   387

{

   388

    list = list->next; 

   389

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

   390

        return NULL; // no next Pdo

   391

    return list_entry(list, ec_pdo_t, list);

   392

}

   393

   394

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

   395

   396

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

   397

 */

   398

void ec_fsm_pdo_conf_action_next_sync(

   399

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

   400

        )

   401

{

   402

    fsm->sync_index++;

   403

   404

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

   405

        if (ec_pdo_list_copy(&fsm->pdos,

   406

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

   407

            fsm->state = ec_fsm_pdo_state_error;

   408

            return;

   409

        }

   410

   411

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

   412

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

   413

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

   414

                        "provide the sync manager information!\n",

   415

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

   416

            continue;

   417

        }

   418

   419

        // get first configured Pdo

   420

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

   421

            // no pdos configured

   422

            ec_fsm_pdo_conf_action_check_assignment(fsm);

   423

            return;

   424

        }

   425

   426

        ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   427

        return;

   428

    }

   429

   430

    fsm->state = ec_fsm_pdo_state_end;

   431

}

   432

   433

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

   434

   435

/** Check if the mapping has to be read, otherwise start to configure it.

   436

 */

   437

void ec_fsm_pdo_conf_action_pdo_mapping(

   438

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

   439

        )

   440

{

   441

    const ec_pdo_t *assigned_pdo;

   442

   443

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

   444

   445

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

   446

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   447

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

   448

        ec_pdo_clear_entries(&fsm->slave_pdo);

   449

    }

   450

   451

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

   452

   453

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

   454

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

   455

                    fsm->pdo->index);

   456

   457

        // pdo mapping is unknown; start loading it

   458

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

   459

                &fsm->slave_pdo);

   460

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   461

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

   462

        return;

   463

    }

   464

   465

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

   466

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   467

}

   468

   469

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

   470

   471

/** Execute the Pdo entry state machine to read the current Pdo's mapping.

   472

 */

   473

void ec_fsm_pdo_conf_state_read_mapping(

   474

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

   475

        )

   476

{

   477

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   478

        return;

   479

   480

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

   481

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

   482

                fsm->pdo->index);

   483

   484

    // check if the mapping must be re-configured

   485

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   486

}

   487

   488

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

   489

   490

/** Check if the mapping has to be re-configured.

   491

 *

   492

 * \todo Display mapping differences.

   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

/** Let the Pdo entry state machine configure the current Pdo's mapping.

   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))