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

void ec_slave_clear(struct kobject *);

    56

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

    57

ssize_t ec_store_slave_attribute(struct kobject *, struct attribute *,

    58

                                 const char *, size_t);

    59

    60

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

    61

    62

/** \cond */

    63

    64

EC_SYSFS_READ_ATTR(info);

    65

EC_SYSFS_READ_WRITE_ATTR(state);

    66

EC_SYSFS_READ_WRITE_ATTR(eeprom);

    67

    68

static struct attribute *def_attrs[] = {

    69

    &attr_info,

    70

    &attr_state,

    71

    &attr_eeprom,

    72

    NULL,

    73

};

    74

    75

static struct sysfs_ops sysfs_ops = {

    76

    .show = ec_show_slave_attribute,

    77

    .store = ec_store_slave_attribute

    78

};

    79

    80

static struct kobj_type ktype_ec_slave = {

    81

    .release = ec_slave_clear,

    82

    .sysfs_ops = &sysfs_ops,

    83

    .default_attrs = def_attrs

    84

};

    85

    86

/** \endcond */

    87

    88

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

    89

    90

/**

    91

   Slave constructor.

    92

   \return 0 in case of success, else < 0

    93

*/

    94

    95

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

    96

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

    97

                  uint16_t ring_position, /**< ring position */

    98

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

    99

                  )

   100

{

   101

    unsigned int i;

   102

   103

    slave->ring_position = ring_position;

   104

    slave->station_address = station_address;

   105

   106

    // init kobject and add it to the hierarchy

   107

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

   108

    kobject_init(&slave->kobj);

   109

    slave->kobj.ktype = &ktype_ec_slave;

   110

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

   111

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

   112

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

   113

        kobject_put(&slave->kobj);

   114

        return -1;

   115

    }

   116

   117

    slave->master = master;

   118

   119

    slave->requested_state = EC_SLAVE_STATE_UNKNOWN;

   120

    slave->current_state = EC_SLAVE_STATE_UNKNOWN;

   121

    slave->error_flag = 0;

   122

    slave->online = 1;

   123

    slave->fmmu_count = 0;

   124

    slave->registered = 0;

   125

   126

    slave->coupler_index = 0;

   127

    slave->coupler_subindex = 0xFFFF;

   128

   129

    slave->base_type = 0;

   130

    slave->base_revision = 0;

   131

    slave->base_build = 0;

   132

    slave->base_fmmu_count = 0;

   133

    slave->base_sync_count = 0;

   134

   135

    slave->eeprom_data = NULL;

   136

    slave->eeprom_size = 0;

   137

    slave->new_eeprom_data = NULL;

   138

    slave->new_eeprom_size = 0;

   139

   140

    slave->sii_alias = 0;

   141

    slave->sii_vendor_id = 0;

   142

    slave->sii_product_code = 0;

   143

    slave->sii_revision_number = 0;

   144

    slave->sii_serial_number = 0;

   145

    slave->sii_rx_mailbox_offset = 0;

   146

    slave->sii_rx_mailbox_size = 0;

   147

    slave->sii_tx_mailbox_offset = 0;

   148

    slave->sii_tx_mailbox_size = 0;

   149

    slave->sii_mailbox_protocols = 0;

   150

    slave->sii_group = NULL;

   151

    slave->sii_image = NULL;

   152

    slave->sii_order = NULL;

   153

    slave->sii_name = NULL;

   154

   155

    INIT_LIST_HEAD(&slave->sii_strings);

   156

    INIT_LIST_HEAD(&slave->sii_syncs);

   157

    INIT_LIST_HEAD(&slave->sii_pdos);

   158

    INIT_LIST_HEAD(&slave->sdo_dictionary);

   159

    INIT_LIST_HEAD(&slave->sdo_confs);

   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

   188

    slave = container_of(kobj, ec_slave_t, kobj);

   189

   190

    // free all string objects

   191

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

   192

        list_del(&string->list);

   193

        kfree(string);

   194

    }

   195

   196

    // free all sync managers

   197

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

   198

        list_del(&sync->list);

   199

        kfree(sync);

   200

    }

   201

   202

    // free all PDOs

   203

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

   204

        list_del(&pdo->list);

   205

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

   206

   207

        // free all PDO entries

   208

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

   209

            list_del(&entry->list);

   210

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

   211

            kfree(entry);

   212

        }

   213

   214

        kfree(pdo);

   215

    }

   216

   217

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

   218

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

   219

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

   220

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

   221

   222

    // free all SDOs

   223

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

   224

        list_del(&sdo->list);

   225

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

   226

   227

        // free all SDO entries

   228

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

   229

            list_del(&en->list);

   230

            kfree(en);

   231

        }

   232

        kfree(sdo);

   233

    }

   234

   235

    // free all SDO configurations

   236

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

   237

        list_del(&sdodata->list);

   238

        kfree(sdodata->data);

   239

        kfree(sdodata);

   240

    }

   241

   242

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

   243

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

   244

}

   245

   246

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

   247

   248

/**

   249

   Fetches data from a STRING category.

   250

   \return 0 in case of success, else < 0

   251

*/

   252

   253

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

   254

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

   255

                           )

   256

