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

const ec_code_msg_t sdo_abort_messages[];

    27

    28

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

    29

    30

/**

    31

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

    32

   \return 0 wenn alles ok, < 0 bei Fehler

    33

 */

    34

    35

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

    36

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

    37

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

    38

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

    39

                          )

    40

{

    41

    size_t rec_size;

    42

    uint8_t *data;

    43

    44

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

    45

    46

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

    47

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

    48

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

    49

    EC_WRITE_U16(data + 3, sdo_index);

    50

    EC_WRITE_U8 (data + 5, sdo_subindex);

    51

    52

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

    53

    54

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

    55

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

    56

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

    57

               sdo_index, sdo_subindex, slave->ring_position);

    58

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

    59

        return -1;

    60

    }

    61

    62

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

    63

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

    64

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

    65

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

    66

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

    67

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

    68

               slave->ring_position);

    69

        ec_print_data(data, rec_size);

    70

        return -1;

    71

    }

    72

    73

    memcpy(target, data + 6, 4);

    74

    return 0;

    75

}

    76

    77

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

    78

    79

/**

    80

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

    81

   \return 0 wenn alles ok, < 0 bei Fehler

    82

 */

    83

    84

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

    85

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

    86

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

    87

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

    88

                           size_t size

    89

                           )

    90

{

    91

    uint8_t *data;

    92

    size_t rec_size;

    93

    94

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

    95

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

    96

        return -1;

    97

    }

    98

    99

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

   100

   101

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

   102

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

   103

                            | 0x1 << 1 // expedited transfer

   104

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

   105

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

   106

    EC_WRITE_U16(data + 3, sdo_index);

   107

    EC_WRITE_U8 (data + 5, sdo_subindex);

   108

    memcpy(data + 6, sdo_data, size);

   109

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

   110

   111

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

   112

   113

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

   114

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

   115

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

   116

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

   117

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   118

        return -1;

   119

    }

   120

   121

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

   122

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

   123

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

   124

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

   125

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

   126

               sdo_index, sdo_subindex, size);

   127

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

   128

               slave->ring_position);

   129

        ec_print_data(data, rec_size);

   130

        return -1;

   131

    }

   132

   133

    return 0;

   134

}

   135

   136

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

   137

   138

/**

   139

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

   140

   \return 0 wenn alles ok, < 0 bei Fehler

   141

 */

   142

   143

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

   144

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

   145

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

   146

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

   147

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

   148

                        )

   149

{

   150

    uint8_t *data;

   151

    size_t rec_size, data_size;

   152

    uint32_t complete_size;

   153

   154

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

   155

   156

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

   157

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

   158

    EC_WRITE_U16(data + 3, sdo_index);

   159

    EC_WRITE_U8 (data + 5, sdo_subindex);

   160

   161

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

   162

   163

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

   164

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

   165

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

   166

               sdo_index, sdo_subindex, slave->ring_position);

   167

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   168

        return -1;

   169

    }

   170

   171

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

   172

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

   173

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

   174

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

   175

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

   176

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

   177

               slave->ring_position);

   178

        ec_print_data(data, rec_size);

   179

        return -1;

   180

    }

   181

   182

    if (rec_size < 10) {

   183

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

   184

        ec_print_data(data, rec_size);

   185

        return -1;

   186

    }

   187

   188

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

   189

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

   190

               complete_size, *size);

   191

        return -1;

   192

    }

   193

   194

    data_size = rec_size - 10;

   195

   196

    if (data_size != complete_size) {

   197

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

   198

        return -1;

   199

    }

   200

   201

    memcpy(target, data + 10, data_size);

   202

    return 0;

   203

}

   204

   205

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

   206

   207

/**

   208

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

   209

   \return 0, wenn alles ok, sonst < 0

   210

*/

   211

   212

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

   213

{

   214

    uint8_t *data;

   215

    size_t rec_size;

   216

    unsigned int i, sdo_count;

   217

    ec_sdo_t *sdo;

   218

    uint16_t sdo_index;

   219

   220

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

   221

   222

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

   223

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

   224

    EC_WRITE_U8 (data + 3, 0x00);

   225

    EC_WRITE_U16(data + 4, 0x0000);

   226

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

   227

   228

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

   229

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

   230

        return -1;

   231

    }

   232

   233

    do {

   234

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

   235

            return -1;

   236

   237

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

   238

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

   239

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

   240

                   slave->ring_position);

   241

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   242

            return -1;

   243

        }

   244

   245

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

   246

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

   247

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

   248

                   slave->ring_position);

   249

            ec_print_data(data, rec_size);

   250

            return -1;

   251

        }

   252

   253

        if (rec_size < 8) {

   254

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

   255

            ec_print_data(data, rec_size);

   256

            return -1;

   257

        }

   258

   259

        sdo_count = (rec_size - 8) / 2;

   260

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

   261

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

   262

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

   263

   264

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

   265

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

   266

                return -1;

   267

            }

   268

   269

            // Initialize SDO object

   270

            sdo->index = sdo_index;

   271

            //sdo->unkown = 0x0000;

   272

            sdo->object_code = 0x00;

   273

            sdo->name = NULL;

   274

            INIT_LIST_HEAD(&sdo->entries);

   275

   276

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

   277

        }

   278

    }

   279

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

   280

   281

    // Alle Beschreibungen laden

   282

    if (ec_slave_fetch_sdo_descriptions(slave)) return -1;

   283

   284

    return 0;

   285

}

   286

   287

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

   288

   289

/**

   290

   Holt die Beschreibungen zu allen bereits bekannten SDOs.

   291

   \return 0, wenn alles ok, sonst < 0

   292

*/

   293

   294

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

   295

{

   296

    uint8_t *data;

   297

    size_t rec_size, name_size;

   298

    ec_sdo_t *sdo;

   299

   300

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

   301

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

   302

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

   303

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

   304

        EC_WRITE_U8 (data + 3, 0x00);

   305

        EC_WRITE_U16(data + 4, 0x0000);

   306

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

   307

   308

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

            ec_print_data(data, rec_size);

   326

            return -1;

   327

        }

   328

   329

        if (rec_size < 12) {

   330

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

   331

            ec_print_data(data, rec_size);

   332

            return -1;

   333

        }

   334

   335

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

   336

        ec_print_data(data, rec_size);

   337

   338

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

   339

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

   340

   341

        name_size = rec_size - 12;

   342

        if (name_size) {

   343

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

   344

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

   345

                return -1;

   346

            }

   347

   348

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

   349

            sdo->name[name_size] = 0;

   350

        }

   351

   352

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

   353

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

   354

            return -1;

   355

        }

   356

   357

        // Alle Entries (Subindizes) laden

   358

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

   359

            return -1;

   360

    }

   361

   362

    return 0;

   363

}

   364

   365

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

   366

   367

/**

   368

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

   369

   \return 0, wenn alles ok, sonst < 0

   370

*/

   371

   372

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

   373

                               ec_sdo_t *sdo, /**< SDO */

   374

                               uint8_t subindices /**< Anzahl Subindizes */

   375

                               )

   376

{

   377

    uint8_t *data;

   378

    size_t rec_size, data_size;

   379

    uint8_t i;

   380

    ec_sdo_entry_t *entry;

   381

   382

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

   383

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

   384

            return -1;

   385

   386

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

   387

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

   388

        EC_WRITE_U8 (data + 3, 0x00);

   389

        EC_WRITE_U16(data + 4, 0x0000);

   390

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

   391

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

   392

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

   393

   394

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

   395

   396

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

   397

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

   398

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

   399

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

   400

                   sdo->index, i);

   401

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   402

            return -1;

   403

        }

   404

   405

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

   406

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

   407

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

   408

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

   409

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

   410

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

   411

                   sdo->index, i);

   412

            ec_print_data(data, rec_size);

   413

            return -1;

   414

        }

   415

   416

        if (rec_size < 16) {

   417

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

   418

            ec_print_data(data, rec_size);

   419

            return -1;

   420

        }

   421

   422

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

   423

            continue;

   424

   425

        data_size = rec_size - 16;

   426

   427

        if (!(entry = (ec_sdo_entry_t *)

   428

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

   429

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

   430

            return -1;

   431

        }

   432

   433

        entry->subindex = i;

   434

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

   435

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

   436

   437

        // memory for name string appended to entry

   438

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

   439

   440

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

   441

        entry->name[data_size] = 0;

   442

   443

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

   444

    }

   445

   446

    return 0;

   447

}

   448

   449

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

   450

   451

/**

   452

   Gibt eine SDO-Abort-Meldung aus.

   453

*/

   454

   455

void ec_canopen_abort_msg(uint32_t abort_code)

   456

{

   457

    const ec_code_msg_t *abort_msg;

   458

   459

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

   460

        if (abort_msg->code == abort_code) {

   461

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

   462

                   abort_msg->code, abort_msg->message);

   463

            return;

   464

        }

   465

    }

   466

   467

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

   468

}

   469

   470

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

   471

   472

const ec_code_msg_t sdo_abort_messages[] = {

   473

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

   474

    {0x05040000, "SDO protocol timeout"},

   475

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

   476

    {0x05040005, "Out of memory"},

   477

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

   478

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

   479

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

   480

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

   481

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

   482

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

   483

     " exceed the PDO length"},

   484

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

   485

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

   486

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

   487

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

   488

     " not match"},

   489

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