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

/**

    35

   \file

    36

   EtherCAT Pdo information state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "sdo_request.h"

    45

#include "fsm_coe_map.h"

    46

    47

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

    48

    49

void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);

    50

void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);

    51

void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);

    52

void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *);

    53

void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);

    54

    55

void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);

    56

void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);

    57

    58

void ec_fsm_coe_map_action_next_dir(ec_fsm_coe_map_t *);

    59

void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);

    60

void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);

    61

    62

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

    63

    64

/** Sdo upload timeout in ms for reading Pdo assigment/mapping.

    65

 */

    66

#define EC_FSM_COE_MAP_UPLOAD_TIMEOUT 3000

    67

    68

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

    69

    70

/**

    71

   Constructor.

    72

*/

    73

    74

void ec_fsm_coe_map_init(

    75

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

    76

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

    77

        )

    78

{

    79

    fsm->fsm_coe = fsm_coe;

    80

    fsm->state = NULL;

    81

    ec_sdo_request_init(&fsm->request);

    82

    fsm->request.response_timeout = EC_FSM_COE_MAP_UPLOAD_TIMEOUT;

    83

    ec_pdo_list_init(&fsm->pdos);

    84

}

    85

    86

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

    87

    88

/**

    89

   Destructor.

    90

*/

    91

    92

void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)

    93

{

    94

    ec_sdo_request_clear(&fsm->request);

    95

    ec_pdo_list_clear(&fsm->pdos);

    96

}

    97

    98

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

    99

   100

/**

   101

   Starts to upload an Sdo from a slave.

   102

*/

   103

   104

void ec_fsm_coe_map_start(

   105

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

   106

        ec_slave_t *slave /**< EtherCAT slave */

   107

        )

   108

{

   109

    fsm->slave = slave;

   110

    fsm->state = ec_fsm_coe_map_state_start;

   111

}

   112

   113

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

   114

   115

/**

   116

   Executes the current state of the state machine.

   117

   \return false, if state machine has terminated

   118

*/

   119

   120

int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   121

{

   122

    fsm->state(fsm);

   123

   124

    return fsm->state != ec_fsm_coe_map_state_end

   125

        && fsm->state != ec_fsm_coe_map_state_error;

   126

}

   127

   128

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

   129

   130

/**

   131

   Returns, if the state machine terminated with success.

   132

   \return non-zero if successful.

   133

*/

   134

   135

int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */)

   136

{

   137

    return fsm->state == ec_fsm_coe_map_state_end;

   138

}

   139

   140

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

   141

 *  state functions

   142

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

   143

   144

/**

   145

 * Start reading Pdo assignment.

   146

 */

   147

   148

void ec_fsm_coe_map_state_start(

   149

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   150

        )

   151

{

   152

    // read Pdo assignment for first direction

   153

    fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT

   154

    ec_fsm_coe_map_action_next_dir(fsm);

   155

}

   156

   157

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

   158

   159

/**

   160

 * Read Pdo assignment of next direction manager.

   161

 */

   162

   163

void ec_fsm_coe_map_action_next_dir(

   164

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   165

        )

   166

{

   167

    ec_slave_t *slave = fsm->slave;

   168

   169

    fsm->dir++;

   170

   171

    if (slave->master->debug_level)

   172

        EC_DBG("Processing dir %u of slave %u.\n",

   173

                fsm->dir, slave->ring_position);

   174

   175

    for (; fsm->dir <= EC_DIR_INPUT; fsm->dir++) {

   176

   177

        if (!(fsm->sync = ec_slave_get_pdo_sync(slave, fsm->dir))) {

   178

            if (slave->master->debug_level)

   179

                EC_DBG("No sync manager for direction %u!\n", fsm->dir);

   180

            continue;

   181

        }

   182

   183

        fsm->sync_sdo_index = 0x1C10 + fsm->sync->index;

   184

   185

        if (slave->master->debug_level)

   186

            EC_DBG("Reading Pdo assignment of sync manager %u of slave %u.\n",

   187

                    fsm->sync->index, slave->ring_position);

   188

   189

        ec_pdo_list_clear_pdos(&fsm->pdos);

   190

   191

        ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);

   192

        ecrt_sdo_request_read(&fsm->request);

   193

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   194

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

   195

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

   196

        return;

   197

    }

   198

   199

    if (slave->master->debug_level)

   200

        EC_DBG("Reading of Pdo assignment finished for slave %u.\n",

   201

                slave->ring_position);

   202

   203

    fsm->state = ec_fsm_coe_map_state_end;

   204

}

   205

   206

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

   207

   208

