34 /*****************************************************************************/ |
34 /*****************************************************************************/ |
35 |
35 |
36 #include "globals.h" |
36 #include "globals.h" |
37 #include "master.h" |
37 #include "master.h" |
38 #include "mailbox.h" |
38 #include "mailbox.h" |
|
39 #include "slave_config.h" |
39 |
40 |
40 #include "fsm_slave.h" |
41 #include "fsm_slave.h" |
41 |
42 |
42 /*****************************************************************************/ |
43 /*****************************************************************************/ |
43 |
44 |
44 void ec_fsm_slave_state_idle(ec_fsm_slave_t *); |
45 void ec_fsm_slave_state_idle(ec_fsm_slave_t *); |
45 void ec_fsm_slave_state_ready(ec_fsm_slave_t *); |
46 void ec_fsm_slave_state_ready(ec_fsm_slave_t *); |
46 int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *); |
47 int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *); |
47 void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); |
48 void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); |
|
49 int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *); |
|
50 void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *); |
48 int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *); |
51 int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *); |
49 void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *); |
52 void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *); |
50 int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *); |
53 int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *); |
51 void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *); |
54 void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *); |
52 |
55 |
174 { |
181 { |
175 ec_slave_t *slave = fsm->slave; |
182 ec_slave_t *slave = fsm->slave; |
176 ec_master_sdo_request_t *request, *next; |
183 ec_master_sdo_request_t *request, *next; |
177 |
184 |
178 // search the first external request to be processed |
185 // search the first external request to be processed |
179 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
186 list_for_each_entry_safe(request, next, |
|
187 &slave->slave_sdo_requests, list) { |
180 |
188 |
181 list_del_init(&request->list); // dequeue |
189 list_del_init(&request->list); // dequeue |
182 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
190 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
183 EC_SLAVE_WARN(slave, "Aborting SDO request," |
191 EC_SLAVE_WARN(slave, "Aborting SDO request," |
184 " slave has error flag set.\n"); |
192 " slave has error flag set.\n"); |
243 // SDO request finished |
252 // SDO request finished |
244 request->state = EC_INT_REQUEST_SUCCESS; |
253 request->state = EC_INT_REQUEST_SUCCESS; |
245 wake_up(&slave->sdo_queue); |
254 wake_up(&slave->sdo_queue); |
246 |
255 |
247 fsm->sdo_request = NULL; |
256 fsm->sdo_request = NULL; |
|
257 fsm->state = ec_fsm_slave_state_ready; |
|
258 } |
|
259 |
|
260 /*****************************************************************************/ |
|
261 |
|
262 /** Check for pending register requests and process one. |
|
263 * |
|
264 * \return non-zero, if a register request is processed. |
|
265 */ |
|
266 int ec_fsm_slave_action_process_reg( |
|
267 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
|
268 ) |
|
269 { |
|
270 ec_slave_t *slave = fsm->slave; |
|
271 ec_reg_request_t *reg, *next; |
|
272 |
|
273 fsm->reg_request = NULL; |
|
274 |
|
275 if (slave->config) { |
|
276 // search the first internal register request to be processed |
|
277 list_for_each_entry(reg, &slave->config->reg_requests, list) { |
|
278 if (reg->state == EC_INT_REQUEST_QUEUED) { |
|
279 fsm->reg_request = reg; |
|
280 break; |
|
281 } |
|
282 } |
|
283 } |
|
284 |
|
285 if (!fsm->reg_request) { |
|
286 // search the first external request to be processed |
|
287 list_for_each_entry_safe(reg, next, &slave->reg_requests, list) { |
|
288 list_del_init(®->list); // dequeue |
|
289 fsm->reg_request = reg; |
|
290 break; |
|
291 } |
|
292 } |
|
293 |
|
294 if (!fsm->reg_request) { // no register request to process |
|
295 return 0; |
|
296 } |
|
297 |
|
298 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
|
299 EC_SLAVE_WARN(slave, "Aborting register request," |
|
300 " slave has error flag set.\n"); |
|
301 reg->state = EC_INT_REQUEST_FAILURE; |
|
302 wake_up(&slave->reg_queue); |
|
303 fsm->state = ec_fsm_slave_state_idle; |
|
304 return 1; |
|
305 } |
|
306 |
|
307 // Found pending register request. Execute it! |
|
308 EC_SLAVE_DBG(slave, 1, "Processing register request...\n"); |
|
309 |
|
310 reg->state = EC_INT_REQUEST_BUSY; |
|
311 |
|
312 // Start register access |
|
313 if (reg->dir == EC_DIR_INPUT) { |
|
314 ec_datagram_fprd(fsm->datagram, slave->station_address, |
|
315 reg->address, reg->transfer_size); |
|
316 ec_datagram_zero(fsm->datagram); |
|
317 } else { |
|
318 ec_datagram_fpwr(fsm->datagram, slave->station_address, |
|
319 reg->address, reg->transfer_size); |
|
320 memcpy(fsm->datagram->data, reg->data, reg->transfer_size); |
|
321 } |
|
322 fsm->datagram->device_index = slave->device_index; |
|
323 ec_master_queue_external_datagram(slave->master, fsm->datagram); |
|
324 fsm->state = ec_fsm_slave_state_reg_request; |
|
325 return 1; |
|
326 } |
|
327 |
|
328 /*****************************************************************************/ |
|
329 |
|
330 /** Slave state: Register request. |
|
331 */ |
|
332 void ec_fsm_slave_state_reg_request( |
|
333 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
|
334 ) |
|
335 { |
|
336 ec_slave_t *slave = fsm->slave; |
|
337 ec_reg_request_t *reg = fsm->reg_request; |
|
338 |
|
339 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
|
340 EC_SLAVE_ERR(slave, "Failed to receive register" |
|
341 " request datagram: "); |
|
342 ec_datagram_print_state(fsm->datagram); |
|
343 reg->state = EC_INT_REQUEST_FAILURE; |
|
344 wake_up(&slave->reg_queue); |
|
345 fsm->state = ec_fsm_slave_state_ready; |
|
346 return; |
|
347 } |
|
348 |
|
349 if (fsm->datagram->working_counter == 1) { |
|
350 if (reg->dir == EC_DIR_INPUT) { // read request |
|
351 memcpy(reg->data, fsm->datagram->data, reg->transfer_size); |
|
352 } |
|
353 |
|
354 reg->state = EC_INT_REQUEST_SUCCESS; |
|
355 EC_SLAVE_DBG(slave, 1, "Register request successful.\n"); |
|
356 } else { |
|
357 reg->state = EC_INT_REQUEST_FAILURE; |
|
358 ec_datagram_print_state(fsm->datagram); |
|
359 EC_SLAVE_ERR(slave, "Register request failed" |
|
360 " (working counter is %u).\n", |
|
361 fsm->datagram->working_counter); |
|
362 } |
|
363 |
|
364 wake_up(&slave->reg_queue); |
248 fsm->state = ec_fsm_slave_state_ready; |
365 fsm->state = ec_fsm_slave_state_ready; |
249 } |
366 } |
250 |
367 |
251 /*****************************************************************************/ |
368 /*****************************************************************************/ |
252 |
369 |