2062 EC_WRITE_U8 (data + 2, (0x1 // size specified |
2062 EC_WRITE_U8 (data + 2, (0x1 // size specified |
2063 | 0x1 << 5)); // Download request |
2063 | 0x1 << 5)); // Download request |
2064 EC_WRITE_U16(data + 3, sdodata->index); |
2064 EC_WRITE_U16(data + 3, sdodata->index); |
2065 EC_WRITE_U8 (data + 5, sdodata->subindex); |
2065 EC_WRITE_U8 (data + 5, sdodata->subindex); |
2066 EC_WRITE_U32(data + 6, sdodata->size); |
2066 EC_WRITE_U32(data + 6, sdodata->size); |
2067 memcpy(data + 6, sdodata->data, sdodata->size); |
2067 memcpy(data + 10, sdodata->data, sdodata->size); |
2068 |
2068 |
2069 ec_master_queue_datagram(fsm->master, datagram); |
2069 ec_master_queue_datagram(fsm->master, datagram); |
2070 fsm->coe_state = ec_fsm_coe_down_request; |
2070 fsm->coe_state = ec_fsm_coe_down_request; |
2071 } |
2071 } |
2072 |
2072 |
2140 |
2140 |
2141 void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */) |
2141 void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */) |
2142 { |
2142 { |
2143 ec_datagram_t *datagram = &fsm->datagram; |
2143 ec_datagram_t *datagram = &fsm->datagram; |
2144 ec_slave_t *slave = fsm->slave; |
2144 ec_slave_t *slave = fsm->slave; |
2145 uint8_t *data; |
2145 uint8_t *data, mbox_type; |
2146 size_t rec_size; |
2146 size_t rec_size; |
2147 ec_sdo_data_t *sdodata = fsm->sdodata; |
2147 ec_sdo_data_t *sdodata = fsm->sdodata; |
2148 |
2148 |
2149 if (datagram->state != EC_DATAGRAM_RECEIVED |
2149 if (datagram->state != EC_DATAGRAM_RECEIVED |
2150 || datagram->working_counter != 1) { |
2150 || datagram->working_counter != 1) { |
2151 fsm->coe_state = ec_fsm_error; |
2151 fsm->coe_state = ec_fsm_error; |
2152 EC_ERR("Reception of CoE download response failed.\n"); |
2152 EC_ERR("Reception of CoE download response failed.\n"); |
2153 return; |
2153 return; |
2154 } |
2154 } |
2155 |
2155 |
2156 if (!(data = ec_slave_mbox_fetch(slave, datagram, 0x03, &rec_size))) { |
2156 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
|
2157 &mbox_type, &rec_size))) { |
2157 fsm->coe_state = ec_fsm_error; |
2158 fsm->coe_state = ec_fsm_error; |
2158 return; |
2159 return; |
|
2160 } |
|
2161 |
|
2162 if (mbox_type != 0x03) { |
|
2163 EC_WARN("Received mailbox protocol 0x%02X as a response." |
|
2164 " Trying again.\n", mbox_type); |
|
2165 fsm->coe_start = get_cycles(); |
|
2166 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
2167 ec_master_queue_datagram(fsm->master, datagram); |
|
2168 fsm->coe_state = ec_fsm_coe_down_check; |
|
2169 return; |
2159 } |
2170 } |
2160 |
2171 |
2161 if (rec_size < 6) { |
2172 if (rec_size < 6) { |
2162 fsm->coe_state = ec_fsm_error; |
2173 fsm->coe_state = ec_fsm_error; |
2163 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |
2174 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |