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

   Canopen-over-EtherCAT functions.

    37

*/

    38

    39

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

    40

    41

#include <linux/delay.h>

    42

#include <linux/slab.h>

    43

#include <linux/module.h>

    44

    45

#include "master.h"

    46

#include "mailbox.h"

    47

    48

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

    49

    50

void ec_canopen_abort_msg(uint32_t);

    51

int ec_slave_fetch_sdo_descriptions(ec_slave_t *, ec_command_t *);

    52

int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_command_t *,

    53

                               ec_sdo_t *, uint8_t);

    54

    55

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

    56

    57

const ec_code_msg_t sdo_abort_messages[];

    58

    59

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

    60

    61

/**

    62

   Reads 32 bit of a CANopen SDO in expedited mode.

    63

   \return 0 in case of success, else < 0

    64

*/

    65

    66

int ec_slave_sdo_read_exp(ec_slave_t *slave, /**< EtherCAT slave */

    67

                          uint16_t sdo_index, /**< SDO index */

    68

                          uint8_t sdo_subindex, /**< SDO subindex */

    69

                          uint8_t *target /**< 4-byte memory */

    70

                          )

    71

{

    72

    ec_command_t command;

    73

    size_t rec_size;

    74

    uint8_t *data;

    75

    76

    ec_command_init(&command);

    77

    78

    if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 6)))

    79

        goto err;

    80

    81

    EC_WRITE_U16(data, 0x2 << 12); // SDO request

    82

    EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer

    83

                            | 0x2 << 5)); // initiate upload request

    84

    EC_WRITE_U16(data + 3, sdo_index);

    85

    EC_WRITE_U8 (data + 5, sdo_subindex);

    86

    87

    if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size)))

    88

        goto err;

    89

    90

    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

    91

        EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

    92

        EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",

    93

               sdo_index, sdo_subindex, slave->ring_position);

    94

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

    95

        goto err;

    96

    }

    97

    98

    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response

    99

        EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response

   100

        EC_READ_U16(data + 3) != sdo_index || // index

   101

        EC_READ_U8 (data + 5) != sdo_subindex) { // subindex

   102

        EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo_index, sdo_subindex);

   103

        EC_ERR("Invalid SDO upload response at slave %i!\n",

   104

               slave->ring_position);

   105

        ec_print_data(data, rec_size);

   106

        goto err;

   107

    }

   108

   109

    memcpy(target, data + 6, 4);

   110

   111

    ec_command_clear(&command);

   112

    return 0;

   113

 err:

   114

    ec_command_clear(&command);

   115

    return -1;

   116

}

   117

   118

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

   119

   120

/**

   121

   Writes a CANopen SDO using expedited mode.

   122

   \return 0 in case of success, else < 0

   123

*/

   124

   125

int ec_slave_sdo_write_exp(ec_slave_t *slave, /**< EtherCAT slave */

   126

                           uint16_t sdo_index, /**< SDO index */

   127

                           uint8_t sdo_subindex, /**< SDO subindex */

   128

                           const uint8_t *sdo_data, /**< new value */

   129

                           size_t size /**< Data size in bytes (1 - 4) */

   130

                           )

   131

