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->varsize_fields);

   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_varsize_t *var, *next_var;

   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 information about variable sized data fields

   235

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

   236

        list_del(&var->list);

   237

        kfree(var);

   238

    }

   239

   240

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

   241

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

   242

}

   243

   244

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

   245

   246

/**

   247

   Fetches data from a STRING category.

   248

   \return 0 in case of success, else < 0

   249

*/

   250

   251

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

   252

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

   253

                           )

   254

{

   255

    unsigned int string_count, i;

   256

    size_t size;

   257

    off_t offset;

   258

    ec_sii_string_t *string;

   259

   260

    string_count = data[0];

   261

    offset = 1;

   262

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

   263

        size = data[offset];

   264

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

   265

        if (!(string = (ec_sii_string_t *)

   266

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

   267

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

   268

            return -1;

   269

        }

   270

        string->size = size;

   271

        // string memory appended to string structure

   272

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

   273

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

   274

        string->data[size] = 0x00;

   275

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

   276

        offset += 1 + size;

   277

    }

   278

   279

    return 0;

   280

}

   281

   282

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

   283

   284

/**

   285

   Fetches data from a GENERAL category.

   286

   \return 0 in case of success, else < 0

   287

*/

   288

   289

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

   290

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

   291

                            )

   292

{

   293

    unsigned int i;

   294

   295

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

   296

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

   297

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

   298

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

   299

   300

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

   301

        slave->sii_physical_layer[i] =

   302

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

   303

}

   304

   305

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

   306

   307

/**

   308

   Fetches data from a SYNC MANAGER category.

   309

   \return 0 in case of success, else < 0

   310

*/

   311

   312

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

   313

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

   314

                        size_t word_count /**< number of words */

   315

                        )

   316

{

   317

    unsigned int sync_count, i;

   318

    ec_sii_sync_t *sync;

   319

   320

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

   321

   322

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

   323

        if (!(sync = (ec_sii_sync_t *)

   324

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

   325

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

   326

            return -1;

   327

        }

   328

   329

        sync->index = i;

   330

        sync->physical_start_address = EC_READ_U16(data);

   331

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

   332

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

   333

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

   334

   335

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

   336

    }

   337

   338

    return 0;

   339

}

   340

   341

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

   342

   343

/**

   344

   Fetches data from a [RT]XPDO category.

   345

   \return 0 in case of success, else < 0

   346

*/

   347

   348

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

   349

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

   350

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

   351

                       ec_sii_pdo_type_t pdo_type /**< PDO type */

   352

                       )

   353

{

   354

    ec_sii_pdo_t *pdo;

   355

    ec_sii_pdo_entry_t *entry;

   356

    unsigned int entry_count, i;

   357

   358

    while (word_count >= 4) {

   359

        if (!(pdo = (ec_sii_pdo_t *)

   360

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

   361

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

   362

            return -1;

   363

        }

   364

   365

        INIT_LIST_HEAD(&pdo->entries);

   366

        pdo->type = pdo_type;

   367

   368

        pdo->index = EC_READ_U16(data);

   369

        entry_count = EC_READ_U8(data + 2);

   370

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

   371

        pdo->name = NULL;

   372

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

   373

   374

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

   375

   376

        word_count -= 4;

   377

        data += 8;

   378

   379

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

   380

            if (!(entry = (ec_sii_pdo_entry_t *)

   381

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

   382

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

   383

                return -1;

   384

            }

   385

   386

            entry->index = EC_READ_U16(data);

   387

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

   388

            entry->name = NULL;

   389

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

   390

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

   391

   392

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

   393

   394

            word_count -= 4;

   395

            data += 8;

   396

        }

   397

    }

   398

   399

    return 0;

   400

}

   401

   402

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

   403

   404

/**

   405

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

   406

   \return 0 in case of success, else < 0

   407

   \todo documentation

   408

*/

   409

   410

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

   411

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

   412

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

   413

                           )

   414

{

   415

    ec_sii_string_t *string;

   416

    char *err_string;

   417

   418

    // Erst alten Speicher freigeben

   419

    if (*ptr) {

   420

        kfree(*ptr);

   421

        *ptr = NULL;

   422

    }

   423

   424

    // Index 0 bedeutet "nicht belegt"

   425

    if (!index) return 0;

   426

   427

    // EEPROM-String mit Index finden und kopieren

   428

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

   429

        if (--index) continue;

   430

   431

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

   432

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

   433

            return -1;

   434

        }

   435

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

   436

        return 0;

   437

    }

   438

   439

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

   440

   441

    err_string = "(string not found)";

   442

   443

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

   444

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

   445

        return -1;

   446

    }

   447

   448

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

   449

    return 0;

   450

}

   451

   452

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

   453

   454

/**

   455

   Prepares an FMMU configuration.

   456

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

   457

   written to the slave in ecrt_master_activate().

   458

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

   459

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

   460

   for each domain.

   461

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

   462

   success.

   463

   \return 0 in case of success, else < 0

   464

*/

   465

   466

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

   467

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

   468

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

   469

                          )

   470

{

   471

    unsigned int i;

   472

   473

    // FMMU configuration already prepared?

   474

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

   475

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

   476

            return 0;

   477

   478

    // reserve new FMMU...

   479

   480

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

   481

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

   482

        return -1;

   483

    }

   484

   485

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

   486

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