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

/**

    35

   \file

    36

   EtherCAT Pdo information state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "sdo_request.h"

    45

#include "fsm_coe_map.h"

    46

    47

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

    48

    49

void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);

    50

void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);

    51

void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);

    52

void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *);

    53

void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);

    54

    55

void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);

    56

void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);

    57

    58

void ec_fsm_coe_map_action_next_dir(ec_fsm_coe_map_t *);

    59

void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);

    60

void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);

    61

    62

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

    63

    64

/**

    65

   Constructor.

    66

*/

    67

    68

void ec_fsm_coe_map_init(

    69

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

    70

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

    71

        )

    72

{

    73

    fsm->fsm_coe = fsm_coe;

    74

    fsm->state = NULL;

    75

    ec_sdo_request_init(&fsm->request);

    76

    ec_pdo_list_init(&fsm->pdos);

    77

}

    78

    79

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

    80

    81

/**

    82

   Destructor.

    83

*/

    84

    85

void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)

    86

{

    87

    ec_sdo_request_clear(&fsm->request);

    88

    ec_pdo_list_clear(&fsm->pdos);

    89

}

    90

    91

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

    92

    93

/**

    94

   Starts to upload an Sdo from a slave.

    95

*/

    96

    97

void ec_fsm_coe_map_start(

    98

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

    99

        ec_slave_t *slave /**< EtherCAT slave */

   100

        )

   101

{

   102

    fsm->slave = slave;

   103

    fsm->state = ec_fsm_coe_map_state_start;

   104

}

   105

   106

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

   107

   108

/**

   109

   Executes the current state of the state machine.

   110

   \return false, if state machine has terminated

   111

*/

   112

   113

int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   114

{

   115

    fsm->state(fsm);

   116

   117

    return fsm->state != ec_fsm_coe_map_state_end

   118

        && fsm->state != ec_fsm_coe_map_state_error;

   119

}

   120

   121

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

   122

   123

/**

   124

   Returns, if the state machine terminated with success.

   125

   \return non-zero if successful.

   126

*/

   127

   128

int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */)

   129

{

   130

    return fsm->state == ec_fsm_coe_map_state_end;

   131

}

   132

   133

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

   134

 *  state functions

   135

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

   136

   137

/**

   138

 * Start reading Pdo assignment.

   139

 */

   140

   141

void ec_fsm_coe_map_state_start(

   142

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   143

        )

   144

{

   145

    // read Pdo assignment for first direction

   146

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

   147

    ec_fsm_coe_map_action_next_dir(fsm);

   148

}

   149

   150

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

   151

   152

/**

   153

 * Read Pdo assignment of next direction manager.

   154

 */

   155

   156

void ec_fsm_coe_map_action_next_dir(

   157

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   158

        )

   159

{

   160

    ec_slave_t *slave = fsm->slave;

   161

   162

    fsm->dir++;

   163

   164

    if (slave->master->debug_level)

   165

        EC_DBG("Processing dir %u of slave %u.\n",

   166

                fsm->dir, slave->ring_position);

   167

   168

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

   169

   170

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

   171

            if (slave->master->debug_level)

   172

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

   173

            continue;

   174

        }

   175

   176

        fsm->sync_sdo_index = 0x1C10 + fsm->sync->index;

   177

   178

        if (slave->master->debug_level)

   179

            EC_DBG("Reading Pdo assignment of sync manager %u of slave %u.\n",

   180

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

   181

   182

        ec_pdo_list_clear_pdos(&fsm->pdos);

   183

   184

        ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);

   185

        fsm->request.response_timeout = 10000;

   186

        ecrt_sdo_request_read(&fsm->request);

   187

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   188

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

   189

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

   190

        return;

   191

    }

   192

   193

    if (slave->master->debug_level)

   194

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

   195

                slave->ring_position);

   196

   197

    fsm->state = ec_fsm_coe_map_state_end;

   198

}

   199

   200

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

   201

   202

/**

   203

 * Count assigned Pdos.

   204

 */

   205

   206

void ec_fsm_coe_map_state_pdo_count(

   207

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   208

        )

   209

