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

/** \file

    35

 * EtherCAT Pdo mapping state machine.

    36

 */

    37

    38

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

    39

    40

#include "globals.h"

    41

#include "master.h"

    42

#include "mailbox.h"

    43

#include "slave_config.h"

    44

    45

#include "fsm_pdo_mapping.h"

    46

    47

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

    48

    49

void ec_fsm_pdo_mapping_state_start(ec_fsm_pdo_mapping_t *);

    50

void ec_fsm_pdo_mapping_state_zero_count(ec_fsm_pdo_mapping_t *);

    51

void ec_fsm_pdo_mapping_state_add_pdo(ec_fsm_pdo_mapping_t *);

    52

void ec_fsm_pdo_mapping_state_pdo_count(ec_fsm_pdo_mapping_t *);

    53

void ec_fsm_pdo_mapping_state_end(ec_fsm_pdo_mapping_t *);

    54

void ec_fsm_pdo_mapping_state_error(ec_fsm_pdo_mapping_t *);

    55

    56

void ec_fsm_pdo_mapping_next_dir(ec_fsm_pdo_mapping_t *);

    57

    58

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

    59

    60

/** Constructor.

    61

 */

    62

void ec_fsm_pdo_mapping_init(

    63

        ec_fsm_pdo_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

/** Destructor.

    74

 */

    75

void ec_fsm_pdo_mapping_clear(

    76

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

    77

        )

    78

{

    79

}

    80

    81

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

    82

    83

/** Start Pdo mapping configuration state machine.

    84

 */

    85

void ec_fsm_pdo_mapping_start(

    86

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

    87

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

    88

        )

    89

{

    90

    fsm->slave = slave;

    91

    fsm->state = ec_fsm_pdo_mapping_state_start;

    92

}

    93

    94

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

    95

    96

/** Get running state.

    97

 *

    98

 * \return false, if state machine has terminated

    99

 */

   100

int ec_fsm_pdo_mapping_running(

   101

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

   102

        )

   103

{

   104

    return fsm->state != ec_fsm_pdo_mapping_state_end

   105

        && fsm->state != ec_fsm_pdo_mapping_state_error;

   106

}

   107

   108

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

   109

   110

/** Executes the current state of the state machine.

   111

 *

   112

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

   113

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

   114

 *

   115

 * \return false, if state machine has terminated

   116

 */

   117

int ec_fsm_pdo_mapping_exec(

   118

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   119

        )

   120

{

   121

    fsm->state(fsm);

   122

    return ec_fsm_pdo_mapping_running(fsm);

   123

}

   124

   125

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

   126

   127

/** Get execution result.

   128

 *

   129

 * \return true, if the state machine terminated gracefully

   130

 */

   131

int ec_fsm_pdo_mapping_success(

   132

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

   133

        )

   134

{

   135

    return fsm->state == ec_fsm_pdo_mapping_state_end;

   136

}

   137

   138

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

   139

 * State functions.

   140

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

   141

   142

/** Start mapping configuration.

   143

 */

   144

void ec_fsm_pdo_mapping_state_start(

   145

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   146

        )

   147

{

   148

    if (!fsm->slave->config) {

   149

        fsm->state = ec_fsm_pdo_mapping_state_end;

   150

        return;

   151

    }

   152

   153

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

   154

    ec_fsm_pdo_mapping_next_dir(fsm);

   155

}

   156

   157

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

   158

   159

/** Process mapping of next direction.

   160

 */

   161

void ec_fsm_pdo_mapping_next_dir(

   162

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   163

        )

   164

{

   165

    fsm->dir++;

   166

   167

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

   168

        fsm->mapping = &fsm->slave->config->mapping[fsm->dir];

   169

   170

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

   171

            if (!list_empty(&fsm->mapping->pdos)) {

   172

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

   173

                fsm->state = ec_fsm_pdo_mapping_state_end;

   174

                return;

   175

            }

   176

            continue;

   177

        }

   178

   179

        // check if mapping has to be altered

   180

        if (ec_pdo_mapping_equal(&fsm->sync->mapping, fsm->mapping))

   181

            continue;

   182

   183

        // Pdo mapping has to be changed. Does the slave support this?

   184

        if (!fsm->slave->sii.mailbox_protocols & EC_MBOX_COE

   185

                || (fsm->slave->sii.has_general

   186

                    && !fsm->slave->sii.coe_details.enable_pdo_assign)) {

   187

            EC_ERR("Slave %u does not support changing the Pdo mapping!\n",

   188

                    fsm->slave->ring_position);

   189

            fsm->state = ec_fsm_pdo_mapping_state_error;

   190

            return;

   191

        }

   192

   193

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

   194

            EC_DBG("Changing Pdo mapping for SM%u of slave %u.\n",

   195

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

   196

        }

   197

   198

        // set mapped Pdo count to zero

   199

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

   200

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

   201

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

   202

        fsm->sdodata.size = 1;

   203

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

   204

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

   205

   206

        fsm->state = ec_fsm_pdo_mapping_state_zero_count;

   207

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

   208

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

   209

        return;

   210

    }

   211

   212

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

   213

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

   214

                fsm->slave->ring_position);

   215

    fsm->state = ec_fsm_pdo_mapping_state_end;

   216

}

   217

   218

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

   219

   220

