master/domain.c
changeset 293 14aeb79aa992
parent 275 ba51285d4ef6
child 294 feea8d850c65
equal deleted inserted replaced
292:2cf6ae0a2419 293:14aeb79aa992
    88     domain->data_size = 0;
    88     domain->data_size = 0;
    89     domain->base_address = 0;
    89     domain->base_address = 0;
    90     domain->response_count = 0xFFFFFFFF;
    90     domain->response_count = 0xFFFFFFFF;
    91 
    91 
    92     INIT_LIST_HEAD(&domain->field_regs);
    92     INIT_LIST_HEAD(&domain->field_regs);
    93     INIT_LIST_HEAD(&domain->commands);
    93     INIT_LIST_HEAD(&domain->datagrams);
    94 
    94 
    95     // init kobject and add it to the hierarchy
    95     // init kobject and add it to the hierarchy
    96     memset(&domain->kobj, 0x00, sizeof(struct kobject));
    96     memset(&domain->kobj, 0x00, sizeof(struct kobject));
    97     kobject_init(&domain->kobj);
    97     kobject_init(&domain->kobj);
    98     domain->kobj.ktype = &ktype_ec_domain;
    98     domain->kobj.ktype = &ktype_ec_domain;
   111    Domain destructor.
   111    Domain destructor.
   112 */
   112 */
   113 
   113 
   114 void ec_domain_clear(struct kobject *kobj /**< kobject of the domain */)
   114 void ec_domain_clear(struct kobject *kobj /**< kobject of the domain */)
   115 {
   115 {
   116     ec_command_t *command, *next;
   116     ec_datagram_t *datagram, *next;
   117     ec_domain_t *domain;
   117     ec_domain_t *domain;
   118 
   118 
   119     domain = container_of(kobj, ec_domain_t, kobj);
   119     domain = container_of(kobj, ec_domain_t, kobj);
   120 
   120 
   121     EC_INFO("Clearing domain %i.\n", domain->index);
   121     EC_INFO("Clearing domain %i.\n", domain->index);
   122 
   122 
   123     list_for_each_entry_safe(command, next, &domain->commands, list) {
   123     list_for_each_entry_safe(datagram, next, &domain->datagrams, list) {
   124         ec_command_clear(command);
   124         ec_datagram_clear(datagram);
   125         kfree(command);
   125         kfree(datagram);
   126     }
   126     }
   127 
   127 
   128     ec_domain_clear_field_regs(domain);
   128     ec_domain_clear_field_regs(domain);
   129 
   129 
   130     kfree(domain);
   130     kfree(domain);
   185 }
   185 }
   186 
   186 
   187 /*****************************************************************************/
   187 /*****************************************************************************/
   188 
   188 
   189 /**
   189 /**
   190    Allocates a process data command and appends it to the list.
   190    Allocates a process data datagram and appends it to the list.
   191    \return 0 in case of success, else < 0
   191    \return 0 in case of success, else < 0
   192 */
   192 */
   193 
   193 
   194 int ec_domain_add_command(ec_domain_t *domain, /**< EtherCAT domain */
   194 int ec_domain_add_datagram(ec_domain_t *domain, /**< EtherCAT domain */
   195                           uint32_t offset, /**< logical offset */
   195                            uint32_t offset, /**< logical offset */
   196                           size_t data_size /**< size of the command data */
   196                            size_t data_size /**< size of the datagram data */
   197                           )
   197                            )
   198 {
   198 {
   199     ec_command_t *command;
   199     ec_datagram_t *datagram;
   200 
   200 
   201     if (!(command = kmalloc(sizeof(ec_command_t), GFP_KERNEL))) {
   201     if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) {
   202         EC_ERR("Failed to allocate domain command!\n");
   202         EC_ERR("Failed to allocate domain datagram!\n");
   203         return -1;
   203         return -1;
   204     }
   204     }
   205 
   205 
   206     ec_command_init(command);
   206     ec_datagram_init(datagram);
   207 
   207 
   208     if (ec_command_lrw(command, offset, data_size)) {
   208     if (ec_datagram_lrw(datagram, offset, data_size)) {
   209         kfree(command);
   209         kfree(datagram);
   210         return -1;
   210         return -1;
   211     }
   211     }
   212 
   212 
   213     list_add_tail(&command->list, &domain->commands);
   213     list_add_tail(&datagram->list, &domain->datagrams);
   214     return 0;
   214     return 0;
   215 }
   215 }
   216 
   216 
   217 /*****************************************************************************/
   217 /*****************************************************************************/
   218 
   218 
   233     ec_fmmu_t *fmmu;
   233     ec_fmmu_t *fmmu;
   234     unsigned int i, j, cmd_count;
   234     unsigned int i, j, cmd_count;
   235     uint32_t field_off, field_off_cmd;
   235     uint32_t field_off, field_off_cmd;
   236     uint32_t cmd_offset;
   236     uint32_t cmd_offset;
   237     size_t cmd_data_size, sync_size;
   237     size_t cmd_data_size, sync_size;
   238     ec_command_t *command;
   238     ec_datagram_t *datagram;
   239 
   239 
   240     domain->base_address = base_address;
   240     domain->base_address = base_address;
   241 
   241 
   242     // calculate size of process data and allocate memory
   242     // calculate size of process data and allocate memory
   243     domain->data_size = 0;
   243     domain->data_size = 0;
   250             if (fmmu->domain == domain) {
   250             if (fmmu->domain == domain) {
   251                 fmmu->logical_start_address = base_address + domain->data_size;
   251                 fmmu->logical_start_address = base_address + domain->data_size;
   252                 sync_size = ec_slave_calc_sync_size(slave, fmmu->sync);
   252                 sync_size = ec_slave_calc_sync_size(slave, fmmu->sync);
   253                 domain->data_size += sync_size;
   253                 domain->data_size += sync_size;
   254                 if (cmd_data_size + sync_size > EC_MAX_DATA_SIZE) {
   254                 if (cmd_data_size + sync_size > EC_MAX_DATA_SIZE) {
   255                     if (ec_domain_add_command(domain, cmd_offset,
   255                     if (ec_domain_add_datagram(domain, cmd_offset,
   256                                               cmd_data_size)) return -1;
   256                                                cmd_data_size)) return -1;
   257                     cmd_offset += cmd_data_size;
   257                     cmd_offset += cmd_data_size;
   258                     cmd_data_size = 0;
   258                     cmd_data_size = 0;
   259                     cmd_count++;
   259                     cmd_count++;
   260                 }
   260                 }
   261                 cmd_data_size += sync_size;
   261                 cmd_data_size += sync_size;
   262             }
   262             }
   263         }
   263         }
   264     }
   264     }
   265 
   265 
   266     // allocate last command
   266     // allocate last datagram
   267     if (cmd_data_size) {
   267     if (cmd_data_size) {
   268         if (ec_domain_add_command(domain, cmd_offset, cmd_data_size))
   268         if (ec_domain_add_datagram(domain, cmd_offset, cmd_data_size))
   269             return -1;
   269             return -1;
   270         cmd_count++;
   270         cmd_count++;
   271     }
   271     }
   272 
   272 
   273     if (!cmd_count) {
   273     if (!cmd_count) {
   281         for (i = 0; i < field_reg->slave->fmmu_count; i++) {
   281         for (i = 0; i < field_reg->slave->fmmu_count; i++) {
   282             fmmu = &field_reg->slave->fmmus[i];
   282             fmmu = &field_reg->slave->fmmus[i];
   283             if (fmmu->domain == domain && fmmu->sync == field_reg->sync) {
   283             if (fmmu->domain == domain && fmmu->sync == field_reg->sync) {
   284                 field_off = fmmu->logical_start_address +
   284                 field_off = fmmu->logical_start_address +
   285                     field_reg->field_offset;
   285                     field_reg->field_offset;
   286                 // search command
   286                 // search datagram
   287                 list_for_each_entry(command, &domain->commands, list) {
   287                 list_for_each_entry(datagram, &domain->datagrams, list) {
   288                     field_off_cmd = field_off - command->address.logical;
   288                     field_off_cmd = field_off - datagram->address.logical;
   289                     if (field_off >= command->address.logical &&
   289                     if (field_off >= datagram->address.logical &&
   290                         field_off_cmd < command->mem_size) {
   290                         field_off_cmd < datagram->mem_size) {
   291                         *field_reg->data_ptr = command->data + field_off_cmd;
   291                         *field_reg->data_ptr = datagram->data + field_off_cmd;
   292                     }
   292                     }
   293                 }
   293                 }
   294                 if (!field_reg->data_ptr) {
   294                 if (!field_reg->data_ptr) {
   295                     EC_ERR("Failed to assign data pointer!\n");
   295                     EC_ERR("Failed to assign data pointer!\n");
   296                     return -1;
   296                     return -1;
   298                 break;
   298                 break;
   299             }
   299             }
   300         }
   300         }
   301     }
   301     }
   302 
   302 
   303     EC_INFO("Domain %i - Allocated %i bytes in %i command%s\n",
   303     EC_INFO("Domain %i - Allocated %i bytes in %i datagram%s\n",
   304             domain->index, domain->data_size, cmd_count,
   304             domain->index, domain->data_size, cmd_count,
   305             cmd_count == 1 ? "" : "s");
   305             cmd_count == 1 ? "" : "s");
   306 
   306 
   307     ec_domain_clear_field_regs(domain);
   307     ec_domain_clear_field_regs(domain);
   308 
   308 
   312 /*****************************************************************************/
   312 /*****************************************************************************/
   313 
   313 
   314 /**
   314 /**
   315    Sets the number of responding slaves and outputs it on demand.
   315    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
   316    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
   317    the working counters of all domain datagrams. Some slaves increase the
   318    working counter by 2, some by 1.
   318    working counter by 2, some by 1.
   319 */
   319 */
   320 
   320 
   321 void ec_domain_response_count(ec_domain_t *domain, /**< EtherCAT domain */
   321 void ec_domain_response_count(ec_domain_t *domain, /**< EtherCAT domain */
   322                               unsigned int count /**< new WC sum */
   322                               unsigned int count /**< new WC sum */
   477 }
   477 }
   478 
   478 
   479 /*****************************************************************************/
   479 /*****************************************************************************/
   480 
   480 
   481 /**
   481 /**
   482    Places all process data commands in the masters command queue.
   482    Places all process data datagrams in the masters datagram queue.
   483    \ingroup RealtimeInterface
   483    \ingroup RealtimeInterface
   484 */
   484 */
   485 
   485 
   486 void ecrt_domain_queue(ec_domain_t *domain /**< EtherCAT domain */)
   486 void ecrt_domain_queue(ec_domain_t *domain /**< EtherCAT domain */)
   487 {
   487 {
   488     ec_command_t *command;
   488     ec_datagram_t *datagram;
   489 
   489 
   490     list_for_each_entry(command, &domain->commands, list) {
   490     list_for_each_entry(datagram, &domain->datagrams, list) {
   491         ec_master_queue_command(domain->master, command);
   491         ec_master_queue_datagram(domain->master, datagram);
   492     }
   492     }
   493 }
   493 }
   494 
   494 
   495 /*****************************************************************************/
   495 /*****************************************************************************/
   496 
   496 
   500 */
   500 */
   501 
   501 
   502 void ecrt_domain_process(ec_domain_t *domain /**< EtherCAT domain */)
   502 void ecrt_domain_process(ec_domain_t *domain /**< EtherCAT domain */)
   503 {
   503 {
   504     unsigned int working_counter_sum;
   504     unsigned int working_counter_sum;
   505     ec_command_t *command;
   505     ec_datagram_t *datagram;
   506 
   506 
   507     working_counter_sum = 0;
   507     working_counter_sum = 0;
   508 
   508 
   509     list_for_each_entry(command, &domain->commands, list) {
   509     list_for_each_entry(datagram, &domain->datagrams, list) {
   510         if (command->state == EC_CMD_RECEIVED) {
   510         if (datagram->state == EC_CMD_RECEIVED) {
   511             working_counter_sum += command->working_counter;
   511             working_counter_sum += datagram->working_counter;
   512         }
   512         }
   513     }
   513     }
   514 
   514 
   515     ec_domain_response_count(domain, working_counter_sum);
   515     ec_domain_response_count(domain, working_counter_sum);
   516 }
   516 }
   517 
   517 
   518 /*****************************************************************************/
   518 /*****************************************************************************/
   519 
   519 
   520 /**
   520 /**
   521    Returns the state of a domain.
   521    Returns the state of a domain.
   522    \return 0 if all commands were received, else -1.
   522    \return 0 if all datagrams were received, else -1.
   523    \ingroup RealtimeInterface
   523    \ingroup RealtimeInterface
   524 */
   524 */
   525 
   525 
   526 int ecrt_domain_state(ec_domain_t *domain /**< EtherCAT domain */)
   526 int ecrt_domain_state(ec_domain_t *domain /**< EtherCAT domain */)
   527 {
   527 {
   528     ec_command_t *command;
   528     ec_datagram_t *datagram;
   529 
   529 
   530     list_for_each_entry(command, &domain->commands, list) {
   530     list_for_each_entry(datagram, &domain->datagrams, list) {
   531         if (command->state != EC_CMD_RECEIVED) return -1;
   531         if (datagram->state != EC_CMD_RECEIVED) return -1;
   532     }
   532     }
   533 
   533 
   534     return 0;
   534     return 0;
   535 }
   535 }
   536 
   536