1033 |
1033 |
1034 if (master->debug_level) |
1034 if (master->debug_level) |
1035 EC_DBG("Uploading SDO 0x%04X:%i from slave %i.\n", |
1035 EC_DBG("Uploading SDO 0x%04X:%i from slave %i.\n", |
1036 sdo->index, entry->subindex, slave->ring_position); |
1036 sdo->index, entry->subindex, slave->ring_position); |
1037 |
1037 |
1038 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 6))) { |
1038 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1039 fsm->state = ec_fsm_coe_error; |
1039 fsm->state = ec_fsm_coe_error; |
1040 return; |
1040 return; |
1041 } |
1041 } |
1042 |
1042 |
1043 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1043 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1044 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1044 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1045 EC_WRITE_U16(data + 3, sdo->index); |
1045 EC_WRITE_U16(data + 3, sdo->index); |
1046 EC_WRITE_U8 (data + 5, entry->subindex); |
1046 EC_WRITE_U8 (data + 5, entry->subindex); |
|
1047 memset(data + 6, 0x00, 4); |
1047 |
1048 |
1048 if (master->debug_level) { |
1049 if (master->debug_level) { |
1049 EC_DBG("Upload request:\n"); |
1050 EC_DBG("Upload request:\n"); |
1050 ec_print_data(data, 6); |
1051 ec_print_data(data, 10); |
1051 } |
1052 } |
1052 |
1053 |
1053 ec_master_queue_datagram(fsm->slave->master, datagram); |
1054 ec_master_queue_datagram(fsm->slave->master, datagram); |
1054 fsm->state = ec_fsm_coe_up_request; |
1055 fsm->state = ec_fsm_coe_up_request; |
1055 } |
1056 } |
1175 fsm->state = ec_fsm_coe_error; |
1176 fsm->state = ec_fsm_coe_error; |
1176 return; |
1177 return; |
1177 } |
1178 } |
1178 |
1179 |
1179 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1180 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
1180 EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response |
1181 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1181 EC_READ_U16(data + 3) != sdo->index || // index |
1182 EC_READ_U16(data + 3) != sdo->index || // index |
1182 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1183 EC_READ_U8 (data + 5) != entry->subindex) { // subindex |
1183 EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex); |
1184 EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex); |
1184 EC_ERR("Invalid SDO upload response at slave %i!\n", |
1185 EC_ERR("Invalid SDO upload response at slave %i!\n", |
1185 slave->ring_position); |
1186 slave->ring_position); |
1190 |
1191 |
1191 data_size = rec_size - 10; |
1192 data_size = rec_size - 10; |
1192 expedited = EC_READ_U8(data + 2) & 0x02; |
1193 expedited = EC_READ_U8(data + 2) & 0x02; |
1193 |
1194 |
1194 if (expedited) { |
1195 if (expedited) { |
1195 if (master->debug_level) |
|
1196 EC_WARN("Received expedited response upon normal request!\n"); |
|
1197 |
|
1198 size_specified = EC_READ_U8(data + 2) & 0x01; |
1196 size_specified = EC_READ_U8(data + 2) & 0x01; |
1199 if (size_specified) { |
1197 if (size_specified) { |
1200 complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1198 complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2); |
1201 } |
1199 } |
1202 else { |
1200 else { |