{

   210

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

   211

   212

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

   213

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

   214

                fsm->slave->ring_position);

   215

        fsm->state = ec_fsm_coe_map_state_error;

   216

        return;

   217

    }

   218

   219

    if (fsm->request.data_size != sizeof(uint8_t)) {

   220

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   221

                "from slave %u.\n", fsm->request.data_size,

   222

                fsm->request.index, fsm->request.subindex,

   223

                fsm->slave->ring_position);

   224

        fsm->state = ec_fsm_coe_map_state_error;

   225

        return;

   226

    }

   227

    fsm->sync_subindices = EC_READ_U8(fsm->request.data);

   228

   229

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

   230

        EC_DBG("  %u Pdos assigned.\n", fsm->sync_subindices);

   231

   232

    // read first Pdo

   233

    fsm->sync_subindex = 1;

   234

    ec_fsm_coe_map_action_next_pdo(fsm);

   235

}

   236

   237

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

   238

   239

/**

   240

 * Read next Pdo.

   241

 */

   242

   243

void ec_fsm_coe_map_action_next_pdo(

   244

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   245

        )

   246

{

   247

    if (fsm->sync_subindex <= fsm->sync_subindices) {

   248

        ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index,

   249

                fsm->sync_subindex);

   250

        fsm->request.response_timeout = 0;

   251

        ecrt_sdo_request_read(&fsm->request);

   252

        fsm->state = ec_fsm_coe_map_state_pdo;

   253

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

   254

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

   255

        return;

   256

    }

   257

   258

    // finished reading Pdo assignment/mapping

   259

   260

    if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {

   261

        fsm->state = ec_fsm_coe_map_state_error;

   262

        return;

   263

    }

   264

   265

    fsm->sync->assign_source = EC_ASSIGN_COE;

   266

    ec_pdo_list_clear_pdos(&fsm->pdos);

   267

   268

    // next direction

   269

    ec_fsm_coe_map_action_next_dir(fsm);

   270

}

   271

   272

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

   273

   274

/**

   275

 * Fetch Pdo information.

   276

 */

   277

   278

void ec_fsm_coe_map_state_pdo(

   279

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   280

        )

   281

{

   282

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

   283

   284

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

   285

        EC_ERR("Failed to read assigned Pdo index from slave %u.\n",

   286

                fsm->slave->ring_position);

   287

        fsm->state = ec_fsm_coe_map_state_error;

   288

        return;

   289

    }

   290

   291

    if (fsm->request.data_size != sizeof(uint16_t)) {

   292

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   293

                "from slave %u.\n", fsm->request.data_size,

   294

                fsm->request.index, fsm->request.subindex,

   295

                fsm->slave->ring_position);

   296

        fsm->state = ec_fsm_coe_map_state_error;

   297

        return;

   298

    }

   299

   300

    if (!(fsm->pdo = (ec_pdo_t *)

   301

                kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   302

        EC_ERR("Failed to allocate Pdo.\n");

   303

        fsm->state = ec_fsm_coe_map_state_error;

   304

        return;

   305

    }

   306

   307

    ec_pdo_init(fsm->pdo);

   308

    fsm->pdo->index = EC_READ_U16(fsm->request.data);

   309

    fsm->pdo->dir = ec_sync_direction(fsm->sync);

   310

   311

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

   312

        EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);

   313

   314

    list_add_tail(&fsm->pdo->list, &fsm->pdos.list);

   315

   316

    ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);

   317

    fsm->request.response_timeout = 0;

   318

    ecrt_sdo_request_read(&fsm->request);

   319

    fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   320

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

   321

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

   322

}

   323

   324

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

   325

   326

/**

   327

 * Read number of mapped Pdo entries.

   328

 */

   329

   330

void ec_fsm_coe_map_state_pdo_entry_count(

   331

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   332

        )

   333

