162 ec_slave_t *slave; |
162 ec_slave_t *slave; |
163 ec_slave_ident_t *ident; |
163 ec_slave_ident_t *ident; |
164 unsigned int i; |
164 unsigned int i; |
165 unsigned char data[2]; |
165 unsigned char data[2]; |
166 |
166 |
167 if (master->slaves || master->slave_count) |
167 if (master->slaves || master->slave_count) { |
168 EC_WARN("Slave scan already done!\n"); |
168 EC_ERR("Slave scan already done!\n"); |
169 ec_master_clear_slaves(master); |
169 return -1; |
|
170 } |
170 |
171 |
171 // Determine number of slaves on bus |
172 // Determine number of slaves on bus |
172 |
|
173 ec_frame_init_brd(&frame, master, 0x0000, 4); |
173 ec_frame_init_brd(&frame, master, 0x0000, 4); |
174 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
174 if (unlikely(ec_frame_send_receive(&frame))) return -1; |
175 |
|
176 master->slave_count = frame.working_counter; |
175 master->slave_count = frame.working_counter; |
177 EC_INFO("Found %i slaves on bus.\n", master->slave_count); |
176 EC_INFO("Found %i slaves on bus.\n", master->slave_count); |
178 |
177 |
179 if (!master->slave_count) return 0; |
178 if (!master->slave_count) return 0; |
180 |
179 |
181 if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count |
180 if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count |
182 * sizeof(ec_slave_t), |
181 * sizeof(ec_slave_t), |
183 GFP_KERNEL))) { |
182 GFP_KERNEL))) { |
184 EC_ERR("Could not allocate memory for bus slaves!\n"); |
183 EC_ERR("Could not allocate memory for slaves!\n"); |
185 return -1; |
184 return -1; |
186 } |
185 } |
187 |
186 |
188 // Init slaves |
187 // Init slaves |
189 for (i = 0; i < master->slave_count; i++) { |
188 for (i = 0; i < master->slave_count; i++) { |
202 EC_WRITE_U16(data, slave->station_address); |
201 EC_WRITE_U16(data, slave->station_address); |
203 |
202 |
204 ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010, |
203 ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010, |
205 sizeof(uint16_t), data); |
204 sizeof(uint16_t), data); |
206 |
205 |
207 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
206 if (unlikely(ec_frame_send_receive(&frame))) { |
208 EC_ERR("Writing station address failed on slave %i!\n", i); |
207 EC_ERR("Writing station address failed on slave %i!\n", i); |
209 return -1; |
208 return -1; |
210 } |
209 } |
211 |
210 |
212 // Fetch all slave information |
211 // Fetch all slave information |
484 // Resetting FMMU's |
483 // Resetting FMMU's |
485 if (slave->base_fmmu_count) { |
484 if (slave->base_fmmu_count) { |
486 memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
485 memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
487 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600, |
486 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600, |
488 EC_FMMU_SIZE * slave->base_fmmu_count, data); |
487 EC_FMMU_SIZE * slave->base_fmmu_count, data); |
489 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
488 if (unlikely(ec_frame_send_receive(&frame))) { |
490 EC_ERR("Resetting FMMUs failed on slave %i!\n", |
489 EC_ERR("Resetting FMMUs failed on slave %i!\n", |
491 slave->ring_position); |
490 slave->ring_position); |
492 return -1; |
491 return -1; |
493 } |
492 } |
494 } |
493 } |
496 // Resetting Sync Manager channels |
495 // Resetting Sync Manager channels |
497 if (slave->base_sync_count) { |
496 if (slave->base_sync_count) { |
498 memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
497 memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
499 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800, |
498 ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800, |
500 EC_SYNC_SIZE * slave->base_sync_count, data); |
499 EC_SYNC_SIZE * slave->base_sync_count, data); |
501 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
500 if (unlikely(ec_frame_send_receive(&frame))) { |
502 EC_ERR("Resetting sync managers failed on slave %i!\n", |
501 EC_ERR("Resetting sync managers failed on slave %i!\n", |
503 slave->ring_position); |
502 slave->ring_position); |
504 return -1; |
503 return -1; |
505 } |
504 } |
506 } |
505 } |