1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

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

    14

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

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

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

    19

 *  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

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/** \file

    31

 * EtherCAT PDO configuration state machine.

    32

 */

    33

    34

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

    35

    36

#include "globals.h"

    37

#include "master.h"

    38

#include "mailbox.h"

    39

#include "slave_config.h"

    40

    41

#include "fsm_pdo.h"

    42

    43

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

    44

    45

void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);

    46

void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);

    47

void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);

    48

void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);

    49

    50

void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);

    51

void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);

    52

    53

void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);

    54

void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);

    55

void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);

    56

void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);

    57

void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);

    58

void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);

    59

    60

void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);

    61

void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);

    62

void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);

    63

void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);

    64

void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);

    65

void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);

    66

    67

void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);

    68

void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);

    69

    70

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

    71

    72

/** Constructor.

    73

 */

    74

void ec_fsm_pdo_init(

    75

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

    76

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

    77

        )

    78

{

    79

    fsm->fsm_coe = fsm_coe;

    80

    ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);

    81

    ec_pdo_list_init(&fsm->pdos);

    82

    ec_sdo_request_init(&fsm->request);

    83

    ec_pdo_init(&fsm->slave_pdo);

    84

}

    85

    86

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

    87

    88

/** Destructor.

    89

 */

    90

void ec_fsm_pdo_clear(

    91

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

    92

        )

    93

{

    94

    ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);

    95

    ec_pdo_list_clear(&fsm->pdos);

    96

    ec_sdo_request_clear(&fsm->request);

    97

    ec_pdo_clear(&fsm->slave_pdo);

    98

}

    99

   100

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

   101

   102

/** Print the current and desired PDO assignment.

   103

 */

   104

void ec_fsm_pdo_print(

   105

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

   106

        )

   107

{

   108

    printk("Currently assigned PDOs: ");

   109

    ec_pdo_list_print(&fsm->sync->pdos);

   110

    printk(". PDOs to assign: ");

   111

    ec_pdo_list_print(&fsm->pdos);

   112

    printk("\n");

   113

}

   114

   115

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

   116

   117

/** Start reading the PDO configuration.

   118

 */

   119

void ec_fsm_pdo_start_reading(

   120

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

   121

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

   122

        )

   123

{

   124

    fsm->slave = slave;

   125

    fsm->state = ec_fsm_pdo_read_state_start;

   126

}

   127

   128

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

   129

   130

/** Start writing the PDO configuration.

   131

 */

   132

void ec_fsm_pdo_start_configuration(

   133

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

   134

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

   135

        )

   136

{

   137

    fsm->slave = slave;

   138

    fsm->state = ec_fsm_pdo_conf_state_start;

   139

}

   140

   141

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

   142

   143

/** Get running state.

   144

 *

   145

 * \return false, if state machine has terminated

   146

 */

   147

int ec_fsm_pdo_running(

   148

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

   149

        )

   150

{

   151

    return fsm->state != ec_fsm_pdo_state_end

   152

        && fsm->state != ec_fsm_pdo_state_error;

   153

}

   154

   155

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

   156

   157

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

   158

 *

   159

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

   160

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

   161

 *

   162

 * \return false, if state machine has terminated

   163

 */

   164

int ec_fsm_pdo_exec(

   165

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

   166

        )

   167

{

   168

    fsm->state(fsm);

   169

    return ec_fsm_pdo_running(fsm);

   170

}

   171

   172

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

   173

   174

/** Get execution result.

   175

 *

   176

 * \return true, if the state machine terminated gracefully

   177

 */

   178

int ec_fsm_pdo_success(

   179

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

   180

        )

   181

{

   182

    return fsm->state == ec_fsm_pdo_state_end;

   183

}

   184

   185

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

   186

 * Reading state funtions.

   187

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

   188

   189

/** Start reading PDO assignment.

   190

 */

   191

void ec_fsm_pdo_read_state_start(

   192

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

   193

        )

   194

{

   195

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

   196

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

   197

    ec_fsm_pdo_read_action_next_sync(fsm);

   198

}

   199

   200

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

   201

   202

/** Read PDO assignment of next sync manager.

   203

 */

   204

void ec_fsm_pdo_read_action_next_sync(

   205

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

   206

        )

   207

{

   208

    ec_slave_t *slave = fsm->slave;

   209

   210

    fsm->sync_index++;

   211

   212

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

   213

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

   214

            continue;

   215

   216

        if (slave->master->debug_level)

   217

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

   218

   219

        ec_pdo_list_clear_pdos(&fsm->pdos);

   220

   221

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

   222

        ecrt_sdo_request_read(&fsm->request);

   223

        fsm->state = ec_fsm_pdo_read_state_pdo_count;

   224

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

   225

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

   226

        return;

   227

    }

   228

   229

    if (slave->master->debug_level)

   230

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

   231

   232

    ec_pdo_list_clear_pdos(&fsm->pdos);

   233

    fsm->state = ec_fsm_pdo_state_end;

   234

}

   235

   236

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

   237

   238

/** Count assigned PDOs.

   239

 */

   240

void ec_fsm_pdo_read_state_pdo_count(

   241

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

   242

        )

   243

{

   244

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

   245

   246

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

   247

        EC_ERR("Failed to read number of assigned PDOs for SM%u"

   248

                " of slave %u.\n", fsm->sync_index, fsm->slave->ring_position);

   249

        ec_fsm_pdo_read_action_next_sync(fsm);

   250

        return;

   251

    }

   252

   253

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

   254

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

   255

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

   256

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

   257

                fsm->slave->ring_position);

   258

        ec_fsm_pdo_read_action_next_sync(fsm);

   259

        return;

   260

    }

   261

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

   262

   263

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

   264

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

   265

   266

    // read first PDO

   267

    fsm->pdo_pos = 1;

   268

    ec_fsm_pdo_read_action_next_pdo(fsm);

   269

}

   270

   271

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

   272

   273

