master/domain.c
branchstable-1.0
changeset 1624 9dc190591c0f
parent 1621 4bbe090553f7
equal deleted inserted replaced
1623:05622513f627 1624:9dc190591c0f
    36    EtherCAT domain methods.
    36    EtherCAT domain methods.
    37 */
    37 */
    38 
    38 
    39 /*****************************************************************************/
    39 /*****************************************************************************/
    40 
    40 
       
    41 #include <linux/module.h>
       
    42 
    41 #include "globals.h"
    43 #include "globals.h"
    42 #include "domain.h"
    44 #include "domain.h"
    43 #include "master.h"
    45 #include "master.h"
    44 
    46 
    45 /*****************************************************************************/
    47 /*****************************************************************************/
    49 
    51 
    50 /*****************************************************************************/
    52 /*****************************************************************************/
    51 
    53 
    52 /** \cond */
    54 /** \cond */
    53 
    55 
    54 EC_SYSFS_READ_ATTR(data_size);
    56 EC_SYSFS_READ_ATTR(image_size);
    55 
    57 
    56 static struct attribute *def_attrs[] = {
    58 static struct attribute *def_attrs[] = {
    57     &attr_data_size,
    59     &attr_image_size,
    58     NULL,
    60     NULL,
    59 };
    61 };
    60 
    62 
    61 static struct sysfs_ops sysfs_ops = {
    63 static struct sysfs_ops sysfs_ops = {
    62     .show = &ec_show_domain_attribute,
    64     .show = &ec_show_domain_attribute,
    88     domain->data_size = 0;
    90     domain->data_size = 0;
    89     domain->base_address = 0;
    91     domain->base_address = 0;
    90     domain->response_count = 0xFFFFFFFF;
    92     domain->response_count = 0xFFFFFFFF;
    91 
    93 
    92     INIT_LIST_HEAD(&domain->field_regs);
    94     INIT_LIST_HEAD(&domain->field_regs);
    93     INIT_LIST_HEAD(&domain->commands);
    95     INIT_LIST_HEAD(&domain->datagrams);
    94 
    96 
    95     // init kobject and add it to the hierarchy
    97     // init kobject and add it to the hierarchy
    96     memset(&domain->kobj, 0x00, sizeof(struct kobject));
    98     memset(&domain->kobj, 0x00, sizeof(struct kobject));
    97     kobject_init(&domain->kobj);
    99     kobject_init(&domain->kobj);
    98     domain->kobj.ktype = &ktype_ec_domain;
   100     domain->kobj.ktype = &ktype_ec_domain;
   111    Domain destructor.
   113    Domain destructor.
   112 */
   114 */
   113 
   115 
   114 void ec_domain_clear(struct kobject *kobj /**< kobject of the domain */)
   116 void ec_domain_clear(struct kobject *kobj /**< kobject of the domain */)
   115 {
   117 {
   116     ec_command_t *command, *next;
   118     ec_datagram_t *datagram, *next;
   117     ec_domain_t *domain;
   119     ec_domain_t *domain;
   118 
   120 
   119     domain = container_of(kobj, ec_domain_t, kobj);
   121     domain = container_of(kobj, ec_domain_t, kobj);
   120 
   122 
   121     EC_INFO("Clearing domain %i.\n", domain->index);
   123     EC_INFO("Clearing domain %i.\n", domain->index);
   122 
   124 
   123     list_for_each_entry_safe(command, next, &domain->commands, list) {
   125     list_for_each_entry_safe(datagram, next, &domain->datagrams, list) {
   124         ec_command_clear(command);
   126         ec_datagram_clear(datagram);
   125         kfree(command);
   127         kfree(datagram);
   126     }
   128     }
   127 
   129 
   128     ec_domain_clear_field_regs(domain);
   130     ec_domain_clear_field_regs(domain);
   129 
   131 
   130     kfree(domain);
   132     kfree(domain);
   185 }
   187 }
   186 
   188 
   187 /*****************************************************************************/
   189 /*****************************************************************************/
   188 
   190 
   189 /**
   191 /**
   190    Allocates a process data command and appends it to the list.
   192    Allocates a process data datagram and appends it to the list.
   191    \return 0 in case of success, else < 0
   193    \return 0 in case of success, else < 0
   192 */
   194 */
   193 
   195 
   194 int ec_domain_add_command(ec_domain_t *domain, /**< EtherCAT domain */
   196 int ec_domain_add_datagram(ec_domain_t *domain, /**< EtherCAT domain */
   195                           uint32_t offset, /**< logical offset */
   197                            uint32_t offset, /**< logical offset */
   196                           size_t data_size /**< size of the command data */
   198                            size_t data_size /**< size of the datagram data */
   197                           )
   199                            )
   198 {
   200 {
   199     ec_command_t *command;
   201     ec_datagram_t *datagram;
   200 
   202 
   201     if (!(command = kmalloc(sizeof(ec_command_t), GFP_KERNEL))) {
   203     if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) {
   202         EC_ERR("Failed to allocate domain command!\n");
   204         EC_ERR("Failed to allocate domain datagram!\n");
   203         return -1;
   205         return -1;
   204     }
   206     }
   205 
   207 
   206     ec_command_init(command);
   208     ec_datagram_init(datagram);
   207 
   209 
   208     if (ec_command_lrw(command, offset, data_size)) {
   210     if (ec_datagram_lrw(datagram, offset, data_size)) {
   209         kfree(command);
   211         kfree(datagram);
   210         return -1;
   212         return -1;
   211     }
   213     }
   212 
   214 
   213     list_add_tail(&command->list, &domain->commands);
   215     list_add_tail(&datagram->list, &domain->datagrams);
   214     return 0;
   216     return 0;
   215 }
   217 }
   216 
   218 
   217 /*****************************************************************************/
   219 /*****************************************************************************/
   218 
   220 
   233     ec_fmmu_t *fmmu;
   235     ec_fmmu_t *fmmu;
   234     unsigned int i, j, cmd_count;
   236     unsigned int i, j, cmd_count;
   235     uint32_t field_off, field_off_cmd;
   237     uint32_t field_off, field_off_cmd;
   236     uint32_t cmd_offset;
   238     uint32_t cmd_offset;
   237     size_t cmd_data_size, sync_size;
   239     size_t cmd_data_size, sync_size;
   238     ec_command_t *command;
   240     ec_datagram_t *datagram;
   239 
   241 
   240     domain->base_address = base_address;
   242     domain->base_address = base_address;
   241 
   243 
   242     // calculate size of process data and allocate memory
   244     // calculate size of process data and allocate memory
   243     domain->data_size = 0;
   245     domain->data_size = 0;
   250             if (fmmu->domain == domain) {
   252             if (fmmu->domain == domain) {
   251                 fmmu->logical_start_address = base_address + domain->data_size;
   253                 fmmu->logical_start_address = base_address + domain->data_size;
   252                 sync_size = ec_slave_calc_sync_size(slave, fmmu->sync);
   254                 sync_size = ec_slave_calc_sync_size(slave, fmmu->sync);
   253                 domain->data_size += sync_size;
   255                 domain->data_size += sync_size;
   254                 if (cmd_data_size + sync_size > EC_MAX_DATA_SIZE) {
   256                 if (cmd_data_size + sync_size > EC_MAX_DATA_SIZE) {
   255                     if (ec_domain_add_command(domain, cmd_offset,
   257                     if (ec_domain_add_datagram(domain, cmd_offset,
   256                                               cmd_data_size)) return -1;
   258                                                cmd_data_size)) return -1;
   257                     cmd_offset += cmd_data_size;
   259                     cmd_offset += cmd_data_size;
   258                     cmd_data_size = 0;
   260                     cmd_data_size = 0;
   259                     cmd_count++;
   261                     cmd_count++;
   260                 }
   262                 }
   261                 cmd_data_size += sync_size;
   263                 cmd_data_size += sync_size;
   262             }
   264             }
   263         }
   265         }
   264     }
   266     }
   265 
   267 
   266     // allocate last command
   268     // allocate last datagram
   267     if (cmd_data_size) {
   269     if (cmd_data_size) {
   268         if (ec_domain_add_command(domain, cmd_offset, cmd_data_size))
   270         if (ec_domain_add_datagram(domain, cmd_offset, cmd_data_size))
   269             return -1;
   271             return -1;
   270         cmd_count++;
   272         cmd_count++;
   271     }
   273     }
   272 
   274 
   273     if (!cmd_count) {
   275     if (!cmd_count) {
   281         for (i = 0; i < field_reg->slave->fmmu_count; i++) {
   283         for (i = 0; i < field_reg->slave->fmmu_count; i++) {
   282             fmmu = &field_reg->slave->fmmus[i];
   284             fmmu = &field_reg->slave->fmmus[i];
   283             if (fmmu->domain == domain && fmmu->sync == field_reg->sync) {
   285             if (fmmu->domain == domain && fmmu->sync == field_reg->sync) {
   284                 field_off = fmmu->logical_start_address +
   286                 field_off = fmmu->logical_start_address +
   285                     field_reg->field_offset;
   287                     field_reg->field_offset;
   286                 // search command
   288                 // search datagram
   287                 list_for_each_entry(command, &domain->commands, list) {
   289                 list_for_each_entry(datagram, &domain->datagrams, list) {
   288                     field_off_cmd = field_off - command->address.logical;
   290                     field_off_cmd = field_off - datagram->address.logical;
   289                     if (field_off >= command->address.logical &&
   291                     if (field_off >= datagram->address.logical &&
   290                         field_off_cmd < command->mem_size) {
   292                         field_off_cmd < datagram->mem_size) {
   291                         *field_reg->data_ptr = command->data + field_off_cmd;
   293                         *field_reg->data_ptr = datagram->data + field_off_cmd;
   292                     }
   294                     }
   293                 }
   295                 }
   294                 if (!field_reg->data_ptr) {
   296                 if (!field_reg->data_ptr) {
   295                     EC_ERR("Failed to assign data pointer!\n");
   297                     EC_ERR("Failed to assign data pointer!\n");
   296                     return -1;
   298                     return -1;
   298                 break;
   300                 break;
   299             }
   301             }
   300         }
   302         }
   301     }
   303     }
   302 
   304 
   303     EC_INFO("Domain %i - Allocated %i bytes in %i command%s\n",
   305     EC_INFO("Domain %i - Allocated %i bytes in %i datagram%s\n",
   304             domain->index, domain->data_size, cmd_count,
   306             domain->index, domain->data_size, cmd_count,
   305             cmd_count == 1 ? "" : "s");
   307             cmd_count == 1 ? "" : "s");
   306 
   308 
   307     ec_domain_clear_field_regs(domain);
   309     ec_domain_clear_field_regs(domain);
   308 
   310 
   312 /*****************************************************************************/
   314 /*****************************************************************************/
   313 
   315 
   314 /**
   316 /**
   315    Sets the number of responding slaves and outputs it on demand.
   317    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
   318    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
   319    the working counters of all domain datagrams. Some slaves increase the
   318    working counter by 2, some by 1.
   320    working counter by 2, some by 1.
   319 */
   321 */
   320 
   322 
   321 void ec_domain_response_count(ec_domain_t *domain, /**< EtherCAT domain */
   323 void ec_domain_response_count(ec_domain_t *domain, /**< EtherCAT domain */
   322                               unsigned int count /**< new WC sum */
   324                               unsigned int count /**< new WC sum */
   341                                  char *buffer /**< memory to store data in */
   343                                  char *buffer /**< memory to store data in */
   342                                  )
   344                                  )
   343 {
   345 {
   344     ec_domain_t *domain = container_of(kobj, ec_domain_t, kobj);
   346     ec_domain_t *domain = container_of(kobj, ec_domain_t, kobj);
   345 
   347 
   346     if (attr == &attr_data_size) {
   348     if (attr == &attr_image_size) {
   347         return sprintf(buffer, "%i\n", domain->data_size);
   349         return sprintf(buffer, "%i\n", domain->data_size);
   348     }
   350     }
   349 
   351 
   350     return 0;
   352     return 0;
   351 }
   353 }
   477 }
   479 }
   478 
   480 
   479 /*****************************************************************************/
   481 /*****************************************************************************/
   480 
   482 
   481 /**
   483 /**
   482    Places all process data commands in the masters command queue.
   484    Places all process data datagrams in the masters datagram queue.
   483    \ingroup RealtimeInterface
   485    \ingroup RealtimeInterface
   484 */
   486 */
   485 
   487 
   486 void ecrt_domain_queue(ec_domain_t *domain /**< EtherCAT domain */)
   488 void ecrt_domain_queue(ec_domain_t *domain /**< EtherCAT domain */)
   487 {
   489 {
   488     ec_command_t *command;
   490     ec_datagram_t *datagram;
   489 
   491 
   490     list_for_each_entry(command, &domain->commands, list) {
   492     list_for_each_entry(datagram, &domain->datagrams, list) {
   491         ec_master_queue_command(domain->master, command);
   493         ec_master_queue_datagram(domain->master, datagram);
   492     }
   494     }
   493 }
   495 }
   494 
   496 
   495 /*****************************************************************************/
   497 /*****************************************************************************/
   496 
   498 
   500 */
   502 */
   501 
   503 
   502 void ecrt_domain_process(ec_domain_t *domain /**< EtherCAT domain */)
   504 void ecrt_domain_process(ec_domain_t *domain /**< EtherCAT domain */)
   503 {
   505 {
   504     unsigned int working_counter_sum;
   506     unsigned int working_counter_sum;
   505     ec_command_t *command;
   507     ec_datagram_t *datagram;
   506 
   508 
   507     working_counter_sum = 0;
   509     working_counter_sum = 0;
   508 
   510 
   509     list_for_each_entry(command, &domain->commands, list) {
   511     list_for_each_entry(datagram, &domain->datagrams, list) {
   510         if (command->state == EC_CMD_RECEIVED) {
   512         if (datagram->state == EC_CMD_RECEIVED) {
   511             working_counter_sum += command->working_counter;
   513             working_counter_sum += datagram->working_counter;
   512         }
   514         }
   513     }
   515     }
   514 
   516 
   515     ec_domain_response_count(domain, working_counter_sum);
   517     ec_domain_response_count(domain, working_counter_sum);
   516 }
   518 }
   517 
   519 
   518 /*****************************************************************************/
   520 /*****************************************************************************/
   519 
   521 
   520 /**
   522 /**
   521    Returns the state of a domain.
   523    Returns the state of a domain.
   522    \return 0 if all commands were received, else -1.
   524    \return 0 if all datagrams were received, else -1.
   523    \ingroup RealtimeInterface
   525    \ingroup RealtimeInterface
   524 */
   526 */
   525 
   527 
   526 int ecrt_domain_state(ec_domain_t *domain /**< EtherCAT domain */)
   528 int ecrt_domain_state(ec_domain_t *domain /**< EtherCAT domain */)
   527 {
   529 {
   528     ec_command_t *command;
   530     ec_datagram_t *datagram;
   529 
   531 
   530     list_for_each_entry(command, &domain->commands, list) {
   532     list_for_each_entry(datagram, &domain->datagrams, list) {
   531         if (command->state != EC_CMD_RECEIVED) return -1;
   533         if (datagram->state != EC_CMD_RECEIVED) return -1;
   532     }
   534     }
   533 
   535 
   534     return 0;
   536     return 0;
   535 }
   537 }
   536 
   538