73 void ec_fsm_coe_error(ec_fsm_coe_t *); |
73 void ec_fsm_coe_error(ec_fsm_coe_t *); |
74 |
74 |
75 /*****************************************************************************/ |
75 /*****************************************************************************/ |
76 |
76 |
77 /** |
77 /** |
78 SDO abort messages. |
78 Sdo abort messages. |
79 The "abort SDO transfer request" supplies an abort code, |
79 The "abort Sdo transfer request" supplies an abort code, |
80 which can be translated to clear text. This table does |
80 which can be translated to clear text. This table does |
81 the mapping of the codes and messages. |
81 the mapping of the codes and messages. |
82 */ |
82 */ |
83 |
83 |
84 const ec_code_msg_t sdo_abort_messages[] = { |
84 const ec_code_msg_t sdo_abort_messages[] = { |
85 {0x05030000, "Toggle bit not changed"}, |
85 {0x05030000, "Toggle bit not changed"}, |
86 {0x05040000, "SDO protocol timeout"}, |
86 {0x05040000, "Sdo protocol timeout"}, |
87 {0x05040001, "Client/Server command specifier not valid or unknown"}, |
87 {0x05040001, "Client/Server command specifier not valid or unknown"}, |
88 {0x05040005, "Out of memory"}, |
88 {0x05040005, "Out of memory"}, |
89 {0x06010000, "Unsupported access to an object"}, |
89 {0x06010000, "Unsupported access to an object"}, |
90 {0x06010001, "Attempt to read a write-only object"}, |
90 {0x06010001, "Attempt to read a write-only object"}, |
91 {0x06010002, "Attempt to write a read-only object"}, |
91 {0x06010002, "Attempt to write a read-only object"}, |
92 {0x06020000, "This object does not exist in the object directory"}, |
92 {0x06020000, "This object does not exist in the object directory"}, |
93 {0x06040041, "The object cannot be mapped into the PDO"}, |
93 {0x06040041, "The object cannot be mapped into the Pdo"}, |
94 {0x06040042, "The number and length of the objects to be mapped would" |
94 {0x06040042, "The number and length of the objects to be mapped would" |
95 " exceed the PDO length"}, |
95 " exceed the Pdo length"}, |
96 {0x06040043, "General parameter incompatibility reason"}, |
96 {0x06040043, "General parameter incompatibility reason"}, |
97 {0x06040047, "Gerneral internal incompatibility in device"}, |
97 {0x06040047, "Gerneral internal incompatibility in device"}, |
98 {0x06060000, "Access failure due to a hardware error"}, |
98 {0x06060000, "Access failure due to a hardware error"}, |
99 {0x06070010, "Data type does not match, length of service parameter does" |
99 {0x06070010, "Data type does not match, length of service parameter does" |
100 " not match"}, |
100 " not match"}, |
119 }; |
119 }; |
120 |
120 |
121 /*****************************************************************************/ |
121 /*****************************************************************************/ |
122 |
122 |
123 /** |
123 /** |
124 Outputs an SDO abort message. |
124 Outputs an Sdo abort message. |
125 */ |
125 */ |
126 |
126 |
127 void ec_canopen_abort_msg(uint32_t abort_code) |
127 void ec_canopen_abort_msg(uint32_t abort_code) |
128 { |
128 { |
129 const ec_code_msg_t *abort_msg; |
129 const ec_code_msg_t *abort_msg; |
130 |
130 |
131 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
131 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
132 if (abort_msg->code == abort_code) { |
132 if (abort_msg->code == abort_code) { |
133 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
133 EC_ERR("Sdo abort message 0x%08X: \"%s\".\n", |
134 abort_msg->code, abort_msg->message); |
134 abort_msg->code, abort_msg->message); |
135 return; |
135 return; |
136 } |
136 } |
137 } |
137 } |
138 |
138 |
139 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
139 EC_ERR("Unknown Sdo abort code 0x%08X.\n", abort_code); |
140 } |
140 } |
141 |
141 |
142 /*****************************************************************************/ |
142 /*****************************************************************************/ |
143 |
143 |
144 /** |
144 /** |
178 } |
178 } |
179 |
179 |
180 /*****************************************************************************/ |
180 /*****************************************************************************/ |
181 |
181 |
182 /** |
182 /** |
183 Starts to download an SDO to a slave. |
183 Starts to download an Sdo to a slave. |
184 */ |
184 */ |
185 |
185 |
186 void ec_fsm_coe_download(ec_fsm_coe_t *fsm, /**< finite state machine */ |
186 void ec_fsm_coe_download(ec_fsm_coe_t *fsm, /**< finite state machine */ |
187 ec_slave_t *slave, /**< EtherCAT slave */ |
187 ec_slave_t *slave, /**< EtherCAT slave */ |
188 ec_sdo_data_t *sdodata /**< SDO data object */ |
188 ec_sdo_data_t *sdodata /**< Sdo data object */ |
189 ) |
189 ) |
190 { |
190 { |
191 fsm->slave = slave; |
191 fsm->slave = slave; |
192 fsm->sdodata = sdodata; |
192 fsm->sdodata = sdodata; |
193 fsm->state = ec_fsm_coe_down_start; |
193 fsm->state = ec_fsm_coe_down_start; |
194 } |
194 } |
195 |
195 |
196 /*****************************************************************************/ |
196 /*****************************************************************************/ |
197 |
197 |
198 /** |
198 /** |
199 Starts to upload an SDO from a slave. |
199 Starts to upload an Sdo from a slave. |
200 */ |
200 */ |
201 |
201 |
202 void ec_fsm_coe_upload(ec_fsm_coe_t *fsm, /**< finite state machine */ |
202 void ec_fsm_coe_upload(ec_fsm_coe_t *fsm, /**< finite state machine */ |
203 ec_slave_t *slave, /**< EtherCAT slave */ |
203 ec_slave_t *slave, /**< EtherCAT slave */ |
204 ec_sdo_request_t *request /**< SDO request */ |
204 ec_sdo_request_t *request /**< Sdo request */ |
205 ) |
205 ) |
206 { |
206 { |
207 fsm->slave = slave; |
207 fsm->slave = slave; |
208 fsm->request = request; |
208 fsm->request = request; |
209 fsm->state = ec_fsm_coe_up_start; |
209 fsm->state = ec_fsm_coe_up_start; |
252 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
252 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
253 fsm->state = ec_fsm_coe_error; |
253 fsm->state = ec_fsm_coe_error; |
254 return; |
254 return; |
255 } |
255 } |
256 |
256 |
257 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
257 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
258 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
258 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
259 EC_WRITE_U8 (data + 3, 0x00); |
259 EC_WRITE_U8 (data + 3, 0x00); |
260 EC_WRITE_U16(data + 4, 0x0000); |
260 EC_WRITE_U16(data + 4, 0x0000); |
261 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
261 EC_WRITE_U16(data + 6, 0x0001); // deliver all Sdos! |
262 |
262 |
263 fsm->retries = EC_FSM_RETRIES; |
263 fsm->retries = EC_FSM_RETRIES; |
264 fsm->state = ec_fsm_coe_dict_request; |
264 fsm->state = ec_fsm_coe_dict_request; |
265 } |
265 } |
266 |
266 |
334 |
334 |
335 if (!ec_slave_mbox_check(datagram)) { |
335 if (!ec_slave_mbox_check(datagram)) { |
336 if (datagram->cycles_received |
336 if (datagram->cycles_received |
337 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
337 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
338 fsm->state = ec_fsm_coe_error; |
338 fsm->state = ec_fsm_coe_error; |
339 EC_ERR("Timeout while checking SDO dictionary on slave %i.\n", |
339 EC_ERR("Timeout while checking Sdo dictionary on slave %i.\n", |
340 slave->ring_position); |
340 slave->ring_position); |
341 return; |
341 return; |
342 } |
342 } |
343 |
343 |
344 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
344 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
398 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
398 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
399 fsm->state = ec_fsm_coe_error; |
399 fsm->state = ec_fsm_coe_error; |
400 return; |
400 return; |
401 } |
401 } |
402 |
402 |
403 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
403 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
404 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
404 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response |
405 EC_ERR("SDO information error response at slave %i!\n", |
405 EC_ERR("Sdo information error response at slave %i!\n", |
406 slave->ring_position); |
406 slave->ring_position); |
407 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
407 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
408 fsm->state = ec_fsm_coe_error; |
408 fsm->state = ec_fsm_coe_error; |
409 return; |
409 return; |
410 } |
410 } |
411 |
411 |
412 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
412 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
413 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
413 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
414 EC_ERR("Invalid SDO list response at slave %i!\n", |
414 EC_ERR("Invalid Sdo list response at slave %i!\n", |
415 slave->ring_position); |
415 slave->ring_position); |
416 ec_print_data(data, rec_size); |
416 ec_print_data(data, rec_size); |
417 fsm->state = ec_fsm_coe_error; |
417 fsm->state = ec_fsm_coe_error; |
418 return; |
418 return; |
419 } |
419 } |
429 |
429 |
430 for (i = 0; i < sdo_count; i++) { |
430 for (i = 0; i < sdo_count; i++) { |
431 sdo_index = EC_READ_U16(data + 8 + i * 2); |
431 sdo_index = EC_READ_U16(data + 8 + i * 2); |
432 if (!sdo_index) { |
432 if (!sdo_index) { |
433 if (slave->master->debug_level) |
433 if (slave->master->debug_level) |
434 EC_WARN("SDO dictionary of slave %i contains index 0x0000.\n", |
434 EC_WARN("Sdo dictionary of slave %i contains index 0x0000.\n", |
435 slave->ring_position); |
435 slave->ring_position); |
436 continue; |
436 continue; |
437 } |
437 } |
438 |
438 |
439 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_ATOMIC))) { |
439 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_ATOMIC))) { |
440 EC_ERR("Failed to allocate memory for SDO!\n"); |
440 EC_ERR("Failed to allocate memory for Sdo!\n"); |
441 fsm->state = ec_fsm_coe_error; |
441 fsm->state = ec_fsm_coe_error; |
442 return; |
442 return; |
443 } |
443 } |
444 |
444 |
445 if (ec_sdo_init(sdo, sdo_index, slave)) { |
445 if (ec_sdo_init(sdo, sdo_index, slave)) { |
446 EC_ERR("Failed to init SDO!\n"); |
446 EC_ERR("Failed to init Sdo!\n"); |
447 fsm->state = ec_fsm_coe_error; |
447 fsm->state = ec_fsm_coe_error; |
448 return; |
448 return; |
449 } |
449 } |
450 |
450 |
451 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
451 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
452 } |
452 } |
453 |
453 |
454 fragments_left = EC_READ_U16(data + 4); |
454 fragments_left = EC_READ_U16(data + 4); |
455 if (slave->master->debug_level && fragments_left) { |
455 if (slave->master->debug_level && fragments_left) { |
456 EC_DBG("SDO list fragments left: %u\n", fragments_left); |
456 EC_DBG("Sdo list fragments left: %u\n", fragments_left); |
457 } |
457 } |
458 |
458 |
459 if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again. |
459 if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again. |
460 fsm->cycles_start = datagram->cycles_sent; |
460 fsm->cycles_start = datagram->cycles_sent; |
461 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
461 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
463 fsm->state = ec_fsm_coe_dict_check; |
463 fsm->state = ec_fsm_coe_dict_check; |
464 return; |
464 return; |
465 } |
465 } |
466 |
466 |
467 if (list_empty(&slave->sdo_dictionary)) { |
467 if (list_empty(&slave->sdo_dictionary)) { |
468 // no SDOs in dictionary. finished. |
468 // no Sdos in dictionary. finished. |
469 fsm->state = ec_fsm_coe_end; // success |
469 fsm->state = ec_fsm_coe_end; // success |
470 return; |
470 return; |
471 } |
471 } |
472 |
472 |
473 // fetch SDO descriptions |
473 // fetch Sdo descriptions |
474 fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list); |
474 fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list); |
475 |
475 |
476 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
476 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
477 fsm->state = ec_fsm_coe_error; |
477 fsm->state = ec_fsm_coe_error; |
478 return; |
478 return; |
479 } |
479 } |
480 |
480 |
481 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
481 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
482 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
482 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
483 EC_WRITE_U8 (data + 3, 0x00); |
483 EC_WRITE_U8 (data + 3, 0x00); |
484 EC_WRITE_U16(data + 4, 0x0000); |
484 EC_WRITE_U16(data + 4, 0x0000); |
485 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
485 EC_WRITE_U16(data + 6, fsm->sdo->index); // Sdo index |
486 |
486 |
487 fsm->retries = EC_FSM_RETRIES; |
487 fsm->retries = EC_FSM_RETRIES; |
488 fsm->state = ec_fsm_coe_dict_desc_request; |
488 fsm->state = ec_fsm_coe_dict_desc_request; |
489 } |
489 } |
490 |
490 |
503 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
503 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
504 return; // FIXME: check for response first? |
504 return; // FIXME: check for response first? |
505 |
505 |
506 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
506 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
507 fsm->state = ec_fsm_coe_error; |
507 fsm->state = ec_fsm_coe_error; |
508 EC_ERR("Failed to receive CoE SDO description request datagram for" |
508 EC_ERR("Failed to receive CoE Sdo description request datagram for" |
509 " slave %i (datagram state %i).\n", |
509 " slave %i (datagram state %i).\n", |
510 slave->ring_position, datagram->state); |
510 slave->ring_position, datagram->state); |
511 return; |
511 return; |
512 } |
512 } |
513 |
513 |
514 if (datagram->working_counter != 1) { |
514 if (datagram->working_counter != 1) { |
515 fsm->state = ec_fsm_coe_error; |
515 fsm->state = ec_fsm_coe_error; |
516 EC_ERR("Reception of CoE SDO description" |
516 EC_ERR("Reception of CoE Sdo description" |
517 " request failed on slave %i: ", slave->ring_position); |
517 " request failed on slave %i: ", slave->ring_position); |
518 ec_datagram_print_wc_error(datagram); |
518 ec_datagram_print_wc_error(datagram); |
519 return; |
519 return; |
520 } |
520 } |
521 |
521 |
558 |
558 |
559 if (!ec_slave_mbox_check(datagram)) { |
559 if (!ec_slave_mbox_check(datagram)) { |
560 if (datagram->cycles_received |
560 if (datagram->cycles_received |
561 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
561 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
562 fsm->state = ec_fsm_coe_error; |
562 fsm->state = ec_fsm_coe_error; |
563 EC_ERR("Timeout while checking SDO description on slave %i.\n", |
563 EC_ERR("Timeout while checking Sdo description on slave %i.\n", |
564 slave->ring_position); |
564 slave->ring_position); |
565 return; |
565 return; |
566 } |
566 } |
567 |
567 |
568 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
568 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
595 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
595 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
596 return; // FIXME: request again? |
596 return; // FIXME: request again? |
597 |
597 |
598 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
598 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
599 fsm->state = ec_fsm_coe_error; |
599 fsm->state = ec_fsm_coe_error; |
600 EC_ERR("Failed to receive CoE SDO description response datagram from" |
600 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
601 " slave %i (datagram state %i).\n", |
601 " slave %i (datagram state %i).\n", |
602 slave->ring_position, datagram->state); |
602 slave->ring_position, datagram->state); |
603 return; |
603 return; |
604 } |
604 } |
605 |
605 |
606 if (datagram->working_counter != 1) { |
606 if (datagram->working_counter != 1) { |
607 fsm->state = ec_fsm_coe_error; |
607 fsm->state = ec_fsm_coe_error; |
608 EC_ERR("Reception of CoE SDO description" |
608 EC_ERR("Reception of CoE Sdo description" |
609 " response failed on slave %i: ", slave->ring_position); |
609 " response failed on slave %i: ", slave->ring_position); |
610 ec_datagram_print_wc_error(datagram); |
610 ec_datagram_print_wc_error(datagram); |
611 return; |
611 return; |
612 } |
612 } |
613 |
613 |
621 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
621 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
622 fsm->state = ec_fsm_coe_error; |
622 fsm->state = ec_fsm_coe_error; |
623 return; |
623 return; |
624 } |
624 } |
625 |
625 |
626 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
626 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
627 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
627 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
628 EC_ERR("SDO information error response at slave %i while" |
628 EC_ERR("Sdo information error response at slave %i while" |
629 " fetching SDO 0x%04X!\n", slave->ring_position, |
629 " fetching Sdo 0x%04X!\n", slave->ring_position, |
630 sdo->index); |
630 sdo->index); |
631 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
631 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
632 fsm->state = ec_fsm_coe_error; |
632 fsm->state = ec_fsm_coe_error; |
633 return; |
633 return; |
634 } |
634 } |
635 |
635 |
636 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
636 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
637 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
637 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
638 EC_READ_U16(data + 6) != sdo->index) { // SDO index |
638 EC_READ_U16(data + 6) != sdo->index) { // Sdo index |
639 EC_ERR("Invalid object description response at slave %i while" |
639 EC_ERR("Invalid object description response at slave %i while" |
640 " fetching SDO 0x%04X!\n", slave->ring_position, |
640 " fetching Sdo 0x%04X!\n", slave->ring_position, |
641 sdo->index); |
641 sdo->index); |
642 ec_print_data(data, rec_size); |
642 ec_print_data(data, rec_size); |
643 fsm->state = ec_fsm_coe_error; |
643 fsm->state = ec_fsm_coe_error; |
644 return; |
644 return; |
645 } |
645 } |
679 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
679 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
680 fsm->state = ec_fsm_coe_error; |
680 fsm->state = ec_fsm_coe_error; |
681 return; |
681 return; |
682 } |
682 } |
683 |
683 |
684 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
684 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
685 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
685 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
686 EC_WRITE_U8 (data + 3, 0x00); |
686 EC_WRITE_U8 (data + 3, 0x00); |
687 EC_WRITE_U16(data + 4, 0x0000); |
687 EC_WRITE_U16(data + 4, 0x0000); |
688 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
688 EC_WRITE_U16(data + 6, sdo->index); // Sdo index |
689 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
689 EC_WRITE_U8 (data + 8, fsm->subindex); // Sdo subindex |
690 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
690 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
691 |
691 |
692 fsm->retries = EC_FSM_RETRIES; |
692 fsm->retries = EC_FSM_RETRIES; |
693 fsm->state = ec_fsm_coe_dict_entry_request; |
693 fsm->state = ec_fsm_coe_dict_entry_request; |
694 } |
694 } |
709 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
709 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
710 return; // FIXME: check for response first? |
710 return; // FIXME: check for response first? |
711 |
711 |
712 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
712 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
713 fsm->state = ec_fsm_coe_error; |
713 fsm->state = ec_fsm_coe_error; |
714 EC_ERR("Failed to receive CoE SDO entry request datagram for" |
714 EC_ERR("Failed to receive CoE Sdo entry request datagram for" |
715 " slave %i (datagram state %i).\n", |
715 " slave %i (datagram state %i).\n", |
716 slave->ring_position, datagram->state); |
716 slave->ring_position, datagram->state); |
717 return; |
717 return; |
718 } |
718 } |
719 |
719 |
720 if (datagram->working_counter != 1) { |
720 if (datagram->working_counter != 1) { |
721 fsm->state = ec_fsm_coe_error; |
721 fsm->state = ec_fsm_coe_error; |
722 EC_ERR("Reception of CoE SDO entry request failed on slave %i: ", |
722 EC_ERR("Reception of CoE Sdo entry request failed on slave %i: ", |
723 slave->ring_position); |
723 slave->ring_position); |
724 ec_datagram_print_wc_error(datagram); |
724 ec_datagram_print_wc_error(datagram); |
725 return; |
725 return; |
726 } |
726 } |
727 |
727 |
765 |
765 |
766 if (!ec_slave_mbox_check(datagram)) { |
766 if (!ec_slave_mbox_check(datagram)) { |
767 if (datagram->cycles_received |
767 if (datagram->cycles_received |
768 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
768 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
769 fsm->state = ec_fsm_coe_error; |
769 fsm->state = ec_fsm_coe_error; |
770 EC_ERR("Timeout while checking SDO entry on slave %i.\n", |
770 EC_ERR("Timeout while checking Sdo entry on slave %i.\n", |
771 slave->ring_position); |
771 slave->ring_position); |
772 return; |
772 return; |
773 } |
773 } |
774 |
774 |
775 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
775 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
803 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
803 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
804 return; // FIXME: request again? |
804 return; // FIXME: request again? |
805 |
805 |
806 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
806 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
807 fsm->state = ec_fsm_coe_error; |
807 fsm->state = ec_fsm_coe_error; |
808 EC_ERR("Failed to receive CoE SDO description response datagram from" |
808 EC_ERR("Failed to receive CoE Sdo description response datagram from" |
809 " slave %i (datagram state %i).\n", |
809 " slave %i (datagram state %i).\n", |
810 slave->ring_position, datagram->state); |
810 slave->ring_position, datagram->state); |
811 return; |
811 return; |
812 } |
812 } |
813 |
813 |
814 if (datagram->working_counter != 1) { |
814 if (datagram->working_counter != 1) { |
815 fsm->state = ec_fsm_coe_error; |
815 fsm->state = ec_fsm_coe_error; |
816 EC_ERR("Reception of CoE SDO description" |
816 EC_ERR("Reception of CoE Sdo description" |
817 " response failed on slave %i: ", slave->ring_position); |
817 " response failed on slave %i: ", slave->ring_position); |
818 ec_datagram_print_wc_error(datagram); |
818 ec_datagram_print_wc_error(datagram); |
819 return; |
819 return; |
820 } |
820 } |
821 |
821 |
829 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
829 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
830 fsm->state = ec_fsm_coe_error; |
830 fsm->state = ec_fsm_coe_error; |
831 return; |
831 return; |
832 } |
832 } |
833 |
833 |
834 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
834 if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information |
835 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
835 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response |
836 EC_ERR("SDO information error response at slave %i while" |
836 EC_ERR("Sdo information error response at slave %i while" |
837 " fetching SDO entry 0x%04X:%i!\n", slave->ring_position, |
837 " fetching Sdo entry 0x%04X:%i!\n", slave->ring_position, |
838 sdo->index, fsm->subindex); |
838 sdo->index, fsm->subindex); |
839 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
839 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
840 fsm->state = ec_fsm_coe_error; |
840 fsm->state = ec_fsm_coe_error; |
841 return; |
841 return; |
842 } |
842 } |
843 |
843 |
844 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
844 if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information |
845 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
845 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response |
846 EC_READ_U16(data + 6) != sdo->index || // SDO index |
846 EC_READ_U16(data + 6) != sdo->index || // Sdo index |
847 EC_READ_U8(data + 8) != fsm->subindex) { // SDO subindex |
847 EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex |
848 EC_ERR("Invalid entry description response at slave %i while" |
848 EC_ERR("Invalid entry description response at slave %i while" |
849 " fetching SDO entry 0x%04X:%i!\n", slave->ring_position, |
849 " fetching Sdo entry 0x%04X:%i!\n", slave->ring_position, |
850 sdo->index, fsm->subindex); |
850 sdo->index, fsm->subindex); |
851 ec_print_data(data, rec_size); |
851 ec_print_data(data, rec_size); |
852 fsm->state = ec_fsm_coe_error; |
852 fsm->state = ec_fsm_coe_error; |
853 return; |
853 return; |
854 } |
854 } |
898 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
898 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
899 fsm->state = ec_fsm_coe_error; |
899 fsm->state = ec_fsm_coe_error; |
900 return; |
900 return; |
901 } |
901 } |
902 |
902 |
903 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
903 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
904 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
904 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
905 EC_WRITE_U8 (data + 3, 0x00); |
905 EC_WRITE_U8 (data + 3, 0x00); |
906 EC_WRITE_U16(data + 4, 0x0000); |
906 EC_WRITE_U16(data + 4, 0x0000); |
907 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
907 EC_WRITE_U16(data + 6, sdo->index); // Sdo index |
908 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
908 EC_WRITE_U8 (data + 8, fsm->subindex); // Sdo subindex |
909 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
909 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
910 |
910 |
911 fsm->retries = EC_FSM_RETRIES; |
911 fsm->retries = EC_FSM_RETRIES; |
912 fsm->state = ec_fsm_coe_dict_entry_request; |
912 fsm->state = ec_fsm_coe_dict_entry_request; |
913 return; |
913 return; |
914 } |
914 } |
915 |
915 |
916 // another SDO description to fetch? |
916 // another Sdo description to fetch? |
917 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
917 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
918 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
918 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
919 |
919 |
920 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
920 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) { |
921 fsm->state = ec_fsm_coe_error; |
921 fsm->state = ec_fsm_coe_error; |
922 return; |
922 return; |
923 } |
923 } |
924 |
924 |
925 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
925 EC_WRITE_U16(data, 0x8 << 12); // Sdo information |
926 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
926 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
927 EC_WRITE_U8 (data + 3, 0x00); |
927 EC_WRITE_U8 (data + 3, 0x00); |
928 EC_WRITE_U16(data + 4, 0x0000); |
928 EC_WRITE_U16(data + 4, 0x0000); |
929 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
929 EC_WRITE_U16(data + 6, fsm->sdo->index); // Sdo index |
930 |
930 |
931 fsm->retries = EC_FSM_RETRIES; |
931 fsm->retries = EC_FSM_RETRIES; |
932 fsm->state = ec_fsm_coe_dict_desc_request; |
932 fsm->state = ec_fsm_coe_dict_desc_request; |
933 return; |
933 return; |
934 } |
934 } |
950 ec_slave_t *slave = fsm->slave; |
950 ec_slave_t *slave = fsm->slave; |
951 ec_sdo_data_t *sdodata = fsm->sdodata; |
951 ec_sdo_data_t *sdodata = fsm->sdodata; |
952 uint8_t *data; |
952 uint8_t *data; |
953 |
953 |
954 if (fsm->slave->master->debug_level) |
954 if (fsm->slave->master->debug_level) |
955 EC_DBG("Downloading SDO 0x%04X:%i to slave %i.\n", |
955 EC_DBG("Downloading Sdo 0x%04X:%i to slave %i.\n", |
956 sdodata->index, sdodata->subindex, slave->ring_position); |
956 sdodata->index, sdodata->subindex, slave->ring_position); |
957 |
957 |
958 if (slave->sii_rx_mailbox_size < 6 + 10 + sdodata->size) { |
958 if (slave->sii_rx_mailbox_size < 6 + 10 + sdodata->size) { |
959 EC_ERR("SDO fragmenting not supported yet!\n"); |
959 EC_ERR("Sdo fragmenting not supported yet!\n"); |
960 fsm->state = ec_fsm_coe_error; |
960 fsm->state = ec_fsm_coe_error; |
961 return; |
961 return; |
962 } |
962 } |
963 |
963 |
964 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
964 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
965 sdodata->size + 10))) { |
965 sdodata->size + 10))) { |
966 fsm->state = ec_fsm_coe_error; |
966 fsm->state = ec_fsm_coe_error; |
967 return; |
967 return; |
968 } |
968 } |
969 |
969 |
970 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
970 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
971 EC_WRITE_U8 (data + 2, (0x1 // size specified |
971 EC_WRITE_U8 (data + 2, (0x1 // size specified |
972 | 0x1 << 5)); // Download request |
972 | 0x1 << 5)); // Download request |
973 EC_WRITE_U16(data + 3, sdodata->index); |
973 EC_WRITE_U16(data + 3, sdodata->index); |
974 EC_WRITE_U8 (data + 5, sdodata->subindex); |
974 EC_WRITE_U8 (data + 5, sdodata->subindex); |
975 EC_WRITE_U32(data + 6, sdodata->size); |
975 EC_WRITE_U32(data + 6, sdodata->size); |
1049 |
1049 |
1050 if (!ec_slave_mbox_check(datagram)) { |
1050 if (!ec_slave_mbox_check(datagram)) { |
1051 if (datagram->cycles_received |
1051 if (datagram->cycles_received |
1052 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1052 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1053 fsm->state = ec_fsm_coe_error; |
1053 fsm->state = ec_fsm_coe_error; |
1054 EC_ERR("Timeout while checking SDO configuration on slave %i.\n", |
1054 EC_ERR("Timeout while checking Sdo configuration on slave %i.\n", |
1055 slave->ring_position); |
1055 slave->ring_position); |
1056 return; |
1056 return; |
1057 } |
1057 } |
1058 |
1058 |
1059 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1059 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1118 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |
1118 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |
1119 ec_print_data(data, rec_size); |
1119 ec_print_data(data, rec_size); |
1120 return; |
1120 return; |
1121 } |
1121 } |
1122 |
1122 |
1123 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1123 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1124 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1124 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1125 fsm->state = ec_fsm_coe_error; |
1125 fsm->state = ec_fsm_coe_error; |
1126 EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n", |
1126 EC_ERR("Sdo download 0x%04X:%X (%i bytes) aborted on slave %i.\n", |
1127 sdodata->index, sdodata->subindex, sdodata->size, |
1127 sdodata->index, sdodata->subindex, sdodata->size, |
1128 slave->ring_position); |
1128 slave->ring_position); |
1129 if (rec_size < 10) { |
1129 if (rec_size < 10) { |
1130 EC_ERR("Incomplete Abort command:\n"); |
1130 EC_ERR("Incomplete Abort command:\n"); |
1131 ec_print_data(data, rec_size); |
1131 ec_print_data(data, rec_size); |
1133 else |
1133 else |
1134 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1134 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1135 return; |
1135 return; |
1136 } |
1136 } |
1137 |
1137 |
1138 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1138 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1139 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1139 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
1140 EC_READ_U16(data + 3) != sdodata->index || // index |
1140 EC_READ_U16(data + 3) != sdodata->index || // index |
1141 EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex |
1141 EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex |
1142 fsm->state = ec_fsm_coe_error; |
1142 fsm->state = ec_fsm_coe_error; |
1143 EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n", |
1143 EC_ERR("Sdo download 0x%04X:%X (%i bytes) failed:\n", |
1144 sdodata->index, sdodata->subindex, sdodata->size); |
1144 sdodata->index, sdodata->subindex, sdodata->size); |
1145 EC_ERR("Invalid SDO download response at slave %i!\n", |
1145 EC_ERR("Invalid Sdo download response at slave %i!\n", |
1146 slave->ring_position); |
1146 slave->ring_position); |
1147 ec_print_data(data, rec_size); |
1147 ec_print_data(data, rec_size); |
1148 return; |
1148 return; |
1149 } |
1149 } |
1150 |
1150 |
1165 ec_sdo_request_t *request = fsm->request; |
1165 ec_sdo_request_t *request = fsm->request; |
1166 ec_sdo_entry_t *entry = request->entry; |
1166 ec_sdo_entry_t *entry = request->entry; |
1167 uint8_t *data; |
1167 uint8_t *data; |
1168 |
1168 |
1169 if (master->debug_level) |
1169 if (master->debug_level) |
1170 EC_DBG("Uploading SDO 0x%04X:%i from slave %i.\n", |
1170 EC_DBG("Uploading Sdo 0x%04X:%i from slave %i.\n", |
1171 entry->sdo->index, entry->subindex, slave->ring_position); |
1171 entry->sdo->index, entry->subindex, slave->ring_position); |
1172 |
1172 |
1173 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1173 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1174 fsm->state = ec_fsm_coe_error; |
1174 fsm->state = ec_fsm_coe_error; |
1175 return; |
1175 return; |
1176 } |
1176 } |
1177 |
1177 |
1178 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1178 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1179 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1179 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1180 EC_WRITE_U16(data + 3, entry->sdo->index); |
1180 EC_WRITE_U16(data + 3, entry->sdo->index); |
1181 EC_WRITE_U8 (data + 5, entry->subindex); |
1181 EC_WRITE_U8 (data + 5, entry->subindex); |
1182 memset(data + 6, 0x00, 4); |
1182 memset(data + 6, 0x00, 4); |
1183 |
1183 |
1260 |
1260 |
1261 if (!ec_slave_mbox_check(datagram)) { |
1261 if (!ec_slave_mbox_check(datagram)) { |
1262 if (datagram->cycles_received |
1262 if (datagram->cycles_received |
1263 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1263 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1264 fsm->state = ec_fsm_coe_error; |
1264 fsm->state = ec_fsm_coe_error; |
1265 EC_ERR("Timeout while checking SDO upload on slave %i.\n", |
1265 EC_ERR("Timeout while checking Sdo upload on slave %i.\n", |
1266 slave->ring_position); |
1266 slave->ring_position); |
1267 return; |
1267 return; |
1268 } |
1268 } |
1269 |
1269 |
1270 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1270 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1332 fsm->state = ec_fsm_coe_error; |
1332 fsm->state = ec_fsm_coe_error; |
1333 return; |
1333 return; |
1334 } |
1334 } |
1335 |
1335 |
1336 if (rec_size < 3) { |
1336 if (rec_size < 3) { |
1337 EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size); |
1337 EC_ERR("Received currupted Sdo upload response (%u bytes)!\n", rec_size); |
1338 ec_print_data(data, rec_size); |
1338 ec_print_data(data, rec_size); |
1339 fsm->state = ec_fsm_coe_error; |
1339 fsm->state = ec_fsm_coe_error; |
1340 return; |
1340 return; |
1341 } |
1341 } |
1342 |
1342 |
1343 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1343 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1344 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1344 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1345 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
1345 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1346 entry->sdo->index, entry->subindex, slave->ring_position); |
1346 entry->sdo->index, entry->subindex, slave->ring_position); |
1347 if (rec_size >= 10) |
1347 if (rec_size >= 10) |
1348 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1348 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1349 else |
1349 else |
1350 EC_ERR("No abort message.\n"); |
1350 EC_ERR("No abort message.\n"); |
1361 // normal or expedited? |
1361 // normal or expedited? |
1362 expedited = EC_READ_U8(data + 2) & 0x02; |
1362 expedited = EC_READ_U8(data + 2) & 0x02; |
1363 |
1363 |
1364 if (expedited) { |
1364 if (expedited) { |
1365 if (rec_size < 7) { |
1365 if (rec_size < 7) { |
1366 EC_ERR("Received currupted SDO expedited upload" |
1366 EC_ERR("Received currupted Sdo expedited upload" |
1367 " response (only %u bytes)!\n", rec_size); |
1367 " response (only %u bytes)!\n", rec_size); |
1368 ec_print_data(data, rec_size); |
1368 ec_print_data(data, rec_size); |
1369 fsm->state = ec_fsm_coe_error; |
1369 fsm->state = ec_fsm_coe_error; |
1370 return; |
1370 return; |
1371 } |
1371 } |
1372 |
1372 |
1373 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1373 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1374 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1374 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1375 EC_READ_U16(data + 3) != entry->sdo->index || // index |
1375 EC_READ_U16(data + 3) != entry->sdo->index || // index |
1376 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1376 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1377 EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1377 EC_ERR("Sdo upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1378 EC_ERR("Invalid SDO upload response at slave %i!\n", |
1378 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1379 slave->ring_position); |
1379 slave->ring_position); |
1380 ec_print_data(data, rec_size); |
1380 ec_print_data(data, rec_size); |
1381 fsm->state = ec_fsm_coe_error; |
1381 fsm->state = ec_fsm_coe_error; |
1382 return; |
1382 return; |
1383 } |
1383 } |
1388 } else { |
1388 } else { |
1389 complete_size = 4; |
1389 complete_size = 4; |
1390 } |
1390 } |
1391 |
1391 |
1392 if (rec_size < 6 + complete_size) { |
1392 if (rec_size < 6 + complete_size) { |
1393 EC_ERR("Received currupted SDO expedited upload" |
1393 EC_ERR("Received currupted Sdo expedited upload" |
1394 " response (only %u bytes)!\n", rec_size); |
1394 " response (only %u bytes)!\n", rec_size); |
1395 ec_print_data(data, rec_size); |
1395 ec_print_data(data, rec_size); |
1396 fsm->state = ec_fsm_coe_error; |
1396 fsm->state = ec_fsm_coe_error; |
1397 return; |
1397 return; |
1398 } |
1398 } |
1399 |
1399 |
1400 if (!(request->data = (uint8_t *) |
1400 if (!(request->data = (uint8_t *) |
1401 kmalloc(complete_size + 1, GFP_ATOMIC))) { |
1401 kmalloc(complete_size + 1, GFP_ATOMIC))) { |
1402 EC_ERR("Failed to allocate %i bytes of SDO data!\n", |
1402 EC_ERR("Failed to allocate %i bytes of Sdo data!\n", |
1403 complete_size); |
1403 complete_size); |
1404 fsm->state = ec_fsm_coe_error; |
1404 fsm->state = ec_fsm_coe_error; |
1405 return; |
1405 return; |
1406 } |
1406 } |
1407 request->data[complete_size] = 0x00; // just to be sure... |
1407 request->data[complete_size] = 0x00; // just to be sure... |
1409 memcpy(request->data, data + 6, complete_size); |
1409 memcpy(request->data, data + 6, complete_size); |
1410 request->size = complete_size; |
1410 request->size = complete_size; |
1411 |
1411 |
1412 } else { // normal |
1412 } else { // normal |
1413 if (rec_size < 10) { |
1413 if (rec_size < 10) { |
1414 EC_ERR("Received currupted SDO normal upload" |
1414 EC_ERR("Received currupted Sdo normal upload" |
1415 " response (only %u bytes)!\n", rec_size); |
1415 " response (only %u bytes)!\n", rec_size); |
1416 ec_print_data(data, rec_size); |
1416 ec_print_data(data, rec_size); |
1417 fsm->state = ec_fsm_coe_error; |
1417 fsm->state = ec_fsm_coe_error; |
1418 return; |
1418 return; |
1419 } |
1419 } |
1420 |
1420 |
1421 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1421 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1422 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1422 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1423 EC_READ_U16(data + 3) != entry->sdo->index || // index |
1423 EC_READ_U16(data + 3) != entry->sdo->index || // index |
1424 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1424 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1425 EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1425 EC_ERR("Sdo upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1426 EC_ERR("Invalid SDO upload response at slave %i!\n", |
1426 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1427 slave->ring_position); |
1427 slave->ring_position); |
1428 ec_print_data(data, rec_size); |
1428 ec_print_data(data, rec_size); |
1429 fsm->state = ec_fsm_coe_error; |
1429 fsm->state = ec_fsm_coe_error; |
1430 return; |
1430 return; |
1431 } |
1431 } |
1452 memcpy(request->data, data + 10, data_size); |
1452 memcpy(request->data, data + 10, data_size); |
1453 request->size = complete_size; |
1453 request->size = complete_size; |
1454 fsm->toggle = 0; |
1454 fsm->toggle = 0; |
1455 |
1455 |
1456 if (data_size < complete_size) { |
1456 if (data_size < complete_size) { |
1457 EC_WARN("SDO data incomplete (%i / %i).\n", |
1457 EC_WARN("Sdo data incomplete (%i / %i).\n", |
1458 data_size, complete_size); |
1458 data_size, complete_size); |
1459 |
1459 |
1460 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, |
1460 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, |
1461 0x03, 3))) { |
1461 0x03, 3))) { |
1462 fsm->state = ec_fsm_coe_error; |
1462 fsm->state = ec_fsm_coe_error; |
1463 return; |
1463 return; |
1464 } |
1464 } |
1465 |
1465 |
1466 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1466 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1467 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1467 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1468 | 0x3 << 5)); // upload segment request |
1468 | 0x3 << 5)); // upload segment request |
1469 |
1469 |
1470 if (master->debug_level) { |
1470 if (master->debug_level) { |
1471 EC_DBG("Upload segment request:\n"); |
1471 EC_DBG("Upload segment request:\n"); |
1551 |
1551 |
1552 if (!ec_slave_mbox_check(datagram)) { |
1552 if (!ec_slave_mbox_check(datagram)) { |
1553 if (datagram->cycles_received |
1553 if (datagram->cycles_received |
1554 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1554 - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) { |
1555 fsm->state = ec_fsm_coe_error; |
1555 fsm->state = ec_fsm_coe_error; |
1556 EC_ERR("Timeout while checking SDO upload segment on slave %i.\n", |
1556 EC_ERR("Timeout while checking Sdo upload segment on slave %i.\n", |
1557 slave->ring_position); |
1557 slave->ring_position); |
1558 return; |
1558 return; |
1559 } |
1559 } |
1560 |
1560 |
1561 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1561 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1623 fsm->state = ec_fsm_coe_error; |
1623 fsm->state = ec_fsm_coe_error; |
1624 return; |
1624 return; |
1625 } |
1625 } |
1626 |
1626 |
1627 if (rec_size < 10) { |
1627 if (rec_size < 10) { |
1628 EC_ERR("Received currupted SDO upload segment response!\n"); |
1628 EC_ERR("Received currupted Sdo upload segment response!\n"); |
1629 ec_print_data(data, rec_size); |
1629 ec_print_data(data, rec_size); |
1630 fsm->state = ec_fsm_coe_error; |
1630 fsm->state = ec_fsm_coe_error; |
1631 return; |
1631 return; |
1632 } |
1632 } |
1633 |
1633 |
1634 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1634 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1635 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1635 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1636 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
1636 EC_ERR("Sdo upload 0x%04X:%X aborted on slave %i.\n", |
1637 entry->sdo->index, entry->subindex, slave->ring_position); |
1637 entry->sdo->index, entry->subindex, slave->ring_position); |
1638 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1638 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
1639 fsm->state = ec_fsm_coe_error; |
1639 fsm->state = ec_fsm_coe_error; |
1640 return; |
1640 return; |
1641 } |
1641 } |
1642 |
1642 |
1643 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1643 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1644 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1644 EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response |
1645 EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1645 EC_ERR("Sdo upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex); |
1646 EC_ERR("Invalid SDO upload segment response at slave %i!\n", |
1646 EC_ERR("Invalid Sdo upload segment response at slave %i!\n", |
1647 slave->ring_position); |
1647 slave->ring_position); |
1648 ec_print_data(data, rec_size); |
1648 ec_print_data(data, rec_size); |
1649 fsm->state = ec_fsm_coe_error; |
1649 fsm->state = ec_fsm_coe_error; |
1650 return; |
1650 return; |
1651 } |
1651 } |
1653 last_segment = EC_READ_U8(data + 2) & 0x01; |
1653 last_segment = EC_READ_U8(data + 2) & 0x01; |
1654 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1654 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1655 data_size = rec_size - 10; |
1655 data_size = rec_size - 10; |
1656 |
1656 |
1657 if (data_size != seg_size) { |
1657 if (data_size != seg_size) { |
1658 EC_WARN("SDO segment data invalid (%i / %i)" |
1658 EC_WARN("Sdo segment data invalid (%i / %i)" |
1659 " - Fragmenting not implemented.\n", |
1659 " - Fragmenting not implemented.\n", |
1660 data_size, seg_size); |
1660 data_size, seg_size); |
1661 } |
1661 } |
1662 |
1662 |
1663 memcpy(request->data + request->size, data + 10, data_size); |
1663 memcpy(request->data + request->size, data + 10, data_size); |
1669 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) { |
1669 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) { |
1670 fsm->state = ec_fsm_coe_error; |
1670 fsm->state = ec_fsm_coe_error; |
1671 return; |
1671 return; |
1672 } |
1672 } |
1673 |
1673 |
1674 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1674 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1675 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1675 EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle |
1676 | 0x3 << 5)); // upload segment request |
1676 | 0x3 << 5)); // upload segment request |
1677 |
1677 |
1678 if (master->debug_level) { |
1678 if (master->debug_level) { |
1679 EC_DBG("Upload segment request:\n"); |
1679 EC_DBG("Upload segment request:\n"); |