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 *, ec_datagram_t *);

    46

void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);

    47

void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    48

void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *, ec_datagram_t *);

    49

    50

void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);

    51

void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    52

    53

void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *, ec_datagram_t *);

    54

void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    55

void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    56

void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);

    57

void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    58

void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);

    59

    60

void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);

    61

void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    62

void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    63

void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    64

void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *, ec_datagram_t *);

    65

void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    66

    67

void ec_fsm_pdo_state_end(ec_fsm_pdo_t *, ec_datagram_t *);

    68

void ec_fsm_pdo_state_error(ec_fsm_pdo_t *, ec_datagram_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

        ec_datagram_t *datagram /**< Datagram to use. */

   167

        )

   168

{

   169

    fsm->state(fsm, datagram);

   170

   171

    return ec_fsm_pdo_running(fsm);

   172

}

   173

   174

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

   175

   176

/** Get execution result.

   177

 *

   178

 * \return true, if the state machine terminated gracefully

   179

 */

   180

int ec_fsm_pdo_success(

   181

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

   182

        )

   183

{

   184

    return fsm->state == ec_fsm_pdo_state_end;

   185

}

   186

   187

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

   188

 * Reading state funtions.

   189

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

   190

   191

/** Start reading PDO assignment.

   192

 */

   193

void ec_fsm_pdo_read_state_start(

   194

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

   195

        ec_datagram_t *datagram /**< Datagram to use. */

   196

        )

   197

{

   198

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

   199

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

   200

    ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   201

}

   202

   203

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

   204

   205

/** Read PDO assignment of next sync manager.

   206

 */

   207

void ec_fsm_pdo_read_action_next_sync(

   208

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

   209

        ec_datagram_t *datagram /**< Datagram to use. */

   210

        )

   211

{

   212

    ec_slave_t *slave = fsm->slave;

   213

   214

    fsm->sync_index++;

   215

   216

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

   217

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

   218

            continue;

   219

   220

        EC_SLAVE_DBG(slave, 1, "Reading PDO assignment of SM%u.\n",

   221

                fsm->sync_index);

   222

   223

        ec_pdo_list_clear_pdos(&fsm->pdos);

   224

   225

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

   226

        ecrt_sdo_request_read(&fsm->request);

   227

        fsm->state = ec_fsm_pdo_read_state_pdo_count;

   228

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

   229

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

   230

        return;

   231

    }

   232

   233

    EC_SLAVE_DBG(slave, 1, "Reading of PDO configuration finished.\n");

   234

   235

    ec_pdo_list_clear_pdos(&fsm->pdos);

   236

    fsm->state = ec_fsm_pdo_state_end;

   237

}

   238

   239

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

   240

   241

/** Count assigned PDOs.

   242

 */

   243

void ec_fsm_pdo_read_state_pdo_count(

   244

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

   245

        ec_datagram_t *datagram /**< Datagram to use. */

   246

        )

   247

{

   248

    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {

   249

        return;

   250

    }

   251

   252

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

   253

        EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs"

   254

                " for SM%u.\n", fsm->sync_index);

   255

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   256

        return;

   257

    }

   258

   259

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

   260

        EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"

   261

                " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,

   262

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

   263

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   264

        return;

   265

    }

   266

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

   267

   268

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

   269

   270

    // read first PDO

   271

    fsm->pdo_pos = 1;

   272

    ec_fsm_pdo_read_action_next_pdo(fsm, datagram);

   273

}

   274

   275

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

   276

   277

/** Read next PDO.

   278

 */

   279

void ec_fsm_pdo_read_action_next_pdo(

   280

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

   281

        ec_datagram_t *datagram /**< Datagram to use. */

   282

        )

   283

{

   284

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

   285

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

   286

                fsm->pdo_pos);

   287

        ecrt_sdo_request_read(&fsm->request);

   288

        fsm->state = ec_fsm_pdo_read_state_pdo;

   289

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

   290

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

   291

        return;

   292

    }

   293

   294

    // finished reading PDO configuration

   295

   296

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

   297

    ec_pdo_list_clear_pdos(&fsm->pdos);

   298

   299

    // next sync manager

   300

    ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   301

}

   302

   303

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

   304

   305

/** Fetch PDO information.

   306

 */

   307

void ec_fsm_pdo_read_state_pdo(

   308

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

   309

        ec_datagram_t *datagram /**< Datagram to use. */

   310

        )

   311

