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

    12

 *  License, or (at your option) any later version.

    13

 *

    14

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

    15

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

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

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

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT slave methods.

    37

*/

    38

    39

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

    40

    41

#include <linux/module.h>

    42

#include <linux/delay.h>

    43

    44

#include "globals.h"

    45

#include "slave.h"

    46

#include "datagram.h"

    47

#include "master.h"

    48

    49

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

    50

    51

extern const ec_code_msg_t al_status_messages[];

    52

    53

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

    54

    55

ssize_t ec_show_slave_attribute(struct kobject *, struct attribute *, char *);

    56

ssize_t ec_store_slave_attribute(struct kobject *, struct attribute *,

    57

                                 const char *, size_t);

    58

    59

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

    60

    61

/** \cond */

    62

    63

EC_SYSFS_READ_ATTR(info);

    64

EC_SYSFS_READ_WRITE_ATTR(state);

    65

EC_SYSFS_READ_WRITE_ATTR(eeprom);

    66

    67

static struct attribute *def_attrs[] = {

    68

    &attr_info,

    69

    &attr_state,

    70

    &attr_eeprom,

    71

    NULL,

    72

};

    73

    74

static struct sysfs_ops sysfs_ops = {

    75

    .show = ec_show_slave_attribute,

    76

    .store = ec_store_slave_attribute

    77

};

    78

    79

static struct kobj_type ktype_ec_slave = {

    80

    .release = ec_slave_clear,

    81

    .sysfs_ops = &sysfs_ops,

    82

    .default_attrs = def_attrs

    83

};

    84

    85

/** \endcond */

    86

    87

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

    88

    89

/**

    90

   Slave constructor.

    91

   \return 0 in case of success, else < 0

    92

*/

    93

    94

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

    95

                  ec_master_t *master, /**< EtherCAT master */

    96

                  uint16_t ring_position, /**< ring position */

    97

                  uint16_t station_address /**< station address to configure */

    98

                  )

    99

{

   100

    unsigned int i;

   101

   102

    slave->ring_position = ring_position;

   103

    slave->station_address = station_address;

   104

   105

    // init kobject and add it to the hierarchy

   106

    memset(&slave->kobj, 0x00, sizeof(struct kobject));

   107

    kobject_init(&slave->kobj);

   108

    slave->kobj.ktype = &ktype_ec_slave;

   109

    slave->kobj.parent = &master->kobj;

   110

    if (kobject_set_name(&slave->kobj, "slave%03i", slave->ring_position)) {

   111

        EC_ERR("Failed to set kobject name.\n");

   112

        kobject_put(&slave->kobj);

   113

        return -1;

   114

    }

   115

   116

    slave->master = master;

   117

   118

    slave->requested_state = EC_SLAVE_STATE_UNKNOWN;

   119

    slave->current_state = EC_SLAVE_STATE_UNKNOWN;

   120

    slave->error_flag = 0;

   121

    slave->online = 1;

   122

    slave->fmmu_count = 0;

   123

    slave->registered = 0;

   124

   125

    slave->coupler_index = 0;

   126

    slave->coupler_subindex = 0xFFFF;

   127

   128

    slave->base_type = 0;

   129

    slave->base_revision = 0;

   130

    slave->base_build = 0;

   131

    slave->base_fmmu_count = 0;

   132

    slave->base_sync_count = 0;

   133

   134

    slave->eeprom_data = NULL;

   135

    slave->eeprom_size = 0;

   136

    slave->new_eeprom_data = NULL;

   137

    slave->new_eeprom_size = 0;

   138

   139

    slave->sii_alias = 0;

   140

    slave->sii_vendor_id = 0;

   141

    slave->sii_product_code = 0;

   142

    slave->sii_revision_number = 0;

   143

    slave->sii_serial_number = 0;

   144

    slave->sii_rx_mailbox_offset = 0;

   145

    slave->sii_rx_mailbox_size = 0;

   146

    slave->sii_tx_mailbox_offset = 0;

   147

    slave->sii_tx_mailbox_size = 0;

   148

    slave->sii_mailbox_protocols = 0;

   149

    slave->sii_group = NULL;

   150

    slave->sii_image = NULL;

   151

    slave->sii_order = NULL;

   152

    slave->sii_name = NULL;

   153

   154

    INIT_LIST_HEAD(&slave->sii_strings);

   155

    INIT_LIST_HEAD(&slave->sii_syncs);

   156

    INIT_LIST_HEAD(&slave->sii_pdos);

   157

    INIT_LIST_HEAD(&slave->sdo_dictionary);

   158

    INIT_LIST_HEAD(&slave->sdo_confs);

   159

    INIT_LIST_HEAD(&slave->varsize_fields);

   160

   161

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

   162

        slave->dl_link[i] = 0;

   163

        slave->dl_loop[i] = 0;

   164

        slave->dl_signal[i] = 0;

   165

        slave->sii_physical_layer[i] = 0xFF;

   166

    }

   167

   168

    return 0;

   169

}

   170

   171

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

   172

   173

