1

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

     2

 *

     3

 *  d o m a i n . c

     4

 *

     5

 *  EtherCAT domain methods.

     6

 *

     7

 *  $Id$

     8

 *

     9

 *  Copyright (C) 2006  Florian Pose, Ingenieurgemeinschaft IgH

    10

 *

    11

 *  This file is part of the IgH EtherCAT Master.

    12

 *

    13

 *  The IgH EtherCAT Master is free software; you can redistribute it

    14

 *  and/or modify it under the terms of the GNU General Public License

    15

 *  as published by the Free Software Foundation; version 2 of the License.

    16

 *

    17

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

    18

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

    19

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    20

 *  GNU General Public License for more details.

    21

 *

    22

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

    23

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

    24

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

    25

 *

    26

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

    27

    28

#include "globals.h"

    29

#include "domain.h"

    30

#include "master.h"

    31

    32

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

    33

    34

void ec_domain_clear_field_regs(ec_domain_t *);

    35

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

    36

    37

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

    38

    39

EC_SYSFS_READ_ATTR(data_size);

    40

    41

static struct attribute *def_attrs[] = {

    42

    &attr_data_size,

    43

    NULL,

    44

};

    45

    46

static struct sysfs_ops sysfs_ops = {

    47

    .show = &ec_show_domain_attribute,

    48

    .store = NULL

    49

};

    50

    51

static struct kobj_type ktype_ec_domain = {

    52

    .release = ec_domain_clear,

    53

    .sysfs_ops = &sysfs_ops,

    54

    .default_attrs = def_attrs

    55

};

    56

    57

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

    58

    59

/**

    60

   Domain constructor.

    61

   \return 0 in case of success, else < 0

    62

   \ingroup Domain

    63

*/

    64

    65

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

    66

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

    67

                   unsigned int index /**< domain index */

    68

                   )

    69

{

    70

    domain->master = master;

    71

    domain->index = index;

    72

    domain->data_size = 0;

    73

    domain->base_address = 0;

    74

    domain->response_count = 0xFFFFFFFF;

    75

    76

    INIT_LIST_HEAD(&domain->field_regs);

    77

    INIT_LIST_HEAD(&domain->commands);

    78

    79

    // init kobject and add it to the hierarchy

    80

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

    81

    kobject_init(&domain->kobj);

    82

    domain->kobj.ktype = &ktype_ec_domain;

    83

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

    84

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

    85

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

    86

        return -1;

    87

    }

    88

    89

    return 0;

    90

}

    91

    92

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

    93

    94

/**

    95

   Domain destructor.

    96

   \ingroup Domain

    97

*/

    98

    99

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

   100

{

   101

    ec_command_t *command, *next;

   102

    ec_domain_t *domain;

   103

   104

    domain = container_of(kobj, ec_domain_t, kobj);

   105

   106

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

   107

   108

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

   109

        ec_command_clear(command);

   110

        kfree(command);

   111

    }

   112

   113

    ec_domain_clear_field_regs(domain);

   114

   115

    kfree(domain);

   116

}

   117

   118

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

   119

   120

/**

   121

   Registeres a data field in a domain.

   122

   \return 0 in case of success, else < 0

   123

   \ingroup Domain

   124

*/

   125

   126

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

   127

                        ec_slave_t *slave, /**< slave */

   128

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

   129

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

   130

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

   131

                                           pointer */

   132

                        )

   133

{

   134

    ec_field_reg_t *field_reg;

   135

   136

    if (!(field_reg =

   137

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

   138

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

   139

        return -1;

   140

    }

   141

   142

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

   143

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

   144

        kfree(field_reg);

   145

        return -1;

   146

    }

   147

   148

    field_reg->slave = slave;

   149

    field_reg->sync = sync;

   150

    field_reg->field_offset = field_offset;

   151

    field_reg->data_ptr = data_ptr;

   152

   153

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

   154

    return 0;

   155

}

   156

   157

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

   158

   159

