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_conf_action_map(ec_fsm_pdo_entry_t *);

    61

    62

void ec_fsm_pdo_entry_state_end(ec_fsm_pdo_entry_t *);

    63

void ec_fsm_pdo_entry_state_error(ec_fsm_pdo_entry_t *);

    64

    65

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

    66

    67

/** Constructor.

    68

 */

    69

void ec_fsm_pdo_entry_init(

    70

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

    71

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

    72

        )

    73

{

    74

    fsm->fsm_coe = fsm_coe;

    75

    ec_sdo_request_init(&fsm->request);

    76

}

    77

    78

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

    79

    80

/** Destructor.

    81

 */

    82

void ec_fsm_pdo_entry_clear(

    83

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

    84

        )

    85

{

    86

    ec_sdo_request_clear(&fsm->request);

    87

}

    88

    89

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

    90

    91

/** Start reading a Pdo's entries.

    92

 */

    93

void ec_fsm_pdo_entry_start_reading(

    94

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

    95

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

    96

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

    97

        )

    98

{

    99

    fsm->slave = slave;

   100

    fsm->target_pdo = pdo;

   101

   102

    ec_pdo_clear_entries(fsm->target_pdo);

   103

   104

    fsm->state = ec_fsm_pdo_entry_read_state_start;

   105

}

   106

   107

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

   108

   109

/** Start Pdo mapping state machine.

   110

 */

   111

void ec_fsm_pdo_entry_start_configuration(

   112

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

   113

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

   114

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

   115

        )

   116

{

   117

    fsm->slave = slave;

   118

    fsm->source_pdo = pdo;

   119

   120

    fsm->state = ec_fsm_pdo_entry_conf_state_start;

   121

}

   122

   123

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

   124

   125

/** Get running state.

   126

 *

   127

 * \return false, if state machine has terminated

   128

 */

   129

int ec_fsm_pdo_entry_running(

   130

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

   131

        )

   132

{

   133

    return fsm->state != ec_fsm_pdo_entry_state_end

   134

        && fsm->state != ec_fsm_pdo_entry_state_error;

   135

}

   136

   137

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

   138

   139

/** Executes the current state.

   140

 *

   141

 * \return false, if state machine has terminated

   142

 */

   143

int ec_fsm_pdo_entry_exec(

   144

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

   145

        )

   146

{

   147

    fsm->state(fsm);

   148

    return ec_fsm_pdo_entry_running(fsm);

   149

}

   150

   151

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

   152

   153

/** Get execution result.

   154

 *

   155

 * \return true, if the state machine terminated gracefully

   156

 */

   157

int ec_fsm_pdo_entry_success(

   158

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

   159

        )

   160

{

   161

    return fsm->state == ec_fsm_pdo_entry_state_end;

   162

}

   163

   164

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

   165

 * Reading state functions.

   166

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

   167

   168

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

   169

 */

   170

void ec_fsm_pdo_entry_read_state_start(

   171

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

   172

        )

   173

{

   174

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

   175

    ecrt_sdo_request_read(&fsm->request);

   176

   177

    fsm->state = ec_fsm_pdo_entry_read_state_count;

   178

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

   179

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

   180

}

   181

   182

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

   183

   184

/** Read number of mapped Pdo entries.

   185

 */

   186

void ec_fsm_pdo_entry_read_state_count(

   187

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

   188

        )

   189

{

   190

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   191

        return;

   192

   193

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

   194

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

   195

        fsm->state = ec_fsm_pdo_entry_state_error;

   196

        return;

   197

    }

   198

   199

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

   200

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

   201

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

   202

                fsm->request.subindex);

   203

        fsm->state = ec_fsm_pdo_entry_state_error;

   204

        return;

   205

    }

   206

   207

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

   208

   209

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

   210

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

   211

   212

    // read first Pdo entry

   213

    fsm->entry_pos = 1;

   214

    ec_fsm_pdo_entry_read_action_next(fsm);

   215

}

   216

   217

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

   218

   219

/** Read next Pdo entry.

   220

 */

   221

void ec_fsm_pdo_entry_read_action_next(

   222

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

   223

        )

   224

{

   225

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

   226

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

   227

        ecrt_sdo_request_read(&fsm->request);

   228

        fsm->state = ec_fsm_pdo_entry_read_state_entry;

   229

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

   230

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

   231

        return;

   232

    }

   233

   234

    // finished reading entries.

   235

    fsm->state = ec_fsm_pdo_entry_state_end;

   236

}

   237

   238

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

   239

   240

/** Read Pdo entry information.

   241

 */

   242

void ec_fsm_pdo_entry_read_state_entry(

   243

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

   244

        )

   245

