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 "globals.h"

    42

#include "domain.h"

    43

#include "master.h"

    44

    45

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

    46

    47

void ec_domain_clear_field_regs(ec_domain_t *);

    48

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

    49

    50

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

    51

    52

/** \cond */

    53

    54

EC_SYSFS_READ_ATTR(data_size);

    55

    56

static struct attribute *def_attrs[] = {

    57

    &attr_data_size,

    58

    NULL,

    59

};

    60

    61

static struct sysfs_ops sysfs_ops = {

    62

    .show = &ec_show_domain_attribute,

    63

    .store = NULL

    64

};

    65

    66

static struct kobj_type ktype_ec_domain = {

    67

    .release = ec_domain_clear,

    68

    .sysfs_ops = &sysfs_ops,

    69

    .default_attrs = def_attrs

    70

};

    71

    72

/** \endcond */

    73

    74

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

    75

    76

/**

    77

   Domain constructor.

    78

   \return 0 in case of success, else < 0

    79

*/

    80

    81

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

    82

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

    83

                   unsigned int index /**< domain index */

    84

                   )

    85

{

    86

    domain->master = master;

    87

    domain->index = index;

    88

    domain->data_size = 0;

    89

    domain->base_address = 0;

    90

    domain->response_count = 0xFFFFFFFF;

    91

    92

    INIT_LIST_HEAD(&domain->field_regs);

    93

    INIT_LIST_HEAD(&domain->commands);

    94

    95

    // init kobject and add it to the hierarchy

    96

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

    97

    kobject_init(&domain->kobj);

    98

    domain->kobj.ktype = &ktype_ec_domain;

    99

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

   100

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

   101

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

   102

        return -1;

   103

    }

   104

   105

    return 0;

   106

}

   107

   108

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

   109

   110

/**

   111

   Domain destructor.

   112

*/

   113

   114

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

   115

{

   116

    ec_command_t *command, *next;

   117

    ec_domain_t *domain;

   118

   119

    domain = container_of(kobj, ec_domain_t, kobj);

   120

   121

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

   122

   123

    list_for_each_entry_safe(command, next, &domain->commands, list) {

   124

        ec_command_clear(command);

   125

        kfree(command);

   126

    }

   127

   128

    ec_domain_clear_field_regs(domain);

   129

   130

    kfree(domain);

   131

}

   132

   133

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

   134

   135

/**

   136

   Registeres a data field in a domain.

   137

   \return 0 in case of success, else < 0

   138

*/

   139

   140

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

   141

                        ec_slave_t *slave, /**< slave */

   142

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

   143

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

   144

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

   145

                                           pointer */

   146

                        )

   147

{

   148

    ec_field_reg_t *field_reg;

   149

   150

    if (!(field_reg =

   151

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

   152

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

   153

        return -1;

   154

    }

   155

   156

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

   157

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

   158

        kfree(field_reg);

   159

        return -1;

   160

    }

   161

   162

    field_reg->slave = slave;

   163

    field_reg->sync = sync;

   164

    field_reg->field_offset = field_offset;

   165

    field_reg->data_ptr = data_ptr;

   166

   167

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

   168

    return 0;

   169

}

   170

   171

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

   172

   173

/**

   174

   Clears the list of the registered data fields.

   175

*/

   176

   177

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

   178

{

   179

    ec_field_reg_t *field_reg, *next;

   180

   181

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

   182

        list_del(&field_reg->list);

   183

        kfree(field_reg);

   184

    }

   185

}

   186

   187

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

   188

   189

/**

   190

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

   191

   \return 0 in case of success, else < 0

   192

*/

   193

   194

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

   195

                          uint32_t offset, /**< logical offset */

   196

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

   197

                          )

   198

{

   199

    ec_command_t *command;

   200

   201

    if (!(command = kmalloc(sizeof(ec_command_t), GFP_KERNEL))) {

   202

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

   203

        return -1;

   204

    }

   205

   206

    ec_command_init(command);

   207

   208

    if (ec_command_lrw(command, offset, data_size)) {

   209

        kfree(command);

   210

        return -1;

   211

    }

   212

   213

    list_add_tail(&command->list, &domain->commands);

   214

    return 0;

   215

}

   216

   217

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

   218

   219

/**

   220

   Creates a domain.

   221

   Reserves domain memory, calculates the logical addresses of the

   222

   corresponding FMMUs and sets the process data pointer of the registered

   223

   data fields.

   224

   \return 0 in case of success, else < 0

   225

*/

   226

   227

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

   228

                    uint32_t base_address /**< logical base address */

   229

                    )

   230

