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_datagram_t *);

    52

int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_datagram_t *,

    53

                               ec_sdo_t *, uint8_t);

    54

    55

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

    56

    57

/**

    58

   Reads 32 bit of a CANopen SDO in expedited mode.

    59

   \return 0 in case of success, else < 0

    60

*/

    61

    62

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

    63

                          uint16_t sdo_index, /**< SDO index */

    64

                          uint8_t sdo_subindex, /**< SDO subindex */

    65

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

    66

                          )

    67

{

    68

    ec_datagram_t datagram;

    69

    size_t rec_size;

    70

    uint8_t *data;

    71

    72

    ec_datagram_init(&datagram);

    73

    74

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

    75

        goto err;

    76

    77

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

    78

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

    79

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

    80

    EC_WRITE_U16(data + 3, sdo_index);

    81

    EC_WRITE_U8 (data + 5, sdo_subindex);

    82

    83

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

    84

        goto err;

    85

    86

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

    87

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

    88

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

    89

               sdo_index, sdo_subindex, slave->ring_position);

    90

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

    91

        goto err;

    92

    }

    93

    94

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

    95

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

    96

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

    97

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

    98

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

    99

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

   100

               slave->ring_position);

   101

        ec_print_data(data, rec_size);

   102

        goto err;

   103

    }

   104

   105

    memcpy(target, data + 6, 4);

   106

   107

    ec_datagram_clear(&datagram);

   108

    return 0;

   109

 err:

   110

    ec_datagram_clear(&datagram);

   111

    return -1;

   112

}

   113

   114

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

   115

   116

/**

   117

   Writes a CANopen SDO using expedited mode.

   118

   \return 0 in case of success, else < 0

   119

*/

   120

   121

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

   122

                           uint16_t sdo_index, /**< SDO index */

   123

                           uint8_t sdo_subindex, /**< SDO subindex */

   124

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

   125

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

   126

                           )

   127

{

   128

    uint8_t *data;

   129

    size_t rec_size;

   130

    ec_datagram_t datagram;

   131

   132

    ec_datagram_init(&datagram);

   133

   134

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

   135

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

   136

        goto err;

   137

    }

   138

   139

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

   140

        goto err;

   141

   142

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

   143

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

   144

                            | 0x1 << 1 // expedited transfer

   145

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

   146

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

   147

    EC_WRITE_U16(data + 3, sdo_index);

   148

    EC_WRITE_U8 (data + 5, sdo_subindex);

   149

    memcpy(data + 6, sdo_data, size);

   150

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

   151

   152

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

   153

        goto err;

   154

   155

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

   156

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

   157

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

   158

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

   159

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   160

        return -1;

   161

    }

   162

   163

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

   164

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

   165

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

   166

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

   167

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

   168

               sdo_index, sdo_subindex, size);

   169

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

   170

               slave->ring_position);

   171

        ec_print_data(data, rec_size);

   172

        return -1;

   173

    }

   174

   175

    ec_datagram_clear(&datagram);

   176

    return 0;

   177

 err:

   178

    ec_datagram_clear(&datagram);

   179

    return -1;

   180

}

   181

   182

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

   183

   184

/**

   185

   Reads a CANopen SDO in normal mode.

   186

   \return 0 in case of success, else < 0

   187

   \ingroup RealtimeInterface

   188

   \todo Make size non-pointer.

   189

*/

   190

   191

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

   192

                        uint16_t sdo_index, /**< SDO index */

   193

                        uint8_t sdo_subindex, /**< SDO subindex */

   194

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

   195

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

   196

                        )

   197

{

   198

    uint8_t *data;

   199

    size_t rec_size, data_size;

   200

    uint32_t complete_size;

   201

    ec_datagram_t datagram;

   202

   203

    ec_datagram_init(&datagram);

   204

   205

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

   206

        goto err;

   207

   208

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

   209

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

   210

    EC_WRITE_U16(data + 3, sdo_index);

   211

    EC_WRITE_U8 (data + 5, sdo_subindex);

   212

   213

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

   214

        goto err;

   215

   216

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

   217

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

   218

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

   219

               sdo_index, sdo_subindex, slave->ring_position);

   220

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   221

        goto err;

   222

    }

   223

   224

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

   225

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

   226

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

   227

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

   228

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

   229

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

   230

               slave->ring_position);

   231

        ec_print_data(data, rec_size);

   232

        goto err;

   233

    }

   234

   235

    if (rec_size < 10) {

   236

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

   237

        ec_print_data(data, rec_size);

   238

        goto err;

   239

    }

   240

   241

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

   242

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

   243

               complete_size, *size);

   244

        goto err;

   245

    }

   246

   247

    data_size = rec_size - 10;

   248

   249

    if (data_size != complete_size) {

   250

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

   251

        goto err;

   252

    }

   253

   254

    memcpy(target, data + 10, data_size);

   255

   256

    ec_datagram_clear(&datagram);

   257

    return 0;

   258

 err:

   259

    ec_datagram_clear(&datagram);

   260

    return -1;

   261

}

   262

   263

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

   264

   265