{

   246

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

   247

   248

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

   249

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

   250

        fsm->state = ec_fsm_pdo_entry_state_error;

   251

        return;

   252

    }

   253

   254

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

   255

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

   256

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

   257

                fsm->request.subindex);

   258

        fsm->state = ec_fsm_pdo_entry_state_error;

   259

    } else {

   260

        uint32_t pdo_entry_info;

   261

        ec_pdo_entry_t *pdo_entry;

   262

   263

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

   264

   265

        if (!(pdo_entry = (ec_pdo_entry_t *)

   266

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

   267

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

   268

            fsm->state = ec_fsm_pdo_entry_state_error;

   269

            return;

   270

        }

   271

   272

        ec_pdo_entry_init(pdo_entry);

   273

        pdo_entry->index = pdo_entry_info >> 16;

   274

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

   275

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   276

   277

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

   278

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

   279

                ec_pdo_entry_clear(pdo_entry);

   280

                kfree(pdo_entry);

   281

                fsm->state = ec_fsm_pdo_entry_state_error;

   282

                return;

   283

            }

   284

        }

   285

   286

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

   287

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

   288

                    pdo_entry->index, pdo_entry->subindex,

   289

                    pdo_entry->bit_length,

   290

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

   291

        }

   292

   293

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

   294

   295

        // next Pdo entry

   296

        fsm->entry_pos++;

   297

        ec_fsm_pdo_entry_read_action_next(fsm);

   298

    }

   299

}

   300

   301

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

   302

 * Configuration state functions.

   303

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

   304

   305

/** Start Pdo mapping.

   306

 */

   307

void ec_fsm_pdo_entry_conf_state_start(

   308

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

   309

        )

   310

{

   311

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

   312

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

   313

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

   314

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

   315

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

   316

                fsm->slave->ring_position);

   317

        fsm->state = ec_fsm_pdo_entry_state_error;

   318

        return;

   319

    }

   320

   321

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

   322

        fsm->state = ec_fsm_pdo_entry_state_error;

   323

        return;

   324

    }

   325

   326

    // set mapped Pdo entry count to zero

   327

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

   328

    fsm->request.data_size = 1;

   329

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

   330

    ecrt_sdo_request_write(&fsm->request);

   331

   332

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

   333

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

   334

   335

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;

   336

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

   337

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

   338

}

   339

   340

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

   341

   342

/** Process next Pdo entry.

   343

 */

   344

ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry(

   345

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

   346

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

   347

        )

   348

{

   349

    list = list->next; 

   350

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

   351

        return NULL; // no next entry

   352

    return list_entry(list, ec_pdo_entry_t, list);

   353

}

   354

   355

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

   356

   357

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

   358

 */

   359

void ec_fsm_pdo_entry_conf_state_zero_entry_count(

   360

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

   361

        )

   362

{

   363

    if (ec_fsm_coe_exec(fsm->fsm_coe))

   364

        return;

   365

   366

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

   367

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

   368

        fsm->state = ec_fsm_pdo_entry_state_error;

   369

        return;

   370

    }

   371

   372

    // find first entry

   373

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   374

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

   375

   376

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

   377

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

   378

   379

        fsm->state = ec_fsm_pdo_entry_state_end; // finished

   380

        return;

   381

    }

   382

   383

    // add first entry

   384

    fsm->entry_pos = 1;

   385

    ec_fsm_pdo_entry_conf_action_map(fsm);

   386

}

   387

   388

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

   389

   390

/** Starts to add a Pdo entry.

   391

 */

   392

void ec_fsm_pdo_entry_conf_action_map(

   393

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

   394

        )

   395

{

   396

    uint32_t value;

   397

   398

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

   399

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

   400

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

   401

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

   402

   403

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

   404

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

   405

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

   406

    fsm->request.data_size = 4;

   407

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

   408

    ecrt_sdo_request_write(&fsm->request);

   409

   410

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;

   411

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

   412

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

   413

}

   414

   415

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

   416

   417

/** Add a Pdo entry.

   418

 */

   419

void ec_fsm_pdo_entry_conf_state_map_entry(

   420

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

   421

        )

   422

{

   423

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

   424

   425

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

   426

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

   427

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

   428

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

   429

        fsm->state = ec_fsm_pdo_entry_state_error;

   430

        return;

   431

    }

   432

   433

    // find next entry

   434

    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(

   435

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

   436

   437

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

   438

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

   439

        fsm->request.data_size = 1;

   440

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

   441

        ecrt_sdo_request_write(&fsm->request);

   442

   443

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

   444

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

   445

   446

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;

   447

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

   448

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

   449

        return;

   450

    }

   451

   452

    // add next entry

   453

    fsm->entry_pos++;

   454

    ec_fsm_pdo_entry_conf_action_map(fsm);

   455

}

   456

   457

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

   458

   459

/** Set the number of entries.

   460

 */

   461

void ec_fsm_pdo_entry_conf_state_set_entry_count(

   462

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

   463

        )

   464

{

   465

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

   466

   467

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

   468

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

   469

        fsm->state = ec_fsm_pdo_entry_state_error;

   470

        return;

   471

    }

   472

   473

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

   474

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

   475

                fsm->source_pdo->index);

   476

   477

    fsm->state = ec_fsm_pdo_entry_state_end; // finished

   478

}

   479

   480

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

   481

 * Common state functions

   482

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

   483

   484

/** State: ERROR.

   485

 */

   486

void ec_fsm_pdo_entry_state_error(

   487

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

   488

        )

   489

{

   490

}

   491

   492

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

   493

   494

/** State: END.

   495

 */

   496

void ec_fsm_pdo_entry_state_end(

   497

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

   498

        )

   499

{

   500

}

   501

   502

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