/**

   174

   Slave destructor.

   175

*/

   176

   177

void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */)

   178

{

   179

    ec_slave_t *slave;

   180

    ec_sii_string_t *string, *next_str;

   181

    ec_sii_sync_t *sync, *next_sync;

   182

    ec_sii_pdo_t *pdo, *next_pdo;

   183

    ec_sii_pdo_entry_t *entry, *next_ent;

   184

    ec_sdo_t *sdo, *next_sdo;

   185

    ec_sdo_entry_t *en, *next_en;

   186

    ec_sdo_data_t *sdodata, *next_sdodata;

   187

    ec_varsize_t *var, *next_var;

   188

   189

    slave = container_of(kobj, ec_slave_t, kobj);

   190

   191

    // free all string objects

   192

    list_for_each_entry_safe(string, next_str, &slave->sii_strings, list) {

   193

        list_del(&string->list);

   194

        kfree(string);

   195

    }

   196

   197

    // free all sync managers

   198

    list_for_each_entry_safe(sync, next_sync, &slave->sii_syncs, list) {

   199

        list_del(&sync->list);

   200

        kfree(sync);

   201

    }

   202

   203

    // free all PDOs

   204

    list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) {

   205

        list_del(&pdo->list);

   206

        if (pdo->name) kfree(pdo->name);

   207

   208

        // free all PDO entries

   209

        list_for_each_entry_safe(entry, next_ent, &pdo->entries, list) {

   210

            list_del(&entry->list);

   211

            if (entry->name) kfree(entry->name);

   212

            kfree(entry);

   213

        }

   214

   215

        kfree(pdo);

   216

    }

   217

   218

    if (slave->sii_group) kfree(slave->sii_group);

   219

    if (slave->sii_image) kfree(slave->sii_image);

   220

    if (slave->sii_order) kfree(slave->sii_order);

   221

    if (slave->sii_name) kfree(slave->sii_name);

   222

   223

    // free all SDOs

   224

    list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {

   225

        list_del(&sdo->list);

   226

        if (sdo->name) kfree(sdo->name);

   227

   228

        // free all SDO entries

   229

        list_for_each_entry_safe(en, next_en, &sdo->entries, list) {

   230

            list_del(&en->list);

   231

            kfree(en);

   232

        }

   233

        kfree(sdo);

   234

    }

   235

   236

    // free all SDO configurations

   237

    list_for_each_entry_safe(sdodata, next_sdodata, &slave->sdo_confs, list) {

   238

        list_del(&sdodata->list);

   239

        kfree(sdodata->data);

   240

        kfree(sdodata);

   241

    }

   242

   243

    // free information about variable sized data fields

   244

    list_for_each_entry_safe(var, next_var, &slave->varsize_fields, list) {

   245

        list_del(&var->list);

   246

        kfree(var);

   247

    }

   248

   249

    if (slave->eeprom_data) kfree(slave->eeprom_data);

   250

    if (slave->new_eeprom_data) kfree(slave->new_eeprom_data);

   251

}

   252

   253

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

   254

   255

