137 int ec_domain_add_datagram_pair( |
137 int ec_domain_add_datagram_pair( |
138 ec_domain_t *domain, /**< EtherCAT domain. */ |
138 ec_domain_t *domain, /**< EtherCAT domain. */ |
139 uint32_t logical_offset, /**< Logical offset. */ |
139 uint32_t logical_offset, /**< Logical offset. */ |
140 size_t data_size, /**< Size of the data. */ |
140 size_t data_size, /**< Size of the data. */ |
141 uint8_t *data, /**< Process data. */ |
141 uint8_t *data, /**< Process data. */ |
142 const unsigned int used[] /**< Used by inputs/outputs. */ |
142 const unsigned int used[] /**< Slave config counter for in/out. */ |
143 ) |
143 ) |
144 { |
144 { |
145 ec_datagram_pair_t *datagram_pair; |
145 ec_datagram_pair_t *datagram_pair; |
146 int ret; |
146 int ret; |
147 unsigned int dev_idx; |
147 unsigned int dev_idx; |
203 return 0; |
203 return 0; |
204 } |
204 } |
205 |
205 |
206 /*****************************************************************************/ |
206 /*****************************************************************************/ |
207 |
207 |
|
208 /** Domain finish helper function. |
|
209 * |
|
210 * Detects, if a slave configuration has already been taken into account for |
|
211 * a datagram's expected working counter calculation. |
|
212 * |
|
213 * Walks through the list of all FMMU configurations for the current datagram |
|
214 * and ends before the current datagram. |
|
215 */ |
|
216 int shall_count( |
|
217 const ec_fmmu_config_t *cur_fmmu, /**< Current FMMU with direction to |
|
218 search for. */ |
|
219 const ec_fmmu_config_t *first_fmmu /**< Datagram's first FMMU. */ |
|
220 ) |
|
221 { |
|
222 for (; first_fmmu != cur_fmmu; |
|
223 first_fmmu = list_entry(first_fmmu->list.next, |
|
224 ec_fmmu_config_t, list)) { |
|
225 |
|
226 if (first_fmmu->sc == cur_fmmu->sc |
|
227 && first_fmmu->dir == cur_fmmu->dir) { |
|
228 return 0; // was already counted |
|
229 } |
|
230 } |
|
231 |
|
232 return 1; |
|
233 } |
|
234 |
|
235 /*****************************************************************************/ |
|
236 |
208 /** Finishes a domain. |
237 /** Finishes a domain. |
209 * |
238 * |
210 * This allocates the necessary datagrams and writes the correct logical |
239 * This allocates the necessary datagrams and writes the correct logical |
211 * addresses to every configured FMMU. |
240 * addresses to every configured FMMU. |
212 * |
241 * |
223 uint32_t datagram_offset; |
252 uint32_t datagram_offset; |
224 size_t datagram_size; |
253 size_t datagram_size; |
225 unsigned int datagram_count; |
254 unsigned int datagram_count; |
226 unsigned int datagram_used[EC_DIR_COUNT]; |
255 unsigned int datagram_used[EC_DIR_COUNT]; |
227 ec_fmmu_config_t *fmmu; |
256 ec_fmmu_config_t *fmmu; |
228 ec_fmmu_config_t *fmmu_temp; |
257 const ec_fmmu_config_t *datagram_first_fmmu = NULL; |
229 const ec_datagram_pair_t *datagram_pair; |
258 const ec_datagram_pair_t *datagram_pair; |
230 int ret; |
259 int ret; |
231 |
260 |
232 domain->logical_base_address = base_address; |
261 domain->logical_base_address = base_address; |
233 |
262 |
239 domain->data_size, domain->index); |
268 domain->data_size, domain->index); |
240 return -ENOMEM; |
269 return -ENOMEM; |
241 } |
270 } |
242 } |
271 } |
243 |
272 |
244 // Cycle through all domain FMMUS and |
273 // Cycle through all domain FMMUs and |
245 // - correct the logical base addresses |
274 // - correct the logical base addresses |
246 // - set up the datagrams to carry the process data |
275 // - set up the datagrams to carry the process data |
|
276 // - calculate the datagrams' expected working counters |
247 datagram_offset = 0; |
277 datagram_offset = 0; |
248 datagram_size = 0; |
278 datagram_size = 0; |
249 datagram_count = 0; |
279 datagram_count = 0; |
250 datagram_used[EC_DIR_OUTPUT] = 0; |
280 datagram_used[EC_DIR_OUTPUT] = 0; |
251 datagram_used[EC_DIR_INPUT] = 0; |
281 datagram_used[EC_DIR_INPUT] = 0; |
252 |
282 |
253 list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) { |
283 if (!list_empty(&domain->fmmu_configs)) { |
254 // we have to remove the constness, sorry FIXME |
284 datagram_first_fmmu = |
255 ec_slave_config_t *sc = (ec_slave_config_t *) fmmu_temp->sc; |
285 list_entry(domain->fmmu_configs.next, ec_fmmu_config_t, list); |
256 sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0; |
|
257 } |
286 } |
258 |
287 |
259 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
288 list_for_each_entry(fmmu, &domain->fmmu_configs, list) { |
|
289 |
260 // Correct logical FMMU address |
290 // Correct logical FMMU address |
261 fmmu->logical_start_address += base_address; |
291 fmmu->logical_start_address += base_address; |
262 |
292 |
263 // Increment Input/Output counter to determine datagram types |
293 // Increment Input/Output counter to determine datagram types |
264 // and calculate expected working counters |
294 // and calculate expected working counters |
265 if (fmmu->sc->used_for_fmmu_datagram[fmmu->dir] == 0) { |
295 if (shall_count(fmmu, datagram_first_fmmu)) { |
266 ec_slave_config_t *sc = (ec_slave_config_t *)fmmu->sc; |
|
267 datagram_used[fmmu->dir]++; |
296 datagram_used[fmmu->dir]++; |
268 sc->used_for_fmmu_datagram[fmmu->dir] = 1; |
|
269 } |
297 } |
270 |
298 |
271 // If the current FMMU's data do not fit in the current datagram, |
299 // If the current FMMU's data do not fit in the current datagram, |
272 // allocate a new one. |
300 // allocate a new one. |
273 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
301 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
275 domain->logical_base_address + datagram_offset, |
303 domain->logical_base_address + datagram_offset, |
276 datagram_size, domain->data + datagram_offset, |
304 datagram_size, domain->data + datagram_offset, |
277 datagram_used); |
305 datagram_used); |
278 if (ret < 0) |
306 if (ret < 0) |
279 return ret; |
307 return ret; |
|
308 |
280 datagram_offset += datagram_size; |
309 datagram_offset += datagram_size; |
281 datagram_size = 0; |
310 datagram_size = 0; |
282 datagram_count++; |
311 datagram_count++; |
283 datagram_used[EC_DIR_OUTPUT] = 0; |
312 datagram_used[EC_DIR_OUTPUT] = 0; |
284 datagram_used[EC_DIR_INPUT] = 0; |
313 datagram_used[EC_DIR_INPUT] = 0; |
285 list_for_each_entry(fmmu_temp, &domain->fmmu_configs, list) { |
314 datagram_first_fmmu = fmmu; |
286 ec_slave_config_t *sc = (ec_slave_config_t *)fmmu_temp->sc; |
|
287 sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0; |
|
288 } |
|
289 } |
315 } |
290 |
316 |
291 datagram_size += fmmu->data_size; |
317 datagram_size += fmmu->data_size; |
292 } |
318 } |
293 |
319 |