{

   257

    unsigned int string_count, i;

   258

    size_t size;

   259

    off_t offset;

   260

    ec_sii_string_t *string;

   261

   262

    string_count = data[0];

   263

    offset = 1;

   264

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

   265

        size = data[offset];

   266

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

   267

        if (!(string = (ec_sii_string_t *)

   268

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

   269

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

   270

            return -1;

   271

        }

   272

        string->size = size;

   273

        // string memory appended to string structure

   274

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

   275

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

   276

        string->data[size] = 0x00;

   277

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

   278

        offset += 1 + size;

   279

    }

   280

   281

    return 0;

   282

}

   283

   284

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

   285

   286

/**

   287

   Fetches data from a GENERAL category.

   288

   \return 0 in case of success, else < 0

   289

*/

   290

   291

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

   292

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

   293

                            )

   294

{

   295

    unsigned int i;

   296

   297

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

   298

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

   299

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

   300

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

   301

   302

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

   303

        slave->sii_physical_layer[i] =

   304

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

   305

}

   306

   307

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

   308

   309

/**

   310

   Fetches data from a SYNC MANAGER category.

   311

   \return 0 in case of success, else < 0

   312

*/

   313

   314

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

   315

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

   316

                        size_t word_count /**< number of words */

   317

                        )

   318

{

   319

    unsigned int sync_count, i;

   320

    ec_sii_sync_t *sync;

   321

   322

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

   323

   324

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

   325

        if (!(sync = (ec_sii_sync_t *)

   326

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

   327

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

   328

            return -1;

   329

        }

   330

   331

        sync->index = i;

   332

        sync->physical_start_address = EC_READ_U16(data);

   333

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

   334

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

   335

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

   336

   337

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

   338

    }

   339

   340

    return 0;

   341

}

   342

   343

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

   344

   345

/**

   346

   Fetches data from a [RT]XPDO category.

   347

   \return 0 in case of success, else < 0

   348

*/

   349

   350

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

   351

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

   352

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

   353

                       ec_sii_pdo_type_t pdo_type /**< PDO type */

   354

                       )

   355

{

   356

    ec_sii_pdo_t *pdo;

   357

    ec_sii_pdo_entry_t *entry;

   358

    unsigned int entry_count, i;

   359

   360

    while (word_count >= 4) {

   361

        if (!(pdo = (ec_sii_pdo_t *)

   362

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

   363

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

   364

            return -1;

   365

        }

   366

   367

        INIT_LIST_HEAD(&pdo->entries);

   368

        pdo->type = pdo_type;

   369

   370

        pdo->index = EC_READ_U16(data);

   371

        entry_count = EC_READ_U8(data + 2);

   372

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

   373

        pdo->name = NULL;

   374

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

   375

   376

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

   377

   378

        word_count -= 4;

   379

        data += 8;

   380

   381

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

   382

            if (!(entry = (ec_sii_pdo_entry_t *)

   383

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

   384

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

   385

                return -1;

   386

            }

   387

   388

            entry->index = EC_READ_U16(data);

   389

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

   390

            entry->name = NULL;

   391

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

   392

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

   393

   394

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

   395

   396

            word_count -= 4;

   397

            data += 8;

   398

        }

   399

    }

   400

   401

    return 0;

   402

}

   403

   404

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

   405

   406

/**

   407

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

   408

   \return 0 in case of success, else < 0

   409

   \todo documentation

   410

*/

   411

   412

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

   413

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

   414

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

   415

                           )

   416

{

   417

    ec_sii_string_t *string;

   418

    char *err_string;

   419

   420

    // Erst alten Speicher freigeben

   421

    if (*ptr) {

   422

        kfree(*ptr);

   423

        *ptr = NULL;

   424

    }

   425

   426

    // Index 0 bedeutet "nicht belegt"

   427

    if (!index) return 0;

   428

   429

    // EEPROM-String mit Index finden und kopieren

   430

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

   431

        if (--index) continue;

   432

   433

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

   434

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

   435

            return -1;

   436

        }

   437

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

   438

        return 0;

   439

    }

   440

   441

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

   442

   443

    err_string = "(string not found)";

   444

   445

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

   446

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

   447

        return -1;

   448

    }

   449

   450

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

   451

    return 0;

   452

}

   453

   454

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

   455

   456

/**

   457

   Prepares an FMMU configuration.

   458

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

   459

   written to the slave in ecrt_master_activate().

   460

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

   461

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

   462

   for each domain.

   463

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

   464

   success.

   465

   \return 0 in case of success, else < 0

   466

*/

   467

   468

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

   469

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

   470

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

   471

                          )

   472

{

   473

    unsigned int i;

   474

   475

    // FMMU configuration already prepared?

   476

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

   477

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

   478

            return 0;

   479

   480

    // reserve new FMMU...

   481

   482

    if (slave->fmmu_count >= slave->base_fmmu_count) {

   483

        EC_ERR("Slave %i FMMU limit reached!\n", slave->ring_position);

   484

        return -1;

   485

    }

   486