/**

   256

   Fetches data from a STRING category.

   257

   \return 0 in case of success, else < 0

   258

*/

   259

   260

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

   261

                           const uint8_t *data /**< category data */

   262

                           )

   263

{

   264

    unsigned int string_count, i;

   265

    size_t size;

   266

    off_t offset;

   267

    ec_sii_string_t *string;

   268

   269

    string_count = data[0];

   270

    offset = 1;

   271

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

   272

        size = data[offset];

   273

        // allocate memory for string structure and data at a single blow

   274

        if (!(string = (ec_sii_string_t *)

   275

              kmalloc(sizeof(ec_sii_string_t) + size + 1, GFP_ATOMIC))) {

   276

            EC_ERR("Failed to allocate string memory.\n");

   277

            return -1;

   278

        }

   279

        string->size = size;

   280

        // string memory appended to string structure

   281

        string->data = (char *) string + sizeof(ec_sii_string_t);

   282

        memcpy(string->data, data + offset + 1, size);

   283

        string->data[size] = 0x00;

   284

        list_add_tail(&string->list, &slave->sii_strings);

   285

        offset += 1 + size;

   286

    }

   287

   288

    return 0;

   289

}

   290

   291

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

   292

   293

/**

   294

   Fetches data from a GENERAL category.

   295

   \return 0 in case of success, else < 0

   296

*/

   297

   298

void ec_slave_fetch_general(ec_slave_t *slave, /**< EtherCAT slave */

   299

                            const uint8_t *data /**< category data */

   300

                            )

   301

{

   302

    unsigned int i;

   303

   304

    ec_slave_locate_string(slave, data[0], &slave->sii_group);

   305

    ec_slave_locate_string(slave, data[1], &slave->sii_image);

   306

    ec_slave_locate_string(slave, data[2], &slave->sii_order);

   307

    ec_slave_locate_string(slave, data[3], &slave->sii_name);

   308

   309

    for (i = 0; i < 4; i++)

   310

        slave->sii_physical_layer[i] =

   311

            (data[4] & (0x03 << (i * 2))) >> (i * 2);

   312

}

   313

   314

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

   315

   316

/**

   317

   Fetches data from a SYNC MANAGER category.

   318

   \return 0 in case of success, else < 0

   319

*/

   320

   321

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

   322

                        const uint8_t *data, /**< category data */

   323

                        size_t word_count /**< number of words */

   324

                        )

   325

{

   326

    unsigned int sync_count, i;

   327

    ec_sii_sync_t *sync;

   328

   329

    sync_count = word_count / 4; // sync manager struct is 4 words long

   330

   331

    for (i = 0; i < sync_count; i++, data += 8) {

   332

        if (!(sync = (ec_sii_sync_t *)

   333

              kmalloc(sizeof(ec_sii_sync_t), GFP_ATOMIC))) {

   334

            EC_ERR("Failed to allocate Sync-Manager memory.\n");

   335

            return -1;

   336

        }

   337

   338

        sync->index = i;

   339

        sync->physical_start_address = EC_READ_U16(data);

   340

        sync->length                 = EC_READ_U16(data + 2);

   341

        sync->control_register       = EC_READ_U8 (data + 4);

   342

        sync->enable                 = EC_READ_U8 (data + 6);

   343

   344

        list_add_tail(&sync->list, &slave->sii_syncs);

   345

    }

   346

   347

    return 0;

   348

}

   349

   350

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

   351

   352

/**

   353

   Fetches data from a [RT]XPDO category.

   354

   \return 0 in case of success, else < 0

   355

*/

   356

   357

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

   358

                       const uint8_t *data, /**< category data */

   359

                       size_t word_count, /**< number of words */

   360

                       ec_sii_pdo_type_t pdo_type /**< PDO type */

   361

                       )

   362

