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

/** Start reading the PDO configuration.

   103

 */

   104

void ec_fsm_pdo_start_reading(

   105

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

   106

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

   107

        )

   108

{

   109

    fsm->slave = slave;

   110

    fsm->state = ec_fsm_pdo_read_state_start;

   111

}

   112

   113

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

   114

   115

/** Start writing the PDO configuration.

   116

 */

   117

void ec_fsm_pdo_start_configuration(

   118

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

   119

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

   120

        )

   121

{

   122

    fsm->slave = slave;

   123

    fsm->state = ec_fsm_pdo_conf_state_start;

   124

}

   125

   126

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

   127

   128

/** Get running state.

   129

 *

   130

 * \return false, if state machine has terminated

   131

 */

   132

int ec_fsm_pdo_running(

   133

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

   134

        )

   135

{

   136

    return fsm->state != ec_fsm_pdo_state_end

   137

        && fsm->state != ec_fsm_pdo_state_error;

   138

}

   139

   140

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

   141

   142

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

   143

 *

   144

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

   145

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

   146

 *

   147

 * \return false, if state machine has terminated

   148

 */

   149

int ec_fsm_pdo_exec(

   150

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

   151

        )

   152

{

   153

    fsm->state(fsm);

   154

    return ec_fsm_pdo_running(fsm);

   155

}

   156

   157

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

   158

   159

/** Get execution result.

   160

 *

   161

 * \return true, if the state machine terminated gracefully

   162

 */

   163

int ec_fsm_pdo_success(

   164

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

   165

        )

   166

{

   167

    return fsm->state == ec_fsm_pdo_state_end;

   168

}

   169

   170

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

   171

 * Reading state funtions.

   172

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

   173

   174

/** Start reading PDO assignment.

   175

 */

   176

void ec_fsm_pdo_read_state_start(

   177

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

   178

        )

   179

{

   180

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

   181

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

   182

    ec_fsm_pdo_read_action_next_sync(fsm);

   183

}

   184

   185

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

   186

   187

/** Read PDO assignment of next sync manager.

   188

 */

   189

void ec_fsm_pdo_read_action_next_sync(

   190

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

   191

        )

   192

{

   193

    ec_slave_t *slave = fsm->slave;

   194

   195

    fsm->sync_index++;

   196

   197

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

   198

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

   199

            continue;

   200

   201

        if (slave->master->debug_level)

   202

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

   203

   204

        ec_pdo_list_clear_pdos(&fsm->pdos);

   205

   206

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

   207

        ecrt_sdo_request_read(&fsm->request);

   208

        fsm->state = ec_fsm_pdo_read_state_pdo_count;

   209

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

   210

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

   211

        return;

   212

    }

   213

   214

    if (slave->master->debug_level)

   215

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

   216

   217

    ec_pdo_list_clear_pdos(&fsm->pdos);

   218

    fsm->state = ec_fsm_pdo_state_end;

   219

}

   220

   221

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

   222

   223

/** Count assigned PDOs.

   224

 */

   225

void ec_fsm_pdo_read_state_pdo_count(

   226

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

   227

        )

   228

{

   229

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

   230

   231

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

   232

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

   233

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

   234

        ec_fsm_pdo_read_action_next_sync(fsm);

   235

        return;

   236

    }

   237

   238

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

   239

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

   240

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

   241

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

   242

                fsm->slave->ring_position);

   243

        ec_fsm_pdo_read_action_next_sync(fsm);

   244

        return;

   245

    }

   246

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

   247

   248

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

   249

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

   250

   251

    // read first PDO

   252

    fsm->pdo_pos = 1;

   253

    ec_fsm_pdo_read_action_next_pdo(fsm);

   254

}

   255

   256

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

   257

   258

/** Read next PDO.

   259

 */

   260

void ec_fsm_pdo_read_action_next_pdo(

   261

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

   262

        )

   263

{

   264

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

   265

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

   266

                fsm->pdo_pos);

   267

        ecrt_sdo_request_read(&fsm->request);

   268

        fsm->state = ec_fsm_pdo_read_state_pdo;

   269

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

   270

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

   271

        return;

   272

    }

   273

   274

    // finished reading PDO configuration

   275

   276

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

   277

    ec_pdo_list_clear_pdos(&fsm->pdos);

   278

   279

    // next sync manager

   280

    ec_fsm_pdo_read_action_next_sync(fsm);

   281

}

   282

   283

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

   284

   285

/** Fetch PDO information.

   286

 */

   287

void ec_fsm_pdo_read_state_pdo(

   288

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

   289

        )

   290

{

   291

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

   292

   293

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

   294

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

   295

                fsm->pdo_pos, fsm->sync_index);

   296

        ec_fsm_pdo_read_action_next_sync(fsm);

   297

        return;

   298

    }

   299

   300

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

   301

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

   302

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

   303

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

   304

                fsm->slave->ring_position);

   305

        ec_fsm_pdo_read_action_next_sync(fsm);

   306

        return;

   307

    }

   308

   309

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

   310

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

   311

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

   312

        ec_fsm_pdo_read_action_next_sync(fsm);

   313

        return;

   314

    }

   315

   316

    ec_pdo_init(fsm->pdo);

   317

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

   318

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

   319

   320

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

   321

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

   322

   323

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

   324

   325

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   326

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

   327

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

   328

}

   329

   330

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

   331

   332

/** Fetch PDO information.

   333

 */

   334

void ec_fsm_pdo_read_state_pdo_entries(

   335

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

   336

        )

   337

{

   338

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   339

        return;

   340

   341

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

   342

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

   343

                fsm->pdo->index);

   344

        ec_fsm_pdo_read_action_next_sync(fsm);

   345

        return;

   346

    }

   347

   348

    // next PDO

   349

    fsm->pdo_pos++;

   350

    ec_fsm_pdo_read_action_next_pdo(fsm);

   351

}

   352

   353

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

   354

 * Writing state functions.

   355

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

   356

   357

/** Start PDO configuration.

   358

 */

   359

void ec_fsm_pdo_conf_state_start(

   360

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

   361

        )

   362

{

   363

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

   364

        fsm->state = ec_fsm_pdo_state_end;

   365

        return;

   366

    }

   367

   368

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

   369

    ec_fsm_pdo_conf_action_next_sync(fsm);

   370

}

   371

   372

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

   373

   374

/** Assign next PDO.

   375

 */

   376

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   377

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

   378

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

   379

        )

   380

{

   381

    list = list->next; 

   382

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

   383

        return NULL; // no next PDO

   384

    return list_entry(list, ec_pdo_t, list);

   385

}

   386

   387

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

   388

   389

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

   390

 */

   391

void ec_fsm_pdo_conf_action_next_sync(

   392

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

   393

        )

   394

{

   395

    fsm->sync_index++;

   396

   397

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

   398

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

   399

            fsm->state = ec_fsm_pdo_state_error;

   400

            return;

   401

        }

   402

   403

        if (ec_pdo_list_copy(&fsm->pdos,

   404

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

   405

            fsm->state = ec_fsm_pdo_state_error;

   406

            return;

   407

        }

   408

   409

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

   410

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

   411

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

   412

                        "provide the sync manager information!\n",

   413

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

   414

            continue;

   415

        }

   416

   417

        // get first configured PDO

   418

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

   419

            // no pdos configured

   420

            ec_fsm_pdo_conf_action_check_assignment(fsm);

   421

            return;

   422

        }

   423

   424

        ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   425

        return;

   426

    }

   427

   428

    fsm->state = ec_fsm_pdo_state_end;

   429

}

   430

   431

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

   432

   433

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

   434

 */

   435

void ec_fsm_pdo_conf_action_pdo_mapping(

   436

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

   437

        )

   438

{

   439

    const ec_pdo_t *assigned_pdo;

   440

   441

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

   442

   443

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

   444

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   445

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

   446

        ec_pdo_clear_entries(&fsm->slave_pdo);

   447

    }

   448

   449

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

   450

   451

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

   452

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

   453

                    fsm->pdo->index);

   454

   455

        // pdo mapping is unknown; start loading it

   456

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

   457

                &fsm->slave_pdo);

   458

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   459

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

   460

        return;

   461

    }

   462

   463

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

   464

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   465

}

   466

   467

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

   468

   469

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

   470

 */

   471

void ec_fsm_pdo_conf_state_read_mapping(

   472

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

   473

        )

   474

{

   475

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   476

        return;

   477

   478

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

   479

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

   480

                fsm->pdo->index);

   481

   482

    // check if the mapping must be re-configured

   483

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   484

}

   485

   486

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

   487

   488

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

   489

 *

   490

 * \todo Display mapping differences.

   491

 */

   492

void ec_fsm_pdo_conf_action_check_mapping(

   493

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

   494

        )

   495

{

   496

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

   497

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

   498

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

   499

                    fsm->pdo->index);

   500

        ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);

   501

        return;

   502

    }

   503

   504

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

   505

        // TODO display diff

   506

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

   507

    }

   508

   509

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

   510

            fsm->pdo);

   511

    fsm->state = ec_fsm_pdo_conf_state_mapping;

   512

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

   513

}

   514

   515

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

   516