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 not reserved for mailbox

   144

    fsm->sync_index = 1; // next is 2

   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

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

   161

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

   162

            continue;

   163

   164

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

   165

   166

        if (slave->master->debug_level)

   167

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

   168

                    fsm->sync_index, slave->ring_position);

   169

   170

        ec_pdo_list_clear_pdos(&fsm->pdos);

   171

   172

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

   173

        ecrt_sdo_request_read(&fsm->request);

   174

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   175

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

   176

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

   177

        return;

   178

    }

   179

   180

    if (slave->master->debug_level)

   181

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

   182

                slave->ring_position);

   183

   184

    fsm->state = ec_fsm_coe_map_state_end;

   185

}

   186

   187

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

   188

   189

/**

   190

 * Count assigned Pdos.

   191

 */

   192

   193

void ec_fsm_coe_map_state_pdo_count(

   194

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

   195

        )

   196

{

   197

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

   198

   199

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

   200

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

   201

                fsm->slave->ring_position);

   202

        fsm->state = ec_fsm_coe_map_state_error;

   203

        return;

   204

    }

   205

   206

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

   207

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

   208

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

   209

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

   210

                fsm->slave->ring_position);

   211

        fsm->state = ec_fsm_coe_map_state_error;

   212

        return;

   213

    }

   214

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

   215

   216

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

   217

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

   218

   219

    // read first Pdo

   220

    fsm->sync_subindex = 1;

   221

    ec_fsm_coe_map_action_next_pdo(fsm);

   222

}

   223

   224

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

   225

   226

/**

   227

 * Read next Pdo.

   228

 */

   229

   230

void ec_fsm_coe_map_action_next_pdo(

   231

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

   232

        )

   233

{

   234

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

   235

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

   236

                fsm->sync_subindex);

   237

        ecrt_sdo_request_read(&fsm->request);

   238

        fsm->state = ec_fsm_coe_map_state_pdo;

   239

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

   240

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

   241

        return;

   242

    }

   243

   244

    // finished reading Pdo assignment/mapping

   245

   246

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

   247

        fsm->state = ec_fsm_coe_map_state_error;

   248

        return;

   249

    }

   250

   251

    fsm->sync->assign_source = EC_ASSIGN_COE;

   252

    ec_pdo_list_clear_pdos(&fsm->pdos);

   253

   254

    // next sync manager

   255

    ec_fsm_coe_map_action_next_sync(fsm);

   256

}

   257

   258

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

   259

   260

/**

   261

 * Fetch Pdo information.

   262

 */

   263

   264

void ec_fsm_coe_map_state_pdo(

   265

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

   266

        )

   267

{

   268

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

   269

   270

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

   271

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

   272

                fsm->slave->ring_position);

   273

        fsm->state = ec_fsm_coe_map_state_error;

   274

        return;

   275

    }

   276

   277

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

   278

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

   279

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

   280

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

   281

                fsm->slave->ring_position);

   282

        fsm->state = ec_fsm_coe_map_state_error;

   283

        return;

   284

    }

   285

   286

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

   287

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

   288

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

   289

        fsm->state = ec_fsm_coe_map_state_error;

   290

        return;

   291

    }

   292

   293

    ec_pdo_init(fsm->pdo);

   294

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

   295

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

   296

   297

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

   298

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

   299

   300

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

   301

   302

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

   303

    ecrt_sdo_request_read(&fsm->request);

   304

    fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   305

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

   306

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

   307

}

   308

   309

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

   310

   311

/**

   312

 * Read number of mapped Pdo entries.

   313

 */

   314

   315

void ec_fsm_coe_map_state_pdo_entry_count(

   316

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

   317

        )

   318

{

   319

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

   320

   321

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

   322

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

   323

                fsm->slave->ring_position);

   324

        fsm->state = ec_fsm_coe_map_state_error;

   325

        return;

   326

    }

   327

   328

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

   329

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

   330

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

   331

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

   332

                fsm->slave->ring_position);

   333

        fsm->state = ec_fsm_coe_map_state_error;

   334

        return;

   335

    }

   336

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

   337

   338

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

   339

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

   340

   341

    // read first Pdo entry

   342

    fsm->pdo_subindex = 1;

   343

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   344

}

   345

   346

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

   347

   348

/**

   349

 * Read next Pdo entry.

   350

 */

   351

   352

void ec_fsm_coe_map_action_next_pdo_entry(

   353

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

   354

        )

   355

{

   356

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

   357

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

   358

        ecrt_sdo_request_read(&fsm->request);

   359

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   360

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

   361

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

   362

        return;

   363

    }

   364

   365

    // next Pdo

   366

    fsm->sync_subindex++;

   367

    ec_fsm_coe_map_action_next_pdo(fsm);

   368

}

   369

   370

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

   371

   372

/**

   373

 * Read Pdo entry information.

   374

 */

   375

   376

void ec_fsm_coe_map_state_pdo_entry(

   377

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

   378

        )

   379

{

   380

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

   381

   382

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

   383

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

   384

                fsm->slave->ring_position);

   385

        fsm->state = ec_fsm_coe_map_state_error;

   386

        return;

   387

    }

   388

   389

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

   390

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

   391

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

   392

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

   393

                fsm->slave->ring_position);

   394

        fsm->state = ec_fsm_coe_map_state_error;

   395

    } else {

   396

        uint32_t pdo_entry_info;

   397

        ec_pdo_entry_t *pdo_entry;

   398

   399

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

   400

   401

        if (!(pdo_entry = (ec_pdo_entry_t *)

   402

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

   403

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

   404

            fsm->state = ec_fsm_coe_map_state_error;

   405

            return;

   406

        }

   407

   408

        ec_pdo_entry_init(pdo_entry);

   409

        pdo_entry->index = pdo_entry_info >> 16;

   410

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

   411

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   412

   413

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

   414

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

   415

                ec_pdo_entry_clear(pdo_entry);

   416

                kfree(pdo_entry);

   417

                fsm->state = ec_fsm_coe_map_state_error;

   418

                return;

   419

            }

   420

        }

   421

   422

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

   423

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

   424

                    pdo_entry->index, pdo_entry->subindex,

   425

                    pdo_entry->bit_length,

   426

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

   427

        }

   428

   429

        // next Pdo entry

   430

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

   431

        fsm->pdo_subindex++;

   432

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   433

    }

   434

}

   435

   436

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

   437

   438

/**

   439

   State: ERROR.

   440

*/

   441

   442

void ec_fsm_coe_map_state_error(

   443

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

   444

        )

   445

{

   446

}

   447

   448

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

   449

   450

/**

   451

   State: END.

   452

*/

   453

   454

void ec_fsm_coe_map_state_end(

   455

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

   456

        )

   457

{

   458

}

   459

   460

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