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 domain methods.

    37

*/

    38

    39

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

    40

    41

#include <linux/module.h>

    42

    43

#include "globals.h"

    44

#include "domain.h"

    45

#include "master.h"

    46

    47

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

    48

    49

void ec_domain_clear_field_regs(ec_domain_t *);

    50

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

    51

    52

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

    53

    54

/** \cond */

    55

    56

EC_SYSFS_READ_ATTR(data_size);

    57

    58

static struct attribute *def_attrs[] = {

    59

    &attr_data_size,

    60

    NULL,

    61

};

    62

    63

static struct sysfs_ops sysfs_ops = {

    64

    .show = &ec_show_domain_attribute,

    65

    .store = NULL

    66

};

    67

    68

static struct kobj_type ktype_ec_domain = {

    69

    .release = ec_domain_clear,

    70

    .sysfs_ops = &sysfs_ops,

    71

    .default_attrs = def_attrs

    72

};

    73

    74

/** \endcond */

    75

    76

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

    77

    78

/**

    79

   Domain constructor.

    80

   \return 0 in case of success, else < 0

    81

*/

    82

    83

int ec_domain_init(ec_domain_t *domain, /**< EtherCAT domain */

    84

                   ec_master_t *master, /**< owning master */

    85

                   unsigned int index /**< domain index */

    86

                   )

    87

{

    88

    domain->master = master;

    89

    domain->index = index;

    90

    domain->data_size = 0;

    91

    domain->base_address = 0;

    92

    domain->response_count = 0xFFFFFFFF;

    93

    94

    INIT_LIST_HEAD(&domain->field_regs);

    95

    INIT_LIST_HEAD(&domain->datagrams);

    96

    97

    // init kobject and add it to the hierarchy

    98

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

    99

    kobject_init(&domain->kobj);

   100

    domain->kobj.ktype = &ktype_ec_domain;

   101

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

   102

    if (kobject_set_name(&domain->kobj, "domain%i", index)) {

   103

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

   104

        return -1;

   105

    }

   106

   107

    return 0;

   108

}

   109

   110

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

   111

   112

/**

   113

   Domain destructor.

   114

*/

   115

   116

void ec_domain_clear(struct kobject *kobj /**< kobject of the domain */)

   117

{

   118

    ec_datagram_t *datagram, *next;

   119

    ec_domain_t *domain;

   120

   121

    domain = container_of(kobj, ec_domain_t, kobj);

   122

   123

    EC_INFO("Clearing domain %i.\n", domain->index);

   124

   125

    list_for_each_entry_safe(datagram, next, &domain->datagrams, list) {

   126

        ec_datagram_clear(datagram);

   127

        kfree(datagram);

   128

    }

   129

   130

    ec_domain_clear_field_regs(domain);

   131

   132

    kfree(domain);

   133

}

   134

   135

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

   136

   137

/**

   138

   Registeres a data field in a domain.

   139

   \return 0 in case of success, else < 0

   140

*/

   141

   142

int ec_domain_reg_field(ec_domain_t *domain, /**< EtherCAT domain */

   143

                        ec_slave_t *slave, /**< slave */

   144

                        const ec_sync_t *sync, /**< sync manager */

   145

                        uint32_t field_offset, /**< data field offset */

   146

                        void **data_ptr /**< pointer to the process data

   147

                                           pointer */

   148

                        )

   149

{

   150

    ec_field_reg_t *field_reg;

   151

   152

    if (!(field_reg =

   153

          (ec_field_reg_t *) kmalloc(sizeof(ec_field_reg_t), GFP_KERNEL))) {

   154

        EC_ERR("Failed to allocate field registration.\n");

   155

        return -1;

   156

    }

   157

   158

    if (ec_slave_prepare_fmmu(slave, domain, sync)) {

   159

        EC_ERR("FMMU configuration failed.\n");

   160

        kfree(field_reg);

   161

        return -1;

   162

    }

   163

   164

    field_reg->slave = slave;

   165

    field_reg->sync = sync;

   166

    field_reg->field_offset = field_offset;

   167

    field_reg->data_ptr = data_ptr;

   168

   169

    list_add_tail(&field_reg->list, &domain->field_regs);

   170

    return 0;

   171

}

   172

   173

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

   174

   175

