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_sync(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->sync_index = 0xff; // next is zero

   155

    fsm->num_configured_syncs = 0;

   156

    ec_fsm_pdo_assign_next_sync(fsm);

   157

}

   158

   159

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

   160

   161

/** Process Pdo assignment of next sync manager.

   162

 */

   163

void ec_fsm_pdo_assign_next_sync(

   164

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

   165

        )

   166

{

   167

    fsm->sync_index++;

   168

   169

    for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {

   170

        fsm->pdos = &fsm->slave->config->sync_configs[fsm->sync_index].pdos;

   171

   172

        if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {

   173

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

   174

                EC_ERR("Slave %u does not provide a configuration for sync "

   175

                        "manager %u!\n", fsm->slave->ring_position,

   176

                        fsm->sync_index);

   177

                fsm->state = ec_fsm_pdo_assign_state_error;

   178

                return;

   179

            }

   180

            continue;

   181

        }

   182

   183

        // check if assignment has to be altered

   184

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

   185

            continue;

   186

   187

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

   188

            EC_DBG("Pdo assignment of SM%u differs in slave %u:\n",

   189

                    fsm->sync_index, fsm->slave->ring_position);

   190

            EC_DBG("Currently assigned Pdos: ");

   191

            ec_pdo_list_print(&fsm->sync->pdos);

   192

            printk("\n");

   193

            EC_DBG("Pdos to assign: ");

   194

            ec_pdo_list_print(fsm->pdos);

   195

            printk("\n");

   196

        }

   197

   198

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

   199

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

   200

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

   201

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

   202

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

   203

                    fsm->slave->ring_position);

   204

            fsm->state = ec_fsm_pdo_assign_state_error;

   205

            return;

   206

        }

   207

   208

        fsm->num_configured_syncs++;

   209

   210

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

   211

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

   212

                    fsm->sync_index, fsm->slave->ring_position);

   213

        }

   214

   215

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

   216

            fsm->state = ec_fsm_pdo_assign_state_error;

   217

            return;

   218

        }

   219

   220

        // set mapped Pdo count to zero

   221

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

   222

        fsm->request.data_size = 1;

   223

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

   224

        ecrt_sdo_request_write(&fsm->request);

   225

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

   226

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

   227

   228

        fsm->state = ec_fsm_pdo_assign_state_zero_count;

   229

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

   230

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

   231

        return;

   232

    }

   233

   234

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

   235

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

   236

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

   237

    fsm->state = ec_fsm_pdo_assign_state_end;

   238

}

   239

   240

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

   241

   242

/** Assign next Pdo.

   243

 */

   244

ec_pdo_t *ec_fsm_pdo_assign_next_pdo(

   245

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

   246

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

   247

        )

   248

{

   249

    list = list->next; 

   250

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

   251

        return NULL; // no next Pdo

   252

    return list_entry(list, ec_pdo_t, list);

   253

}

   254

   255

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

   256

   257

/** Assign a Pdo.

   258

 */

   259

void ec_fsm_pdo_assign_add_pdo(

   260

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

   261

        )

   262

{

   263

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

   264

    fsm->request.data_size = 2;

   265

    ec_sdo_request_address(&fsm->request,

   266

            0x1C10 + fsm->sync_index, fsm->pdo_count);

   267

    ecrt_sdo_request_write(&fsm->request);

   268

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

   269

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

   270

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

   271

   272

    fsm->state = ec_fsm_pdo_assign_state_add_pdo;

   273

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

   274

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

   275

}

   276

   277

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

   278

   279

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

   280

 */

   281

void ec_fsm_pdo_assign_state_zero_count(

   282

        ec_fsm_pdo_assign_t *fsm /**< Pdo assignment 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 clear Pdo assignment of slave %u.\n",

   289

                fsm->slave->ring_position);

   290

        fsm->state = ec_fsm_pdo_assign_state_error;

   291

        return;

   292

    }

   293

   294

    // assign all Pdos belonging to the current sync manager

   295

   296

    // find first Pdo

   297

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

   298

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

   299

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

   300

                    fsm->sync_index, fsm->slave->ring_position);

   301

        ec_fsm_pdo_assign_next_sync(fsm);

   302

        return;

   303

    }

   304

   305

    // assign first Pdo

   306

    fsm->pdo_count = 1;

   307

    ec_fsm_pdo_assign_add_pdo(fsm);

   308

}

   309

   310

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

   311

   312

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

   313

 */

   314

void ec_fsm_pdo_assign_state_add_pdo(

   315

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

   316

        )

   317

{

   318

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

   319

   320

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

   321

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

   322

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

   323

        fsm->state = ec_fsm_pdo_assign_state_error;

   324

        return;

   325

    }

   326

   327

    // find next Pdo

   328

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

   329

        // no more Pdos to map. write Pdo count

   330

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

   331

        fsm->request.data_size = 1;

   332

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

   333

        ecrt_sdo_request_write(&fsm->request);

   334

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

   335

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

   336

                    fsm->pdo_count);

   337

   338

        fsm->state = ec_fsm_pdo_assign_state_pdo_count;

   339

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

   340

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

   341

        return;

   342

    }

   343

   344

    // add next Pdo to assignment

   345

    fsm->pdo_count++;

   346

    ec_fsm_pdo_assign_add_pdo(fsm);

   347

}

   348

   349

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

   350

   351

/** Set the number of assigned Pdos.

   352

 */

   353

void ec_fsm_pdo_assign_state_pdo_count(

   354

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

   355

        )

   356

{

   357

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

   358

   359

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

   360

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

   361

                fsm->slave->ring_position);

   362

        fsm->state = ec_fsm_pdo_assign_state_error;

   363

        return;

   364

    }

   365

   366

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

   367

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

   368

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

   369

   370

    // assignment for this sync manager finished

   371

    ec_fsm_pdo_assign_next_sync(fsm);

   372

}

   373

   374

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

   375

 * Common state functions

   376

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

   377

   378

/** State: ERROR.

   379

 */

   380

void ec_fsm_pdo_assign_state_error(

   381

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

   382

        )

   383

{

   384

}

   385

   386

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

   387

   388

/** State: END.

   389

 */

   390

void ec_fsm_pdo_assign_state_end(

   391

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

   392

        )

   393

{

   394

}

   395

   396

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