1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    15

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

 *  You should have received a copy of the GNU General Public License

    20

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/** \file

    35

 * EtherCAT Pdo mapping state machine.

    36

 */

    37

    38

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

    39

    40

#include "globals.h"

    41

#include "master.h"

    42

#include "mailbox.h"

    43

#include "slave_config.h"

    44

    45

#include "fsm_pdo_entry.h"

    46

    47

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

    48

    49

void ec_fsm_pdo_entry_read_state_start(ec_fsm_pdo_entry_t *);

    50

void ec_fsm_pdo_entry_read_state_count(ec_fsm_pdo_entry_t *);

    51

void ec_fsm_pdo_entry_read_state_entry(ec_fsm_pdo_entry_t *);

    52

    53

void ec_fsm_pdo_entry_read_action_next(ec_fsm_pdo_entry_t *);

    54

    55

void ec_fsm_pdo_entry_conf_state_start(ec_fsm_pdo_entry_t *);

    56

void ec_fsm_pdo_entry_conf_state_zero_entry_count(ec_fsm_pdo_entry_t *);

    57

void ec_fsm_pdo_entry_conf_state_map_entry(ec_fsm_pdo_entry_t *);

    58

void ec_fsm_pdo_entry_conf_state_set_entry_count(ec_fsm_pdo_entry_t *);

    59

    60

void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);

    61

void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);

    62

    63

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

    64

    65

/** Constructor.

    66

 */

    67

void ec_fsm_pdo_entry_init(

    68

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

    69

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

    70

        )

    71

{

    72

    fsm->fsm_coe = fsm_coe;

    73

    ec_sdo_request_init(&fsm->request);

    74

}

    75

    76

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

    77

    78

/** Destructor.

    79

 */

    80

void ec_fsm_pdo_entry_clear(

    81

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

    82

        )

    83

{

    84

    ec_sdo_request_clear(&fsm->request);

    85

}

    86

    87

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

    88

    89

/** Start reading a Pdo's entries.

    90

 */

    91

void ec_fsm_pdo_entry_start_reading(

    92

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

    93

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

    94

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

    95

        )

    96

{

    97

    fsm->slave = slave;

    98

    fsm->target_pdo = pdo;

    99

   100

    ec_pdo_clear_entries(fsm->target_pdo);

   101

   102

    fsm->state = ec_fsm_pdo_entry_read_state_start;

   103

}

   104

   105

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

   106

   107

/** Start Pdo mapping state machine.

   108

 */

   109

void ec_fsm_pdo_entry_start_configuration(

   110

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

   111

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

   112

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

   113

        )

   114

{

   115

    fsm->slave = slave;

   116

    fsm->source_pdo = pdo;

   117

   118

    fsm->state = ec_fsm_pdo_entry_conf_state_start;

   119

}

   120

   121

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

   122

   123

/** Get running state.

   124

 *

   125

 * \return false, if state machine has terminated

   126

 */

   127

int ec_fsm_pdo_entry_running(

   128

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

   129

        )

   130

{

   131

    return fsm->state != ec_fsm_pdo_entry_state_end

   132

        && fsm->state != ec_fsm_pdo_entry_state_error;

   133

}

   134

   135

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

   136

   137

/** Executes the current state.

   138

 *

   139

 * \return false, if state machine has terminated

   140

 */

   141

int ec_fsm_pdo_entry_exec(

   142

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

   143

        )

   144

{

   145

    fsm->state(fsm);

   146

    return ec_fsm_pdo_entry_running(fsm);

   147

}

   148

   149

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

   150

   151

/** Get execution result.

   152

 *

   153

 * \return true, if the state machine terminated gracefully

   154

 */

   155

int ec_fsm_pdo_entry_success(

   156

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

   157

        )

   158

{

   159

    return fsm->state == ec_fsm_pdo_entry_state_end;

   160

}

   161

   162

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

   163

 * Reading state functions.

   164

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

   165

   166

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

   167

 */

   168

void ec_fsm_pdo_entry_read_state_start(

   169

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

   170

        )

   171

{

   172

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

   173

    ecrt_sdo_request_read(&fsm->request);

   174

   175

    fsm->state = ec_fsm_pdo_entry_read_state_count;

   176

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

   177

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

   178

}

   179

   180

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

   181

   182

/** Read number of mapped Pdo entries.

   183

 */

   184

void ec_fsm_pdo_entry_read_state_count(

   185

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

   186

        )

   187

{

   188

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   189

        return;

   190

   191

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

   192

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

   193

        fsm->state = ec_fsm_pdo_entry_state_error;

   194

        return;

   195

    }

   196

   197

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

   198

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

   199

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

   200

                fsm->request.subindex);

   201

        fsm->state = ec_fsm_pdo_entry_state_error;

   202

        return;

   203

    }

   204

   205

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

   206

   207

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

   208

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

   209

   210

    // read first Pdo entry

   211

    fsm->entry_pos = 1;

   212

    ec_fsm_pdo_entry_read_action_next(fsm);

   213

}

   214

   215

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

   216

   217

/** Read next Pdo entry.

   218

 */

   219

void ec_fsm_pdo_entry_read_action_next(

   220

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

   221

        )

   222

{

   223

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

   224

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

   225

        ecrt_sdo_request_read(&fsm->request);

   226

        fsm->state = ec_fsm_pdo_entry_read_state_entry;

   227

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

   228

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

   229

        return;

   230

    }

   231

   232

    // finished reading entries.

   233

    fsm->state = ec_fsm_pdo_entry_state_end;

   234

}

   235

   236

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

   237

   238

/** Read Pdo entry information.

   239

 */

   240