{

   334

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

   335

   336

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

   337

        EC_ERR("Failed to read number of mapped Pdo entries from slave %u.\n",

   338

                fsm->slave->ring_position);

   339

        fsm->state = ec_fsm_coe_map_state_error;

   340

        return;

   341

    }

   342

   343

    if (fsm->request.data_size != sizeof(uint8_t)) {

   344

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   345

                "from slave %u.\n", fsm->request.data_size,

   346

                fsm->request.index, fsm->request.subindex,

   347

                fsm->slave->ring_position);

   348

        fsm->state = ec_fsm_coe_map_state_error;

   349

        return;

   350

    }

   351

    fsm->pdo_subindices = EC_READ_U8(fsm->request.data);

   352

   353

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

   354

        EC_DBG("    %u Pdo entries mapped.\n", fsm->pdo_subindices);

   355

   356

    // read first Pdo entry

   357

    fsm->pdo_subindex = 1;

   358

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   359

}

   360

   361

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

   362

   363

/**

   364

 * Read next Pdo entry.

   365

 */

   366

   367

void ec_fsm_coe_map_action_next_pdo_entry(

   368

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   369

        )

   370

{

   371

    if (fsm->pdo_subindex <= fsm->pdo_subindices) {

   372

        ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex);

   373

        fsm->request.response_timeout = 0;

   374

        ecrt_sdo_request_read(&fsm->request);

   375

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   376

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

   377

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

   378

        return;

   379

    }

   380

   381

    // next Pdo

   382

    fsm->sync_subindex++;

   383

    ec_fsm_coe_map_action_next_pdo(fsm);

   384

}

   385

   386

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

   387

   388

/**

   389

 * Read Pdo entry information.

   390

 */

   391

   392

void ec_fsm_coe_map_state_pdo_entry(

   393

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   394

        )

   395

{

   396

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

   397

   398

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

   399

        EC_ERR("Failed to read mapped Pdo entry from slave %u.\n",

   400

                fsm->slave->ring_position);

   401

        fsm->state = ec_fsm_coe_map_state_error;

   402

        return;

   403

    }

   404

   405

    if (fsm->request.data_size != sizeof(uint32_t)) {

   406

        EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   407

                "from slave %u.\n", fsm->request.data_size,

   408

                fsm->request.index, fsm->request.subindex,

   409

                fsm->slave->ring_position);

   410

        fsm->state = ec_fsm_coe_map_state_error;

   411

    } else {

   412

        uint32_t pdo_entry_info;

   413

        ec_pdo_entry_t *pdo_entry;

   414

   415

        pdo_entry_info = EC_READ_U32(fsm->request.data);

   416

   417

        if (!(pdo_entry = (ec_pdo_entry_t *)

   418

                    kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {

   419

            EC_ERR("Failed to allocate Pdo entry.\n");

   420

            fsm->state = ec_fsm_coe_map_state_error;

   421

            return;

   422

        }

   423

   424

        ec_pdo_entry_init(pdo_entry);

   425

        pdo_entry->index = pdo_entry_info >> 16;

   426

        pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;

   427

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   428

   429

        if (!pdo_entry->index && !pdo_entry->subindex) {

   430

            if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {

   431

                ec_pdo_entry_clear(pdo_entry);

   432

                kfree(pdo_entry);

   433

                fsm->state = ec_fsm_coe_map_state_error;

   434

                return;

   435

            }

   436

        }

   437

   438

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

   439

            EC_DBG("    Pdo entry 0x%04X:%02X, %u bit,  \"%s\".\n",

   440

                    pdo_entry->index, pdo_entry->subindex,

   441

                    pdo_entry->bit_length,

   442

                    pdo_entry->name ? pdo_entry->name : "???");

   443

        }

   444

   445

        // next Pdo entry

   446

        list_add_tail(&pdo_entry->list, &fsm->pdo->entries);

   447

        fsm->pdo_subindex++;

   448

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   449

    }

   450

}

   451

   452

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

   453

   454

/**

   455

   State: ERROR.

   456

*/

   457

   458

void ec_fsm_coe_map_state_error(

   459

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   460

        )

   461

{

   462

}

   463

   464

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

   465

   466

/**

   467

   State: END.

   468

*/

   469

   470

void ec_fsm_coe_map_state_end(

   471

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   472

        )

   473

{

   474

}

   475

   476

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