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 } |