1

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

     2

 *

     3

 *  d o m a i n . c

     4

 *

     5

 *  EtherCAT domain methods.

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include "globals.h"

    12

#include "domain.h"

    13

#include "master.h"

    14

    15

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

    16

    17

void ec_domain_clear_field_regs(ec_domain_t *);

    18

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

    19

    20

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

    21

    22

EC_SYSFS_READ_ATTR(data_size);

    23

    24

static struct attribute *def_attrs[] = {

    25

    &attr_data_size,

    26

    NULL,

    27

};

    28

    29

static struct sysfs_ops sysfs_ops = {

    30

    .show = &ec_show_domain_attribute,

    31

    .store = NULL

    32

};

    33

    34

static struct kobj_type ktype_ec_domain = {

    35

    .release = ec_domain_clear,

    36

    .sysfs_ops = &sysfs_ops,

    37

    .default_attrs = def_attrs

    38

};

    39

    40

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

    41

    42

/**

    43

   Domain constructor.

    44

   \return 0 in case of success, else < 0

    45

*/

    46

    47

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

    48

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

    49

                   unsigned int index /**< domain index */

    50

                   )

    51

{

    52

    domain->master = master;

    53

    domain->index = index;

    54

    domain->data_size = 0;

    55

    domain->base_address = 0;

    56

    domain->response_count = 0xFFFFFFFF;

    57

    58

    INIT_LIST_HEAD(&domain->field_regs);

    59

    INIT_LIST_HEAD(&domain->commands);

    60

    61

    // init kobject and add it to the hierarchy

    62

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

    63

    kobject_init(&domain->kobj);

    64

    domain->kobj.ktype = &ktype_ec_domain;

    65

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

    66

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

    67

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

    68

        return -1;

    69

    }

    70

    71

    return 0;

    72

}

    73

    74

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

    75

    76

/**

    77

   Domain destructor.

    78

*/

    79

    80

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

    81

{

    82

    ec_command_t *command, *next;

    83

    ec_domain_t *domain;

    84

    85

    domain = container_of(kobj, ec_domain_t, kobj);

    86

    87

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

    88

    89

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

    90

        ec_command_clear(command);

    91

        kfree(command);

    92

    }

    93

    94

    ec_domain_clear_field_regs(domain);

    95

    96

    kfree(domain);

    97

}

    98

    99

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

   100

   101

/**

   102

   Registeres a data field in a domain.

   103

   \return 0 in case of success, else < 0

   104

*/

   105

   106

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

   107

                        ec_slave_t *slave, /**< slave */

   108

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

   109

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

   110

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

   111

                                           pointer */

   112

                        )

   113

{

   114

    ec_field_reg_t *field_reg;

   115

   116

    if (!(field_reg =

   117

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

   118

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

   119

        return -1;

   120

    }

   121

   122

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

   123

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

   124

        kfree(field_reg);

   125

        return -1;

   126

    }

   127

   128

    field_reg->slave = slave;

   129

    field_reg->sync = sync;

   130

    field_reg->field_offset = field_offset;

   131

    field_reg->data_ptr = data_ptr;

   132

   133

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

   134

    return 0;

   135

}

   136

   137

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

   138

   139

/**

   140

   Clears the list of the registered data fields.

   141

*/

   142

   143

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

   144

{

   145

    ec_field_reg_t *field_reg, *next;

   146

   147

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

   148

        list_del(&field_reg->list);

   149

        kfree(field_reg);

   150

    }

   151

}

   152

   153

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

   154

   155

/**

   156

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

   157

   \return 0 in case of success, else < 0

   158

*/

   159

   160

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

   161

                          uint32_t offset, /**< logical offset */

   162

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

   163

                          )

   164

{

   165

    ec_command_t *command;

   166

   167

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

   168

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

   169

        return -1;

   170

    }

   171

   172

    ec_command_init(command);

   173

   174

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

   175

        kfree(command);

   176

        return -1;

   177

    }

   178

   179

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

   180

    return 0;

   181

}

   182

   183

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

   184

   185

