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_DBG("Changing mapping of PDO 0x%04X.\n", pdo->index);

   135

        EC_DBG(""); ec_fsm_pdo_entry_print(fsm);

   136

    }

   137

   138

    fsm->state = ec_fsm_pdo_entry_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_entry_running(

   148

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

   149

        )

   150

{

   151

    return fsm->state != ec_fsm_pdo_entry_state_end

   152

        && fsm->state != ec_fsm_pdo_entry_state_error;

   153

}

   154

   155

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

   156

   157

/** Executes the current state.

   158

 *

   159

 * \return false, if state machine has terminated

   160

 */

   161

int ec_fsm_pdo_entry_exec(

   162

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

   163

        )

   164

{

   165

    fsm->state(fsm);

   166

    return ec_fsm_pdo_entry_running(fsm);

   167

}

   168

   169

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

   170

   171

/** Get execution result.

   172

 *

   173

 * \return true, if the state machine terminated gracefully

   174

 */

   175

int ec_fsm_pdo_entry_success(

   176

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

   177

        )

   178

{

   179

    return fsm->state == ec_fsm_pdo_entry_state_end;

   180

}

   181

   182

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

   183

 * Reading state functions.

   184

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

   185

   186

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

   187

 */

   188

void ec_fsm_pdo_entry_read_state_start(

   189

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

   190

        )

   191

{

   192

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

   193

    ecrt_sdo_request_read(&fsm->request);

   194

   195

    fsm->state = ec_fsm_pdo_entry_read_state_count;

   196

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

   197

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

   198

}

   199

   200

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

   201

   202

/** Read number of mapped PDO entries.

   203

 */

   204

void ec_fsm_pdo_entry_read_state_count(

   205

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

   206

        )

   207

{

   208

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   209

        return;

   210

   211

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

   212

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

   213

        fsm->state = ec_fsm_pdo_entry_state_error;

   214

        return;

   215

    }

   216

   217

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

   218

        EC_ERR("Invalid data size %zu at uploading SDO 0x%04X:%02X.\n",

   219

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

   220

                fsm->request.subindex);

   221

        fsm->state = ec_fsm_pdo_entry_state_error;

   222

        return;

   223

    }

   224

   225

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

   226

   227

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

   228

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

   229

   230

    // read first PDO entry

   231

    fsm->entry_pos = 1;

   232

    ec_fsm_pdo_entry_read_action_next(fsm);

   233

}

   234

   235

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

   236

   237

/** Read next PDO entry.

   238

 */

   239

void ec_fsm_pdo_entry_read_action_next(

   240

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

   241

        )

   242

{

   243

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

   244

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

   245

        ecrt_sdo_request_read(&fsm->request);

   246

        fsm->state = ec_fsm_pdo_entry_read_state_entry;

   247

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

   248

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

   249

        return;

   250

    }

   251

   252

    // finished reading entries.

   253

    fsm->state = ec_fsm_pdo_entry_state_end;

   254

}

   255

   256

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

   257

   258

/** Read PDO entry information.

   259

 */

   260

void ec_fsm_pdo_entry_read_state_entry(

   261

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

   262

        )

   263

{

   264

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

   265

   266

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

   267

        EC_ERR("Failed to read mapped PDO entry.\n");

   268

        fsm->state = ec_fsm_pdo_entry_state_error;

   269

        return;

   270

    }

   271

   272

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

   273

        EC_ERR("Invalid data size %zu at uploading SDO 0x%04X:%02X.\n",

   274

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

   275

                fsm->request.subindex);

   276

        fsm->state = ec_fsm_pdo_entry_state_error;

   277

    } else {

   278

        uint32_t pdo_entry_info;

   279

        ec_pdo_entry_t *pdo_entry;

   280

   281

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

   282

   283

        if (!(pdo_entry = (ec_pdo_entry_t *)

   284

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

   285

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

   286

            fsm->state = ec_fsm_pdo_entry_state_error;

   287

            return;

   288

        }

   289

   290

        ec_pdo_entry_init(pdo_entry);

   291

        pdo_entry->index = pdo_entry_info >> 16;

   292

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

   293

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   294

   295

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

   296

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

   297

                ec_pdo_entry_clear(pdo_entry);

   298

                kfree(pdo_entry);

   299

                fsm->state = ec_fsm_pdo_entry_state_error;

   300

                return;

   301

            }

   302

        }

   303

   304

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

   305

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

   306

                    pdo_entry->index, pdo_entry->subindex,

   307

                    pdo_entry->bit_length,

   308

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

   309

        }

   310

   311

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

   312

   313

        // next PDO entry

   314

        fsm->entry_pos++;

   315

        ec_fsm_pdo_entry_read_action_next(fsm);

   316

    }

   317

}

   318

   319

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

   320

 * Configuration state functions.

   321

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

   322

   323