/**

   209

 * Count assigned Pdos.

   210

 */

   211

   212

void ec_fsm_coe_map_state_pdo_count(

   213

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   214

        )

   215

{

   216

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

   217

   218

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

   219

        EC_ERR("Failed to read number of assigned Pdos from slave %u.\n",

   220

                fsm->slave->ring_position);

   221

        fsm->state = ec_fsm_coe_map_state_error;

   222

        return;

   223

    }

   224

   225

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

   226

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   227

                "from slave %u.\n", fsm->request.data_size,

   228

                fsm->request.index, fsm->request.subindex,

   229

                fsm->slave->ring_position);

   230

        fsm->state = ec_fsm_coe_map_state_error;

   231

        return;

   232

    }

   233

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

   234

   235

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

   236

        EC_DBG("  %u Pdos assigned.\n", fsm->sync_subindices);

   237

   238

    // read first Pdo

   239

    fsm->sync_subindex = 1;

   240

    ec_fsm_coe_map_action_next_pdo(fsm);

   241

}

   242

   243

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

   244

   245

/**

   246

 * Read next Pdo.

   247

 */

   248

   249

void ec_fsm_coe_map_action_next_pdo(

   250

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   251

        )

   252

{

   253

    if (fsm->sync_subindex <= fsm->sync_subindices) {

   254

        ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index,

   255

                fsm->sync_subindex);

   256

        ecrt_sdo_request_read(&fsm->request);

   257

        fsm->state = ec_fsm_coe_map_state_pdo;

   258

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

   259

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

   260

        return;

   261

    }

   262

   263

    // finished reading Pdo assignment/mapping

   264

   265

    if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {

   266

        fsm->state = ec_fsm_coe_map_state_error;

   267

        return;

   268

    }

   269

   270

    fsm->sync->assign_source = EC_ASSIGN_COE;

   271

    ec_pdo_list_clear_pdos(&fsm->pdos);

   272

   273

    // next direction

   274

    ec_fsm_coe_map_action_next_dir(fsm);

   275

}

   276

   277

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

   278

   279

/**

   280

 * Fetch Pdo information.

   281

 */

   282

   283

void ec_fsm_coe_map_state_pdo(

   284

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   285

        )

   286

{

   287

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

   288

   289

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

   290

        EC_ERR("Failed to read assigned Pdo index from slave %u.\n",

   291

                fsm->slave->ring_position);

   292

        fsm->state = ec_fsm_coe_map_state_error;

   293

        return;

   294

    }

   295

   296

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

   297

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   298

                "from slave %u.\n", fsm->request.data_size,

   299

                fsm->request.index, fsm->request.subindex,

   300

                fsm->slave->ring_position);

   301

        fsm->state = ec_fsm_coe_map_state_error;

   302

        return;

   303

    }

   304

   305

    if (!(fsm->pdo = (ec_pdo_t *)

   306

                kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   307

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

   308

        fsm->state = ec_fsm_coe_map_state_error;

   309

        return;

   310

    }

   311

   312

    ec_pdo_init(fsm->pdo);

   313

    fsm->pdo->index = EC_READ_U16(fsm->request.data);

   314

    fsm->pdo->dir = ec_sync_direction(fsm->sync);

   315

   316

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

   317

        EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);

   318

   319

    list_add_tail(&fsm->pdo->list, &fsm->pdos.list);

   320

   321

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

   322

    ecrt_sdo_request_read(&fsm->request);

   323

    fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   324

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

   325

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

   326

}

   327

   328

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

   329

   330