/** Process mapping of next Pdo.

   221

 */

   222

ec_pdo_t *ec_fsm_pdo_mapping_next_pdo(

   223

        const ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */

   224

        const struct list_head *list /**< current Pdo list item */

   225

        )

   226

{

   227

    list = list->next; 

   228

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

   229

        return NULL; // no next Pdo

   230

    return list_entry(list, ec_pdo_t, list);

   231

}

   232

   233

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

   234

   235

/** Add a Pdo to the mapping.

   236

 */

   237

void ec_fsm_pdo_mapping_add_pdo(

   238

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   239

        )

   240

{

   241

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

   242

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

   243

    fsm->sdodata.size = 2;

   244

   245

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

   246

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

   247

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

   248

   249

    fsm->state = ec_fsm_pdo_mapping_state_add_pdo;

   250

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

   251

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

   252

}

   253

   254

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

   255

   256

/** Set the number of mapped Pdos to zero.

   257

 */

   258

void ec_fsm_pdo_mapping_state_zero_count(

   259

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   260

        )

   261

{

   262

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

   263

   264

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

   265

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

   266

                fsm->slave->ring_position);

   267

        fsm->state = ec_fsm_pdo_mapping_state_error;

   268

        return;

   269

    }

   270

   271

    // map all Pdos belonging to the current sync manager

   272

   273

    // find first Pdo

   274

    if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->mapping->pdos))) {

   275

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

   276

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

   277

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

   278

        ec_fsm_pdo_mapping_next_dir(fsm);

   279

        return;

   280

    }

   281

   282

    // add first Pdo to mapping

   283

    fsm->pdo_count = 1;

   284

    ec_fsm_pdo_mapping_add_pdo(fsm);

   285

}

   286

   287

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

   288

   289

/** Add a Pdo to the sync managers mapping.

   290

 */

   291

void ec_fsm_pdo_mapping_state_add_pdo(

   292

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   293

        )

   294

{

   295

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

   296

   297

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

   298

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

   299

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

   300

        fsm->state = ec_fsm_pdo_mapping_state_error;

   301

        return;

   302

    }

   303

   304

    // find next Pdo

   305

    if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) {

   306

        // no more Pdos to map. write Pdo count

   307

        fsm->sdodata.subindex = 0;

   308

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

   309

        fsm->sdodata.size = 1;

   310

   311

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

   312

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

   313

                    fsm->pdo_count);

   314

   315

        fsm->state = ec_fsm_pdo_mapping_state_pdo_count;

   316

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

   317

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

   318

        return;

   319

    }

   320

   321

    // add next Pdo to mapping

   322

    fsm->pdo_count++;

   323

    ec_fsm_pdo_mapping_add_pdo(fsm);

   324

}

   325

   326

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

   327

   328

/** Set the number of mapped Pdos.

   329

 */

   330

void ec_fsm_pdo_mapping_state_pdo_count(

   331

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   332

        )

   333

{

   334

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

   335

   336

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

   337

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

   338

                fsm->slave->ring_position);

   339

        fsm->state = ec_fsm_pdo_mapping_state_error;

   340

        return;

   341

    }

   342

   343

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

   344

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

   345

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

   346

   347

    // mapping configuration for this direction finished

   348

    ec_fsm_pdo_mapping_next_dir(fsm);

   349

}

   350

   351

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

   352

 * Common state functions

   353

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

   354

   355

/** State: ERROR.

   356

 */

   357

void ec_fsm_pdo_mapping_state_error(

   358

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   359

        )

   360

{

   361

}

   362

   363

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

   364

   365

/** State: END.

   366

 */

   367

void ec_fsm_pdo_mapping_state_end(

   368

        ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   369

        )

   370

{

   371

}

   372

   373

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