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 assignment 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_assign.h"

    46

    47

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

    48

    49

void ec_fsm_pdo_assign_state_start(ec_fsm_pdo_assign_t *);

    50

void ec_fsm_pdo_assign_state_zero_count(ec_fsm_pdo_assign_t *);

    51

void ec_fsm_pdo_assign_state_add_pdo(ec_fsm_pdo_assign_t *);

    52

void ec_fsm_pdo_assign_state_pdo_count(ec_fsm_pdo_assign_t *);

    53

void ec_fsm_pdo_assign_state_end(ec_fsm_pdo_assign_t *);

    54

void ec_fsm_pdo_assign_state_error(ec_fsm_pdo_assign_t *);

    55

    56

void ec_fsm_pdo_assign_next_dir(ec_fsm_pdo_assign_t *);

    57

    58

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

    59

    60

/** Constructor.

    61

 */

    62

void ec_fsm_pdo_assign_init(

    63

        ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */

    64

        ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */

    65

        )

    66

{

    67

    fsm->fsm_coe = fsm_coe;

    68

    ec_sdo_request_init(&fsm->request);

    69

}

    70

    71

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

    72

    73

/** Destructor.

    74

 */

    75

void ec_fsm_pdo_assign_clear(

    76

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

    77

        )

    78

{

    79

    ec_sdo_request_clear(&fsm->request);

    80

}

    81

    82

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

    83

    84

/** Start Pdo assignment state machine.

    85

 */

    86

void ec_fsm_pdo_assign_start(

    87

        ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */

    88

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

    89

        )

    90

{

    91

    fsm->slave = slave;

    92

    fsm->state = ec_fsm_pdo_assign_state_start;

    93

}

    94

    95

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

    96

    97

/** Get running state.

    98

 *

    99

 * \return false, if state machine has terminated

   100

 */

   101

int ec_fsm_pdo_assign_running(

   102

        const ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   103

        )

   104

{

   105

    return fsm->state != ec_fsm_pdo_assign_state_end

   106

        && fsm->state != ec_fsm_pdo_assign_state_error;

   107

}

   108

   109

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

   110

   111

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

   112

 *

   113

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

   114

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

   115

 *

   116

 * \return false, if state machine has terminated

   117

 */

   118

int ec_fsm_pdo_assign_exec(

   119

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   120

        )

   121

{

   122

    fsm->state(fsm);

   123

    return ec_fsm_pdo_assign_running(fsm);

   124

}

   125

   126

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

   127

   128

/** Get execution result.

   129

 *

   130

 * \return true, if the state machine terminated gracefully

   131

 */

   132

int ec_fsm_pdo_assign_success(

   133

        const ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   134

        )

   135

{

   136

    return fsm->state == ec_fsm_pdo_assign_state_end;

   137

}

   138

   139

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

   140

 * State functions.

   141

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

   142

   143

/** Start Pdo assignment.

   144

 */

   145

void ec_fsm_pdo_assign_state_start(

   146

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   147

        )

   148

{

   149

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

   150

        fsm->state = ec_fsm_pdo_assign_state_end;

   151

        return;

   152

    }

   153

   154

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

   155

    ec_fsm_pdo_assign_next_dir(fsm);

   156

}

   157

   158

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

   159

   160

/** Process Pdo assignment of next direction.

   161

 */

   162

void ec_fsm_pdo_assign_next_dir(

   163

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   164

        )

   165

{

   166

    fsm->dir++;

   167

   168

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

   169

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

   170

   171

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

   172

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

   173

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

   174

                fsm->state = ec_fsm_pdo_assign_state_end;

   175

                return;

   176

            }

   177

            continue;

   178

        }

   179

   180

        // check if assignment has to be altered

   181

        if (ec_pdo_list_equal(&fsm->sync->pdos, fsm->pdos))

   182

            continue;

   183

   184

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

   185

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

   186

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

   187

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

   188

            EC_ERR("Slave %u does not support assigning Pdos!\n",

   189

                    fsm->slave->ring_position);

   190

            fsm->state = ec_fsm_pdo_assign_state_error;

   191

            return;

   192

        }

   193

   194

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

   195

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

   196

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

   197

        }

   198

   199

        if (ec_sdo_request_alloc(&fsm->request, 2)) {

   200

            fsm->state = ec_fsm_pdo_assign_state_error;

   201

            return;

   202

        }

   203

   204

        // set mapped Pdo count to zero

   205

        EC_WRITE_U8(fsm->request.data, 0); // zero Pdos mapped

   206

        fsm->request.data_size = 1;

   207

        ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0);

   208

        ecrt_sdo_request_write(&fsm->request);

   209

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

   210

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

   211

   212

        fsm->state = ec_fsm_pdo_assign_state_zero_count;

   213

        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

   214

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

   215

        return;

   216

    }

   217

   218

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

   219

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

   220

                fsm->slave->ring_position);

   221

    fsm->state = ec_fsm_pdo_assign_state_end;

   222

}

   223

   224

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

   225

   226

