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

    fsm->num_configured_dirs = 0;

   156

    ec_fsm_pdo_assign_next_dir(fsm);

   157

}

   158

   159

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

   160

   161

/** Process Pdo assignment of next direction.

   162

 */

   163

void ec_fsm_pdo_assign_next_dir(

   164

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

   165

        )

   166

{

   167

    fsm->dir++;

   168

   169

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

   170

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

   171

   172

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

   173

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

   174

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

   175

                fsm->state = ec_fsm_pdo_assign_state_end;

   176

                return;

   177

            }

   178

            continue;

   179

        }

   180

   181

        // check if assignment has to be altered

   182

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

   183

            continue;

   184

   185

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

   186

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

   187

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

   188

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

   189

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

   190

                    fsm->slave->ring_position);

   191

            fsm->state = ec_fsm_pdo_assign_state_error;

   192

            return;

   193

        }

   194

   195

        fsm->num_configured_dirs++;

   196

   197

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

   198

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

   199

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

   200

        }

   201

   202

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

   203

            fsm->state = ec_fsm_pdo_assign_state_error;

   204

            return;

   205

        }

   206

   207

        // set mapped Pdo count to zero

   208

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

   209

        fsm->request.data_size = 1;

   210

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

   211

        ecrt_sdo_request_write(&fsm->request);

   212

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

   213

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

   214

   215

        fsm->state = ec_fsm_pdo_assign_state_zero_count;

   216

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

   217

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

   218

        return;

   219

    }

   220

   221

    if (fsm->slave->master->debug_level && !fsm->num_configured_dirs)

   222

        EC_DBG("Pdo assignments of slave %u are already configured"

   223

                " correctly.\n", fsm->slave->ring_position);

   224

    fsm->state = ec_fsm_pdo_assign_state_end;

   225

}

   226

   227

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

   228

   229

/** Assign next Pdo.

   230

 */

   231

ec_pdo_t *ec_fsm_pdo_assign_next_pdo(

   232

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

   233

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

   234

        )

   235

{

   236

    list = list->next; 

   237

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

   238

        return NULL; // no next Pdo

   239

    return list_entry(list, ec_pdo_t, list);

   240

}

   241

   242

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

   243

   244

/** Assign a Pdo.

   245

 */

   246

void ec_fsm_pdo_assign_add_pdo(

   247

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

   248

        )

   249

{

   250

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

   251

    fsm->request.data_size = 2;

   252

    ec_sdo_request_address(&fsm->request,

   253

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

   254

    ecrt_sdo_request_write(&fsm->request);

   255

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

   256

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

   257

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

   258

   259

    fsm->state = ec_fsm_pdo_assign_state_add_pdo;

   260

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

   261

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

   262

}

   263

   264

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

   265

   266

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

   267

 */

   268

void ec_fsm_pdo_assign_state_zero_count(

   269

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

   270

        )

   271

{

   272

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

   273

   274

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

   275

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

   276

                fsm->slave->ring_position);

   277

        fsm->state = ec_fsm_pdo_assign_state_error;

   278

        return;

   279

    }

   280

   281

    // assign all Pdos belonging to the current sync manager

   282

   283

    // find first Pdo

   284

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

   285

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

   286

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

   287

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

   288

        ec_fsm_pdo_assign_next_dir(fsm);

   289

        return;

   290

    }

   291

   292

    // assign first Pdo

   293

    fsm->pdo_count = 1;

   294

    ec_fsm_pdo_assign_add_pdo(fsm);

   295

}

   296

   297

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

   298

   299

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

   300

 */

   301

void ec_fsm_pdo_assign_state_add_pdo(

   302

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

   303

        )

   304

{

   305

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

   306

   307

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

   308

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

   309

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

   310

        fsm->state = ec_fsm_pdo_assign_state_error;

   311

        return;

   312

    }

   313

   314

    // find next Pdo

   315

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

   316

        // no more Pdos to map. write Pdo count

   317

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

   318

        fsm->request.data_size = 1;

   319

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

   320

        ecrt_sdo_request_write(&fsm->request);

   321

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

   322

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

   323

                    fsm->pdo_count);

   324

   325

        fsm->state = ec_fsm_pdo_assign_state_pdo_count;

   326

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

   327

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

   328

        return;

   329

    }

   330

   331

    // add next Pdo to assignment

   332

    fsm->pdo_count++;

   333

    ec_fsm_pdo_assign_add_pdo(fsm);

   334

}

   335

   336

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

   337

   338

/** Set the number of assigned Pdos.

   339

 */

   340

void ec_fsm_pdo_assign_state_pdo_count(

   341

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

   342

        )

   343

{

   344

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

   345

   346

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

   347

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

   348

                fsm->slave->ring_position);

   349

        fsm->state = ec_fsm_pdo_assign_state_error;

   350

        return;

   351

    }

   352

   353

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

   354

        EC_DBG("Successfully configured Pdo assignment for SM%u of"

   355

                " slave %u.\n", fsm->sync->index, fsm->slave->ring_position);

   356

   357

    // assignment for this direction finished

   358

    ec_fsm_pdo_assign_next_dir(fsm);

   359

}

   360

   361

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

   362

 * Common state functions

   363

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

   364

   365

/** State: ERROR.

   366

 */

   367

void ec_fsm_pdo_assign_state_error(

   368

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

   369

        )

   370

{

   371

}

   372

   373

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

   374

   375

/** State: END.

   376

 */

   377

void ec_fsm_pdo_assign_state_end(

   378

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

   379

        )

   380

{

   381

}

   382

   383

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