/**

   176

   Clears the list of the registered data fields.

   177

*/

   178

   179

void ec_domain_clear_field_regs(ec_domain_t *domain /**< EtherCAT domain */)

   180

{

   181

    ec_field_reg_t *field_reg, *next;

   182

   183

    list_for_each_entry_safe(field_reg, next, &domain->field_regs, list) {

   184

        list_del(&field_reg->list);

   185

        kfree(field_reg);

   186

    }

   187

}

   188

   189

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

   190

   191

/**

   192

   Allocates a process data datagram and appends it to the list.

   193

   \return 0 in case of success, else < 0

   194

*/

   195

   196

int ec_domain_add_datagram(ec_domain_t *domain, /**< EtherCAT domain */

   197

                           uint32_t offset, /**< logical offset */

   198

                           size_t data_size /**< size of the datagram data */

   199

                           )

   200

{

   201

    ec_datagram_t *datagram;

   202

   203

    if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) {

   204

        EC_ERR("Failed to allocate domain datagram!\n");

   205

        return -1;

   206

    }

   207

   208

    ec_datagram_init(datagram);

   209

   210

    if (ec_datagram_lrw(datagram, offset, data_size)) {

   211

        kfree(datagram);

   212

        return -1;

   213

    }

   214

   215

    list_add_tail(&datagram->list, &domain->datagrams);

   216

    return 0;

   217

}

   218

   219

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

   220

   221

/**

   222

   Creates a domain.

   223

   Reserves domain memory, calculates the logical addresses of the

   224

   corresponding FMMUs and sets the process data pointer of the registered

   225

   data fields.

   226

   \return 0 in case of success, else < 0

   227

*/

   228

   229

int ec_domain_alloc(ec_domain_t *domain, /**< EtherCAT domain */

   230

                    uint32_t base_address /**< logical base address */

   231

                    )

   232

{

   233

    ec_field_reg_t *field_reg;

   234

    ec_slave_t *slave;

   235

    ec_fmmu_t *fmmu;

   236

    unsigned int i, j, cmd_count;

   237

    uint32_t field_off, field_off_cmd;

   238

    uint32_t cmd_offset;

   239

    size_t cmd_data_size, sync_size;

   240

    ec_datagram_t *datagram;

   241

   242

    domain->base_address = base_address;

   243

   244

    // calculate size of process data and allocate memory

   245

    domain->data_size = 0;

   246

    cmd_offset = base_address;

   247

    cmd_data_size = 0;

   248

    cmd_count = 0;

   249

    list_for_each_entry(slave, &domain->master->slaves, list) {

   250

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

   251

            fmmu = &slave->fmmus[j];

   252

            if (fmmu->domain == domain) {

   253

                fmmu->logical_start_address = base_address + domain->data_size;

   254

                sync_size = ec_slave_calc_sync_size(slave, fmmu->sync);

   255

                domain->data_size += sync_size;

   256

                if (cmd_data_size + sync_size > EC_MAX_DATA_SIZE) {

   257

                    if (ec_domain_add_datagram(domain, cmd_offset,

   258

                                               cmd_data_size)) return -1;

   259

                    cmd_offset += cmd_data_size;

   260

                    cmd_data_size = 0;

   261

                    cmd_count++;

   262

                }

   263

                cmd_data_size += sync_size;

   264

            }

   265

        }

   266

    }

   267

   268

    // allocate last datagram

   269

    if (cmd_data_size) {

   270

        if (ec_domain_add_datagram(domain, cmd_offset, cmd_data_size))

   271

            return -1;

   272

        cmd_count++;

   273

    }

   274

   275

    if (!cmd_count) {

   276

        EC_WARN("Domain %i contains no data!\n", domain->index);

   277

        ec_domain_clear_field_regs(domain);

   278

        return 0;

   279

    }

   280

   281

    // set all process data pointers

   282

    list_for_each_entry(field_reg, &domain->field_regs, list) {

   283

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

   284

            fmmu = &field_reg->slave->fmmus[i];

   285

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

   286

                field_off = fmmu->logical_start_address +

   287

                    field_reg->field_offset;

   288

                // search datagram

   289

                list_for_each_entry(datagram, &domain->datagrams, list) {

   290

                    field_off_cmd = field_off - datagram->address.logical;

   291

                    if (field_off >= datagram->address.logical &&

   292

                        field_off_cmd < datagram->mem_size) {

   293

                        *field_reg->data_ptr = datagram->data + field_off_cmd;

   294

                    }

   295

                }

   296

                if (!field_reg->data_ptr) {

   297

                    EC_ERR("Failed to assign data pointer!\n");

   298

                    return -1;

   299

                }

   300

                break;

   301

            }

   302

        }

   303

    }

   304

   305

    EC_INFO("Domain %i - Allocated %i bytes in %i datagram%s\n",

   306

            domain->index, domain->data_size, cmd_count,

   307

            cmd_count == 1 ? "" : "s");

   308

   309

    ec_domain_clear_field_regs(domain);

   310

   311

    return 0;

   312

}

   313

   314

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

   315

   316

