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_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

        kobject_del(&sdo->kobj);

   225

        kobject_put(&sdo->kobj);

   226

    }

   227

   228

    // free all SDO configurations

   229

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

   230

        list_del(&sdodata->list);

   231

        kfree(sdodata->data);

   232

        kfree(sdodata);

   233

    }

   234

   235

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

   236

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

   237

}

   238

   239

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

   240

   241

/**

   242

   Fetches data from a STRING category.

   243

   \return 0 in case of success, else < 0

   244

*/

   245

   246

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

   247

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

   248

                           )

   249

{

   250

    unsigned int string_count, i;

   251

    size_t size;

   252

    off_t offset;

   253

    ec_sii_string_t *string;

   254

   255

    string_count = data[0];

   256

    offset = 1;

   257

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

   258

        size = data[offset];

   259

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

   260

        if (!(string = (ec_sii_string_t *)

   261

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

   262

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

   263

            return -1;

   264

        }

   265

        string->size = size;

   266

        // string memory appended to string structure

   267

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

   268

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

   269

        string->data[size] = 0x00;

   270

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

   271

        offset += 1 + size;

   272

    }

   273

   274

    return 0;

   275

}

   276

   277

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

   278

   279

/**

   280

   Fetches data from a GENERAL category.

   281

   \return 0 in case of success, else < 0

   282

*/

   283

   284

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

   285

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

   286

                            )

   287

{

   288

    unsigned int i;

   289

   290

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

   291

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

   292

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

   293

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

   294

   295

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

   296

        slave->sii_physical_layer[i] =

   297

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

   298

}

   299

   300

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

   301

   302

/**

   303

   Fetches data from a SYNC MANAGER category.

   304

   \return 0 in case of success, else < 0

   305

*/

   306

   307

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

   308

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

   309

                        size_t word_count /**< number of words */

   310

                        )

   311

{

   312

    unsigned int sync_count, i;

   313

    ec_sii_sync_t *sync;

   314

   315

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

   316

   317

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

   318

        if (!(sync = (ec_sii_sync_t *)

   319

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

   320

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

   321

            return -1;

   322

        }

   323

   324

        sync->index = i;

   325

        sync->physical_start_address = EC_READ_U16(data);

   326

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

   327

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

   328

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

   329

   330

        sync->est_length = 0;

   331

   332

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

   333

    }

   334

   335

    return 0;

   336

}

   337

   338

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

   339

   340

/**

   341

   Fetches data from a [RT]XPDO category.

   342

   \return 0 in case of success, else < 0

   343

*/

   344

   345

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

   346

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

   347

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

   348

                       ec_sii_pdo_type_t pdo_type /**< PDO type */

   349

                       )

   350

{

   351

    ec_sii_pdo_t *pdo;

   352

    ec_sii_pdo_entry_t *entry;

   353

    unsigned int entry_count, i;

   354

   355

    while (word_count >= 4) {

   356

        if (!(pdo = (ec_sii_pdo_t *)

   357

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

   358

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

   359

            return -1;

   360

        }

   361

   362

        INIT_LIST_HEAD(&pdo->entries);

   363

        pdo->type = pdo_type;

   364

   365

        pdo->index = EC_READ_U16(data);

   366

        entry_count = EC_READ_U8(data + 2);

   367

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

   368

        pdo->name = NULL;

   369

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

   370

   371

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

   372

   373

        word_count -= 4;

   374

        data += 8;

   375

   376

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

   377

            if (!(entry = (ec_sii_pdo_entry_t *)

   378

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

   379

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

   380

                return -1;

   381

            }

   382

   383

            entry->index = EC_READ_U16(data);

   384

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

   385

            entry->name = NULL;

   386

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

   387

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

   388

   389

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

   390

   391

            word_count -= 4;

   392

            data += 8;

   393

        }

   394

    }

   395

   396

    return 0;

   397

}

   398

   399

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

   400

   401

/**

   402

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

   403

   \return 0 in case of success, else < 0

   404

   \todo documentation

   405

*/

   406

   407

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

   408

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

   409

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

   410

                           )

   411

{

   412

    ec_sii_string_t *string;

   413

    char *err_string;

   414

   415

    // Erst alten Speicher freigeben

   416

    if (*ptr) {

   417

        kfree(*ptr);

   418

        *ptr = NULL;

   419

    }

   420

   421

    // Index 0 bedeutet "nicht belegt"

   422

    if (!index) return 0;

   423

   424

    // EEPROM-String mit Index finden und kopieren

   425

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

   426

        if (--index) continue;

   427

   428

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

   429

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

   430

            return -1;

   431

        }

   432

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

   433

        return 0;

   434

    }

   435

   436

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

   437

   438

    err_string = "(string not found)";

   439

   440

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

   441

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

   442

        return -1;

   443

    }

   444

   445

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

   446

    return 0;

   447

}

   448

   449

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

   450

   451

/**

   452

   Prepares an FMMU configuration.

   453

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

   454

   written to the slave in ecrt_master_activate().

   455

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

   456

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

   457

   for each domain.

   458

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

   459

   success.

   460

   \return 0 in case of success, else < 0

   461

*/

   462

   463

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

   464

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

   465

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

   466

                          )

   467

{

   468

    unsigned int i;

   469

    ec_fmmu_t *fmmu;

   470

   471

    // FMMU configuration already prepared?

   472

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

   473

        fmmu = &slave->fmmus[i];

   474

        if (fmmu->domain == domain && fmmu->sync == sync)

   475

            return 0;

   476

    }

   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

    fmmu = &slave->fmmus[slave->fmmu_count];