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

    52

int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_sdo_t *, uint8_t);

    53

    54

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

    55

    56

const ec_code_msg_t sdo_abort_messages[];

    57

    58

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

    59

    60

/**

    61

   Reads 32 bit of a CANopen SDO in expedited mode.

    62

   \return 0 in case of success, else < 0

    63

*/

    64

    65

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

    66

                          uint16_t sdo_index, /**< SDO index */

    67

                          uint8_t sdo_subindex, /**< SDO subindex */

    68

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

    69

                          )

    70

{

    71

    size_t rec_size;

    72

    uint8_t *data;

    73

    74

    if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 6))) return -1;

    75

    76

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

    77

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

    78

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

    79

    EC_WRITE_U16(data + 3, sdo_index);

    80

    EC_WRITE_U8 (data + 5, sdo_subindex);

    81

    82

    if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1;

    83

    84

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

    85

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

    86

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

    87

               sdo_index, sdo_subindex, slave->ring_position);

    88

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

    89

        return -1;

    90

    }

    91

    92

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

    93

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

    94

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

    95

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

    96

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

    97

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

    98

               slave->ring_position);

    99

        ec_print_data(data, rec_size);

   100

        return -1;

   101

    }

   102

   103

    memcpy(target, data + 6, 4);

   104

    return 0;

   105

}

   106

   107

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

   108

   109

/**

   110

   Writes a CANopen SDO using expedited mode.

   111

   \return 0 in case of success, else < 0

   112

*/

   113

   114

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

   115

                           uint16_t sdo_index, /**< SDO index */

   116

                           uint8_t sdo_subindex, /**< SDO subindex */

   117

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

   118

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

   119

                           )

   120

{

   121

    uint8_t *data;

   122

    size_t rec_size;

   123

   124

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

   125

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

   126

        return -1;

   127

    }

   128

   129

    if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 0x0A))) return -1;

   130

   131

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

   132

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

   133

                            | 0x1 << 1 // expedited transfer

   134

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

   135

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

   136

    EC_WRITE_U16(data + 3, sdo_index);

   137

    EC_WRITE_U8 (data + 5, sdo_subindex);

   138

    memcpy(data + 6, sdo_data, size);

   139

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

   140

   141

    if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1;

   142

   143

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

   144

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

   145

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

   146

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

   147

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   148

        return -1;

   149

    }

   150

   151

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

   152

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

   153

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

   154

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

   155

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

   156

               sdo_index, sdo_subindex, size);

   157

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

   158

               slave->ring_position);

   159

        ec_print_data(data, rec_size);

   160

        return -1;

   161

    }

   162

   163

    return 0;

   164

}

   165

   166

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

   167

   168

/**

   169

   Reads a CANopen SDO in normal mode.

   170

   \return 0 in case of success, else < 0

   171

   \ingroup RealtimeInterface

   172

   \todo Make size non-pointer.

   173

*/

   174

   175

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

   176

                        uint16_t sdo_index, /**< SDO index */

   177

                        uint8_t sdo_subindex, /**< SDO subindex */

   178

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

   179

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

   180

                        )

   181

{

   182

    uint8_t *data;

   183

    size_t rec_size, data_size;

   184

    uint32_t complete_size;

   185

   186

    if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 6))) return -1;

   187

   188

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

   189

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

   190

    EC_WRITE_U16(data + 3, sdo_index);

   191

    EC_WRITE_U8 (data + 5, sdo_subindex);

   192

   193

    if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1;

   194

   195

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

   196

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

   197

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

   198

               sdo_index, sdo_subindex, slave->ring_position);

   199

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   200

        return -1;

   201

    }

   202

   203

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

   204

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

   205

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

   206

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

   207

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

   208

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

   209

               slave->ring_position);

   210

        ec_print_data(data, rec_size);

   211

        return -1;

   212

    }

   213

   214

    if (rec_size < 10) {

   215

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

   216

        ec_print_data(data, rec_size);

   217

        return -1;

   218

    }

   219

   220

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

   221

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

   222

               complete_size, *size);

   223

        return -1;

   224

    }

   225

   226

    data_size = rec_size - 10;

   227

   228

    if (data_size != complete_size) {

   229

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

   230

        return -1;

   231

    }

   232

   233

    memcpy(target, data + 10, data_size);

   234

    return 0;

   235

}

   236

   237

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

   238

   239

/**

   240

   Fetches the SDO dictionary of a slave.

   241

   \return 0 in case of success, else < 0

   242

*/

   243

   244

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

   245

