178 int ec_fsm_slave_action_process_sdo( |
178 int ec_fsm_slave_action_process_sdo( |
179 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
179 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
180 ) |
180 ) |
181 { |
181 { |
182 ec_slave_t *slave = fsm->slave; |
182 ec_slave_t *slave = fsm->slave; |
183 ec_master_sdo_request_t *request, *next; |
183 ec_sdo_request_t *request, *next; |
184 |
184 |
185 // search the first external request to be processed |
185 // search the first external request to be processed |
186 list_for_each_entry_safe(request, next, &slave->sdo_requests, list) { |
186 list_for_each_entry_safe(request, next, &slave->sdo_requests, list) { |
187 |
187 |
188 list_del_init(&request->list); // dequeue |
188 list_del_init(&request->list); // dequeue |
189 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
189 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
190 EC_SLAVE_WARN(slave, "Aborting SDO request," |
190 EC_SLAVE_WARN(slave, "Aborting SDO request," |
191 " slave has error flag set.\n"); |
191 " slave has error flag set.\n"); |
192 request->req.state = EC_INT_REQUEST_FAILURE; |
192 request->state = EC_INT_REQUEST_FAILURE; |
193 wake_up(&slave->sdo_queue); |
193 wake_up(&slave->sdo_queue); |
194 fsm->sdo_request = NULL; |
194 fsm->sdo_request = NULL; |
195 fsm->state = ec_fsm_slave_state_idle; |
195 fsm->state = ec_fsm_slave_state_idle; |
196 return 0; |
196 return 0; |
197 } |
197 } |
198 |
198 |
199 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
199 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
200 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
200 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
201 request->req.state = EC_INT_REQUEST_FAILURE; |
201 request->state = EC_INT_REQUEST_FAILURE; |
202 wake_up(&slave->sdo_queue); |
202 wake_up(&slave->sdo_queue); |
203 fsm->sdo_request = NULL; |
203 fsm->sdo_request = NULL; |
204 fsm->state = ec_fsm_slave_state_idle; |
204 fsm->state = ec_fsm_slave_state_idle; |
205 return 0; |
205 return 0; |
206 } |
206 } |
207 |
207 |
208 request->req.state = EC_INT_REQUEST_BUSY; |
208 request->state = EC_INT_REQUEST_BUSY; |
209 |
209 |
210 // Found pending SDO request. Execute it! |
210 // Found pending SDO request. Execute it! |
211 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
211 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
212 |
212 |
213 // Start SDO transfer |
213 // Start SDO transfer |
214 fsm->sdo_request = &request->req; |
214 fsm->sdo_request = request; |
215 fsm->state = ec_fsm_slave_state_sdo_request; |
215 fsm->state = ec_fsm_slave_state_sdo_request; |
216 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
216 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request); |
217 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
217 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
218 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
218 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
219 return 1; |
219 return 1; |
220 } |
220 } |
221 return 0; |
221 return 0; |