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 mapping state machine.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "fsm_mapping.h"

    45

    46

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

    47

    48

void ec_fsm_mapping_state_start(ec_fsm_mapping_t *);

    49

void ec_fsm_mapping_state_zero_count(ec_fsm_mapping_t *);

    50

void ec_fsm_mapping_state_add_pdo(ec_fsm_mapping_t *);

    51

void ec_fsm_mapping_state_pdo_count(ec_fsm_mapping_t *);

    52

void ec_fsm_mapping_state_end(ec_fsm_mapping_t *);

    53

void ec_fsm_mapping_state_error(ec_fsm_mapping_t *);

    54

    55

void ec_fsm_mapping_next_sync(ec_fsm_mapping_t *);

    56

    57

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

    58

    59

/**

    60

 * Constructor.

    61

 */

    62

    63

void ec_fsm_mapping_init(ec_fsm_mapping_t *fsm, /**< mapping state machine */

    64

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

    65

        )

    66

{

    67

    fsm->fsm_coe = fsm_coe;

    68

    fsm->sdodata.data = (uint8_t *) &fsm->sdo_value;

    69

}

    70

    71

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

    72

    73

/**

    74

 * Destructor.

    75

 */

    76

    77

void ec_fsm_mapping_clear(ec_fsm_mapping_t *fsm /**< mapping state machine */)

    78

{

    79

}

    80

    81

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

    82

    83

/**

    84

 * Start PDO mapping configuration state machine.

    85

 */

    86

    87

void ec_fsm_mapping_start(

    88

        ec_fsm_mapping_t *fsm, /**< mapping state machine */

    89

        ec_slave_t *slave /**< slave to configure */

    90

        )

    91

{

    92

    fsm->slave = slave;

    93

    fsm->state = ec_fsm_mapping_state_start;

    94

}

    95

    96

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

    97

    98

/**

    99

 * \return false, if state machine has terminated

   100

 */

   101

   102

int ec_fsm_mapping_running(

   103

        const ec_fsm_mapping_t *fsm /**< mapping state machine */

   104

        )

   105

{

   106

    return fsm->state != ec_fsm_mapping_state_end

   107

        && fsm->state != ec_fsm_mapping_state_error;

   108

}

   109

   110

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

   111

   112

/**

   113

 * Executes the current state of the state machine.

   114

 * If the state machine's datagram is not sent or received yet, the execution

   115

 * of the state machine is delayed to the next cycle.

   116

 * \return false, if state machine has terminated

   117

 */

   118

   119

int ec_fsm_mapping_exec(

   120

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   121

        )

   122

{

   123

    fsm->state(fsm);

   124

    return ec_fsm_mapping_running(fsm);

   125

}

   126

   127

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

   128

   129

/**

   130

 * \return true, if the state machine terminated gracefully

   131

 */

   132

   133

int ec_fsm_mapping_success(

   134

        const ec_fsm_mapping_t *fsm /**< mapping state machine */

   135

        )

   136

{

   137

    return fsm->state == ec_fsm_mapping_state_end;

   138

}

   139

   140

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

   142

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

   143

   144

/**

   146

 */

   147

   148

void ec_fsm_mapping_state_start(

   149

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   150

        )

   151

{

   152

    fsm->dir = EC_DIR_OUTPUT;

   153

    ec_fsm_mapping_next_sync(fsm);

   154

}

   155

   156

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

   157

   158

/**

   160

 */

   161

   162

void ec_fsm_mapping_next_sync(

   163

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   164

        )

   165

{

   166

    while (1) {

   167

        if (fsm->dir > EC_DIR_INPUT) {

   168

            // no more directions to configure mappings for

   169

            fsm->state = ec_fsm_mapping_state_end;

   170

            return;

   171

        }

   172

   173

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

   174

            // no sync manager found for this direction

   175

            fsm->dir++;

   176

            continue;

   177

        }

   178

   179

        fsm->dir++;

   180

        if (fsm->sync->alt_mapping)

   181

            break;

   182

    }

   183

   184

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

   185

        EC_DBG("Configuring PDO mapping for SM%u of slave %i.\n",

   186

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

   187

    }

   188

   189

    // set mapped PDO count to zero

   190

    fsm->sdodata.index = 0x1C10 + fsm->sync->index;

   191

    fsm->sdodata.subindex = 0; // mapped PDO count

   192

    EC_WRITE_U8(&fsm->sdo_value, 0); // zero PDOs mapped

   193

    fsm->sdodata.size = 1;

   194

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

   195

        EC_DBG("Setting PDO count to zero for SM%u.\n", fsm->sync->index);

   196

   197

    fsm->state = ec_fsm_mapping_state_zero_count;

   198

    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   199

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

   200

}

   201

   202

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

   203

   204

/**

   206

 */

   207

   208

ec_pdo_t *ec_fsm_mapping_next_pdo(

   209

        ec_fsm_mapping_t *fsm, /**< mapping state machine */

   210

        struct list_head *list /**< current PDO list item */

   211

        )

   212

{

   213

    ec_pdo_t *pdo;

   214

   215

    do {

   216

        list = list->next; 

   217

        if (list == &fsm->sync->pdos)

   218

            return NULL; // no next PDO

   219

        pdo = list_entry(list, ec_pdo_t, list);

   220

    }

   221

    while (pdo->sync_index != fsm->sync->index);

   222

   223

    return pdo;

   224

}

   225

   226

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

   227

   228

/**

   230

 */

   231

   232

void ec_fsm_mapping_state_zero_count(

   233

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   234

        )

   235

{

   236

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

   237

   238

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

   239

        EC_ERR("Failed to clear PDO mapping for slave %u.\n",

   240

                fsm->slave->ring_position);

   241

        fsm->state = ec_fsm_mapping_state_error;

   242

        return;

   243

    }

   244

   245

    // map all PDOs belonging to the current sync manager

   246

   247

    // find first PDO

   248

    if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   249

                    fsm, &fsm->sync->pdos))) {

   250

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

   251

            EC_DBG("No PDOs to map for SM%u of slave %u.\n",

   252

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

   253

        ec_fsm_mapping_next_sync(fsm);

   254

        return;

   255

    }

   256

   257

    // add first PDO to mapping

   258

    fsm->pdo_count = 1;

   259

    fsm->sdodata.subindex = fsm->pdo_count;

   260

    EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);

   261

    fsm->sdodata.size = 2;

   262

   263

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

   264

        EC_DBG("Mapping PDO 0x%04X at position %u.\n",

   265

                fsm->pdo->index, fsm->sdodata.subindex);

   266

   267

    fsm->state = ec_fsm_mapping_state_add_pdo;

   268

    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   269

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

   270

}

   271

   272

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

   273

   274

/**

   276

 */

   277

   278

void ec_fsm_mapping_state_add_pdo(

   279

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   280

        )

   281

{

   282

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

   283

   284

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

   285

        EC_ERR("Failed to map PDO 0x%04X for SM%u of slave %u.\n",

   286

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

   287

        fsm->state = ec_fsm_mapping_state_error;

   288

        return;

   289

    }

   290

   291

    // find next PDO

   292

    if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   293

                    fsm, &fsm->pdo->list))) {

   294

        // no more PDOs to map. write PDO count

   295

        fsm->sdodata.subindex = 0;

   296

        EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count);

   297

        fsm->sdodata.size = 1;

   298

   299

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

   300

            EC_DBG("Setting number of mapped PDOs to %u.\n",

   301

                    fsm->pdo_count);

   302

   303

        fsm->state = ec_fsm_mapping_state_pdo_count;

   304

        ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   305

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

   306

        return;

   307

    }

   308

   309

    // add next PDO to mapping

   310

    fsm->pdo_count++;

   311

    fsm->sdodata.subindex = fsm->pdo_count;

   312

    EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);

   313

    fsm->sdodata.size = 2;

   314

   315

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

   316

        EC_DBG("Mapping PDO 0x%04X at position %u.\n",

   317

                fsm->pdo->index, fsm->sdodata.subindex);

   318

   319

    ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);

   320

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

   321

}

   322

   323

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

   324

   325

/**

   327

 */

   328

   329

void ec_fsm_mapping_state_pdo_count(

   330

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   331

        )

   332

{

   333

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

   334

   335

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

   336

        EC_ERR("Failed to set number of mapped PDOs for slave %u.\n",

   337

                fsm->slave->ring_position);

   338

        fsm->state = ec_fsm_mapping_state_error;

   339

        return;

   340

    }

   341

   342

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

   343

        EC_DBG("Successfully set PDO mapping for SM%u of slave %u.\n",

   344

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

   345

   346

    // mapping configuration for this sync manager complete.

   347

    ec_fsm_mapping_next_sync(fsm);

   348

}

   349

   350

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

   351

 * Common state functions

   352

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

   353

   354

/**

   355

   State: ERROR.

   356

*/

   357

   358

void ec_fsm_mapping_state_error(

   359

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   360

        )

   361

{

   362

}

   363

   364

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

   365

   366

/**

   367

   State: END.

   368

*/

   369

   370

void ec_fsm_mapping_state_end(

   371

        ec_fsm_mapping_t *fsm /**< mapping state machine */

   372

        )

   373

{

   374

}

   375

   376

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