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 |