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 EtherCAT PDO configuration state machine.

    31

 */

    32

    33

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

    34

    35

#include "globals.h"

    36

#include "master.h"

    37

#include "mailbox.h"

    38

#include "slave_config.h"

    39

    40

#include "fsm_pdo.h"

    41

    42

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

    43

    44

void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *, ec_datagram_t *);

    45

void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);

    46

void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    47

void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *, ec_datagram_t *);

    48

    49

void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);

    50

void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    51

    52

void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *, ec_datagram_t *);

    53

void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    54

void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    55

void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);

    56

void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    57

void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *);

    58

    59

void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *);

    60

void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    61

void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    62

void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *);

    63

void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *, ec_datagram_t *);

    64

void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *);

    65

    66

void ec_fsm_pdo_state_end(ec_fsm_pdo_t *, ec_datagram_t *);

    67

void ec_fsm_pdo_state_error(ec_fsm_pdo_t *, ec_datagram_t *);

    68

    69

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

    70

    71

/** Constructor.

    72

 */

    73

void ec_fsm_pdo_init(

    74

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

    75

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

    76

        )

    77

{

    78

    fsm->fsm_coe = fsm_coe;

    79

    ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);

    80

    ec_pdo_list_init(&fsm->pdos);

    81

    ec_sdo_request_init(&fsm->request);

    82

    ec_pdo_init(&fsm->slave_pdo);

    83

}

    84

    85

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

    86

    87

/** Destructor.

    88

 */

    89

void ec_fsm_pdo_clear(

    90

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

    91

        )

    92

{

    93

    ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);

    94

    ec_pdo_list_clear(&fsm->pdos);

    95

    ec_sdo_request_clear(&fsm->request);

    96

    ec_pdo_clear(&fsm->slave_pdo);

    97

}

    98

    99

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

   100

   101

/** Print the current and desired PDO assignment.

   102

 */

   103

void ec_fsm_pdo_print(

   104

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

   105

        )

   106

{

   107

    printk("Currently assigned PDOs: ");

   108

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

   109

    printk(". PDOs to assign: ");

   110

    ec_pdo_list_print(&fsm->pdos);

   111

    printk("\n");

   112

}

   113

   114

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

   115

   116

/** Start reading the PDO configuration.

   117

 */

   118

void ec_fsm_pdo_start_reading(

   119

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

   120

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

   121

        )

   122

{

   123

    fsm->slave = slave;

   124

    fsm->state = ec_fsm_pdo_read_state_start;

   125

}

   126

   127

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

   128

   129

/** Start writing the PDO configuration.

   130

 */

   131

void ec_fsm_pdo_start_configuration(

   132

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

   133

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

   134

        )

   135

{

   136

    fsm->slave = slave;

   137

    fsm->state = ec_fsm_pdo_conf_state_start;

   138

}

   139

   140

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

   141

   142

/** Get running state.

   143

 *

   144

 * \return false, if state machine has terminated

   145

 */

   146

int ec_fsm_pdo_running(

   147

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

   148

        )

   149

{

   150

    return fsm->state != ec_fsm_pdo_state_end

   151

        && fsm->state != ec_fsm_pdo_state_error;

   152

}

   153

   154

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

   155

   156

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

   157

 *

   158

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

   159

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

   160

 *

   161

 * \return false, if state machine has terminated

   162

 */

   163

int ec_fsm_pdo_exec(

   164

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

   165

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

   166

        )

   167

{

   168

    fsm->state(fsm, datagram);

   169

   170

    return ec_fsm_pdo_running(fsm);

   171

}

   172

   173

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

   174

   175

/** Get execution result.

   176

 *

   177

 * \return true, if the state machine terminated gracefully

   178

 */

   179

int ec_fsm_pdo_success(

   180

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

   181

        )

   182

{

   183

    return fsm->state == ec_fsm_pdo_state_end;

   184

}

   185

   186

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

   187

 * Reading state funtions.

   188

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

   189

   190

/** Start reading PDO assignment.

   191

 */

   192

void ec_fsm_pdo_read_state_start(

   193

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

   194

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

   195

        )

   196

{

   197

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

   198

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

   199

    ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   200

}

   201

   202

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

   203

   204

/** Read PDO assignment of next sync manager.

   205

 */

   206

void ec_fsm_pdo_read_action_next_sync(

   207

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

   208

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

   209

        )

   210

{

   211

    ec_slave_t *slave = fsm->slave;

   212

   213

    fsm->sync_index++;

   214

   215

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

   216

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

   217

            continue;

   218

   219

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

   220

                fsm->sync_index);

   221

   222

        ec_pdo_list_clear_pdos(&fsm->pdos);

   223

   224

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

   225

        ecrt_sdo_request_read(&fsm->request);

   226

        fsm->state = ec_fsm_pdo_read_state_pdo_count;

   227

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

   228

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

   229

        return;

   230

    }

   231

   232

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

   233

   234

    ec_pdo_list_clear_pdos(&fsm->pdos);

   235

    fsm->state = ec_fsm_pdo_state_end;

   236

}

   237

   238

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

   239

   240

/** Count assigned PDOs.

   241

 */

   242

void ec_fsm_pdo_read_state_pdo_count(

   243

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

   244

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

   245

        )

   246

{

   247

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

   248

        return;

   249

    }

   250

   251

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

   252

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

   253

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

   254

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   255

        return;

   256

    }

   257

   258

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

   259

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

   260

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

   261

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

   262

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   263

        return;

   264

    }

   265

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

   266

   267

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

   268

   269

    // read first PDO

   270

    fsm->pdo_pos = 1;

   271

    ec_fsm_pdo_read_action_next_pdo(fsm, datagram);

   272

}

   273

   274

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

   275

   276

/** Read next PDO.

   277

 */

   278

void ec_fsm_pdo_read_action_next_pdo(

   279

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

   280

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

   281

        )

   282

{

   283

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

   284

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

   285

                fsm->pdo_pos);

   286

        ecrt_sdo_request_read(&fsm->request);

   287

        fsm->state = ec_fsm_pdo_read_state_pdo;

   288

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

   289

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

   290

        return;

   291

    }

   292

   293

    // finished reading PDO configuration

   294

   295

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

   296

    ec_pdo_list_clear_pdos(&fsm->pdos);

   297

   298

    // next sync manager

   299

    ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   300

}

   301

   302

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

   303

   304

/** Fetch PDO information.

   305

 */

   306

void ec_fsm_pdo_read_state_pdo(

   307

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

   308

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

   309

        )

   310

{

   311

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

   312

        return;

   313

    }

   314

   315

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

   316

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

   317

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

   318

                fsm->pdo_pos, fsm->sync_index);

   319

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   320

        return;

   321

    }

   322

   323

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

   324

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

   325

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

   326

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

   327

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   328

        return;

   329

    }

   330

   331

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

   332

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

   333

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

   334

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   335

        return;

   336

    }

   337

   338

    ec_pdo_init(fsm->pdo);

   339

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

   340

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

   341

   342

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

   343

   344

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

   345

   346

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   347

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

   348

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

   349

}

   350

   351

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

   352

   353

/** Fetch PDO information.

   354

 */

   355

void ec_fsm_pdo_read_state_pdo_entries(

   356

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

   357

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

   358

        )

   359

{

   360

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

   361

        return;

   362

    }

   363

   364

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

   365

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

   366

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

   367

        ec_fsm_pdo_read_action_next_sync(fsm, datagram);

   368

        return;

   369

    }

   370

   371

    // next PDO

   372

    fsm->pdo_pos++;

   373

    ec_fsm_pdo_read_action_next_pdo(fsm, datagram);

   374

}

   375

   376

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

   377

 * Writing state functions.

   378

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

   379

   380

/** Start PDO configuration.

   381

 */

   382

void ec_fsm_pdo_conf_state_start(

   383

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

   384

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

   385

        )

   386

{

   387

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

   388

        fsm->state = ec_fsm_pdo_state_end;

   389

        return;

   390

    }

   391

   392

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

   393

    ec_fsm_pdo_conf_action_next_sync(fsm, datagram);

   394

}

   395

   396

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

   397

   398

/** Assign next PDO.

   399

 */

   400

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   401

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

   402

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

   403

        )

   404

{

   405

    list = list->next;

   406

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

   407

        return NULL; // no next PDO

   408

    return list_entry(list, ec_pdo_t, list);

   409

}

   410

   411

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

   412

   413

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

   414

 */

   415

void ec_fsm_pdo_conf_action_next_sync(

   416

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

   417

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

   418

        )

   419

{

   420

    fsm->sync_index++;

   421

   422

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

   423

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

   424

            // slave configuration removed in the meantime

   425

            fsm->state = ec_fsm_pdo_state_error;

   426

            return;

   427

        }

   428

   429

        if (ec_pdo_list_copy(&fsm->pdos,

   430

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

   431

        {

   432

            fsm->state = ec_fsm_pdo_state_error;

   433

            return;

   434

        }

   435

   436

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

   437

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

   438

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

   439

                        " but slave does not provide the"

   440

                        " sync manager information!\n",

   441

                        fsm->sync_index);

   442

            continue;

   443

        }

   444

   445

        // get first configured PDO

   446

        if (!(fsm->pdo =

   447

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

   448

            // no pdos configured

   449

            ec_fsm_pdo_conf_action_check_assignment(fsm, datagram);

   450

            return;

   451

        }

   452

   453

        ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram);

   454

        return;

   455

    }

   456

   457

    fsm->state = ec_fsm_pdo_state_end;

   458

}

   459

   460

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

   461

   462

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

   463

 */

   464

void ec_fsm_pdo_conf_action_pdo_mapping(

   465

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

   466

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

   467

        )

   468

{

   469

    const ec_pdo_t *assigned_pdo;

   470

   471

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

   472

   473

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

   474

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   475

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

   476

        ec_pdo_clear_entries(&fsm->slave_pdo);

   477

    }

   478

   479

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

   480

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

   481

                fsm->pdo->index);

   482

   483

        // pdo mapping is unknown; start loading it

   484

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

   485

                &fsm->slave_pdo);

   486

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   487

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

   488

        return;

   489

    }

   490

   491

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

   492

    ec_fsm_pdo_conf_action_check_mapping(fsm, datagram);

   493

}

   494

   495

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

   496

   497

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

   498

 */

   499

void ec_fsm_pdo_conf_state_read_mapping(

   500

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

   501

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

   502

        )

   503

{

   504

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

   505

        return;

   506

    }