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; version 2 of the License.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    14

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    16

 *  GNU General Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License

    19

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

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

    23

    24

/**

    25

   \file

    26

   EtherCAT domain methods.

    27

*/

    28

    29

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

    30

    31

#include "globals.h"

    32

#include "domain.h"

    33

#include "master.h"

    34

    35

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

    36

    37

void ec_domain_clear_field_regs(ec_domain_t *);

    38

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

    39

    40

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

    41

    42

/** \cond */

    43

    44

EC_SYSFS_READ_ATTR(data_size);

    45

    46

static struct attribute *def_attrs[] = {

    47

    &attr_data_size,

    48

    NULL,

    49

};

    50

    51

static struct sysfs_ops sysfs_ops = {

    52

    .show = &ec_show_domain_attribute,

    53

    .store = NULL

    54

};

    55

    56

static struct kobj_type ktype_ec_domain = {

    57

    .release = ec_domain_clear,

    58

    .sysfs_ops = &sysfs_ops,

    59

    .default_attrs = def_attrs

    60

};

    61

    62

/** \endcond */

    63

    64

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

    65

    66

/**

    67

   Domain constructor.

    68

   \return 0 in case of success, else < 0

    69

*/

    70

    71

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

    72

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

    73

                   unsigned int index /**< domain index */

    74

                   )

    75

{

    76

    domain->master = master;

    77

    domain->index = index;

    78

    domain->data_size = 0;

    79

    domain->base_address = 0;

    80

    domain->response_count = 0xFFFFFFFF;

    81

    82

    INIT_LIST_HEAD(&domain->field_regs);

    83

    INIT_LIST_HEAD(&domain->commands);

    84

    85

    // init kobject and add it to the hierarchy

    86

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

    87

    kobject_init(&domain->kobj);

    88

    domain->kobj.ktype = &ktype_ec_domain;

    89

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

    90

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

    91

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

    92

        return -1;

    93

    }

    94

    95

    return 0;

    96

}

    97

    98

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

    99

   100

/**

   101

   Domain destructor.

   102

*/

   103

   104

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

   105

{

   106

    ec_command_t *command, *next;

   107

    ec_domain_t *domain;

   108

   109

    domain = container_of(kobj, ec_domain_t, kobj);

   110

   111

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

   112

   113

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

   114

        ec_command_clear(command);

   115

        kfree(command);

   116

    }

   117

   118

    ec_domain_clear_field_regs(domain);

   119

   120

    kfree(domain);

   121

}

   122

   123

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

   124

   125

/**

   126

   Registeres a data field in a domain.

   127

   \return 0 in case of success, else < 0

   128

*/

   129

   130

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

   131

                        ec_slave_t *slave, /**< slave */

   132

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

   133

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

   134

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

   135

                                           pointer */

   136

                        )

   137

{

   138

    ec_field_reg_t *field_reg;

   139

   140

    if (!(field_reg =

   141

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

   142

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

   143

        return -1;

   144

    }

   145

   146

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

   147

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

   148

        kfree(field_reg);

   149

        return -1;

   150

    }

   151

   152

    field_reg->slave = slave;

   153

    field_reg->sync = sync;

   154

    field_reg->field_offset = field_offset;

   155

    field_reg->data_ptr = data_ptr;

   156

   157

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

   158

    return 0;

   159

}

   160

   161

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

   162

   163

/**

   164

   Clears the list of the registered data fields.

   165

*/

   166

   167

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

   168

{

   169

    ec_field_reg_t *field_reg, *next;

   170

   171

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

   172

        list_del(&field_reg->list);

   173

        kfree(field_reg);

   174

    }

   175

}

   176

   177

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

   178

   179

/**

   180

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

   181

   \return 0 in case of success, else < 0

   182

*/

   183

   184

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

   185

                          uint32_t offset, /**< logical offset */

   186

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

   187

                          )

   188

{

   189

    ec_command_t *command;

   190

   191

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

   192

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

   193

        return -1;

   194

    }

   195

   196

    ec_command_init(command);

   197

   198

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

   199

        kfree(command);

   200

        return -1;

   201

    }

   202

   203

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

   204

    return 0;

   205

}

   206

   207

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

   208

   209

/**

   210

   Creates a domain.

   211

   Reserves domain memory, calculates the logical addresses of the

   212

   corresponding FMMUs and sets the process data pointer of the registered

   213

   data fields.

   214

   \return 0 in case of success, else < 0

   215

*/

   216

   217

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

   218

                    uint32_t base_address /**< Logische Basisadresse */

   219

                    )

   220

{

   221

    ec_field_reg_t *field_reg;

   222

    ec_slave_t *slave;

   223

    ec_fmmu_t *fmmu;

   224

    unsigned int i, j, cmd_count;

   225

    uint32_t field_off, field_off_cmd;

   226

    uint32_t cmd_offset;

   227

    size_t cmd_data_size;

   228

    ec_command_t *command;

   229

   230

    domain->base_address = base_address;

   231

   232

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

   233

    domain->data_size = 0;

   234

    cmd_offset = base_address;

   235

    cmd_data_size = 0;

   236

    cmd_count = 0;

   237

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

   238

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

   239

            fmmu = &slave->fmmus[j];

   240

            if (fmmu->domain == domain) {

   241

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

   242

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

   243

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

   244

                    if (ec_domain_add_command(domain, cmd_offset,

   245

                                              cmd_data_size)) return -1;

   246

                    cmd_offset += cmd_data_size;

   247

                    cmd_data_size = 0;

   248

                    cmd_count++;

   249

                }

   250

                cmd_data_size += fmmu->sync->size;

   251

            }

   252

        }

   253

    }

   254

   255

    // Letztes Kommando allozieren

   256

    if (cmd_data_size) {

   257

        if (ec_domain_add_command(domain, cmd_offset, cmd_data_size))

   258

            return -1;

   259

        cmd_count++;

   260

    }

   261

   262

    if (!cmd_count) {

   263

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

   264

        ec_domain_clear_field_regs(domain);

   265

        return 0;

   266

    }

   267

   268

    // Alle Prozessdatenzeiger setzen

   269

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

   270

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

   271

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

   272

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

   273

                field_off = fmmu->logical_start_address +

   274

                    field_reg->field_offset;

   275

                // Kommando suchen

   276

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

   277

                    field_off_cmd = field_off - command->address.logical;

   278

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

   279

                        field_off_cmd < command->mem_size) {

   280

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

   281

                    }

   282

                }

   283

                if (!field_reg->data_ptr) {

   284

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

   285

                    return -1;

   286

                }

   287

                break;

   288

            }

   289

        }

   290

    }

   291

   292

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

   293

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

   294

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

   295

   296

    ec_domain_clear_field_regs(domain);

   297

   298

    return 0;

   299

}

   300

   301

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

   302

   303