/**

   331

 * Read number of mapped Pdo entries.

   332

 */

   333

   334

void ec_fsm_coe_map_state_pdo_entry_count(

   335

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   336

        )

   337

{

   338

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

   339

   340

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

   341

        EC_ERR("Failed to read number of mapped Pdo entries from slave %u.\n",

   342

                fsm->slave->ring_position);

   343

        fsm->state = ec_fsm_coe_map_state_error;

   344

        return;

   345

    }

   346

   347

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

   348

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   349

                "from slave %u.\n", fsm->request.data_size,

   350

                fsm->request.index, fsm->request.subindex,

   351

                fsm->slave->ring_position);

   352

        fsm->state = ec_fsm_coe_map_state_error;

   353

        return;

   354

    }

   355

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

   356

   357

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

   358

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

   359

   360

    // read first Pdo entry

   361

    fsm->pdo_subindex = 1;

   362

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   363

}

   364

   365

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

   366

   367

/**

   368

 * Read next Pdo entry.

   369

 */

   370

   371

void ec_fsm_coe_map_action_next_pdo_entry(

   372

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   373

        )

   374

{

   375

    if (fsm->pdo_subindex <= fsm->pdo_subindices) {

   376

        ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex);

   377

        ecrt_sdo_request_read(&fsm->request);

   378

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   379

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

   380

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

   381

        return;

   382

    }

   383

   384

    // next Pdo

   385

    fsm->sync_subindex++;

   386

    ec_fsm_coe_map_action_next_pdo(fsm);

   387

}

   388

   389

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

   390

   391

/**

   392

 * Read Pdo entry information.

   393

 */

   394

   395

void ec_fsm_coe_map_state_pdo_entry(

   396

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   397

        )

   398

{

   399

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

   400

   401

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

   402

        EC_ERR("Failed to read mapped Pdo entry from slave %u.\n",

   403

                fsm->slave->ring_position);

   404

        fsm->state = ec_fsm_coe_map_state_error;

   405

        return;

   406

    }

   407

   408

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

   409

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   410

                "from slave %u.\n", fsm->request.data_size,

   411

                fsm->request.index, fsm->request.subindex,

   412

                fsm->slave->ring_position);

   413

        fsm->state = ec_fsm_coe_map_state_error;

   414

    } else {

   415

        uint32_t pdo_entry_info;

   416

        ec_pdo_entry_t *pdo_entry;

   417

   418

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

   419

   420

        if (!(pdo_entry = (ec_pdo_entry_t *)

   421

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

   422

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

   423

            fsm->state = ec_fsm_coe_map_state_error;

   424

            return;

   425

        }

   426

   427

        ec_pdo_entry_init(pdo_entry);

   428

        pdo_entry->index = pdo_entry_info >> 16;

   429

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

   430

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   431

   432

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

   433

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

   434

                ec_pdo_entry_clear(pdo_entry);

   435

                kfree(pdo_entry);

   436

                fsm->state = ec_fsm_coe_map_state_error;

   437

                return;

   438

            }

   439

        }

   440

   441

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

   442

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

   443

                    pdo_entry->index, pdo_entry->subindex,

   444

                    pdo_entry->bit_length,

   445

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

   446

        }

   447

   448

        // next Pdo entry

   449

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

   450

        fsm->pdo_subindex++;

   451

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   452

    }

   453

}

   454

   455

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

   456

   457

/**

   458

   State: ERROR.

   459

*/

   460

   461

void ec_fsm_coe_map_state_error(

   462

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   463

        )

   464

{

   465

}

   466

   467

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

   468

   469

/**

   470

   State: END.

   471

*/

   472

   473

void ec_fsm_coe_map_state_end(

   474

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   475

        )

   476

{

   477

}

   478

   479

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