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 mapping 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_entry.h"

    42

    43

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

    44

    45

void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);

    46

void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);

    47

void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);

    48

    49

void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);

    50

    51

void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);

    52

void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);

    53

void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);

    54

void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);

    55

    56

void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *);

    57

    58

void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);

    59

void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);

    60

    61

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

    62

    63

/** Constructor.

    64

 */

    65

void ec_fsm_pdo_entry_init(

    66

        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */

    67

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

    68

        )

    69

{

    70

    fsm->fsm_coe = fsm_coe;

    71

    ec_sdo_request_init(&fsm->request);

    72

}

    73

    74

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

    75

    76

/** Destructor.

    77

 */

    78

void ec_fsm_pdo_entry_clear(

    79

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

    80

        )

    81

{

    82

    ec_sdo_request_clear(&fsm->request);

    83

}

    84

    85

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

    86

    87

/** Print the current and desired PDO mapping.

    88

 */

    89

void ec_fsm_pdo_entry_print(

    90

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

    91

        )

    92

{

    93

    printk("Currently mapped PDO entries: ");

    94

    ec_pdo_print_entries(fsm->cur_pdo);

    95

    printk(". Entries to map: ");

    96

    ec_pdo_print_entries(fsm->source_pdo);

    97

    printk("\n");

    98

}

    99

   100

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

   101

   102

/** Start reading a PDO's entries.

   103

 */

   104

void ec_fsm_pdo_entry_start_reading(

   105

        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */

   106

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

   107

        ec_pdo_t *pdo /**< PDO to read entries for. */

   108

        )

   109

{

   110

    fsm->slave = slave;

   111

    fsm->target_pdo = pdo;

   112

   113

    ec_pdo_clear_entries(fsm->target_pdo);

   114

   115

    fsm->state = ec_fsm_pdo_entry_read_state_start;

   116

}

   117

   118

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

   119

   120

/** Start PDO mapping state machine.

   121

 */

   122

void ec_fsm_pdo_entry_start_configuration(

   123

        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */

   124

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

   125

        const ec_pdo_t *pdo, /**< PDO with the desired entries. */

   126

        const ec_pdo_t *cur_pdo /**< Current PDO mapping. */

   127

        )

   128

{

   129

    fsm->slave = slave;

   130

    fsm->source_pdo = pdo;

   131

    fsm->cur_pdo = cur_pdo;

   132

   133

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

   134

        EC_SLAVE_DBG(slave, 1, "Changing mapping of PDO 0x%04X.\n",

   135

                pdo->index);

   136

        EC_SLAVE_DBG(slave, 1, ""); ec_fsm_pdo_entry_print(fsm);

   137

    }

   138

   139

    fsm->state = ec_fsm_pdo_entry_conf_state_start;

   140

}

   141

   142

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

   143

   144

/** Get running state.

   145

 *

   146

 * \return false, if state machine has terminated

   147

 */

   148

int ec_fsm_pdo_entry_running(

   149

        const ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   150

        )

   151

{

   152

    return fsm->state != ec_fsm_pdo_entry_state_end

   153

        && fsm->state != ec_fsm_pdo_entry_state_error;

   154

}

   155

   156

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

   157

   158

/** Executes the current state.

   159

 *

   160

 * \return false, if state machine has terminated

   161

 */

   162

int ec_fsm_pdo_entry_exec(

   163

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   164

        )

   165

{

   166

    fsm->state(fsm);

   167

    return ec_fsm_pdo_entry_running(fsm);

   168

}

   169

   170

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

   171

   172

/** Get execution result.

   173

 *

   174

 * \return true, if the state machine terminated gracefully

   175

 */

   176

int ec_fsm_pdo_entry_success(

   177

        const ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   178

        )

   179

{

   180

    return fsm->state == ec_fsm_pdo_entry_state_end;

   181

}

   182

   183

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

   184

 * Reading state functions.

   185

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

   186

   187

/** Request reading the number of mapped PDO entries.

   188

 */

   189

void ec_fsm_pdo_entry_read_state_start(

   190

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   191

        )

   192

{

   193

    ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, 0);

   194

    ecrt_sdo_request_read(&fsm->request);

   195

   196

    fsm->state = ec_fsm_pdo_entry_read_state_count;

   197

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

   198

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

   199

}

   200

   201

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

   202

   203

/** Read number of mapped PDO entries.

   204

 */

   205

void ec_fsm_pdo_entry_read_state_count(

   206

        ec_fsm_pdo_entry_t *fsm /**< finite state machine */

   207

        )

   208