/** Start PDO mapping.

   324

 */

   325

void ec_fsm_pdo_entry_conf_state_start(

   326

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

   327

        )

   328

{

   329

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

   330

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

   331

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

   332

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

   333

        EC_WARN("Slave %u does not support changing the PDO mapping!\n",

   334

                fsm->slave->ring_position);

   335

        EC_WARN(""); ec_fsm_pdo_entry_print(fsm);

   336

        fsm->state = ec_fsm_pdo_entry_state_error;

   337

        return;

   338

    }

   339

   340

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

   341

        fsm->state = ec_fsm_pdo_entry_state_error;

   342

        return;

   343

    }

   344

   345

    // set mapped PDO entry count to zero

   346

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

   347

    fsm->request.data_size = 1;

   348

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

   349

    ecrt_sdo_request_write(&fsm->request);

   350

   351

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

   352

        EC_DBG("Setting entry count to zero.\n");

   353

   354

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   355

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

   356

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

   357

}

   358

   359

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

   360

   361

/** Process next PDO entry.

   362

 */

   363

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   364

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

   365

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

   366

        )

   367

{

   368

    list = list->next; 

   369

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

   370

        return NULL; // no next entry

   371

    return list_entry(list, ec_pdo_entry_t, list);

   372

}

   373

   374

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

   375

   376

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

   377

 */

   378

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   379

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

   380

        )

   381

{

   382

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   383

        return;

   384

   385

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

   386

        EC_WARN("Failed to clear PDO mapping.\n");

   387

        EC_WARN(""); ec_fsm_pdo_entry_print(fsm);

   388

        fsm->state = ec_fsm_pdo_entry_state_error;

   389

        return;

   390

    }

   391

   392

    // find first entry

   393

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   394

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

   395

   396

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

   397

            EC_DBG("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

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

   419

        EC_DBG("Mapping PDO entry 0x%04X:%02X (%u bit) 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_WARN("Failed to map PDO entry 0x%04X:%02X (%u bit) to "

   447

                "position %u.\n", fsm->entry->index, fsm->entry->subindex,

   448

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

   449

        EC_WARN(""); ec_fsm_pdo_entry_print(fsm);

   450

        fsm->state = ec_fsm_pdo_entry_state_error;

   451

        return;

   452

    }

   453

   454

    // find next entry

   455

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   456

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

   457

   458

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

   459

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

   460

        fsm->request.data_size = 1;

   461

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

   462

        ecrt_sdo_request_write(&fsm->request);

   463

   464

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

   465

            EC_DBG("Setting number of PDO entries to %u.\n", fsm->entry_pos);

   466

   467

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   468

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

   469

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

   470

        return;

   471

    }

   472

   473

    // add next entry

   474

    fsm->entry_pos++;

   475

    ec_fsm_pdo_entry_conf_action_map(fsm);

   476

}

   477

   478

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

   479

   480

/** Set the number of entries.

   481

 */

   482

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   483

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

   484

        )

   485

{

   486

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

   487

   488

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

   489

        EC_WARN("Failed to set number of entries.\n");

   490

        EC_WARN(""); ec_fsm_pdo_entry_print(fsm);

   491

        fsm->state = ec_fsm_pdo_entry_state_error;

   492

        return;

   493

    }

   494

   495

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

   496

        EC_DBG("Successfully configured mapping for PDO 0x%04X.\n",

   497

                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

   516

/** State: END.