188 ec_master_t *master = fsm->master; |
188 ec_master_t *master = fsm->master; |
189 |
189 |
190 if (datagram->state == EC_DATAGRAM_TIMED_OUT) |
190 if (datagram->state == EC_DATAGRAM_TIMED_OUT) |
191 return; // always retry |
191 return; // always retry |
192 |
192 |
193 if (datagram->state != EC_DATAGRAM_RECEIVED) { // link is down |
|
194 fsm->slaves_responding = 0; |
|
195 list_for_each_entry(slave, &master->slaves, list) { |
|
196 ec_slave_set_online_state(slave, EC_SLAVE_OFFLINE); |
|
197 } |
|
198 fsm->state = ec_fsm_master_state_error; |
|
199 return; |
|
200 } |
|
201 |
|
202 // bus topology change? |
193 // bus topology change? |
203 if (datagram->working_counter != fsm->slaves_responding) { |
194 if (datagram->working_counter != fsm->slaves_responding) { |
204 fsm->topology_change_pending = 1; |
195 fsm->topology_change_pending = 1; |
205 fsm->slaves_responding = datagram->working_counter; |
196 fsm->slaves_responding = datagram->working_counter; |
206 |
197 EC_INFO("%u slave(s) responding.\n", fsm->slaves_responding); |
207 EC_INFO("%u slave%s responding.\n", |
198 } |
208 fsm->slaves_responding, |
199 |
209 fsm->slaves_responding == 1 ? "" : "s"); |
200 if (datagram->state != EC_DATAGRAM_RECEIVED) { // link is down |
|
201 fsm->state = ec_fsm_master_state_error; |
|
202 return; |
210 } |
203 } |
211 |
204 |
212 // slave states changed? |
205 // slave states changed? |
213 if (EC_READ_U8(datagram->data) != fsm->slave_states) { |
206 if (EC_READ_U8(datagram->data) != fsm->slave_states) { |
214 char states[EC_STATE_STRING_SIZE]; |
207 char states[EC_STATE_STRING_SIZE]; |
302 ec_fsm_master_t *fsm /**< master state machine */ |
295 ec_fsm_master_t *fsm /**< master state machine */ |
303 ) |
296 ) |
304 { |
297 { |
305 ec_master_t *master = fsm->master; |
298 ec_master_t *master = fsm->master; |
306 ec_sii_write_request_t *request; |
299 ec_sii_write_request_t *request; |
307 ec_slave_t *slave; |
|
308 |
300 |
309 // search the first request to be processed |
301 // search the first request to be processed |
310 while (1) { |
302 while (1) { |
311 down(&master->sii_sem); |
303 down(&master->sii_sem); |
312 if (list_empty(&master->sii_requests)) { |
304 if (list_empty(&master->sii_requests)) { |
318 ec_sii_write_request_t, list); |
310 ec_sii_write_request_t, list); |
319 list_del_init(&request->list); // dequeue |
311 list_del_init(&request->list); // dequeue |
320 request->state = EC_REQUEST_BUSY; |
312 request->state = EC_REQUEST_BUSY; |
321 up(&master->sii_sem); |
313 up(&master->sii_sem); |
322 |
314 |
323 slave = request->slave; |
|
324 if (slave->online_state == EC_SLAVE_OFFLINE) { |
|
325 EC_ERR("Discarding SII data, slave %i offline.\n", |
|
326 slave->ring_position); |
|
327 request->state = EC_REQUEST_FAILURE; |
|
328 wake_up(&master->sii_queue); |
|
329 continue; |
|
330 } |
|
331 |
|
332 // found pending SII write operation. execute it! |
315 // found pending SII write operation. execute it! |
333 if (master->debug_level) |
316 if (master->debug_level) |
334 EC_DBG("Writing SII data to slave %i...\n", |
317 EC_DBG("Writing SII data to slave %u...\n", |
335 slave->ring_position); |
318 request->slave->ring_position); |
336 fsm->sii_request = request; |
319 fsm->sii_request = request; |
337 fsm->sii_index = 0; |
320 fsm->sii_index = 0; |
338 ec_fsm_sii_write(&fsm->fsm_sii, request->slave, request->word_offset, |
321 ec_fsm_sii_write(&fsm->fsm_sii, request->slave, request->word_offset, |
339 request->data, EC_FSM_SII_USE_CONFIGURED_ADDRESS); |
322 request->data, EC_FSM_SII_USE_CONFIGURED_ADDRESS); |
340 fsm->state = ec_fsm_master_state_write_sii; |
323 fsm->state = ec_fsm_master_state_write_sii; |
412 request->req.state = EC_REQUEST_BUSY; |
395 request->req.state = EC_REQUEST_BUSY; |
413 up(&master->sdo_sem); |
396 up(&master->sdo_sem); |
414 |
397 |
415 slave = request->slave; |
398 slave = request->slave; |
416 if (slave->current_state == EC_SLAVE_STATE_INIT || |
399 if (slave->current_state == EC_SLAVE_STATE_INIT || |
417 slave->online_state == EC_SLAVE_OFFLINE || |
|
418 slave->error_flag) { |
400 slave->error_flag) { |
419 EC_ERR("Discarding Sdo request, slave %u not ready.\n", |
401 EC_ERR("Discarding Sdo request, slave %u not ready.\n", |
420 slave->ring_position); |
402 slave->ring_position); |
421 request->req.state = EC_REQUEST_FAILURE; |
403 request->req.state = EC_REQUEST_FAILURE; |
422 wake_up(&master->sdo_queue); |
404 wake_up(&master->sdo_queue); |
423 continue; |
405 continue; |
424 } |
406 } |
425 |
407 |
426 // Found pending Sdo request. Execute it! |
408 // Found pending Sdo request. Execute it! |
427 if (master->debug_level) |
409 if (master->debug_level) |
428 EC_DBG("Processing Sdo request for slave %i...\n", |
410 EC_DBG("Processing Sdo request for slave %u...\n", |
429 slave->ring_position); |
411 slave->ring_position); |
430 |
412 |
431 // Start uploading Sdo |
413 // Start uploading Sdo |
432 fsm->idle = 0; |
414 fsm->idle = 0; |
433 fsm->sdo_request = &request->req; |
415 fsm->sdo_request = &request->req; |
457 |
439 |
458 // check if any slaves are not in the state, they're supposed to be |
440 // check if any slaves are not in the state, they're supposed to be |
459 // FIXME do not check all slaves in every cycle... |
441 // FIXME do not check all slaves in every cycle... |
460 list_for_each_entry(slave, &master->slaves, list) { |
442 list_for_each_entry(slave, &master->slaves, list) { |
461 if (slave->error_flag |
443 if (slave->error_flag |
462 || slave->online_state == EC_SLAVE_OFFLINE |
|
463 || slave->requested_state == EC_SLAVE_STATE_UNKNOWN |
444 || slave->requested_state == EC_SLAVE_STATE_UNKNOWN |
464 || (slave->current_state == slave->requested_state |
445 || (slave->current_state == slave->requested_state |
465 && slave->self_configured)) continue; |
446 && slave->self_configured)) continue; |
466 |
447 |
467 if (master->debug_level) { |
448 if (master->debug_level) { |
527 list_for_each_entry(slave, &master->slaves, list) { |
508 list_for_each_entry(slave, &master->slaves, list) { |
528 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE) |
509 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE) |
529 || slave->sdo_dictionary_fetched |
510 || slave->sdo_dictionary_fetched |
530 || slave->current_state == EC_SLAVE_STATE_INIT |
511 || slave->current_state == EC_SLAVE_STATE_INIT |
531 || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ |
512 || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ |
532 || slave->online_state == EC_SLAVE_OFFLINE |
|
533 || slave->error_flag) continue; |
513 || slave->error_flag) continue; |
534 |
514 |
535 if (master->debug_level) { |
515 if (master->debug_level) { |
536 EC_DBG("Fetching Sdo dictionary from slave %u.\n", |
516 EC_DBG("Fetching Sdo dictionary from slave %u.\n", |
537 slave->ring_position); |
517 slave->ring_position); |
606 fsm->state = ec_fsm_master_state_error; |
586 fsm->state = ec_fsm_master_state_error; |
607 return; |
587 return; |
608 } |
588 } |
609 |
589 |
610 // did the slave not respond to its station address? |
590 // did the slave not respond to its station address? |
611 if (datagram->working_counter == 0) { |
591 if (datagram->working_counter != 1) { |
612 ec_slave_set_online_state(slave, EC_SLAVE_OFFLINE); |
592 if (!slave->error_flag) { |
613 ec_fsm_master_action_next_slave_state(fsm); |
593 slave->error_flag = 1; |
614 return; |
594 if (fsm->master->debug_level) |
615 } |
595 EC_DBG("Slave %u did not respond to state query.\n", |
616 |
596 fsm->slave->ring_position); |
617 // FIXME what to to on multiple response? |
597 } |
618 |
598 fsm->topology_change_pending = 1; |
619 // slave responded |
599 fsm->state = ec_fsm_master_state_error; |
|
600 return; |
|
601 } |
|
602 |
|
603 // a single slave responded |
620 ec_slave_set_state(slave, EC_READ_U8(datagram->data)); // set app state first |
604 ec_slave_set_state(slave, EC_READ_U8(datagram->data)); // set app state first |
621 ec_slave_set_online_state(slave, EC_SLAVE_ONLINE); |
|
622 |
605 |
623 // check, if new slave state has to be acknowledged |
606 // check, if new slave state has to be acknowledged |
624 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR && !slave->error_flag) { |
607 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR && !slave->error_flag) { |
625 fsm->idle = 0; |
608 fsm->idle = 0; |
626 fsm->state = ec_fsm_master_state_acknowledge; |
609 fsm->state = ec_fsm_master_state_acknowledge; |