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

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/** \file

    28

 * EtherCAT PDO mapping state machine.

    29

 */

    30

    31

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

    32

    33

#include "globals.h"

    34

#include "master.h"

    35

#include "mailbox.h"

    36

#include "slave_config.h"

    37

    38

#include "fsm_pdo_entry.h"

    39

    40

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

    41

    42

void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);

    43

void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);

    44

void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);

    45

    46

void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);

    47

    48

void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);

    49

void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);

    50

void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);

    51

void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);

    52

    53

void ec_fsm_pdo_entry_conf_action_map(ec_fsm_pdo_entry_t *);

    54

    55

void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);

    56

void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);

    57

    58

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

    59

    60

/** Constructor.

    61

 */

    62

void ec_fsm_pdo_entry_init(

    63

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

    64

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

    65

        )

    66

{

    67

    fsm->fsm_coe = fsm_coe;

    68

    ec_sdo_request_init(&fsm->request);

    69

}

    70

    71

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

    72

    73

/** Destructor.

    74

 */

    75

void ec_fsm_pdo_entry_clear(

    76

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

    77

        )

    78

{

    79

    ec_sdo_request_clear(&fsm->request);

    80

}

    81

    82

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

    83

    84

/** Start reading a PDO's entries.

    85

 */

    86

void ec_fsm_pdo_entry_start_reading(

    87

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

    88

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

    89

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

    90

        )

    91

{

    92

    fsm->slave = slave;

    93

    fsm->target_pdo = pdo;

    94

    95

    ec_pdo_clear_entries(fsm->target_pdo);

    96

    97

    fsm->state = ec_fsm_pdo_entry_read_state_start;

    98

}

    99

   100

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

   101

   102

/** Start PDO mapping state machine.

   103

 */

   104

void ec_fsm_pdo_entry_start_configuration(

   105

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

   106

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

   107

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

   108

        )

   109

{

   110

    fsm->slave = slave;

   111

    fsm->source_pdo = pdo;

   112

   113

    fsm->state = ec_fsm_pdo_entry_conf_state_start;

   114

}

   115

   116

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

   117

   118

/** Get running state.

   119

 *

   120

 * \return false, if state machine has terminated

   121

 */

   122

int ec_fsm_pdo_entry_running(

   123

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

   124

        )

   125

{

   126

    return fsm->state != ec_fsm_pdo_entry_state_end

   127

        && fsm->state != ec_fsm_pdo_entry_state_error;

   128

}

   129

   130

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

   131

   132

/** Executes the current state.

   133

 *

   134

 * \return false, if state machine has terminated

   135

 */

   136

int ec_fsm_pdo_entry_exec(

   137

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

   138

        )

   139

{

   140

    fsm->state(fsm);

   141

    return ec_fsm_pdo_entry_running(fsm);

   142

}

   143

   144

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

   145

   146

/** Get execution result.

   147

 *

   148

 * \return true, if the state machine terminated gracefully

   149

 */

   150

int ec_fsm_pdo_entry_success(

   151

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

   152

        )

   153

{

   154

    return fsm->state == ec_fsm_pdo_entry_state_end;

   155

}

   156

   157

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

   158

 * Reading state functions.

   159

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

   160

   161

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

   162

 */

   163

void ec_fsm_pdo_entry_read_state_start(

   164

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

   165

        )

   166

{

   167

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

   168

    ecrt_sdo_request_read(&fsm->request);

   169

   170

    fsm->state = ec_fsm_pdo_entry_read_state_count;

   171

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

   172

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

   173

}

   174

   175

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

   176

   177

/** Read number of mapped PDO entries.

   178

 */

   179

void ec_fsm_pdo_entry_read_state_count(

   180

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

   181

        )

   182

{

   183

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   184

        return;

   185

   186

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

   187

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

   188

        fsm->state = ec_fsm_pdo_entry_state_error;

   189

        return;

   190

    }

   191

   192

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

   193

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

   194

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

   195

                fsm->request.subindex);

   196

        fsm->state = ec_fsm_pdo_entry_state_error;

   197

        return;

   198

    }

   199

   200

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

   201

   202

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

   203

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

   204

   205

    // read first PDO entry

   206

    fsm->entry_pos = 1;

   207

    ec_fsm_pdo_entry_read_action_next(fsm);

   208

}

   209

   210

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

   211

   212

/** Read next PDO entry.

   213

 */

   214

void ec_fsm_pdo_entry_read_action_next(

   215

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

   216

        )

   217

{

   218

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

   219

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

   220

        ecrt_sdo_request_read(&fsm->request);

   221

        fsm->state = ec_fsm_pdo_entry_read_state_entry;

   222

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

   223

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

   224

        return;

   225

    }

   226

   227

    // finished reading entries.

   228

    fsm->state = ec_fsm_pdo_entry_state_end;

   229

}

   230

   231

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

   232

   233

/** Read PDO entry information.

   234

 */

   235

void ec_fsm_pdo_entry_read_state_entry(

   236

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

   237

        )

   238

