193 fsm->state = ec_fsm_pdo_mapping_state_end; |
193 fsm->state = ec_fsm_pdo_mapping_state_end; |
194 return; |
194 return; |
195 } |
195 } |
196 |
196 |
197 if (fsm->slave->master->debug_level) { |
197 if (fsm->slave->master->debug_level) { |
198 EC_DBG("Configuring PDO mapping for SM%u of slave %u.\n", |
198 EC_DBG("Configuring Pdo mapping for SM%u of slave %u.\n", |
199 fsm->sync->index, fsm->slave->ring_position); |
199 fsm->sync->index, fsm->slave->ring_position); |
200 } |
200 } |
201 |
201 |
202 // set mapped PDO count to zero |
202 // set mapped Pdo count to zero |
203 fsm->sdodata.index = 0x1C10 + fsm->sync->index; |
203 fsm->sdodata.index = 0x1C10 + fsm->sync->index; |
204 fsm->sdodata.subindex = 0; // mapped PDO count |
204 fsm->sdodata.subindex = 0; // mapped Pdo count |
205 EC_WRITE_U8(&fsm->sdo_value, 0); // zero PDOs mapped |
205 EC_WRITE_U8(&fsm->sdo_value, 0); // zero Pdos mapped |
206 fsm->sdodata.size = 1; |
206 fsm->sdodata.size = 1; |
207 if (fsm->slave->master->debug_level) |
207 if (fsm->slave->master->debug_level) |
208 EC_DBG("Setting PDO count to zero for SM%u.\n", fsm->sync->index); |
208 EC_DBG("Setting Pdo count to zero for SM%u.\n", fsm->sync->index); |
209 |
209 |
210 fsm->state = ec_fsm_pdo_mapping_state_zero_count; |
210 fsm->state = ec_fsm_pdo_mapping_state_zero_count; |
211 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
211 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
212 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
212 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
213 } |
213 } |
214 |
214 |
215 /*****************************************************************************/ |
215 /*****************************************************************************/ |
216 |
216 |
217 /** Process mapping of next PDO. |
217 /** Process mapping of next Pdo. |
218 */ |
218 */ |
219 ec_pdo_t *ec_fsm_pdo_mapping_next_pdo( |
219 ec_pdo_t *ec_fsm_pdo_mapping_next_pdo( |
220 const ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */ |
220 const ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */ |
221 const struct list_head *list /**< current PDO list item */ |
221 const struct list_head *list /**< current Pdo list item */ |
222 ) |
222 ) |
223 { |
223 { |
224 list = list->next; |
224 list = list->next; |
225 if (list == &fsm->mapping->pdos) |
225 if (list == &fsm->mapping->pdos) |
226 return NULL; // no next PDO |
226 return NULL; // no next Pdo |
227 return list_entry(list, ec_pdo_t, list); |
227 return list_entry(list, ec_pdo_t, list); |
228 } |
228 } |
229 |
229 |
230 /*****************************************************************************/ |
230 /*****************************************************************************/ |
231 |
231 |
238 fsm->sdodata.subindex = fsm->pdo_count; |
238 fsm->sdodata.subindex = fsm->pdo_count; |
239 EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); |
239 EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); |
240 fsm->sdodata.size = 2; |
240 fsm->sdodata.size = 2; |
241 |
241 |
242 if (fsm->slave->master->debug_level) |
242 if (fsm->slave->master->debug_level) |
243 EC_DBG("Mapping PDO 0x%04X at position %u.\n", |
243 EC_DBG("Mapping Pdo 0x%04X at position %u.\n", |
244 fsm->pdo->index, fsm->sdodata.subindex); |
244 fsm->pdo->index, fsm->sdodata.subindex); |
245 |
245 |
246 fsm->state = ec_fsm_pdo_mapping_state_add_pdo; |
246 fsm->state = ec_fsm_pdo_mapping_state_add_pdo; |
247 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
247 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
248 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
248 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
249 } |
249 } |
250 |
250 |
251 /*****************************************************************************/ |
251 /*****************************************************************************/ |
252 |
252 |
253 /** Set the number of mapped PDOs to zero. |
253 /** Set the number of mapped Pdos to zero. |
254 */ |
254 */ |
255 void ec_fsm_pdo_mapping_state_zero_count( |
255 void ec_fsm_pdo_mapping_state_zero_count( |
256 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
256 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
257 ) |
257 ) |
258 { |
258 { |
259 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
259 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
260 |
260 |
261 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
261 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
262 EC_ERR("Failed to clear PDO mapping for slave %u.\n", |
262 EC_ERR("Failed to clear Pdo mapping for slave %u.\n", |
263 fsm->slave->ring_position); |
263 fsm->slave->ring_position); |
264 fsm->state = ec_fsm_pdo_mapping_state_error; |
264 fsm->state = ec_fsm_pdo_mapping_state_error; |
265 return; |
265 return; |
266 } |
266 } |
267 |
267 |
268 // map all PDOs belonging to the current sync manager |
268 // map all Pdos belonging to the current sync manager |
269 |
269 |
270 // find first PDO |
270 // find first Pdo |
271 if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->mapping->pdos))) { |
271 if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->mapping->pdos))) { |
272 if (fsm->slave->master->debug_level) |
272 if (fsm->slave->master->debug_level) |
273 EC_DBG("No PDOs to map for SM%u of slave %u.\n", |
273 EC_DBG("No Pdos to map for SM%u of slave %u.\n", |
274 fsm->sync->index, fsm->slave->ring_position); |
274 fsm->sync->index, fsm->slave->ring_position); |
275 ec_fsm_pdo_mapping_next_sync(fsm); |
275 ec_fsm_pdo_mapping_next_sync(fsm); |
276 return; |
276 return; |
277 } |
277 } |
278 |
278 |
279 // add first PDO to mapping |
279 // add first Pdo to mapping |
280 fsm->pdo_count = 1; |
280 fsm->pdo_count = 1; |
281 ec_fsm_pdo_mapping_add_pdo(fsm); |
281 ec_fsm_pdo_mapping_add_pdo(fsm); |
282 } |
282 } |
283 |
283 |
284 /*****************************************************************************/ |
284 /*****************************************************************************/ |
285 |
285 |
286 /** Add a PDO to the sync managers mapping. |
286 /** Add a Pdo to the sync managers mapping. |
287 */ |
287 */ |
288 void ec_fsm_pdo_mapping_state_add_pdo( |
288 void ec_fsm_pdo_mapping_state_add_pdo( |
289 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
289 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
290 ) |
290 ) |
291 { |
291 { |
292 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
292 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
293 |
293 |
294 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
294 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
295 EC_ERR("Failed to map PDO 0x%04X for SM%u of slave %u.\n", |
295 EC_ERR("Failed to map Pdo 0x%04X for SM%u of slave %u.\n", |
296 fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position); |
296 fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position); |
297 fsm->state = ec_fsm_pdo_mapping_state_error; |
297 fsm->state = ec_fsm_pdo_mapping_state_error; |
298 return; |
298 return; |
299 } |
299 } |
300 |
300 |
301 // find next PDO |
301 // find next Pdo |
302 if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) { |
302 if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) { |
303 // no more PDOs to map. write PDO count |
303 // no more Pdos to map. write Pdo count |
304 fsm->sdodata.subindex = 0; |
304 fsm->sdodata.subindex = 0; |
305 EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count); |
305 EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count); |
306 fsm->sdodata.size = 1; |
306 fsm->sdodata.size = 1; |
307 |
307 |
308 if (fsm->slave->master->debug_level) |
308 if (fsm->slave->master->debug_level) |
309 EC_DBG("Setting number of mapped PDOs to %u.\n", |
309 EC_DBG("Setting number of mapped Pdos to %u.\n", |
310 fsm->pdo_count); |
310 fsm->pdo_count); |
311 |
311 |
312 fsm->state = ec_fsm_pdo_mapping_state_pdo_count; |
312 fsm->state = ec_fsm_pdo_mapping_state_pdo_count; |
313 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
313 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
314 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
314 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
315 return; |
315 return; |
316 } |
316 } |
317 |
317 |
318 // add next PDO to mapping |
318 // add next Pdo to mapping |
319 fsm->pdo_count++; |
319 fsm->pdo_count++; |
320 ec_fsm_pdo_mapping_add_pdo(fsm); |
320 ec_fsm_pdo_mapping_add_pdo(fsm); |
321 } |
321 } |
322 |
322 |
323 /*****************************************************************************/ |
323 /*****************************************************************************/ |
324 |
324 |
325 /** Set the number of mapped PDOs. |
325 /** Set the number of mapped Pdos. |
326 */ |
326 */ |
327 void ec_fsm_pdo_mapping_state_pdo_count( |
327 void ec_fsm_pdo_mapping_state_pdo_count( |
328 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
328 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
329 ) |
329 ) |
330 { |
330 { |
331 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
331 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
332 |
332 |
333 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
333 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
334 EC_ERR("Failed to set number of mapped PDOs for slave %u.\n", |
334 EC_ERR("Failed to set number of mapped Pdos for slave %u.\n", |
335 fsm->slave->ring_position); |
335 fsm->slave->ring_position); |
336 fsm->state = ec_fsm_pdo_mapping_state_error; |
336 fsm->state = ec_fsm_pdo_mapping_state_error; |
337 return; |
337 return; |
338 } |
338 } |
339 |
339 |
340 if (fsm->slave->master->debug_level) |
340 if (fsm->slave->master->debug_level) |
341 EC_DBG("Successfully set PDO mapping for SM%u of slave %u.\n", |
341 EC_DBG("Successfully set Pdo mapping for SM%u of slave %u.\n", |
342 fsm->sync->index, fsm->slave->ring_position); |
342 fsm->sync->index, fsm->slave->ring_position); |
343 |
343 |
344 // mapping configuration for this sync manager complete. |
344 // mapping configuration for this sync manager complete. |
345 ec_fsm_pdo_mapping_next_sync(fsm); |
345 ec_fsm_pdo_mapping_next_sync(fsm); |
346 } |
346 } |