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 "canopen.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->state = NULL;

    74

    fsm->fsm_coe = fsm_coe;

    75

    ec_pdo_mapping_init(&fsm->mapping);

    76

}

    77

    78

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

    79

    80

/**

    81

   Destructor.

    82

*/

    83

    84

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

    85

{

    86

    ec_pdo_mapping_clear(&fsm->mapping);

    87

}

    88

    89

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

    90

    91

/**

    92

   Starts to upload an Sdo from a slave.

    93

*/

    94

    95

void ec_fsm_coe_map_start(

    96

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

    97

        ec_slave_t *slave /**< EtherCAT slave */

    98

        )

    99

{

   100

    fsm->slave = slave;

   101

    fsm->state = ec_fsm_coe_map_state_start;

   102

}

   103

   104

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

   105

   106

/**

   107

   Executes the current state of the state machine.

   108

   \return false, if state machine has terminated

   109

*/

   110

   111

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

   112

{

   113

    fsm->state(fsm);

   114

   115

    return fsm->state != ec_fsm_coe_map_state_end

   116

        && fsm->state != ec_fsm_coe_map_state_error;

   117

}

   118

   119

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

   120

   121

/**

   122

   Returns, if the state machine terminated with success.

   123

   \return non-zero if successful.

   124

*/

   125

   126

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

   127

{

   128

    return fsm->state == ec_fsm_coe_map_state_end;

   129

}

   130

   131

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

   132

 *  state functions

   133

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

   134

   135

/**

   136

 * Start reading mapping.

   137

 */

   138

   139

void ec_fsm_coe_map_state_start(

   140

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

   141

        )

   142

{

   143

    // read mapping for first direction

   144

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

   145

    ec_fsm_coe_map_action_next_dir(fsm);

   146

}

   147

   148

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

   149

   150

/**

   151

 * Read mapping of next direction manager.

   152

 */

   153

   154

void ec_fsm_coe_map_action_next_dir(

   155

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

   156

        )

   157

{

   158

    ec_slave_t *slave = fsm->slave;

   159

   160

    fsm->dir++;

   161

   162

    if (slave->master->debug_level)

   163

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

   164

                fsm->dir, slave->ring_position);

   165

   166

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

   167

   168

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

   169

            if (slave->master->debug_level)

   170

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

   171

            continue;

   172

        }

   173

   174

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

   175

   176

        if (slave->master->debug_level)

   177

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

   178

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

   179

   180

        ec_pdo_mapping_clear_pdos(&fsm->mapping);

   181

   182

        ec_sdo_request_init_read(&fsm->request, slave, fsm->sync_sdo_index, 0);

   183

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   184

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

   185

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

   186

        return;

   187

    }

   188

   189

    if (slave->master->debug_level)

   190

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

   191

                slave->ring_position);

   192

   193

    fsm->state = ec_fsm_coe_map_state_end;

   194

}

   195

   196

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

   197

   198

/**

   199

 * Count mapped Pdos.

   200

 */

   201

   202

void ec_fsm_coe_map_state_pdo_count(

   203

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

   204

        )

   205

{

   206

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

   207

   208

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

   209

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

   210

                fsm->slave->ring_position);

   211

        fsm->state = ec_fsm_coe_map_state_error;

   212

        return;

   213

    }

   214

   215

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

   216

   217

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

   218

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

   219

   220

    // read first Pdo

   221

    fsm->sync_subindex = 1;

   222

    ec_fsm_coe_map_action_next_pdo(fsm);

   223

}

   224

   225

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

   226

   227

/**

   228

 * Read next Pdo.

   229

 */

   230

   231

void ec_fsm_coe_map_action_next_pdo(

   232

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

   233

        )

   234

