116 // init kobject and add it to the hierarchy |
116 // init kobject and add it to the hierarchy |
117 memset(&domain->kobj, 0x00, sizeof(struct kobject)); |
117 memset(&domain->kobj, 0x00, sizeof(struct kobject)); |
118 kobject_init(&domain->kobj); |
118 kobject_init(&domain->kobj); |
119 domain->kobj.ktype = &ktype_ec_domain; |
119 domain->kobj.ktype = &ktype_ec_domain; |
120 domain->kobj.parent = &master->kobj; |
120 domain->kobj.parent = &master->kobj; |
121 if (kobject_set_name(&domain->kobj, "domain%i", index)) { |
121 if (kobject_set_name(&domain->kobj, "domain%u", index)) { |
122 EC_ERR("Failed to set kobj name.\n"); |
122 EC_ERR("Failed to set kobj name.\n"); |
123 kobject_put(&domain->kobj); |
123 kobject_put(&domain->kobj); |
124 return -1; |
124 return -1; |
125 } |
125 } |
126 if (kobject_add(&domain->kobj)) { |
126 if (kobject_add(&domain->kobj)) { |
226 data_reg->slave = sync->slave; |
226 data_reg->slave = sync->slave; |
227 data_reg->sync = sync; |
227 data_reg->sync = sync; |
228 data_reg->sync_offset = byte_offset; |
228 data_reg->sync_offset = byte_offset; |
229 data_reg->data_ptr = data_ptr; |
229 data_reg->data_ptr = data_ptr; |
230 list_add_tail(&data_reg->list, &domain->data_regs); |
230 list_add_tail(&data_reg->list, &domain->data_regs); |
231 |
|
232 return 0; |
231 return 0; |
233 } |
232 } |
234 |
233 |
235 /*****************************************************************************/ |
234 /*****************************************************************************/ |
236 |
235 |
335 datagram_data_size)) |
334 datagram_data_size)) |
336 return -1; |
335 return -1; |
337 datagram_count++; |
336 datagram_count++; |
338 } |
337 } |
339 |
338 |
340 if (!datagram_count) { |
339 if (datagram_count) { |
341 EC_WARN("Domain %i contains no data!\n", domain->index); |
340 // set all process data pointers |
342 ec_domain_clear_data_regs(domain); |
341 list_for_each_entry(data_reg, &domain->data_regs, list) { |
343 return 0; |
342 for (i = 0; i < data_reg->slave->fmmu_count; i++) { |
344 } |
343 fmmu = &data_reg->slave->fmmus[i]; |
345 |
344 if (fmmu->domain == domain && fmmu->sync == data_reg->sync) { |
346 // set all process data pointers |
345 pdo_off = |
347 list_for_each_entry(data_reg, &domain->data_regs, list) { |
346 fmmu->logical_start_address + data_reg->sync_offset; |
348 for (i = 0; i < data_reg->slave->fmmu_count; i++) { |
347 // search datagram |
349 fmmu = &data_reg->slave->fmmus[i]; |
348 list_for_each_entry(datagram, &domain->datagrams, list) { |
350 if (fmmu->domain == domain && fmmu->sync == data_reg->sync) { |
349 log_addr = EC_READ_U32(datagram->address); |
351 pdo_off = fmmu->logical_start_address + data_reg->sync_offset; |
350 pdo_off_datagram = pdo_off - log_addr; |
352 // search datagram |
351 if (pdo_off >= log_addr && |
353 list_for_each_entry(datagram, &domain->datagrams, list) { |
352 pdo_off_datagram < datagram->mem_size) { |
354 log_addr = EC_READ_U32(datagram->address); |
353 *data_reg->data_ptr = datagram->data + |
355 pdo_off_datagram = pdo_off - log_addr; |
354 pdo_off_datagram; |
356 if (pdo_off >= log_addr && |
355 } |
357 pdo_off_datagram < datagram->mem_size) { |
|
358 *data_reg->data_ptr = datagram->data + |
|
359 pdo_off_datagram; |
|
360 } |
356 } |
|
357 if (!data_reg->data_ptr) { |
|
358 EC_ERR("Failed to assign data pointer!\n"); |
|
359 return -1; |
|
360 } |
|
361 break; |
361 } |
362 } |
362 if (!data_reg->data_ptr) { |
|
363 EC_ERR("Failed to assign data pointer!\n"); |
|
364 return -1; |
|
365 } |
|
366 break; |
|
367 } |
363 } |
368 } |
364 } |
369 } |
365 |
370 |
366 EC_INFO("Domain %u - Allocated %u bytes in %u datagram%s.\n", |
371 EC_INFO("Domain %i - Allocated %i bytes in %i datagram%s.\n", |
367 domain->index, domain->data_size, datagram_count, |
372 domain->index, domain->data_size, datagram_count, |
368 datagram_count == 1 ? "" : "s"); |
373 datagram_count == 1 ? "" : "s"); |
369 } else { // !datagram_count |
|
370 EC_WARN("Domain %u contains no data!\n", domain->index); |
|
371 } |
374 |
372 |
375 ec_domain_clear_data_regs(domain); |
373 ec_domain_clear_data_regs(domain); |
376 |
|
377 return 0; |
374 return 0; |
378 } |
375 } |
379 |
376 |
380 /*****************************************************************************/ |
377 /*****************************************************************************/ |
381 |
378 |
426 sync = &slave->sii_syncs[i]; |
423 sync = &slave->sii_syncs[i]; |
427 list_for_each_entry(pdo, &sync->pdos, list) { |
424 list_for_each_entry(pdo, &sync->pdos, list) { |
428 list_for_each_entry(entry, &pdo->entries, list) { |
425 list_for_each_entry(entry, &pdo->entries, list) { |
429 if (entry->index != pdo_entry_index || |
426 if (entry->index != pdo_entry_index || |
430 entry->subindex != pdo_entry_subindex) continue; |
427 entry->subindex != pdo_entry_subindex) continue; |
431 |
|
432 // PDO entry found |
428 // PDO entry found |
433 if (ec_domain_reg_pdo_entry(domain, sync, entry, data_ptr)) { |
429 if (ec_domain_reg_pdo_entry(domain, sync, entry, data_ptr)) { |
434 return -1; |
430 return -1; |
435 } |
431 } |
436 |
|
437 return 0; |
432 return 0; |
438 } |
433 } |
439 } |
434 } |
440 } |
435 } |
441 |
436 |
522 // estimate sync manager length |
517 // estimate sync manager length |
523 sync_length = offset + length; |
518 sync_length = offset + length; |
524 if (sync->est_length < sync_length) { |
519 if (sync->est_length < sync_length) { |
525 sync->est_length = sync_length; |
520 sync->est_length = sync_length; |
526 if (domain->master->debug_level) { |
521 if (domain->master->debug_level) { |
527 EC_DBG("Estimating length of sync manager %i of slave %i to %i.\n", |
522 EC_DBG("Estimating length of sync manager %u of slave %u to %u.\n", |
528 sync->index, slave->ring_position, sync_length); |
523 sync->index, slave->ring_position, sync_length); |
529 } |
524 } |
530 } |
525 } |
531 |
526 |
532 list_add_tail(&data_reg->list, &domain->data_regs); |
527 list_add_tail(&data_reg->list, &domain->data_regs); |
564 |
559 |
565 if (domain->working_counter_changes && |
560 if (domain->working_counter_changes && |
566 jiffies - domain->notify_jiffies > HZ) { |
561 jiffies - domain->notify_jiffies > HZ) { |
567 domain->notify_jiffies = jiffies; |
562 domain->notify_jiffies = jiffies; |
568 if (domain->working_counter_changes == 1) { |
563 if (domain->working_counter_changes == 1) { |
569 EC_INFO("Domain %i working counter change: %i\n", domain->index, |
564 EC_INFO("Domain %u working counter change: %u\n", domain->index, |
570 domain->response_count); |
565 domain->response_count); |
571 } |
566 } |
572 else { |
567 else { |
573 EC_INFO("Domain %i: %u WC changes. Current response count: %i\n", |
568 EC_INFO("Domain %u: %u WC changes. Current response count: %u\n", |
574 domain->index, domain->working_counter_changes, |
569 domain->index, domain->working_counter_changes, |
575 domain->response_count); |
570 domain->response_count); |
576 } |
571 } |
577 domain->working_counter_changes = 0; |
572 domain->working_counter_changes = 0; |
578 } |
573 } |