/**

   304

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

   305

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

   306

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

   307

   working counter by 2, some by 1.

   308

*/

   309

   310

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

   311

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

   312

                              )

   313

{

   314

    if (count != domain->response_count) {

   315

        domain->response_count = count;

   316

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

   317

                count);

   318

    }

   319

}

   320

   321

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

   322

   323

/**

   324

   Formats attribute data for SysFS reading.

   325

   \return number of bytes to read

   326

*/

   327

   328

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

   329

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

   330

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

   331

                                 )

   332

{

   333

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

   334

   335

    if (attr == &attr_data_size) {

   336

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

   337

    }

   338

   339

    return 0;

   340

}

   341

   342

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

   343

 *  Realtime interface

   344

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

   345

   346

/**

   347

   Registers a data field in a domain.

   348

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

   349

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

   350

   registered.

   351

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

   352

   is an array of the respective size.

   353

   \return pointer to the slave on success, else NULL

   354

   \ingroup RealtimeInterface

   355

*/

   356

   357

ec_slave_t *ecrt_domain_register_field(ec_domain_t *domain,

   358

                                       /**< EtherCAT domain */

   359

                                       const char *address,

   360

                                       /**< ASCII address of the slave,

   361

                                          see ecrt_master_get_slave() */

   362

                                       const char *vendor_name,

   363

                                       /**< vendor name */

   364

                                       const char *product_name,

   365

                                       /**< product name */

   366

                                       void **data_ptr,

   367

                                       /**< address of the process data

   368

                                          pointer */

   369

                                       const char *field_name,

   370

                                       /**< data field name */

   371

                                       unsigned int field_index,

   372

                                       /**< offset of data fields with

   373

                                          \a field_type  */

   374

                                       unsigned int field_count

   375

                                       /**< number of data fields (with

   376

                                          the same type) to register */

   377

                                       )

   378

{

   379

    ec_slave_t *slave;

   380

    const ec_slave_type_t *type;

   381

    ec_master_t *master;

   382

    const ec_sync_t *sync;

   383

    const ec_field_t *field;

   384

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

   385

    uint32_t field_offset;

   386

   387

    master = domain->master;

   388

   389

    // translate address

   390

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

   391

   392

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

   393

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

   394

               slave->ring_position);

   395

        return NULL;

   396

    }

   397

   398

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

   399

        strcmp(product_name, type->product_name)) {

   400

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

   401

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

   402

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

   403

        return NULL;

   404

    }

   405

   406

    if (!data_ptr) {

   407

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

   408

        slave->registered = 1;

   409

    }

   410

   411

    if (!field_count) field_count = 1;

   412

    orig_field_index = field_index;

   413

    orig_field_count = field_count;

   414

   415

    field_counter = 0;

   416

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

   417

        sync = type->sync_managers[i];

   418

        field_offset = 0;

   419

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

   420

            field = sync->fields[j];

   421

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

   422

                if (field_counter++ == field_index) {

   423

                    if (data_ptr)

   424

                        ec_domain_reg_field(domain, slave, sync, field_offset,

   425

                                            data_ptr++);

   426

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

   427

                    field_index++;

   428

                }

   429

            }

   430

            field_offset += field->size;

   431

        }

   432

    }

   433

   434

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

   435

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

   436

           product_name, field_name, orig_field_index, orig_field_count);

   437

    return NULL;

   438

}

   439

   440

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

   441

   442

/**

   443

   Registeres a bunch of data fields.

   444

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

   445

   \return 0 in case of success, else < 0

   446

   \ingroup RealtimeInterface

   447

*/

   448

   449

int ecrt_domain_register_field_list(ec_domain_t *domain,

   450

                                    /**< EtherCAT domain */

   451

                                    const ec_field_init_t *fields

   452

                                    /**< array of data field registrations */

   453

                                    )

   454

{

   455

    const ec_field_init_t *field;

   456

   457

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

   458

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

   459

                                        field->vendor_name,

   460

                                        field->product_name, field->data_ptr,

   461

                                        field->field_name, field->field_index,

   462

                                        field->field_count))

   463

            return -1;

   464

   465

    return 0;

   466

}

   467

   468

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

   469

   470

/**

   471

   Places all process data commands in the masters command queue.

   472

   \ingroup RealtimeInterface

   473

*/

   474

   475

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

   476

{

   477

    ec_command_t *command;

   478

   479

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

   480

        ec_master_queue_command(domain->master, command);

   481

    }

   482

}

   483

   484

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

   485

   486

/**

   487

   Processes received process data.

   488

   \ingroup RealtimeInterface