201 uint32_t datagram_offset; |
201 uint32_t datagram_offset; |
202 size_t datagram_size; |
202 size_t datagram_size; |
203 unsigned int datagram_count; |
203 unsigned int datagram_count; |
204 unsigned int datagram_used[EC_DIR_COUNT]; |
204 unsigned int datagram_used[EC_DIR_COUNT]; |
205 ec_fmmu_config_t *fmmu; |
205 ec_fmmu_config_t *fmmu; |
|
206 ec_fmmu_config_t *fmmu_temp; |
206 const ec_datagram_t *datagram; |
207 const ec_datagram_t *datagram; |
207 int ret; |
208 int ret; |
208 |
209 |
209 domain->logical_base_address = base_address; |
210 domain->logical_base_address = base_address; |
210 |
211 |
223 datagram_offset = 0; |
224 datagram_offset = 0; |
224 datagram_size = 0; |
225 datagram_size = 0; |
225 datagram_count = 0; |
226 datagram_count = 0; |
226 datagram_used[EC_DIR_OUTPUT] = 0; |
227 datagram_used[EC_DIR_OUTPUT] = 0; |
227 datagram_used[EC_DIR_INPUT] = 0; |
228 datagram_used[EC_DIR_INPUT] = 0; |
228 |
229 list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) { |
|
230 ec_slave_config_t *sc = (ec_slave_config_t *)fmmu_temp->sc; // we have to remove the constness, sorry |
|
231 sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0; |
|
232 } |
229 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
233 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
230 // Correct logical FMMU address |
234 // Correct logical FMMU address |
231 fmmu->logical_start_address += base_address; |
235 fmmu->logical_start_address += base_address; |
232 |
236 |
233 // Increment Input/Output counter to determine datagram types |
237 // Increment Input/Output counter to determine datagram types |
234 // and calculate expected working counters |
238 // and calculate expected working counters |
235 datagram_used[fmmu->dir]++; |
239 if (fmmu->sc->used_for_fmmu_datagram[fmmu->dir] == 0) { |
236 |
240 ec_slave_config_t *sc = (ec_slave_config_t *)fmmu->sc; |
|
241 datagram_used[fmmu->dir]++; |
|
242 sc->used_for_fmmu_datagram[fmmu->dir] = 1; |
|
243 } |
237 // If the current FMMU's data do not fit in the current datagram, |
244 // If the current FMMU's data do not fit in the current datagram, |
238 // allocate a new one. |
245 // allocate a new one. |
239 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
246 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
240 ret = ec_domain_add_datagram(domain, |
247 ret = ec_domain_add_datagram(domain, |
241 domain->logical_base_address + datagram_offset, |
248 domain->logical_base_address + datagram_offset, |
246 datagram_offset += datagram_size; |
253 datagram_offset += datagram_size; |
247 datagram_size = 0; |
254 datagram_size = 0; |
248 datagram_count++; |
255 datagram_count++; |
249 datagram_used[EC_DIR_OUTPUT] = 0; |
256 datagram_used[EC_DIR_OUTPUT] = 0; |
250 datagram_used[EC_DIR_INPUT] = 0; |
257 datagram_used[EC_DIR_INPUT] = 0; |
|
258 list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) { |
|
259 ec_slave_config_t *sc = (ec_slave_config_t *)fmmu_temp->sc; |
|
260 sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0; |
|
261 } |
251 } |
262 } |
252 |
263 |
253 datagram_size += fmmu->data_size; |
264 datagram_size += fmmu->data_size; |
254 } |
265 } |
255 |
266 |