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

    fsm->state = ec_fsm_pdo_state_end;

   218

}

   219

   220

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

   221

   222

/** Count assigned PDOs.

   223

 */

   224

void ec_fsm_pdo_read_state_pdo_count(

   225

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

   226

        )

   227

{

   228

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

   229

   230

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

   231

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

   232

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

   233

        fsm->state = ec_fsm_pdo_state_error;

   234

        return;

   235

    }

   236

   237

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

   238

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

   239

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

   240

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

   241

                fsm->slave->ring_position);

   242

        fsm->state = ec_fsm_pdo_state_error;

   243

        return;

   244

    }

   245

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

   246

   247

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

   248

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

   249

   250

    // read first PDO

   251

    fsm->pdo_pos = 1;

   252

    ec_fsm_pdo_read_action_next_pdo(fsm);

   253

}

   254

   255

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

   256

   257

/** Read next PDO.

   258

 */

   259

void ec_fsm_pdo_read_action_next_pdo(

   260

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

   261

        )

   262

{

   263

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

   264

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

   265

                fsm->pdo_pos);

   266

        ecrt_sdo_request_read(&fsm->request);

   267

        fsm->state = ec_fsm_pdo_read_state_pdo;

   268

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

   269

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

   270

        return;

   271

    }

   272

   273

    // finished reading PDO configuration

   274

   275

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

   276

        fsm->state = ec_fsm_pdo_state_error;

   277

        return;

   278

    }

   279

   280

    ec_pdo_list_clear_pdos(&fsm->pdos);

   281

   282

    // next sync manager

   283

    ec_fsm_pdo_read_action_next_sync(fsm);

   284

}

   285

   286

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

   287

   288

/** Fetch PDO information.

   289

 */

   290

void ec_fsm_pdo_read_state_pdo(

   291

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

   292

        )

   293

{

   294

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

   295

   296

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

   297

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

   298

                fsm->pdo_pos, fsm->sync_index);

   299

        fsm->state = ec_fsm_pdo_state_error;

   300

        return;

   301

    }

   302

   303

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

   304

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

   305

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

   306

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

   307

                fsm->slave->ring_position);

   308

        fsm->state = ec_fsm_pdo_state_error;

   309

        return;

   310

    }

   311

   312

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

   313

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

   314

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

   315

        fsm->state = ec_fsm_pdo_state_error;

   316

        return;

   317

    }

   318

   319

    ec_pdo_init(fsm->pdo);

   320

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

   321

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

   322

   323

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

   324

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

   325

   326

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

   327

   328

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   329

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

   330

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

   331

}

   332

   333

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

   334

   335

/** Fetch PDO information.

   336

 */

   337

void ec_fsm_pdo_read_state_pdo_entries(

   338

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

   339

        )

   340

{

   341

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   342

        return;

   343

   344

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

   345

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

   346

                fsm->pdo->index);

   347

        fsm->state = ec_fsm_pdo_state_error;

   348

        return;

   349

    }

   350

   351

    // next PDO

   352

    fsm->pdo_pos++;

   353

    ec_fsm_pdo_read_action_next_pdo(fsm);

   354

}

   355

   356

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

   357

 * Writing state functions.

   358

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

   359

   360

/** Start PDO configuration.

   361

 */

   362

void ec_fsm_pdo_conf_state_start(

   363

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

   364

        )

   365

{

   366

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

   367

        fsm->state = ec_fsm_pdo_state_end;

   368

        return;

   369

    }

   370

   371

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

   372

    ec_fsm_pdo_conf_action_next_sync(fsm);

   373

}

   374

   375

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

   376

   377

/** Assign next PDO.

   378

 */

   379

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   380

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

   381

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

   382

        )

   383

{

   384

    list = list->next; 

   385

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

   386

        return NULL; // no next PDO

   387

    return list_entry(list, ec_pdo_t, list);

   388

}

   389

   390

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

   391

   392

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

   393

 */

   394

void ec_fsm_pdo_conf_action_next_sync(

   395

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

   396

        )

   397

{

   398

    fsm->sync_index++;

   399

   400

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

   401

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

   402

            fsm->state = ec_fsm_pdo_state_error;

   403

            return;

   404

        }

   405

   406

        if (ec_pdo_list_copy(&fsm->pdos,

   407

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

   408

            fsm->state = ec_fsm_pdo_state_error;

   409

            return;

   410

        }

   411

   412

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

   413

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

   414

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

   415

                        "provide the sync manager information!\n",

   416

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

   417

            continue;

   418

        }

   419

   420

        // get first configured PDO

   421

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

   422

            // no pdos configured

   423

            ec_fsm_pdo_conf_action_check_assignment(fsm);

   424

            return;

   425

        }

   426

   427

        ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   428

        return;

   429

    }

   430

   431

    fsm->state = ec_fsm_pdo_state_end;

   432

}

   433

   434

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

   435

   436

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

   437

 */

   438

void ec_fsm_pdo_conf_action_pdo_mapping(

   439

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

   440

        )

   441

{

   442

    const ec_pdo_t *assigned_pdo;

   443

   444

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

   445

   446

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

   447

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   448

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

   449

        ec_pdo_clear_entries(&fsm->slave_pdo);

   450

    }

   451

   452

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

   453

   454

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

   455

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

   456

                    fsm->pdo->index);

   457

   458

        // pdo mapping is unknown; start loading it

   459

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

   460

                &fsm->slave_pdo);

   461

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   462

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

   463

        return;

   464

    }

   465

   466

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

   467

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   468

}

   469

   470

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

   471

   472

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

   473

 */

   474

void ec_fsm_pdo_conf_state_read_mapping(

   475

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

   476

        )

   477

{

   478

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   479

        return;

   480

   481

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

   482

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

   483

                fsm->pdo->index);

   484

   485

    // check if the mapping must be re-configured

   486

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   487

}

   488

   489

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

   490

   491

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

   492

 *

   493

 * \todo Display mapping differences.

   494

 */

   495

void ec_fsm_pdo_conf_action_check_mapping(

   496

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

   497

        )

   498

{

   499

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

   500

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

   501

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

   502

                    fsm->pdo->index);

   503

        ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);

   504

        return;

   505

    }

   506

   507

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

   508

        // TODO display diff

   509

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

   510

    }

   511

   512

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

   513

            fsm->pdo);

   514

    fsm->state = ec_fsm_pdo_conf_state_mapping;

   515

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

   516

}

   517