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

    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

 *

    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 /**< Logische Basisadresse */

   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;

   238

    ec_command_t *command;

   239

   240

    domain->base_address = base_address;

   241

   242

    // Größe der Prozessdaten berechnen und Kommandos allozieren

   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

                domain->data_size += fmmu->sync->size;

   253

                if (cmd_data_size + fmmu->sync->size > EC_MAX_DATA_SIZE) {

   254

                    if (ec_domain_add_command(domain, cmd_offset,

   255

                                              cmd_data_size)) return -1;

   256

                    cmd_offset += cmd_data_size;

   257

                    cmd_data_size = 0;

   258

                    cmd_count++;

   259

                }

   260

                cmd_data_size += fmmu->sync->size;

   261

            }

   262

        }

   263

    }

   264

   265

    // Letztes Kommando allozieren

   266

    if (cmd_data_size) {

   267

        if (ec_domain_add_command(domain, cmd_offset, cmd_data_size))

   268

            return -1;

   269

        cmd_count++;

   270

    }

   271

   272

    if (!cmd_count) {

   273

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

   274

        ec_domain_clear_field_regs(domain);

   275

        return 0;

   276

    }

   277

   278

    // Alle Prozessdatenzeiger setzen

   279

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

   280

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

   281

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

   282

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

   283

                field_off = fmmu->logical_start_address +

   284

                    field_reg->field_offset;

   285

                // Kommando suchen

   286

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

   287

                    field_off_cmd = field_off - command->address.logical;

   288

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

   289

                        field_off_cmd < command->mem_size) {

   290

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

   291

                    }

   292

                }

   293

                if (!field_reg->data_ptr) {

   294

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

   295

                    return -1;

   296

                }

   297

                break;

   298

            }

   299

        }

   300

    }

   301

   302

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

   303

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

   304

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

   305

   306

    ec_domain_clear_field_regs(domain);

   307

   308

    return 0;

   309

}

   310

   311

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

   312

   313

/**

   314

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

   315

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

   316

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

   317

   working counter by 2, some by 1.

   318

*/

   319

   320

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

   321

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

   322

                              )

   323

{

   324

    if (count != domain->response_count) {

   325

        domain->response_count = count;

   326

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

   327

                count);

   328

    }

   329

}

   330

   331

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

   332

   333

/**

   334

   Formats attribute data for SysFS reading.

   335

   \return number of bytes to read

   336

*/

   337

   338

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

   339

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

   340

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

   341

                                 )

   342

{

   343

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

   344

   345

    if (attr == &attr_data_size) {

   346

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

   347

    }

   348

   349

    return 0;

   350

}

   351

   352

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

   353

 *  Realtime interface

   354

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

   355

   356

/**

   357

   Registers a data field in a domain.

   358

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

   359

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

   360

   registered.

   361

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

   362

   is an array of the respective size.

   363

   \return pointer to the slave on success, else NULL

   364

   \ingroup RealtimeInterface

   365

*/

   366

   367

ec_slave_t *ecrt_domain_register_field(ec_domain_t *domain,

   368

                                       /**< EtherCAT domain */

   369

                                       const char *address,

   370

                                       /**< ASCII address of the slave,

   371

                                          see ecrt_master_get_slave() */

   372

                                       const char *vendor_name,

   373

                                       /**< vendor name */

   374

                                       const char *product_name,

   375

                                       /**< product name */

   376

                                       void **data_ptr,

   377

                                       /**< address of the process data

   378

                                          pointer */

   379

                                       const char *field_name,

   380

                                       /**< data field name */

   381

                                       unsigned int field_index,

   382

                                       /**< offset of data fields with

   383

                                          \a field_type  */

   384

                                       unsigned int field_count

   385

                                       /**< number of data fields (with

   386

                                          the same type) to register */

   387

                                       )

   388

{

   389

    ec_slave_t *slave;

   390

    const ec_slave_type_t *type;

   391

    ec_master_t *master;

   392

    const ec_sync_t *sync;

   393

    const ec_field_t *field;

   394

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

   395

    uint32_t field_offset;

   396

   397

    master = domain->master;

   398

   399

    // translate address

   400

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

   401

   402

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

   403

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

   404

               slave->ring_position);

   405

        return NULL;

   406

    }

   407

   408

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

   409

        strcmp(product_name, type->product_name)) {

   410

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

   411

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

   412

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

   413

        return NULL;

   414

    }

   415

   416

    if (!data_ptr) {

   417

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

   418

        slave->registered = 1;

   419

    }

   420

   421

    if (!field_count) field_count = 1;

   422

    orig_field_index = field_index;

   423

    orig_field_count = field_count;

   424

   425

    field_counter = 0;

   426

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

   427

        sync = type->sync_managers[i];

   428

        field_offset = 0;

   429

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

   430

            field = sync->fields[j];

   431

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

   432

                if (field_counter++ == field_index) {

   433

                    if (data_ptr)

   434

                        ec_domain_reg_field(domain, slave, sync, field_offset,

   435

                                            data_ptr++);

   436

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

   437

                    field_index++;

   438

                }

   439

            }

   440

            field_offset += field->size;

   441

        }

   442

    }

   443

   444

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

   445

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

   446

           product_name, field_name, orig_field_index, orig_field_count);

   447

    return NULL;

   448

}

   449

   450

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

   451

   452

/**

   453

   Registeres a bunch of data fields.

   454

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

   455

   \return 0 in case of success, else < 0

   456

   \ingroup RealtimeInterface

   457

*/

   458

   459

int ecrt_domain_register_field_list(ec_domain_t *domain,

   460

                                    /**< EtherCAT domain */

   461

                                    const ec_field_init_t *fields

   462

                                    /**< array of data field registrations */

   463

                                    )

   464

{

   465

    const ec_field_init_t *field;

   466

   467

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

   468

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

   469

                                        field->vendor_name,

   470

                                        field->product_name, field->data_ptr,

   471

                                        field->field_name, field->field_index,

   472

                                        field->field_count))

   473

            return -1;

   474

   475

    return 0;

   476

}

   477

   478

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

   479

   480

/**

   481

   Places all process data commands in the masters command queue.

   482

   \ingroup RealtimeInterface

   483

*/

   484

   485

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

   486

{

   487

    ec_command_t *command;

   488