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

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

   217

                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

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

   230

   231

    ec_pdo_list_clear_pdos(&fsm->pdos);

   232

    fsm->state = ec_fsm_pdo_state_end;

   233

}

   234

   235

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

   236

   237

/** Count assigned PDOs.

   238

 */

   239

void ec_fsm_pdo_read_state_pdo_count(

   240

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

   241

        )

   242

{

   243

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

   244

   245

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

   246

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

   247

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

   248

        ec_fsm_pdo_read_action_next_sync(fsm);

   249

        return;

   250

    }

   251

   252

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

   253

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

   254

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

   255

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

   256

        ec_fsm_pdo_read_action_next_sync(fsm);

   257

        return;

   258

    }

   259

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

   260

   261

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

   262

   263

    // read first PDO

   264

    fsm->pdo_pos = 1;

   265

    ec_fsm_pdo_read_action_next_pdo(fsm);

   266

}

   267

   268

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

   269

   270

/** Read next PDO.

   271

 */

   272

void ec_fsm_pdo_read_action_next_pdo(

   273

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

   274

        )

   275

{

   276

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

   277

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

   278

                fsm->pdo_pos);

   279

        ecrt_sdo_request_read(&fsm->request);

   280

        fsm->state = ec_fsm_pdo_read_state_pdo;

   281

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

   282

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

   283

        return;

   284

    }

   285

   286

    // finished reading PDO configuration

   287

   288

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

   289

    ec_pdo_list_clear_pdos(&fsm->pdos);

   290

   291

    // next sync manager

   292

    ec_fsm_pdo_read_action_next_sync(fsm);

   293

}

   294

   295

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

   296

   297

/** Fetch PDO information.

   298

 */

   299

void ec_fsm_pdo_read_state_pdo(

   300

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

   301

        )

   302

{

   303

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

   304

   305

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

   306

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

   307

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

   308

                fsm->pdo_pos, fsm->sync_index);

   309

        ec_fsm_pdo_read_action_next_sync(fsm);

   310

        return;

   311

    }

   312

   313

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

   314

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

   315

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

   316

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

   317

        ec_fsm_pdo_read_action_next_sync(fsm);

   318

        return;

   319

    }

   320

   321

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

   322

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

   323

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

   324

        ec_fsm_pdo_read_action_next_sync(fsm);

   325

        return;

   326

    }

   327

   328

    ec_pdo_init(fsm->pdo);

   329

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

   330

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

   331

   332

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

   333

   334

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

   335

   336

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   337

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

   338

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

   339

}

   340

   341

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

   342

   343

/** Fetch PDO information.

   344

 */

   345

void ec_fsm_pdo_read_state_pdo_entries(

   346

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

   347

        )

   348

{

   349

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   350

        return;

   351

   352

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

   353

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

   354

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

   355

        ec_fsm_pdo_read_action_next_sync(fsm);

   356

        return;

   357

    }

   358

   359

    // next PDO

   360

    fsm->pdo_pos++;

   361

    ec_fsm_pdo_read_action_next_pdo(fsm);

   362

}

   363

   364

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

   365

 * Writing state functions.

   366

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

   367

   368

/** Start PDO configuration.

   369

 */

   370

void ec_fsm_pdo_conf_state_start(

   371

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

   372

        )

   373

{

   374

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

   375

        fsm->state = ec_fsm_pdo_state_end;

   376

        return;

   377

    }

   378

   379

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

   380

    ec_fsm_pdo_conf_action_next_sync(fsm);

   381

}

   382

   383

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

   384

   385

/** Assign next PDO.

   386

 */

   387

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   388

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

   389

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

   390

        )

   391

{

   392

    list = list->next; 

   393

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

   394

        return NULL; // no next PDO

   395

    return list_entry(list, ec_pdo_t, list);

   396

}

   397

   398

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

   399

   400

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

   401

 */

   402

void ec_fsm_pdo_conf_action_next_sync(

   403

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

   404

        )

   405

{

   406

    fsm->sync_index++;

   407

   408

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

   409

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

   410

            // slave configuration removed in the meantime

   411

            fsm->state = ec_fsm_pdo_state_error;

   412

            return;

   413

        }

   414

   415

        if (ec_pdo_list_copy(&fsm->pdos,

   416

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

   417

        {

   418

            fsm->state = ec_fsm_pdo_state_error;

   419

            return;

   420

        }

   421

   422

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

   423

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

   424

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

   425

                        " but slave does not provide the"

   426

                        " sync manager information!\n",

   427

                        fsm->sync_index);

   428

            continue;

   429

        }

   430

   431

        // get first configured PDO

   432

        if (!(fsm->pdo =

   433

                    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

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

   466

                fsm->pdo->index);

   467

   468

        // pdo mapping is unknown; start loading it

   469

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

   470

                &fsm->slave_pdo);

   471

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   472

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

   473

        return;

   474

    }

   475

   476

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

   477

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   478

}

   479

   480

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

   481

   482

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

   483

 */

   484

void ec_fsm_pdo_conf_state_read_mapping(

   485

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

   486

        )

   487

{

   488

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   489

        return;

   490

   491

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

   492

        EC_SLAVE_WARN(fsm->slave,

   493

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

   494

                fsm->pdo->index);

   495

   496

    // check if the mapping must be re-configured

   497

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   498

}

   499

   500

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

   501

   502

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

   503

 *

   504

 * \todo Display mapping differences.

   505

 */

   506

void ec_fsm_pdo_conf_action_check_mapping(

   507

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

   508

        )

   509

{

   510

    // check, if slave supports PDO configuration

   511

    if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)

   512

            && fsm->slave->sii.has_general

   513

            && fsm->slave->sii.coe_details.enable_pdo_configuration) {