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_sync(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

/**

    65

   Constructor.

    66

*/

    67

    68

void ec_fsm_coe_map_init(

    69

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

    70

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

    71

        )

    72

{

    73

    fsm->fsm_coe = fsm_coe;

    74

    fsm->state = NULL;

    75

    ec_sdo_request_init(&fsm->request);

    76

    ec_pdo_list_init(&fsm->pdos);

    77

}

    78

    79

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

    80

    81

/**

    82

   Destructor.

    83

*/

    84

    85

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

    86

{

    87

    ec_sdo_request_clear(&fsm->request);

    88

    ec_pdo_list_clear(&fsm->pdos);

    89

}

    90

    91

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

    92

    93

/**

    94

   Starts to upload an Sdo from a slave.

    95

*/

    96

    97

void ec_fsm_coe_map_start(

    98

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

    99

        ec_slave_t *slave /**< EtherCAT slave */

   100

        )

   101

{

   102

    fsm->slave = slave;

   103

    fsm->state = ec_fsm_coe_map_state_start;

   104

}

   105

   106

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

   107

   108

/**

   109

   Executes the current state of the state machine.

   110

   \return false, if state machine has terminated

   111

*/

   112

   113

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

   114

{

   115

    fsm->state(fsm);

   116

   117

    return fsm->state != ec_fsm_coe_map_state_end

   118

        && fsm->state != ec_fsm_coe_map_state_error;

   119

}

   120

   121

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

   122

   123

/**

   124

   Returns, if the state machine terminated with success.

   125

   \return non-zero if successful.

   126

*/

   127

   128

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

   129

{

   130

    return fsm->state == ec_fsm_coe_map_state_end;

   131

}

   132

   133

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

   134

 *  state functions

   135

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

   136

   137

/** Start reading Pdo assignment.

   138

 */

   139

void ec_fsm_coe_map_state_start(

   140

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

   141

        )

   142

{

   143

    // read Pdo assignment for first sync manager

   144

    fsm->sync_index = 0xff; // next is 0

   145

    ec_fsm_coe_map_action_next_sync(fsm);

   146

}

   147

   148

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

   149

   150

/** Read Pdo assignment of next sync manager.

   151

 */

   152

void ec_fsm_coe_map_action_next_sync(

   153

        ec_fsm_coe_map_t *fsm /**< Finite state machine */

   154

        )

   155

{

   156

    ec_slave_t *slave = fsm->slave;

   157

   158

    fsm->sync_index++;

   159

   160

    if (slave->master->debug_level)

   161

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

   162

                fsm->sync_index, slave->ring_position);

   163

   164

    for (; fsm->sync_index < EC_MAX_SYNCS; fsm->sync_index++) {

   165

        if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index))) {

   166

            if (slave->master->debug_level)

   167

                EC_DBG("Slave %u does not provide a configuration for "

   168

                        "SM%u!\n", fsm->slave->ring_position, fsm->sync_index);

   169

            continue;

   170

        }

   171

   172

        fsm->sync_sdo_index = 0x1C10 + fsm->sync_index;

   173

   174

        if (slave->master->debug_level)

   175

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

   176

                    fsm->sync_index, slave->ring_position);

   177

   178

        ec_pdo_list_clear_pdos(&fsm->pdos);

   179

   180

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

   181

        ecrt_sdo_request_read(&fsm->request);

   182

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   183

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

   184

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

   185

        return;

   186

    }

   187

   188

    if (slave->master->debug_level)

   189

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

   190

                slave->ring_position);

   191

   192

    fsm->state = ec_fsm_coe_map_state_end;

   193

}

   194

   195

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

   196

   197

/**

   198

 * Count assigned Pdos.

   199

 */

   200

   201

void ec_fsm_coe_map_state_pdo_count(

   202

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

   203

        )

   204

{

   205

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

   206

   207

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

   208

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

   209

                fsm->slave->ring_position);

   210

        fsm->state = ec_fsm_coe_map_state_error;

   211

        return;

   212

    }

   213

   214

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

   215

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

   216

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

   217

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

   218

                fsm->slave->ring_position);

   219

        fsm->state = ec_fsm_coe_map_state_error;

   220

        return;

   221

    }

   222

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

   223

   224

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

   225

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

   226

   227

    // read first Pdo

   228

    fsm->sync_subindex = 1;

   229

    ec_fsm_coe_map_action_next_pdo(fsm);

   230

}

   231

   232

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

   233

   234

/**

   235

 * Read next Pdo.

   236

 */

   237

   238

void ec_fsm_coe_map_action_next_pdo(

   239

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

   240

        )

   241