void ec_fsm_pdo_entry_read_state_entry(

   241

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

   242

        )

   243

{

   244

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

   245

   246

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

   247

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

   248

        fsm->state = ec_fsm_pdo_entry_state_error;

   249

        return;

   250

    }

   251

   252

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

   253

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

   254

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

   255

                fsm->request.subindex);

   256

        fsm->state = ec_fsm_pdo_entry_state_error;

   257

    } else {

   258

        uint32_t pdo_entry_info;

   259

        ec_pdo_entry_t *pdo_entry;

   260

   261

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

   262

   263

        if (!(pdo_entry = (ec_pdo_entry_t *)

   264

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

   265

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

   266

            fsm->state = ec_fsm_pdo_entry_state_error;

   267

            return;

   268

        }

   269

   270

        ec_pdo_entry_init(pdo_entry);

   271

        pdo_entry->index = pdo_entry_info >> 16;

   272

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

   273

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   274

   275

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

   276

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

   277

                ec_pdo_entry_clear(pdo_entry);

   278

                kfree(pdo_entry);

   279

                fsm->state = ec_fsm_pdo_entry_state_error;

   280

                return;

   281

            }

   282

        }

   283

   284

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

   285

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

   286

                    pdo_entry->index, pdo_entry->subindex,

   287

                    pdo_entry->bit_length,

   288

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

   289

        }

   290

   291

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

   292

   293

        // next Pdo entry

   294

        fsm->entry_pos++;

   295

        ec_fsm_pdo_entry_read_action_next(fsm);

   296

    }

   297

}

   298

   299

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

   300

 * Configuration state functions.

   301

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

   302

   303

/** Start Pdo mapping.

   304

 */

   305

void ec_fsm_pdo_entry_conf_state_start(

   306

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

   307

        )

   308

{

   309

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

   310

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

   311

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

   312

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

   313

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

   314

                fsm->slave->ring_position);

   315

        fsm->state = ec_fsm_pdo_entry_state_error;

   316

        return;

   317

    }

   318

   319

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

   320

        fsm->state = ec_fsm_pdo_entry_state_error;

   321

        return;

   322

    }

   323

   324

    // set mapped Pdo entry count to zero

   325

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

   326

    fsm->request.data_size = 1;

   327

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

   328

    ecrt_sdo_request_write(&fsm->request);

   329

   330

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

   331

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

   332

   333

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   334

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

   335

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

   336

}

   337

   338

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

   339

   340

/** Process next Pdo entry.

   341

 */

   342

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   343

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

   344

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

   345

        )

   346

{

   347

    list = list->next; 

   348

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

   349

        return NULL; // no next entry

   350

    return list_entry(list, ec_pdo_entry_t, list);

   351

}

   352

   353

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

   354

   355

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

   356

 */

   357

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   358

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

   359

        )

   360

{

   361

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   362

        return;

   363

   364

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

   365

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

   366

        fsm->state = ec_fsm_pdo_entry_state_error;

   367

        return;

   368

    }

   369

   370

    // find first entry

   371

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   372

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

   373

   374

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

   375

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

   376

   377

        fsm->state = ec_fsm_pdo_entry_state_end; // finished

   378

        return;

   379

    }

   380

   381

    // add first entry

   382

    fsm->entry_pos = 1;

   383

    ec_fsm_pdo_entry_conf_action_map(fsm);

   384

}

   385

   386

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

   387

   388

/** Starts to add a Pdo entry.

   389

 */

   390

void ec_fsm_pdo_entry_conf_action_map(

   391

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

   392

        )

   393

{

   394

    uint32_t value;

   395

   396

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

   397

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

   398

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

   399

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

   400

   401

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

   402

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

   403

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

   404

    fsm->request.data_size = 4;

   405

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

   406

    ecrt_sdo_request_write(&fsm->request);

   407

   408

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;

   409

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

   410

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

   411

}

   412

   413

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

   414

   415

/** Add a Pdo entry.

   416

 */

   417

void ec_fsm_pdo_entry_conf_state_map_entry(

   418

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

   419

        )

   420

{

   421

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

   422

   423

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

   424

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

   425

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

   426

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

   427

        fsm->state = ec_fsm_pdo_entry_state_error;

   428

        return;

   429

    }

   430

   431

    // find next entry

   432

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   433

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

   434

   435

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

   436

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

   437

        fsm->request.data_size = 1;

   438

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

   439

        ecrt_sdo_request_write(&fsm->request);

   440

   441

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

   442

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

   443

   444

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   445

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

   446

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

   447

        return;

   448

    }

   449

   450

    // add next entry

   451

    fsm->entry_pos++;

   452

    ec_fsm_pdo_entry_conf_action_map(fsm);

   453

}

   454

   455

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

   456

   457

/** Set the number of entries.

   458

 */

   459

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   460

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

   461

        )

   462

{

   463

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

   464

   465

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

   466

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

   467

        fsm->state = ec_fsm_pdo_entry_state_error;

   468

        return;

   469

    }

   470

   471

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

   472

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

   473

                fsm->source_pdo->index);

   474

   475

    fsm->state = ec_fsm_pdo_entry_state_end; // finished

   476

}

   477

   478

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

   479

 * Common state functions

   480

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

   481

   482

/** State: ERROR.

   483

 */

   484

void ec_fsm_pdo_entry_state_error(

   485

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

   486

        )

   487

{

   488

}

   489

   490

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

   491

   492

/** State: END.

   493

 */

   494

void ec_fsm_pdo_entry_state_end(

   495

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

   496

        )

   497

{

   498

}

   499

   500

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