{

   209

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   210

        return;

   211

   212

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

   213

        EC_SLAVE_ERR(fsm->slave,

   214

                "Failed to read number of mapped PDO entries.\n");

   215

        fsm->state = ec_fsm_pdo_entry_state_error;

   216

        return;

   217

    }

   218

   219

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

   220

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

   221

                " SDO 0x%04X:%02X.\n",

   222

                fsm->request.data_size, fsm->request.index,

   223

                fsm->request.subindex);

   224

        fsm->state = ec_fsm_pdo_entry_state_error;

   225

        return;

   226

    }

   227

   228

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

   229

   230

    EC_SLAVE_DBG(fsm->slave, 1, "%u PDO entries mapped.\n", fsm->entry_count);

   231

   232

    // read first PDO entry

   233

    fsm->entry_pos = 1;

   234

    ec_fsm_pdo_entry_read_action_next(fsm);

   235

}

   236

   237

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

   238

   239

/** Read next PDO entry.

   240

 */

   241

void ec_fsm_pdo_entry_read_action_next(

   242

        ec_fsm_pdo_entry_t *fsm /**< finite state machine */

   243

        )

   244

{

   245

    if (fsm->entry_pos <= fsm->entry_count) {

   246

        ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, fsm->entry_pos);

   247

        ecrt_sdo_request_read(&fsm->request);

   248

        fsm->state = ec_fsm_pdo_entry_read_state_entry;

   249

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

   250

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

   251

        return;

   252

    }

   253

   254

    // finished reading entries.

   255

    fsm->state = ec_fsm_pdo_entry_state_end;

   256

}

   257

   258

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

   259

   260

/** Read PDO entry information.

   261

 */

   262

void ec_fsm_pdo_entry_read_state_entry(

   263

        ec_fsm_pdo_entry_t *fsm /**< finite state machine */

   264

        )

   265

{

   266

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

   267

   268

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

   269

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

   270

        fsm->state = ec_fsm_pdo_entry_state_error;

   271

        return;

   272

    }

   273

   274

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

   275

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

   276

                " uploading SDO 0x%04X:%02X.\n",

   277

                fsm->request.data_size, fsm->request.index,

   278

                fsm->request.subindex);

   279

        fsm->state = ec_fsm_pdo_entry_state_error;

   280

    } else {

   281

        uint32_t pdo_entry_info;

   282

        ec_pdo_entry_t *pdo_entry;

   283

   284

        pdo_entry_info = EC_READ_U32(fsm->request.data);

   285

   286

        if (!(pdo_entry = (ec_pdo_entry_t *)

   287

                    kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {

   288

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

   289

            fsm->state = ec_fsm_pdo_entry_state_error;

   290

            return;

   291

        }

   292

   293

        ec_pdo_entry_init(pdo_entry);

   294

        pdo_entry->index = pdo_entry_info >> 16;

   295

        pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;

   296

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   297

   298

        if (!pdo_entry->index && !pdo_entry->subindex) {

   299

            if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {

   300

                ec_pdo_entry_clear(pdo_entry);

   301

                kfree(pdo_entry);

   302

                fsm->state = ec_fsm_pdo_entry_state_error;

   303

                return;

   304

            }

   305

        }

   306

   307

        EC_SLAVE_DBG(fsm->slave, 1,

   308

                "PDO entry 0x%04X:%02X, %u bit, \"%s\".\n",

   309

                pdo_entry->index, pdo_entry->subindex,

   310

                pdo_entry->bit_length,

   311

                pdo_entry->name ? pdo_entry->name : "???");

   312

   313

        list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries);

   314

   315

        // next PDO entry

   316

        fsm->entry_pos++;

   317

        ec_fsm_pdo_entry_read_action_next(fsm);

   318

    }

   319

}

   320

   321

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

   322

 * Configuration state functions.

   323

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

   324

   325

/** Start PDO mapping.

   326

 */

   327

void ec_fsm_pdo_entry_conf_state_start(

   328

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   329

        )

   330

{

   331

    // PDO mapping has to be changed. Does the slave support this?

   332

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

   333

            || (fsm->slave->sii.has_general

   334

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

   335

            EC_SLAVE_WARN(fsm->slave, "Slave does not support"

   336

                    " changing the PDO mapping!\n");

   337

        EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);

   338

        fsm->state = ec_fsm_pdo_entry_state_error;

   339

        return;

   340

    }

   341

   342

    if (ec_sdo_request_alloc(&fsm->request, 4)) {

   343

        fsm->state = ec_fsm_pdo_entry_state_error;

   344

        return;

   345

    }

   346

   347

    // set mapped PDO entry count to zero

   348

    EC_WRITE_U8(fsm->request.data, 0);

   349

    fsm->request.data_size = 1;

   350

    ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0);

   351

    ecrt_sdo_request_write(&fsm->request);

   352

   353

    EC_SLAVE_DBG(fsm->slave, 1, "Setting entry count to zero.\n");

   354

   355

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   356

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

   357

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

   358

}

   359

   360

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

   361

   362

