1072 ec_sdo_request_t *request = fsm->request; |
1072 ec_sdo_request_t *request = fsm->request; |
1073 uint8_t *data; |
1073 uint8_t *data; |
1074 uint8_t size; |
1074 uint8_t size; |
1075 |
1075 |
1076 if (fsm->slave->master->debug_level) { |
1076 if (fsm->slave->master->debug_level) { |
1077 EC_DBG("Downloading SDO 0x%04X:%02X to slave %u.\n", |
1077 char subidxstr[10]; |
1078 request->index, request->subindex, slave->ring_position); |
1078 if (request->complete_access) { |
|
1079 subidxstr[0] = 0x00; |
|
1080 } else { |
|
1081 sprintf(subidxstr, ":%02X", request->subindex); |
|
1082 } |
|
1083 EC_DBG("Downloading SDO 0x%04X%s to slave %u.\n", |
|
1084 request->index, subidxstr, slave->ring_position); |
1079 ec_print_data(request->data, request->data_size); |
1085 ec_print_data(request->data, request->data_size); |
1080 } |
1086 } |
1081 |
1087 |
1082 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1088 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1083 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1089 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1095 size = 4 - request->data_size; |
1101 size = 4 - request->data_size; |
1096 |
1102 |
1097 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1103 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1098 EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited |
1104 EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited |
1099 | size << 2 |
1105 | size << 2 |
|
1106 | ((request->complete_access ? 1 : 0) << 4) |
1100 | 0x1 << 5)); // Download request |
1107 | 0x1 << 5)); // Download request |
1101 EC_WRITE_U16(data + 3, request->index); |
1108 EC_WRITE_U16(data + 3, request->index); |
1102 EC_WRITE_U8 (data + 5, request->subindex); |
1109 EC_WRITE_U8 (data + 5, |
|
1110 request->complete_access ? 0x00 : request->subindex); |
1103 memcpy(data + 6, request->data, request->data_size); |
1111 memcpy(data + 6, request->data, request->data_size); |
1104 memset(data + 6 + request->data_size, 0x00, 4 - request->data_size); |
1112 memset(data + 6 + request->data_size, 0x00, 4 - request->data_size); |
1105 |
1113 |
1106 if (slave->master->debug_level) { |
1114 if (slave->master->debug_level) { |
1107 EC_DBG("Expedited download request:\n"); |
1115 EC_DBG("Expedited download request:\n"); |
1108 ec_print_data(data, 10); |
1116 ec_print_data(data, 10); |
1109 } |
1117 } |
1110 } |
1118 } |
1111 else { // request->data_size > 4, use normal transfer type |
1119 else { // request->data_size > 4, use normal transfer type |
1112 if (slave->configured_rx_mailbox_size < 6 + 10 + request->data_size) { |
1120 if (slave->configured_rx_mailbox_size < 6 + 10 + request->data_size) { |
1113 EC_ERR("SDO fragmenting not supported yet!\n"); |
1121 EC_ERR("SDO fragmenting not supported yet!\n"); // FIXME |
1114 fsm->state = ec_fsm_coe_error; |
1122 fsm->state = ec_fsm_coe_error; |
1115 return; |
1123 return; |
1116 } |
1124 } |
1117 |
1125 |
1118 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1126 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1122 return; |
1130 return; |
1123 } |
1131 } |
1124 |
1132 |
1125 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1133 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1126 EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal |
1134 EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal |
|
1135 | ((request->complete_access ? 1 : 0) << 4) |
1127 | 0x1 << 5)); // Download request |
1136 | 0x1 << 5)); // Download request |
1128 EC_WRITE_U16(data + 3, request->index); |
1137 EC_WRITE_U16(data + 3, request->index); |
1129 EC_WRITE_U8 (data + 5, request->subindex); |
1138 EC_WRITE_U8 (data + 5, |
|
1139 request->complete_access ? 0x00 : request->subindex); |
1130 EC_WRITE_U32(data + 6, request->data_size); |
1140 EC_WRITE_U32(data + 6, request->data_size); |
1131 memcpy(data + 10, request->data, request->data_size); |
1141 memcpy(data + 10, request->data, request->data_size); |
1132 |
1142 |
1133 if (slave->master->debug_level) { |
1143 if (slave->master->debug_level) { |
1134 EC_DBG("Normal download request:\n"); |
1144 EC_DBG("Normal download request:\n"); |
1309 return; |
1319 return; |
1310 } |
1320 } |
1311 |
1321 |
1312 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1322 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
1313 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1323 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
1314 fsm->state = ec_fsm_coe_error; |
1324 char subidxstr[10]; |
1315 EC_ERR("SDO download 0x%04X:%02X (%u bytes) aborted on slave %u.\n", |
1325 fsm->state = ec_fsm_coe_error; |
1316 request->index, request->subindex, request->data_size, |
1326 if (request->complete_access) { |
1317 slave->ring_position); |
1327 subidxstr[0] = 0x00; |
|
1328 } else { |
|
1329 sprintf(subidxstr, ":%02X", request->subindex); |
|
1330 } |
|
1331 EC_ERR("SDO download 0x%04X%s (%u bytes) aborted on slave %u.\n", |
|
1332 request->index, subidxstr, request->data_size, |
|
1333 slave->ring_position); |
1318 if (rec_size < 10) { |
1334 if (rec_size < 10) { |
1319 EC_ERR("Incomplete Abort command:\n"); |
1335 EC_ERR("Incomplete abort command:\n"); |
1320 ec_print_data(data, rec_size); |
1336 ec_print_data(data, rec_size); |
1321 } else { |
1337 } else { |
1322 fsm->request->abort_code = EC_READ_U32(data + 6); |
1338 fsm->request->abort_code = EC_READ_U32(data + 6); |
1323 ec_canopen_abort_msg(fsm->request->abort_code); |
1339 ec_canopen_abort_msg(fsm->request->abort_code); |
1324 } |
1340 } |
1803 ec_slave_t *slave = fsm->slave; |
1819 ec_slave_t *slave = fsm->slave; |
1804 ec_master_t *master = slave->master; |
1820 ec_master_t *master = slave->master; |
1805 uint8_t *data, mbox_prot; |
1821 uint8_t *data, mbox_prot; |
1806 size_t rec_size, data_size; |
1822 size_t rec_size, data_size; |
1807 ec_sdo_request_t *request = fsm->request; |
1823 ec_sdo_request_t *request = fsm->request; |
1808 uint32_t seg_size; |
|
1809 unsigned int last_segment; |
1824 unsigned int last_segment; |
1810 |
1825 |
1811 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1826 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1812 return; // FIXME: request again? |
1827 return; // FIXME: request again? |
1813 |
1828 |
1881 fsm->retries = EC_FSM_RETRIES; |
1896 fsm->retries = EC_FSM_RETRIES; |
1882 fsm->state = ec_fsm_coe_up_seg_check; |
1897 fsm->state = ec_fsm_coe_up_seg_check; |
1883 return; |
1898 return; |
1884 } |
1899 } |
1885 |
1900 |
1886 last_segment = EC_READ_U8(data + 2) & 0x01; |
1901 data_size = rec_size - 3; /* Header of segment upload is smaller than |
1887 seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1902 normal upload */ |
1888 if (rec_size > 10) { |
1903 if (rec_size == 10) { |
1889 data_size = rec_size - 3; /* Header of segment upload is smaller than |
1904 uint8_t seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1; |
1890 normal upload */ |
1905 data_size -= seg_size; |
1891 } else { // == 10 |
|
1892 /* seg_size contains the number of trailing bytes to ignore. */ |
|
1893 data_size = rec_size - seg_size; |
|
1894 } |
1906 } |
1895 |
1907 |
1896 if (request->data_size + data_size > fsm->complete_size) { |
1908 if (request->data_size + data_size > fsm->complete_size) { |
1897 EC_ERR("SDO upload 0x%04X:%02X failed on slave %u: Fragment" |
1909 EC_ERR("SDO upload 0x%04X:%02X failed on slave %u: Fragment" |
1898 " exceeding complete size!\n", |
1910 " exceeding complete size!\n", |
1902 } |
1914 } |
1903 |
1915 |
1904 memcpy(request->data + request->data_size, data + 3, data_size); |
1916 memcpy(request->data + request->data_size, data + 3, data_size); |
1905 request->data_size += data_size; |
1917 request->data_size += data_size; |
1906 |
1918 |
|
1919 last_segment = EC_READ_U8(data + 2) & 0x01; |
1907 if (!last_segment) { |
1920 if (!last_segment) { |
1908 fsm->toggle = !fsm->toggle; |
1921 fsm->toggle = !fsm->toggle; |
1909 ec_fsm_coe_up_prepare_segment_request(fsm); |
1922 ec_fsm_coe_up_prepare_segment_request(fsm); |
1910 fsm->retries = EC_FSM_RETRIES; |
1923 fsm->retries = EC_FSM_RETRIES; |
1911 fsm->state = ec_fsm_coe_up_seg_request; |
1924 fsm->state = ec_fsm_coe_up_seg_request; |