{

   242

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

   243

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

   244

                fsm->sync_subindex);

   245

        ecrt_sdo_request_read(&fsm->request);

   246

        fsm->state = ec_fsm_coe_map_state_pdo;

   247

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

   248

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

   249

        return;

   250

    }

   251

   252

    // finished reading Pdo assignment/mapping

   253

   254

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

   255

        fsm->state = ec_fsm_coe_map_state_error;

   256

        return;

   257

    }

   258

   259

    fsm->sync->assign_source = EC_ASSIGN_COE;

   260

    ec_pdo_list_clear_pdos(&fsm->pdos);

   261

   262

    // next sync manager

   263

    ec_fsm_coe_map_action_next_sync(fsm);

   264

}

   265

   266

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

   267

   268

/**

   269

 * Fetch Pdo information.

   270

 */

   271

   272

void ec_fsm_coe_map_state_pdo(

   273

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

   274

        )

   275

{

   276

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

   277

   278

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

   279

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

   280

                fsm->slave->ring_position);

   281

        fsm->state = ec_fsm_coe_map_state_error;

   282

        return;

   283

    }

   284

   285

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

   286

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

   287

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

   288

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

   289

                fsm->slave->ring_position);

   290

        fsm->state = ec_fsm_coe_map_state_error;

   291

        return;

   292

    }

   293

   294

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

   295

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

   296

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

   297

        fsm->state = ec_fsm_coe_map_state_error;

   298

        return;

   299

    }

   300

   301

    ec_pdo_init(fsm->pdo);

   302

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

   303

    fsm->pdo->sync_index = fsm->sync_index;

   304

   305

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

   306

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

   307

   308

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

   309

   310

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

   311

    ecrt_sdo_request_read(&fsm->request);

   312

    fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   313

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

   314

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

   315

}

   316

   317

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

   318

   319

/**

   320

 * Read number of mapped Pdo entries.

   321

 */

   322

   323

void ec_fsm_coe_map_state_pdo_entry_count(

   324

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

   325

        )

   326

{

   327

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

   328

   329

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

   330

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

   331

                fsm->slave->ring_position);

   332

        fsm->state = ec_fsm_coe_map_state_error;

   333

        return;

   334

    }

   335

   336

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

   337

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

   338

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

   339

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

   340

                fsm->slave->ring_position);

   341

        fsm->state = ec_fsm_coe_map_state_error;

   342

        return;

   343

    }

   344

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

   345

   346

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

   347

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

   348

   349

    // read first Pdo entry

   350

    fsm->pdo_subindex = 1;

   351

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   352

}

   353

   354

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

   355

   356

/**

   357

 * Read next Pdo entry.

   358

 */

   359

   360

void ec_fsm_coe_map_action_next_pdo_entry(

   361

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

   362

        )

   363

{

   364

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

   365

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

   366

        ecrt_sdo_request_read(&fsm->request);

   367

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   368

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

   369

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

   370

        return;

   371

    }

   372

   373

    // next Pdo

   374

    fsm->sync_subindex++;

   375

    ec_fsm_coe_map_action_next_pdo(fsm);

   376

}

   377

   378

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

   379

   380

/**

   381

 * Read Pdo entry information.

   382

 */

   383

   384

void ec_fsm_coe_map_state_pdo_entry(

   385

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

   386

        )

   387

{

   388

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

   389

   390

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

   391

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

   392

                fsm->slave->ring_position);

   393

        fsm->state = ec_fsm_coe_map_state_error;

   394

        return;

   395

    }

   396

   397

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

   398

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

   399

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

   400

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

   401

                fsm->slave->ring_position);

   402

        fsm->state = ec_fsm_coe_map_state_error;

   403

    } else {

   404

        uint32_t pdo_entry_info;

   405

        ec_pdo_entry_t *pdo_entry;

   406

   407

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

   408

   409

        if (!(pdo_entry = (ec_pdo_entry_t *)

   410

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

   411

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

   412

            fsm->state = ec_fsm_coe_map_state_error;

   413

            return;

   414

        }

   415

   416

        ec_pdo_entry_init(pdo_entry);

   417

        pdo_entry->index = pdo_entry_info >> 16;

   418

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

   419

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   420

   421

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

   422

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

   423

                ec_pdo_entry_clear(pdo_entry);

   424

                kfree(pdo_entry);

   425

                fsm->state = ec_fsm_coe_map_state_error;

   426

                return;

   427

            }

   428

        }

   429

   430

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

   431

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

   432

                    pdo_entry->index, pdo_entry->subindex,

   433

                    pdo_entry->bit_length,

   434

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

   435

        }

   436

   437

        // next Pdo entry

   438

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

   439

        fsm->pdo_subindex++;

   440

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   441

    }

   442

}

   443

   444

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

   445

   446

/**

   447

   State: ERROR.

   448

*/

   449

   450

void ec_fsm_coe_map_state_error(

   451

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

   452

        )

   453

{

   454

}

   455

   456

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

   457

   458

/**

   459

   State: END.

   460

*/

   461

   462

void ec_fsm_coe_map_state_end(

   463

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

   464

        )

   465

{

   466

}

   467

   468

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