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

/** Start reading a PDO's entries.

    88

 */

    89

void ec_fsm_pdo_entry_start_reading(

    90

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

    91

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

    92

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

    93

        )

    94

{

    95

    fsm->slave = slave;

    96

    fsm->target_pdo = pdo;

    97

    98

    ec_pdo_clear_entries(fsm->target_pdo);

    99

   100

    fsm->state = ec_fsm_pdo_entry_read_state_start;

   101

}

   102

   103

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

   104

   105

/** Start PDO mapping state machine.

   106

 */

   107

void ec_fsm_pdo_entry_start_configuration(

   108

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

   109

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

   110

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

   111

        )

   112

{

   113

    fsm->slave = slave;

   114

    fsm->source_pdo = pdo;

   115

   116

    fsm->state = ec_fsm_pdo_entry_conf_state_start;

   117

}

   118

   119

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

   120

   121

/** Get running state.

   122

 *

   123

 * \return false, if state machine has terminated

   124

 */

   125

int ec_fsm_pdo_entry_running(

   126

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

   127

        )

   128

{

   129

    return fsm->state != ec_fsm_pdo_entry_state_end

   130

        && fsm->state != ec_fsm_pdo_entry_state_error;

   131

}

   132

   133

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

   134

   135

/** Executes the current state.

   136

 *

   137

 * \return false, if state machine has terminated

   138

 */

   139

int ec_fsm_pdo_entry_exec(

   140

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

   141

        )

   142

{

   143

    fsm->state(fsm);

   144

    return ec_fsm_pdo_entry_running(fsm);

   145

}

   146

   147

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

   148

   149

/** Get execution result.

   150

 *

   151

 * \return true, if the state machine terminated gracefully

   152

 */

   153

int ec_fsm_pdo_entry_success(

   154

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

   155

        )

   156

{

   157

    return fsm->state == ec_fsm_pdo_entry_state_end;

   158

}

   159

   160

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

   161

 * Reading state functions.

   162

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

   163

   164

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

   165

 */

   166

void ec_fsm_pdo_entry_read_state_start(

   167

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

   168

        )

   169

{

   170

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

   171

    ecrt_sdo_request_read(&fsm->request);

   172

   173

    fsm->state = ec_fsm_pdo_entry_read_state_count;

   174

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

   175

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

   176

}

   177

   178

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

   179

   180

/** Read number of mapped PDO entries.

   181

 */

   182

void ec_fsm_pdo_entry_read_state_count(

   183

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

   184

        )

   185

{

   186

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   187

        return;

   188

   189

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

   190

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

   191

        fsm->state = ec_fsm_pdo_entry_state_error;

   192

        return;

   193

    }

   194

   195

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

   196

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

   197

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

   198

                fsm->request.subindex);

   199

        fsm->state = ec_fsm_pdo_entry_state_error;

   200

        return;

   201

    }

   202

   203

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

   204

   205

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

   206

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

   207

   208

    // read first PDO entry

   209

    fsm->entry_pos = 1;

   210

    ec_fsm_pdo_entry_read_action_next(fsm);

   211

}

   212

   213

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

   214

   215

/** Read next PDO entry.

   216

 */

   217

void ec_fsm_pdo_entry_read_action_next(

   218

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

   219

        )

   220

{

   221

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

   222

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

   223

        ecrt_sdo_request_read(&fsm->request);

   224

        fsm->state = ec_fsm_pdo_entry_read_state_entry;

   225

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

   226

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

   227

        return;

   228

    }

   229

   230

    // finished reading entries.

   231

    fsm->state = ec_fsm_pdo_entry_state_end;

   232

}

   233

   234

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

   235

   236

/** Read PDO entry information.

   237

 */

   238

void ec_fsm_pdo_entry_read_state_entry(

   239

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

   240

        )

   241

{

   242

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

   243

   244

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

   245

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

   246

        fsm->state = ec_fsm_pdo_entry_state_error;

   247

        return;

   248

    }

   249

   250

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

   251

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

   252

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

   253

                fsm->request.subindex);

   254

        fsm->state = ec_fsm_pdo_entry_state_error;

   255

    } else {

   256

        uint32_t pdo_entry_info;

   257

        ec_pdo_entry_t *pdo_entry;

   258

   259

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

   260

   261

        if (!(pdo_entry = (ec_pdo_entry_t *)

   262

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

   263

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

   264

            fsm->state = ec_fsm_pdo_entry_state_error;

   265

            return;

   266

        }

   267

   268

        ec_pdo_entry_init(pdo_entry);

   269

        pdo_entry->index = pdo_entry_info >> 16;

   270

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

   271

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   272

   273

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

   274

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

   275

                ec_pdo_entry_clear(pdo_entry);

   276

                kfree(pdo_entry);

   277

                fsm->state = ec_fsm_pdo_entry_state_error;

   278

                return;

   279

            }

   280

        }

   281

   282

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

   283

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

   284

                    pdo_entry->index, pdo_entry->subindex,

   285

                    pdo_entry->bit_length,

   286

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

   287

        }

   288

   289

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

   290

   291

        // next PDO entry

   292

        fsm->entry_pos++;

   293

        ec_fsm_pdo_entry_read_action_next(fsm);

   294

    }

   295

}

   296

   297

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

   298

 * Configuration state functions.

   299

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

   300

   301

