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_prot; |
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_prot, &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_prot != 0x03) { // CoE |
|
2163 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
|
2164 fsm->coe_state = ec_fsm_error; |
|
2165 return; |
2159 } |
2166 } |
2160 |
2167 |
2161 if (rec_size < 6) { |
2168 if (rec_size < 6) { |
2162 fsm->coe_state = ec_fsm_error; |
2169 fsm->coe_state = ec_fsm_error; |
2163 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |
2170 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |