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

#include "mailbox.h"

    17

    18

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

    19

    20

void ec_canopen_abort_msg(uint32_t);

    21

int ec_slave_fetch_sdo_descriptions(ec_slave_t *);

    22

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

    23

    24

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

    25

    26

/**

    27

   SDO Abort Code Messages

    28

*/

    29

    30

typedef struct

    31

{

    32

    uint32_t code;

    33

    const char *message;

    34

}

    35

ec_sdo_abort_message_t;

    36

    37

const ec_sdo_abort_message_t sdo_abort_messages[];

    38

    39

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

    40

    41

/**

    42

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

    43

   \return 0 wenn alles ok, < 0 bei Fehler

    44

 */

    45

    46

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

    47

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

    48

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

    49

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

    50

                          )

    51

{

    52

    size_t rec_size;

    53

    uint8_t *data;

    54

    55

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

    56

    57

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

    58

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

    59

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

    60

    EC_WRITE_U16(data + 3, sdo_index);

    61

    EC_WRITE_U8 (data + 5, sdo_subindex);

    62

    63

    if (!(data = ec_slave_mbox_simple_io(slave, &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;

   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

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

   109

   110

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

   111

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

   112

                            | 0x1 << 1 // expedited transfer

   113

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

   114

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

   115

    EC_WRITE_U16(data + 3, sdo_index);

   116

    EC_WRITE_U8 (data + 5, sdo_subindex);

   117

    memcpy(data + 6, sdo_data, size);

   118

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

   119

   120

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

   121

   122

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

   123

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

   124

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

   125

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

   126

               slave->ring_position);

   127

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   128

        return -1;

   129

    }

   130

   131

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

   132

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

   133

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

   134

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

   135

    {

   136

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

   137

               slave->ring_position);

   138

        return -1;

   139

    }

   140

   141

    return 0;

   142

}

   143

   144

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

   145

   146

/**

   147

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

   148

   \return 0 wenn alles ok, < 0 bei Fehler

   149

 */

   150

   151

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

   152

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

   153

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

   154

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

   155

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

   156

                        )

   157

{

   158

    uint8_t *data;

   159

    size_t rec_size, data_size;

   160

    uint32_t complete_size;

   161

   162

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

   163

   164

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

   165

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

   166

    EC_WRITE_U16(data + 3, sdo_index);

   167

    EC_WRITE_U8 (data + 5, sdo_subindex);

   168

   169

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

   170

   171

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

   172

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

   173

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

   174

               sdo_index, sdo_subindex, slave->ring_position);

   175

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   176

        return -1;

   177

    }

   178

   179

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

   180

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

   181

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

   182

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

   183

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

   184

               slave->ring_position);

   185

        return -1;

   186

    }

   187

   188

    if (rec_size < 10) {

   189

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

   190

        return -1;

   191

    }

   192

   193

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

   194

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

   195

               complete_size, *size);

   196

        return -1;

   197

    }

   198

   199

    data_size = rec_size - 10;

   200

   201

    if (data_size == complete_size) {

   202

        memcpy(target, data + 10, data_size);

   203

    }

   204

    else {

   205

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

   206

        return -1;

   207

    }

   208

   209

    return 0;

   210

}

   211

   212

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

   213

   214

/**

   215

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

   216

   \return 0, wenn alles ok, sonst < 0

   217

*/

   218

   219

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

   220

{

   221

    uint8_t *data;

   222

    size_t rec_size;

   223

    unsigned int i, sdo_count;

   224

    ec_sdo_t *sdo;

   225

    uint16_t sdo_index;

   226

   227

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

   228

   229

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

   230

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

   231

    EC_WRITE_U8 (data + 3, 0x00);

   232

    EC_WRITE_U16(data + 4, 0x0000);

   233

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

   234

   235

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

   236

        EC_ERR("Mailbox checking failed on slave %i!\n",

   237

               slave->ring_position);

   238

        return -1;

   239

    }

   240

   241

    do {

   242

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

   243

            return -1;

   244

   245

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

   246

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

   247

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

   248

                   slave->ring_position);

   249

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   250

            return -1;

   251

        }

   252

   253

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

   254

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

   255

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

   256

                   slave->ring_position);

   257

            return -1;

   258

        }

   259

   260

        if (rec_size < 8) {

   261

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

   262

            return -1;

   263

        }

   264

   265

        sdo_count = (rec_size - 8) / 2;

   266

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

   267

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

   268

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

   269

   270

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

   271

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

   272

                return -1;

   273

            }

   274

            sdo->index = sdo_index;

   275

            sdo->name = NULL;

   276

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

   277

        }

   278

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

   279

   280

    // Alle Beschreibungen laden

   281

    if (ec_slave_fetch_sdo_descriptions(slave)) return -1;

   282

   283

    return 0;

   284

}

   285

   286

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

   287

   288

/**

   289

   Holt die Beschreibungen zu allen bereits bekannten SDOs.

   290

   \return 0, wenn alles ok, sonst < 0

   291

*/

   292

   293

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

   294

{

   295

    uint8_t *data;

   296

    size_t rec_size, name_size;

   297

    ec_sdo_t *sdo;

   298

   299

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

   300

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

   301

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

   302

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

   303

        EC_WRITE_U8 (data + 3, 0x00);

   304

        EC_WRITE_U16(data + 4, 0x0000);

   305

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

   306

   307

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

   308

   309

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

   310

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

   311

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

   312

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

   313

                   sdo->index);

   314

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   315

            return -1;

   316

        }

   317

   318

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

   319

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

   320

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

   321

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

   322

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

   323

                   sdo->index);

   324

            return -1;

   325

        }

   326

   327

        if (rec_size < 12) {

   328

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

   329

            return -1;

   330

        }

   331

   332

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

   333

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

   334

        INIT_LIST_HEAD(&sdo->entries);

   335

   336

        name_size = rec_size - 12;

   337

        if (name_size) {

   338

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

   339

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

   340

                return -1;

   341

            }

   342

   343

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

   344

            sdo->name[name_size] = 0;

   345

        }

   346

   347

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

   348

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

   349

            return -1;

   350

        }

   351

   352

        // Alle Entries (Subindizes) laden

   353

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

   354

            return -1;

   355

    }

   356

   357

    return 0;

   358

}

   359

   360

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

   361

   362

/**

   363

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

   364

   \return 0, wenn alles ok, sonst < 0

   365

*/

   366

   367

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

   368

                               ec_sdo_t *sdo, /**< SDO */

   369

                               uint8_t subindices /**< Anzahl Subindizes */

   370

                               )

   371

{

   372

    uint8_t *data;

   373

    size_t rec_size, data_size;

   374

    uint8_t i;

   375

    ec_sdo_entry_t *entry;

   376

   377

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

   378

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

   379

            return -1;

   380

   381

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

   382

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

   383

        EC_WRITE_U8 (data + 3, 0x00);

   384

        EC_WRITE_U16(data + 4, 0x0000);

   385

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

   386

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

   387

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

   388

   389

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

   390

   391

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

   392

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

   393

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

   394

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

   395

                   sdo->index, i);

   396

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   397

            return -1;

   398

        }

   399

   400

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

   401

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

   402

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

   403

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

   404

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

   405

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

   406

                   sdo->index, i);

   407

            return -1;

   408

        }

   409

   410

        if (rec_size < 16) {

   411

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

   412

            return -1;

   413

        }

   414

   415

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

   416

            continue;

   417

   418

        data_size = rec_size - 16;

   419

   420

        if (!(entry = (ec_sdo_entry_t *)

   421

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

   422

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

   423

            return -1;

   424

        }

   425

   426

        entry->subindex = i;

   427

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

   428

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

   429

        if (!data_size) {

   430

            entry->name = NULL;

   431

        }

   432

        else {

   433

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

   434

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

   435

            entry->name[data_size] = 0;

   436

        }

   437

   438

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

   439

    }

   440

   441

    return 0;

   442

}

   443

   444

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

   445

   446

/**

   447

   Gibt eine SDO-Abort-Meldung aus.

   448

*/

   449

   450

void ec_canopen_abort_msg(uint32_t abort_code)

   451

{

   452

    const ec_sdo_abort_message_t *abort_msg;

   453

   454

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

   455

        if (abort_msg->code == abort_code) {

   456

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

   457

                   abort_msg->code, abort_msg->message);

   458

            return;

   459

        }

   460

    }

   461

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

   462

}

   463

   464

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

   465

   466

const ec_sdo_abort_message_t sdo_abort_messages[] = {

   467

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

   468

    {0x05040000, "SDO protocol timeout"},

   469

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

   470

    {0x05040005, "Out of memory"},

   471

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

   472

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

   473

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

   474

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

   475

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

   476

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

   477

     " exceed the PDO length"},

   478

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

   479

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

   480

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

   481

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

   482

     " not match"},

   483

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

   484

     " high"},

   485

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

   486

     " low"},

   487

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

   488

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

   489

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

   490

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

   491

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

   492

    {0x08000000, "General error"},