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

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

   332

        fsm->state = ec_fsm_pdo_entry_state_error;

   333

        return;

   334

    }

   335

   336

    // set mapped PDO entry count to zero

   337

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

   338

    fsm->request.data_size = 1;

   339

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

   340

    ecrt_sdo_request_write(&fsm->request);

   341

   342

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

   343

   344

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   345

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

   346

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

   347

}

   348

   349

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

   350

   351

/** Process next PDO entry.

   352

 */

   353

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   354

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

   355

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

   356

        )

   357

{

   358

    list = list->next; 

   359

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

   360

        return NULL; // no next entry

   361

    return list_entry(list, ec_pdo_entry_t, list);

   362

}

   363

   364

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

   365

   366

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

   367

 */

   368

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   369

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

   370

        )

   371

{

   372

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   373

        return;

   374

   375

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

   376

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

   377

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

   378

        fsm->state = ec_fsm_pdo_entry_state_error;

   379

        return;

   380

    }

   381

   382

    // find first entry

   383

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   384

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

   385

   386

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

   387

   388

        fsm->state = ec_fsm_pdo_entry_state_end; // finished

   389

        return;

   390

    }

   391

   392

    // add first entry

   393

    fsm->entry_pos = 1;

   394

    ec_fsm_pdo_entry_conf_action_map(fsm);

   395

}

   396

   397

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

   398

   399

/** Starts to add a PDO entry.

   400

 */

   401

void ec_fsm_pdo_entry_conf_action_map(

   402

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

   403

        )

   404

{

   405

    uint32_t value;

   406

   407

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

   408

            " at position %u.\n",

   409

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

   410

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

   411

   412

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

   413

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

   414

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

   415

    fsm->request.data_size = 4;

   416

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

   417

    ecrt_sdo_request_write(&fsm->request);

   418

   419

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;

   420

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

   421

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

   422

}

   423

   424

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

   425

   426

/** Add a PDO entry.

   427

 */

   428

void ec_fsm_pdo_entry_conf_state_map_entry(

   429

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

   430

        )

   431

{

   432

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

   433

   434

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

   435

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

   436

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

   437

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

   438

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

   439

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

   440

        fsm->state = ec_fsm_pdo_entry_state_error;

   441

        return;

   442

    }

   443

   444

    // find next entry

   445

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   446

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

   447

   448

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

   449

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

   450

        fsm->request.data_size = 1;

   451

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

   452

        ecrt_sdo_request_write(&fsm->request);

   453

   454

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

   455

                fsm->entry_pos);

   456

   457

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   458

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

   459

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

   460

        return;

   461

    }

   462

   463

    // add next entry

   464

    fsm->entry_pos++;

   465

    ec_fsm_pdo_entry_conf_action_map(fsm);

   466

}

   467

   468

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

   469

   470

/** Set the number of entries.

   471

 */

   472

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   473

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

   474

        )

   475

{

   476

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

   477

   478

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

   479

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

   480

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

   481

        fsm->state = ec_fsm_pdo_entry_state_error;

   482

        return;

   483

    }

   484

   485

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

   486

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

   487

   488

    fsm->state = ec_fsm_pdo_entry_state_end; // finished

   489

}

   490

   491

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

   492

 * Common state functions

   493

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

   494

   495

/** State: ERROR.

   496

 */

   497

void ec_fsm_pdo_entry_state_error(

   498

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

   499

        )

   500

{

   501

}

   502

   503

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

   504

   505

/** State: END.

   506

 */

   507

void ec_fsm_pdo_entry_state_end(

   508

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

   509

        )

   510

{

   511

}

   512

   513

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