{

   235

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

   236

        ec_sdo_request_init_read(&fsm->request, fsm->slave,

   237

                fsm->sync_sdo_index, fsm->sync_subindex);

   238

        fsm->state = ec_fsm_coe_map_state_pdo;

   239

        ec_fsm_coe_upload(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 mapping/configuration

   245

   246

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

   247

        fsm->state = ec_fsm_coe_map_state_error;

   248

        return;

   249

    }

   250

   251

    fsm->sync->mapping_source = EC_SYNC_MAPPING_COE;

   252

    ec_pdo_mapping_clear_pdos(&fsm->mapping);

   253

   254

    // next direction

   255

    ec_fsm_coe_map_action_next_dir(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 mapped 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->pdo = (ec_pdo_t *)

   278

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

   279

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

   280

        fsm->state = ec_fsm_coe_map_state_error;

   281

        return;

   282

    }

   283

   284

    ec_pdo_init(fsm->pdo);

   285

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

   286

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

   287

   288

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

   289

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

   290

   291

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

   292

   293

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

   294

    fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   295

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

   296

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

   297

}

   298

   299

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

   300

   301

/**

   302

 * Read number of Pdo entries.

   303

 */

   304

   305

void ec_fsm_coe_map_state_pdo_entry_count(

   306

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

   307

        )

   308

{

   309

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

   310

   311

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

   312

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

   313

                fsm->slave->ring_position);

   314

        fsm->state = ec_fsm_coe_map_state_error;

   315

        return;

   316

    }

   317

   318

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

   319

   320

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

   321

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

   322

   323

    // read first Pdo entry

   324

    fsm->pdo_subindex = 1;

   325

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   326

}

   327

   328

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

   329

   330

/**

   331

 * Read next Pdo entry.

   332

 */

   333

   334

void ec_fsm_coe_map_action_next_pdo_entry(

   335

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

   336

        )

   337

{

   338

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

   339

        ec_sdo_request_init_read(&fsm->request, fsm->slave,

   340

                fsm->pdo->index, fsm->pdo_subindex);

   341

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   342

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

   343

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

   344

        return;

   345

    }

   346

   347

    // next Pdo

   348

    fsm->sync_subindex++;

   349

    ec_fsm_coe_map_action_next_pdo(fsm);

   350

}

   351

   352

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

   353

   354

/**

   355

 * Read Pdo entry information.

   356

 */

   357

   358

void ec_fsm_coe_map_state_pdo_entry(

   359

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

   360

        )

   361

{

   362

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

   363

   364

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

   365

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

   366

                fsm->slave->ring_position);

   367

        fsm->state = ec_fsm_coe_map_state_error;

   368

        return;

   369

    }

   370

   371

    {

   372

        uint32_t pdo_entry_info;

   373

        ec_pdo_entry_t *pdo_entry;

   374

   375

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

   376

   377

        if (!(pdo_entry = (ec_pdo_entry_t *)

   378

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

   379

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

   380

            fsm->state = ec_fsm_coe_map_state_error;

   381

            return;

   382

        }

   383

   384

        ec_pdo_entry_init(pdo_entry);

   385

        pdo_entry->index = pdo_entry_info >> 16;

   386

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

   387

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   388

   389

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

   390

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

   391

                ec_pdo_entry_clear(pdo_entry);

   392

                kfree(pdo_entry);

   393

                fsm->state = ec_fsm_coe_map_state_error;

   394

                return;

   395

            }

   396

        }

   397

   398

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

   399

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

   400

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

   401

                    pdo_entry->bit_length);

   402

        }

   403

   404

        // next Pdo entry

   405

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

   406

        fsm->pdo_subindex++;

   407

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   408

    }

   409

}

   410

   411

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

   412

   413

/**

   414

   State: ERROR.

   415

*/

   416

   417

void ec_fsm_coe_map_state_error(

   418

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

   419

        )

   420

{

   421

}

   422

   423

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

   424

   425

/**

   426

   State: END.

   427

*/

   428

   429

void ec_fsm_coe_map_state_end(

   430

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

   431

        )

   432

{

   433

}

   434

   435

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