58 /*****************************************************************************/ |
58 /*****************************************************************************/ |
59 |
59 |
60 /** Constructor. |
60 /** Constructor. |
61 */ |
61 */ |
62 void ec_fsm_pdo_entry_init( |
62 void ec_fsm_pdo_entry_init( |
63 ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ |
63 ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */ |
64 ec_fsm_coe_t *fsm_coe /**< CoE state machine to use. */ |
64 ec_fsm_coe_t *fsm_coe /**< CoE state machine to use. */ |
65 ) |
65 ) |
66 { |
66 { |
67 fsm->fsm_coe = fsm_coe; |
67 fsm->fsm_coe = fsm_coe; |
68 ec_sdo_request_init(&fsm->request); |
68 ec_sdo_request_init(&fsm->request); |
71 /*****************************************************************************/ |
71 /*****************************************************************************/ |
72 |
72 |
73 /** Destructor. |
73 /** Destructor. |
74 */ |
74 */ |
75 void ec_fsm_pdo_entry_clear( |
75 void ec_fsm_pdo_entry_clear( |
76 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
76 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
77 ) |
77 ) |
78 { |
78 { |
79 ec_sdo_request_clear(&fsm->request); |
79 ec_sdo_request_clear(&fsm->request); |
80 } |
80 } |
81 |
81 |
82 /*****************************************************************************/ |
82 /*****************************************************************************/ |
83 |
83 |
84 /** Start reading a Pdo's entries. |
84 /** Start reading a PDO's entries. |
85 */ |
85 */ |
86 void ec_fsm_pdo_entry_start_reading( |
86 void ec_fsm_pdo_entry_start_reading( |
87 ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ |
87 ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */ |
88 ec_slave_t *slave, /**< slave to configure */ |
88 ec_slave_t *slave, /**< slave to configure */ |
89 ec_pdo_t *pdo /**< Pdo to read entries for. */ |
89 ec_pdo_t *pdo /**< PDO to read entries for. */ |
90 ) |
90 ) |
91 { |
91 { |
92 fsm->slave = slave; |
92 fsm->slave = slave; |
93 fsm->target_pdo = pdo; |
93 fsm->target_pdo = pdo; |
94 |
94 |
97 fsm->state = ec_fsm_pdo_entry_read_state_start; |
97 fsm->state = ec_fsm_pdo_entry_read_state_start; |
98 } |
98 } |
99 |
99 |
100 /*****************************************************************************/ |
100 /*****************************************************************************/ |
101 |
101 |
102 /** Start Pdo mapping state machine. |
102 /** Start PDO mapping state machine. |
103 */ |
103 */ |
104 void ec_fsm_pdo_entry_start_configuration( |
104 void ec_fsm_pdo_entry_start_configuration( |
105 ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ |
105 ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */ |
106 ec_slave_t *slave, /**< slave to configure */ |
106 ec_slave_t *slave, /**< slave to configure */ |
107 const ec_pdo_t *pdo /**< Pdo with the desired entries. */ |
107 const ec_pdo_t *pdo /**< PDO with the desired entries. */ |
108 ) |
108 ) |
109 { |
109 { |
110 fsm->slave = slave; |
110 fsm->slave = slave; |
111 fsm->source_pdo = pdo; |
111 fsm->source_pdo = pdo; |
112 |
112 |
146 /** Get execution result. |
146 /** Get execution result. |
147 * |
147 * |
148 * \return true, if the state machine terminated gracefully |
148 * \return true, if the state machine terminated gracefully |
149 */ |
149 */ |
150 int ec_fsm_pdo_entry_success( |
150 int ec_fsm_pdo_entry_success( |
151 const ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
151 const ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
152 ) |
152 ) |
153 { |
153 { |
154 return fsm->state == ec_fsm_pdo_entry_state_end; |
154 return fsm->state == ec_fsm_pdo_entry_state_end; |
155 } |
155 } |
156 |
156 |
157 /****************************************************************************** |
157 /****************************************************************************** |
158 * Reading state functions. |
158 * Reading state functions. |
159 *****************************************************************************/ |
159 *****************************************************************************/ |
160 |
160 |
161 /** Request reading the number of mapped Pdo entries. |
161 /** Request reading the number of mapped PDO entries. |
162 */ |
162 */ |
163 void ec_fsm_pdo_entry_read_state_start( |
163 void ec_fsm_pdo_entry_read_state_start( |
164 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
164 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
165 ) |
165 ) |
166 { |
166 { |
167 ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, 0); |
167 ec_sdo_request_address(&fsm->request, fsm->target_pdo->index, 0); |
168 ecrt_sdo_request_read(&fsm->request); |
168 ecrt_sdo_request_read(&fsm->request); |
169 |
169 |
172 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
172 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
173 } |
173 } |
174 |
174 |
175 /*****************************************************************************/ |
175 /*****************************************************************************/ |
176 |
176 |
177 /** Read number of mapped Pdo entries. |
177 /** Read number of mapped PDO entries. |
178 */ |
178 */ |
179 void ec_fsm_pdo_entry_read_state_count( |
179 void ec_fsm_pdo_entry_read_state_count( |
180 ec_fsm_pdo_entry_t *fsm /**< finite state machine */ |
180 ec_fsm_pdo_entry_t *fsm /**< finite state machine */ |
181 ) |
181 ) |
182 { |
182 { |
183 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
183 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
184 return; |
184 return; |
185 |
185 |
186 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
186 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
187 EC_ERR("Failed to read number of mapped Pdo entries.\n"); |
187 EC_ERR("Failed to read number of mapped PDO entries.\n"); |
188 fsm->state = ec_fsm_pdo_entry_state_error; |
188 fsm->state = ec_fsm_pdo_entry_state_error; |
189 return; |
189 return; |
190 } |
190 } |
191 |
191 |
192 if (fsm->request.data_size != sizeof(uint8_t)) { |
192 if (fsm->request.data_size != sizeof(uint8_t)) { |
193 EC_ERR("Invalid data size %u at uploading Sdo 0x%04X:%02X.\n", |
193 EC_ERR("Invalid data size %u at uploading SDO 0x%04X:%02X.\n", |
194 fsm->request.data_size, fsm->request.index, |
194 fsm->request.data_size, fsm->request.index, |
195 fsm->request.subindex); |
195 fsm->request.subindex); |
196 fsm->state = ec_fsm_pdo_entry_state_error; |
196 fsm->state = ec_fsm_pdo_entry_state_error; |
197 return; |
197 return; |
198 } |
198 } |
199 |
199 |
200 fsm->entry_count = EC_READ_U8(fsm->request.data); |
200 fsm->entry_count = EC_READ_U8(fsm->request.data); |
201 |
201 |
202 if (fsm->slave->master->debug_level) |
202 if (fsm->slave->master->debug_level) |
203 EC_DBG("%u Pdo entries mapped.\n", fsm->entry_count); |
203 EC_DBG("%u PDO entries mapped.\n", fsm->entry_count); |
204 |
204 |
205 // read first Pdo entry |
205 // read first PDO entry |
206 fsm->entry_pos = 1; |
206 fsm->entry_pos = 1; |
207 ec_fsm_pdo_entry_read_action_next(fsm); |
207 ec_fsm_pdo_entry_read_action_next(fsm); |
208 } |
208 } |
209 |
209 |
210 /*****************************************************************************/ |
210 /*****************************************************************************/ |
211 |
211 |
212 /** Read next Pdo entry. |
212 /** Read next PDO entry. |
213 */ |
213 */ |
214 void ec_fsm_pdo_entry_read_action_next( |
214 void ec_fsm_pdo_entry_read_action_next( |
215 ec_fsm_pdo_entry_t *fsm /**< finite state machine */ |
215 ec_fsm_pdo_entry_t *fsm /**< finite state machine */ |
216 ) |
216 ) |
217 { |
217 { |
228 fsm->state = ec_fsm_pdo_entry_state_end; |
228 fsm->state = ec_fsm_pdo_entry_state_end; |
229 } |
229 } |
230 |
230 |
231 /*****************************************************************************/ |
231 /*****************************************************************************/ |
232 |
232 |
233 /** Read Pdo entry information. |
233 /** Read PDO entry information. |
234 */ |
234 */ |
235 void ec_fsm_pdo_entry_read_state_entry( |
235 void ec_fsm_pdo_entry_read_state_entry( |
236 ec_fsm_pdo_entry_t *fsm /**< finite state machine */ |
236 ec_fsm_pdo_entry_t *fsm /**< finite state machine */ |
237 ) |
237 ) |
238 { |
238 { |
239 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
239 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
240 |
240 |
241 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
241 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
242 EC_ERR("Failed to read mapped Pdo entry.\n"); |
242 EC_ERR("Failed to read mapped PDO entry.\n"); |
243 fsm->state = ec_fsm_pdo_entry_state_error; |
243 fsm->state = ec_fsm_pdo_entry_state_error; |
244 return; |
244 return; |
245 } |
245 } |
246 |
246 |
247 if (fsm->request.data_size != sizeof(uint32_t)) { |
247 if (fsm->request.data_size != sizeof(uint32_t)) { |
248 EC_ERR("Invalid data size %u at uploading Sdo 0x%04X:%02X.\n", |
248 EC_ERR("Invalid data size %u at uploading SDO 0x%04X:%02X.\n", |
249 fsm->request.data_size, fsm->request.index, |
249 fsm->request.data_size, fsm->request.index, |
250 fsm->request.subindex); |
250 fsm->request.subindex); |
251 fsm->state = ec_fsm_pdo_entry_state_error; |
251 fsm->state = ec_fsm_pdo_entry_state_error; |
252 } else { |
252 } else { |
253 uint32_t pdo_entry_info; |
253 uint32_t pdo_entry_info; |
275 return; |
275 return; |
276 } |
276 } |
277 } |
277 } |
278 |
278 |
279 if (fsm->slave->master->debug_level) { |
279 if (fsm->slave->master->debug_level) { |
280 EC_DBG("Pdo entry 0x%04X:%02X, %u bit, \"%s\".\n", |
280 EC_DBG("PDO entry 0x%04X:%02X, %u bit, \"%s\".\n", |
281 pdo_entry->index, pdo_entry->subindex, |
281 pdo_entry->index, pdo_entry->subindex, |
282 pdo_entry->bit_length, |
282 pdo_entry->bit_length, |
283 pdo_entry->name ? pdo_entry->name : "???"); |
283 pdo_entry->name ? pdo_entry->name : "???"); |
284 } |
284 } |
285 |
285 |
286 list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries); |
286 list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries); |
287 |
287 |
288 // next Pdo entry |
288 // next PDO entry |
289 fsm->entry_pos++; |
289 fsm->entry_pos++; |
290 ec_fsm_pdo_entry_read_action_next(fsm); |
290 ec_fsm_pdo_entry_read_action_next(fsm); |
291 } |
291 } |
292 } |
292 } |
293 |
293 |
294 /****************************************************************************** |
294 /****************************************************************************** |
295 * Configuration state functions. |
295 * Configuration state functions. |
296 *****************************************************************************/ |
296 *****************************************************************************/ |
297 |
297 |
298 /** Start Pdo mapping. |
298 /** Start PDO mapping. |
299 */ |
299 */ |
300 void ec_fsm_pdo_entry_conf_state_start( |
300 void ec_fsm_pdo_entry_conf_state_start( |
301 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
301 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
302 ) |
302 ) |
303 { |
303 { |
304 // Pdo mapping has to be changed. Does the slave support this? |
304 // PDO mapping has to be changed. Does the slave support this? |
305 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
305 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
306 || (fsm->slave->sii.has_general |
306 || (fsm->slave->sii.has_general |
307 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { |
307 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { |
308 EC_WARN("Slave %u does not support changing the Pdo mapping!\n", |
308 EC_WARN("Slave %u does not support changing the PDO mapping!\n", |
309 fsm->slave->ring_position); |
309 fsm->slave->ring_position); |
310 fsm->state = ec_fsm_pdo_entry_state_error; |
310 fsm->state = ec_fsm_pdo_entry_state_error; |
311 return; |
311 return; |
312 } |
312 } |
313 |
313 |
314 if (ec_sdo_request_alloc(&fsm->request, 4)) { |
314 if (ec_sdo_request_alloc(&fsm->request, 4)) { |
315 fsm->state = ec_fsm_pdo_entry_state_error; |
315 fsm->state = ec_fsm_pdo_entry_state_error; |
316 return; |
316 return; |
317 } |
317 } |
318 |
318 |
319 // set mapped Pdo entry count to zero |
319 // set mapped PDO entry count to zero |
320 EC_WRITE_U8(fsm->request.data, 0); |
320 EC_WRITE_U8(fsm->request.data, 0); |
321 fsm->request.data_size = 1; |
321 fsm->request.data_size = 1; |
322 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
322 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
323 ecrt_sdo_request_write(&fsm->request); |
323 ecrt_sdo_request_write(&fsm->request); |
324 |
324 |
330 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
330 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
331 } |
331 } |
332 |
332 |
333 /*****************************************************************************/ |
333 /*****************************************************************************/ |
334 |
334 |
335 /** Process next Pdo entry. |
335 /** Process next PDO entry. |
336 */ |
336 */ |
337 ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry( |
337 ec_pdo_entry_t *ec_fsm_pdo_entry_conf_next_entry( |
338 const ec_fsm_pdo_entry_t *fsm, /**< Pdo mapping state machine. */ |
338 const ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */ |
339 const struct list_head *list /**< current entry list item */ |
339 const struct list_head *list /**< current entry list item */ |
340 ) |
340 ) |
341 { |
341 { |
342 list = list->next; |
342 list = list->next; |
343 if (list == &fsm->source_pdo->entries) |
343 if (list == &fsm->source_pdo->entries) |
348 /*****************************************************************************/ |
348 /*****************************************************************************/ |
349 |
349 |
350 /** Set the number of mapped entries to zero. |
350 /** Set the number of mapped entries to zero. |
351 */ |
351 */ |
352 void ec_fsm_pdo_entry_conf_state_zero_entry_count( |
352 void ec_fsm_pdo_entry_conf_state_zero_entry_count( |
353 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
353 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
354 ) |
354 ) |
355 { |
355 { |
356 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
356 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
357 return; |
357 return; |
358 |
358 |
359 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
359 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
360 EC_WARN("Failed to clear Pdo mapping.\n"); |
360 EC_WARN("Failed to clear PDO mapping.\n"); |
361 fsm->state = ec_fsm_pdo_entry_state_error; |
361 fsm->state = ec_fsm_pdo_entry_state_error; |
362 return; |
362 return; |
363 } |
363 } |
364 |
364 |
365 // find first entry |
365 // find first entry |
378 ec_fsm_pdo_entry_conf_action_map(fsm); |
378 ec_fsm_pdo_entry_conf_action_map(fsm); |
379 } |
379 } |
380 |
380 |
381 /*****************************************************************************/ |
381 /*****************************************************************************/ |
382 |
382 |
383 /** Starts to add a Pdo entry. |
383 /** Starts to add a PDO entry. |
384 */ |
384 */ |
385 void ec_fsm_pdo_entry_conf_action_map( |
385 void ec_fsm_pdo_entry_conf_action_map( |
386 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
386 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
387 ) |
387 ) |
388 { |
388 { |
389 uint32_t value; |
389 uint32_t value; |
390 |
390 |
391 if (fsm->slave->master->debug_level) |
391 if (fsm->slave->master->debug_level) |
392 EC_DBG("Mapping Pdo entry 0x%04X:%02X (%u bit) at position %u.\n", |
392 EC_DBG("Mapping PDO entry 0x%04X:%02X (%u bit) at position %u.\n", |
393 fsm->entry->index, fsm->entry->subindex, |
393 fsm->entry->index, fsm->entry->subindex, |
394 fsm->entry->bit_length, fsm->entry_pos); |
394 fsm->entry->bit_length, fsm->entry_pos); |
395 |
395 |
396 value = fsm->entry->index << 16 |
396 value = fsm->entry->index << 16 |
397 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
397 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
405 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
405 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
406 } |
406 } |
407 |
407 |
408 /*****************************************************************************/ |
408 /*****************************************************************************/ |
409 |
409 |
410 /** Add a Pdo entry. |
410 /** Add a PDO entry. |
411 */ |
411 */ |
412 void ec_fsm_pdo_entry_conf_state_map_entry( |
412 void ec_fsm_pdo_entry_conf_state_map_entry( |
413 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
413 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
414 ) |
414 ) |
415 { |
415 { |
416 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
416 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
417 |
417 |
418 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
418 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
419 EC_WARN("Failed to map Pdo entry 0x%04X:%02X (%u bit) to " |
419 EC_WARN("Failed to map PDO entry 0x%04X:%02X (%u bit) to " |
420 "position %u.\n", fsm->entry->index, fsm->entry->subindex, |
420 "position %u.\n", fsm->entry->index, fsm->entry->subindex, |
421 fsm->entry->bit_length, fsm->entry_pos); |
421 fsm->entry->bit_length, fsm->entry_pos); |
422 fsm->state = ec_fsm_pdo_entry_state_error; |
422 fsm->state = ec_fsm_pdo_entry_state_error; |
423 return; |
423 return; |
424 } |
424 } |
432 fsm->request.data_size = 1; |
432 fsm->request.data_size = 1; |
433 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
433 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
434 ecrt_sdo_request_write(&fsm->request); |
434 ecrt_sdo_request_write(&fsm->request); |
435 |
435 |
436 if (fsm->slave->master->debug_level) |
436 if (fsm->slave->master->debug_level) |
437 EC_DBG("Setting number of Pdo entries to %u.\n", fsm->entry_pos); |
437 EC_DBG("Setting number of PDO entries to %u.\n", fsm->entry_pos); |
438 |
438 |
439 fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count; |
439 fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count; |
440 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
440 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
441 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
441 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
442 return; |
442 return; |
475 *****************************************************************************/ |
475 *****************************************************************************/ |
476 |
476 |
477 /** State: ERROR. |
477 /** State: ERROR. |
478 */ |
478 */ |
479 void ec_fsm_pdo_entry_state_error( |
479 void ec_fsm_pdo_entry_state_error( |
480 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
480 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
481 ) |
481 ) |
482 { |
482 { |
483 } |
483 } |
484 |
484 |
485 /*****************************************************************************/ |
485 /*****************************************************************************/ |
486 |
486 |
487 /** State: END. |
487 /** State: END. |
488 */ |
488 */ |
489 void ec_fsm_pdo_entry_state_end( |
489 void ec_fsm_pdo_entry_state_end( |
490 ec_fsm_pdo_entry_t *fsm /**< Pdo mapping state machine. */ |
490 ec_fsm_pdo_entry_t *fsm /**< PDO mapping state machine. */ |
491 ) |
491 ) |
492 { |
492 { |
493 } |
493 } |
494 |
494 |
495 /*****************************************************************************/ |
495 /*****************************************************************************/ |