166 */ |
166 */ |
167 |
167 |
168 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *fsm /**< slave state machine */) |
168 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *fsm /**< slave state machine */) |
169 { |
169 { |
170 if (fsm->slave->master->debug_level) { |
170 if (fsm->slave->master->debug_level) { |
171 EC_DBG("Configuring slave %i...\n", fsm->slave->ring_position); |
171 EC_DBG("Configuring slave %u...\n", fsm->slave->ring_position); |
172 } |
172 } |
173 |
173 |
174 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); |
174 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); |
175 ec_fsm_change_exec(&fsm->fsm_change); |
175 ec_fsm_change_exec(&fsm->fsm_change); |
176 fsm->state = ec_fsm_slave_config_state_init; |
176 fsm->state = ec_fsm_slave_config_state_init; |
196 fsm->state = ec_fsm_slave_config_state_error; |
196 fsm->state = ec_fsm_slave_config_state_error; |
197 return; |
197 return; |
198 } |
198 } |
199 |
199 |
200 if (master->debug_level) { |
200 if (master->debug_level) { |
201 EC_DBG("Slave %i is now in INIT.\n", slave->ring_position); |
201 EC_DBG("Slave %u is now in INIT.\n", slave->ring_position); |
202 } |
202 } |
203 |
203 |
204 // check and reset CRC fault counters |
204 // check and reset CRC fault counters |
205 //ec_slave_check_crc(slave); |
205 //ec_slave_check_crc(slave); |
206 // TODO: Implement state machine for CRC checking. |
206 // TODO: Implement state machine for CRC checking. |
209 ec_fsm_slave_config_enter_mbox_sync(fsm); |
209 ec_fsm_slave_config_enter_mbox_sync(fsm); |
210 return; |
210 return; |
211 } |
211 } |
212 |
212 |
213 if (master->debug_level) |
213 if (master->debug_level) |
214 EC_DBG("Clearing FMMU configurations of slave %i...\n", |
214 EC_DBG("Clearing FMMU configurations of slave %u...\n", |
215 slave->ring_position); |
215 slave->ring_position); |
216 |
216 |
217 // clear FMMU configurations |
217 // clear FMMU configurations |
218 ec_datagram_fpwr(datagram, slave->station_address, |
218 ec_datagram_fpwr(datagram, slave->station_address, |
219 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
219 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
236 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
236 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
237 return; |
237 return; |
238 |
238 |
239 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
239 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
240 fsm->state = ec_fsm_slave_config_state_error; |
240 fsm->state = ec_fsm_slave_config_state_error; |
241 EC_ERR("Failed receive FMMU clearing datagram for slave %i.\n", |
241 EC_ERR("Failed receive FMMU clearing datagram for slave %u.\n", |
242 fsm->slave->ring_position); |
242 fsm->slave->ring_position); |
243 return; |
243 return; |
244 } |
244 } |
245 |
245 |
246 if (datagram->working_counter != 1) { |
246 if (datagram->working_counter != 1) { |
247 fsm->slave->error_flag = 1; |
247 fsm->slave->error_flag = 1; |
248 fsm->state = ec_fsm_slave_config_state_error; |
248 fsm->state = ec_fsm_slave_config_state_error; |
249 EC_ERR("Failed to clear FMMUs of slave %i: ", |
249 EC_ERR("Failed to clear FMMUs of slave %u: ", |
250 fsm->slave->ring_position); |
250 fsm->slave->ring_position); |
251 ec_datagram_print_wc_error(datagram); |
251 ec_datagram_print_wc_error(datagram); |
252 return; |
252 return; |
253 } |
253 } |
254 |
254 |
272 |
272 |
273 // slave is now in INIT |
273 // slave is now in INIT |
274 if (slave->current_state == slave->requested_state) { |
274 if (slave->current_state == slave->requested_state) { |
275 fsm->state = ec_fsm_slave_config_state_end; // successful |
275 fsm->state = ec_fsm_slave_config_state_end; // successful |
276 if (master->debug_level) { |
276 if (master->debug_level) { |
277 EC_DBG("Finished configuration of slave %i.\n", |
277 EC_DBG("Finished configuration of slave %u.\n", |
278 slave->ring_position); |
278 slave->ring_position); |
279 } |
279 } |
280 return; |
280 return; |
281 } |
281 } |
282 |
282 |
288 ec_fsm_slave_config_enter_preop(fsm); |
288 ec_fsm_slave_config_enter_preop(fsm); |
289 return; |
289 return; |
290 } |
290 } |
291 |
291 |
292 if (master->debug_level) { |
292 if (master->debug_level) { |
293 EC_DBG("Configuring mailbox sync managers of slave %i.\n", |
293 EC_DBG("Configuring mailbox sync managers of slave %u.\n", |
294 slave->ring_position); |
294 slave->ring_position); |
295 } |
295 } |
296 |
296 |
297 if (slave->sii.sync_count >= 2) { // mailbox configuration provided |
297 if (slave->sii.sync_count >= 2) { // mailbox configuration provided |
298 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
298 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
350 return; |
350 return; |
351 |
351 |
352 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
352 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
353 fsm->state = ec_fsm_slave_config_state_error; |
353 fsm->state = ec_fsm_slave_config_state_error; |
354 EC_ERR("Failed to receive sync manager configuration datagram for" |
354 EC_ERR("Failed to receive sync manager configuration datagram for" |
355 " slave %i (datagram state %i).\n", |
355 " slave %u (datagram state %u).\n", |
356 slave->ring_position, datagram->state); |
356 slave->ring_position, datagram->state); |
357 return; |
357 return; |
358 } |
358 } |
359 |
359 |
360 if (datagram->working_counter != 1) { |
360 if (datagram->working_counter != 1) { |
361 slave->error_flag = 1; |
361 slave->error_flag = 1; |
362 fsm->state = ec_fsm_slave_config_state_error; |
362 fsm->state = ec_fsm_slave_config_state_error; |
363 EC_ERR("Failed to set sync managers of slave %i: ", |
363 EC_ERR("Failed to set sync managers of slave %u: ", |
364 slave->ring_position); |
364 slave->ring_position); |
365 ec_datagram_print_wc_error(datagram); |
365 ec_datagram_print_wc_error(datagram); |
366 return; |
366 return; |
367 } |
367 } |
368 |
368 |
404 |
404 |
405 // slave is now in PREOP |
405 // slave is now in PREOP |
406 slave->jiffies_preop = fsm->datagram->jiffies_received; |
406 slave->jiffies_preop = fsm->datagram->jiffies_received; |
407 |
407 |
408 if (master->debug_level) { |
408 if (master->debug_level) { |
409 EC_DBG("Slave %i is now in PREOP.\n", slave->ring_position); |
409 EC_DBG("Slave %u is now in PREOP.\n", slave->ring_position); |
410 } |
410 } |
411 |
411 |
412 if (slave->current_state == slave->requested_state) { |
412 if (slave->current_state == slave->requested_state) { |
413 fsm->state = ec_fsm_slave_config_state_end; // successful |
413 fsm->state = ec_fsm_slave_config_state_end; // successful |
414 if (master->debug_level) { |
414 if (master->debug_level) { |
549 return; |
549 return; |
550 |
550 |
551 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
551 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
552 fsm->state = ec_fsm_slave_config_state_error; |
552 fsm->state = ec_fsm_slave_config_state_error; |
553 EC_ERR("Failed to receive process data sync manager configuration" |
553 EC_ERR("Failed to receive process data sync manager configuration" |
554 " datagram for slave %i (datagram state %i).\n", |
554 " datagram for slave %u (datagram state %u).\n", |
555 slave->ring_position, datagram->state); |
555 slave->ring_position, datagram->state); |
556 return; |
556 return; |
557 } |
557 } |
558 |
558 |
559 if (datagram->working_counter != 1) { |
559 if (datagram->working_counter != 1) { |
560 slave->error_flag = 1; |
560 slave->error_flag = 1; |
561 fsm->state = ec_fsm_slave_config_state_error; |
561 fsm->state = ec_fsm_slave_config_state_error; |
562 EC_ERR("Failed to set process data sync managers of slave %i: ", |
562 EC_ERR("Failed to set process data sync managers of slave %u: ", |
563 slave->ring_position); |
563 slave->ring_position); |
564 ec_datagram_print_wc_error(datagram); |
564 ec_datagram_print_wc_error(datagram); |
565 return; |
565 return; |
566 } |
566 } |
567 |
567 |
683 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
683 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
684 return; |
684 return; |
685 |
685 |
686 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
686 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
687 fsm->state = ec_fsm_slave_config_state_error; |
687 fsm->state = ec_fsm_slave_config_state_error; |
688 EC_ERR("Failed to receive FMMUs datagram for slave %i" |
688 EC_ERR("Failed to receive FMMUs datagram for slave %u" |
689 " (datagram state %i).\n", |
689 " (datagram state %u).\n", |
690 slave->ring_position, datagram->state); |
690 slave->ring_position, datagram->state); |
691 return; |
691 return; |
692 } |
692 } |
693 |
693 |
694 if (datagram->working_counter != 1) { |
694 if (datagram->working_counter != 1) { |
695 slave->error_flag = 1; |
695 slave->error_flag = 1; |
696 fsm->state = ec_fsm_slave_config_state_error; |
696 fsm->state = ec_fsm_slave_config_state_error; |
697 EC_ERR("Failed to set FMMUs of slave %i: ", |
697 EC_ERR("Failed to set FMMUs of slave %u: ", |
698 slave->ring_position); |
698 slave->ring_position); |
699 ec_datagram_print_wc_error(datagram); |
699 ec_datagram_print_wc_error(datagram); |
700 return; |
700 return; |
701 } |
701 } |
702 |
702 |
737 } |
737 } |
738 |
738 |
739 // slave is now in SAFEOP |
739 // slave is now in SAFEOP |
740 |
740 |
741 if (master->debug_level) { |
741 if (master->debug_level) { |
742 EC_DBG("Slave %i is now in SAFEOP.\n", slave->ring_position); |
742 EC_DBG("Slave %u is now in SAFEOP.\n", slave->ring_position); |
743 } |
743 } |
744 |
744 |
745 if (fsm->slave->current_state == fsm->slave->requested_state) { |
745 if (fsm->slave->current_state == fsm->slave->requested_state) { |
746 fsm->state = ec_fsm_slave_config_state_end; // successful |
746 fsm->state = ec_fsm_slave_config_state_end; // successful |
747 if (master->debug_level) { |
747 if (master->debug_level) { |
748 EC_DBG("Finished configuration of slave %i.\n", |
748 EC_DBG("Finished configuration of slave %u.\n", |
749 slave->ring_position); |
749 slave->ring_position); |
750 } |
750 } |
751 return; |
751 return; |
752 } |
752 } |
753 |
753 |
778 } |
778 } |
779 |
779 |
780 // slave is now in OP |
780 // slave is now in OP |
781 |
781 |
782 if (master->debug_level) { |
782 if (master->debug_level) { |
783 EC_DBG("Slave %i is now in OP.\n", slave->ring_position); |
783 EC_DBG("Slave %u is now in OP.\n", slave->ring_position); |
784 EC_DBG("Finished configuration of slave %i.\n", slave->ring_position); |
784 EC_DBG("Finished configuration of slave %u.\n", slave->ring_position); |
785 } |
785 } |
786 |
786 |
787 fsm->state = ec_fsm_slave_config_state_end; // successful |
787 fsm->state = ec_fsm_slave_config_state_end; // successful |
788 } |
788 } |
789 |
789 |