00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00035
00036
00037 #include <linux/module.h>
00038 #include <linux/delay.h>
00039
00040 #include "globals.h"
00041 #include "datagram.h"
00042 #include "master.h"
00043 #include "slave_config.h"
00044
00045 #include "slave.h"
00046
00047
00048
00049 extern const ec_code_msg_t al_status_messages[];
00050
00051
00052
00053 char *ec_slave_sii_string(ec_slave_t *, unsigned int);
00054
00055
00056
00062 void ec_slave_init(
00063 ec_slave_t *slave,
00064 ec_master_t *master,
00065 ec_device_index_t dev_idx,
00066 uint16_t ring_position,
00067 uint16_t station_address
00068 )
00069 {
00070 unsigned int i;
00071
00072 slave->master = master;
00073 slave->device_index = dev_idx;
00074 slave->ring_position = ring_position;
00075 slave->station_address = station_address;
00076 slave->effective_alias = 0x0000;
00077
00078 slave->config = NULL;
00079 slave->requested_state = EC_SLAVE_STATE_PREOP;
00080 slave->current_state = EC_SLAVE_STATE_UNKNOWN;
00081 slave->error_flag = 0;
00082 slave->force_config = 0;
00083 slave->configured_rx_mailbox_offset = 0x0000;
00084 slave->configured_rx_mailbox_size = 0x0000;
00085 slave->configured_tx_mailbox_offset = 0x0000;
00086 slave->configured_tx_mailbox_size = 0x0000;
00087
00088 slave->base_type = 0;
00089 slave->base_revision = 0;
00090 slave->base_build = 0;
00091 slave->base_fmmu_count = 0;
00092 slave->base_sync_count = 0;
00093
00094 for (i = 0; i < EC_MAX_PORTS; i++) {
00095 slave->ports[i].desc = EC_PORT_NOT_IMPLEMENTED;
00096
00097 slave->ports[i].link.link_up = 0;
00098 slave->ports[i].link.loop_closed = 0;
00099 slave->ports[i].link.signal_detected = 0;
00100 slave->sii.physical_layer[i] = 0xFF;
00101
00102 slave->ports[i].receive_time = 0U;
00103
00104 slave->ports[i].next_slave = NULL;
00105 slave->ports[i].delay_to_next_dc = 0U;
00106 }
00107
00108 slave->base_fmmu_bit_operation = 0;
00109 slave->base_dc_supported = 0;
00110 slave->base_dc_range = EC_DC_32;
00111 slave->has_dc_system_time = 0;
00112 slave->transmission_delay = 0U;
00113
00114 slave->sii_words = NULL;
00115 slave->sii_nwords = 0;
00116
00117 slave->sii.alias = 0x0000;
00118 slave->sii.vendor_id = 0x00000000;
00119 slave->sii.product_code = 0x00000000;
00120 slave->sii.revision_number = 0x00000000;
00121 slave->sii.serial_number = 0x00000000;
00122 slave->sii.boot_rx_mailbox_offset = 0x0000;
00123 slave->sii.boot_rx_mailbox_size = 0x0000;
00124 slave->sii.boot_tx_mailbox_offset = 0x0000;
00125 slave->sii.boot_tx_mailbox_size = 0x0000;
00126 slave->sii.std_rx_mailbox_offset = 0x0000;
00127 slave->sii.std_rx_mailbox_size = 0x0000;
00128 slave->sii.std_tx_mailbox_offset = 0x0000;
00129 slave->sii.std_tx_mailbox_size = 0x0000;
00130 slave->sii.mailbox_protocols = 0;
00131
00132 slave->sii.strings = NULL;
00133 slave->sii.string_count = 0;
00134
00135 slave->sii.has_general = 0;
00136 slave->sii.group = NULL;
00137 slave->sii.image = NULL;
00138 slave->sii.order = NULL;
00139 slave->sii.name = NULL;
00140 memset(&slave->sii.coe_details, 0x00, sizeof(ec_sii_coe_details_t));
00141 memset(&slave->sii.general_flags, 0x00, sizeof(ec_sii_general_flags_t));
00142 slave->sii.current_on_ebus = 0;
00143
00144 slave->sii.syncs = NULL;
00145 slave->sii.sync_count = 0;
00146
00147 INIT_LIST_HEAD(&slave->sii.pdos);
00148
00149 INIT_LIST_HEAD(&slave->sdo_dictionary);
00150
00151 slave->sdo_dictionary_fetched = 0;
00152 slave->jiffies_preop = 0;
00153
00154 INIT_LIST_HEAD(&slave->sdo_requests);
00155 INIT_LIST_HEAD(&slave->reg_requests);
00156 INIT_LIST_HEAD(&slave->foe_requests);
00157 INIT_LIST_HEAD(&slave->soe_requests);
00158
00159
00160 ec_fsm_slave_init(&slave->fsm, slave);
00161 }
00162
00163
00164
00170 void ec_slave_clear(ec_slave_t *slave )
00171 {
00172 ec_sdo_t *sdo, *next_sdo;
00173 unsigned int i;
00174 ec_pdo_t *pdo, *next_pdo;
00175
00176
00177
00178 while (!list_empty(&slave->sdo_requests)) {
00179 ec_sdo_request_t *request =
00180 list_entry(slave->sdo_requests.next, ec_sdo_request_t, list);
00181 list_del_init(&request->list);
00182 EC_SLAVE_WARN(slave, "Discarding SDO request,"
00183 " slave about to be deleted.\n");
00184 request->state = EC_INT_REQUEST_FAILURE;
00185 }
00186
00187 while (!list_empty(&slave->reg_requests)) {
00188 ec_reg_request_t *reg =
00189 list_entry(slave->reg_requests.next, ec_reg_request_t, list);
00190 list_del_init(®->list);
00191 EC_SLAVE_WARN(slave, "Discarding register request,"
00192 " slave about to be deleted.\n");
00193 reg->state = EC_INT_REQUEST_FAILURE;
00194 }
00195
00196 while (!list_empty(&slave->foe_requests)) {
00197 ec_foe_request_t *request =
00198 list_entry(slave->foe_requests.next, ec_foe_request_t, list);
00199 list_del_init(&request->list);
00200 EC_SLAVE_WARN(slave, "Discarding FoE request,"
00201 " slave about to be deleted.\n");
00202 request->state = EC_INT_REQUEST_FAILURE;
00203 }
00204
00205 while (!list_empty(&slave->soe_requests)) {
00206 ec_soe_request_t *request =
00207 list_entry(slave->soe_requests.next, ec_soe_request_t, list);
00208 list_del_init(&request->list);
00209 EC_SLAVE_WARN(slave, "Discarding SoE request,"
00210 " slave about to be deleted.\n");
00211 request->state = EC_INT_REQUEST_FAILURE;
00212 }
00213
00214 wake_up_all(&slave->master->request_queue);
00215
00216 if (slave->config) {
00217 ec_slave_config_detach(slave->config);
00218 }
00219
00220
00221 list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
00222 list_del(&sdo->list);
00223 ec_sdo_clear(sdo);
00224 kfree(sdo);
00225 }
00226
00227
00228 if (slave->sii.strings) {
00229 for (i = 0; i < slave->sii.string_count; i++)
00230 kfree(slave->sii.strings[i]);
00231 kfree(slave->sii.strings);
00232 }
00233
00234
00235 ec_slave_clear_sync_managers(slave);
00236
00237
00238 list_for_each_entry_safe(pdo, next_pdo, &slave->sii.pdos, list) {
00239 list_del(&pdo->list);
00240 ec_pdo_clear(pdo);
00241 kfree(pdo);
00242 }
00243
00244 if (slave->sii_words) {
00245 kfree(slave->sii_words);
00246 }
00247
00248 ec_fsm_slave_clear(&slave->fsm);
00249 }
00250
00251
00252
00255 void ec_slave_clear_sync_managers(ec_slave_t *slave )
00256 {
00257 unsigned int i;
00258
00259 if (slave->sii.syncs) {
00260 for (i = 0; i < slave->sii.sync_count; i++) {
00261 ec_sync_clear(&slave->sii.syncs[i]);
00262 }
00263 kfree(slave->sii.syncs);
00264 slave->sii.syncs = NULL;
00265 }
00266 }
00267
00268
00269
00274 void ec_slave_set_state(ec_slave_t *slave,
00275 ec_slave_state_t new_state
00276 )
00277 {
00278 if (new_state != slave->current_state) {
00279 if (slave->master->debug_level) {
00280 char old_state[EC_STATE_STRING_SIZE],
00281 cur_state[EC_STATE_STRING_SIZE];
00282 ec_state_string(slave->current_state, old_state, 0);
00283 ec_state_string(new_state, cur_state, 0);
00284 EC_SLAVE_DBG(slave, 0, "%s -> %s.\n", old_state, cur_state);
00285 }
00286 slave->current_state = new_state;
00287 }
00288 }
00289
00290
00291
00296 void ec_slave_request_state(ec_slave_t *slave,
00297 ec_slave_state_t state
00298 )
00299 {
00300 slave->requested_state = state;
00301 slave->error_flag = 0;
00302 }
00303
00304
00305
00312 int ec_slave_fetch_sii_strings(
00313 ec_slave_t *slave,
00314 const uint8_t *data,
00315 size_t data_size
00316 )
00317 {
00318 int i, err;
00319 size_t size;
00320 off_t offset;
00321
00322 slave->sii.string_count = data[0];
00323
00324 if (slave->sii.string_count) {
00325 if (!(slave->sii.strings =
00326 kmalloc(sizeof(char *) * slave->sii.string_count,
00327 GFP_KERNEL))) {
00328 EC_SLAVE_ERR(slave, "Failed to allocate string array memory.\n");
00329 err = -ENOMEM;
00330 goto out_zero;
00331 }
00332
00333 offset = 1;
00334 for (i = 0; i < slave->sii.string_count; i++) {
00335 size = data[offset];
00336
00337 if (!(slave->sii.strings[i] =
00338 kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) {
00339 EC_SLAVE_ERR(slave, "Failed to allocate string memory.\n");
00340 err = -ENOMEM;
00341 goto out_free;
00342 }
00343 memcpy(slave->sii.strings[i], data + offset + 1, size);
00344 slave->sii.strings[i][size] = 0x00;
00345 offset += 1 + size;
00346 }
00347 }
00348
00349 return 0;
00350
00351 out_free:
00352 for (i--; i >= 0; i--)
00353 kfree(slave->sii.strings[i]);
00354 kfree(slave->sii.strings);
00355 slave->sii.strings = NULL;
00356 out_zero:
00357 slave->sii.string_count = 0;
00358 return err;
00359 }
00360
00361
00362
00368 int ec_slave_fetch_sii_general(
00369 ec_slave_t *slave,
00370 const uint8_t *data,
00371 size_t data_size
00372 )
00373 {
00374 unsigned int i;
00375 uint8_t flags;
00376
00377 if (data_size != 32) {
00378 EC_SLAVE_ERR(slave, "Wrong size of general category (%zu/32).\n",
00379 data_size);
00380 return -EINVAL;
00381 }
00382
00383 slave->sii.group = ec_slave_sii_string(slave, data[0]);
00384 slave->sii.image = ec_slave_sii_string(slave, data[1]);
00385 slave->sii.order = ec_slave_sii_string(slave, data[2]);
00386 slave->sii.name = ec_slave_sii_string(slave, data[3]);
00387
00388 for (i = 0; i < 4; i++)
00389 slave->sii.physical_layer[i] =
00390 (data[4] & (0x03 << (i * 2))) >> (i * 2);
00391
00392
00393 flags = EC_READ_U8(data + 5);
00394 slave->sii.coe_details.enable_sdo = (flags >> 0) & 0x01;
00395 slave->sii.coe_details.enable_sdo_info = (flags >> 1) & 0x01;
00396 slave->sii.coe_details.enable_pdo_assign = (flags >> 2) & 0x01;
00397 slave->sii.coe_details.enable_pdo_configuration = (flags >> 3) & 0x01;
00398 slave->sii.coe_details.enable_upload_at_startup = (flags >> 4) & 0x01;
00399 slave->sii.coe_details.enable_sdo_complete_access = (flags >> 5) & 0x01;
00400
00401
00402 flags = EC_READ_U8(data + 0x000B);
00403 slave->sii.general_flags.enable_safeop = (flags >> 0) & 0x01;
00404 slave->sii.general_flags.enable_not_lrw = (flags >> 1) & 0x01;
00405
00406 slave->sii.current_on_ebus = EC_READ_S16(data + 0x0C);
00407 slave->sii.has_general = 1;
00408 return 0;
00409 }
00410
00411
00412
00419 int ec_slave_fetch_sii_syncs(
00420 ec_slave_t *slave,
00421 const uint8_t *data,
00422 size_t data_size
00423 )
00424 {
00425 unsigned int i, count, total_count;
00426 ec_sync_t *sync;
00427 size_t memsize;
00428 ec_sync_t *syncs;
00429 uint8_t index;
00430
00431
00432 if (data_size % 8) {
00433 EC_SLAVE_ERR(slave, "Invalid SII sync manager category size %zu.\n",
00434 data_size);
00435 return -EINVAL;
00436 }
00437
00438 count = data_size / 8;
00439
00440 if (count) {
00441 total_count = count + slave->sii.sync_count;
00442 if (total_count > EC_MAX_SYNC_MANAGERS) {
00443 EC_SLAVE_ERR(slave, "Exceeded maximum number of"
00444 " sync managers!\n");
00445 return -EOVERFLOW;
00446 }
00447 memsize = sizeof(ec_sync_t) * total_count;
00448 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) {
00449 EC_SLAVE_ERR(slave, "Failed to allocate %zu bytes"
00450 " for sync managers.\n", memsize);
00451 return -ENOMEM;
00452 }
00453
00454 for (i = 0; i < slave->sii.sync_count; i++)
00455 ec_sync_init_copy(syncs + i, slave->sii.syncs + i);
00456
00457
00458 for (i = 0; i < count; i++, data += 8) {
00459 index = i + slave->sii.sync_count;
00460 sync = &syncs[index];
00461
00462 ec_sync_init(sync, slave);
00463 sync->physical_start_address = EC_READ_U16(data);
00464 sync->default_length = EC_READ_U16(data + 2);
00465 sync->control_register = EC_READ_U8(data + 4);
00466 sync->enable = EC_READ_U8(data + 6);
00467 }
00468
00469 if (slave->sii.syncs)
00470 kfree(slave->sii.syncs);
00471 slave->sii.syncs = syncs;
00472 slave->sii.sync_count = total_count;
00473 }
00474
00475 return 0;
00476 }
00477
00478
00479
00485 int ec_slave_fetch_sii_pdos(
00486 ec_slave_t *slave,
00487 const uint8_t *data,
00488 size_t data_size,
00489 ec_direction_t dir
00490 )
00491 {
00492 int ret;
00493 ec_pdo_t *pdo;
00494 ec_pdo_entry_t *entry;
00495 unsigned int entry_count, i;
00496
00497 while (data_size >= 8) {
00498 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
00499 EC_SLAVE_ERR(slave, "Failed to allocate PDO memory.\n");
00500 return -ENOMEM;
00501 }
00502
00503 ec_pdo_init(pdo);
00504 pdo->index = EC_READ_U16(data);
00505 entry_count = EC_READ_U8(data + 2);
00506 pdo->sync_index = EC_READ_U8(data + 3);
00507 ret = ec_pdo_set_name(pdo,
00508 ec_slave_sii_string(slave, EC_READ_U8(data + 5)));
00509 if (ret) {
00510 ec_pdo_clear(pdo);
00511 kfree(pdo);
00512 return ret;
00513 }
00514 list_add_tail(&pdo->list, &slave->sii.pdos);
00515
00516 data_size -= 8;
00517 data += 8;
00518
00519 for (i = 0; i < entry_count; i++) {
00520 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
00521 EC_SLAVE_ERR(slave, "Failed to allocate PDO entry memory.\n");
00522 return -ENOMEM;
00523 }
00524
00525 ec_pdo_entry_init(entry);
00526 entry->index = EC_READ_U16(data);
00527 entry->subindex = EC_READ_U8(data + 2);
00528 ret = ec_pdo_entry_set_name(entry,
00529 ec_slave_sii_string(slave, EC_READ_U8(data + 3)));
00530 if (ret) {
00531 ec_pdo_entry_clear(entry);
00532 kfree(entry);
00533 return ret;
00534 }
00535 entry->bit_length = EC_READ_U8(data + 5);
00536 list_add_tail(&entry->list, &pdo->entries);
00537
00538 data_size -= 8;
00539 data += 8;
00540 }
00541
00542
00543 if (pdo->sync_index >= 0) {
00544 ec_sync_t *sync;
00545
00546 if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) {
00547 EC_SLAVE_ERR(slave, "Invalid SM index %i for PDO 0x%04X.",
00548 pdo->sync_index, pdo->index);
00549 return -ENOENT;
00550 }
00551
00552 ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo);
00553 if (ret)
00554 return ret;
00555 }
00556 }
00557
00558 return 0;
00559 }
00560
00561
00562
00568 char *ec_slave_sii_string(
00569 ec_slave_t *slave,
00570 unsigned int index
00571 )
00572 {
00573 if (!index--)
00574 return NULL;
00575
00576 if (index >= slave->sii.string_count) {
00577 EC_SLAVE_DBG(slave, 1, "String %u not found.\n", index);
00578 return NULL;
00579 }
00580
00581 return slave->sii.strings[index];
00582 }
00583
00584
00585
00590 ec_sync_t *ec_slave_get_sync(
00591 ec_slave_t *slave,
00592 uint8_t sync_index
00593 )
00594 {
00595 if (sync_index < slave->sii.sync_count) {
00596 return &slave->sii.syncs[sync_index];
00597 } else {
00598 return NULL;
00599 }
00600 }
00601
00602
00603
00608 void ec_slave_sdo_dict_info(const ec_slave_t *slave,
00609 unsigned int *sdo_count,
00610 unsigned int *entry_count
00612 )
00613 {
00614 unsigned int sdos = 0, entries = 0;
00615 ec_sdo_t *sdo;
00616 ec_sdo_entry_t *entry;
00617
00618 list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
00619 sdos++;
00620 list_for_each_entry(entry, &sdo->entries, list) {
00621 entries++;
00622 }
00623 }
00624
00625 *sdo_count = sdos;
00626 *entry_count = entries;
00627 }
00628
00629
00630
00636 ec_sdo_t *ec_slave_get_sdo(
00637 ec_slave_t *slave,
00638 uint16_t index
00639 )
00640 {
00641 ec_sdo_t *sdo;
00642
00643 list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
00644 if (sdo->index != index)
00645 continue;
00646 return sdo;
00647 }
00648
00649 return NULL;
00650 }
00651
00652
00653
00662 const ec_sdo_t *ec_slave_get_sdo_const(
00663 const ec_slave_t *slave,
00664 uint16_t index
00665 )
00666 {
00667 const ec_sdo_t *sdo;
00668
00669 list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
00670 if (sdo->index != index)
00671 continue;
00672 return sdo;
00673 }
00674
00675 return NULL;
00676 }
00677
00678
00679
00684 const ec_sdo_t *ec_slave_get_sdo_by_pos_const(
00685 const ec_slave_t *slave,
00686 uint16_t sdo_position
00687 )
00688 {
00689 const ec_sdo_t *sdo;
00690
00691 list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
00692 if (sdo_position--)
00693 continue;
00694 return sdo;
00695 }
00696
00697 return NULL;
00698 }
00699
00700
00701
00706 uint16_t ec_slave_sdo_count(
00707 const ec_slave_t *slave
00708 )
00709 {
00710 const ec_sdo_t *sdo;
00711 uint16_t count = 0;
00712
00713 list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
00714 count++;
00715 }
00716
00717 return count;
00718 }
00719
00720
00721
00725 const ec_pdo_t *ec_slave_find_pdo(
00726 const ec_slave_t *slave,
00727 uint16_t index
00728 )
00729 {
00730 unsigned int i;
00731 const ec_sync_t *sync;
00732 const ec_pdo_t *pdo;
00733
00734 for (i = 0; i < slave->sii.sync_count; i++) {
00735 sync = &slave->sii.syncs[i];
00736
00737 if (!(pdo = ec_pdo_list_find_pdo_const(&sync->pdos, index)))
00738 continue;
00739
00740 return pdo;
00741 }
00742
00743 return NULL;
00744 }
00745
00746
00747
00750 void ec_slave_find_names_for_pdo(
00751 ec_slave_t *slave,
00752 ec_pdo_t *pdo
00753 )
00754 {
00755 const ec_sdo_t *sdo;
00756 ec_pdo_entry_t *pdo_entry;
00757 const ec_sdo_entry_t *sdo_entry;
00758
00759 list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
00760 if (sdo->index == pdo->index) {
00761 ec_pdo_set_name(pdo, sdo->name);
00762 } else {
00763 list_for_each_entry(pdo_entry, &pdo->entries, list) {
00764 if (sdo->index == pdo_entry->index) {
00765 sdo_entry = ec_sdo_get_entry_const(
00766 sdo, pdo_entry->subindex);
00767 if (sdo_entry) {
00768 ec_pdo_entry_set_name(pdo_entry,
00769 sdo_entry->description);
00770 }
00771 }
00772 }
00773 }
00774 }
00775 }
00776
00777
00778
00781 void ec_slave_attach_pdo_names(
00782 ec_slave_t *slave
00783 )
00784 {
00785 unsigned int i;
00786 ec_sync_t *sync;
00787 ec_pdo_t *pdo;
00788
00789 for (i = 0; i < slave->sii.sync_count; i++) {
00790 sync = slave->sii.syncs + i;
00791 list_for_each_entry(pdo, &sync->pdos.list, list) {
00792 ec_slave_find_names_for_pdo(slave, pdo);
00793 }
00794 }
00795 }
00796
00797
00798
00803 unsigned int ec_slave_get_previous_port(
00804 ec_slave_t *slave,
00805 unsigned int port_index
00806 )
00807 {
00808 static const unsigned int prev_table[EC_MAX_PORTS] = {
00809 2, 3, 1, 0
00810 };
00811
00812 if (port_index >= EC_MAX_PORTS) {
00813 EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n",
00814 __func__, port_index);
00815 }
00816
00817 do {
00818 port_index = prev_table[port_index];
00819 if (slave->ports[port_index].next_slave) {
00820 return port_index;
00821 }
00822 } while (port_index);
00823
00824 return 0;
00825 }
00826
00827
00828
00833 unsigned int ec_slave_get_next_port(
00834 ec_slave_t *slave,
00835 unsigned int port_index
00836 )
00837 {
00838 static const unsigned int next_table[EC_MAX_PORTS] = {
00839 3, 2, 0, 1
00840 };
00841
00842 if (port_index >= EC_MAX_PORTS) {
00843 EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n",
00844 __func__, port_index);
00845 }
00846
00847 do {
00848 port_index = next_table[port_index];
00849 if (slave->ports[port_index].next_slave) {
00850 return port_index;
00851 }
00852 } while (port_index);
00853
00854 return 0;
00855 }
00856
00857
00858
00863 uint32_t ec_slave_calc_rtt_sum(
00864 ec_slave_t *slave
00865 )
00866 {
00867 uint32_t rtt_sum = 0, rtt;
00868 unsigned int port_index = ec_slave_get_next_port(slave, 0);
00869
00870 while (port_index != 0) {
00871 unsigned int prev_index =
00872 ec_slave_get_previous_port(slave, port_index);
00873
00874 rtt = slave->ports[port_index].receive_time -
00875 slave->ports[prev_index].receive_time;
00876 rtt_sum += rtt;
00877 port_index = ec_slave_get_next_port(slave, port_index);
00878 }
00879
00880 return rtt_sum;
00881 }
00882
00883
00884
00889 ec_slave_t *ec_slave_find_next_dc_slave(
00890 ec_slave_t *slave
00891 )
00892 {
00893 unsigned int port_index;
00894 ec_slave_t *dc_slave = NULL;
00895
00896 if (slave->base_dc_supported) {
00897 dc_slave = slave;
00898 } else {
00899 port_index = ec_slave_get_next_port(slave, 0);
00900
00901 while (port_index != 0) {
00902 ec_slave_t *next = slave->ports[port_index].next_slave;
00903
00904 if (next) {
00905 dc_slave = ec_slave_find_next_dc_slave(next);
00906
00907 if (dc_slave) {
00908 break;
00909 }
00910 }
00911 port_index = ec_slave_get_next_port(slave, port_index);
00912 }
00913 }
00914
00915 return dc_slave;
00916 }
00917
00918
00919
00922 void ec_slave_calc_port_delays(
00923 ec_slave_t *slave
00924 )
00925 {
00926 unsigned int port_index;
00927 ec_slave_t *next_slave, *next_dc;
00928 uint32_t rtt, next_rtt_sum;
00929
00930 if (!slave->base_dc_supported)
00931 return;
00932
00933 port_index = ec_slave_get_next_port(slave, 0);
00934
00935 while (port_index != 0) {
00936 next_slave = slave->ports[port_index].next_slave;
00937 next_dc = ec_slave_find_next_dc_slave(next_slave);
00938
00939 if (next_dc) {
00940 unsigned int prev_port =
00941 ec_slave_get_previous_port(slave, port_index);
00942
00943 rtt = slave->ports[port_index].receive_time -
00944 slave->ports[prev_port].receive_time;
00945 next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
00946
00947 slave->ports[port_index].delay_to_next_dc =
00948 (rtt - next_rtt_sum) / 2;
00949 next_dc->ports[0].delay_to_next_dc =
00950 (rtt - next_rtt_sum) / 2;
00951
00952 #if 0
00953 EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u"
00954 " next_rtt_sum=%u delay=%u\n",
00955 slave->ring_position, port_index, rtt, next_rtt_sum,
00956 slave->ports[port_index].delay_to_next_dc);
00957 #endif
00958 }
00959
00960 port_index = ec_slave_get_next_port(slave, port_index);
00961 }
00962 }
00963
00964
00965
00968 void ec_slave_calc_transmission_delays_rec(
00969 ec_slave_t *slave,
00970 uint32_t *delay
00971 )
00972 {
00973 unsigned int i;
00974 ec_slave_t *next_dc;
00975
00976 EC_SLAVE_DBG(slave, 1, "%s(delay = %u ns)\n", __func__, *delay);
00977
00978 slave->transmission_delay = *delay;
00979
00980 i = ec_slave_get_next_port(slave, 0);
00981
00982 while (i != 0) {
00983 ec_slave_port_t *port = &slave->ports[i];
00984 next_dc = ec_slave_find_next_dc_slave(port->next_slave);
00985 if (next_dc) {
00986 *delay = *delay + port->delay_to_next_dc;
00987 #if 0
00988 EC_SLAVE_DBG(slave, 1, "%u:%u %u\n",
00989 slave->ring_position, i, *delay);
00990 #endif
00991 ec_slave_calc_transmission_delays_rec(next_dc, delay);
00992 }
00993
00994 i = ec_slave_get_next_port(slave, i);
00995 }
00996
00997 *delay = *delay + slave->ports[0].delay_to_next_dc;
00998 }
00999
01000
