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; version 2 of the License.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    14

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    16

 *  GNU General Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License

    19

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

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

    23

    24

/**

    25

   \file

    26

   Canopen-over-EtherCAT functions.

    27

*/

    28

    29

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

    30

    31

#include <linux/delay.h>

    32

#include <linux/slab.h>

    33

#include <linux/module.h>

    34

    35

#include "master.h"

    36

#include "mailbox.h"

    37

    38

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

    39

    40

void ec_canopen_abort_msg(uint32_t);

    41

int ec_slave_fetch_sdo_descriptions(ec_slave_t *);

    42

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

    43

    44

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

    45

    46

const ec_code_msg_t sdo_abort_messages[];

    47

    48

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

    49

    50

/**

    51

   Reads 32 bit of a CANopen SDO in expedited mode.

    52

   \return 0 in case of success, else < 0

    53

*/

    54

    55

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

    56

                          uint16_t sdo_index, /**< SDO index */

    57

                          uint8_t sdo_subindex, /**< SDO subindex */

    58

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

    59

                          )

    60

{

    61

    size_t rec_size;

    62

    uint8_t *data;

    63

    64

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

    65

    66

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

    67

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

    68

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

    69

    EC_WRITE_U16(data + 3, sdo_index);

    70

    EC_WRITE_U8 (data + 5, sdo_subindex);

    71

    72

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

    73

    74

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

    75

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

    76

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

    77

               sdo_index, sdo_subindex, slave->ring_position);

    78

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

    79

        return -1;

    80

    }

    81

    82

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

    83

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

    84

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

    85

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

    86

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

    87

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

    88

               slave->ring_position);

    89

        ec_print_data(data, rec_size);

    90

        return -1;

    91

    }

    92

    93

    memcpy(target, data + 6, 4);

    94

    return 0;

    95

}

    96

    97

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

    98

    99

/**

   100

   Writes a CANopen SDO using expedited mode.

   101

   \return 0 in case of success, else < 0

   102

*/

   103

   104

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

   105

                           uint16_t sdo_index, /**< SDO index */

   106

                           uint8_t sdo_subindex, /**< SDO subindex */

   107

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

   108

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

   109

                           )

   110

{

   111

    uint8_t *data;

   112

    size_t rec_size;

   113

   114

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

   115

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

   116

        return -1;

   117

    }

   118

   119

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

   120

   121

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

   122

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

   123

                            | 0x1 << 1 // expedited transfer

   124

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

   125

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

   126

    EC_WRITE_U16(data + 3, sdo_index);

   127

    EC_WRITE_U8 (data + 5, sdo_subindex);

   128

    memcpy(data + 6, sdo_data, size);

   129

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

   130

   131

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

   132

   133

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

   134

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

   135

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

   136

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

   137

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   138

        return -1;

   139

    }

   140

   141

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

   142

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

   143

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

   144

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

   145

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

   146

               sdo_index, sdo_subindex, size);

   147

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

   148

               slave->ring_position);

   149

        ec_print_data(data, rec_size);

   150

        return -1;

   151

    }

   152

   153

    return 0;

   154

}

   155

   156

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

   157

   158

/**

   159

   Reads a CANopen SDO in normal mode.

   160

   \return 0 in case of success, else < 0

   161

   \ingroup RealtimeInterface

   162

   \todo Make size non-pointer.

   163

*/

   164

   165

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

   166

                        uint16_t sdo_index, /**< SDO index */

   167

                        uint8_t sdo_subindex, /**< SDO subindex */

   168

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

   169

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

   170

                        )

   171

{

   172

    uint8_t *data;

   173

    size_t rec_size, data_size;

   174

    uint32_t complete_size;

   175

   176

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

   177

   178

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

   179

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

   180

    EC_WRITE_U16(data + 3, sdo_index);

   181

    EC_WRITE_U8 (data + 5, sdo_subindex);

   182

   183

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

   184

   185

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

   186

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

   187

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

   188

               sdo_index, sdo_subindex, slave->ring_position);

   189

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   190

        return -1;

   191

    }

   192

   193

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

   194

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

   195

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

   196

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

   197

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

   198

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

   199

               slave->ring_position);

   200

        ec_print_data(data, rec_size);

   201

        return -1;

   202

    }

   203

   204

    if (rec_size < 10) {

   205

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

   206

        ec_print_data(data, rec_size);

   207

        return -1;

   208

    }

   209

   210

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

   211

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

   212

               complete_size, *size);

   213

        return -1;

   214

    }

   215

   216

    data_size = rec_size - 10;

   217

   218

    if (data_size != complete_size) {

   219

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

   220

        return -1;

   221

    }

   222

   223

    memcpy(target, data + 10, data_size);

   224

    return 0;

   225

}

   226

   227

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

   228

   229

/**

   230

   Fetches the SDO dictionary of a slave.

   231

   \return 0 in case of success, else < 0

   232

*/

   233

   234

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

   235

{

   236

    uint8_t *data;

   237

    size_t rec_size;

   238

    unsigned int i, sdo_count;

   239

    ec_sdo_t *sdo;

   240

    uint16_t sdo_index;

   241

   242

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

   243

   244

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

   245

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

   246

    EC_WRITE_U8 (data + 3, 0x00);

   247

    EC_WRITE_U16(data + 4, 0x0000);

   248

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

   249

   250

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

   251

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

   252

        return -1;

   253

    }

   254

   255

    do {

   256

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

   257

            return -1;

   258

   259

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

   260

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

   261

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

   262

                   slave->ring_position);

   263

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   264

            return -1;

   265

        }

   266

   267

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

   268

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

   269

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

   270

                   slave->ring_position);

   271

            ec_print_data(data, rec_size);

   272

            return -1;

   273

        }

   274

   275

        if (rec_size < 8) {

   276

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

   277

            ec_print_data(data, rec_size);

   278

            return -1;

   279

        }

   280

   281

        sdo_count = (rec_size - 8) / 2;

   282

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

   283

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

   284

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

   285

   286

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

   287

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

   288

                return -1;

   289

            }

   290

   291

            // Initialize SDO object

   292

            sdo->index = sdo_index;

   293

            //sdo->unkown = 0x0000;

   294

            sdo->object_code = 0x00;

   295

            sdo->name = NULL;

   296

            INIT_LIST_HEAD(&sdo->entries);

   297

   298

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

   299

        }

   300

    }

   301

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

   302

   303

    // Fetch all SDO descriptions

   304

    if (ec_slave_fetch_sdo_descriptions(slave)) return -1;

   305

   306

    return 0;

   307

}

   308

   309

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

   310

   311

/**

   312

   Fetches the SDO descriptions for the known SDOs.

   313

   \return 0 in case of success, else < 0

   314

*/

   315

   316

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

   317

{

   318

    uint8_t *data;

   319

    size_t rec_size, name_size;

   320

    ec_sdo_t *sdo;

   321

   322

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

   323

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

   324

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

   325

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

   326

        EC_WRITE_U8 (data + 3, 0x00);

   327

        EC_WRITE_U16(data + 4, 0x0000);

   328

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

   329

   330

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

   331

   332

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

   333

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

   334

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

   335

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

   336

                   sdo->index);

   337

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   338

            return -1;

   339

        }

   340

   341

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

   342

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

   343

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

   344

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

   345

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

   346

                   sdo->index);

   347

            ec_print_data(data, rec_size);

   348

            return -1;

   349

        }

   350

   351

        if (rec_size < 12) {

   352

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

   353

            ec_print_data(data, rec_size);

   354

            return -1;

   355

        }

   356

   357

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

   358

        ec_print_data(data, rec_size);

   359

   360

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

   361

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

   362

   363

        name_size = rec_size - 12;

   364

        if (name_size) {

   365

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

   366

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

   367

                return -1;

   368

            }

   369

   370

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

   371

            sdo->name[name_size] = 0;

   372

        }

   373

   374

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

   375

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

   376

            return -1;

   377

        }

   378

   379

        // Fetch all entries (subindices)

   380

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

   381

            return -1;

   382

    }

   383

   384

    return 0;

   385

}

   386

   387

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

   388

   389

/**

   390

   Fetches all entries (subindices) to an SDO.

   391

   \return 0 in case of success, else < 0

   392

*/

   393

   394

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

   395

                               ec_sdo_t *sdo, /**< SDO */

   396

                               uint8_t subindices /**< number of subindices */

   397

                               )

   398

{

   399

    uint8_t *data;

   400

    size_t rec_size, data_size;

   401

    uint8_t i;

   402

    ec_sdo_entry_t *entry;

   403

   404

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

   405

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

   406

            return -1;

   407

   408

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

   409

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

   410

        EC_WRITE_U8 (data + 3, 0x00);

   411

        EC_WRITE_U16(data + 4, 0x0000);

   412

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

   413

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

   414

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

   415

   416

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

   417

   418

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

   419

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

   420

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

   421

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

   422

                   sdo->index, i);

   423

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   424

            return -1;

   425

        }

   426

   427

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

   428

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

   429

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

   430

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

   431

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

   432

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

   433

                   sdo->index, i);

   434

            ec_print_data(data, rec_size);

   435

            return -1;

   436

        }

   437

   438

        if (rec_size < 16) {

   439

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

   440

            ec_print_data(data, rec_size);

   441

            return -1;

   442

        }

   443

   444

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

   445

            continue;

   446

   447

        data_size = rec_size - 16;

   448

   449

        if (!(entry = (ec_sdo_entry_t *)

   450

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

   451

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

   452

            return -1;

   453

        }

   454

   455

        entry->subindex = i;

   456

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

   457

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

   458

   459

        // memory for name string appended to entry

   460

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

   461

   462

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

   463

        entry->name[data_size] = 0;

   464

   465

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

   466

    }

   467

   468

    return 0;

   469

}

   470

   471

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

   472

   473

/**

   474

   Outputs an SDO abort message.

   475

*/

   476

   477

void ec_canopen_abort_msg(uint32_t abort_code)

   478

{

   479

    const ec_code_msg_t *abort_msg;

   480

   481

    for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {

   482

        if (abort_msg->code == abort_code) {

   483

            EC_ERR("SDO abort message 0x%08X: \"%s\".\n",

   484

                   abort_msg->code, abort_msg->message);

   485

            return;

   486

        }