1051 { |
1051 { |
1052 ec_datagram_t *datagram = fsm->datagram; |
1052 ec_datagram_t *datagram = fsm->datagram; |
1053 ec_slave_t *slave = fsm->slave; |
1053 ec_slave_t *slave = fsm->slave; |
1054 ec_sdo_request_t *request = fsm->request; |
1054 ec_sdo_request_t *request = fsm->request; |
1055 uint8_t *data; |
1055 uint8_t *data; |
|
1056 uint8_t size; |
1056 |
1057 |
1057 if (fsm->slave->master->debug_level) { |
1058 if (fsm->slave->master->debug_level) { |
1058 EC_DBG("Downloading Sdo 0x%04X:%02X to slave %u.\n", |
1059 EC_DBG("Downloading Sdo 0x%04X:%02X to slave %u.\n", |
1059 request->index, request->subindex, slave->ring_position); |
1060 request->index, request->subindex, slave->ring_position); |
1060 ec_print_data(request->data, request->data_size); |
1061 ec_print_data(request->data, request->data_size); |
1063 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1064 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
1064 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1065 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
1065 fsm->state = ec_fsm_coe_error; |
1066 fsm->state = ec_fsm_coe_error; |
1066 return; |
1067 return; |
1067 } |
1068 } |
1068 |
1069 |
1069 if (slave->sii.rx_mailbox_size < 6 + 10 + request->data_size) { |
1070 if (request->data_size <= 4) { // use expedited transfer type |
1070 EC_ERR("Sdo fragmenting not supported yet!\n"); |
1071 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) { |
1071 fsm->state = ec_fsm_coe_error; |
1072 fsm->state = ec_fsm_coe_error; |
1072 return; |
1073 return; |
1073 } |
1074 } |
1074 |
1075 |
1075 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1076 size = 4 - request->data_size; |
1076 request->data_size + 10))) { |
1077 |
1077 fsm->state = ec_fsm_coe_error; |
1078 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1078 return; |
1079 EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited |
1079 } |
1080 | size << 2 |
1080 |
1081 | 0x1 << 5)); // Download request |
1081 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
1082 EC_WRITE_U16(data + 3, request->index); |
1082 EC_WRITE_U8 (data + 2, (0x1 // size specified |
1083 EC_WRITE_U8 (data + 5, request->subindex); |
1083 | 0x1 << 5)); // Download request |
1084 memcpy(data + 6, request->data, request->data_size); |
1084 EC_WRITE_U16(data + 3, request->index); |
1085 |
1085 EC_WRITE_U8 (data + 5, request->subindex); |
1086 if (slave->master->debug_level) { |
1086 EC_WRITE_U32(data + 6, request->data_size); |
1087 EC_DBG("Expedited download request:\n"); |
1087 memcpy(data + 10, request->data, request->data_size); |
1088 ec_print_data(data, 10 + request->data_size); |
1088 |
1089 } |
1089 if (slave->master->debug_level) { |
1090 } |
1090 EC_DBG("Download request:\n"); |
1091 else { // request->data_size > 4, use normal transfer type |
1091 ec_print_data(data, 10 + request->data_size); |
1092 if (slave->sii.rx_mailbox_size < 6 + 10 + request->data_size) { |
1092 } |
1093 EC_ERR("Sdo fragmenting not supported yet!\n"); |
|
1094 fsm->state = ec_fsm_coe_error; |
|
1095 return; |
|
1096 } |
|
1097 |
|
1098 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
|
1099 request->data_size + 10))) { |
|
1100 fsm->state = ec_fsm_coe_error; |
|
1101 return; |
|
1102 } |
|
1103 |
|
1104 EC_WRITE_U16(data, 0x2 << 12); // Sdo request |
|
1105 EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal |
|
1106 | 0x1 << 5)); // Download request |
|
1107 EC_WRITE_U16(data + 3, request->index); |
|
1108 EC_WRITE_U8 (data + 5, request->subindex); |
|
1109 EC_WRITE_U32(data + 6, request->data_size); |
|
1110 memcpy(data + 10, request->data, request->data_size); |
|
1111 |
|
1112 if (slave->master->debug_level) { |
|
1113 EC_DBG("Normal download request:\n"); |
|
1114 ec_print_data(data, 10 + request->data_size); |
|
1115 } |
|
1116 } |
1093 |
1117 |
1094 fsm->request->jiffies_sent = jiffies; |
1118 fsm->request->jiffies_sent = jiffies; |
1095 fsm->retries = EC_FSM_RETRIES; |
1119 fsm->retries = EC_FSM_RETRIES; |
1096 fsm->state = ec_fsm_coe_down_request; |
1120 fsm->state = ec_fsm_coe_down_request; |
1097 } |
1121 } |