188 */ |
188 */ |
189 int ec_domain_add_datagram( |
189 int ec_domain_add_datagram( |
190 ec_domain_t *domain, /**< EtherCAT domain. */ |
190 ec_domain_t *domain, /**< EtherCAT domain. */ |
191 uint32_t logical_offset, /**< Logical offset. */ |
191 uint32_t logical_offset, /**< Logical offset. */ |
192 size_t data_size, /**< Size of the data. */ |
192 size_t data_size, /**< Size of the data. */ |
193 uint8_t *data /**< Process data. */ |
193 uint8_t *data, /**< Process data. */ |
|
194 const unsigned int used[] /**< Used by inputs/outputs. */ |
194 ) |
195 ) |
195 { |
196 { |
196 ec_datagram_t *datagram; |
197 ec_datagram_t *datagram; |
197 |
198 |
198 if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { |
199 if (!(datagram = kmalloc(sizeof(ec_datagram_t), GFP_KERNEL))) { |
202 |
203 |
203 ec_datagram_init(datagram); |
204 ec_datagram_init(datagram); |
204 snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, |
205 snprintf(datagram->name, EC_DATAGRAM_NAME_SIZE, |
205 "domain%u-%u", domain->index, logical_offset); |
206 "domain%u-%u", domain->index, logical_offset); |
206 |
207 |
207 if (ec_datagram_lrw(datagram, logical_offset, data_size, data)) { |
208 if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs |
208 kfree(datagram); |
209 if (ec_datagram_lrw(datagram, logical_offset, data_size, data)) { |
209 return -1; |
210 kfree(datagram); |
|
211 return -1; |
|
212 } |
|
213 } else if (used[EC_DIR_OUTPUT]) { // outputs only |
|
214 if (ec_datagram_lwr(datagram, logical_offset, data_size, data)) { |
|
215 kfree(datagram); |
|
216 return -1; |
|
217 } |
|
218 } else { // inputs only (or nothing) |
|
219 if (ec_datagram_lrd(datagram, logical_offset, data_size, data)) { |
|
220 kfree(datagram); |
|
221 return -1; |
|
222 } |
210 } |
223 } |
211 |
224 |
212 list_add_tail(&datagram->list, &domain->datagrams); |
225 list_add_tail(&datagram->list, &domain->datagrams); |
213 return 0; |
226 return 0; |
214 } |
227 } |
231 ) |
244 ) |
232 { |
245 { |
233 uint32_t datagram_offset; |
246 uint32_t datagram_offset; |
234 size_t datagram_size; |
247 size_t datagram_size; |
235 unsigned int datagram_count, i; |
248 unsigned int datagram_count, i; |
|
249 unsigned int datagram_used[2]; |
236 ec_slave_config_t *sc; |
250 ec_slave_config_t *sc; |
237 ec_fmmu_config_t *fmmu; |
251 ec_fmmu_config_t *fmmu; |
|
252 const ec_datagram_t *datagram; |
238 |
253 |
239 domain->logical_base_address = base_address; |
254 domain->logical_base_address = base_address; |
240 |
255 |
241 if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) { |
256 if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) { |
242 if (!(domain->data = |
257 if (!(domain->data = |
250 // Cycle through all domain FMMUS, correct the logical base addresses and |
265 // Cycle through all domain FMMUS, correct the logical base addresses and |
251 // set up the datagrams to carry the process data. |
266 // set up the datagrams to carry the process data. |
252 datagram_offset = 0; |
267 datagram_offset = 0; |
253 datagram_size = 0; |
268 datagram_size = 0; |
254 datagram_count = 0; |
269 datagram_count = 0; |
|
270 datagram_used[EC_DIR_OUTPUT] = 0; |
|
271 datagram_used[EC_DIR_INPUT] = 0; |
|
272 |
255 list_for_each_entry(sc, &domain->master->configs, list) { |
273 list_for_each_entry(sc, &domain->master->configs, list) { |
256 for (i = 0; i < sc->used_fmmus; i++) { |
274 for (i = 0; i < sc->used_fmmus; i++) { |
257 fmmu = &sc->fmmu_configs[i]; |
275 fmmu = &sc->fmmu_configs[i]; |
258 if (fmmu->domain != domain) |
276 if (fmmu->domain != domain) |
259 continue; |
277 continue; |
260 |
278 |
261 // Correct logical FMMU address |
279 // Correct logical FMMU address |
262 fmmu->logical_start_address += base_address; |
280 fmmu->logical_start_address += base_address; |
263 |
281 |
|
282 // Increment Input/Output counter |
|
283 datagram_used[fmmu->dir]++; |
|
284 |
264 // If the current FMMU's data do not fit in the current datagram, |
285 // If the current FMMU's data do not fit in the current datagram, |
265 // allocate a new one. |
286 // allocate a new one. |
266 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
287 if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) { |
267 if (ec_domain_add_datagram(domain, |
288 if (ec_domain_add_datagram(domain, |
268 domain->logical_base_address + datagram_offset, |
289 domain->logical_base_address + datagram_offset, |
269 datagram_size, domain->data + datagram_offset)) |
290 datagram_size, domain->data + datagram_offset, |
|
291 datagram_used)) |
270 return -1; |
292 return -1; |
271 datagram_offset += datagram_size; |
293 datagram_offset += datagram_size; |
272 datagram_size = 0; |
294 datagram_size = 0; |
273 datagram_count++; |
295 datagram_count++; |
|
296 datagram_used[EC_DIR_OUTPUT] = 0; |
|
297 datagram_used[EC_DIR_INPUT] = 0; |
274 } |
298 } |
275 |
299 |
276 datagram_size += fmmu->data_size; |
300 datagram_size += fmmu->data_size; |
277 } |
301 } |
278 } |
302 } |
279 |
303 |
280 // allocate last datagram, if data are left |
304 // allocate last datagram, if data are left |
281 if (datagram_size) { |
305 if (datagram_size) { |
282 if (ec_domain_add_datagram(domain, |
306 if (ec_domain_add_datagram(domain, |
283 domain->logical_base_address + datagram_offset, |
307 domain->logical_base_address + datagram_offset, |
284 datagram_size, domain->data + datagram_offset)) |
308 datagram_size, domain->data + datagram_offset, |
|
309 datagram_used)) |
285 return -1; |
310 return -1; |
286 datagram_count++; |
311 datagram_count++; |
287 } |
312 } |
288 |
313 |
289 EC_INFO("Domain %u with logical offset %u contains %u bytes in %u" |
314 EC_INFO("Domain %u with logical offset %u contains %u bytes.\n", |
290 " datagram%s.\n", domain->index, domain->logical_base_address, |
315 domain->index, domain->logical_base_address, domain->data_size); |
291 domain->data_size, datagram_count, datagram_count == 1 ? "" : "s"); |
316 list_for_each_entry(datagram, &domain->datagrams, list) { |
|
317 EC_INFO(" Datagram %s, logical offset %u, size %u, type %s.\n", |
|
318 datagram->name, EC_READ_U32(datagram->address), |
|
319 datagram->data_size, ec_datagram_type_string(datagram)); |
|
320 } |
|
321 |
292 return 0; |
322 return 0; |
293 } |
323 } |
294 |
324 |
295 /*****************************************************************************/ |
325 /*****************************************************************************/ |
296 |
326 |