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

    46

void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *, ec_datagram_t *);

    47

void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *, ec_datagram_t *);

    48

    49

void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *, ec_datagram_t *);

    50

    51

void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *, ec_datagram_t *);

    52

void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *,

    53

        ec_datagram_t *);

    54

void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *,

    55

        ec_datagram_t *);

    56

void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *,

    57

        ec_datagram_t *);

    58

    59

void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *, ec_datagram_t *);

    60

    61

void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *, ec_datagram_t *);

    62

void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *, ec_datagram_t *);

    63

    64

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

    65

    66

/** Constructor.

    67

 */

    68

void ec_fsm_pdo_entry_init(

    69

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

    70

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

    71

        )

    72

{

    73

    fsm->fsm_coe = fsm_coe;

    74

    ec_sdo_request_init(&fsm->request);

    75

}

    76

    77

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

    78

    79

/** Destructor.

    80

 */

    81

void ec_fsm_pdo_entry_clear(

    82

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

    83

        )

    84

{

    85

    ec_sdo_request_clear(&fsm->request);

    86

}

    87

    88

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

    89

    90

/** Print the current and desired PDO mapping.

    91

 */

    92

void ec_fsm_pdo_entry_print(

    93

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

    94

        )

    95

{

    96

    printk("Currently mapped PDO entries: ");

    97

    ec_pdo_print_entries(fsm->cur_pdo);

    98

    printk(". Entries to map: ");

    99

    ec_pdo_print_entries(fsm->source_pdo);

   100

    printk("\n");

   101

}

   102

   103

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

   104

   105

/** Start reading a PDO's entries.

   106

 */

   107

void ec_fsm_pdo_entry_start_reading(

   108

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

   109

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

   110

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

   111

        )

   112

{

   113

    fsm->slave = slave;

   114

    fsm->target_pdo = pdo;

   115

   116

    ec_pdo_clear_entries(fsm->target_pdo);

   117

   118

    fsm->state = ec_fsm_pdo_entry_read_state_start;

   119

}

   120

   121

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

   122

   123

/** Start PDO mapping state machine.

   124

 */

   125

void ec_fsm_pdo_entry_start_configuration(

   126

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

   127

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

   128

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

   129

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

   130

        )

   131

{

   132

    fsm->slave = slave;

   133

    fsm->source_pdo = pdo;

   134

    fsm->cur_pdo = cur_pdo;

   135

   136

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

   137

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

   138

                pdo->index);

   139

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

   140

    }

   141

   142

    fsm->state = ec_fsm_pdo_entry_conf_state_start;

   143

}

   144

   145

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

   146

   147

/** Get running state.

   148

 *

   149

 * \return false, if state machine has terminated

   150

 */

   151

int ec_fsm_pdo_entry_running(

   152

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

   153

        )

   154

{

   155

    return fsm->state != ec_fsm_pdo_entry_state_end

   156

        && fsm->state != ec_fsm_pdo_entry_state_error;

   157

}

   158

   159

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

   160

   161

/** Executes the current state.

   162

 *

   163

 * \return false, if state machine has terminated

   164

 */

   165

int ec_fsm_pdo_entry_exec(

   166

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

   167

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

   168

        )

   169

{

   170

    fsm->state(fsm, datagram);

   171

   172

    return ec_fsm_pdo_entry_running(fsm);

   173

}

   174

   175

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

   176

   177

/** Get execution result.

   178

 *

   179

 * \return true, if the state machine terminated gracefully

   180

 */

   181

int ec_fsm_pdo_entry_success(

   182

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

   183

        )

   184

{

   185

    return fsm->state == ec_fsm_pdo_entry_state_end;

   186

}

   187

   188

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

   189

 * Reading state functions.

   190

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

   191

   192

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

   193

 */

   194

void ec_fsm_pdo_entry_read_state_start(

   195

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

   196

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

   197

        )

   198

{

   199

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

   200

    ecrt_sdo_request_read(&fsm->request);

   201

   202

    fsm->state = ec_fsm_pdo_entry_read_state_count;

   203

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

   204

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

   205

}

   206

   207

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

   208

   209

/** Read number of mapped PDO entries.

   210

 */

   211

void ec_fsm_pdo_entry_read_state_count(

   212

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

   213

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

   214

        )

   215

{

   216

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

   217

        return;

   218

    }

   219

   220

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

   221

        EC_SLAVE_ERR(fsm->slave,

   222

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

   223

        fsm->state = ec_fsm_pdo_entry_state_error;

   224

        return;

   225

    }

   226

   227

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

   228

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

   229

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

   230

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

   231

                fsm->request.subindex);

   232

        fsm->state = ec_fsm_pdo_entry_state_error;

   233

        return;

   234

    }

   235

   236

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

   237

   238

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

   239

   240

    // read first PDO entry

   241

    fsm->entry_pos = 1;

   242

    ec_fsm_pdo_entry_read_action_next(fsm, datagram);

   243

}

   244

   245

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

   246

   247

/** Read next PDO entry.

   248

 */

   249

void ec_fsm_pdo_entry_read_action_next(

   250

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

   251

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

   252

        )

   253

{

   254

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

   255

        ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index,

   256

                fsm->entry_pos);

   257

        ecrt_sdo_request_read(&fsm->request);

   258

        fsm->state = ec_fsm_pdo_entry_read_state_entry;

   259

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

   260

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

   261

        return;

   262

    }

   263

   264

    // finished reading entries.

   265

    fsm->state = ec_fsm_pdo_entry_state_end;

   266

}

   267

   268

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

   269

   270