{

   312

    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {

   313

        return;

   314

    }

   315

   316

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

   317

        EC_SLAVE_ERR(fsm->slave, "Failed to read index of"

   318

                " assigned PDO %u from SM%u.\n",

   319

                fsm->pdo_pos, fsm->sync_index);

   320

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   321

        return;

   322

    }

   323

   324

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

   325

        EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"

   326

                " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,

   327

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

   328

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   329

        return;

   330

    }

   331

   332

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

   333

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

   334

        EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n");

   335

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   336

        return;

   337

    }

   338

   339

    ec_pdo_init(fsm->pdo);

   340

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

   341

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

   342

   343

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

   344

   345

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

   346

   347

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   348

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

   349

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

   350

}

   351

   352

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

   353

   354

/** Fetch PDO information.

   355

 */

   356

void ec_fsm_pdo_read_state_pdo_entries(

   357

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

   358

        ec_datagram_t *datagram /**< Datagram to use. */

   359

        )

   360

{

   361

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) {

   362

        return;

   363

    }

   364

   365

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

   366

        EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries"

   367

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

   368

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   369

        return;

   370

    }

   371

   372

    // next PDO

   373

    fsm->pdo_pos++;

   374

    ec_fsm_pdo_read_action_next_pdo(fsm, datagram);

   375

}

   376

   377

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

   378

 * Writing state functions.

   379

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

   380

   381

/** Start PDO configuration.

   382

 */

   383

void ec_fsm_pdo_conf_state_start(

   384

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

   385

        ec_datagram_t *datagram /**< Datagram to use. */

   386

        )

   387

{

   388

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

   389

        fsm->state = ec_fsm_pdo_state_end;

   390

        return;

   391

    }

   392

   393

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

   394

    ec_fsm_pdo_conf_action_next_sync(fsm, datagram);

   395

}

   396

   397

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

   398

   399

/** Assign next PDO.

   400

 *

   401

 * \return Next PDO, or NULL.

   402

 */

   403

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   404

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

   405

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

   406

        )

   407

{

   408

    list = list->next;

   409

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

   410

        return NULL; // no next PDO

   411

    return list_entry(list, ec_pdo_t, list);

   412

}

   413

   414

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

   415

   416

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

   417

 */

   418

void ec_fsm_pdo_conf_action_next_sync(

   419

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

   420

        ec_datagram_t *datagram /**< Datagram to use. */

   421

        )

   422

{

   423

    fsm->sync_index++;

   424

   425

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

   426

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

   427

            // slave configuration removed in the meantime

   428

            fsm->state = ec_fsm_pdo_state_error;

   429

            return;

   430

        }

   431

   432

        if (ec_pdo_list_copy(&fsm->pdos,

   433

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

   434

        {

   435

            fsm->state = ec_fsm_pdo_state_error;

   436

            return;

   437

        }

   438

   439

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

   440

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

   441

                EC_SLAVE_WARN(fsm->slave, "PDOs configured for SM%u,"

   442

                        " but slave does not provide the"

   443

                        " sync manager information!\n",

   444

                        fsm->sync_index);

   445

            continue;

   446

        }

   447

   448

        // get first configured PDO

   449

        if (!(fsm->pdo =

   450

                    ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {

   451

            // no pdos configured

   452

            ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);

   453

            return;

   454

        }

   455

   456

        ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);

   457

        return;

   458

    }

   459

   460

    fsm->state = ec_fsm_pdo_state_end;

   461

}

   462

   463

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

   464

   465

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

   466

 */

   467

void ec_fsm_pdo_conf_action_pdo_mapping(

   468

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

   469

        ec_datagram_t *datagram /**< Datagram to use. */

   470

        )

   471

{

   472

    const ec_pdo_t *assigned_pdo;

   473

   474

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

   475

   476

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

   477

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   478

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

   479

        ec_pdo_clear_entries(&fsm->slave_pdo);

   480

    }

   481

   482

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

   483

        EC_SLAVE_DBG(fsm->slave, 1, "Reading mapping of PDO 0x%04X.\n",

   484

                fsm->pdo->index);

   485

   486

        // pdo mapping is unknown; start loading it

   487

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

   488

                &fsm->slave_pdo);

   489

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   490

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

   491

        return;

   492

    }

   493

   494

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

   495

    ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);

   496

}

   497

   498

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

   499

   500

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

   501

 */

   502

void ec_fsm_pdo_conf_state_read_mapping(

   503

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

   504

        ec_datagram_t *datagram /**< Datagram to use. */

   505

        )

   506

{