{

   363

    ec_sii_pdo_t *pdo;

   364

    ec_sii_pdo_entry_t *entry;

   365

    unsigned int entry_count, i;

   366

   367

    while (word_count >= 4) {

   368

        if (!(pdo = (ec_sii_pdo_t *)

   369

              kmalloc(sizeof(ec_sii_pdo_t), GFP_ATOMIC))) {

   370

            EC_ERR("Failed to allocate PDO memory.\n");

   371

            return -1;

   372

        }

   373

   374

        INIT_LIST_HEAD(&pdo->entries);

   375

        pdo->type = pdo_type;

   376

   377

        pdo->index = EC_READ_U16(data);

   378

        entry_count = EC_READ_U8(data + 2);

   379

        pdo->sync_index = EC_READ_U8(data + 3);

   380

        pdo->name = NULL;

   381

        ec_slave_locate_string(slave, EC_READ_U8(data + 5), &pdo->name);

   382

   383

        list_add_tail(&pdo->list, &slave->sii_pdos);

   384

   385

        word_count -= 4;

   386

        data += 8;

   387

   388

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

   389

            if (!(entry = (ec_sii_pdo_entry_t *)

   390

                  kmalloc(sizeof(ec_sii_pdo_entry_t), GFP_ATOMIC))) {

   391

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

   392

                return -1;

   393

            }

   394

   395

            entry->index = EC_READ_U16(data);

   396

            entry->subindex = EC_READ_U8(data + 2);

   397

            entry->name = NULL;

   398

            ec_slave_locate_string(slave, EC_READ_U8(data + 3), &entry->name);

   399

            entry->bit_length = EC_READ_U8(data + 5);

   400

   401

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

   402

   403

            word_count -= 4;

   404

            data += 8;

   405

        }

   406

    }

   407

   408

    return 0;

   409

}

   410

   411

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

   412

   413

/**

   414

   Searches the string list for an index and allocates a new string.

   415

   \return 0 in case of success, else < 0

   416

   \todo documentation

   417

*/

   418

   419

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

   420

                           unsigned int index, /**< string index */

   421

                           char **ptr /**< Address of the string pointer */

   422

                           )

   423

{

   424

    ec_sii_string_t *string;

   425

    char *err_string;

   426

   427

    // Erst alten Speicher freigeben

   428

    if (*ptr) {

   429

        kfree(*ptr);

   430

        *ptr = NULL;

   431

    }

   432

   433

    // Index 0 bedeutet "nicht belegt"

   434

    if (!index) return 0;

   435

   436

    // EEPROM-String mit Index finden und kopieren

   437

    list_for_each_entry(string, &slave->sii_strings, list) {

   438

        if (--index) continue;

   439

   440

        if (!(*ptr = (char *) kmalloc(string->size + 1, GFP_ATOMIC))) {

   441

            EC_ERR("Unable to allocate string memory.\n");

   442

            return -1;

   443

        }

   444

        memcpy(*ptr, string->data, string->size + 1);

   445

        return 0;

   446

    }

   447

   448

    EC_WARN("String %i not found in slave %i.\n", index, slave->ring_position);

   449

   450

    err_string = "(string not found)";

   451

   452

    if (!(*ptr = (char *) kmalloc(strlen(err_string) + 1, GFP_ATOMIC))) {

   453

        EC_WARN("Unable to allocate string memory.\n");

   454

        return -1;

   455

    }

   456

   457

    memcpy(*ptr, err_string, strlen(err_string) + 1);

   458

    return 0;

   459

}

   460

   461

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

   462

   463

/**

   464

   Prepares an FMMU configuration.

   465

   Configuration data for the FMMU is saved in the slave structure and is

   466

   written to the slave in ecrt_master_activate().

   467

   The FMMU configuration is done in a way, that the complete data range

   468

   of the corresponding sync manager is covered. Seperate FMMUs are configured

   469

   for each domain.

   470

   If the FMMU configuration is already prepared, the function returns with

   471

   success.

   472

   \return 0 in case of success, else < 0

   473

*/

   474

   475

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

   476

                          const ec_domain_t *domain, /**< domain */

   477

                          const ec_sii_sync_t *sync  /**< sync manager */

   478

                          )

   479

{

   480

    unsigned int i;

   481

   482

    // FMMU configuration already prepared?

   483

    for (i = 0; i < slave->fmmu_count; i++)

   484

        if (slave->fmmus[i].domain == domain && slave->fmmus[i].sync == sync)

   485

            return 0;

   486