70 ec_fsm_coe_t *fsm_coe /*< CoE state machine to use */ |
72 ec_fsm_coe_t *fsm_coe /*< CoE state machine to use */ |
71 ) |
73 ) |
72 { |
74 { |
73 fsm->state = NULL; |
75 fsm->state = NULL; |
74 fsm->fsm_coe = fsm_coe; |
76 fsm->fsm_coe = fsm_coe; |
|
77 INIT_LIST_HEAD(&fsm->pdos); |
75 } |
78 } |
76 |
79 |
77 /*****************************************************************************/ |
80 /*****************************************************************************/ |
78 |
81 |
79 /** |
82 /** |
80 Destructor. |
83 Destructor. |
81 */ |
84 */ |
82 |
85 |
83 void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */) |
86 void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */) |
84 { |
87 { |
|
88 ec_fsm_coe_map_clear_pdos(fsm); |
|
89 } |
|
90 |
|
91 /*****************************************************************************/ |
|
92 |
|
93 /** |
|
94 */ |
|
95 |
|
96 void ec_fsm_coe_map_clear_pdos( |
|
97 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
|
98 ) |
|
99 { |
|
100 ec_pdo_t *pdo, *next; |
|
101 |
|
102 // free all PDOs |
|
103 list_for_each_entry_safe(pdo, next, &fsm->pdos, list) { |
|
104 list_del(&pdo->list); |
|
105 ec_pdo_clear(pdo); |
|
106 kfree(pdo); |
|
107 } |
85 } |
108 } |
86 |
109 |
87 /*****************************************************************************/ |
110 /*****************************************************************************/ |
88 |
111 |
89 /** |
112 /** |
150 ) |
173 ) |
151 { |
174 { |
152 ec_slave_t *slave = fsm->slave; |
175 ec_slave_t *slave = fsm->slave; |
153 ec_sdo_entry_t *entry; |
176 ec_sdo_entry_t *entry; |
154 |
177 |
155 for (; fsm->sync_index < 4; fsm->sync_index++) { |
178 for (; fsm->sync_index < slave->sii_sync_count; fsm->sync_index++) { |
156 if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index))) |
179 if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index))) |
157 continue; |
180 continue; |
158 |
181 |
159 if (slave->master->debug_level) |
182 if (slave->master->debug_level) |
160 EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n", |
183 EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n", |
161 fsm->sync_index, slave->ring_position); |
184 fsm->sync_index, slave->ring_position); |
|
185 |
|
186 ec_fsm_coe_map_clear_pdos(fsm); |
162 |
187 |
163 if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) { |
188 if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) { |
164 EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", |
189 EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", |
165 fsm->sync_sdo->index, |
190 fsm->sync_sdo->index, |
166 fsm->slave->ring_position); |
191 fsm->slave->ring_position); |
233 ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); |
258 ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); |
234 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
259 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
235 return; |
260 return; |
236 } |
261 } |
237 |
262 |
238 // next sync manager |
263 { |
239 fsm->sync_index++; |
264 ec_sync_t *sync = fsm->slave->sii_syncs + fsm->sync_index; |
240 ec_fsm_coe_map_action_next_sync(fsm); |
265 ec_pdo_t *pdo; |
|
266 |
|
267 // exchange sync manager PDO mapping |
|
268 ec_sync_clear_pdos(sync); |
|
269 list_for_each_entry(pdo, &fsm->pdos, list) { |
|
270 ec_sync_add_pdo(sync, pdo); |
|
271 } |
|
272 ec_fsm_coe_map_clear_pdos(fsm); |
|
273 |
|
274 // next sync manager |
|
275 fsm->sync_index++; |
|
276 ec_fsm_coe_map_action_next_sync(fsm); |
|
277 } |
241 } |
278 } |
242 |
279 |
243 /*****************************************************************************/ |
280 /*****************************************************************************/ |
244 |
281 |
245 /** |
282 /** |
257 fsm->state = ec_fsm_coe_map_state_error; |
294 fsm->state = ec_fsm_coe_map_state_error; |
258 return; |
295 return; |
259 } |
296 } |
260 |
297 |
261 { |
298 { |
262 uint16_t pdo_index = EC_READ_U16(fsm->request.data); |
|
263 ec_sdo_entry_t *entry; |
299 ec_sdo_entry_t *entry; |
264 |
300 |
|
301 if (!(fsm->pdo = (ec_pdo_t *) |
|
302 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
|
303 EC_ERR("Failed to allocate PDO.\n"); |
|
304 fsm->state = ec_fsm_coe_map_state_error; |
|
305 return; |
|
306 } |
|
307 |
|
308 ec_pdo_init(fsm->pdo); |
|
309 fsm->pdo->index = EC_READ_U16(fsm->request.data); |
|
310 fsm->pdo->type = |
|
311 ec_sync_get_pdo_type(fsm->slave->sii_syncs + fsm->sync_index); |
|
312 |
265 if (fsm->slave->master->debug_level) |
313 if (fsm->slave->master->debug_level) |
266 EC_DBG(" PDO 0x%04X.\n", pdo_index); |
314 EC_DBG(" PDO 0x%04X.\n", fsm->pdo->index); |
267 |
315 |
268 if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, pdo_index))) { |
316 if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, fsm->pdo->index))) { |
269 EC_ERR("Slave %u has no SDO 0x%04X.\n", |
317 EC_ERR("Slave %u has no SDO 0x%04X.\n", |
270 fsm->slave->ring_position, pdo_index); |
318 fsm->slave->ring_position, fsm->pdo->index); |
271 fsm->state = ec_fsm_coe_map_state_error; |
319 ec_pdo_clear(fsm->pdo); |
272 return; |
320 kfree(fsm->pdo); |
|
321 fsm->state = ec_fsm_coe_map_state_error; |
|
322 return; |
|
323 } |
|
324 |
|
325 if (fsm->pdo_sdo->name && strlen(fsm->pdo_sdo->name)) { |
|
326 if (!(fsm->pdo->name = (char *) |
|
327 kmalloc(strlen(fsm->pdo_sdo->name) + 1, GFP_KERNEL))) { |
|
328 EC_ERR("Failed to allocate PDO name.\n"); |
|
329 ec_pdo_clear(fsm->pdo); |
|
330 kfree(fsm->pdo); |
|
331 fsm->state = ec_fsm_coe_map_state_error; |
|
332 return; |
|
333 } |
|
334 memcpy(fsm->pdo->name, fsm->pdo_sdo->name, |
|
335 strlen(fsm->pdo_sdo->name) + 1); |
|
336 } else { |
|
337 fsm->pdo->name = NULL; |
273 } |
338 } |
274 |
339 |
275 if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) { |
340 if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) { |
276 EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", |
341 EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n", |
277 fsm->pdo_sdo->index, |
342 fsm->pdo_sdo->index, |
278 fsm->slave->ring_position); |
343 fsm->slave->ring_position); |
279 fsm->state = ec_fsm_coe_map_state_error; |
344 ec_pdo_clear(fsm->pdo); |
280 return; |
345 kfree(fsm->pdo); |
281 } |
346 fsm->state = ec_fsm_coe_map_state_error; |
|
347 return; |
|
348 } |
|
349 |
|
350 list_add_tail(&fsm->pdo->list, &fsm->pdos); |
282 |
351 |
283 ec_sdo_request_init_read(&fsm->request, entry); |
352 ec_sdo_request_init_read(&fsm->request, entry); |
284 fsm->state = ec_fsm_coe_map_state_pdo_entry_count; |
353 fsm->state = ec_fsm_coe_map_state_pdo_entry_count; |
285 ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); |
354 ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request); |
286 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
355 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
367 return; |
436 return; |
368 } |
437 } |
369 |
438 |
370 { |
439 { |
371 uint32_t pdo_entry_info; |
440 uint32_t pdo_entry_info; |
372 uint16_t pdo_entry_index; |
|
373 ec_sdo_t *sdo; |
441 ec_sdo_t *sdo; |
|
442 ec_sdo_entry_t *entry; |
|
443 ec_pdo_entry_t *pdo_entry; |
374 |
444 |
375 pdo_entry_info = EC_READ_U32(fsm->request.data); |
445 pdo_entry_info = EC_READ_U32(fsm->request.data); |
376 pdo_entry_index = pdo_entry_info >> 16; |
446 |
377 |
447 if (!(pdo_entry = (ec_pdo_entry_t *) |
378 if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry_index))) { |
448 kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
|
449 EC_ERR("Failed to allocate PDO entry.\n"); |
|
450 fsm->state = ec_fsm_coe_map_state_error; |
|
451 return; |
|
452 } |
|
453 |
|
454 pdo_entry->index = pdo_entry_info >> 16; |
|
455 pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF; |
|
456 pdo_entry->bit_length = pdo_entry_info & 0xFF; |
|
457 |
|
458 if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry->index))) { |
379 EC_ERR("Slave %u has no SDO 0x%04X.\n", |
459 EC_ERR("Slave %u has no SDO 0x%04X.\n", |
380 fsm->slave->ring_position, pdo_entry_index); |
460 fsm->slave->ring_position, pdo_entry->index); |
381 fsm->state = ec_fsm_coe_map_state_error; |
461 kfree(pdo_entry); |
382 return; |
462 fsm->state = ec_fsm_coe_map_state_error; |
|
463 return; |
|
464 } |
|
465 |
|
466 if (!(entry = ec_sdo_get_entry(sdo, pdo_entry->subindex))) { |
|
467 EC_ERR("Slave %u has no SDO entry 0x%04X:%u.\n", |
|
468 fsm->slave->ring_position, pdo_entry->index, |
|
469 pdo_entry->subindex); |
|
470 kfree(pdo_entry); |
|
471 fsm->state = ec_fsm_coe_map_state_error; |
|
472 return; |
|
473 } |
|
474 |
|
475 if (entry->description && strlen(entry->description)) { |
|
476 if (!(pdo_entry->name = (char *) |
|
477 kmalloc(strlen(entry->description) + 1, GFP_KERNEL))) { |
|
478 EC_ERR("Failed to allocate PDO entry name.\n"); |
|
479 kfree(pdo_entry); |
|
480 fsm->state = ec_fsm_coe_map_state_error; |
|
481 return; |
|
482 } |
|
483 memcpy(pdo_entry->name, entry->description, strlen(entry->description) + 1); |
|
484 } else { |
|
485 pdo_entry->name = NULL; |
383 } |
486 } |
384 |
487 |
385 if (fsm->slave->master->debug_level) { |
488 if (fsm->slave->master->debug_level) { |
386 size_t bitsize = pdo_entry_info & 0xFFFF; |
|
387 EC_DBG(" PDO entry 0x%04X \"%s\" (%u bit).\n", |
489 EC_DBG(" PDO entry 0x%04X \"%s\" (%u bit).\n", |
388 pdo_entry_index, sdo->name, bitsize); |
490 pdo_entry->index, pdo_entry->name ? pdo_entry->name : "???", |
389 } |
491 pdo_entry->bit_length); |
|
492 } |
|
493 |
|
494 list_add_tail(&pdo_entry->list, &fsm->pdo->entries); |
390 |
495 |
391 // next PDO entry |
496 // next PDO entry |
392 fsm->pdo_subindex++; |
497 fsm->pdo_subindex++; |
393 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
498 ec_fsm_coe_map_action_next_pdo_entry(fsm); |
394 } |
499 } |