176 // search the first external request to be processed |
176 // search the first external request to be processed |
177 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
177 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
178 |
178 |
179 list_del_init(&request->list); // dequeue |
179 list_del_init(&request->list); // dequeue |
180 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
180 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
181 EC_SLAVE_WARN(slave, "Aborting SDO request," |
181 EC_SLAVE_WARN(slave, "Aborting SDO request %p," |
182 " slave has error flag set.\n"); |
182 " slave has error flag set.\n",request); |
183 request->req.state = EC_INT_REQUEST_FAILURE; |
183 request->req.state = EC_INT_REQUEST_FAILURE; |
|
184 kref_put(&request->refcount,ec_master_sdo_request_release); |
184 wake_up(&slave->sdo_queue); |
185 wake_up(&slave->sdo_queue); |
185 fsm->sdo_request = NULL; |
186 fsm->sdo_request = NULL; |
186 fsm->state = ec_fsm_slave_state_idle; |
187 fsm->state = ec_fsm_slave_state_idle; |
187 return 0; |
188 return 0; |
188 } |
189 } |
189 |
190 |
190 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
191 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
191 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
192 EC_SLAVE_WARN(slave, "Aborting SDO request %p, slave is in INIT.\n",request); |
192 request->req.state = EC_INT_REQUEST_FAILURE; |
193 request->req.state = EC_INT_REQUEST_FAILURE; |
|
194 kref_put(&request->refcount,ec_master_sdo_request_release); |
193 wake_up(&slave->sdo_queue); |
195 wake_up(&slave->sdo_queue); |
194 fsm->sdo_request = NULL; |
196 fsm->sdo_request = NULL; |
195 fsm->state = ec_fsm_slave_state_idle; |
197 fsm->state = ec_fsm_slave_state_idle; |
196 return 0; |
198 return 0; |
197 } |
199 } |
198 |
200 |
199 request->req.state = EC_INT_REQUEST_BUSY; |
201 request->req.state = EC_INT_REQUEST_BUSY; |
200 |
202 |
201 // Found pending SDO request. Execute it! |
203 // Found pending SDO request. Execute it! |
202 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
204 EC_SLAVE_DBG(slave, 1, "Processing SDO request %p...\n",request); |
203 |
205 |
204 // Start SDO transfer |
206 // Start SDO transfer |
205 fsm->sdo_request = &request->req; |
207 fsm->sdo_request = request; |
206 fsm->state = ec_fsm_slave_state_sdo_request; |
208 fsm->state = ec_fsm_slave_state_sdo_request; |
207 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
209 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
208 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
210 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
209 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
211 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
210 return 1; |
212 return 1; |
219 void ec_fsm_slave_state_sdo_request( |
221 void ec_fsm_slave_state_sdo_request( |
220 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
222 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
221 ) |
223 ) |
222 { |
224 { |
223 ec_slave_t *slave = fsm->slave; |
225 ec_slave_t *slave = fsm->slave; |
224 ec_sdo_request_t *request = fsm->sdo_request; |
226 ec_master_sdo_request_t *request = fsm->sdo_request; |
225 |
227 |
226 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
228 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
227 { |
229 { |
228 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
230 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
229 return; |
231 return; |
230 } |
232 } |
231 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
233 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
232 EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); |
234 EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n",request); |
233 request->state = EC_INT_REQUEST_FAILURE; |
235 request->req.state = EC_INT_REQUEST_FAILURE; |
|
236 kref_put(&request->refcount,ec_master_sdo_request_release); |
234 wake_up(&slave->sdo_queue); |
237 wake_up(&slave->sdo_queue); |
235 fsm->sdo_request = NULL; |
238 fsm->sdo_request = NULL; |
236 fsm->state = ec_fsm_slave_state_idle; |
239 fsm->state = ec_fsm_slave_state_idle; |
237 return; |
240 return; |
238 } |
241 } |
239 |
242 |
240 EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); |
243 EC_SLAVE_DBG(slave, 1, "Finished SDO request %p.\n",request); |
241 |
244 |
242 // SDO request finished |
245 // SDO request finished |
243 request->state = EC_INT_REQUEST_SUCCESS; |
246 request->req.state = EC_INT_REQUEST_SUCCESS; |
|
247 kref_put(&request->refcount,ec_master_sdo_request_release); |
244 wake_up(&slave->sdo_queue); |
248 wake_up(&slave->sdo_queue); |
245 |
249 |
246 fsm->sdo_request = NULL; |
250 fsm->sdo_request = NULL; |
247 fsm->state = ec_fsm_slave_state_ready; |
251 fsm->state = ec_fsm_slave_state_ready; |
248 } |
252 } |