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

/**

    20

   SDO Abort Code Messages

    21

*/

    22

    23

typedef struct

    24

{

    25

    uint32_t code;

    26

    const char *message;

    27

}

    28

ec_sdo_abort_message_t;

    29

    30

const ec_sdo_abort_message_t sdo_abort_messages[];

    31

    32

void ec_canopen_abort_msg(uint32_t);

    33

int ec_slave_fetch_sdo_descriptions(ec_slave_t *);

    34

    35

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

    36

    37

/**

    38

   Schreibt ein CANopen-SDO (service data object).

    39

 */

    40

    41

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

    42

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

    43

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

    44

                         uint32_t value, /**< Neuer Wert */

    45

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

    46

                         )

    47

{

    48

    uint8_t data[0x0A];

    49

    unsigned int i;

    50

    size_t rec_size;

    51

    52

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

    53

        EC_ERR("Invalid SDO data size: %i!\n", size);

    54

        return -1;

    55

    }

    56

    57

    EC_WRITE_U16(data,     0x02 << 12); // Number (0), Service (SDO request)

    58

    EC_WRITE_U8 (data + 2, 0x23 | ((4 - size) << 2)); // Spec., exp., init.

    59

    EC_WRITE_U16(data + 3, sdo_index);

    60

    EC_WRITE_U8 (data + 5, sdo_subindex);

    61

    62

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

    63

        EC_WRITE_U8(data + 6 + i, value & 0xFF);

    64

        value >>= 8;

    65

    }

    66

    67

    // Mailox senden und empfangen

    68

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

    69

    70

    rec_size = 0x0A;

    71

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

    72

    73

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

    74

        EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req.

    75

        EC_ERR("SDO download of 0x%04X:%X (value %X, size %X) aborted on slave"

    76

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

    77

               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 != 0x03 || // SDO response

    83

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

    84

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

    85

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

    86

    {

    87

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

    88

               slave->ring_position);

    89

        return -1;

    90

    }

    91

    92

    return 0;

    93

}

    94

    95

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

    96

    97

/**

    98

   Liest ein CANopen-SDO (service data object).

    99

 */

   100

   101

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

   102

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

   103

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

   104

                        uint32_t *value /**< Speicher für gel. Wert */

   105

                        )

   106

{

   107

    uint8_t data[0x20];

   108

    size_t rec_size;

   109

   110

    EC_WRITE_U16(data,     0x2000); // Number (0), Service = SDO request

   111

    EC_WRITE_U8 (data + 2, 0x1 << 1 | 0x2 << 5); // Expedited upload request

   112

    EC_WRITE_U16(data + 3, sdo_index);

   113

    EC_WRITE_U8 (data + 5, sdo_subindex);

   114

   115

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

   116

   117

    rec_size = 0x20;

   118

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

   119

   120

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

   121

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

   122

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

   123

               sdo_index, sdo_subindex, slave->ring_position);

   124

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   125

        return -1;

   126

    }

   127

   128

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

   129

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

   130

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

   131

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

   132

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

   133

               slave->ring_position);

   134

        return -1;

   135

    }

   136

   137

    *value = EC_READ_U32(data + 6);

   138

    return 0;

   139

}

   140

   141

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

   142

   143

/**

   144

   Schweibt ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse).

   145

   146

   Siehe ecrt_slave_sdo_write()

   147

   148

   \return 0 wenn alles ok, < 0 bei Fehler

   149

 */

   150

   151

int ecrt_master_sdo_write(ec_master_t *master,

   152

                          /**< EtherCAT-Master */

   153

                          const char *addr,

   154

                          /**< Addresse, siehe ec_master_slave_address() */

   155

                          uint16_t index,

   156

                          /**< SDO-Index */

   157

                          uint8_t subindex,

   158

                          /**< SDO-Subindex */

   159

                          uint32_t value,

   160

                          /**< Neuer Wert */

   161

                          size_t size

   162

                          /**< Größe des Datenfeldes */

   163

                          )

   164

{

   165

    ec_slave_t *slave;

   166

    if (!(slave = ec_master_slave_address(master, addr))) return -1;

   167

    return ecrt_slave_sdo_write(slave, index, subindex, value, size);

   168

}

   169

   170

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

   171

   172

/**

   173

   Liest ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse).

   174

   175

   Siehe ecrt_slave_sdo_read()

   176

   177

   \return 0 wenn alles ok, < 0 bei Fehler

   178

 */

   179

   180

int ecrt_master_sdo_read(ec_master_t *master,

   181

                         /**< EtherCAT-Slave */

   182

                         const char *addr,

   183

                         /**< Addresse, siehe ec_master_slave_address() */

   184

                         uint16_t index,

   185

                         /**< SDO-Index */

   186

                         uint8_t subindex,

   187

                         /**< SDO-Subindex */

   188

                         uint32_t *value

   189

                         /**< Speicher für gel. Wert */

   190

                         )

   191

{

   192

    ec_slave_t *slave;

   193

    if (!(slave = ec_master_slave_address(master, addr))) return -1;

   194

    return ecrt_slave_sdo_read(slave, index, subindex, value);

   195

}

   196

   197

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

   198

   199

/**

   200

   Holt das Object-Dictionary aus dem Slave.

   201

   202

   \return 0, wenn alles ok, sonst < 0

   203

*/

   204

   205

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

   206

{

   207

    uint8_t data[0xF0];

   208

    size_t rec_size;

   209

    unsigned int i, sdo_count;

   210

    ec_sdo_t *sdo;

   211

    uint16_t sdo_index;

   212

   213

    EC_WRITE_U16(data,     0x8000); // Number (0), Service = SDO information

   214

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

   215

    EC_WRITE_U8 (data + 3,   0x00); // res.

   216

    EC_WRITE_U16(data + 4, 0x0000); // fragments left

   217

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

   218

   219

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

   220

   221

    do {

   222

        rec_size = 0xF0;

   223

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

   224

   225

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

   226

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

   227

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

   228

                   slave->ring_position);

   229

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   230

            return -1;

   231

        }

   232

   233

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

   234

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

   235

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

   236

                   slave->ring_position);

   237

            return -1;

   238

        }

   239

   240

        if (rec_size < 8) {

   241

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

   242

            return -1;

   243

        }

   244

   245

        sdo_count = (rec_size - 8) / 2;

   246

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

   247

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

   248

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

   249

   250

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

   251

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

   252

                return -1;

   253

            }

   254

            sdo->index = sdo_index;

   255

            sdo->name = NULL;

   256

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

   257

        }

   258

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

   259

   260

    // Jetzt alle Beschreibungen holen

   261

    if (ec_slave_fetch_sdo_descriptions(slave)) return -1;

   262

   263

    return 0;

   264

}

   265

   266

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

   267

   268

/**

   269

   Holt die Beschreibungen zu allen bereits bekannten SDOs.

   270

   271

   \return 0, wenn alles ok, sonst < 0

   272

*/

   273

   274

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

   275

{

   276

    uint8_t data[0xF0];

   277

    size_t rec_size, name_size;

   278

    ec_sdo_t *sdo;

   279

   280

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

   281

        EC_WRITE_U16(data,     0x8000); // Number (0), Service = SDO inform.

   282

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

   283

        EC_WRITE_U8 (data + 3,   0x00); // res.

   284

        EC_WRITE_U16(data + 4, 0x0000); // fragments left

   285

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

   286

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

   287

   288

        rec_size = 0xF0;

   289

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

   290

            return -1;

   291

   292

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

   293

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

   294

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

   295

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

   296

                   sdo->index);

   297

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   298

            return -1;

   299

        }

   300

   301

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

   302

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

   303

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

   304

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

   305

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

   306

                   sdo->index);

   307

            return -1;

   308

        }

   309

   310

        if (rec_size < 12) {

   311

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

   312

            return -1;

   313

        }

   314

   315

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

   316

        sdo->max_subindex = EC_READ_U8(data + 10);

   317

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

   318

   319

        name_size = rec_size - 12;

   320

        if (!name_size) {

   321

            EC_WARN("Object 0x%04X name size is 0...", sdo->index);

   322

            continue;

   323

        }

   324

   325

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

   326

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

   327

            return -1;

   328

        }

   329

   330

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

   331

        sdo->name[name_size] = 0;

   332

   333

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

   334

            EC_WARN("Fragment follows in object description!\n");

   335

    }

   336

   337

    return 0;

   338

}

   339

   340

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

   341

   342

/**

   343

   Gibt eine SDO-Abort-Meldung aus.

   344

*/

   345

   346

void ec_canopen_abort_msg(uint32_t abort_code)

   347

{

   348

    const ec_sdo_abort_message_t *abort_msg;

   349

   350

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

   351

        if (abort_msg->code == abort_code) {

   352

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

   353

                   abort_msg->code, abort_msg->message);

   354

            return;

   355

        }

   356

    }

   357

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

   358

}

   359

   360

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

   361

   362

const ec_sdo_abort_message_t sdo_abort_messages[] = {

   363

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

   364

    {0x05040000, "SDO protocol timeout"},

   365

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

   366

    {0x05040005, "Out of memory"},

   367

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

   368

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

   369

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

   370

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

   371

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

   372

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

   373

     " exceed the PDO length"},

   374

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

   375

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

   376

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

   377

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

   378

     " not match"},

   379

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

   380

     " high"},

   381

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

   382

     " low"},

   383

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

   384

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

   385

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

   386

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

   387

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

   388

    {0x08000000, "General error"},

   389

    {0x08000020, "Data cannot be transferred or stored to the application"},

   390

    {0x08000021, "Data cannot be transferred or stored to the application"

   391

     " because of local control"},

   392

    {0x08000022, "Data cannot be transferred or stored to the application"

   393

     " because of the present device state"},

   394

    {0x08000023, "Object dictionary dynamic generation fails or no object"

   395

     " dictionary is present"},

   396

    {}

   397

};

   398

   399

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

   400

   401

EXPORT_SYMBOL(ecrt_slave_sdo_write);

   402

EXPORT_SYMBOL(ecrt_slave_sdo_read);

   403

EXPORT_SYMBOL(ecrt_master_sdo_write);

   404

EXPORT_SYMBOL(ecrt_master_sdo_read);

   405

   406

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

   407

   408

/* Emacs-Konfiguration

   409

;;; Local Variables: ***

   410

;;; c-basic-offset:4 ***

   411

;;; End: ***

   412

*/