87 } |
87 } |
88 |
88 |
89 /*****************************************************************************/ |
89 /*****************************************************************************/ |
90 |
90 |
91 /** |
91 /** |
92 Starts to upload an SDO from a slave. |
92 Starts to upload an Sdo from a slave. |
93 */ |
93 */ |
94 |
94 |
95 void ec_fsm_coe_map_start( |
95 void ec_fsm_coe_map_start( |
96 ec_fsm_coe_map_t *fsm, /**< finite state machine */ |
96 ec_fsm_coe_map_t *fsm, /**< finite state machine */ |
97 ec_slave_t *slave /**< EtherCAT slave */ |
97 ec_slave_t *slave /**< EtherCAT slave */ |
161 for (; fsm->sync_index < slave->sii_sync_count; fsm->sync_index++) { |
161 for (; fsm->sync_index < slave->sii_sync_count; fsm->sync_index++) { |
162 if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index))) |
162 if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index))) |
163 continue; |
163 continue; |
164 |
164 |
165 if (slave->master->debug_level) |
165 if (slave->master->debug_level) |
166 EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n", |
166 EC_DBG("Reading Pdo mapping of sync manager %u of slave %u.\n", |
167 fsm->sync_index, slave->ring_position); |
167 fsm->sync_index, slave->ring_position); |
168 |
168 |
169 ec_pdo_mapping_clear_pdos(&fsm->mapping); |
169 ec_pdo_mapping_clear_pdos(&fsm->mapping); |
170 |
170 |
171 if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) { |
171 if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) { |
172 EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", |
172 EC_ERR("Sdo 0x%04X has no subindex 0 on slave %u.\n", |
173 fsm->sync_sdo->index, |
173 fsm->sync_sdo->index, |
174 fsm->slave->ring_position); |
174 fsm->slave->ring_position); |
175 fsm->state = ec_fsm_coe_map_state_error; |
175 fsm->state = ec_fsm_coe_map_state_error; |
176 return; |
176 return; |
177 } |
177 } |
187 } |
187 } |
188 |
188 |
189 /*****************************************************************************/ |
189 /*****************************************************************************/ |
190 |
190 |
191 /** |
191 /** |
192 * Count mapped PDOs. |
192 * Count mapped Pdos. |
193 */ |
193 */ |
194 |
194 |
195 void ec_fsm_coe_map_state_pdo_count( |
195 void ec_fsm_coe_map_state_pdo_count( |
196 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
196 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
197 ) |
197 ) |
198 { |
198 { |
199 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
199 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
200 |
200 |
201 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
201 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
202 EC_ERR("Failed to read number of mapped PDOs from slave %u.\n", |
202 EC_ERR("Failed to read number of mapped Pdos from slave %u.\n", |
203 fsm->slave->ring_position); |
203 fsm->slave->ring_position); |
204 fsm->state = ec_fsm_coe_map_state_error; |
204 fsm->state = ec_fsm_coe_map_state_error; |
205 return; |
205 return; |
206 } |
206 } |
207 |
207 |
208 fsm->sync_subindices = EC_READ_U8(fsm->request.data); |
208 fsm->sync_subindices = EC_READ_U8(fsm->request.data); |
209 |
209 |
210 if (fsm->slave->master->debug_level) |
210 if (fsm->slave->master->debug_level) |
211 EC_DBG(" %u PDOs mapped.\n", fsm->sync_subindices); |
211 EC_DBG(" %u Pdos mapped.\n", fsm->sync_subindices); |
212 |
212 |
213 // read first PDO |
213 // read first Pdo |
214 fsm->sync_subindex = 1; |
214 fsm->sync_subindex = 1; |
215 ec_fsm_coe_map_action_next_pdo(fsm); |
215 ec_fsm_coe_map_action_next_pdo(fsm); |
216 } |
216 } |
217 |
217 |
218 /*****************************************************************************/ |
218 /*****************************************************************************/ |
219 |
219 |
220 /** |
220 /** |
221 * Read next PDO. |
221 * Read next Pdo. |
222 */ |
222 */ |
223 |
223 |
224 void ec_fsm_coe_map_action_next_pdo( |
224 void ec_fsm_coe_map_action_next_pdo( |
225 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
225 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
226 ) |
226 ) |
228 ec_sdo_entry_t *entry; |
228 ec_sdo_entry_t *entry; |
229 |
229 |
230 if (fsm->sync_subindex <= fsm->sync_subindices) { |
230 if (fsm->sync_subindex <= fsm->sync_subindices) { |
231 if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, |
231 if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, |
232 fsm->sync_subindex))) { |
232 fsm->sync_subindex))) { |
233 EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n", |
233 EC_ERR("Sdo 0x%04X has no subindex %u on slave %u.\n", |
234 fsm->sync_sdo->index, |
234 fsm->sync_sdo->index, |
235 fsm->sync_subindex, |
235 fsm->sync_subindex, |
236 fsm->slave->ring_position); |
236 fsm->slave->ring_position); |
237 fsm->state = ec_fsm_coe_map_state_error; |
237 fsm->state = ec_fsm_coe_map_state_error; |
238 return; |
238 return; |
263 } |
263 } |
264 |
264 |
265 /*****************************************************************************/ |
265 /*****************************************************************************/ |
266 |
266 |
267 /** |
267 /** |
268 * Fetch PDO information. |
268 * Fetch Pdo information. |
269 */ |
269 */ |
270 |
270 |
271 void ec_fsm_coe_map_state_pdo( |
271 void ec_fsm_coe_map_state_pdo( |
272 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
272 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
273 ) |
273 ) |
274 { |
274 { |
275 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
275 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
276 |
276 |
277 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
277 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
278 EC_ERR("Failed to read mapped PDO index from slave %u.\n", |
278 EC_ERR("Failed to read mapped Pdo index from slave %u.\n", |
279 fsm->slave->ring_position); |
279 fsm->slave->ring_position); |
280 fsm->state = ec_fsm_coe_map_state_error; |
280 fsm->state = ec_fsm_coe_map_state_error; |
281 return; |
281 return; |
282 } |
282 } |
283 |
283 |
284 { |
284 { |
285 ec_sdo_entry_t *entry; |
285 ec_sdo_entry_t *entry; |
286 |
286 |
287 if (!(fsm->pdo = (ec_pdo_t *) |
287 if (!(fsm->pdo = (ec_pdo_t *) |
288 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
288 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
289 EC_ERR("Failed to allocate PDO.\n"); |
289 EC_ERR("Failed to allocate Pdo.\n"); |
290 fsm->state = ec_fsm_coe_map_state_error; |
290 fsm->state = ec_fsm_coe_map_state_error; |
291 return; |
291 return; |
292 } |
292 } |
293 |
293 |
294 ec_pdo_init(fsm->pdo); |
294 ec_pdo_init(fsm->pdo); |
295 fsm->pdo->index = EC_READ_U16(fsm->request.data); |
295 fsm->pdo->index = EC_READ_U16(fsm->request.data); |
296 fsm->pdo->dir = |
296 fsm->pdo->dir = |
297 ec_sync_direction(fsm->slave->sii_syncs + fsm->sync_index); |
297 ec_sync_direction(fsm->slave->sii_syncs + fsm->sync_index); |
298 |
298 |
299 if (fsm->slave->master->debug_level) |
299 if (fsm->slave->master->debug_level) |
300 EC_DBG(" PDO 0x%04X.\n", fsm->pdo->index); |
300 EC_DBG(" Pdo 0x%04X.\n", fsm->pdo->index); |
301 |
301 |
302 if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, fsm->pdo->index))) { |
302 if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, fsm->pdo->index))) { |
303 EC_ERR("Slave %u has no SDO 0x%04X.\n", |
303 EC_ERR("Slave %u has no Sdo 0x%04X.\n", |
304 fsm->slave->ring_position, fsm->pdo->index); |
304 fsm->slave->ring_position, fsm->pdo->index); |
305 ec_pdo_clear(fsm->pdo); |
305 ec_pdo_clear(fsm->pdo); |
306 kfree(fsm->pdo); |
306 kfree(fsm->pdo); |
307 fsm->state = ec_fsm_coe_map_state_error; |
307 fsm->state = ec_fsm_coe_map_state_error; |
308 return; |
308 return; |
314 fsm->state = ec_fsm_coe_map_state_error; |
314 fsm->state = ec_fsm_coe_map_state_error; |
315 return; |
315 return; |
316 } |
316 } |
317 |
317 |
318 if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) { |
318 if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) { |
319 EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", |
319 EC_ERR("Sdo 0x%04X has no subindex 0 on slave %u.\n", |
320 fsm->pdo_sdo->index, |
320 fsm->pdo_sdo->index, |
321 fsm->slave->ring_position); |
321 fsm->slave->ring_position); |
322 ec_pdo_clear(fsm->pdo); |
322 ec_pdo_clear(fsm->pdo); |
323 kfree(fsm->pdo); |
323 kfree(fsm->pdo); |
324 fsm->state = ec_fsm_coe_map_state_error; |
324 fsm->state = ec_fsm_coe_map_state_error; |
336 } |
336 } |
337 |
337 |
338 /*****************************************************************************/ |
338 /*****************************************************************************/ |
339 |
339 |
340 /** |
340 /** |
341 * Read number of PDO entries. |
341 * Read number of Pdo entries. |
342 */ |
342 */ |
343 |
343 |
344 void ec_fsm_coe_map_state_pdo_entry_count( |
344 void ec_fsm_coe_map_state_pdo_entry_count( |
345 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
345 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
346 ) |
346 ) |
347 { |
347 { |
348 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
348 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
349 |
349 |
350 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
350 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
351 EC_ERR("Failed to read number of mapped PDO entries from slave %u.\n", |
351 EC_ERR("Failed to read number of mapped Pdo entries from slave %u.\n", |
352 fsm->slave->ring_position); |
352 fsm->slave->ring_position); |
353 fsm->state = ec_fsm_coe_map_state_error; |
353 fsm->state = ec_fsm_coe_map_state_error; |
354 return; |
354 return; |
355 } |
355 } |
356 |
356 |
357 fsm->pdo_subindices = EC_READ_U8(fsm->request.data); |
357 fsm->pdo_subindices = EC_READ_U8(fsm->request.data); |
358 |
358 |
359 if (fsm->slave->master->debug_level) |
359 if (fsm->slave->master->debug_level) |
360 EC_DBG(" %u PDO entries mapped.\n", fsm->pdo_subindices); |
360 EC_DBG(" %u Pdo entries mapped.\n", fsm->pdo_subindices); |
361 |
361 |
362 // read first PDO entry |
362 // read first Pdo entry |
363 fsm->pdo_subindex = 1; |
363 fsm->pdo_subindex = 1; |
364 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
364 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
365 } |
365 } |
366 |
366 |
367 /*****************************************************************************/ |
367 /*****************************************************************************/ |
368 |
368 |
369 /** |
369 /** |
370 * Read next PDO entry. |
370 * Read next Pdo entry. |
371 */ |
371 */ |
372 |
372 |
373 void ec_fsm_coe_map_action_next_pdo_entry( |
373 void ec_fsm_coe_map_action_next_pdo_entry( |
374 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
374 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
375 ) |
375 ) |
377 ec_sdo_entry_t *entry; |
377 ec_sdo_entry_t *entry; |
378 |
378 |
379 if (fsm->pdo_subindex <= fsm->pdo_subindices) { |
379 if (fsm->pdo_subindex <= fsm->pdo_subindices) { |
380 if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, |
380 if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, |
381 fsm->pdo_subindex))) { |
381 fsm->pdo_subindex))) { |
382 EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n", |
382 EC_ERR("Sdo 0x%04X has no subindex %u on slave %u.\n", |
383 fsm->pdo_sdo->index, fsm->pdo_subindex, |
383 fsm->pdo_sdo->index, fsm->pdo_subindex, |
384 fsm->slave->ring_position); |
384 fsm->slave->ring_position); |
385 fsm->state = ec_fsm_coe_map_state_error; |
385 fsm->state = ec_fsm_coe_map_state_error; |
386 return; |
386 return; |
387 } |
387 } |
391 ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); |
391 ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); |
392 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
392 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
393 return; |
393 return; |
394 } |
394 } |
395 |
395 |
396 // next PDO |
396 // next Pdo |
397 fsm->sync_subindex++; |
397 fsm->sync_subindex++; |
398 ec_fsm_coe_map_action_next_pdo(fsm); |
398 ec_fsm_coe_map_action_next_pdo(fsm); |
399 } |
399 } |
400 |
400 |
401 /*****************************************************************************/ |
401 /*****************************************************************************/ |
402 |
402 |
403 /** |
403 /** |
404 * Read PDO entry information. |
404 * Read Pdo entry information. |
405 */ |
405 */ |
406 |
406 |
407 void ec_fsm_coe_map_state_pdo_entry( |
407 void ec_fsm_coe_map_state_pdo_entry( |
408 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
408 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
409 ) |
409 ) |
410 { |
410 { |
411 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
411 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
412 |
412 |
413 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
413 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
414 EC_ERR("Failed to read index of mapped PDO entry from slave %u.\n", |
414 EC_ERR("Failed to read index of mapped Pdo entry from slave %u.\n", |
415 fsm->slave->ring_position); |
415 fsm->slave->ring_position); |
416 fsm->state = ec_fsm_coe_map_state_error; |
416 fsm->state = ec_fsm_coe_map_state_error; |
417 return; |
417 return; |
418 } |
418 } |
419 |
419 |
425 |
425 |
426 pdo_entry_info = EC_READ_U32(fsm->request.data); |
426 pdo_entry_info = EC_READ_U32(fsm->request.data); |
427 |
427 |
428 if (!(pdo_entry = (ec_pdo_entry_t *) |
428 if (!(pdo_entry = (ec_pdo_entry_t *) |
429 kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
429 kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
430 EC_ERR("Failed to allocate PDO entry.\n"); |
430 EC_ERR("Failed to allocate Pdo entry.\n"); |
431 fsm->state = ec_fsm_coe_map_state_error; |
431 fsm->state = ec_fsm_coe_map_state_error; |
432 return; |
432 return; |
433 } |
433 } |
434 |
434 |
435 ec_pdo_entry_init(pdo_entry); |
435 ec_pdo_entry_init(pdo_entry); |
436 pdo_entry->index = pdo_entry_info >> 16; |
436 pdo_entry->index = pdo_entry_info >> 16; |
437 pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF; |
437 pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF; |
438 pdo_entry->bit_length = pdo_entry_info & 0xFF; |
438 pdo_entry->bit_length = pdo_entry_info & 0xFF; |
439 |
439 |
440 if (!pdo_entry->index && !pdo_entry->subindex) { |
440 if (!pdo_entry->index && !pdo_entry->subindex) { |
441 // we have a gap in the PDO, next PDO entry |
441 // we have a gap in the Pdo, next Pdo entry |
442 if (fsm->slave->master->debug_level) { |
442 if (fsm->slave->master->debug_level) { |
443 EC_DBG(" PDO entry gap: %u bit.\n", |
443 EC_DBG(" Pdo entry gap: %u bit.\n", |
444 pdo_entry->bit_length); |
444 pdo_entry->bit_length); |
445 } |
445 } |
446 |
446 |
447 if (ec_pdo_entry_set_name(pdo_entry, "Gap")) { |
447 if (ec_pdo_entry_set_name(pdo_entry, "Gap")) { |
448 ec_pdo_entry_clear(pdo_entry); |
448 ec_pdo_entry_clear(pdo_entry); |
456 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
456 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
457 return; |
457 return; |
458 } |
458 } |
459 |
459 |
460 if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry->index))) { |
460 if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry->index))) { |
461 EC_ERR("Slave %u has no SDO 0x%04X.\n", |
461 EC_ERR("Slave %u has no Sdo 0x%04X.\n", |
462 fsm->slave->ring_position, pdo_entry->index); |
462 fsm->slave->ring_position, pdo_entry->index); |
463 ec_pdo_entry_clear(pdo_entry); |
463 ec_pdo_entry_clear(pdo_entry); |
464 kfree(pdo_entry); |
464 kfree(pdo_entry); |
465 fsm->state = ec_fsm_coe_map_state_error; |
465 fsm->state = ec_fsm_coe_map_state_error; |
466 return; |
466 return; |
467 } |
467 } |
468 |
468 |
469 if (!(entry = ec_sdo_get_entry(sdo, pdo_entry->subindex))) { |
469 if (!(entry = ec_sdo_get_entry(sdo, pdo_entry->subindex))) { |
470 EC_ERR("Slave %u has no SDO entry 0x%04X:%u.\n", |
470 EC_ERR("Slave %u has no Sdo entry 0x%04X:%u.\n", |
471 fsm->slave->ring_position, pdo_entry->index, |
471 fsm->slave->ring_position, pdo_entry->index, |
472 pdo_entry->subindex); |
472 pdo_entry->subindex); |
473 ec_pdo_entry_clear(pdo_entry); |
473 ec_pdo_entry_clear(pdo_entry); |
474 kfree(pdo_entry); |
474 kfree(pdo_entry); |
475 fsm->state = ec_fsm_coe_map_state_error; |
475 fsm->state = ec_fsm_coe_map_state_error; |
482 fsm->state = ec_fsm_coe_map_state_error; |
482 fsm->state = ec_fsm_coe_map_state_error; |
483 return; |
483 return; |
484 } |
484 } |
485 |
485 |
486 if (fsm->slave->master->debug_level) { |
486 if (fsm->slave->master->debug_level) { |
487 EC_DBG(" PDO entry 0x%04X \"%s\" (%u bit).\n", pdo_entry->index, |
487 EC_DBG(" Pdo entry 0x%04X \"%s\" (%u bit).\n", pdo_entry->index, |
488 pdo_entry->name ? pdo_entry->name : "???", |
488 pdo_entry->name ? pdo_entry->name : "???", |
489 pdo_entry->bit_length); |
489 pdo_entry->bit_length); |
490 } |
490 } |
491 |
491 |
492 list_add_tail(&pdo_entry->list, &fsm->pdo->entries); |
492 list_add_tail(&pdo_entry->list, &fsm->pdo->entries); |
493 |
493 |
494 // next PDO entry |
494 // next Pdo entry |
495 fsm->pdo_subindex++; |
495 fsm->pdo_subindex++; |
496 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
496 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
497 } |
497 } |
498 } |
498 } |
499 |
499 |