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

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

   184

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

   185

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

   186

        }

   187

   188

        // set mapped Pdo count to zero

   189

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

   190

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

   191

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

   192

        fsm->sdodata.size = 1;

   193

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

   194

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

   195

   196

        fsm->state = ec_fsm_pdo_mapping_state_zero_count;

   197

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

   198

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

   199

        return;

   200

    }

   201

   202

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

   203

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

   204

                fsm->slave->ring_position);

   205

    fsm->state = ec_fsm_pdo_mapping_state_end;

   206

}

   207

   208

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

   209

   210

/** Process mapping of next Pdo.

   211

 */

   212

ec_pdo_t *ec_fsm_pdo_mapping_next_pdo(

   213

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

   214

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

   215

        )

   216

{

   217

    list = list->next; 

   218

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

   219

        return NULL; // no next Pdo

   220

    return list_entry(list, ec_pdo_t, list);

   221

}

   222

   223

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

   224

   225

/** Add a Pdo to the mapping.

   226

 */

   227

void ec_fsm_pdo_mapping_add_pdo(

   228

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

   229

        )

   230

{

   231

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

   232

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

   233

    fsm->sdodata.size = 2;

   234

   235

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

   236

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

   237

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

   238

   239

    fsm->state = ec_fsm_pdo_mapping_state_add_pdo;

   240

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

   241

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

   242

}

   243

   244

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

   245

   246

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

   247

 */

   248

void ec_fsm_pdo_mapping_state_zero_count(

   249

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

   250

        )

   251

{

   252

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

   253

   254

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

   255

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

   256

                fsm->slave->ring_position);

   257

        fsm->state = ec_fsm_pdo_mapping_state_error;

   258

        return;

   259

    }

   260

   261

    // map all Pdos belonging to the current sync manager

   262

   263

    // find first Pdo

   264

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

   265

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

   266

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

   267

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

   268

        ec_fsm_pdo_mapping_next_dir(fsm);

   269

        return;

   270

    }

   271

   272

    // add first Pdo to mapping

   273

    fsm->pdo_count = 1;

   274

    ec_fsm_pdo_mapping_add_pdo(fsm);

   275

}

   276

   277

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

   278

   279

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

   280

 */

   281

void ec_fsm_pdo_mapping_state_add_pdo(

   282

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

   283

        )

   284

{

   285

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

   286

   287

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

   288

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

   289

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

   290

        fsm->state = ec_fsm_pdo_mapping_state_error;

   291

        return;

   292

    }

   293

   294

    // find next Pdo

   295

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

   296

        // no more Pdos to map. write Pdo count

   297

        fsm->sdodata.subindex = 0;

   298

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

   299

        fsm->sdodata.size = 1;

   300

   301

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

   302

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

   303

                    fsm->pdo_count);

   304

   305

        fsm->state = ec_fsm_pdo_mapping_state_pdo_count;

   306

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

   307

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

   308

        return;

   309

    }

   310

   311

    // add next Pdo to mapping

   312

    fsm->pdo_count++;

   313

    ec_fsm_pdo_mapping_add_pdo(fsm);

   314

}

   315

   316

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

   317

   318

/** Set the number of mapped Pdos.

   319

 */

   320

void ec_fsm_pdo_mapping_state_pdo_count(

   321

        ec_fsm_pdo_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_pdo_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 direction finished

   338

    ec_fsm_pdo_mapping_next_dir(fsm);

   339

}

   340

   341

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

   342

 * Common state functions

   343

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

   344

   345

/** State: ERROR.

   346

 */

   347

void ec_fsm_pdo_mapping_state_error(

   348

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

   349

        )

   350

{

   351

}

   352

   353

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

   354

   355

/** State: END.

   356

 */

   357

void ec_fsm_pdo_mapping_state_end(

   358

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

   359

        )

   360

{

   361

}

   362

   363

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