{

   132

    uint8_t *data;

   133

    size_t rec_size;

   134

    ec_command_t command;

   135

   136

    ec_command_init(&command);

   137

   138

    if (size == 0 || size > 4) {

   139

        EC_ERR("Invalid data size!\n");

   140

        goto err;

   141

    }

   142

   143

    if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 0x0A)))

   144

        goto err;

   145

   146

    EC_WRITE_U16(data, 0x2 << 12); // SDO request

   147

    EC_WRITE_U8 (data + 2, (0x1 // size specified

   148

                            | 0x1 << 1 // expedited transfer

   149

                            | (4 - size) << 2 // data set size

   150

                            | 0x1 << 5)); // initiate download request

   151

    EC_WRITE_U16(data + 3, sdo_index);

   152

    EC_WRITE_U8 (data + 5, sdo_subindex);

   153

    memcpy(data + 6, sdo_data, size);

   154

    if (size < 4) memset(data + 6 + size, 0x00, 4 - size);

   155

   156

    if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size)))

   157

        goto err;

   158

   159

    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

   160

        EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

   161

        EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n",

   162

               sdo_index, sdo_subindex, size, slave->ring_position);

   163

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   164

        return -1;

   165

    }

   166

   167

    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response

   168

        EC_READ_U8 (data + 2) >> 5 != 0x3 || // download response

   169

        EC_READ_U16(data + 3) != sdo_index || // index

   170

        EC_READ_U8 (data + 5) != sdo_subindex) { // subindex

   171

        EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n",

   172

               sdo_index, sdo_subindex, size);

   173

        EC_ERR("Invalid SDO download response at slave %i!\n",

   174

               slave->ring_position);

   175

        ec_print_data(data, rec_size);

   176

        return -1;

   177

    }

   178

   179

    ec_command_clear(&command);

   180

    return 0;

   181

 err:

   182

    ec_command_clear(&command);

   183

    return -1;

   184

}

   185

   186

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

   187

   188

/**

   189

   Reads a CANopen SDO in normal mode.

   190

   \return 0 in case of success, else < 0

   191

   \ingroup RealtimeInterface

   192

   \todo Make size non-pointer.

   193

*/

   194

   195

int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT slave */

   196

                        uint16_t sdo_index, /**< SDO index */

   197

                        uint8_t sdo_subindex, /**< SDO subindex */

   198

                        uint8_t *target, /**< memory for value */

   199

                        size_t *size /**< target memory size */

   200

                        )

   201

{

   202

    uint8_t *data;

   203

    size_t rec_size, data_size;

   204

    uint32_t complete_size;

   205

    ec_command_t command;

   206

   207

    ec_command_init(&command);

   208

   209

    if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 6)))

   210

        goto err;

   211

   212

    EC_WRITE_U16(data, 0x2 << 12); // SDO request

   213

    EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request

   214

    EC_WRITE_U16(data + 3, sdo_index);

   215

    EC_WRITE_U8 (data + 5, sdo_subindex);

   216

   217

    if (!(data = ec_slave_mbox_simple_io(slave, &command, &rec_size)))

   218

        goto err;

   219

   220

    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

   221

        EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

   222

        EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",

   223

               sdo_index, sdo_subindex, slave->ring_position);

   224

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   225

        goto err;

   226

    }

   227

   228

    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response

   229

        EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response

   230

        EC_READ_U16(data + 3) != sdo_index || // index

   231

        EC_READ_U8 (data + 5) != sdo_subindex) { // subindex

   232

        EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo_index, sdo_subindex);

   233

        EC_ERR("Invalid SDO upload response at slave %i!\n",

   234

               slave->ring_position);

   235

        ec_print_data(data, rec_size);

   236

        goto err;

   237

    }

   238

   239

    if (rec_size < 10) {

   240

        EC_ERR("Received currupted SDO upload response!\n");

   241

        ec_print_data(data, rec_size);

   242

        goto err;

   243

    }

   244

   245

    if ((complete_size = EC_READ_U32(data + 6)) > *size) {

   246

        EC_ERR("SDO data does not fit into buffer (%i / %i)!\n",

   247

               complete_size, *size);

   248

        goto err;

   249

    }

   250

   251

    data_size = rec_size - 10;

   252

   253

    if (data_size != complete_size) {

   254

        EC_ERR("SDO data incomplete - Fragmenting not implemented.\n");

   255

        goto err;

   256

    }

   257

   258

    memcpy(target, data + 10, data_size);

   259

   260

    ec_command_clear(&command);

   261

    return 0;

   262

 err:

   263

    ec_command_clear(&command);

   264

    return -1;

   265

}

   266

   267

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

   268

   269

/**

   270

   Fetches the SDO dictionary of a slave.

   271

   \return 0 in case of success, else < 0

   272

*/

   273

   274