/**

   266

   Fetches the SDO dictionary of a slave.

   267

   \return 0 in case of success, else < 0

   268

*/

   269

   270

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

   271

{

   272

    uint8_t *data;

   273

    size_t rec_size;

   274

    unsigned int i, sdo_count;

   275

    ec_sdo_t *sdo;

   276

    uint16_t sdo_index;

   277

    ec_datagram_t datagram;

   278

   279

    ec_datagram_init(&datagram);

   280

   281

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

   282

        goto err;

   283

   284

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

   285

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

   286

    EC_WRITE_U8 (data + 3, 0x00);

   287

    EC_WRITE_U16(data + 4, 0x0000);

   288

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

   289

   290

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

   291

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

   292

        goto err;

   293

    }

   294

   295

    do {

   296

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

   297

                                                  0x03, &rec_size)))

   298

            goto err;

   299

   300

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

   301

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

   302

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

   303

                   slave->ring_position);

   304

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   305

            goto err;

   306

        }

   307

   308

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

   309

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

   310

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

   311

                   slave->ring_position);

   312

            ec_print_data(data, rec_size);

   313

            goto err;

   314

        }

   315

   316

        if (rec_size < 8) {

   317

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

   318

            ec_print_data(data, rec_size);

   319

            goto err;

   320

        }

   321

   322

        sdo_count = (rec_size - 8) / 2;

   323

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

   324

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

   325

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

   326

   327

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

   328

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

   329

                goto err;

   330

            }

   331

   332

            // Initialize SDO object

   333

            sdo->index = sdo_index;

   334

            //sdo->unkown = 0x0000;

   335

            sdo->object_code = 0x00;

   336

            sdo->name = NULL;

   337

            INIT_LIST_HEAD(&sdo->entries);

   338

   339

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

   340

        }

   341

    }

   342

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

   343

   344

    // Fetch all SDO descriptions

   345

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

   346

   347

    ec_datagram_clear(&datagram);

   348

    return 0;

   349

 err:

   350

    ec_datagram_clear(&datagram);

   351

    return -1;

   352

}

   353

   354

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

   355

   356

/**

   357

   Fetches the SDO descriptions for the known SDOs.

   358

   \return 0 in case of success, else < 0

   359

*/

   360

   361

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

   362

                                    ec_datagram_t *datagram /**< datagram */

   363

                                    )

   364

{

   365

    uint8_t *data;

   366

    size_t rec_size, name_size;

   367

    ec_sdo_t *sdo;

   368

   369

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

   370

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

   371

            return -1;

   372

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

   373

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

   374

        EC_WRITE_U8 (data + 3, 0x00);

   375

        EC_WRITE_U16(data + 4, 0x0000);

   376

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

   377

   378

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

   379

            return -1;

   380

   381

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

   382

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

   383

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

   384

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

   385

                   sdo->index);

   386

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   387

            return -1;

   388

        }

   389

   390

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

   391

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

   392

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

   393

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

   394

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

   395

                   sdo->index);

   396

            ec_print_data(data, rec_size);

   397

            return -1;

   398

        }

   399

   400

        if (rec_size < 12) {

   401

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

   402

            ec_print_data(data, rec_size);

   403

            return -1;

   404

        }

   405

   406

#if 0

   407

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

   408

        ec_print_data(data, rec_size);

   409

#endif

   410

   411

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

   412

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

   413

   414

        name_size = rec_size - 12;

   415

        if (name_size) {

   416

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

   417

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

   418

                return -1;

   419

            }

   420

   421

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

   422

            sdo->name[name_size] = 0;

   423

        }

   424

   425

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

   426

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

   427

            return -1;

   428

        }

   429

   430

        // Fetch all entries (subindices)

   431

        if (ec_slave_fetch_sdo_entries(slave, datagram, sdo,

   432

                                       EC_READ_U8(data + 10)))

   433

            return -1;

   434

    }

   435

   436

    return 0;

   437

}

   438

   439

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

   440

   441

/**

   442

   Fetches all entries (subindices) to an SDO.

   443

   \return 0 in case of success, else < 0

   444

*/

   445

   446

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

   447

                               ec_datagram_t *datagram, /**< datagram */

   448

                               ec_sdo_t *sdo, /**< SDO */

   449

                               uint8_t subindices /**< number of subindices */

   450

                               )

   451

{

   452

    uint8_t *data;

   453

    size_t rec_size, data_size;

   454

    uint8_t i;

   455

    ec_sdo_entry_t *entry;

   456

   457

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

   458

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

   459

            return -1;

   460

   461

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

   462

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

   463

        EC_WRITE_U8 (data + 3, 0x00);

   464

        EC_WRITE_U16(data + 4, 0x0000);

   465

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

   466

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

   467

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

   468

   469

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

   470

            return -1;

   471

   472

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

   473

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

   474

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

   475

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

   476

                   sdo->index, i);

   477

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   478

            return -1;

   479

        }

   480

   481

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

   482

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

   483

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