/** Assign next Pdo.

   227

 */

   228

ec_pdo_t *ec_fsm_pdo_assign_next_pdo(

   229

        const ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */

   230

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

   231

        )

   232

{

   233

    list = list->next; 

   234

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

   235

        return NULL; // no next Pdo

   236

    return list_entry(list, ec_pdo_t, list);

   237

}

   238

   239

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

   240

   241

/** Assign a Pdo.

   242

 */

   243

void ec_fsm_pdo_assign_add_pdo(

   244

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   245

        )

   246

{

   247

    EC_WRITE_U16(fsm->request.data, fsm->pdo->index);

   248

    fsm->request.data_size = 2;

   249

    ec_sdo_request_address(&fsm->request,

   250

            0x1C10 + fsm->sync->index, fsm->pdo_count);

   251

    ecrt_sdo_request_write(&fsm->request);

   252

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

   253

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

   254

                fsm->pdo->index, fsm->pdo_count);

   255

   256

    fsm->state = ec_fsm_pdo_assign_state_add_pdo;

   257

    ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

   258

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

   259

}

   260

   261

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

   262

   263

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

   264

 */

   265

void ec_fsm_pdo_assign_state_zero_count(

   266

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   267

        )

   268

{

   269

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

   270

   271

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

   272

        EC_ERR("Failed to clear Pdo assignment of slave %u.\n",

   273

                fsm->slave->ring_position);

   274

        fsm->state = ec_fsm_pdo_assign_state_error;

   275

        return;

   276

    }

   277

   278

    // assign all Pdos belonging to the current sync manager

   279

   280

    // find first Pdo

   281

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

   282

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

   283

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

   284

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

   285

        ec_fsm_pdo_assign_next_dir(fsm);

   286

        return;

   287

    }

   288

   289

    // assign first Pdo

   290

    fsm->pdo_count = 1;

   291

    ec_fsm_pdo_assign_add_pdo(fsm);

   292

}

   293

   294

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

   295

   296

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

   297

 */

   298

void ec_fsm_pdo_assign_state_add_pdo(

   299

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   300

        )

   301

{

   302

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

   303

   304

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

   305

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

   306

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

   307

        fsm->state = ec_fsm_pdo_assign_state_error;

   308

        return;

   309

    }

   310

   311

    // find next Pdo

   312

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

   313

        // no more Pdos to map. write Pdo count

   314

        EC_WRITE_U8(fsm->request.data, fsm->pdo_count);

   315

        fsm->request.data_size = 1;

   316

        ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0);

   317

        ecrt_sdo_request_write(&fsm->request);

   318

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

   319

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

   320

                    fsm->pdo_count);

   321

   322

        fsm->state = ec_fsm_pdo_assign_state_pdo_count;

   323

        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

   324

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

   325

        return;

   326

    }

   327

   328

    // add next Pdo to assignment

   329

    fsm->pdo_count++;

   330

    ec_fsm_pdo_assign_add_pdo(fsm);

   331

}

   332

   333

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

   334

   335

/** Set the number of assigned Pdos.

   336

 */

   337

void ec_fsm_pdo_assign_state_pdo_count(

   338

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   339

        )

   340

{

   341

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

   342

   343

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

   344

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

   345

                fsm->slave->ring_position);

   346

        fsm->state = ec_fsm_pdo_assign_state_error;

   347

        return;

   348

    }

   349

   350

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

   351

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

   352

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

   353

   354

    // assignment for this direction finished

   355

    ec_fsm_pdo_assign_next_dir(fsm);

   356

}

   357

   358

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

   359

 * Common state functions

   360

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

   361

   362

/** State: ERROR.

   363

 */

   364

void ec_fsm_pdo_assign_state_error(

   365

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   366

        )

   367

{

   368

}

   369

   370

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

   371

   372

/** State: END.

   373

 */

   374

void ec_fsm_pdo_assign_state_end(

   375

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   376

        )

   377

{

   378

}

   379

   380

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