/**

   160

   Clears the list of the registered data fields.

   161

   \ingroup Domain

   162

*/

   163

   164

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

   165

{

   166

    ec_field_reg_t *field_reg, *next;

   167

   168

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

   169

        list_del(&field_reg->list);

   170

        kfree(field_reg);

   171

    }

   172

}

   173

   174

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

   175

   176

/**

   177

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

   178

   \return 0 in case of success, else < 0

   179

   \ingroup Domain

   180

*/

   181

   182

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

   183

                          uint32_t offset, /**< logical offset */

   184

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

   185

                          )

   186

{

   187

    ec_command_t *command;

   188

   189

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

   190

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

   191

        return -1;

   192

    }

   193

   194

    ec_command_init(command);

   195

   196

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

   197

        kfree(command);

   198

        return -1;

   199

    }

   200

   201

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

   202

    return 0;

   203

}

   204

   205

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

   206

   207

/**

   208

   Creates a domain.

   209

   Reserves domain memory, calculates the logical addresses of the

   210

   corresponding FMMUs and sets the process data pointer of the registered

   211

   data fields.

   212

   \return 0 in case of success, else < 0

   213

   \ingroup Domain

   214

*/

   215

   216

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

   217

                    uint32_t base_address /**< Logische Basisadresse */

   218

                    )

   219

{

   220

    ec_field_reg_t *field_reg;

   221

    ec_slave_t *slave;

   222

    ec_fmmu_t *fmmu;

   223

    unsigned int i, j, cmd_count;

   224

    uint32_t field_off, field_off_cmd;

   225

    uint32_t cmd_offset;

   226

    size_t cmd_data_size;

   227

    ec_command_t *command;

   228

   229

    domain->base_address = base_address;

   230

   231

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

   232

    domain->data_size = 0;

   233

    cmd_offset = base_address;

   234

    cmd_data_size = 0;

   235

    cmd_count = 0;

   236

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

   237

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

   238

            fmmu = &slave->fmmus[j];

   239

            if (fmmu->domain == domain) {

   240

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

   241

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

   242

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

   243

                    if (ec_domain_add_command(domain, cmd_offset,

   244

                                              cmd_data_size)) return -1;

   245

                    cmd_offset += cmd_data_size;

   246

                    cmd_data_size = 0;

   247

                    cmd_count++;

   248

                }

   249

                cmd_data_size += fmmu->sync->size;

   250

            }

   251

        }

   252

    }

   253

   254

    // Letztes Kommando allozieren

   255

    if (cmd_data_size) {

   256

        if (ec_domain_add_command(domain, cmd_offset, cmd_data_size))

   257

            return -1;

   258

        cmd_count++;

   259

    }

   260

   261

    if (!cmd_count) {

   262

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

   263

        ec_domain_clear_field_regs(domain);

   264

        return 0;

   265

    }

   266

   267

    // Alle Prozessdatenzeiger setzen

   268

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

   269

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

   270

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

   271

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

   272

                field_off = fmmu->logical_start_address +

   273

                    field_reg->field_offset;

   274

                // Kommando suchen

   275

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

   276

                    field_off_cmd = field_off - command->address.logical;

   277

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

   278

                        field_off_cmd < command->mem_size) {

   279

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

   280

                    }

   281

                }

   282

                if (!field_reg->data_ptr) {

   283

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

   284

                    return -1;

   285

                }

   286

                break;

   287

            }

   288

        }

   289

    }

   290

   291

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

   292

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

   293

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

   294

   295

    ec_domain_clear_field_regs(domain);

   296

   297

    return 0;

   298

}

   299

   300

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

   301

   302

/**

   303

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

   304

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

   305

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

   306

   working counter by 2, some by 1.

   307

   \ingroup Domain

   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

   \ingroup Domain

   327

*/

   328

   329

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

   330

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

   331

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

   332

                                 )

   333