/** Start PDO mapping.

   302

 */

   303

void ec_fsm_pdo_entry_conf_state_start(

   304

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

   305

        )

   306

{

   307

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

   308

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

   309

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

   310

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

   311

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

   312

                fsm->slave->ring_position);

   313

        fsm->state = ec_fsm_pdo_entry_state_error;

   314

        return;

   315

    }

   316

   317

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

   318

        fsm->state = ec_fsm_pdo_entry_state_error;

   319

        return;

   320

    }

   321

   322

    // set mapped PDO entry count to zero

   323

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

   324

    fsm->request.data_size = 1;

   325

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

   326

    ecrt_sdo_request_write(&fsm->request);

   327

   328

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

   329

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

   330

   331

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   332

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

   333

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

   334

}

   335

   336

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

   337

   338

/** Process next PDO entry.

   339

 */

   340

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   341

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

   342

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

   343

        )

   344

{

   345

    list = list->next; 

   346

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

   347

        return NULL; // no next entry

   348

    return list_entry(list, ec_pdo_entry_t, list);

   349

}

   350

   351

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

   352

   353

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

   354

 */

   355

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   356

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

   357

        )

   358

{

   359

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   360

        return;

   361

   362

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

   363

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

   364

        fsm->state = ec_fsm_pdo_entry_state_error;

   365

        return;

   366

    }

   367

   368

    // find first entry

   369

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   370

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

   371

   372

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

   373

            EC_DBG("No entries to map.\n");

   374

   375

        fsm->state = ec_fsm_pdo_entry_state_end; // finished

   376

        return;

   377

    }

   378

   379

    // add first entry

   380

    fsm->entry_pos = 1;

   381

    ec_fsm_pdo_entry_conf_action_map(fsm);

   382

}

   383

   384

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

   385

   386

/** Starts to add a PDO entry.

   387

 */

   388

void ec_fsm_pdo_entry_conf_action_map(

   389

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

   390

        )

   391

{

   392

    uint32_t value;

   393

   394

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

   395

        EC_DBG("Mapping PDO entry 0x%04X:%02X (%u bit) at position %u.\n",

   396

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

   397

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

   398

   399

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

   400

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

   401

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

   402

    fsm->request.data_size = 4;

   403

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

   404

    ecrt_sdo_request_write(&fsm->request);

   405

   406

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;

   407

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

   408

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

   409

}

   410

   411

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

   412

   413

/** Add a PDO entry.

   414

 */

   415

void ec_fsm_pdo_entry_conf_state_map_entry(

   416

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

   417

        )

   418

{

   419

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

   420

   421

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

   422

        EC_WARN("Failed to map PDO entry 0x%04X:%02X (%u bit) to "

   423

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

   424

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

   425

        fsm->state = ec_fsm_pdo_entry_state_error;

   426

        return;

   427

    }

   428

   429

    // find next entry

   430

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   431

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

   432

   433

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

   434

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

   435

        fsm->request.data_size = 1;

   436

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

   437

        ecrt_sdo_request_write(&fsm->request);

   438

   439

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

   440

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

   441

   442

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   443

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

   444

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

   445

        return;

   446

    }

   447

   448

    // add next entry

   449

    fsm->entry_pos++;

   450

    ec_fsm_pdo_entry_conf_action_map(fsm);

   451

}

   452

   453

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

   454

   455

/** Set the number of entries.

   456

 */

   457

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   458

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

   459

        )

   460

{

   461

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

   462

   463

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

   464

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

   465

        fsm->state = ec_fsm_pdo_entry_state_error;

   466

        return;

   467

    }

   468

   469

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

   470

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

   471

                fsm->source_pdo->index);

   472

   473

    fsm->state = ec_fsm_pdo_entry_state_end; // finished

   474

}

   475

   476

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

   477

 * Common state functions

   478

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

   479

   480

/** State: ERROR.

   481

 */

   482

void ec_fsm_pdo_entry_state_error(

   483

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

   484

        )

   485

{

   486

}

   487

   488

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

   489

   490

/** State: END.

   491

 */

   492

void ec_fsm_pdo_entry_state_end(

   493

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

   494

        )

   495

{

   496

}

   497

   498

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