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

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

   166

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

   167

   168

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

   169

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

   170

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

   171

                fsm->state = ec_fsm_pdo_mapping_state_end;

   172

                return;

   173

            }

   174

            continue;

   175

        }

   176

   177

        // check if mapping has to be altered

   178

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

   179

            continue;

   180

   181

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

   182

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

   183

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

   184

        }

   185

   186

        // set mapped Pdo count to zero

   187

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

   188

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

   189

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

   190

        fsm->sdodata.size = 1;

   191

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

   192

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

   193

   194

        fsm->state = ec_fsm_pdo_mapping_state_zero_count;

   195

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

   196

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

   197

        return;

   198

    }

   199

   200

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

   201

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

   202

                fsm->slave->ring_position);

   203

    fsm->state = ec_fsm_pdo_mapping_state_end;

   204

}

   205

   206

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

   207

   208

/** Process mapping of next Pdo.

   209

 */

   210

ec_pdo_t *ec_fsm_pdo_mapping_next_pdo(

   211

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

   212

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

   213

        )

   214

{

   215

    list = list->next; 

   216

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

   217

        return NULL; // no next Pdo

   218

    return list_entry(list, ec_pdo_t, list);

   219

}

   220

   221

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

   222

   223

/** Add a Pdo to the mapping.

   224

 */

   225

void ec_fsm_pdo_mapping_add_pdo(

   226

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

   227

        )

   228

{

   229

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

   230

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

   231

    fsm->sdodata.size = 2;

   232

   233

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

   234

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

   235

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

   236

   237

    fsm->state = ec_fsm_pdo_mapping_state_add_pdo;

   238

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

   239

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

   240

}

   241

   242

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

   243

   244

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

   245

 */

   246

void ec_fsm_pdo_mapping_state_zero_count(

   247

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

   248

        )

   249

{

   250

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

   251

   252

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

   253

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

   254

                fsm->slave->ring_position);

   255

        fsm->state = ec_fsm_pdo_mapping_state_error;

   256

        return;

   257

    }

   258

   259

    // map all Pdos belonging to the current sync manager

   260

   261

    // find first Pdo

   262

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

   263

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

   264

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

   265

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

   266

        ec_fsm_pdo_mapping_next_dir(fsm);

   267

        return;

   268

    }

   269

   270

    // add first Pdo to mapping

   271

    fsm->pdo_count = 1;

   272

    ec_fsm_pdo_mapping_add_pdo(fsm);

   273

}

   274

   275

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

   276

   277

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

   278

 */

   279

void ec_fsm_pdo_mapping_state_add_pdo(

   280

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

   281

        )

   282

{

   283

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

   284

   285

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

   286

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

   287

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

   288

        fsm->state = ec_fsm_pdo_mapping_state_error;

   289

        return;

   290

    }

   291

   292

    // find next Pdo

   293

    if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(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_pdo_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

    ec_fsm_pdo_mapping_add_pdo(fsm);

   312

}

   313

   314

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

   315

   316

/** Set the number of mapped Pdos.

   317

 */

   318

void ec_fsm_pdo_mapping_state_pdo_count(

   319

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

   320

        )

   321

{

   322

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

   323

   324

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

   325

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

   326

                fsm->slave->ring_position);

   327

        fsm->state = ec_fsm_pdo_mapping_state_error;

   328

        return;

   329

    }

   330

   331

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

   332

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

   333

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

   334

   335

    // mapping configuration for this direction finished

   336

    ec_fsm_pdo_mapping_next_dir(fsm);

   337

}

   338

   339

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

   340

 * Common state functions

   341

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

   342

   343

/** State: ERROR.

   344

 */

   345

void ec_fsm_pdo_mapping_state_error(

   346

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

   347

        )

   348

{

   349

}

   350

   351

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

   352

   353

/** State: END.

   354

 */

   355

void ec_fsm_pdo_mapping_state_end(

   356

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

   357

        )

   358

{

   359

}

   360

   361

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