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

   160

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

   161

        slave->dl_link[i] = 0;

   162

        slave->dl_loop[i] = 0;

   163

        slave->dl_signal[i] = 0;

   164

        slave->sii_physical_layer[i] = 0xFF;

   165

    }

   166

   167

    return 0;

   168

}

   169

   170

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

   171

   172

/**

   173

   Slave destructor.

   174

*/

   175

   176

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

   177

{

   178

    ec_slave_t *slave;

   179

    ec_sii_string_t *string, *next_str;

   180

    ec_sii_sync_t *sync, *next_sync;

   181

    ec_sii_pdo_t *pdo, *next_pdo;

   182

    ec_sii_pdo_entry_t *entry, *next_ent;

   183

    ec_sdo_t *sdo, *next_sdo;

   184

    ec_sdo_entry_t *en, *next_en;

   185

    ec_sdo_data_t *sdodata, *next_sdodata;

   186

   187

    slave = container_of(kobj, ec_slave_t, kobj);

   188

   189

    // free all string objects

   190

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

   191

        list_del(&string->list);

   192

        kfree(string);

   193

    }

   194

   195

    // free all sync managers

   196

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

   197

        list_del(&sync->list);

   198

        kfree(sync);

   199

    }

   200

   201

    // free all PDOs

   202

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

   203

        list_del(&pdo->list);

   204

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

   205

   206

        // free all PDO entries

   207

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

   208

            list_del(&entry->list);

   209

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

   210

            kfree(entry);

   211

        }

   212

   213

        kfree(pdo);

   214

    }

   215

   216

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

   217

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

   218

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

   219

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

   220

   221

    // free all SDOs

   222

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

   223

        list_del(&sdo->list);

   224

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

   225

   226

        // free all SDO entries

   227

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

   228

            list_del(&en->list);

   229

            kfree(en);

   230

        }

   231

        kfree(sdo);

   232

    }

   233

   234

    // free all SDO configurations

   235

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

   236

        list_del(&sdodata->list);

   237

        kfree(sdodata->data);

   238

        kfree(sdodata);

   239

    }

   240

   241

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

   242

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

   243

}

   244

   245

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

   246

   247

/**

   248

   Fetches data from a STRING category.

   249

   \return 0 in case of success, else < 0

   250

*/

   251

   252

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

   253

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

   254

                           )

   255

{

   256

    unsigned int string_count, i;

   257

    size_t size;

   258

    off_t offset;

   259

    ec_sii_string_t *string;

   260

   261

    string_count = data[0];

   262

    offset = 1;

   263

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

   264

        size = data[offset];

   265

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

   266

        if (!(string = (ec_sii_string_t *)

   267

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

   268

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

   269

            return -1;

   270

        }

   271

        string->size = size;

   272

        // string memory appended to string structure

   273

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

   274

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

   275

        string->data[size] = 0x00;

   276

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

   277

        offset += 1 + size;

   278

    }

   279

   280

    return 0;

   281

}

   282

   283

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

   284

   285

/**

   286

   Fetches data from a GENERAL category.

   287

   \return 0 in case of success, else < 0

   288

*/

   289

   290

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

   291

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

   292

                            )

   293

{

   294

    unsigned int i;

   295

   296

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

   297

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

   298

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

   299

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

   300

   301

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

   302

        slave->sii_physical_layer[i] =

   303

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

   304

}

   305

   306

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

   307

   308

/**

   309

   Fetches data from a SYNC MANAGER category.

   310

   \return 0 in case of success, else < 0

   311

*/

   312

   313

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

   314

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

   315

                        size_t word_count /**< number of words */

   316

                        )

   317

{

   318

    unsigned int sync_count, i;

   319

    ec_sii_sync_t *sync;

   320

   321

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

   322

   323

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

   324

        if (!(sync = (ec_sii_sync_t *)

   325

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

   326

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

   327

            return -1;

   328

        }

   329

   330

        sync->index = i;

   331

        sync->physical_start_address = EC_READ_U16(data);

   332

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

   333

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

   334

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

   335

   336

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

   337

    }

   338

   339

    return 0;

   340

}

   341

   342

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

   343

   344

/**

   345

   Fetches data from a [RT]XPDO category.

   346

   \return 0 in case of success, else < 0

   347

*/

   348

   349

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

   350

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

   351

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

   352

                       ec_sii_pdo_type_t pdo_type /**< PDO type */

   353

                       )

   354

{

   355

    ec_sii_pdo_t *pdo;

   356

    ec_sii_pdo_entry_t *entry;

   357

    unsigned int entry_count, i;

   358

   359

    while (word_count >= 4) {

   360

        if (!(pdo = (ec_sii_pdo_t *)

   361

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

   362

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

   363

            return -1;

   364

        }

   365

   366

        INIT_LIST_HEAD(&pdo->entries);

   367

        pdo->type = pdo_type;

   368

   369

        pdo->index = EC_READ_U16(data);

   370

        entry_count = EC_READ_U8(data + 2);

   371

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

   372

        pdo->name = NULL;

   373

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

   374

   375

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

   376

   377

        word_count -= 4;

   378

        data += 8;

   379

   380

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

   381

            if (!(entry = (ec_sii_pdo_entry_t *)

   382

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

   383

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

   384

                return -1;

   385

            }

   386

   387

            entry->index = EC_READ_U16(data);

   388

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

   389

            entry->name = NULL;

   390

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

   391

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

   392

   393

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

   394

   395

            word_count -= 4;

   396

            data += 8;

   397

        }

   398

    }

   399

   400

    return 0;

   401

}

   402

   403

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

   404

   405

/**

   406

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

   407

   \return 0 in case of success, else < 0

   408

   \todo documentation

   409

*/

   410

   411

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

   412

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

   413

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

   414

                           )

   415

{

   416

    ec_sii_string_t *string;

   417

    char *err_string;

   418

   419

    // Erst alten Speicher freigeben

   420

    if (*ptr) {

   421

        kfree(*ptr);

   422

        *ptr = NULL;

   423

    }

   424

   425

    // Index 0 bedeutet "nicht belegt"

   426

    if (!index) return 0;

   427

   428

    // EEPROM-String mit Index finden und kopieren

   429

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

   430

        if (--index) continue;

   431

   432

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

   433

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

   434

            return -1;

   435

        }

   436

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

   437

        return 0;

   438

    }

   439

   440

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

   441

   442

    err_string = "(string not found)";

   443

   444

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

   445

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

   446

        return -1;

   447

    }

   448

   449

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

   450

    return 0;

   451

}

   452

   453

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

   454

   455

/**

   456

   Prepares an FMMU configuration.

   457

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

   458

   written to the slave in ecrt_master_activate().

   459

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

   460

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

   461

   for each domain.

   462

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

   463

   success.

   464

   \return 0 in case of success, else < 0

   465

*/

   466

   467

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

   468

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

   469

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

   470

                          )

   471

{

   472

    unsigned int i;

   473

   474

    // FMMU configuration already prepared?

   475

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

   476

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

   477

            return 0;

   478

   479

    // reserve new FMMU...

   480

   481

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

   482

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

   483

        return -1;

   484

    }

   485

   486

    slave->fmmus[slave->fmmu_count].domain = domain;