{

   239

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

   240

   241

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

   242

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

   243

        fsm->state = ec_fsm_pdo_entry_state_error;

   244

        return;

   245

    }

   246

   247

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

   248

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

   249

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

   250

                fsm->request.subindex);

   251

        fsm->state = ec_fsm_pdo_entry_state_error;

   252

    } else {

   253

        uint32_t pdo_entry_info;

   254

        ec_pdo_entry_t *pdo_entry;

   255

   256

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

   257

   258

        if (!(pdo_entry = (ec_pdo_entry_t *)

   259

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

   260

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

   261

            fsm->state = ec_fsm_pdo_entry_state_error;

   262

            return;

   263

        }

   264

   265

        ec_pdo_entry_init(pdo_entry);

   266

        pdo_entry->index = pdo_entry_info >> 16;

   267

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

   268

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   269

   270

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

   271

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

   272

                ec_pdo_entry_clear(pdo_entry);

   273

                kfree(pdo_entry);

   274

                fsm->state = ec_fsm_pdo_entry_state_error;

   275

                return;

   276

            }

   277

        }

   278

   279

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

   280

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

   281

                    pdo_entry->index, pdo_entry->subindex,

   282

                    pdo_entry->bit_length,

   283

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

   284

        }

   285

   286

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

   287

   288

        // next PDO entry

   289

        fsm->entry_pos++;

   290

        ec_fsm_pdo_entry_read_action_next(fsm);

   291

    }

   292

}

   293

   294

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

   295

 * Configuration state functions.

   296

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

   297

   298

/** Start PDO mapping.

   299

 */

   300

void ec_fsm_pdo_entry_conf_state_start(

   301

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

   302

        )

   303

{

   304

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

   305

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

   306

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

   307

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

   308

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

   309

                fsm->slave->ring_position);

   310

        fsm->state = ec_fsm_pdo_entry_state_error;

   311

        return;

   312

    }

   313

   314

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

   315

        fsm->state = ec_fsm_pdo_entry_state_error;

   316

        return;

   317

    }

   318

   319

    // set mapped PDO entry count to zero

   320

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

   321

    fsm->request.data_size = 1;

   322

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

   323

    ecrt_sdo_request_write(&fsm->request);

   324

   325

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

   326

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

   327

   328

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   329

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

   330

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

   331

}

   332

   333

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

   334

   335

/** Process next PDO entry.

   336

 */

   337

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   338

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

   339

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

   340

        )

   341

{

   342

    list = list->next; 

   343

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

   344

        return NULL; // no next entry

   345

    return list_entry(list, ec_pdo_entry_t, list);

   346

}

   347

   348

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

   349

   350

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

   351

 */

   352

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   353

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

   354

        )

   355

{

   356

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   357

        return;

   358

   359

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

   360

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

   361

        fsm->state = ec_fsm_pdo_entry_state_error;

   362

        return;

   363

    }

   364

   365

    // find first entry

   366

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   367

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

   368

   369

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

   370

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

   371

   372

        fsm->state = ec_fsm_pdo_entry_state_end; // finished

   373

        return;

   374

    }

   375

   376

    // add first entry

   377

    fsm->entry_pos = 1;

   378

    ec_fsm_pdo_entry_conf_action_map(fsm);

   379

}

   380

   381

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

   382

   383

/** Starts to add a PDO entry.

   384

 */

   385

void ec_fsm_pdo_entry_conf_action_map(

   386

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

   387

        )

   388

{

   389

    uint32_t value;

   390

   391

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

   392

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

   393

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

   394

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

   395

   396

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

   397

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

   398

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

   399

    fsm->request.data_size = 4;

   400

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

   401

    ecrt_sdo_request_write(&fsm->request);

   402

   403

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;

   404

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

   405

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

   406

}

   407

   408

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

   409

   410

/** Add a PDO entry.

   411

 */

   412

void ec_fsm_pdo_entry_conf_state_map_entry(

   413

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

   414

        )

   415

{

   416

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

   417

   418

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

   419

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

   420

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

   421

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

   422

        fsm->state = ec_fsm_pdo_entry_state_error;

   423

        return;

   424

    }

   425

   426

    // find next entry

   427

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   428

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

   429

   430

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

   431

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

   432

        fsm->request.data_size = 1;

   433

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

   434

        ecrt_sdo_request_write(&fsm->request);

   435

   436

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

   437

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

   438

   439

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   440

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

   441

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

   442

        return;

   443

    }

   444

   445

    // add next entry

   446

    fsm->entry_pos++;

   447

    ec_fsm_pdo_entry_conf_action_map(fsm);

   448

}

   449

   450

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

   451

   452

/** Set the number of entries.

   453

 */

   454

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   455

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

   456

        )

   457

{

   458

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

   459

   460

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

   461

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

   462

        fsm->state = ec_fsm_pdo_entry_state_error;

   463

        return;

   464

    }

   465

   466

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

   467

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

   468

                fsm->source_pdo->index);

   469

   470

    fsm->state = ec_fsm_pdo_entry_state_end; // finished

   471

}

   472

   473

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

   474

 * Common state functions

   475

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

   476

   477

/** State: ERROR.

   478

 */

   479

void ec_fsm_pdo_entry_state_error(

   480

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

   481

        )

   482

{

   483

}

   484

   485

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

   486

   487

/** State: END.

   488

 */

   489

void ec_fsm_pdo_entry_state_end(

   490

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

   491

        )

   492

{

   493

}

   494

   495

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