/**

   317

   Sets the number of responding slaves and outputs it on demand.

   318

   This number isn't really the number of responding slaves, but the sum of

   319

   the working counters of all domain datagrams. Some slaves increase the

   320

   working counter by 2, some by 1.

   321

*/

   322

   323

void ec_domain_response_count(ec_domain_t *domain, /**< EtherCAT domain */

   324

                              unsigned int count /**< new WC sum */

   325

                              )

   326

{

   327

    if (count != domain->response_count) {

   328

        domain->response_count = count;

   329

        EC_INFO("Domain %i working counter change: %i\n", domain->index,

   330

                count);

   331

    }

   332

}

   333

   334

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

   335

   336

/**

   337

   Formats attribute data for SysFS reading.

   338

   \return number of bytes to read

   339

*/

   340

   341

ssize_t ec_show_domain_attribute(struct kobject *kobj, /**< kobject */

   342

                                 struct attribute *attr, /**< attribute */

   343

                                 char *buffer /**< memory to store data in */

   344

                                 )

   345

{

   346

    ec_domain_t *domain = container_of(kobj, ec_domain_t, kobj);

   347

   348

    if (attr == &attr_data_size) {

   349

        return sprintf(buffer, "%i\n", domain->data_size);

   350

    }

   351

   352

    return 0;

   353

}

   354

   355

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

   356

 *  Realtime interface

   357

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

   358

   359

/**

   360

   Registers a data field in a domain.

   361

   - If \a data_ptr is NULL, the slave is only checked against its type.

   362

   - If \a field_count is 0, it is assumed that one data field is to be

   363

   registered.

   364

   - If \a field_count is greater then 1, it is assumed that \a data_ptr

   365

   is an array of the respective size.

   366

   \return pointer to the slave on success, else NULL

   367

   \ingroup RealtimeInterface

   368

*/

   369

   370

ec_slave_t *ecrt_domain_register_field(ec_domain_t *domain,

   371

                                       /**< EtherCAT domain */

   372

                                       const char *address,

   373

                                       /**< ASCII address of the slave,

   374

                                          see ecrt_master_get_slave() */

   375

                                       const char *vendor_name,

   376

                                       /**< vendor name */

   377

                                       const char *product_name,

   378

                                       /**< product name */

   379

                                       void **data_ptr,

   380

                                       /**< address of the process data

   381

                                          pointer */

   382

                                       const char *field_name,

   383

                                       /**< data field name */

   384

                                       unsigned int field_index,

   385

                                       /**< offset of data fields with

   386

                                          \a field_type  */

   387

                                       unsigned int field_count

   388

                                       /**< number of data fields (with

   389

                                          the same type) to register */

   390

                                       )

   391