/**

   186

   Creates a domain.

   187

   Reserves domain memory, calculates the logical addresses of the

   188

   corresponding FMMUs and sets the process data pointer of the registered

   189

   data fields.

   190

   \return 0 in case of success, else < 0

   191

*/

   192

   193

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

   194

                    uint32_t base_address /**< Logische Basisadresse */

   195

                    )

   196

{

   197

    ec_field_reg_t *field_reg;

   198

    ec_slave_t *slave;

   199

    ec_fmmu_t *fmmu;

   200

    unsigned int i, j, cmd_count;

   201

    uint32_t field_off, field_off_cmd;

   202

    uint32_t cmd_offset;

   203

    size_t cmd_data_size;

   204

    ec_command_t *command;

   205

   206

    domain->base_address = base_address;

   207

   208

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

   209

    domain->data_size = 0;

   210

    cmd_offset = base_address;

   211

    cmd_data_size = 0;

   212

    cmd_count = 0;

   213

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

   214

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

   215

            fmmu = &slave->fmmus[j];

   216

            if (fmmu->domain == domain) {

   217

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

   218

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

   219

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

   220

                    if (ec_domain_add_command(domain, cmd_offset,

   221

                                              cmd_data_size)) return -1;

   222

                    cmd_offset += cmd_data_size;

   223

                    cmd_data_size = 0;

   224

                    cmd_count++;

   225

                }

   226

                cmd_data_size += fmmu->sync->size;

   227

            }

   228

        }

   229

    }

   230

   231

    // Letztes Kommando allozieren

   232

    if (cmd_data_size) {

   233

        if (ec_domain_add_command(domain, cmd_offset, cmd_data_size))

   234

            return -1;

   235

        cmd_count++;

   236

    }

   237

   238

    if (!cmd_count) {

   239

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

   240

        ec_domain_clear_field_regs(domain);

   241

        return 0;

   242

    }

   243

   244

    // Alle Prozessdatenzeiger setzen

   245

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

   246

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

   247

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

   248

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

   249

                field_off = fmmu->logical_start_address +

   250

                    field_reg->field_offset;

   251

                // Kommando suchen

   252

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

   253

                    field_off_cmd = field_off - command->address.logical;

   254

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

   255

                        field_off_cmd < command->mem_size) {

   256

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

   257

                    }

   258

                }

   259

                if (!field_reg->data_ptr) {

   260

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

   261

                    return -1;

   262

                }

   263

                break;

   264

            }

   265

        }

   266

    }

   267

   268

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

   269

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

   270

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

   271

   272

    ec_domain_clear_field_regs(domain);

   273

   274

    return 0;

   275

}

   276

   277

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

   278

   279

/**

   280

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

   281

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

   282

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

   283

   working counter by 2, some by 1.

   284

*/

   285

   286

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

   287

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

   288

                              )

   289

{

   290

    if (count != domain->response_count) {

   291

        domain->response_count = count;

   292

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

   293

                count);

   294

    }

   295

}

   296

   297

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

   298

   299

/**

   300

   Formats attribute data for SysFS reading.

   301

   \return number of bytes to read

   302

*/

   303

   304

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

   305

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

   306

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

   307

                                 )

   308

{

   309

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

   310

   311

    if (attr == &attr_data_size) {

   312

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

   313

    }

   314

   315

    return 0;

   316

}

   317

   318

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

   319

 *  Realtime interface

   320

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

   321

   322

/**

   323

   Registers a data field in a domain.

   324

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

   325

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

   326

   registered.

   327

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

   328

   is an array of the respective size.

   329

   \return pointer to the slave on success, else NULL

   330

*/

   331

   332