int ec_slave_fetch_sdo_list(ec_slave_t *slave /**< EtherCAT slave */)

   275

{

   276

    uint8_t *data;

   277

    size_t rec_size;

   278

    unsigned int i, sdo_count;

   279

    ec_sdo_t *sdo;

   280

    uint16_t sdo_index;

   281

    ec_command_t command;

   282

   283

    ec_command_init(&command);

   284

   285

    if (!(data = ec_slave_mbox_prepare_send(slave, &command, 0x03, 8)))

   286

        goto err;

   287

   288

    EC_WRITE_U16(data, 0x8 << 12); // SDO information

   289

    EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request

   290

    EC_WRITE_U8 (data + 3, 0x00);

   291

    EC_WRITE_U16(data + 4, 0x0000);

   292

    EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!

   293

   294

    if (unlikely(ec_master_simple_io(slave->master, &command))) {

   295

        EC_ERR("Mailbox checking failed on slave %i!\n", slave->ring_position);

   296

        goto err;

   297

    }

   298

   299

    do {

   300

        if (!(data = ec_slave_mbox_simple_receive(slave, &command,

   301

                                                  0x03, &rec_size)))

   302

            goto err;

   303

   304

        if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information

   305

            (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response

   306

            EC_ERR("SDO information error response at slave %i!\n",

   307

                   slave->ring_position);

   308

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   309

            goto err;

   310

        }

   311

   312

        if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information

   313

            (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response

   314

            EC_ERR("Invalid SDO list response at slave %i!\n",

   315

                   slave->ring_position);

   316

            ec_print_data(data, rec_size);

   317

            goto err;

   318

        }

   319

   320

        if (rec_size < 8) {

   321

            EC_ERR("Invalid data size!\n");

   322

            ec_print_data(data, rec_size);

   323

            goto err;

   324

        }

   325

   326

        sdo_count = (rec_size - 8) / 2;

   327

        for (i = 0; i < sdo_count; i++) {

   328

            sdo_index = EC_READ_U16(data + 8 + i * 2);

   329

            if (!sdo_index) continue; // sometimes index is 0... ???

   330

   331

            if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) {

   332

                EC_ERR("Failed to allocate memory for SDO!\n");

   333

                goto err;

   334

            }

   335

   336

            // Initialize SDO object

   337

            sdo->index = sdo_index;

   338

            //sdo->unkown = 0x0000;

   339

            sdo->object_code = 0x00;

   340

            sdo->name = NULL;

   341

            INIT_LIST_HEAD(&sdo->entries);

   342

   343

            list_add_tail(&sdo->list, &slave->sdo_dictionary);

   344

        }

   345

    }

   346

    while (EC_READ_U8(data + 2) & 0x80);

   347

   348

    // Fetch all SDO descriptions

   349

    if (ec_slave_fetch_sdo_descriptions(slave, &command)) goto err;

   350

   351

    ec_command_clear(&command);

   352

    return 0;

   353

 err:

   354

    ec_command_clear(&command);

   355

    return -1;

   356

}

   357

   358

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

   359

   360

/**

   361

   Fetches the SDO descriptions for the known SDOs.

   362

   \return 0 in case of success, else < 0

   363

*/

   364

   365

int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave, /**< EtherCAT slave */

   366

                                    ec_command_t *command /**< command */

   367

                                    )

   368