/** Process next PDO entry.

   363

 */

   364

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   365

        const ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */

   366

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

   367

        )

   368

{

   369

    list = list->next; 

   370

    if (list == &fsm->source_pdo->entries)

   371

        return NULL; // no next entry

   372

    return list_entry(list, ec_pdo_entry_t, list);

   373

}

   374

   375

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

   376

   377

/** Set the number of mapped entries to zero.

   378

 */

   379

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   380

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   381

        )

   382

{

   383

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   384

        return;

   385

   386

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

   387

        EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n");

   388

        EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);

   389

        fsm->state = ec_fsm_pdo_entry_state_error;

   390

        return;

   391

    }

   392

   393

    // find first entry

   394

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   395

                    fsm, &fsm->source_pdo->entries))) {

   396

   397

        EC_SLAVE_DBG(fsm->slave, 1, "No entries to map.\n");

   398

   399

        fsm->state = ec_fsm_pdo_entry_state_end; // finished

   400

        return;

   401

    }

   402

   403

    // add first entry

   404

    fsm->entry_pos = 1;

   405

    ec_fsm_pdo_entry_conf_action_map(fsm);

   406

}

   407

   408

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

   409

   410

/** Starts to add a PDO entry.

   411

 */

   412

void ec_fsm_pdo_entry_conf_action_map(

   413

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   414

        )

   415

{

   416

    uint32_t value;

   417

   418

    EC_SLAVE_DBG(fsm->slave, 1, "Mapping PDO entry 0x%04X:%02X (%u bit)"

   419

            " at position %u.\n",

   420

            fsm->entry->index, fsm->entry->subindex,

   421

            fsm->entry->bit_length, fsm->entry_pos);

   422

   423

    value = fsm->entry->index << 16

   424

        | fsm->entry->subindex << 8 | fsm->entry->bit_length;

   425

    EC_WRITE_U32(fsm->request.data, value);

   426

    fsm->request.data_size = 4;

   427

    ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, fsm->entry_pos);

   428

    ecrt_sdo_request_write(&fsm->request);

   429

   430

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;

   431

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

   432

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

   433

}

   434

   435

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

   436

   437

/** Add a PDO entry.

   438

 */

   439

void ec_fsm_pdo_entry_conf_state_map_entry(

   440

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   441

        )

   442

{

   443

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

   444

   445

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

   446

        EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"

   447

                " 0x%04X:%02X (%u bit) to position %u.\n",

   448

                fsm->entry->index, fsm->entry->subindex,

   449

                fsm->entry->bit_length, fsm->entry_pos);

   450

        EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);

   451

        fsm->state = ec_fsm_pdo_entry_state_error;

   452

        return;

   453

    }

   454

   455

    // find next entry

   456

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   457

                    fsm, &fsm->entry->list))) {

   458

   459

        // No more entries to add. Write entry count.

   460

        EC_WRITE_U8(fsm->request.data, fsm->entry_pos);

   461

        fsm->request.data_size = 1;

   462

        ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0);

   463

        ecrt_sdo_request_write(&fsm->request);

   464

   465

        EC_SLAVE_DBG(fsm->slave, 1, "Setting number of PDO entries to %u.\n",

   466

                fsm->entry_pos);

   467

   468

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   469

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

   470

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

   471

        return;

   472

    }

   473

   474

    // add next entry

   475

    fsm->entry_pos++;

   476

    ec_fsm_pdo_entry_conf_action_map(fsm);

   477

}

   478

   479

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

   480

   481

/** Set the number of entries.

   482

 */

   483

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   484

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   485

        )

   486

{

   487

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

   488

   489

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

   490

        EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n");

   491

        EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);

   492

        fsm->state = ec_fsm_pdo_entry_state_error;

   493

        return;

   494

    }

   495

   496

    EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured"

   497

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

   498

   499

    fsm->state = ec_fsm_pdo_entry_state_end; // finished

   500

}

   501

   502

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

   503

 * Common state functions

   504

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

   505

   506

/** State: ERROR.

   507

 */

   508

void ec_fsm_pdo_entry_state_error(

   509

        ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */

   510

        )

   511

{

   512

}

   513

   514

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

   515