1

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

     2

 *

     3

 *  c a n o p e n . c

     4

 *

     5

 *  CANopen over EtherCAT

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/delay.h>

    12

#include <linux/slab.h>

    13

#include <linux/module.h>

    14

    15

#include "master.h"

    16

    17

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

    18

    19

void ec_canopen_abort_msg(uint32_t);

    20

int ec_slave_fetch_sdo_descriptions(ec_slave_t *);

    21

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

    22

    23

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

    24

    25

/**

    26

   SDO Abort Code Messages

    27

*/

    28

    29

typedef struct

    30

{

    31

    uint32_t code;

    32

    const char *message;

    33

}

    34

ec_sdo_abort_message_t;

    35

    36

const ec_sdo_abort_message_t sdo_abort_messages[];

    37

    38

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

    39

    40

/**

    41

   Liest 32 Bit eines CANopen-SDOs im Expedited-Modus aus einem Slave.

    42

   \return 0 wenn alles ok, < 0 bei Fehler

    43

 */

    44

    45

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

    46

                          uint16_t sdo_index, /**< SDO-Index */

    47

                          uint8_t sdo_subindex, /**< SDO-Subindex */

    48

                          uint8_t *target /**< Speicher für 4 Bytes */

    49

                          )

    50

{

    51

    uint8_t data[0x20];

    52

    size_t rec_size;

    53

    54

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

    55

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

    56

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

    57

    EC_WRITE_U16(data + 3, sdo_index);

    58

    EC_WRITE_U8 (data + 5, sdo_subindex);

    59

    60

    if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1;

    61

    62

    rec_size = 0x20;

    63

    if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;

    64

    65

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

    66

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

    67

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

    68

               sdo_index, sdo_subindex, slave->ring_position);

    69

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

    70

        return -1;

    71

    }

    72

    73

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

    74

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

    75

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

    76

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

    77

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

    78

               slave->ring_position);

    79

        return -1;

    80

    }

    81

    82

    memcpy(target, data + 6, 4);

    83

    return 0;

    84

}

    85

    86

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

    87

    88

/**

    89

   Beschreibt ein CANopen-SDO eines Slaves im Expedited-Modus.

    90

   \return 0 wenn alles ok, < 0 bei Fehler

    91

 */

    92

    93

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

    94

                           uint16_t sdo_index, /**< SDO-Index */

    95

                           uint8_t sdo_subindex, /**< SDO-Subindex */

    96

                           const uint8_t *sdo_data, /**< Neuer Wert */

    97

                           size_t size

    98

                           )

    99

{

   100

    uint8_t data[0x0A];

   101

    size_t rec_size;

   102

   103

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

   104

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

   105

        return -1;

   106

    }

   107

   108

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

   109

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

   110

                            | 0x1 << 1 // expedited transfer

   111

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

   112

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

   113

    EC_WRITE_U16(data + 3, sdo_index);

   114

    EC_WRITE_U8 (data + 5, sdo_subindex);

   115

   116

    memcpy(data + 6, sdo_data, size);

   117

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

   118

   119

    // Mailox senden und empfangen

   120

    if (ec_slave_mailbox_send(slave, 0x03, data, 0x0A)) return -1;

   121

   122

    rec_size = 0x0A;

   123

    if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;

   124

   125

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

   126

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

   127

        EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!"

   128

               " slave %i.\n", sdo_index, sdo_subindex, size,

   129

               slave->ring_position);

   130

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   131

        return -1;

   132

    }

   133

   134

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

   135

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

   136

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

   137

        EC_READ_U8 (data + 5) != sdo_subindex) // Subindex

   138

    {

   139

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

   140

               slave->ring_position);

   141

        return -1;

   142

    }

   143

   144

    return 0;

   145

}

   146

   147

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

   148

   149

/**

   150

   Liest ein CANopen-SDO im "Normal-Modus" aus einem Slave.

   151

   \return 0 wenn alles ok, < 0 bei Fehler

   152

 */

   153

   154

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

   155

                        uint16_t sdo_index, /**< SDO-Index */

   156

                        uint8_t sdo_subindex, /**< SDO-Subindex */

   157

                        uint8_t *target, /**< Speicher für gel. Wert */

   158

                        size_t *size /**< Größe des Speichers */

   159

                        )

   160

{

   161

    uint8_t data[0x20];

   162

    size_t rec_size, data_size;

   163

    uint32_t complete_size;

   164

   165

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

   166

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

   167

    EC_WRITE_U16(data + 3, sdo_index);

   168

    EC_WRITE_U8 (data + 5, sdo_subindex);

   169

   170

    if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1;

   171

   172

    rec_size = 0x20;

   173

    if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;

   174

   175

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

   176

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

   177

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

   178

               sdo_index, sdo_subindex, slave->ring_position);

   179

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   180

        return -1;

   181

    }

   182

   183

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

   184

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

   185

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

   186

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

   187

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

   188

               slave->ring_position);

   189

        return -1;

   190

    }

   191

   192

    if (rec_size < 10) {

   193

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

   194

        return -1;

   195

    }

   196

   197

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

   198

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

   199

               complete_size, *size);

   200

        return -1;

   201

    }

   202

   203

    data_size = rec_size - 10;

   204

   205

    if (data_size == complete_size) {

   206

        memcpy(target, data + 10, data_size);

   207

    }

   208

    else {

   209

        EC_ERR("SDO data incomplete.\n");

   210

        return -1;

   211

    }

   212

   213

    return 0;

   214

}

   215

   216

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

   217

   218

/**

   219

   Lädt das gesamte SDO-Dictionary aus einem Slave.

   220

   \return 0, wenn alles ok, sonst < 0

   221

*/

   222

   223

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

   224

{

   225

    uint8_t data[0xF0];

   226

    size_t rec_size;

   227

    unsigned int i, sdo_count;

   228

    ec_sdo_t *sdo;

   229

    uint16_t sdo_index;

   230

   231

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

   232

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

   233

    EC_WRITE_U8 (data + 3, 0x00);

   234

    EC_WRITE_U16(data + 4, 0x0000);

   235

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

   236

   237

    if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1;

   238

   239

    do {

   240

        rec_size = 0xF0;

   241

        if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;

   242

   243

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

   244

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

   245

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

   246

                   slave->ring_position);

   247

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   248

            return -1;

   249

        }

   250

   251

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

   252

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

   253

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

   254

                   slave->ring_position);

   255

            return -1;

   256

        }

   257

   258

        if (rec_size < 8) {

   259

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

   260

            return -1;

   261

        }

   262

   263

        sdo_count = (rec_size - 8) / 2;

   264

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

   265

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

   266

            if (!sdo_index) continue; // Manchmal ist der Index 0... ???

   267

   268

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

   269

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

   270

                return -1;

   271

            }

   272

            sdo->index = sdo_index;

   273

            sdo->name = NULL;

   274

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

   275

        }

   276

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

   277

   278

    // Alle Beschreibungen laden

   279

    if (ec_slave_fetch_sdo_descriptions(slave)) return -1;

   280

   281

    return 0;

   282

}

   283

   284

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

   285

   286

/**

   287

   Holt die Beschreibungen zu allen bereits bekannten SDOs.

   288

   \return 0, wenn alles ok, sonst < 0

   289

*/

   290

   291

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

   292

{

   293

    uint8_t data[0xF0];

   294

    size_t rec_size, name_size;

   295

    ec_sdo_t *sdo;

   296

   297

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

   298

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

   299

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

   300

        EC_WRITE_U8 (data + 3, 0x00);

   301

        EC_WRITE_U16(data + 4, 0x0000);

   302

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

   303

   304

        if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1;

   305

   306

        rec_size = 0xF0;

   307

        if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size))

   308

            return -1;

   309

   310

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

   311

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

   312

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

   313

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

   314

                   sdo->index);

   315

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   316

            return -1;

   317

        }

   318

   319

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

   320

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

   321

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

   322

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

   323

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

   324

                   sdo->index);

   325

            return -1;

   326

        }

   327

   328

        if (rec_size < 12) {

   329

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

   330

            return -1;

   331

        }

   332

   333

        sdo->type = EC_READ_U16(data + 8);

   334

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

   335

        INIT_LIST_HEAD(&sdo->entries);

   336

   337

        name_size = rec_size - 12;

   338

        if (name_size) {

   339

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

   340

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

   341

                return -1;

   342

            }

   343

   344

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

   345

            sdo->name[name_size] = 0;

   346

        }

   347

   348

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

   349

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

   350

            return -1;

   351

        }

   352

   353

        // Alle Entries (Subindizes) laden

   354

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

   355

            return -1;

   356

    }

   357

   358

    return 0;

   359

}

   360

   361

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

   362

   363

/**

   364

   Lädt alle Entries (Subindizes) zu einem SDO.

   365

   \return 0, wenn alles ok, sonst < 0

   366

*/

   367

   368

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

   369

                               ec_sdo_t *sdo, /**< SDO */

   370

                               uint8_t subindices /**< Anzahl Subindizes */

   371

                               )

   372

{

   373

    uint8_t data[0xF0];

   374

    size_t rec_size, data_size;

   375

    uint8_t i;

   376

    ec_sdo_entry_t *entry;

   377

   378

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

   379

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

   380

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

   381

        EC_WRITE_U8 (data + 3, 0x00);

   382

        EC_WRITE_U16(data + 4, 0x0000);

   383

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

   384

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

   385

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

   386

   387

        if (ec_slave_mailbox_send(slave, 0x03, data, 10)) return -1;

   388

   389

        rec_size = 0xF0;

   390

        if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size))

   391

            return -1;

   392

   393

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

   394

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

   395

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

   396

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

   397

                   sdo->index, i);

   398

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   399

            return -1;

   400

        }

   401

   402

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

   403

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

   404

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

   405

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

   406

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

   407

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

   408

                   sdo->index, i);

   409

            return -1;

   410

        }

   411

   412

        if (rec_size < 16) {

   413

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

   414

            return -1;

   415

        }

   416

   417

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

   418

            continue;

   419

   420

        data_size = rec_size - 16;

   421

   422

        if (!(entry = (ec_sdo_entry_t *)

   423

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

   424

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

   425

            return -1;

   426

        }

   427

   428

        entry->subindex = i;

   429

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

   430

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

   431

        if (!data_size) {

   432

            entry->name = NULL;

   433

        }

   434

        else {

   435

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

   436

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

   437

            entry->name[data_size] = 0;

   438

        }

   439

   440

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

   441

    }

   442

   443

    return 0;

   444

}

   445

   446

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

   447

   448

/**

   449

   Gibt eine SDO-Abort-Meldung aus.

   450

*/

   451

   452

void ec_canopen_abort_msg(uint32_t abort_code)

   453

{

   454

    const ec_sdo_abort_message_t *abort_msg;

   455

   456

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

   457

        if (abort_msg->code == abort_code) {

   458

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

   459

                   abort_msg->code, abort_msg->message);

   460

            return;

   461

        }

   462

    }

   463

    EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code);

   464

}

   465

   466

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

   467

   468

const ec_sdo_abort_message_t sdo_abort_messages[] = {

   469

    {0x05030000, "Toggle bit not changed"},

   470

    {0x05040000, "SDO protocol timeout"},

   471

    {0x05040001, "Client/Server command specifier not valid or unknown"},

   472

    {0x05040005, "Out of memory"},

   473

    {0x06010000, "Unsupported access to an object"},

   474

    {0x06010001, "Attempt to read a write-only object"},

   475

    {0x06010002, "Attempt to write a read-only object"},

   476

    {0x06020000, "This object does not exist in the object directory"},

   477

    {0x06040041, "The object cannot be mapped into the PDO"},

   478

    {0x06040042, "The number and length of the objects to be mapped would"

   479

     " exceed the PDO length"},

   480

    {0x06040043, "General parameter incompatibility reason"},

   481

    {0x06040047, "Gerneral internal incompatibility in device"},

   482

    {0x06060000, "Access failure due to a hardware error"},

   483

    {0x06070010, "Data type does not match, length of service parameter does"

   484

     " not match"},

   485

    {0x06070012, "Data type does not match, length of service parameter too"

   486

     " high"},

   487

    {0x06070013, "Data type does not match, length of service parameter too"

   488

     " low"},

   489

    {0x06090011, "Subindex does not exist"},

   490

    {0x06090030, "Value range of parameter exceeded"},

   491

    {0x06090031, "Value of parameter written too high"},

   492

    {0x06090032, "Value of parameter written too low"},

   493

    {0x06090036, "Maximum value is less than minimum value"},

   494

    {0x08000000, "General error"},