72 void ec_fsm_coe_error(ec_fsm_coe_t *); |
72 void ec_fsm_coe_error(ec_fsm_coe_t *); |
73 |
73 |
74 /*****************************************************************************/ |
74 /*****************************************************************************/ |
75 |
75 |
76 /** |
76 /** |
77 Sdo abort messages. |
77 SDO abort messages. |
78 The "abort Sdo transfer request" supplies an abort code, |
78 The "abort SDO transfer request" supplies an abort code, |
79 which can be translated to clear text. This table does |
79 which can be translated to clear text. This table does |
80 the mapping of the codes and messages. |
80 the mapping of the codes and messages. |
81 */ |
81 */ |
82 |
82 |
83 const ec_code_msg_t sdo_abort_messages[] = { |
83 const ec_code_msg_t sdo_abort_messages[] = { |
84 {0x05030000, "Toggle bit not changed"}, |
84 {0x05030000, "Toggle bit not changed"}, |
85 {0x05040000, "Sdo protocol timeout"}, |
85 {0x05040000, "SDO protocol timeout"}, |
86 {0x05040001, "Client/Server command specifier not valid or unknown"}, |
86 {0x05040001, "Client/Server command specifier not valid or unknown"}, |
87 {0x05040005, "Out of memory"}, |
87 {0x05040005, "Out of memory"}, |
88 {0x06010000, "Unsupported access to an object"}, |
88 {0x06010000, "Unsupported access to an object"}, |
89 {0x06010001, "Attempt to read a write-only object"}, |
89 {0x06010001, "Attempt to read a write-only object"}, |
90 {0x06010002, "Attempt to write a read-only object"}, |
90 {0x06010002, "Attempt to write a read-only object"}, |
91 {0x06020000, "This object does not exist in the object directory"}, |
91 {0x06020000, "This object does not exist in the object directory"}, |
92 {0x06040041, "The object cannot be mapped into the Pdo"}, |
92 {0x06040041, "The object cannot be mapped into the PDO"}, |
93 {0x06040042, "The number and length of the objects to be mapped would" |
93 {0x06040042, "The number and length of the objects to be mapped would" |
94 " exceed the Pdo length"}, |
94 " exceed the PDO length"}, |
95 {0x06040043, "General parameter incompatibility reason"}, |
95 {0x06040043, "General parameter incompatibility reason"}, |
96 {0x06040047, "Gerneral internal incompatibility in device"}, |
96 {0x06040047, "Gerneral internal incompatibility in device"}, |
97 {0x06060000, "Access failure due to a hardware error"}, |
97 {0x06060000, "Access failure due to a hardware error"}, |
98 {0x06070010, "Data type does not match, length of service parameter does" |
98 {0x06070010, "Data type does not match, length of service parameter does" |
99 " not match"}, |
99 " not match"}, |
118 }; |
118 }; |
119 |
119 |
120 /*****************************************************************************/ |
120 /*****************************************************************************/ |
121 |
121 |
122 /** |
122 /** |
123 Outputs an Sdo abort message. |
123 Outputs an SDO abort message. |
124 */ |
124 */ |
125 |
125 |
126 void ec_canopen_abort_msg(uint32_t abort_code) |
126 void ec_canopen_abort_msg(uint32_t abort_code) |
127 { |
127 { |
128 const ec_code_msg_t *abort_msg; |
128 const ec_code_msg_t *abort_msg; |
129 |
129 |
130 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
130 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
131 if (abort_msg->code == abort_code) { |
131 if (abort_msg->code == abort_code) { |
132 EC_ERR("Sdo abort message 0x%08X: \"%s\".\n", |
132 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
133 abort_msg->code, abort_msg->message); |
133 abort_msg->code, abort_msg->message); |
134 return; |
134 return; |
135 } |
135 } |
136 } |
136 } |
137 |
137 |
138 EC_ERR("Unknown Sdo abort code 0x%08X.\n", abort_code); |
138 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
139 } |
139 } |
140 |
140 |
141 /*****************************************************************************/ |
141 /*****************************************************************************/ |
142 |
142 |
143 /** |
143 /** |
177 } |
177 } |
178 |
178 |
179 /*****************************************************************************/ |
179 /*****************************************************************************/ |
180 |
180 |
181 /** |
181 /** |
182 Starts to transfer an Sdo to/from a slave. |
182 Starts to transfer an SDO to/from a slave. |
183 */ |
183 */ |
184 |
184 |
185 void ec_fsm_coe_transfer( |
185 void ec_fsm_coe_transfer( |
186 ec_fsm_coe_t *fsm, /**< State machine. */ |
186 ec_fsm_coe_t *fsm, /**< State machine. */ |
187 ec_slave_t *slave, /**< EtherCAT slave. */ |
187 ec_slave_t *slave, /**< EtherCAT slave. */ |
188 ec_sdo_request_t *request /**< Sdo request. */ |
188 ec_sdo_request_t *request /**< SDO request. */ |
189 ) |
189 ) |
190 { |
190 { |
191 fsm->slave = slave; |
191 fsm->slave = slave; |
192 fsm->request = request; |
192 fsm->request = request; |
193 if (request->dir == EC_DIR_OUTPUT) |
193 if (request->dir == EC_DIR_OUTPUT) |
273 fsm->state = ec_fsm_coe_error; |
273 fsm->state = ec_fsm_coe_error; |
274 return; |
274 return; |
275 } |
275 } |
276 |
276 |
277 if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) { |
277 if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) { |
278 EC_ERR("Slave %u does not support Sdo information service!\n", |
278 EC_ERR("Slave %u does not support SDO information service!\n", |
279 slave->ring_position); |
279 slave->ring_position); |
280 fsm->state = ec_fsm_coe_error; |
280 fsm->state = ec_fsm_coe_error; |
281 return; |
281 return; |
282 } |
282 } |
283 |
283 |
284 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
284 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
285 fsm->state = ec_fsm_coe_error; |
285 fsm->state = ec_fsm_coe_error; |
286 return; |
286 return; |
287 } |
287 } |
288 |
288 |
289 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
289 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
290 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
290 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
291 EC_WRITE_U8 (data + 3, 0x00); |
291 EC_WRITE_U8 (data + 3, 0x00); |
292 EC_WRITE_U16(data + 4, 0x0000); |
292 EC_WRITE_U16(data + 4, 0x0000); |
293 EC_WRITE_U16(data + 6, 0x0001); // deliver all Sdos! |
293 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
294 |
294 |
295 fsm->retries = EC_FSM_RETRIES; |
295 fsm->retries = EC_FSM_RETRIES; |
296 fsm->state = ec_fsm_coe_dict_request; |
296 fsm->state = ec_fsm_coe_dict_request; |
297 } |
297 } |
298 |
298 |
367 if (!ec_slave_mbox_check(datagram)) { |
367 if (!ec_slave_mbox_check(datagram)) { |
368 unsigned long diff_ms = |
368 unsigned long diff_ms = |
369 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
369 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
370 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
370 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
371 fsm->state = ec_fsm_coe_error; |
371 fsm->state = ec_fsm_coe_error; |
372 EC_ERR("Timeout while waiting for Sdo dictionary list response " |
372 EC_ERR("Timeout while waiting for SDO dictionary list response " |
373 "on slave %u.\n", slave->ring_position); |
373 "on slave %u.\n", slave->ring_position); |
374 return; |
374 return; |
375 } |
375 } |
376 |
376 |
377 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
377 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
440 fsm->state = ec_fsm_coe_dict_check; |
440 fsm->state = ec_fsm_coe_dict_check; |
441 return; |
441 return; |
442 } |
442 } |
443 |
443 |
444 if (rec_size < 3) { |
444 if (rec_size < 3) { |
445 EC_ERR("Received corrupted Sdo dictionary response (size %u).\n", |
445 EC_ERR("Received corrupted SDO dictionary response (size %u).\n", |
446 rec_size); |
446 rec_size); |
447 fsm->state = ec_fsm_coe_error; |
447 fsm->state = ec_fsm_coe_error; |
448 return; |
448 return; |
449 } |
449 } |
450 |
450 |
451 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
451 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
452 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
452 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
453 EC_ERR("Sdo information error response at slave %u!\n", |
453 EC_ERR("SDO information error response at slave %u!\n", |
454 slave->ring_position); |
454 slave->ring_position); |
455 if (rec_size < 10) { |
455 if (rec_size < 10) { |
456 EC_ERR("Incomplete Sdo information error response:\n"); |
456 EC_ERR("Incomplete SDO information error response:\n"); |
457 ec_print_data(data, rec_size); |
457 ec_print_data(data, rec_size); |
458 } else { |
458 } else { |
459 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
459 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
460 } |
460 } |
461 fsm->state = ec_fsm_coe_error; |
461 fsm->state = ec_fsm_coe_error; |
462 return; |
462 return; |
463 } |
463 } |
464 |
464 |
465 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
465 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
466 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
466 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
467 if (fsm->slave->master->debug_level) { |
467 if (fsm->slave->master->debug_level) { |
468 EC_DBG("Invalid Sdo list response at slave %u! Retrying...\n", |
468 EC_DBG("Invalid SDO list response at slave %u! Retrying...\n", |
469 slave->ring_position); |
469 slave->ring_position); |
470 ec_print_data(data, rec_size); |
470 ec_print_data(data, rec_size); |
471 } |
471 } |
472 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
472 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
473 fsm->retries = EC_FSM_RETRIES; |
473 fsm->retries = EC_FSM_RETRIES; |
486 |
486 |
487 for (i = 0; i < sdo_count; i++) { |
487 for (i = 0; i < sdo_count; i++) { |
488 sdo_index = EC_READ_U16(data + 8 + i * 2); |
488 sdo_index = EC_READ_U16(data + 8 + i * 2); |
489 if (!sdo_index) { |
489 if (!sdo_index) { |
490 if (slave->master->debug_level) |
490 if (slave->master->debug_level) |
491 EC_WARN("Sdo dictionary of slave %u contains index 0x0000.\n", |
491 EC_WARN("SDO dictionary of slave %u contains index 0x0000.\n", |
492 slave->ring_position); |
492 slave->ring_position); |
493 continue; |
493 continue; |
494 } |
494 } |
495 |
495 |
496 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) { |
496 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) { |
497 EC_ERR("Failed to allocate memory for Sdo!\n"); |
497 EC_ERR("Failed to allocate memory for SDO!\n"); |
498 fsm->state = ec_fsm_coe_error; |
498 fsm->state = ec_fsm_coe_error; |
499 return; |
499 return; |
500 } |
500 } |
501 |
501 |
502 ec_sdo_init(sdo, slave, sdo_index); |
502 ec_sdo_init(sdo, slave, sdo_index); |
503 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
503 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
504 } |
504 } |
505 |
505 |
506 fragments_left = EC_READ_U16(data + 4); |
506 fragments_left = EC_READ_U16(data + 4); |
507 if (slave->master->debug_level && fragments_left) { |
507 if (slave->master->debug_level && fragments_left) { |
508 EC_DBG("Sdo list fragments left: %u\n", fragments_left); |
508 EC_DBG("SDO list fragments left: %u\n", fragments_left); |
509 } |
509 } |
510 |
510 |
511 if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again. |
511 if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again. |
512 fsm->jiffies_start = datagram->jiffies_sent; |
512 fsm->jiffies_start = datagram->jiffies_sent; |
513 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
513 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
515 fsm->state = ec_fsm_coe_dict_check; |
515 fsm->state = ec_fsm_coe_dict_check; |
516 return; |
516 return; |
517 } |
517 } |
518 |
518 |
519 if (list_empty(&slave->sdo_dictionary)) { |
519 if (list_empty(&slave->sdo_dictionary)) { |
520 // no Sdos in dictionary. finished. |
520 // no SDOs in dictionary. finished. |
521 fsm->state = ec_fsm_coe_end; // success |
521 fsm->state = ec_fsm_coe_end; // success |
522 return; |
522 return; |
523 } |
523 } |
524 |
524 |
525 // fetch Sdo descriptions |
525 // fetch SDO descriptions |
526 fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list); |
526 fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list); |
527 |
527 |
528 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
528 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
529 fsm->state = ec_fsm_coe_error; |
529 fsm->state = ec_fsm_coe_error; |
530 return; |
530 return; |
531 } |
531 } |
532 |
532 |
533 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
533 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
534 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
534 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
535 EC_WRITE_U8 (data + 3, 0x00); |
535 EC_WRITE_U8 (data + 3, 0x00); |
536 EC_WRITE_U16(data + 4, 0x0000); |
536 EC_WRITE_U16(data + 4, 0x0000); |
537 EC_WRITE_U16(data + 6, fsm->sdo->index); // Sdo index |
537 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
538 |
538 |
539 fsm->retries = EC_FSM_RETRIES; |
539 fsm->retries = EC_FSM_RETRIES; |
540 fsm->state = ec_fsm_coe_dict_desc_request; |
540 fsm->state = ec_fsm_coe_dict_desc_request; |
541 } |
541 } |
542 |
542 |
555 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
555 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
556 return; // FIXME: check for response first? |
556 return; // FIXME: check for response first? |
557 |
557 |
558 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
558 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
559 fsm->state = ec_fsm_coe_error; |
559 fsm->state = ec_fsm_coe_error; |
560 EC_ERR("Failed to receive CoE Sdo description request datagram for" |
560 EC_ERR("Failed to receive CoE SDO description request datagram for" |
561 " slave %u (datagram state %u).\n", |
561 " slave %u (datagram state %u).\n", |
562 slave->ring_position, datagram->state); |
562 slave->ring_position, datagram->state); |
563 return; |
563 return; |
564 } |
564 } |
565 |
565 |
566 if (datagram->working_counter != 1) { |
566 if (datagram->working_counter != 1) { |
567 fsm->state = ec_fsm_coe_error; |
567 fsm->state = ec_fsm_coe_error; |
568 EC_ERR("Reception of CoE Sdo description" |
568 EC_ERR("Reception of CoE SDO description" |
569 " request failed on slave %u: ", slave->ring_position); |
569 " request failed on slave %u: ", slave->ring_position); |
570 ec_datagram_print_wc_error(datagram); |
570 ec_datagram_print_wc_error(datagram); |
571 return; |
571 return; |
572 } |
572 } |
573 |
573 |
611 if (!ec_slave_mbox_check(datagram)) { |
611 if (!ec_slave_mbox_check(datagram)) { |
612 unsigned long diff_ms = |
612 unsigned long diff_ms = |
613 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
613 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
614 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
614 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
615 fsm->state = ec_fsm_coe_error; |
615 fsm->state = ec_fsm_coe_error; |
616 EC_ERR("Timeout while waiting for Sdo object description " |
616 EC_ERR("Timeout while waiting for SDO object description " |
617 "response on slave %u.\n", slave->ring_position); |
617 "response on slave %u.\n", slave->ring_position); |
618 return; |
618 return; |
619 } |
619 } |
620 |
620 |
621 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
621 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
648 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
648 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
649 return; // FIXME: request again? |
649 return; // FIXME: request again? |
650 |
650 |
651 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
651 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
652 fsm->state = ec_fsm_coe_error; |
652 fsm->state = ec_fsm_coe_error; |
653 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
653 EC_ERR("Failed to receive CoE SDO description response datagram from" |
654 " slave %u (datagram state %u).\n", |
654 " slave %u (datagram state %u).\n", |
655 slave->ring_position, datagram->state); |
655 slave->ring_position, datagram->state); |
656 return; |
656 return; |
657 } |
657 } |
658 |
658 |
659 if (datagram->working_counter != 1) { |
659 if (datagram->working_counter != 1) { |
660 fsm->state = ec_fsm_coe_error; |
660 fsm->state = ec_fsm_coe_error; |
661 EC_ERR("Reception of CoE Sdo description" |
661 EC_ERR("Reception of CoE SDO description" |
662 " response failed on slave %u: ", slave->ring_position); |
662 " response failed on slave %u: ", slave->ring_position); |
663 ec_datagram_print_wc_error(datagram); |
663 ec_datagram_print_wc_error(datagram); |
664 return; |
664 return; |
665 } |
665 } |
666 |
666 |
683 fsm->state = ec_fsm_coe_dict_desc_check; |
683 fsm->state = ec_fsm_coe_dict_desc_check; |
684 return; |
684 return; |
685 } |
685 } |
686 |
686 |
687 if (rec_size < 3) { |
687 if (rec_size < 3) { |
688 EC_ERR("Received corrupted Sdo description response (size %u).\n", |
688 EC_ERR("Received corrupted SDO description response (size %u).\n", |
689 rec_size); |
689 rec_size); |
690 fsm->state = ec_fsm_coe_error; |
690 fsm->state = ec_fsm_coe_error; |
691 return; |
691 return; |
692 } |
692 } |
693 |
693 |
694 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
694 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
695 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
695 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
696 EC_ERR("Sdo information error response at slave %u while" |
696 EC_ERR("SDO information error response at slave %u while" |
697 " fetching Sdo 0x%04X!\n", slave->ring_position, |
697 " fetching SDO 0x%04X!\n", slave->ring_position, |
698 sdo->index); |
698 sdo->index); |
699 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
699 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
700 fsm->state = ec_fsm_coe_error; |
700 fsm->state = ec_fsm_coe_error; |
701 return; |
701 return; |
702 } |
702 } |
703 |
703 |
704 if (rec_size < 8) { |
704 if (rec_size < 8) { |
705 EC_ERR("Received corrupted Sdo description response (size %u).\n", |
705 EC_ERR("Received corrupted SDO description response (size %u).\n", |
706 rec_size); |
706 rec_size); |
707 fsm->state = ec_fsm_coe_error; |
707 fsm->state = ec_fsm_coe_error; |
708 return; |
708 return; |
709 } |
709 } |
710 |
710 |
711 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
711 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
712 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
712 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
713 EC_READ_U16(data + 6) != sdo->index) { // Sdo index |
713 EC_READ_U16(data + 6) != sdo->index) { // SDO index |
714 if (fsm->slave->master->debug_level) { |
714 if (fsm->slave->master->debug_level) { |
715 EC_DBG("Invalid object description response at slave %u while" |
715 EC_DBG("Invalid object description response at slave %u while" |
716 " fetching Sdo 0x%04X!\n", slave->ring_position, |
716 " fetching SDO 0x%04X!\n", slave->ring_position, |
717 sdo->index); |
717 sdo->index); |
718 ec_print_data(data, rec_size); |
718 ec_print_data(data, rec_size); |
719 } |
719 } |
720 // check for CoE response again |
720 // check for CoE response again |
721 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
721 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
759 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
759 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
760 fsm->state = ec_fsm_coe_error; |
760 fsm->state = ec_fsm_coe_error; |
761 return; |
761 return; |
762 } |
762 } |
763 |
763 |
764 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
764 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
765 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
765 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
766 EC_WRITE_U8 (data + 3, 0x00); |
766 EC_WRITE_U8 (data + 3, 0x00); |
767 EC_WRITE_U16(data + 4, 0x0000); |
767 EC_WRITE_U16(data + 4, 0x0000); |
768 EC_WRITE_U16(data + 6, sdo->index); // Sdo index |
768 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
769 EC_WRITE_U8 (data + 8, fsm->subindex); // Sdo subindex |
769 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
770 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
770 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
771 |
771 |
772 fsm->retries = EC_FSM_RETRIES; |
772 fsm->retries = EC_FSM_RETRIES; |
773 fsm->state = ec_fsm_coe_dict_entry_request; |
773 fsm->state = ec_fsm_coe_dict_entry_request; |
774 } |
774 } |
789 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
789 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
790 return; // FIXME: check for response first? |
790 return; // FIXME: check for response first? |
791 |
791 |
792 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
792 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
793 fsm->state = ec_fsm_coe_error; |
793 fsm->state = ec_fsm_coe_error; |
794 EC_ERR("Failed to receive CoE Sdo entry request datagram for" |
794 EC_ERR("Failed to receive CoE SDO entry request datagram for" |
795 " slave %u (datagram state %u).\n", |
795 " slave %u (datagram state %u).\n", |
796 slave->ring_position, datagram->state); |
796 slave->ring_position, datagram->state); |
797 return; |
797 return; |
798 } |
798 } |
799 |
799 |
800 if (datagram->working_counter != 1) { |
800 if (datagram->working_counter != 1) { |
801 fsm->state = ec_fsm_coe_error; |
801 fsm->state = ec_fsm_coe_error; |
802 EC_ERR("Reception of CoE Sdo entry request failed on slave %u: ", |
802 EC_ERR("Reception of CoE SDO entry request failed on slave %u: ", |
803 slave->ring_position); |
803 slave->ring_position); |
804 ec_datagram_print_wc_error(datagram); |
804 ec_datagram_print_wc_error(datagram); |
805 return; |
805 return; |
806 } |
806 } |
807 |
807 |
846 if (!ec_slave_mbox_check(datagram)) { |
846 if (!ec_slave_mbox_check(datagram)) { |
847 unsigned long diff_ms = |
847 unsigned long diff_ms = |
848 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
848 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
849 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
849 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
850 fsm->state = ec_fsm_coe_error; |
850 fsm->state = ec_fsm_coe_error; |
851 EC_ERR("Timeout while waiting for Sdo entry description response " |
851 EC_ERR("Timeout while waiting for SDO entry description response " |
852 "on slave %u.\n", slave->ring_position); |
852 "on slave %u.\n", slave->ring_position); |
853 return; |
853 return; |
854 } |
854 } |
855 |
855 |
856 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
856 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
884 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
884 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
885 return; // FIXME: request again? |
885 return; // FIXME: request again? |
886 |
886 |
887 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
887 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
888 fsm->state = ec_fsm_coe_error; |
888 fsm->state = ec_fsm_coe_error; |
889 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
889 EC_ERR("Failed to receive CoE SDO description response datagram from" |
890 " slave %u (datagram state %u).\n", |
890 " slave %u (datagram state %u).\n", |
891 slave->ring_position, datagram->state); |
891 slave->ring_position, datagram->state); |
892 return; |
892 return; |
893 } |
893 } |
894 |
894 |
895 if (datagram->working_counter != 1) { |
895 if (datagram->working_counter != 1) { |
896 fsm->state = ec_fsm_coe_error; |
896 fsm->state = ec_fsm_coe_error; |
897 EC_ERR("Reception of CoE Sdo description" |
897 EC_ERR("Reception of CoE SDO description" |
898 " response failed on slave %u: ", slave->ring_position); |
898 " response failed on slave %u: ", slave->ring_position); |
899 ec_datagram_print_wc_error(datagram); |
899 ec_datagram_print_wc_error(datagram); |
900 return; |
900 return; |
901 } |
901 } |
902 |
902 |
919 fsm->state = ec_fsm_coe_dict_entry_check; |
919 fsm->state = ec_fsm_coe_dict_entry_check; |
920 return; |
920 return; |
921 } |
921 } |
922 |
922 |
923 if (rec_size < 3) { |
923 if (rec_size < 3) { |
924 EC_ERR("Received corrupted Sdo entry description response " |
924 EC_ERR("Received corrupted SDO entry description response " |
925 "(size %u).\n", rec_size); |
925 "(size %u).\n", rec_size); |
926 fsm->state = ec_fsm_coe_error; |
926 fsm->state = ec_fsm_coe_error; |
927 return; |
927 return; |
928 } |
928 } |
929 |
929 |
930 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
930 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
931 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
931 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
932 EC_ERR("Sdo information error response at slave %u while" |
932 EC_ERR("SDO information error response at slave %u while" |
933 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
933 " fetching SDO entry 0x%04X:%02X!\n", slave->ring_position, |
934 sdo->index, fsm->subindex); |
934 sdo->index, fsm->subindex); |
935 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
935 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
936 fsm->state = ec_fsm_coe_error; |
936 fsm->state = ec_fsm_coe_error; |
937 return; |
937 return; |
938 } |
938 } |
939 |
939 |
940 if (rec_size < 9) { |
940 if (rec_size < 9) { |
941 EC_ERR("Received corrupted Sdo entry description response " |
941 EC_ERR("Received corrupted SDO entry description response " |
942 "(size %u).\n", rec_size); |
942 "(size %u).\n", rec_size); |
943 fsm->state = ec_fsm_coe_error; |
943 fsm->state = ec_fsm_coe_error; |
944 return; |
944 return; |
945 } |
945 } |
946 |
946 |
947 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
947 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
948 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
948 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
949 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
949 EC_READ_U16(data + 6) != sdo->index || // SDO index |
950 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
950 EC_READ_U8(data + 8) != fsm->subindex) { // SDO subindex |
951 if (fsm->slave->master->debug_level) { |
951 if (fsm->slave->master->debug_level) { |
952 EC_DBG("Invalid entry description response at slave %u while" |
952 EC_DBG("Invalid entry description response at slave %u while" |
953 " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position, |
953 " fetching SDO entry 0x%04X:%02X!\n", slave->ring_position, |
954 sdo->index, fsm->subindex); |
954 sdo->index, fsm->subindex); |
955 ec_print_data(data, rec_size); |
955 ec_print_data(data, rec_size); |
956 } |
956 } |
957 // check for CoE response again |
957 // check for CoE response again |
958 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
958 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1001 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1001 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1002 fsm->state = ec_fsm_coe_error; |
1002 fsm->state = ec_fsm_coe_error; |
1003 return; |
1003 return; |
1004 } |
1004 } |
1005 |
1005 |
1006 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
1006 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
1007 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
1007 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
1008 EC_WRITE_U8 (data + 3, 0x00); |
1008 EC_WRITE_U8 (data + 3, 0x00); |
1009 EC_WRITE_U16(data + 4, 0x0000); |
1009 EC_WRITE_U16(data + 4, 0x0000); |
1010 EC_WRITE_U16(data + 6, sdo->index); // Sdo index |
1010 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
1011 EC_WRITE_U8 (data + 8, fsm->subindex); // Sdo subindex |
1011 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
1012 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
1012 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
1013 |
1013 |
1014 fsm->retries = EC_FSM_RETRIES; |
1014 fsm->retries = EC_FSM_RETRIES; |
1015 fsm->state = ec_fsm_coe_dict_entry_request; |
1015 fsm->state = ec_fsm_coe_dict_entry_request; |
1016 return; |
1016 return; |
1017 } |
1017 } |
1018 |
1018 |
1019 // another Sdo description to fetch? |
1019 // another SDO description to fetch? |
1020 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
1020 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
1021 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
1021 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
1022 |
1022 |
1023 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
1023 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
1024 fsm->state = ec_fsm_coe_error; |
1024 fsm->state = ec_fsm_coe_error; |
1025 return; |
1025 return; |
1026 } |
1026 } |
1027 |
1027 |
1028 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
1028 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
1029 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
1029 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
1030 EC_WRITE_U8 (data + 3, 0x00); |
1030 EC_WRITE_U8 (data + 3, 0x00); |
1031 EC_WRITE_U16(data + 4, 0x0000); |
1031 EC_WRITE_U16(data + 4, 0x0000); |
1032 EC_WRITE_U16(data + 6, fsm->sdo->index); // Sdo index |
1032 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
1033 |
1033 |
1034 fsm->retries = EC_FSM_RETRIES; |
1034 fsm->retries = EC_FSM_RETRIES; |
1035 fsm->state = ec_fsm_coe_dict_desc_request; |
1035 fsm->state = ec_fsm_coe_dict_desc_request; |
1036 return; |
1036 return; |
1037 } |
1037 } |
1054 ec_sdo_request_t *request = fsm->request; |
1054 ec_sdo_request_t *request = fsm->request; |
1055 uint8_t *data; |
1055 uint8_t *data; |
1056 uint8_t size; |
1056 uint8_t size; |
1057 |
1057 |
1058 if (fsm->slave->master->debug_level) { |
1058 if (fsm->slave->master->debug_level) { |
1059 EC_DBG("Downloading Sdo 0x%04X:%02X to slave %u.\n", |
1059 EC_DBG("Downloading SDO 0x%04X:%02X to slave %u.\n", |
1060 request->index, request->subindex, slave->ring_position); |
1060 request->index, request->subindex, slave->ring_position); |
1061 ec_print_data(request->data, request->data_size); |
1061 ec_print_data(request->data, request->data_size); |
1062 } |
1062 } |
1063 |
1063 |
1064 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1064 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1073 return; |
1073 return; |
1074 } |
1074 } |
1075 |
1075 |
1076 size = 4 - request->data_size; |
1076 size = 4 - request->data_size; |
1077 |
1077 |
1078 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1078 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1079 EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited |
1079 EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited |
1080 | size << 2 |
1080 | size << 2 |
1081 | 0x1 << 5)); // Download request |
1081 | 0x1 << 5)); // Download request |
1082 EC_WRITE_U16(data + 3, request->index); |
1082 EC_WRITE_U16(data + 3, request->index); |
1083 EC_WRITE_U8 (data + 5, request->subindex); |
1083 EC_WRITE_U8 (data + 5, request->subindex); |
1088 ec_print_data(data, 10 + request->data_size); |
1088 ec_print_data(data, 10 + request->data_size); |
1089 } |
1089 } |
1090 } |
1090 } |
1091 else { // request->data_size > 4, use normal transfer type |
1091 else { // request->data_size > 4, use normal transfer type |
1092 if (slave->sii.rx_mailbox_size < 6 + 10 + request->data_size) { |
1092 if (slave->sii.rx_mailbox_size < 6 + 10 + request->data_size) { |
1093 EC_ERR("Sdo fragmenting not supported yet!\n"); |
1093 EC_ERR("SDO fragmenting not supported yet!\n"); |
1094 fsm->state = ec_fsm_coe_error; |
1094 fsm->state = ec_fsm_coe_error; |
1095 return; |
1095 return; |
1096 } |
1096 } |
1097 |
1097 |
1098 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1098 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1099 request->data_size + 10))) { |
1099 request->data_size + 10))) { |
1100 fsm->state = ec_fsm_coe_error; |
1100 fsm->state = ec_fsm_coe_error; |
1101 return; |
1101 return; |
1102 } |
1102 } |
1103 |
1103 |
1104 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1104 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1105 EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal |
1105 EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal |
1106 | 0x1 << 5)); // Download request |
1106 | 0x1 << 5)); // Download request |
1107 EC_WRITE_U16(data + 3, request->index); |
1107 EC_WRITE_U16(data + 3, request->index); |
1108 EC_WRITE_U8 (data + 5, request->subindex); |
1108 EC_WRITE_U8 (data + 5, request->subindex); |
1109 EC_WRITE_U32(data + 6, request->data_size); |
1109 EC_WRITE_U32(data + 6, request->data_size); |
1147 if (!datagram->working_counter) { |
1147 if (!datagram->working_counter) { |
1148 unsigned long diff_ms = |
1148 unsigned long diff_ms = |
1149 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1149 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1150 if (diff_ms < fsm->request->response_timeout) { |
1150 if (diff_ms < fsm->request->response_timeout) { |
1151 if (fsm->slave->master->debug_level) { |
1151 if (fsm->slave->master->debug_level) { |
1152 EC_DBG("Slave %u did not respond to Sdo download request. " |
1152 EC_DBG("Slave %u did not respond to SDO download request. " |
1153 "Retrying after %u ms...\n", |
1153 "Retrying after %u ms...\n", |
1154 slave->ring_position, (u32) diff_ms); |
1154 slave->ring_position, (u32) diff_ms); |
1155 } |
1155 } |
1156 // no response; send request datagram again |
1156 // no response; send request datagram again |
1157 return; |
1157 return; |
1204 if (!ec_slave_mbox_check(datagram)) { |
1204 if (!ec_slave_mbox_check(datagram)) { |
1205 unsigned long diff_ms = |
1205 unsigned long diff_ms = |
1206 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1206 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1207 if (diff_ms >= fsm->request->response_timeout) { |
1207 if (diff_ms >= fsm->request->response_timeout) { |
1208 fsm->state = ec_fsm_coe_error; |
1208 fsm->state = ec_fsm_coe_error; |
1209 EC_ERR("Timeout while waiting for Sdo download response on " |
1209 EC_ERR("Timeout while waiting for SDO download response on " |
1210 "slave %u.\n", slave->ring_position); |
1210 "slave %u.\n", slave->ring_position); |
1211 return; |
1211 return; |
1212 } |
1212 } |
1213 |
1213 |
1214 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1214 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1286 EC_ERR("Received data are too small (%u bytes):\n", rec_size); |
1286 EC_ERR("Received data are too small (%u bytes):\n", rec_size); |
1287 ec_print_data(data, rec_size); |
1287 ec_print_data(data, rec_size); |
1288 return; |
1288 return; |
1289 } |
1289 } |
1290 |
1290 |
1291 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1291 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1292 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1292 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1293 fsm->state = ec_fsm_coe_error; |
1293 fsm->state = ec_fsm_coe_error; |
1294 EC_ERR("Sdo download 0x%04X:%02X (%u bytes) aborted on slave %u.\n", |
1294 EC_ERR("SDO download 0x%04X:%02X (%u bytes) aborted on slave %u.\n", |
1295 request->index, request->subindex, request->data_size, |
1295 request->index, request->subindex, request->data_size, |
1296 slave->ring_position); |
1296 slave->ring_position); |
1297 if (rec_size < 10) { |
1297 if (rec_size < 10) { |
1298 EC_ERR("Incomplete Abort command:\n"); |
1298 EC_ERR("Incomplete Abort command:\n"); |
1299 ec_print_data(data, rec_size); |
1299 ec_print_data(data, rec_size); |
1302 ec_canopen_abort_msg(fsm->request->abort_code); |
1302 ec_canopen_abort_msg(fsm->request->abort_code); |
1303 } |
1303 } |
1304 return; |
1304 return; |
1305 } |
1305 } |
1306 |
1306 |
1307 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1307 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1308 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1308 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1309 EC_READ_U16(data + 3) != request->index || // index |
1309 EC_READ_U16(data + 3) != request->index || // index |
1310 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1310 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1311 if (slave->master->debug_level) { |
1311 if (slave->master->debug_level) { |
1312 EC_DBG("Invalid Sdo download response at slave %u! Retrying...\n", |
1312 EC_DBG("Invalid SDO download response at slave %u! Retrying...\n", |
1313 slave->ring_position); |
1313 slave->ring_position); |
1314 ec_print_data(data, rec_size); |
1314 ec_print_data(data, rec_size); |
1315 } |
1315 } |
1316 // check for CoE response again |
1316 // check for CoE response again |
1317 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1317 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1336 ec_master_t *master = slave->master; |
1336 ec_master_t *master = slave->master; |
1337 ec_sdo_request_t *request = fsm->request; |
1337 ec_sdo_request_t *request = fsm->request; |
1338 uint8_t *data; |
1338 uint8_t *data; |
1339 |
1339 |
1340 if (master->debug_level) |
1340 if (master->debug_level) |
1341 EC_DBG("Uploading Sdo 0x%04X:%02X from slave %u.\n", |
1341 EC_DBG("Uploading SDO 0x%04X:%02X from slave %u.\n", |
1342 request->index, request->subindex, slave->ring_position); |
1342 request->index, request->subindex, slave->ring_position); |
1343 |
1343 |
1344 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1344 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1345 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1345 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1346 fsm->state = ec_fsm_coe_error; |
1346 fsm->state = ec_fsm_coe_error; |
1350 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1350 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1351 fsm->state = ec_fsm_coe_error; |
1351 fsm->state = ec_fsm_coe_error; |
1352 return; |
1352 return; |
1353 } |
1353 } |
1354 |
1354 |
1355 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1355 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1356 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1356 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1357 EC_WRITE_U16(data + 3, request->index); |
1357 EC_WRITE_U16(data + 3, request->index); |
1358 EC_WRITE_U8 (data + 5, request->subindex); |
1358 EC_WRITE_U8 (data + 5, request->subindex); |
1359 memset(data + 6, 0x00, 4); |
1359 memset(data + 6, 0x00, 4); |
1360 |
1360 |
1395 if (!datagram->working_counter) { |
1395 if (!datagram->working_counter) { |
1396 unsigned long diff_ms = |
1396 unsigned long diff_ms = |
1397 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1397 (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1398 if (diff_ms < fsm->request->response_timeout) { |
1398 if (diff_ms < fsm->request->response_timeout) { |
1399 if (fsm->slave->master->debug_level) { |
1399 if (fsm->slave->master->debug_level) { |
1400 EC_DBG("Slave %u did not respond to Sdo upload request. " |
1400 EC_DBG("Slave %u did not respond to SDO upload request. " |
1401 "Retrying after %u ms...\n", |
1401 "Retrying after %u ms...\n", |
1402 slave->ring_position, (u32) diff_ms); |
1402 slave->ring_position, (u32) diff_ms); |
1403 } |
1403 } |
1404 // no response; send request datagram again |
1404 // no response; send request datagram again |
1405 return; |
1405 return; |
1452 if (!ec_slave_mbox_check(datagram)) { |
1452 if (!ec_slave_mbox_check(datagram)) { |
1453 unsigned long diff_ms = |
1453 unsigned long diff_ms = |
1454 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1454 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1455 if (diff_ms >= fsm->request->response_timeout) { |
1455 if (diff_ms >= fsm->request->response_timeout) { |
1456 fsm->state = ec_fsm_coe_error; |
1456 fsm->state = ec_fsm_coe_error; |
1457 EC_ERR("Timeout while waiting for Sdo upload response on " |
1457 EC_ERR("Timeout while waiting for SDO upload response on " |
1458 "slave %u.\n", slave->ring_position); |
1458 "slave %u.\n", slave->ring_position); |
1459 return; |
1459 return; |
1460 } |
1460 } |
1461 |
1461 |
1462 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1462 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1532 return; |
1532 return; |
1533 } |
1533 } |
1534 |
1534 |
1535 if (rec_size < 3) { |
1535 if (rec_size < 3) { |
1536 fsm->state = ec_fsm_coe_error; |
1536 fsm->state = ec_fsm_coe_error; |
1537 EC_ERR("Received currupted Sdo upload response (%u bytes)!\n", rec_size); |
1537 EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size); |
1538 ec_print_data(data, rec_size); |
1538 ec_print_data(data, rec_size); |
1539 return; |
1539 return; |
1540 } |
1540 } |
1541 |
1541 |
1542 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1542 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1543 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1543 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1544 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1544 EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", |
1545 request->index, request->subindex, slave->ring_position); |
1545 request->index, request->subindex, slave->ring_position); |
1546 if (rec_size >= 10) { |
1546 if (rec_size >= 10) { |
1547 request->abort_code = EC_READ_U32(data + 6); |
1547 request->abort_code = EC_READ_U32(data + 6); |
1548 ec_canopen_abort_msg(request->abort_code); |
1548 ec_canopen_abort_msg(request->abort_code); |
1549 } else { |
1549 } else { |
1557 expedited = EC_READ_U8(data + 2) & 0x02; |
1557 expedited = EC_READ_U8(data + 2) & 0x02; |
1558 |
1558 |
1559 if (expedited) { |
1559 if (expedited) { |
1560 if (rec_size < 7) { |
1560 if (rec_size < 7) { |
1561 fsm->state = ec_fsm_coe_error; |
1561 fsm->state = ec_fsm_coe_error; |
1562 EC_ERR("Received currupted Sdo expedited upload" |
1562 EC_ERR("Received currupted SDO expedited upload" |
1563 " response (only %u bytes)!\n", rec_size); |
1563 " response (only %u bytes)!\n", rec_size); |
1564 ec_print_data(data, rec_size); |
1564 ec_print_data(data, rec_size); |
1565 return; |
1565 return; |
1566 } |
1566 } |
1567 |
1567 |
1568 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1568 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1569 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1569 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1570 EC_READ_U16(data + 3) != request->index || // index |
1570 EC_READ_U16(data + 3) != request->index || // index |
1571 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1571 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1572 if (fsm->slave->master->debug_level) { |
1572 if (fsm->slave->master->debug_level) { |
1573 EC_DBG("Invalid Sdo upload expedited response at slave %u!\n", |
1573 EC_DBG("Invalid SDO upload expedited response at slave %u!\n", |
1574 slave->ring_position); |
1574 slave->ring_position); |
1575 ec_print_data(data, rec_size); |
1575 ec_print_data(data, rec_size); |
1576 } |
1576 } |
1577 // check for CoE response again |
1577 // check for CoE response again |
1578 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1578 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1601 return; |
1601 return; |
1602 } |
1602 } |
1603 } else { // normal |
1603 } else { // normal |
1604 if (rec_size < 10) { |
1604 if (rec_size < 10) { |
1605 fsm->state = ec_fsm_coe_error; |
1605 fsm->state = ec_fsm_coe_error; |
1606 EC_ERR("Received currupted Sdo normal upload" |
1606 EC_ERR("Received currupted SDO normal upload" |
1607 " response (only %u bytes)!\n", rec_size); |
1607 " response (only %u bytes)!\n", rec_size); |
1608 ec_print_data(data, rec_size); |
1608 ec_print_data(data, rec_size); |
1609 return; |
1609 return; |
1610 } |
1610 } |
1611 |
1611 |
1612 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1612 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1613 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1613 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1614 EC_READ_U16(data + 3) != request->index || // index |
1614 EC_READ_U16(data + 3) != request->index || // index |
1615 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1615 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1616 if (fsm->slave->master->debug_level) { |
1616 if (fsm->slave->master->debug_level) { |
1617 EC_DBG("Invalid Sdo normal upload response at slave %u!\n", |
1617 EC_DBG("Invalid SDO normal upload response at slave %u!\n", |
1618 slave->ring_position); |
1618 slave->ring_position); |
1619 ec_print_data(data, rec_size); |
1619 ec_print_data(data, rec_size); |
1620 } |
1620 } |
1621 // check for CoE response again |
1621 // check for CoE response again |
1622 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1622 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1646 } |
1646 } |
1647 |
1647 |
1648 fsm->toggle = 0; |
1648 fsm->toggle = 0; |
1649 |
1649 |
1650 if (data_size < complete_size) { |
1650 if (data_size < complete_size) { |
1651 EC_WARN("Sdo data incomplete (%u / %u).\n", |
1651 EC_WARN("SDO data incomplete (%u / %u).\n", |
1652 data_size, complete_size); |
1652 data_size, complete_size); |
1653 |
1653 |
1654 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, |
1654 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, |
1655 0x03, 3))) { |
1655 0x03, 3))) { |
1656 fsm->state = ec_fsm_coe_error; |
1656 fsm->state = ec_fsm_coe_error; |
1657 return; |
1657 return; |
1658 } |
1658 } |
1659 |
1659 |
1660 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1660 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1661 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1661 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1662 | 0x3 << 5)); // upload segment request |
1662 | 0x3 << 5)); // upload segment request |
1663 |
1663 |
1664 if (master->debug_level) { |
1664 if (master->debug_level) { |
1665 EC_DBG("Upload segment request:\n"); |
1665 EC_DBG("Upload segment request:\n"); |
1751 if (!ec_slave_mbox_check(datagram)) { |
1751 if (!ec_slave_mbox_check(datagram)) { |
1752 unsigned long diff_ms = |
1752 unsigned long diff_ms = |
1753 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1753 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1754 if (diff_ms >= fsm->request->response_timeout) { |
1754 if (diff_ms >= fsm->request->response_timeout) { |
1755 fsm->state = ec_fsm_coe_error; |
1755 fsm->state = ec_fsm_coe_error; |
1756 EC_ERR("Timeout while waiting for Sdo upload segment response " |
1756 EC_ERR("Timeout while waiting for SDO upload segment response " |
1757 "on slave %u.\n", slave->ring_position); |
1757 "on slave %u.\n", slave->ring_position); |
1758 return; |
1758 return; |
1759 } |
1759 } |
1760 |
1760 |
1761 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1761 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1831 fsm->state = ec_fsm_coe_up_seg_check; |
1831 fsm->state = ec_fsm_coe_up_seg_check; |
1832 return; |
1832 return; |
1833 } |
1833 } |
1834 |
1834 |
1835 if (rec_size < 10) { |
1835 if (rec_size < 10) { |
1836 EC_ERR("Received currupted Sdo upload segment response!\n"); |
1836 EC_ERR("Received currupted SDO upload segment response!\n"); |
1837 ec_print_data(data, rec_size); |
1837 ec_print_data(data, rec_size); |
1838 fsm->state = ec_fsm_coe_error; |
1838 fsm->state = ec_fsm_coe_error; |
1839 return; |
1839 return; |
1840 } |
1840 } |
1841 |
1841 |
1842 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1842 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1843 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1843 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1844 EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n", |
1844 EC_ERR("SDO upload 0x%04X:%02X aborted on slave %u.\n", |
1845 request->index, request->subindex, slave->ring_position); |
1845 request->index, request->subindex, slave->ring_position); |
1846 request->abort_code = EC_READ_U32(data + 6); |
1846 request->abort_code = EC_READ_U32(data + 6); |
1847 ec_canopen_abort_msg(request->abort_code); |
1847 ec_canopen_abort_msg(request->abort_code); |
1848 fsm->state = ec_fsm_coe_error; |
1848 fsm->state = ec_fsm_coe_error; |
1849 return; |
1849 return; |
1850 } |
1850 } |
1851 |
1851 |
1852 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1852 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1853 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1853 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1854 if (fsm->slave->master->debug_level) { |
1854 if (fsm->slave->master->debug_level) { |
1855 EC_DBG("Invalid Sdo upload segment response at slave %u!\n", |
1855 EC_DBG("Invalid SDO upload segment response at slave %u!\n", |
1856 slave->ring_position); |
1856 slave->ring_position); |
1857 ec_print_data(data, rec_size); |
1857 ec_print_data(data, rec_size); |
1858 } |
1858 } |
1859 // check for CoE response again |
1859 // check for CoE response again |
1860 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1860 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1866 last_segment = EC_READ_U8(data + 2) & 0x01; |
1866 last_segment = EC_READ_U8(data + 2) & 0x01; |
1867 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1867 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1868 data_size = rec_size - 10; |
1868 data_size = rec_size - 10; |
1869 |
1869 |
1870 if (data_size != seg_size) { |
1870 if (data_size != seg_size) { |
1871 EC_WARN("Sdo segment data invalid (%u / %u)" |
1871 EC_WARN("SDO segment data invalid (%u / %u)" |
1872 " - Fragmenting not implemented.\n", |
1872 " - Fragmenting not implemented.\n", |
1873 data_size, seg_size); |
1873 data_size, seg_size); |
1874 } |
1874 } |
1875 |
1875 |
1876 memcpy(request->data + request->data_size, data + 10, data_size); |
1876 memcpy(request->data + request->data_size, data + 10, data_size); |
1882 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) { |
1882 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) { |
1883 fsm->state = ec_fsm_coe_error; |
1883 fsm->state = ec_fsm_coe_error; |
1884 return; |
1884 return; |
1885 } |
1885 } |
1886 |
1886 |
1887 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1887 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1888 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1888 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1889 | 0x3 << 5)); // upload segment request |
1889 | 0x3 << 5)); // upload segment request |
1890 |
1890 |
1891 if (master->debug_level) { |
1891 if (master->debug_level) { |
1892 EC_DBG("Upload segment request:\n"); |
1892 EC_DBG("Upload segment request:\n"); |