129 fsm->slave = slave; |
129 fsm->slave = slave; |
130 fsm->source_pdo = pdo; |
130 fsm->source_pdo = pdo; |
131 fsm->cur_pdo = cur_pdo; |
131 fsm->cur_pdo = cur_pdo; |
132 |
132 |
133 if (fsm->slave->master->debug_level) { |
133 if (fsm->slave->master->debug_level) { |
134 EC_DBG("Changing mapping of PDO 0x%04X.\n", pdo->index); |
134 EC_SLAVE_DBG(slave, 1, "Changing mapping of PDO 0x%04X.\n", |
135 EC_DBG(""); ec_fsm_pdo_entry_print(fsm); |
135 pdo->index); |
|
136 EC_SLAVE_DBG(slave, 1, ""); ec_fsm_pdo_entry_print(fsm); |
136 } |
137 } |
137 |
138 |
138 fsm->state = ec_fsm_pdo_entry_conf_state_start; |
139 fsm->state = ec_fsm_pdo_entry_conf_state_start; |
139 } |
140 } |
140 |
141 |
207 { |
208 { |
208 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
209 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
209 return; |
210 return; |
210 |
211 |
211 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
212 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
212 EC_ERR("Failed to read number of mapped PDO entries.\n"); |
213 EC_SLAVE_ERR(fsm->slave, |
|
214 "Failed to read number of mapped PDO entries.\n"); |
213 fsm->state = ec_fsm_pdo_entry_state_error; |
215 fsm->state = ec_fsm_pdo_entry_state_error; |
214 return; |
216 return; |
215 } |
217 } |
216 |
218 |
217 if (fsm->request.data_size != sizeof(uint8_t)) { |
219 if (fsm->request.data_size != sizeof(uint8_t)) { |
218 EC_ERR("Invalid data size %zu at uploading SDO 0x%04X:%02X.\n", |
220 EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at uploading" |
|
221 " SDO 0x%04X:%02X.\n", |
219 fsm->request.data_size, fsm->request.index, |
222 fsm->request.data_size, fsm->request.index, |
220 fsm->request.subindex); |
223 fsm->request.subindex); |
221 fsm->state = ec_fsm_pdo_entry_state_error; |
224 fsm->state = ec_fsm_pdo_entry_state_error; |
222 return; |
225 return; |
223 } |
226 } |
224 |
227 |
225 fsm->entry_count = EC_READ_U8(fsm->request.data); |
228 fsm->entry_count = EC_READ_U8(fsm->request.data); |
226 |
229 |
227 if (fsm->slave->master->debug_level) |
230 EC_SLAVE_DBG(fsm->slave, 1, "%u PDO entries mapped.\n", fsm->entry_count); |
228 EC_DBG("%u PDO entries mapped.\n", fsm->entry_count); |
|
229 |
231 |
230 // read first PDO entry |
232 // read first PDO entry |
231 fsm->entry_pos = 1; |
233 fsm->entry_pos = 1; |
232 ec_fsm_pdo_entry_read_action_next(fsm); |
234 ec_fsm_pdo_entry_read_action_next(fsm); |
233 } |
235 } |
262 ) |
264 ) |
263 { |
265 { |
264 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
266 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
265 |
267 |
266 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
268 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
267 EC_ERR("Failed to read mapped PDO entry.\n"); |
269 EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n"); |
268 fsm->state = ec_fsm_pdo_entry_state_error; |
270 fsm->state = ec_fsm_pdo_entry_state_error; |
269 return; |
271 return; |
270 } |
272 } |
271 |
273 |
272 if (fsm->request.data_size != sizeof(uint32_t)) { |
274 if (fsm->request.data_size != sizeof(uint32_t)) { |
273 EC_ERR("Invalid data size %zu at uploading SDO 0x%04X:%02X.\n", |
275 EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at" |
|
276 " uploading SDO 0x%04X:%02X.\n", |
274 fsm->request.data_size, fsm->request.index, |
277 fsm->request.data_size, fsm->request.index, |
275 fsm->request.subindex); |
278 fsm->request.subindex); |
276 fsm->state = ec_fsm_pdo_entry_state_error; |
279 fsm->state = ec_fsm_pdo_entry_state_error; |
277 } else { |
280 } else { |
278 uint32_t pdo_entry_info; |
281 uint32_t pdo_entry_info; |
280 |
283 |
281 pdo_entry_info = EC_READ_U32(fsm->request.data); |
284 pdo_entry_info = EC_READ_U32(fsm->request.data); |
282 |
285 |
283 if (!(pdo_entry = (ec_pdo_entry_t *) |
286 if (!(pdo_entry = (ec_pdo_entry_t *) |
284 kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
287 kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
285 EC_ERR("Failed to allocate PDO entry.\n"); |
288 EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO entry.\n"); |
286 fsm->state = ec_fsm_pdo_entry_state_error; |
289 fsm->state = ec_fsm_pdo_entry_state_error; |
287 return; |
290 return; |
288 } |
291 } |
289 |
292 |
290 ec_pdo_entry_init(pdo_entry); |
293 ec_pdo_entry_init(pdo_entry); |
299 fsm->state = ec_fsm_pdo_entry_state_error; |
302 fsm->state = ec_fsm_pdo_entry_state_error; |
300 return; |
303 return; |
301 } |
304 } |
302 } |
305 } |
303 |
306 |
304 if (fsm->slave->master->debug_level) { |
307 EC_SLAVE_DBG(fsm->slave, 1, |
305 EC_DBG("PDO entry 0x%04X:%02X, %u bit, \"%s\".\n", |
308 "PDO entry 0x%04X:%02X, %u bit, \"%s\".\n", |
306 pdo_entry->index, pdo_entry->subindex, |
309 pdo_entry->index, pdo_entry->subindex, |
307 pdo_entry->bit_length, |
310 pdo_entry->bit_length, |
308 pdo_entry->name ? pdo_entry->name : "???"); |
311 pdo_entry->name ? pdo_entry->name : "???"); |
309 } |
|
310 |
312 |
311 list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries); |
313 list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries); |
312 |
314 |
313 // next PDO entry |
315 // next PDO entry |
314 fsm->entry_pos++; |
316 fsm->entry_pos++; |
328 { |
330 { |
329 // PDO mapping has to be changed. Does the slave support this? |
331 // PDO mapping has to be changed. Does the slave support this? |
330 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
332 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
331 || (fsm->slave->sii.has_general |
333 || (fsm->slave->sii.has_general |
332 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { |
334 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { |
333 EC_WARN("Slave %u does not support changing the PDO mapping!\n", |
335 EC_SLAVE_WARN(fsm->slave, "Slave does not support" |
334 fsm->slave->ring_position); |
336 " changing the PDO mapping!\n"); |
335 EC_WARN(""); ec_fsm_pdo_entry_print(fsm); |
337 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); |
336 fsm->state = ec_fsm_pdo_entry_state_error; |
338 fsm->state = ec_fsm_pdo_entry_state_error; |
337 return; |
339 return; |
338 } |
340 } |
339 |
341 |
340 if (ec_sdo_request_alloc(&fsm->request, 4)) { |
342 if (ec_sdo_request_alloc(&fsm->request, 4)) { |
346 EC_WRITE_U8(fsm->request.data, 0); |
348 EC_WRITE_U8(fsm->request.data, 0); |
347 fsm->request.data_size = 1; |
349 fsm->request.data_size = 1; |
348 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
350 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
349 ecrt_sdo_request_write(&fsm->request); |
351 ecrt_sdo_request_write(&fsm->request); |
350 |
352 |
351 if (fsm->slave->master->debug_level) |
353 EC_SLAVE_DBG(fsm->slave, 1, "Setting entry count to zero.\n"); |
352 EC_DBG("Setting entry count to zero.\n"); |
|
353 |
354 |
354 fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count; |
355 fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count; |
355 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
356 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
356 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
357 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
357 } |
358 } |
381 { |
382 { |
382 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
383 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
383 return; |
384 return; |
384 |
385 |
385 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
386 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
386 EC_WARN("Failed to clear PDO mapping.\n"); |
387 EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n"); |
387 EC_WARN(""); ec_fsm_pdo_entry_print(fsm); |
388 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); |
388 fsm->state = ec_fsm_pdo_entry_state_error; |
389 fsm->state = ec_fsm_pdo_entry_state_error; |
389 return; |
390 return; |
390 } |
391 } |
391 |
392 |
392 // find first entry |
393 // find first entry |
393 if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry( |
394 if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry( |
394 fsm, &fsm->source_pdo->entries))) { |
395 fsm, &fsm->source_pdo->entries))) { |
395 |
396 |
396 if (fsm->slave->master->debug_level) |
397 EC_SLAVE_DBG(fsm->slave, 1, "No entries to map.\n"); |
397 EC_DBG("No entries to map.\n"); |
|
398 |
398 |
399 fsm->state = ec_fsm_pdo_entry_state_end; // finished |
399 fsm->state = ec_fsm_pdo_entry_state_end; // finished |
400 return; |
400 return; |
401 } |
401 } |
402 |
402 |
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 uint32_t value; |
416 uint32_t value; |
417 |
417 |
418 if (fsm->slave->master->debug_level) |
418 EC_SLAVE_DBG(fsm->slave, 1, "Mapping PDO entry 0x%04X:%02X (%u bit)" |
419 EC_DBG("Mapping PDO entry 0x%04X:%02X (%u bit) at position %u.\n", |
419 " at position %u.\n", |
420 fsm->entry->index, fsm->entry->subindex, |
420 fsm->entry->index, fsm->entry->subindex, |
421 fsm->entry->bit_length, fsm->entry_pos); |
421 fsm->entry->bit_length, fsm->entry_pos); |
422 |
422 |
423 value = fsm->entry->index << 16 |
423 value = fsm->entry->index << 16 |
424 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
424 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
425 EC_WRITE_U32(fsm->request.data, value); |
425 EC_WRITE_U32(fsm->request.data, value); |
426 fsm->request.data_size = 4; |
426 fsm->request.data_size = 4; |
441 ) |
441 ) |
442 { |
442 { |
443 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
443 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
444 |
444 |
445 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
445 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
446 EC_WARN("Failed to map PDO entry 0x%04X:%02X (%u bit) to " |
446 EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry" |
447 "position %u.\n", fsm->entry->index, fsm->entry->subindex, |
447 " 0x%04X:%02X (%u bit) to position %u.\n", |
|
448 fsm->entry->index, fsm->entry->subindex, |
448 fsm->entry->bit_length, fsm->entry_pos); |
449 fsm->entry->bit_length, fsm->entry_pos); |
449 EC_WARN(""); ec_fsm_pdo_entry_print(fsm); |
450 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); |
450 fsm->state = ec_fsm_pdo_entry_state_error; |
451 fsm->state = ec_fsm_pdo_entry_state_error; |
451 return; |
452 return; |
452 } |
453 } |
453 |
454 |
454 // find next entry |
455 // find next entry |
459 EC_WRITE_U8(fsm->request.data, fsm->entry_pos); |
460 EC_WRITE_U8(fsm->request.data, fsm->entry_pos); |
460 fsm->request.data_size = 1; |
461 fsm->request.data_size = 1; |
461 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
462 ec_sdo_request_address(&fsm->request, fsm->source_pdo->index, 0); |
462 ecrt_sdo_request_write(&fsm->request); |
463 ecrt_sdo_request_write(&fsm->request); |
463 |
464 |
464 if (fsm->slave->master->debug_level) |
465 EC_SLAVE_DBG(fsm->slave, 1, "Setting number of PDO entries to %u.\n", |
465 EC_DBG("Setting number of PDO entries to %u.\n", fsm->entry_pos); |
466 fsm->entry_pos); |
466 |
467 |
467 fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count; |
468 fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count; |
468 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
469 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
469 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
470 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
470 return; |
471 return; |
484 ) |
485 ) |
485 { |
486 { |
486 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
487 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
487 |
488 |
488 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
489 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
489 EC_WARN("Failed to set number of entries.\n"); |
490 EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n"); |
490 EC_WARN(""); ec_fsm_pdo_entry_print(fsm); |
491 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm); |
491 fsm->state = ec_fsm_pdo_entry_state_error; |
492 fsm->state = ec_fsm_pdo_entry_state_error; |
492 return; |
493 return; |
493 } |
494 } |
494 |
495 |
495 if (fsm->slave->master->debug_level) |
496 EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured" |
496 EC_DBG("Successfully configured mapping for PDO 0x%04X.\n", |
497 " mapping for PDO 0x%04X.\n", fsm->source_pdo->index); |
497 fsm->source_pdo->index); |
|
498 |
498 |
499 fsm->state = ec_fsm_pdo_entry_state_end; // finished |
499 fsm->state = ec_fsm_pdo_entry_state_end; // finished |
500 } |
500 } |
501 |
501 |
502 /****************************************************************************** |
502 /****************************************************************************** |