equal
deleted
inserted
replaced
266 EC_SLAVE_WARN(fsm->slave, "Received incomplete CoE Emergency" |
266 EC_SLAVE_WARN(fsm->slave, "Received incomplete CoE Emergency" |
267 " request:\n"); |
267 " request:\n"); |
268 ec_print_data(data, size); |
268 ec_print_data(data, size); |
269 return 1; |
269 return 1; |
270 } |
270 } |
271 |
271 |
272 EC_SLAVE_WARN(fsm->slave, "CoE Emergency Request received:\n" |
272 EC_SLAVE_WARN(fsm->slave, "CoE Emergency Request received:\n" |
273 "Error code 0x%04X, Error register 0x%02X, data:\n", |
273 "Error code 0x%04X, Error register 0x%02X, data:\n", |
274 EC_READ_U16(data + 2), EC_READ_U8(data + 4)); |
274 EC_READ_U16(data + 2), EC_READ_U8(data + 4)); |
275 ec_print_data(data + 5, 5); |
275 ec_print_data(data + 5, 5); |
276 return 1; |
276 return 1; |
420 uint8_t *data, mbox_prot; |
420 uint8_t *data, mbox_prot; |
421 size_t rec_size; |
421 size_t rec_size; |
422 unsigned int sdo_count, i; |
422 unsigned int sdo_count, i; |
423 uint16_t sdo_index, fragments_left; |
423 uint16_t sdo_index, fragments_left; |
424 ec_sdo_t *sdo; |
424 ec_sdo_t *sdo; |
|
425 bool first_segment; |
|
426 size_t index_list_offset; |
425 |
427 |
426 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
428 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
427 return; // FIXME: request again? |
429 return; // FIXME: request again? |
428 |
430 |
429 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
431 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
494 fsm->retries = EC_FSM_RETRIES; |
496 fsm->retries = EC_FSM_RETRIES; |
495 fsm->state = ec_fsm_coe_dict_check; |
497 fsm->state = ec_fsm_coe_dict_check; |
496 return; |
498 return; |
497 } |
499 } |
498 |
500 |
499 if (rec_size < 8 || rec_size % 2) { |
501 first_segment = list_empty(&slave->sdo_dictionary) ? true : false; |
|
502 index_list_offset = first_segment ? 8 : 6; |
|
503 |
|
504 if (rec_size < index_list_offset || rec_size % 2) { |
500 EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size); |
505 EC_SLAVE_ERR(slave, "Invalid data size %zu!\n", rec_size); |
501 ec_print_data(data, rec_size); |
506 ec_print_data(data, rec_size); |
502 fsm->state = ec_fsm_coe_error; |
507 fsm->state = ec_fsm_coe_error; |
503 return; |
508 return; |
504 } |
509 } |
505 |
510 |
506 sdo_count = (rec_size - 8) / 2; |
511 sdo_count = (rec_size - index_list_offset) / 2; |
507 |
512 |
508 for (i = 0; i < sdo_count; i++) { |
513 for (i = 0; i < sdo_count; i++) { |
509 sdo_index = EC_READ_U16(data + 8 + i * 2); |
514 sdo_index = EC_READ_U16(data + index_list_offset + i * 2); |
510 if (!sdo_index) { |
515 if (!sdo_index) { |
511 EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n"); |
516 EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n"); |
512 continue; |
517 continue; |
513 } |
518 } |
514 |
519 |
1106 request->errno = EPROTONOSUPPORT; |
1111 request->errno = EPROTONOSUPPORT; |
1107 fsm->state = ec_fsm_coe_error; |
1112 fsm->state = ec_fsm_coe_error; |
1108 return; |
1113 return; |
1109 } |
1114 } |
1110 |
1115 |
1111 if (slave->configured_rx_mailbox_size < |
1116 if (slave->configured_rx_mailbox_size < |
1112 EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) { |
1117 EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) { |
1113 EC_SLAVE_ERR(slave, "Mailbox too small!\n"); |
1118 EC_SLAVE_ERR(slave, "Mailbox too small!\n"); |
1114 request->errno = EOVERFLOW; |
1119 request->errno = EOVERFLOW; |
1115 fsm->state = ec_fsm_coe_error; |
1120 fsm->state = ec_fsm_coe_error; |
1116 return; |
1121 return; |
1130 data_set_size = 4 - request->data_size; |
1135 data_set_size = 4 - request->data_size; |
1131 |
1136 |
1132 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1137 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1133 EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited |
1138 EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited |
1134 | data_set_size << 2 |
1139 | data_set_size << 2 |
1135 | ((request->complete_access ? 1 : 0) << 4) |
1140 | ((request->complete_access ? 1 : 0) << 4) |
1136 | 0x1 << 5)); // Download request |
1141 | 0x1 << 5)); // Download request |
1137 EC_WRITE_U16(data + 3, request->index); |
1142 EC_WRITE_U16(data + 3, request->index); |
1138 EC_WRITE_U8 (data + 5, |
1143 EC_WRITE_U8 (data + 5, |
1139 request->complete_access ? 0x00 : request->subindex); |
1144 request->complete_access ? 0x00 : request->subindex); |
1140 memcpy(data + 6, request->data, request->data_size); |
1145 memcpy(data + 6, request->data, request->data_size); |
1171 fsm->remaining = request->data_size; |
1176 fsm->remaining = request->data_size; |
1172 |
1177 |
1173 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1178 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1174 EC_WRITE_U8(data + 2, |
1179 EC_WRITE_U8(data + 2, |
1175 0x1 // size indicator, normal |
1180 0x1 // size indicator, normal |
1176 | ((request->complete_access ? 1 : 0) << 4) |
1181 | ((request->complete_access ? 1 : 0) << 4) |
1177 | 0x1 << 5); // Download request |
1182 | 0x1 << 5); // Download request |
1178 EC_WRITE_U16(data + 3, request->index); |
1183 EC_WRITE_U16(data + 3, request->index); |
1179 EC_WRITE_U8 (data + 5, |
1184 EC_WRITE_U8 (data + 5, |
1180 request->complete_access ? 0x00 : request->subindex); |
1185 request->complete_access ? 0x00 : request->subindex); |
1181 EC_WRITE_U32(data + 6, request->data_size); |
1186 EC_WRITE_U32(data + 6, request->data_size); |
1357 return; |
1362 return; |
1358 } |
1363 } |
1359 |
1364 |
1360 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1365 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1361 EC_WRITE_U8(data + 2, (last_segment ? 1 : 0) |
1366 EC_WRITE_U8(data + 2, (last_segment ? 1 : 0) |
1362 | (seg_data_size << 1) |
1367 | (seg_data_size << 1) |
1363 | (fsm->toggle << 4) |
1368 | (fsm->toggle << 4) |
1364 | (0x00 << 5)); // Download segment request |
1369 | (0x00 << 5)); // Download segment request |
1365 memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE, |
1370 memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE, |
1366 request->data + fsm->offset, segment_size); |
1371 request->data + fsm->offset, segment_size); |
1367 if (segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) { |
1372 if (segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) { |