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 CoE mapping 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

/**

    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_mapping_init(&fsm->mapping);

    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_mapping_clear(&fsm->mapping);

    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

/**

   138

 * Start reading mapping.

   139

 */

   140

   141

void ec_fsm_coe_map_state_start(

   142

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

   143

        )

   144

{

   145

    // read mapping for first direction

   146

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

   147

    ec_fsm_coe_map_action_next_dir(fsm);

   148

}

   149

   150

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

   151

   152

/**

   153

 * Read mapping of next direction manager.

   154

 */

   155

   156

void ec_fsm_coe_map_action_next_dir(

   157

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

   158

        )

   159

{

   160

    ec_slave_t *slave = fsm->slave;

   161

   162

    fsm->dir++;

   163

   164

    if (slave->master->debug_level)

   165

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

   166

                fsm->dir, slave->ring_position);

   167

   168

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

   169

   170

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

   171

            if (slave->master->debug_level)

   172

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

   173

            continue;

   174

        }

   175

   176

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

   177

   178

        if (slave->master->debug_level)

   179

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

   180

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

   181

   182

        ec_pdo_mapping_clear_pdos(&fsm->mapping);

   183

   186

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   187

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

   188

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

   189

        return;

   190

    }

   191

   192

    if (slave->master->debug_level)

   193

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

   194

                slave->ring_position);

   195

   196

    fsm->state = ec_fsm_coe_map_state_end;

   197

}

   198

   199

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

   200

   201

/**

   202

 * Count mapped Pdos.

   203

 */

   204

   205

void ec_fsm_coe_map_state_pdo_count(

   206

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

   207

        )

   208

{

   209

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

   210

   211

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

   212

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

   213

                fsm->slave->ring_position);

   214

        fsm->state = ec_fsm_coe_map_state_error;

   215

        return;

   216

    }

   217

   218

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

   219

   220

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

   221

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

   222

   223

    // read first Pdo

   224

    fsm->sync_subindex = 1;

   225

    ec_fsm_coe_map_action_next_pdo(fsm);

   226

}

   227

   228

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

   229

   230

/**

   231

 * Read next Pdo.

   232

 */

   233

   234

void ec_fsm_coe_map_action_next_pdo(

   235

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

   236

        )

   237

{

   238

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

   240

                fsm->sync_subindex);

   242

        fsm->state = ec_fsm_coe_map_state_pdo;

   243

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

   244

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

   245

        return;

   246

    }

   247

   248

    // finished reading Pdo mapping/configuration

   249

   250

    if (ec_pdo_mapping_copy(&fsm->sync->mapping, &fsm->mapping)) {

   251

        fsm->state = ec_fsm_coe_map_state_error;

   252

        return;

   253

    }

   254

   255

    fsm->sync->mapping_source = EC_SYNC_MAPPING_COE;

   256

    ec_pdo_mapping_clear_pdos(&fsm->mapping);

   257

   258

    // next direction

   259

    ec_fsm_coe_map_action_next_dir(fsm);

   260

}

   261

   262

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

   263

   264

/**

   265

 * Fetch Pdo information.

   266

 */

   267

   268

void ec_fsm_coe_map_state_pdo(

   269

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

   270

        )

   271

{

   272

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

   273

   274

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

   275

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

   276

                fsm->slave->ring_position);

   277

        fsm->state = ec_fsm_coe_map_state_error;

   278

        return;

   279

    }

   280

   281

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

   282

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

   283

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

   284

        fsm->state = ec_fsm_coe_map_state_error;

   285

        return;

   286

    }

   287

   288

    ec_pdo_init(fsm->pdo);

   289

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

   290

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

   291

   292

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

   293

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

   294

   295

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

   296

   299

    fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   300

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

   301

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

   302

}

   303

   304

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

   305

   306

/**

   307

 * Read number of Pdo entries.

   308

 */

   309

   310

void ec_fsm_coe_map_state_pdo_entry_count(

   311

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

   312

        )

   313

{

   314

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

   315

   316

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

   317

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

   318

                fsm->slave->ring_position);

   319

        fsm->state = ec_fsm_coe_map_state_error;

   320

        return;

   321

    }

   322

   323

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

   324

   325

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

   326

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

   327

   328

    // read first Pdo entry

   329

    fsm->pdo_subindex = 1;

   330

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   331

}

   332

   333

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

   334

   335

/**

   336

 * Read next Pdo entry.

   337

 */

   338

   339

void ec_fsm_coe_map_action_next_pdo_entry(

   340

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

   341

        )

   342

{

   343

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

   346

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   347

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

   348

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

   349

        return;

   350

    }

   351

   352

    // next Pdo

   353

    fsm->sync_subindex++;

   354

    ec_fsm_coe_map_action_next_pdo(fsm);

   355

}

   356

   357

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

   358

   359

/**

   360

 * Read Pdo entry information.

   361

 */

   362

   363

void ec_fsm_coe_map_state_pdo_entry(

   364

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

   365

        )

   366

{

   367

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

   368

   369

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

   370

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

   371

                fsm->slave->ring_position);

   372

        fsm->state = ec_fsm_coe_map_state_error;

   373

        return;

   374

    }

   375

   376

    {

   377

        uint32_t pdo_entry_info;

   378

        ec_pdo_entry_t *pdo_entry;

   379

   380

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

   381

   382

        if (!(pdo_entry = (ec_pdo_entry_t *)

   383

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

   384

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

   385

            fsm->state = ec_fsm_coe_map_state_error;

   386

            return;

   387

        }

   388

   389

        ec_pdo_entry_init(pdo_entry);

   390

        pdo_entry->index = pdo_entry_info >> 16;

   391

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

   392

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   393

   394

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

   395

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

   396

                ec_pdo_entry_clear(pdo_entry);

   397

                kfree(pdo_entry);

   398

                fsm->state = ec_fsm_coe_map_state_error;

   399

                return;

   400

            }

   401

        }

   402

   403

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

   404

            EC_DBG("    Pdo entry 0x%04X \"%s\" (%u bit).\n", pdo_entry->index,

   405

                    pdo_entry->name ? pdo_entry->name : "???",

   406

                    pdo_entry->bit_length);

   407

        }

   408

   409

        // next Pdo entry

   410

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

   411

        fsm->pdo_subindex++;

   412

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   413

    }

   414

}

   415

   416

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

   417

   418

/**

   419

   State: ERROR.

   420

*/

   421

   422

void ec_fsm_coe_map_state_error(

   423

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

   424

        )

   425

{

   426

}

   427

   428

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

   429

   430

/**

   431

   State: END.

   432

*/

   433

   434

void ec_fsm_coe_map_state_end(

   435

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

   436

        )

   437

{

   438

}

   439

   440

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