{

   392

    ec_slave_t *slave;

   393

    const ec_slave_type_t *type;

   394

    ec_master_t *master;

   395

    const ec_sync_t *sync;

   396

    const ec_field_t *field;

   397

    unsigned int field_counter, i, j, orig_field_index, orig_field_count;

   398

    uint32_t field_offset;

   399

   400

    master = domain->master;

   401

   402

    // translate address

   403

    if (!(slave = ecrt_master_get_slave(master, address))) return NULL;

   404

   405

    if (!(type = slave->type)) {

   406

        EC_ERR("Slave \"%s\" (position %i) has unknown type!\n", address,

   407

               slave->ring_position);

   408

        return NULL;

   409

    }

   410

   411

    if (strcmp(vendor_name, type->vendor_name) ||

   412

        strcmp(product_name, type->product_name)) {

   413

        EC_ERR("Invalid slave type at position %i - Requested: \"%s %s\","

   414

               " found: \"%s %s\".\n", slave->ring_position, vendor_name,

   415

               product_name, type->vendor_name, type->product_name);

   416

        return NULL;

   417

    }

   418

   419

    if (!data_ptr) {

   420

        // data_ptr is NULL => mark slave as "registered" (do not warn)

   421

        slave->registered = 1;

   422

    }

   423

   424

    if (!field_count) field_count = 1;

   425

    orig_field_index = field_index;

   426

    orig_field_count = field_count;

   427

   428

    field_counter = 0;

   429

    for (i = 0; type->sync_managers[i]; i++) {

   430

        sync = type->sync_managers[i];

   431

        field_offset = 0;

   432

        for (j = 0; sync->fields[j]; j++) {

   433

            field = sync->fields[j];

   434

            if (!strcmp(field->name, field_name)) {

   435

                if (field_counter++ == field_index) {

   436

                    if (data_ptr)

   437

                        ec_domain_reg_field(domain, slave, sync, field_offset,

   438

                                            data_ptr++);

   439

                    if (!(--field_count)) return slave;

   440

                    field_index++;

   441

                }

   442

            }

   443

            field_offset += field->size;

   444

        }

   445

    }

   446

   447

    EC_ERR("Slave %i (\"%s %s\") registration mismatch: Field \"%s\","

   448

           " index %i, count %i.\n", slave->ring_position, vendor_name,

   449

           product_name, field_name, orig_field_index, orig_field_count);

   450

    return NULL;

   451

}

   452

   453

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

   454

   455

/**

   456

   Registeres a bunch of data fields.

   457

   Caution! The list has to be terminated with a NULL structure ({})!

   458

   \return 0 in case of success, else < 0

   459

   \ingroup RealtimeInterface

   460

*/

   461

   462

int ecrt_domain_register_field_list(ec_domain_t *domain,

   463

                                    /**< EtherCAT domain */

   464

                                    const ec_field_init_t *fields

   465

                                    /**< array of data field registrations */

   466

                                    )

   467

{

   468

    const ec_field_init_t *field;

   469

   470

    for (field = fields; field->slave_address; field++)

   471

        if (!ecrt_domain_register_field(domain, field->slave_address,

   472

                                        field->vendor_name,

   473

                                        field->product_name, field->data_ptr,

   474

                                        field->field_name, field->field_index,

   475

                                        field->field_count))

   476

            return -1;

   477

   478

    return 0;

   479

}

   480

   481

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

   482

   483

/**

   484

   Places all process data datagrams in the masters datagram queue.

   485

   \ingroup RealtimeInterface

   486

*/