160 */ |
160 */ |
161 void ec_fsm_mapping_next_sync( |
161 void ec_fsm_mapping_next_sync( |
162 ec_fsm_mapping_t *fsm /**< mapping state machine */ |
162 ec_fsm_mapping_t *fsm /**< mapping state machine */ |
163 ) |
163 ) |
164 { |
164 { |
165 while (1) { |
165 ec_direction_t dir; |
166 if (fsm->dir > EC_DIR_INPUT) { |
166 const ec_sync_t *sync; |
167 // no more directions to configure mappings for |
167 const ec_pdo_mapping_t *map; |
168 fsm->state = ec_fsm_mapping_state_end; |
168 |
169 return; |
169 for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) { |
170 } |
170 if (!(sync = ec_slave_get_pdo_sync(fsm->slave, dir))) { |
171 |
171 EC_WARN("No sync manager for direction %u!\n", dir); |
172 if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) { |
|
173 EC_WARN("next_sync(): No sync manager!\n"); |
|
174 fsm->dir++; |
|
175 continue; |
172 continue; |
176 } |
173 } |
177 |
174 |
178 fsm->mapping = &fsm->slave->config->mapping[fsm->dir]; |
175 if (fsm->sync) { // there is a last SM |
179 |
176 if (sync == fsm->sync) // this is the last SM |
180 if (ec_pdo_mapping_equal(&fsm->sync->mapping, fsm->mapping)) { |
177 fsm->sync = NULL; // take the next one |
181 // the mapping for this direction does not have to be altered |
|
182 fsm->dir++; |
|
183 continue; |
|
184 } else { |
178 } else { |
185 fsm->dir++; |
179 map = &fsm->slave->config->mapping[dir]; |
|
180 if (ec_pdo_mapping_equal(&sync->mapping, map)) |
|
181 continue; |
|
182 |
|
183 fsm->sync = sync; |
|
184 fsm->mapping = map; |
186 break; |
185 break; |
187 } |
186 } |
|
187 } |
|
188 |
|
189 if (!sync) { |
|
190 if (fsm->slave->master->debug_level) |
|
191 EC_DBG("Pdo mapping finished for slave %u.\n", |
|
192 fsm->slave->ring_position); |
|
193 fsm->state = ec_fsm_mapping_state_end; |
|
194 return; |
188 } |
195 } |
189 |
196 |
190 if (fsm->slave->master->debug_level) { |
197 if (fsm->slave->master->debug_level) { |
191 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", |
192 fsm->sync->index, fsm->slave->ring_position); |
199 fsm->sync->index, fsm->slave->ring_position); |
216 { |
223 { |
217 list = list->next; |
224 list = list->next; |
218 if (list == &fsm->mapping->pdos) |
225 if (list == &fsm->mapping->pdos) |
219 return NULL; // no next PDO |
226 return NULL; // no next PDO |
220 return list_entry(list, ec_pdo_t, list); |
227 return list_entry(list, ec_pdo_t, list); |
|
228 } |
|
229 |
|
230 /*****************************************************************************/ |
|
231 |
|
232 /** Add a Pdo to the mapping. |
|
233 */ |
|
234 void ec_fsm_mapping_add_pdo( |
|
235 ec_fsm_mapping_t *fsm /**< mapping state machine */ |
|
236 ) |
|
237 { |
|
238 fsm->sdodata.subindex = fsm->pdo_count; |
|
239 EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); |
|
240 fsm->sdodata.size = 2; |
|
241 |
|
242 if (fsm->slave->master->debug_level) |
|
243 EC_DBG("Mapping PDO 0x%04X at position %u.\n", |
|
244 fsm->pdo->index, fsm->sdodata.subindex); |
|
245 |
|
246 fsm->state = ec_fsm_mapping_state_add_pdo; |
|
247 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
|
248 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
221 } |
249 } |
222 |
250 |
223 /*****************************************************************************/ |
251 /*****************************************************************************/ |
224 |
252 |
225 /** Set the number of mapped PDOs to zero. |
253 /** Set the number of mapped PDOs to zero. |
248 return; |
276 return; |
249 } |
277 } |
250 |
278 |
251 // add first PDO to mapping |
279 // add first PDO to mapping |
252 fsm->pdo_count = 1; |
280 fsm->pdo_count = 1; |
253 fsm->sdodata.subindex = fsm->pdo_count; |
281 ec_fsm_mapping_add_pdo(fsm); |
254 EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); |
|
255 fsm->sdodata.size = 2; |
|
256 |
|
257 if (fsm->slave->master->debug_level) |
|
258 EC_DBG("Mapping PDO 0x%04X at position %u.\n", |
|
259 fsm->pdo->index, fsm->sdodata.subindex); |
|
260 |
|
261 fsm->state = ec_fsm_mapping_state_add_pdo; |
|
262 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
|
263 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
|
264 } |
282 } |
265 |
283 |
266 /*****************************************************************************/ |
284 /*****************************************************************************/ |
267 |
285 |
268 /** Add a PDO to the sync managers mapping. |
286 /** Add a PDO to the sync managers mapping. |
297 return; |
315 return; |
298 } |
316 } |
299 |
317 |
300 // add next PDO to mapping |
318 // add next PDO to mapping |
301 fsm->pdo_count++; |
319 fsm->pdo_count++; |
302 fsm->sdodata.subindex = fsm->pdo_count; |
320 ec_fsm_mapping_add_pdo(fsm); |
303 EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); |
|
304 fsm->sdodata.size = 2; |
|
305 |
|
306 if (fsm->slave->master->debug_level) |
|
307 EC_DBG("Mapping PDO 0x%04X at position %u.\n", |
|
308 fsm->pdo->index, fsm->sdodata.subindex); |
|
309 |
|
310 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
|
311 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
|
312 } |
321 } |
313 |
322 |
314 /*****************************************************************************/ |
323 /*****************************************************************************/ |
315 |
324 |
316 /** Set the number of mapped PDOs. |
325 /** Set the number of mapped PDOs. |