ec_slave_t *ecrt_domain_register_field(ec_domain_t *domain,

   333

                                       /**< EtherCAT domain */

   334

                                       const char *address,

   335

                                       /**< ASCII address of the slave,

   336

                                          see ecrt_master_get_slave() */

   337

                                       const char *vendor_name,

   338

                                       /**< vendor name */

   339

                                       const char *product_name,

   340

                                       /**< product name */

   341

                                       void **data_ptr,

   342

                                       /**< address of the process data

   343

                                          pointer */

   344

                                       const char *field_name,

   345

                                       /**< data field name */

   346

                                       unsigned int field_index,

   347

                                       /**< offset of data fields with

   348

                                          \a field_type  */

   349

                                       unsigned int field_count

   350

                                       /**< number of data fields (with

   351

                                          the same type) to register */

   352

                                       )

   353

{

   354

    ec_slave_t *slave;

   355

    const ec_slave_type_t *type;

   356

    ec_master_t *master;

   357

    const ec_sync_t *sync;

   358

    const ec_field_t *field;

   359

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

   360

    uint32_t field_offset;

   361

   362

    master = domain->master;

   363

   364

    // translate address

   365

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

   366

   367

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

   368

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

   369

               slave->ring_position);

   370

        return NULL;

   371

    }

   372

   373

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

   374

        strcmp(product_name, type->product_name)) {

   375

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

   376

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

   377

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

   378

        return NULL;

   379

    }

   380

   381

    if (!data_ptr) {

   382

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

   383

        slave->registered = 1;

   384

    }

   385

   386

    if (!field_count) field_count = 1;

   387

    orig_field_index = field_index;

   388

    orig_field_count = field_count;

   389

   390

    field_counter = 0;

   391

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

   392

        sync = type->sync_managers[i];

   393

        field_offset = 0;

   394

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

   395

            field = sync->fields[j];

   396

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

   397

                if (field_counter++ == field_index) {

   398

                    if (data_ptr)

   399

                        ec_domain_reg_field(domain, slave, sync, field_offset,

   400

                                            data_ptr++);

   401

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

   402

                    field_index++;

   403

                }

   404

            }

   405

            field_offset += field->size;

   406

        }

   407

    }

   408

   409

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

   410

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

   411

           product_name, field_name, orig_field_index, orig_field_count);

   412

    return NULL;

   413

}

   414

   415

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

   416

   417

/**

   418

   Registeres a bunch of data fields.

   419

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

   420

   \return 0 in case of success, else < 0

   421

*/

   422

   423

int ecrt_domain_register_field_list(ec_domain_t *domain,

   424

                                    /**< EtherCAT domain */

   425

                                    const ec_field_init_t *fields

   426

                                    /**< array of data field registrations */

   427

                                    )

   428

{

   429

    const ec_field_init_t *field;

   430

   431

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

   432

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

   433

                                        field->vendor_name,

   434

                                        field->product_name, field->data_ptr,

   435

                                        field->field_name, field->field_index,

   436

                                        field->field_count))

   437

            return -1;

   438

   439

    return 0;

   440

}

   441

   442

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

   443

   444

/**

   445

   Places all process data commands in the masters command queue.

   446

*/

   447

   448

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

   449

{

   450

    ec_command_t *command;

   451

   452

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

   453

        ec_master_queue_command(domain->master, command);

   454

    }

   455

}

   456

   457

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

   458

   459

/**

   460

   Processes received process data.

   461

*/

   462

   463

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

   464

{

   465

    unsigned int working_counter_sum;

   466

    ec_command_t *command;

   467

   468

    working_counter_sum = 0;

   469

   470

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

   471

        if (command->state == EC_CMD_RECEIVED) {

   472

            working_counter_sum += command->working_counter;

   473

        }

   474

    }

   475

   476

    ec_domain_response_count(domain, working_counter_sum);

   477

}

   478

   479

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

   480

   481

/**

   482

   Returns the state of a domain.

   483

   \return 0 if all commands were received, else -1.

   484

*/

   485

   486

int ecrt_domain_state(ec_domain_t *domain /**< EtherCAT domain */)

   487

{