{

   334

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

   335

   336

    if (attr == &attr_data_size) {

   337

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

   338

    }

   339

   340

    return 0;

   341

}

   342

   343

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

   344

 *  Realtime interface

   345

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

   346

   347

/**

   348

   Registers a data field in a domain.

   349

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

   350

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

   351

   registered.

   352

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

   353

   is an array of the respective size.

   354

   \return pointer to the slave on success, else NULL

   355

   \ingroup Domain

   356

*/

   357

   358

ec_slave_t *ecrt_domain_register_field(ec_domain_t *domain,

   359

                                       /**< EtherCAT domain */

   360

                                       const char *address,

   361

                                       /**< ASCII address of the slave,

   362

                                          see ecrt_master_get_slave() */

   363

                                       const char *vendor_name,

   364

                                       /**< vendor name */

   365

                                       const char *product_name,

   366

                                       /**< product name */

   367

                                       void **data_ptr,

   368

                                       /**< address of the process data

   369

                                          pointer */

   370

                                       const char *field_name,

   371

                                       /**< data field name */

   372

                                       unsigned int field_index,

   373

                                       /**< offset of data fields with

   374

                                          \a field_type  */

   375

                                       unsigned int field_count

   376

                                       /**< number of data fields (with

   377

                                          the same type) to register */

   378

                                       )

   379

{

   380

    ec_slave_t *slave;

   381

    const ec_slave_type_t *type;

   382

    ec_master_t *master;

   383

    const ec_sync_t *sync;

   384

    const ec_field_t *field;

   385

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

   386

    uint32_t field_offset;

   387

   388

    master = domain->master;

   389

   390

    // translate address

   391

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

   392

   393

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

   394

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

   395

               slave->ring_position);

   396

        return NULL;

   397

    }

   398

   399

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

   400

        strcmp(product_name, type->product_name)) {

   401

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

   402

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

   403

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

   404

        return NULL;

   405

    }

   406

   407

    if (!data_ptr) {

   408

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

   409

        slave->registered = 1;

   410

    }

   411

   412

    if (!field_count) field_count = 1;

   413

    orig_field_index = field_index;

   414

    orig_field_count = field_count;

   415

   416

    field_counter = 0;

   417

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

   418

        sync = type->sync_managers[i];

   419

        field_offset = 0;

   420

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

   421

            field = sync->fields[j];

   422

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

   423

                if (field_counter++ == field_index) {

   424

                    if (data_ptr)

   425

                        ec_domain_reg_field(domain, slave, sync, field_offset,

   426

                                            data_ptr++);

   427

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

   428

                    field_index++;

   429

                }

   430

            }

   431

            field_offset += field->size;

   432

        }

   433

    }

   434

   435

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

   436

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

   437

           product_name, field_name, orig_field_index, orig_field_count);

   438

    return NULL;

   439

}

   440

   441

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

   442

   443

/**

   444

   Registeres a bunch of data fields.

   445

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

   446

   \return 0 in case of success, else < 0

   447

   \ingroup Domain

   448

*/

   449

   450

int ecrt_domain_register_field_list(ec_domain_t *domain,

   451

                                    /**< EtherCAT domain */

   452

                                    const ec_field_init_t *fields

   453

                                    /**< array of data field registrations */

   454

                                    )

   455

{

   456

    const ec_field_init_t *field;

   457

   458

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

   459

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

   460

                                        field->vendor_name,

   461

                                        field->product_name, field->data_ptr,

   462

                                        field->field_name, field->field_index,

   463

                                        field->field_count))

   464

            return -1;

   465

   466

    return 0;

   467

}

   468

   469

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

   470

   471

/**

   472

   Places all process data commands in the masters command queue.

   473

   \ingroup Domain

   474

*/

   475

   476

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

   477

{

   478

    ec_command_t *command;

   479

   480

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

   481

        ec_master_queue_command(domain->master, command);

   482

    }

   483

}

   484

   485

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

   486

   487

/**