612 || jiffies - slave->jiffies_preop < 5 * HZ |
612 || jiffies - slave->jiffies_preop < 5 * HZ |
613 || !slave->online |
613 || !slave->online |
614 || slave->error_flag) continue; |
614 || slave->error_flag) continue; |
615 |
615 |
616 if (master->debug_level) { |
616 if (master->debug_level) { |
617 EC_DBG("Fetching SDO dictionary of slave %i.\n", |
617 EC_DBG("Fetching SDO dictionary from slave %i.\n", |
618 slave->ring_position); |
618 slave->ring_position); |
619 } |
619 } |
620 |
620 |
621 if (kobject_add(&slave->sdo_kobj)) { |
621 if (kobject_add(&slave->sdo_kobj)) { |
622 EC_ERR("Failed to add SDO kobj of slave %i.\n", |
622 EC_ERR("Failed to add SDO kobj of slave %i.\n", |
1050 if (fsm->coe_state != ec_fsm_end) return; |
1050 if (fsm->coe_state != ec_fsm_end) return; |
1051 |
1051 |
1052 // SDO dictionary fetching finished |
1052 // SDO dictionary fetching finished |
1053 |
1053 |
1054 if (master->debug_level) { |
1054 if (master->debug_level) { |
1055 EC_DBG("Finished fetching SDO dictionary of slave %i.\n", |
1055 unsigned int sdo_count, entry_count; |
1056 slave->ring_position); |
1056 ec_slave_sdo_dict_info(slave, &sdo_count, &entry_count); |
|
1057 EC_DBG("Fetched %i SDOs and %i entries from slave %i.\n", |
|
1058 sdo_count, entry_count, slave->ring_position); |
1057 } |
1059 } |
1058 |
1060 |
1059 // restart master state machine. |
1061 // restart master state machine. |
1060 fsm->master_state = ec_fsm_master_start; |
1062 fsm->master_state = ec_fsm_master_start; |
1061 fsm->master_state(fsm); // execute immediately |
1063 fsm->master_state(fsm); // execute immediately |
2318 CoE state: DICT RESPONSE. |
2320 CoE state: DICT RESPONSE. |
2319 */ |
2321 */ |
2320 |
2322 |
2321 void ec_fsm_coe_dict_response(ec_fsm_t *fsm /**< finite state machine */) |
2323 void ec_fsm_coe_dict_response(ec_fsm_t *fsm /**< finite state machine */) |
2322 { |
2324 { |
2323 ec_master_t *master = fsm->master; |
|
2324 ec_datagram_t *datagram = &fsm->datagram; |
2325 ec_datagram_t *datagram = &fsm->datagram; |
2325 ec_slave_t *slave = fsm->slave; |
2326 ec_slave_t *slave = fsm->slave; |
2326 uint8_t *data, mbox_prot; |
2327 uint8_t *data, mbox_prot; |
2327 size_t rec_size; |
2328 size_t rec_size; |
2328 unsigned int sdo_count, i; |
2329 unsigned int sdo_count, i; |
2373 return; |
2374 return; |
2374 } |
2375 } |
2375 |
2376 |
2376 sdo_count = (rec_size - 8) / 2; |
2377 sdo_count = (rec_size - 8) / 2; |
2377 |
2378 |
2378 if (master->debug_level) { |
|
2379 EC_DBG("Fetching %i SDOs.\n", sdo_count); |
|
2380 } |
|
2381 |
|
2382 for (i = 0; i < sdo_count; i++) { |
2379 for (i = 0; i < sdo_count; i++) { |
2383 sdo_index = EC_READ_U16(data + 8 + i * 2); |
2380 sdo_index = EC_READ_U16(data + 8 + i * 2); |
2384 if (!sdo_index) { |
2381 if (!sdo_index) { |
2385 // sometimes index is 0... ??? |
2382 EC_WARN("SDO dictionary of slave %i contains index 0x0000.\n", |
2386 if (master->debug_level) { |
2383 slave->ring_position); |
2387 EC_DBG("Received SDO with index 0x0000.\n"); |
|
2388 } |
|
2389 continue; |
2384 continue; |
2390 } |
2385 } |
2391 |
2386 |
2392 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_ATOMIC))) { |
2387 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_ATOMIC))) { |
2393 EC_ERR("Failed to allocate memory for SDO!\n"); |
2388 EC_ERR("Failed to allocate memory for SDO!\n"); |
2455 ec_slave_t *slave = fsm->slave; |
2450 ec_slave_t *slave = fsm->slave; |
2456 |
2451 |
2457 if (datagram->state != EC_DATAGRAM_RECEIVED |
2452 if (datagram->state != EC_DATAGRAM_RECEIVED |
2458 || datagram->working_counter != 1) { |
2453 || datagram->working_counter != 1) { |
2459 fsm->coe_state = ec_fsm_error; |
2454 fsm->coe_state = ec_fsm_error; |
2460 EC_ERR("Reception of CoE SDO description request failed.\n"); |
2455 EC_ERR("Reception of CoE SDO description" |
|
2456 " request failed on slave %i.\n", slave->ring_position); |
|
2457 |
2461 return; |
2458 return; |
2462 } |
2459 } |
2463 |
2460 |
2464 fsm->coe_start = datagram->cycles_sent; |
2461 fsm->coe_start = datagram->cycles_sent; |
2465 |
2462 |
2480 ec_slave_t *slave = fsm->slave; |
2477 ec_slave_t *slave = fsm->slave; |
2481 |
2478 |
2482 if (datagram->state != EC_DATAGRAM_RECEIVED |
2479 if (datagram->state != EC_DATAGRAM_RECEIVED |
2483 || datagram->working_counter != 1) { |
2480 || datagram->working_counter != 1) { |
2484 fsm->coe_state = ec_fsm_error; |
2481 fsm->coe_state = ec_fsm_error; |
2485 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
2482 EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n", |
|
2483 slave->ring_position); |
|
2484 |
2486 return; |
2485 return; |
2487 } |
2486 } |
2488 |
2487 |
2489 if (!ec_slave_mbox_check(datagram)) { |
2488 if (!ec_slave_mbox_check(datagram)) { |
2490 if (datagram->cycles_received |
2489 if (datagram->cycles_received |
2522 size_t rec_size, name_size; |
2521 size_t rec_size, name_size; |
2523 |
2522 |
2524 if (datagram->state != EC_DATAGRAM_RECEIVED |
2523 if (datagram->state != EC_DATAGRAM_RECEIVED |
2525 || datagram->working_counter != 1) { |
2524 || datagram->working_counter != 1) { |
2526 fsm->coe_state = ec_fsm_error; |
2525 fsm->coe_state = ec_fsm_error; |
2527 EC_ERR("Reception of CoE SDO description response failed.\n"); |
2526 EC_ERR("Reception of CoE SDO description" |
|
2527 "response failed on slave %i.\n", slave->ring_position); |
2528 return; |
2528 return; |
2529 } |
2529 } |
2530 |
2530 |
2531 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
2531 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
2532 &mbox_prot, &rec_size))) { |
2532 &mbox_prot, &rec_size))) { |
2649 ec_slave_t *slave = fsm->slave; |
2649 ec_slave_t *slave = fsm->slave; |
2650 |
2650 |
2651 if (datagram->state != EC_DATAGRAM_RECEIVED |
2651 if (datagram->state != EC_DATAGRAM_RECEIVED |
2652 || datagram->working_counter != 1) { |
2652 || datagram->working_counter != 1) { |
2653 fsm->coe_state = ec_fsm_error; |
2653 fsm->coe_state = ec_fsm_error; |
2654 EC_ERR("Reception of CoE SDO entry request failed.\n"); |
2654 EC_ERR("Reception of CoE SDO entry request failed on slave %i.\n", |
|
2655 slave->ring_position); |
2655 return; |
2656 return; |
2656 } |
2657 } |
2657 |
2658 |
2658 fsm->coe_start = datagram->cycles_sent; |
2659 fsm->coe_start = datagram->cycles_sent; |
2659 |
2660 |
2674 ec_slave_t *slave = fsm->slave; |
2675 ec_slave_t *slave = fsm->slave; |
2675 |
2676 |
2676 if (datagram->state != EC_DATAGRAM_RECEIVED |
2677 if (datagram->state != EC_DATAGRAM_RECEIVED |
2677 || datagram->working_counter != 1) { |
2678 || datagram->working_counter != 1) { |
2678 fsm->coe_state = ec_fsm_error; |
2679 fsm->coe_state = ec_fsm_error; |
2679 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
2680 EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n", |
|
2681 slave->ring_position); |
2680 return; |
2682 return; |
2681 } |
2683 } |
2682 |
2684 |
2683 if (!ec_slave_mbox_check(datagram)) { |
2685 if (!ec_slave_mbox_check(datagram)) { |
2684 if (datagram->cycles_received |
2686 if (datagram->cycles_received |
2706 CoE state: DICT ENTRY RESPONSE. |
2708 CoE state: DICT ENTRY RESPONSE. |
2707 */ |
2709 */ |
2708 |
2710 |
2709 void ec_fsm_coe_dict_entry_response(ec_fsm_t *fsm /**< finite state machine */) |
2711 void ec_fsm_coe_dict_entry_response(ec_fsm_t *fsm /**< finite state machine */) |
2710 { |
2712 { |
2711 ec_master_t *master = fsm->master; |
|
2712 ec_datagram_t *datagram = &fsm->datagram; |
2713 ec_datagram_t *datagram = &fsm->datagram; |
2713 ec_slave_t *slave = fsm->slave; |
2714 ec_slave_t *slave = fsm->slave; |
2714 ec_sdo_t *sdo = fsm->coe_sdo; |
2715 ec_sdo_t *sdo = fsm->coe_sdo; |
2715 uint8_t *data, mbox_prot; |
2716 uint8_t *data, mbox_prot; |
2716 size_t rec_size, data_size; |
2717 size_t rec_size, data_size; |
2717 ec_sdo_entry_t *entry; |
2718 ec_sdo_entry_t *entry; |
2718 |
2719 |
2719 if (datagram->state != EC_DATAGRAM_RECEIVED |
2720 if (datagram->state != EC_DATAGRAM_RECEIVED |
2720 || datagram->working_counter != 1) { |
2721 || datagram->working_counter != 1) { |
2721 fsm->coe_state = ec_fsm_error; |
2722 fsm->coe_state = ec_fsm_error; |
2722 EC_ERR("Reception of CoE SDO description response failed.\n"); |
2723 EC_ERR("Reception of CoE SDO description" |
|
2724 " response failed on slave %i.\n", slave->ring_position); |
2723 return; |
2725 return; |
2724 } |
2726 } |
2725 |
2727 |
2726 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
2728 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
2727 &mbox_prot, &rec_size))) { |
2729 &mbox_prot, &rec_size))) { |
2838 EC_WRITE_U16(data + 6, fsm->coe_sdo->index); // SDO index |
2840 EC_WRITE_U16(data + 6, fsm->coe_sdo->index); // SDO index |
2839 |
2841 |
2840 ec_master_queue_datagram(fsm->master, datagram); |
2842 ec_master_queue_datagram(fsm->master, datagram); |
2841 fsm->coe_state = ec_fsm_coe_dict_desc_request; |
2843 fsm->coe_state = ec_fsm_coe_dict_desc_request; |
2842 return; |
2844 return; |
2843 } |
|
2844 |
|
2845 if (master->debug_level) { |
|
2846 EC_DBG("Finished fetching SDO descriptions.\n"); |
|
2847 } |
2845 } |
2848 |
2846 |
2849 fsm->coe_state = ec_fsm_end; |
2847 fsm->coe_state = ec_fsm_end; |
2850 } |
2848 } |
2851 |
2849 |