202 EC_WRITE_U16(data, slave->station_address); |
202 EC_WRITE_U16(data, slave->station_address); |
203 |
203 |
204 ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010, |
204 ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010, |
205 sizeof(uint16_t), data); |
205 sizeof(uint16_t), data); |
206 |
206 |
207 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
207 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
208 |
208 EC_ERR("Writing station address failed on slave %i!\n", i); |
209 if (unlikely(frame.working_counter != 1)) { |
|
210 EC_ERR("Slave %i did not repond while writing station address!\n", |
|
211 i); |
|
212 return -1; |
209 return -1; |
213 } |
210 } |
214 |
211 |
215 // Fetch all slave information |
212 // Fetch all slave information |
216 if (ec_slave_fetch(slave)) return -1; |
213 if (ec_slave_fetch(slave)) return -1; |
487 // Resetting FMMU's |
484 // Resetting FMMU's |
488 if (slave->base_fmmu_count) { |
485 if (slave->base_fmmu_count) { |
489 memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
486 memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
490 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600, |
487 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600, |
491 EC_FMMU_SIZE * slave->base_fmmu_count, data); |
488 EC_FMMU_SIZE * slave->base_fmmu_count, data); |
492 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
489 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
493 if (unlikely(frame.working_counter != 1)) { |
490 EC_ERR("Resetting FMMUs failed on slave %i!\n", |
494 EC_ERR("Resetting FMMUs - Slave %i did not respond!\n", |
|
495 slave->ring_position); |
491 slave->ring_position); |
496 return -1; |
492 return -1; |
497 } |
493 } |
498 } |
494 } |
499 |
495 |
500 // Resetting Sync Manager channels |
496 // Resetting Sync Manager channels |
501 if (slave->base_sync_count) { |
497 if (slave->base_sync_count) { |
502 memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
498 memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
503 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800, |
499 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800, |
504 EC_SYNC_SIZE * slave->base_sync_count, data); |
500 EC_SYNC_SIZE * slave->base_sync_count, data); |
505 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
501 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
506 if (unlikely(frame.working_counter != 1)) { |
502 EC_ERR("Resetting sync managers failed on slave %i!\n", |
507 EC_ERR("Resetting SMs - Slave %i did not respond!\n", |
|
508 slave->ring_position); |
503 slave->ring_position); |
509 return -1; |
504 return -1; |
510 } |
505 } |
511 } |
506 } |
512 |
507 |
517 |
512 |
518 ec_sync_config(sync, data); |
513 ec_sync_config(sync, data); |
519 ec_frame_init_npwr(&frame, master, slave->station_address, |
514 ec_frame_init_npwr(&frame, master, slave->station_address, |
520 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE, data); |
515 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE, data); |
521 |
516 |
522 if (unlikely(ec_frame_send_receive(&frame))) return -1; |
517 if (unlikely(ec_frame_send_receive(&frame))) { |
523 |
518 EC_ERR("Setting sync manager %i failed on slave %i!\n", |
524 if (unlikely(frame.working_counter != 1)) { |
|
525 EC_ERR("Setting sync manager %i - Slave %i did not respond!\n", |
|
526 j, slave->ring_position); |
519 j, slave->ring_position); |
527 return -1; |
520 return -1; |
528 } |
521 } |
529 } |
522 } |
530 |
523 |
546 |
539 |
547 ec_fmmu_config(fmmu, data); |
540 ec_fmmu_config(fmmu, data); |
548 ec_frame_init_npwr(&frame, master, slave->station_address, |
541 ec_frame_init_npwr(&frame, master, slave->station_address, |
549 0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE, data); |
542 0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE, data); |
550 |
543 |
551 if (unlikely(ec_frame_send_receive(&frame))) return -1; |
544 if (unlikely(ec_frame_send_receive(&frame))) { |
552 |
545 EC_ERR("Setting FMMU %i failed on slave %i!\n", |
553 if (unlikely(frame.working_counter != 1)) { |
546 j, slave->ring_position); |
554 EC_ERR("Setting FMMU %i - Slave %i did not respond!\n", j, |
|
555 slave->ring_position); |
|
556 return -1; |
547 return -1; |
557 } |
548 } |
558 } |
549 } |
559 |
550 |
560 // Change state to SAVEOP |
551 // Change state to SAVEOP |