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

    while (1) {

   165

        if (fsm->dir > EC_DIR_INPUT) {

   166

            // no more directions 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

            // no sync manager found for this direction

   173

            fsm->dir++;

   174

            continue;

   175

        }

   176

   177

        fsm->dir++;

   178

        if (fsm->sync->alt_mapping)

   179

            break;

   180

    }

   181

   182

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

   183

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

   184

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

   185

    }

   186

   187

    // set mapped PDO count to zero

   188

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

   189

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

   190

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

   191

    fsm->sdodata.size = 1;

   192

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

   193

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

   194

   195

    fsm->state = ec_fsm_mapping_state_zero_count;

   196

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

   197

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

   198

}

   199

   200

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

   201

   202

/**

   203

 */

   204

   205

ec_pdo_t *ec_fsm_mapping_next_pdo(

   206

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

   207

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

   208

        )

   209

{

   210

    ec_pdo_t *pdo;

   211

   212

    do {

   213

        list = list->next; 

   214

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

   215

            return NULL; // no next PDO

   216

        pdo = list_entry(list, ec_pdo_t, list);

   217

    }

   218

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

   219

   220

    return pdo;

   221

}

   222

   223

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

   224

   225

/**

   226

 */

   227

   228

void ec_fsm_mapping_state_zero_count(

   229

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

   230

        )

   231

{

   232

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

   233

   234

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

   235

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

   236

                fsm->slave->ring_position);

   237

        fsm->state = ec_fsm_mapping_state_error;

   238

        return;

   239

    }

   240

   241

    // map all PDOs belonging to the current sync manager

   242

   243

    // find first PDO

   244

    if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   245

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

   246

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

   247

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

   248

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

   249

        ec_fsm_mapping_next_sync(fsm);

   250

        return;

   251

    }

   252

   253

    // add first PDO to mapping

   254

    fsm->pdo_count = 1;

   255

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

   256

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

   257

    fsm->sdodata.size = 2;

   258

   259

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

   260

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

   261

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

   262

   263

    fsm->state = ec_fsm_mapping_state_add_pdo;

   264

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

   265

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

   266

}

   267

   268

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

   269

   270

/**

   271

 */

   272

   273

void ec_fsm_mapping_state_add_pdo(

   274

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

   275

        )

   276

{

   277

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

   278

   279

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

   280

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

   281

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

   282

        fsm->state = ec_fsm_mapping_state_error;

   283

        return;

   284

    }

   285

   286

    // find next PDO

   287

    if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   288

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

   289

        // no more PDOs to map. write PDO count

   290

        fsm->sdodata.subindex = 0;

   291

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

   292

        fsm->sdodata.size = 1;

   293

   294

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

   295

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

   296

                    fsm->pdo_count);

   297

   298

        fsm->state = ec_fsm_mapping_state_pdo_count;

   299

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

   300

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

   301

        return;

   302

    }

   303

   304

    // add next PDO to mapping

   305

    fsm->pdo_count++;

   306

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

   307

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

   308

    fsm->sdodata.size = 2;

   309

   310

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

   311

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

   312

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

   313

   314

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

   315

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

   316

}

   317

   318

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

   319

   320

/**

   321

 */

   322

   323

void ec_fsm_mapping_state_pdo_count(

   324

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

   325

        )

   326

{

   327

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

   328

   329

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

   330

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

   331

                fsm->slave->ring_position);

   332

        fsm->state = ec_fsm_mapping_state_error;

   333

        return;

   334

    }

   335

   336

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

   337

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

   338

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

   339

   340

    // mapping configuration for this sync manager complete.

   341

    ec_fsm_mapping_next_sync(fsm);

   342

}

   343

   344

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

   345

 * Common state functions

   346

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

   347

   348

/**

   349

   State: ERROR.

   350

*/

   351

   352

void ec_fsm_mapping_state_error(

   353

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

   354

        )

   355

{

   356

}

   357

   358

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

   359

   360

/**

   361

   State: END.

   362

*/

   363

   364

void ec_fsm_mapping_state_end(

   365

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

   366

        )

   367

{

   368

}

   369

   370

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