184 } |
184 } |
185 } |
185 } |
186 |
186 |
187 if (!fsm->pdo) { |
187 if (!fsm->pdo) { |
188 if (fsm->slave->master->debug_level && !fsm->num_configured_pdos) |
188 if (fsm->slave->master->debug_level && !fsm->num_configured_pdos) |
189 EC_DBG("Pdo mappings of slave %u are already configured" |
189 EC_DBG("Pdo mappings are already configured correctly.\n"); |
190 " correctly.\n", fsm->slave->ring_position); |
|
191 fsm->state = ec_fsm_pdo_mapping_state_end; |
190 fsm->state = ec_fsm_pdo_mapping_state_end; |
192 return; |
191 return; |
193 } |
192 } |
194 |
193 |
195 // Pdo mapping has to be changed. Does the slave support this? |
194 // Pdo mapping has to be changed. Does the slave support this? |
196 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
195 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
197 || (fsm->slave->sii.has_general |
196 || (fsm->slave->sii.has_general |
198 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { |
197 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) { |
199 EC_ERR("Slave %u does not support changing the Pdo mapping!\n", |
198 EC_WARN("Slave %u does not support Pdo mapping configuration!\n", |
200 fsm->slave->ring_position); |
199 fsm->slave->ring_position); |
201 fsm->state = ec_fsm_pdo_mapping_state_error; |
200 fsm->state = ec_fsm_pdo_mapping_state_error; |
202 return; |
201 return; |
203 } |
202 } |
204 |
203 |
205 if (fsm->slave->master->debug_level) { |
204 if (fsm->slave->master->debug_level) { |
206 EC_DBG("Changing mapping of Pdo 0x%04X of slave %u.\n", |
205 EC_DBG("Changing mapping of Pdo 0x%04X.\n", fsm->pdo->index); |
207 fsm->pdo->index, fsm->slave->ring_position); |
|
208 } |
206 } |
209 |
207 |
210 if (ec_sdo_request_alloc(&fsm->request, 4)) { |
208 if (ec_sdo_request_alloc(&fsm->request, 4)) { |
211 fsm->state = ec_fsm_pdo_mapping_state_error; |
209 fsm->state = ec_fsm_pdo_mapping_state_error; |
212 return; |
210 return; |
216 EC_WRITE_U8(fsm->request.data, 0); |
214 EC_WRITE_U8(fsm->request.data, 0); |
217 fsm->request.data_size = 1; |
215 fsm->request.data_size = 1; |
218 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
216 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
219 ecrt_sdo_request_write(&fsm->request); |
217 ecrt_sdo_request_write(&fsm->request); |
220 if (fsm->slave->master->debug_level) |
218 if (fsm->slave->master->debug_level) |
221 EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n", |
219 EC_DBG("Setting entry count to zero.\n"); |
222 fsm->pdo->index); |
|
223 |
220 |
224 fsm->state = ec_fsm_pdo_mapping_state_zero_count; |
221 fsm->state = ec_fsm_pdo_mapping_state_zero_count; |
225 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
222 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
226 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
223 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
227 } |
224 } |
277 ) |
274 ) |
278 { |
275 { |
279 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
276 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
280 |
277 |
281 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
278 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
282 EC_ERR("Failed to clear Pdo mapping for slave %u.\n", |
279 EC_WARN("Failed to clear mapping of Pdo 0x%04X.\n", fsm->pdo->index); |
283 fsm->slave->ring_position); |
|
284 fsm->state = ec_fsm_pdo_mapping_state_error; |
280 fsm->state = ec_fsm_pdo_mapping_state_error; |
285 return; |
281 return; |
286 } |
282 } |
287 |
283 |
288 // find first entry |
284 // find first entry |
289 if (!(fsm->entry = |
285 if (!(fsm->entry = |
290 ec_fsm_pdo_mapping_next_entry(fsm, &fsm->pdo->entries))) { |
286 ec_fsm_pdo_mapping_next_entry(fsm, &fsm->pdo->entries))) { |
291 if (fsm->slave->master->debug_level) |
287 if (fsm->slave->master->debug_level) |
292 EC_DBG("No entries to map for Pdo 0x%04X of slave %u.\n", |
288 EC_DBG("No entries to map.\n"); |
293 fsm->pdo->index, fsm->slave->ring_position); |
|
294 ec_fsm_pdo_mapping_next_pdo(fsm); |
289 ec_fsm_pdo_mapping_next_pdo(fsm); |
295 return; |
290 return; |
296 } |
291 } |
297 |
292 |
298 // add first entry |
293 // add first entry |
309 ) |
304 ) |
310 { |
305 { |
311 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
306 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
312 |
307 |
313 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
308 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
314 EC_ERR("Failed to add entry 0x%04X:%02X for slave %u.\n", |
309 EC_WARN("Failed to map Pdo entry 0x%04X:%02X at 0x%04X:%02X.\n", |
315 fsm->entry->index, fsm->entry->subindex, |
310 fsm->entry->index, fsm->entry->subindex, fsm->pdo->index, |
316 fsm->slave->ring_position); |
311 fsm->entry_count); |
317 fsm->state = ec_fsm_pdo_mapping_state_error; |
312 fsm->state = ec_fsm_pdo_mapping_state_error; |
318 return; |
313 return; |
319 } |
314 } |
320 |
315 |
321 // find next entry |
316 // find next entry |
323 // No more entries to add. Write entry count. |
318 // No more entries to add. Write entry count. |
324 EC_WRITE_U8(fsm->request.data, fsm->entry_count); |
319 EC_WRITE_U8(fsm->request.data, fsm->entry_count); |
325 fsm->request.data_size = 1; |
320 fsm->request.data_size = 1; |
326 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
321 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
327 ecrt_sdo_request_write(&fsm->request); |
322 ecrt_sdo_request_write(&fsm->request); |
|
323 |
328 if (fsm->slave->master->debug_level) |
324 if (fsm->slave->master->debug_level) |
329 EC_DBG("Setting number of Pdo entries to %u.\n", |
325 EC_DBG("Setting number of Pdo entries to %u.\n", fsm->entry_count); |
330 fsm->entry_count); |
|
331 |
326 |
332 fsm->state = ec_fsm_pdo_mapping_state_entry_count; |
327 fsm->state = ec_fsm_pdo_mapping_state_entry_count; |
333 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
328 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
334 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
329 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
335 return; |
330 return; |
349 ) |
344 ) |
350 { |
345 { |
351 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
346 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
352 |
347 |
353 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
348 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
354 EC_ERR("Failed to set number of entries for slave %u.\n", |
349 EC_WARN("Failed to set number of entries for Pdo 0x%04X.\n", |
355 fsm->slave->ring_position); |
350 fsm->pdo->index); |
356 fsm->state = ec_fsm_pdo_mapping_state_error; |
351 fsm->state = ec_fsm_pdo_mapping_state_error; |
357 return; |
352 return; |
358 } |
353 } |
359 |
354 |
360 if (fsm->slave->master->debug_level) |
355 if (fsm->slave->master->debug_level) |
361 EC_DBG("Successfully configured mapping for Pdo 0x%04X on slave %u.\n", |
356 EC_DBG("Pdo mapping configuration successful.\n"); |
362 fsm->pdo->index, fsm->slave->ring_position); |
|
363 |
357 |
364 ec_fsm_pdo_mapping_next_pdo(fsm); |
358 ec_fsm_pdo_mapping_next_pdo(fsm); |
365 } |
359 } |
366 |
360 |
367 /****************************************************************************** |
361 /****************************************************************************** |