{

   246

    uint8_t *data;

   247

    size_t rec_size;

   248

    unsigned int i, sdo_count;

   249

    ec_sdo_t *sdo;

   250

    uint16_t sdo_index;

   251

   252

    if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 8))) return -1;

   253

   254

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

   255

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

   256

    EC_WRITE_U8 (data + 3, 0x00);

   257

    EC_WRITE_U16(data + 4, 0x0000);

   258

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

   259

   260

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

   261

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

   262

        return -1;

   263

    }

   264

   265

    do {

   266

        if (!(data = ec_slave_mbox_simple_receive(slave, 0x03, &rec_size)))

   267

            return -1;

   268

   269

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

   270

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

   271

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

   272

                   slave->ring_position);

   273

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   274

            return -1;

   275

        }

   276

   277

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

   278

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

   279

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

   280

                   slave->ring_position);

   281

            ec_print_data(data, rec_size);

   282

            return -1;

   283

        }

   284

   285

        if (rec_size < 8) {

   286

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

   287

            ec_print_data(data, rec_size);

   288

            return -1;

   289

        }

   290

   291

        sdo_count = (rec_size - 8) / 2;

   292

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

   293

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

   294

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

   295

   296

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

   297

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

   298

                return -1;

   299

            }

   300

   301

            // Initialize SDO object

   302

            sdo->index = sdo_index;

   303

            //sdo->unkown = 0x0000;

   304

            sdo->object_code = 0x00;

   305

            sdo->name = NULL;

   306

            INIT_LIST_HEAD(&sdo->entries);

   307

   308

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

   309

        }

   310

    }

   311

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

   312

   313

    // Fetch all SDO descriptions

   314

    if (ec_slave_fetch_sdo_descriptions(slave)) return -1;

   315

   316

    return 0;

   317

}

   318

   319

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

   320

   321

/**

   322

   Fetches the SDO descriptions for the known SDOs.

   323

   \return 0 in case of success, else < 0

   324

*/

   325

   326

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

   327

{

   328

    uint8_t *data;

   329

    size_t rec_size, name_size;

   330

    ec_sdo_t *sdo;

   331

   332

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

   333

        if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 8))) return -1;

   334

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

   335

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

   336

        EC_WRITE_U8 (data + 3, 0x00);

   337

        EC_WRITE_U16(data + 4, 0x0000);

   338

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

   339

   340

        if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1;

   341

   342

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

   343

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

   344

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

   345

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

   346

                   sdo->index);

   347

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   348

            return -1;

   349

        }

   350

   351

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

   352

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

   353

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

   354

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

   355

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

   356

                   sdo->index);

   357

            ec_print_data(data, rec_size);

   358

            return -1;

   359

        }

   360

   361

        if (rec_size < 12) {

   362

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

   363

            ec_print_data(data, rec_size);

   364

            return -1;

   365

        }

   366

   367

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

   368

        ec_print_data(data, rec_size);

   369

   370

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

   371

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

   372

   373

        name_size = rec_size - 12;

   374

        if (name_size) {

   375

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

   376

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

   377

                return -1;

   378

            }

   379

   380

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

   381

            sdo->name[name_size] = 0;

   382

        }

   383

   384

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

   385

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

   386

            return -1;

   387

        }

   388

   389

        // Fetch all entries (subindices)

   390

        if (ec_slave_fetch_sdo_entries(slave, sdo, EC_READ_U8(data + 10)))

   391

            return -1;

   392

    }

   393

   394

    return 0;

   395

}

   396

   397

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

   398

   399

/**

   400

   Fetches all entries (subindices) to an SDO.

   401

   \return 0 in case of success, else < 0

   402

*/

   403

   404

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

   405

                               ec_sdo_t *sdo, /**< SDO */

   406

                               uint8_t subindices /**< number of subindices */

   407

                               )

   408

{

   409

    uint8_t *data;

   410

    size_t rec_size, data_size;

   411

    uint8_t i;

   412

    ec_sdo_entry_t *entry;

   413

   414

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

   415

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

   416

            return -1;

   417

   418

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

   419

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

   420

        EC_WRITE_U8 (data + 3, 0x00);

   421

        EC_WRITE_U16(data + 4, 0x0000);

   422

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

   423

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

   424

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

   425

   426

        if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1;

   427

   428

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

   429

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

   430

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

   431

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

   432

                   sdo->index, i);

   433

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   434

            return -1;

   435

        }

   436

   437

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

   438

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

   439

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

   440

            EC_READ_U8(data + 8) != i) { // SDO subindex

   441

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

   442

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

   443

                   sdo->index, i);

   444

            ec_print_data(data, rec_size);

   445

            return -1;

   446

        }

   447

   448

        if (rec_size < 16) {

   449

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

   450

            ec_print_data(data, rec_size);

   451

            return -1;

   452

        }

   453

   454

        if (!EC_READ_U16(data + 12)) // bit length = 0

   455

            continue;

   456

   457

        data_size = rec_size - 16;

   458

   459

        if (!(entry = (ec_sdo_entry_t *)

   460

              kmalloc(sizeof(ec_sdo_entry_t) + data_size + 1, GFP_KERNEL))) {

   461

            EC_ERR("Failed to allocate entry!\n");

   462

            return -1;

   463

        }

   464

   465

        entry->subindex = i;

   466

        entry->data_type = EC_READ_U16(data + 10);

   467

        entry->bit_length = EC_READ_U16(data + 12);

   468

   469

        // memory for name string appended to entry

   470

        entry->name = (uint8_t *) entry + sizeof(ec_sdo_entry_t);

   471

   472

        memcpy(entry->name, data + 16, data_size);

   473

        entry->name[data_size] = 0;

   474

   475

        list_add_tail(&entry->list, &sdo->entries);

   476

    }

   477

   478

    return 0;

   479

}

   480

   481

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

   482

   483

/**

   484

   Outputs an SDO abort message.

   485

*/

   486