/** Read PDO entry information.

   271

 */

   272

void ec_fsm_pdo_entry_read_state_entry(

   273

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

   274

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

   275

        )

   276

{

   277

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

   278

        return;

   279

    }

   280

   281

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

   282

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

   283

        fsm->state = ec_fsm_pdo_entry_state_error;

   284

        return;

   285

    }

   286

   287

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

   288

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

   289

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

   290

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

   291

                fsm->request.subindex);

   292

        fsm->state = ec_fsm_pdo_entry_state_error;

   293

    } else {

   294

        uint32_t pdo_entry_info;

   295

        ec_pdo_entry_t *pdo_entry;

   296

   297

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

   298

   299

        if (!(pdo_entry = (ec_pdo_entry_t *)

   300

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

   301

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

   302

            fsm->state = ec_fsm_pdo_entry_state_error;

   303

            return;

   304

        }

   305

   306

        ec_pdo_entry_init(pdo_entry);

   307

        pdo_entry->index = pdo_entry_info >> 16;

   308

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

   309

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   310

   311

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

   312

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

   313

                ec_pdo_entry_clear(pdo_entry);

   314

                kfree(pdo_entry);

   315

                fsm->state = ec_fsm_pdo_entry_state_error;

   316

                return;

   317

            }

   318

        }

   319

   320

        EC_SLAVE_DBG(fsm->slave, 1,

   321

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

   322

                pdo_entry->index, pdo_entry->subindex,

   323

                pdo_entry->bit_length,

   324

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

   325

   326

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

   327

   328

        // next PDO entry

   329

        fsm->entry_pos++;

   330

        ec_fsm_pdo_entry_read_action_next(fsm, datagram);

   331

    }

   332

}

   333

   334

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

   335

 * Configuration state functions.

   336

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

   337

   338

/** Start PDO mapping.

   339

 */

   340

void ec_fsm_pdo_entry_conf_state_start(

   341

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

   342

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

   343

        )

   344

{

   345

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

   346

        fsm->state = ec_fsm_pdo_entry_state_error;

   347

        return;

   348

    }

   349

   350

    // set mapped PDO entry count to zero

   351

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

   352

    fsm->request.data_size = 1;

   353

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

   354

    ecrt_sdo_request_write(&fsm->request);

   355

   356

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

   357

   358

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   359

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

   360

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

   361

}

   362

   363

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

   364

   365

/** Process next PDO entry.

   366

 *

   367

 * \return Next PDO entry, or NULL.

   368

 */

   369

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   370

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

   371

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

   372

        )

   373

{

   374

    list = list->next;

   375

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

   376

        return NULL; // no next entry

   377

    return list_entry(list, ec_pdo_entry_t, list);

   378

}

   379

   380

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

   381

   382

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

   383

 */

   384

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   385

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

   386

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

   387

        )

   388

{

   389

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

   390

        return;

   391

    }

   392

   393

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

   394

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

   395

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

   396

        fsm->state = ec_fsm_pdo_entry_state_error;

   397

        return;

   398

    }

   399

   400

    // find first entry

   401

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   402

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

   403

   404

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

   405

   406

        fsm->state = ec_fsm_pdo_entry_state_end; // finished

   407

        return;

   408

    }

   409

   410

    // add first entry

   411

    fsm->entry_pos = 1;

   412

    ec_fsm_pdo_entry_conf_action_map(fsm, datagram);

   413

}

   414

   415

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

   416

   417

/** Starts to add a PDO entry.

   418

 */

   419

void ec_fsm_pdo_entry_conf_action_map(

   420

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

   421

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

   422

        )

   423

{

   424

    uint32_t value;

   425

   426

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

   427

            " at position %u.\n",

   428

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

   429

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

   430

   431

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

   432

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

   433

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

   434

    fsm->request.data_size = 4;

   435

    ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index,

   436

            fsm->entry_pos);

   437

    ecrt_sdo_request_write(&fsm->request);

   438

   439

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;

   440

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

   441

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

   442

}

   443

   444

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

   445

   446

/** Add a PDO entry.

   447

 */

   448

void ec_fsm_pdo_entry_conf_state_map_entry(

   449

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

   450

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

   451

        )

   452

{

   453

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

   454

        return;

   455

    }

   456

   457

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

   458

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

   459

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

   460

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

   461

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

   462

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

   463

        fsm->state = ec_fsm_pdo_entry_state_error;

   464

        return;

   465

    }

   466

   467

    // find next entry

   468

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   469

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

   470

   471

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

   472

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

   473

        fsm->request.data_size = 1;

   474

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

   475

        ecrt_sdo_request_write(&fsm->request);

   476

   477

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

   478

                fsm->entry_pos);

   479

   480

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   481

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

   482

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

   483

        return;

   484

    }

   485

   486

    // add next entry

   487

    fsm->entry_pos++;

   488

    ec_fsm_pdo_entry_conf_action_map(fsm, datagram);

   489

}

   490

   491

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

   492

   493

/** Set the number of entries.

   494

 */

   495

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   496

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

   497

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

   498

        )

   499

{

   500

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

   501

        return;

   502

    }

   503

   504

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

   505

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

   506

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

   507

        fsm->state = ec_fsm_pdo_entry_state_error;