{

   231

    ec_field_reg_t *field_reg;

   232

    ec_slave_t *slave;

   233

    ec_fmmu_t *fmmu;

   234

    unsigned int i, j, cmd_count;

   235

    uint32_t field_off, field_off_cmd;

   236

    uint32_t cmd_offset;

   237

    size_t cmd_data_size, sync_size;

   238

    ec_command_t *command;

   239

   240

    domain->base_address = base_address;

   241

   242

    // calculate size of process data and allocate memory

   243

    domain->data_size = 0;

   244

    cmd_offset = base_address;

   245

    cmd_data_size = 0;

   246

    cmd_count = 0;

   247

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

   248

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

   249

            fmmu = &slave->fmmus[j];

   250

            if (fmmu->domain == domain) {

   251

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

   252

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

   253

                domain->data_size += sync_size;

   254

                if (cmd_data_size + sync_size > EC_MAX_DATA_SIZE) {

   255

                    if (ec_domain_add_command(domain, cmd_offset,

   256

                                              cmd_data_size)) return -1;

   257

                    cmd_offset += cmd_data_size;

   258

                    cmd_data_size = 0;

   259

                    cmd_count++;

   260

                }

   261

                cmd_data_size += sync_size;

   262

            }

   263

        }

   264

    }

   265

   266

    // allocate last command

   267

    if (cmd_data_size) {

   268

        if (ec_domain_add_command(domain, cmd_offset, cmd_data_size))

   269

            return -1;

   270

        cmd_count++;

   271

    }

   272

   273

    if (!cmd_count) {

   274

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

   275

        ec_domain_clear_field_regs(domain);

   276

        return 0;

   277

    }

   278

   279

    // set all process data pointers

   280

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

   281

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

   282

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

   283

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

   284

                field_off = fmmu->logical_start_address +

   285

                    field_reg->field_offset;

   286

                // search command

   287

                list_for_each_entry(command, &domain->commands, list) {

   288

                    field_off_cmd = field_off - command->address.logical;

   289

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

   290

                        field_off_cmd < command->mem_size) {

   291

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

   292

                    }

   293

                }

   294

                if (!field_reg->data_ptr) {

   295

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

   296

                    return -1;

   297

                }

   298

                break;

   299

            }

   300

        }

   301

    }

   302

   303

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

   304

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

   305

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

   306

   307

    ec_domain_clear_field_regs(domain);

   308

   309

    return 0;

   310

}

   311

   312

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

   313

   314

/**

   315

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

   316

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

   317

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

   318

   working counter by 2, some by 1.

   319

*/

   320

   321

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

   322

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

   323

                              )

   324

{

   325

    if (count != domain->response_count) {

   326

        domain->response_count = count;

   327

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

   328

                count);

   329

    }

   330

}

   331

   332

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

   333

   334

/**

   335

   Formats attribute data for SysFS reading.

   336

   \return number of bytes to read

   337

*/

   338

   339

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

   340

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

   341

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

   342

                                 )

   343

{

   344

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

   345

   346

    if (attr == &attr_data_size) {

   347

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

   348

    }

   349

   350

    return 0;

   351

}

   352

   353

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

   354

 *  Realtime interface

   355

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

   356

   357

/**

   358

   Registers a data field in a domain.

   359

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

   360

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

   361

   registered.

   362

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

   363

   is an array of the respective size.

   364

   \return pointer to the slave on success, else NULL

   365

   \ingroup RealtimeInterface

   366

*/

   367

   368

ec_slave_t *ecrt_domain_register_field(ec_domain_t *domain,

   369

                                       /**< EtherCAT domain */

   370

                                       const char *address,

   371

                                       /**< ASCII address of the slave,

   372

                                          see ecrt_master_get_slave() */

   373

                                       const char *vendor_name,

   374

                                       /**< vendor name */

   375

                                       const char *product_name,

   376

                                       /**< product name */

   377

                                       void **data_ptr,

   378

                                       /**< address of the process data

   379

                                          pointer */

   380

                                       const char *field_name,

   381

                                       /**< data field name */

   382

                                       unsigned int field_index,

   383

                                       /**< offset of data fields with

   384

                                          \a field_type  */

   385

                                       unsigned int field_count

   386

                                       /**< number of data fields (with

   387

                                          the same type) to register */

   388

                                       )

   389

{

   390

    ec_slave_t *slave;

   391

    const ec_slave_type_t *type;

   392

    ec_master_t *master;

   393

    const ec_sync_t *sync;

   394

    const ec_field_t *field;

   395

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

   396

    uint32_t field_offset;

   397

   398

    master = domain->master;

   399

   400

    // translate address

   401

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

   402

   403

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

   404

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

   405

               slave->ring_position);

   406

        return NULL;

   407

    }

   408

   409

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

   410

        strcmp(product_name, type->product_name)) {

   411

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

   412

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

   413

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

   414

        return NULL;

   415

    }

   416

   417

    if (!data_ptr) {

   418

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

   419

        slave->registered = 1;

   420

    }

   421

   422

    if (!field_count) field_count = 1;

   423

    orig_field_index = field_index;

   424

    orig_field_count = field_count;

   425

   426

    field_counter = 0;

   427

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

   428

        sync = type->sync_managers[i];

   429

        field_offset = 0;

   430

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

   431

            field = sync->fields[j];

   432

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

   433

                if (field_counter++ == field_index) {

   434

                    if (data_ptr)

   435

                        ec_domain_reg_field(domain, slave, sync, field_offset,

   436

                                            data_ptr++);

   437

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

   438

                    field_index++;

   439

                }

   440

            }

   441

            field_offset += field->size;

   442

        }

   443

    }

   444

   445

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

   446

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

   447

           product_name, field_name, orig_field_index, orig_field_count);

   448

    return NULL;

   449

}

   450

   451

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

   452

   453

/**

   454

   Registeres a bunch of data fields.

   455

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

   456

   \return 0 in case of success, else < 0

   457

   \ingroup RealtimeInterface

   458

*/

   459

   460

int ecrt_domain_register_field_list(ec_domain_t *domain,

   461

                                    /**< EtherCAT domain */

   462

                                    const ec_field_init_t *fields

   463

                                    /**< array of data field registrations */

   464

                                    )

   465

{

   466

    const ec_field_init_t *field;

   467

   468

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

   469

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

   470

                                        field->vendor_name,

   471

                                        field->product_name, field->data_ptr,

   472

                                        field->field_name, field->field_index,

   473

                                        field->field_count))

   474

            return -1;

   475

   476

    return 0;

   477

}

   478

   479

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

   480

   481

/**

   482

   Places all process data commands in the masters command queue.

   483

   \ingroup RealtimeInterface

   484

*/

   485

   486

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

   487

{