{

   369

    uint8_t *data;

   370

    size_t rec_size, name_size;

   371

    ec_sdo_t *sdo;

   372

   373

    list_for_each_entry(sdo, &slave->sdo_dictionary, list) {

   374

        if (!(data = ec_slave_mbox_prepare_send(slave, command, 0x03, 8)))

   375

            return -1;

   376

        EC_WRITE_U16(data, 0x8 << 12); // SDO information

   377

        EC_WRITE_U8 (data + 2, 0x03); // Get object description request

   378

        EC_WRITE_U8 (data + 3, 0x00);

   379

        EC_WRITE_U16(data + 4, 0x0000);

   380

        EC_WRITE_U16(data + 6, sdo->index); // SDO index

   381

   382

        if (!(data = ec_slave_mbox_simple_io(slave, command, &rec_size)))

   383

            return -1;

   384

   385

        if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information

   386

            (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response

   387

            EC_ERR("SDO information error response at slave %i while"

   388

                   " fetching SDO 0x%04X!\n", slave->ring_position,

   389

                   sdo->index);

   390

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   391

            return -1;

   392

        }

   393

   394

        if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information

   395

            (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response

   396

            EC_READ_U16(data + 6) != sdo->index) { // SDO index

   397

            EC_ERR("Invalid object description response at slave %i while"

   398

                   " fetching SDO 0x%04X!\n", slave->ring_position,

   399

                   sdo->index);

   400

            ec_print_data(data, rec_size);

   401

            return -1;

   402

        }

   403

   404

        if (rec_size < 12) {

   405

            EC_ERR("Invalid data size!\n");

   406

            ec_print_data(data, rec_size);

   407

            return -1;

   408

        }

   409

   410

#if 0

   411

        EC_DBG("object desc response:\n");

   412

        ec_print_data(data, rec_size);

   413

#endif

   414

   415

        //sdo->unknown = EC_READ_U16(data + 8);

   416

        sdo->object_code = EC_READ_U8(data + 11);

   417

   418

        name_size = rec_size - 12;

   419

        if (name_size) {

   420

            if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) {

   421

                EC_ERR("Failed to allocate SDO name!\n");

   422

                return -1;

   423

            }

   424

   425

            memcpy(sdo->name, data + 12, name_size);

   426

            sdo->name[name_size] = 0;

   427

        }

   428

   429

        if (EC_READ_U8(data + 2) & 0x80) {

   430

            EC_ERR("Fragment follows (not implemented)!\n");

   431

            return -1;

   432

        }

   433

   434

        // Fetch all entries (subindices)

   435

        if (ec_slave_fetch_sdo_entries(slave, command, sdo,

   436

                                       EC_READ_U8(data + 10)))

   437

            return -1;

   438

    }

   439

   440

    return 0;

   441

}

   442

   443

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

   444

   445

/**

   446

   Fetches all entries (subindices) to an SDO.

   447

   \return 0 in case of success, else < 0

   448

*/

   449

   450

int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT slave */

   451

                               ec_command_t *command, /**< command */

   452

                               ec_sdo_t *sdo, /**< SDO */

   453

                               uint8_t subindices /**< number of subindices */

   454

                               )

   455

{

   456

    uint8_t *data;

   457

    size_t rec_size, data_size;

   458

    uint8_t i;

   459

    ec_sdo_entry_t *entry;

   460

   461

    for (i = 1; i <= subindices; i++) {

   462

        if (!(data = ec_slave_mbox_prepare_send(slave, command, 0x03, 10)))

   463

            return -1;

   464

   465

        EC_WRITE_U16(data, 0x8 << 12); // SDO information

   466

        EC_WRITE_U8 (data + 2, 0x05); // Get entry description request

   467

        EC_WRITE_U8 (data + 3, 0x00);

   468

        EC_WRITE_U16(data + 4, 0x0000);

   469

        EC_WRITE_U16(data + 6, sdo->index); // SDO index

   470

        EC_WRITE_U8 (data + 8, i); // SDO subindex

   471

        EC_WRITE_U8 (data + 9, 0x00); // value info (no values)

   472

   473

        if (!(data = ec_slave_mbox_simple_io(slave, command, &rec_size)))

   474

            return -1;

   475

   476

        if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information

   477

            (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response

   478

            EC_ERR("SDO information error response at slave %i while"

   479

                   " fetching SDO entry 0x%04X:%i!\n", slave->ring_position,

   480

                   sdo->index, i);

   481

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   482

            return -1;

   483

        }

   484