/** Read next PDO.

   274

 */

   275

void ec_fsm_pdo_read_action_next_pdo(

   276

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

   277

        )

   278

{

   279

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

   280

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

   281

                fsm->pdo_pos);

   282

        ecrt_sdo_request_read(&fsm->request);

   283

        fsm->state = ec_fsm_pdo_read_state_pdo;

   284

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

   285

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

   286

        return;

   287

    }

   288

   289

    // finished reading PDO configuration

   290

   291

    ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos);

   292

    ec_pdo_list_clear_pdos(&fsm->pdos);

   293

   294

    // next sync manager

   295

    ec_fsm_pdo_read_action_next_sync(fsm);

   296

}

   297

   298

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

   299

   300

/** Fetch PDO information.

   301

 */

   302

void ec_fsm_pdo_read_state_pdo(

   303

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

   304

        )

   305

{

   306

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

   307

   308

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

   309

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

   310

                fsm->pdo_pos, fsm->sync_index);

   311

        ec_fsm_pdo_read_action_next_sync(fsm);

   312

        return;

   313

    }

   314

   315

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

   316

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

   317

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

   318

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

   319

                fsm->slave->ring_position);

   320

        ec_fsm_pdo_read_action_next_sync(fsm);

   321

        return;

   322

    }

   323

   324

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

   325

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

   326

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

   327

        ec_fsm_pdo_read_action_next_sync(fsm);

   328

        return;

   329

    }

   330

   331

    ec_pdo_init(fsm->pdo);

   332

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

   333

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

   334

   335

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

   336

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

   337

   338

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

   339

   340

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   341

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

   342

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

   343

}

   344

   345

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

   346

   347

/** Fetch PDO information.

   348

 */

   349

void ec_fsm_pdo_read_state_pdo_entries(

   350

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

   351

        )

   352

{

   353

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   354

        return;

   355

   356

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

   357

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

   358

                fsm->pdo->index);

   359

        ec_fsm_pdo_read_action_next_sync(fsm);

   360

        return;

   361

    }

   362

   363

    // next PDO

   364

    fsm->pdo_pos++;

   365

    ec_fsm_pdo_read_action_next_pdo(fsm);

   366

}

   367

   368

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

   369

 * Writing state functions.

   370

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

   371

   372

/** Start PDO configuration.

   373

 */

   374

void ec_fsm_pdo_conf_state_start(

   375

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

   376

        )

   377

{

   378

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

   379

        fsm->state = ec_fsm_pdo_state_end;

   380

        return;

   381

    }

   382

   383

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

   384

    ec_fsm_pdo_conf_action_next_sync(fsm);

   385

}

   386

   387

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

   388

   389

/** Assign next PDO.

   390

 */

   391

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   392

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

   393

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

   394

        )

   395

{

   396

    list = list->next; 

   397

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

   398

        return NULL; // no next PDO

   399

    return list_entry(list, ec_pdo_t, list);

   400

}

   401

   402

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

   403

   404

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

   405

 */

   406

void ec_fsm_pdo_conf_action_next_sync(

   407

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

   408

        )

   409

{

   410

    fsm->sync_index++;

   411

   412

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

   413

        if (!fsm->slave->config) { // slave configuration removed in the meantime

   414

            fsm->state = ec_fsm_pdo_state_error;

   415

            return;

   416

        }

   417

   418

        if (ec_pdo_list_copy(&fsm->pdos,

   419

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

   420

            fsm->state = ec_fsm_pdo_state_error;

   421

            return;

   422

        }

   423

   424

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

   425

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

   426

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

   427

                        "provide the sync manager information!\n",

   428

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

   429

            continue;

   430

        }

   431

   432

        // get first configured PDO

   433

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

   434

            // no pdos configured

   435

            ec_fsm_pdo_conf_action_check_assignment(fsm);

   436

            return;

   437

        }

   438

   439

        ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   440

        return;

   441

    }

   442

   443

    fsm->state = ec_fsm_pdo_state_end;

   444

}

   445

   446

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

   447

   448

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

   449

 */

   450

void ec_fsm_pdo_conf_action_pdo_mapping(

   451

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

   452

        )

   453

{

   454

    const ec_pdo_t *assigned_pdo;

   455

   456

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

   457

   458

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

   459

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   460

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

   461

        ec_pdo_clear_entries(&fsm->slave_pdo);

   462

    }

   463

   464

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

   465

   466

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

   467

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

   468

                    fsm->pdo->index);

   469

   470

        // pdo mapping is unknown; start loading it

   471

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

   472

                &fsm->slave_pdo);

   473

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   474

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

   475

        return;

   476

    }

   477

   478

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

   479

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   480

}

   481

   482

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

   483

   484

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

   485

 */

   486

void ec_fsm_pdo_conf_state_read_mapping(

   487

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

   488

        )

   489

{

   490

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   491

        return;

   492

   493

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

   494

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

   495

                fsm->pdo->index);

   496

   497

    // check if the mapping must be re-configured

   498

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   499

}

   500

   501

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

   502

   503

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

   504

 *

   505

 * \todo Display mapping differences.

   506

 */

   507

void ec_fsm_pdo_conf_action_check_mapping(

   508

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

   509

        )

   510

{

   511

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

   512

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

   513

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

   514

                    fsm->pdo->index);

   515

        ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);