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

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

   141

 * PDO mapping state machine

   142

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

   143

   144

/**

   145

 */

   146

   147

void ec_fsm_mapping_state_start(

   148

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

   149

        )

   150

{

   151

    fsm->dir = EC_DIR_OUTPUT;

   152

    ec_fsm_mapping_next_sync(fsm);

   153

}

   154

   155

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

   156

   157

/**

   158

 */

   159

   160

void ec_fsm_mapping_next_sync(

   161

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

   162

        )

   163

{

   164

    do {

   165

        if (fsm->dir > EC_DIR_INPUT) {

   166

            // no more sync managers to configure mappings for

   167

            fsm->state = ec_fsm_mapping_state_end;

   168

            return;

   169

        }

   170

   171

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

   172

            fsm->state = ec_fsm_mapping_state_error;

   173

            return;

   174

        }

   175

        fsm->dir++;

   176

    }

   177

    while (!fsm->sync->alt_mapping);

   178

   179

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

   180

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

   181

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

   182

    }

   183

   184

    // set mapped PDO count to zero

   185

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

   186

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

   187

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

   188

    fsm->sdodata.size = 1;

   189

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

   190

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

   191

   192

    fsm->state = ec_fsm_mapping_state_zero_count;

   193

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

   194

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

   195

}

   196

   197

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

   198

   199

/**

   200

 */

   201

   202

ec_pdo_t *ec_fsm_mapping_next_pdo(

   203

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

   204

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

   205

        )

   206

{

   207

    ec_pdo_t *pdo;

   208

   209

    do {

   210

        list = list->next; 

   211

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

   212

            return NULL; // no next PDO

   213

        pdo = list_entry(list, ec_pdo_t, list);

   214

    }

   215

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

   216

   217

    return pdo;

   218

}

   219

   220

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

   221

   222

/**

   223

 */

   224

   225

void ec_fsm_mapping_state_zero_count(

   226

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

   227

        )

   228

{

   229

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

   230

   231

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

   232

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

   233

                fsm->slave->ring_position);

   234

        fsm->state = ec_fsm_mapping_state_error;

   235

        return;

   236

    }

   237

   238

    // map all PDOs belonging to the current sync manager

   239

   240

    // find first PDO

   241

    if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   242

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

   243

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

   244

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

   245

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

   246

        ec_fsm_mapping_next_sync(fsm);

   247

        return;

   248

    }

   249

   250

    // add first PDO to mapping

   251

    fsm->pdo_count = 1;

   252

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

   253

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

   254

    fsm->sdodata.size = 2;

   255

   256

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

   257

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

   258

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

   259

   260

    fsm->state = ec_fsm_mapping_state_add_pdo;

   261

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

   262

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

   263

}

   264

   265

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

   266

   267

/**

   268

 */

   269

   270

void ec_fsm_mapping_state_add_pdo(

   271

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

   272

        )

   273

{

   274

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

   275

   276

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

   277

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

   278

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

   279

        fsm->state = ec_fsm_mapping_state_error;

   280

        return;

   281

    }

   282

   283

    // find next PDO

   284

    if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   285

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

   286

        // no more PDOs to map. write PDO count

   287

        fsm->sdodata.subindex = 0;

   288

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

   289

        fsm->sdodata.size = 1;

   290

   291

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

   292

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

   293

                    fsm->pdo_count);

   294

   295

        fsm->state = ec_fsm_mapping_state_pdo_count;

   296

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

   297

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

   298

        return;

   299

    }

   300

   301

    // add next PDO to mapping

   302

    fsm->pdo_count++;

   303

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

   304

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

   305

    fsm->sdodata.size = 2;

   306

   307

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

   308

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

   309

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

   310

   311

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

   312

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

   313

}

   314

   315

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

   316

   317

/**

   318

 */

   319

   320

void ec_fsm_mapping_state_pdo_count(

   321

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

   322

        )

   323

{

   324

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

   325

   326

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

   327

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

   328

                fsm->slave->ring_position);

   329

        fsm->state = ec_fsm_mapping_state_error;

   330

        return;

   331

    }

   332

   333

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

   334

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

   335

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

   336

   337

    // mapping configuration for this sync manager complete.

   338

    ec_fsm_mapping_next_sync(fsm);

   339

}

   340

   341

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

   342

 * Common state functions

   343

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

   344

   345

/**

   346

   State: ERROR.

   347

*/

   348

   349

void ec_fsm_mapping_state_error(

   350

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

   351

        )

   352

{

   353

}

   354

   355

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

   356

   357

/**

   358

   State: END.

   359

*/

   360

   361

void ec_fsm_mapping_state_end(

   362

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

   363

        )

   364

{

   365

}

   366

   367

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