51 void ec_fsm_pdo_assign_state_add_pdo(ec_fsm_pdo_assign_t *); |
51 void ec_fsm_pdo_assign_state_add_pdo(ec_fsm_pdo_assign_t *); |
52 void ec_fsm_pdo_assign_state_pdo_count(ec_fsm_pdo_assign_t *); |
52 void ec_fsm_pdo_assign_state_pdo_count(ec_fsm_pdo_assign_t *); |
53 void ec_fsm_pdo_assign_state_end(ec_fsm_pdo_assign_t *); |
53 void ec_fsm_pdo_assign_state_end(ec_fsm_pdo_assign_t *); |
54 void ec_fsm_pdo_assign_state_error(ec_fsm_pdo_assign_t *); |
54 void ec_fsm_pdo_assign_state_error(ec_fsm_pdo_assign_t *); |
55 |
55 |
56 void ec_fsm_pdo_assign_next_dir(ec_fsm_pdo_assign_t *); |
56 void ec_fsm_pdo_assign_next_sync(ec_fsm_pdo_assign_t *); |
57 |
57 |
58 /*****************************************************************************/ |
58 /*****************************************************************************/ |
59 |
59 |
60 /** Constructor. |
60 /** Constructor. |
61 */ |
61 */ |
149 if (!fsm->slave->config) { |
149 if (!fsm->slave->config) { |
150 fsm->state = ec_fsm_pdo_assign_state_end; |
150 fsm->state = ec_fsm_pdo_assign_state_end; |
151 return; |
151 return; |
152 } |
152 } |
153 |
153 |
154 fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT |
154 fsm->sync_index = 0xff; // next is zero |
155 fsm->num_configured_dirs = 0; |
155 fsm->num_configured_syncs = 0; |
156 ec_fsm_pdo_assign_next_dir(fsm); |
156 ec_fsm_pdo_assign_next_sync(fsm); |
157 } |
157 } |
158 |
158 |
159 /*****************************************************************************/ |
159 /*****************************************************************************/ |
160 |
160 |
161 /** Process Pdo assignment of next direction. |
161 /** Process Pdo assignment of next sync manager. |
162 */ |
162 */ |
163 void ec_fsm_pdo_assign_next_dir( |
163 void ec_fsm_pdo_assign_next_sync( |
164 ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */ |
164 ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */ |
165 ) |
165 ) |
166 { |
166 { |
167 fsm->dir++; |
167 fsm->sync_index++; |
168 |
168 |
169 for (; fsm->dir <= EC_DIR_INPUT; fsm->dir++) { |
169 for (; fsm->sync_index < EC_MAX_SYNCS; fsm->sync_index++) { |
170 fsm->pdos = &fsm->slave->config->pdos[fsm->dir]; |
170 fsm->pdos = &fsm->slave->config->sync_configs[fsm->sync_index].pdos; |
171 |
171 |
172 if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) { |
172 if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) { |
173 if (!list_empty(&fsm->pdos->list)) { |
173 if (!list_empty(&fsm->pdos->list)) { |
174 EC_ERR("No sync manager for direction %u!\n", fsm->dir); |
174 EC_ERR("Slave %u does not provide a configuration for sync " |
|
175 "manager %u!\n", fsm->slave->ring_position, |
|
176 fsm->sync_index); |
175 fsm->state = ec_fsm_pdo_assign_state_end; |
177 fsm->state = ec_fsm_pdo_assign_state_end; |
176 return; |
178 return; |
177 } |
179 } |
178 continue; |
180 continue; |
179 } |
181 } |
180 |
182 |
181 if (fsm->slave->master->debug_level) { |
|
182 EC_DBG("Sync Pdos: "); |
|
183 ec_pdo_list_print(&fsm->sync->pdos); |
|
184 printk("\n"); |
|
185 EC_DBG("Config Pdos: "); |
|
186 ec_pdo_list_print(fsm->pdos); |
|
187 printk("\n"); |
|
188 } |
|
189 |
|
190 // check if assignment has to be altered |
183 // check if assignment has to be altered |
191 if (ec_pdo_list_equal(&fsm->sync->pdos, fsm->pdos)) |
184 if (ec_pdo_list_equal(&fsm->sync->pdos, fsm->pdos)) |
192 continue; |
185 continue; |
|
186 |
|
187 if (fsm->slave->master->debug_level) { |
|
188 EC_DBG("Pdo assignment of SM%u differs in slave %u:\n", |
|
189 fsm->sync_index, fsm->slave->ring_position); |
|
190 EC_DBG("Currently assigned Pdos: "); |
|
191 ec_pdo_list_print(&fsm->sync->pdos); |
|
192 printk("\n"); |
|
193 EC_DBG("Pdos to assign: "); |
|
194 ec_pdo_list_print(fsm->pdos); |
|
195 printk("\n"); |
|
196 } |
193 |
197 |
194 // Pdo assignment has to be changed. Does the slave support this? |
198 // Pdo assignment has to be changed. Does the slave support this? |
195 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
199 if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
196 || (fsm->slave->sii.has_general |
200 || (fsm->slave->sii.has_general |
197 && !fsm->slave->sii.coe_details.enable_pdo_assign)) { |
201 && !fsm->slave->sii.coe_details.enable_pdo_assign)) { |
199 fsm->slave->ring_position); |
203 fsm->slave->ring_position); |
200 fsm->state = ec_fsm_pdo_assign_state_error; |
204 fsm->state = ec_fsm_pdo_assign_state_error; |
201 return; |
205 return; |
202 } |
206 } |
203 |
207 |
204 fsm->num_configured_dirs++; |
208 fsm->num_configured_syncs++; |
205 |
209 |
206 if (fsm->slave->master->debug_level) { |
210 if (fsm->slave->master->debug_level) { |
207 EC_DBG("Changing Pdo assignment for SM%u of slave %u.\n", |
211 EC_DBG("Changing Pdo assignment for SM%u of slave %u.\n", |
208 fsm->sync->index, fsm->slave->ring_position); |
212 fsm->sync_index, fsm->slave->ring_position); |
209 } |
213 } |
210 |
214 |
211 if (ec_sdo_request_alloc(&fsm->request, 2)) { |
215 if (ec_sdo_request_alloc(&fsm->request, 2)) { |
212 fsm->state = ec_fsm_pdo_assign_state_error; |
216 fsm->state = ec_fsm_pdo_assign_state_error; |
213 return; |
217 return; |
214 } |
218 } |
215 |
219 |
216 // set mapped Pdo count to zero |
220 // set mapped Pdo count to zero |
217 EC_WRITE_U8(fsm->request.data, 0); // zero Pdos mapped |
221 EC_WRITE_U8(fsm->request.data, 0); // zero Pdos mapped |
218 fsm->request.data_size = 1; |
222 fsm->request.data_size = 1; |
219 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0); |
223 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); |
220 ecrt_sdo_request_write(&fsm->request); |
224 ecrt_sdo_request_write(&fsm->request); |
221 if (fsm->slave->master->debug_level) |
225 if (fsm->slave->master->debug_level) |
222 EC_DBG("Setting Pdo count to zero for SM%u.\n", fsm->sync->index); |
226 EC_DBG("Setting Pdo count to zero for SM%u.\n", fsm->sync_index); |
223 |
227 |
224 fsm->state = ec_fsm_pdo_assign_state_zero_count; |
228 fsm->state = ec_fsm_pdo_assign_state_zero_count; |
225 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
229 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
226 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
230 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
227 return; |
231 return; |
228 } |
232 } |
229 |
233 |
230 if (fsm->slave->master->debug_level && !fsm->num_configured_dirs) |
234 if (fsm->slave->master->debug_level && !fsm->num_configured_syncs) |
231 EC_DBG("Pdo assignments of slave %u are already configured" |
235 EC_DBG("Pdo assignments of slave %u are already configured" |
232 " correctly.\n", fsm->slave->ring_position); |
236 " correctly.\n", fsm->slave->ring_position); |
233 fsm->state = ec_fsm_pdo_assign_state_end; |
237 fsm->state = ec_fsm_pdo_assign_state_end; |
234 } |
238 } |
235 |
239 |
257 ) |
261 ) |
258 { |
262 { |
259 EC_WRITE_U16(fsm->request.data, fsm->pdo->index); |
263 EC_WRITE_U16(fsm->request.data, fsm->pdo->index); |
260 fsm->request.data_size = 2; |
264 fsm->request.data_size = 2; |
261 ec_sdo_request_address(&fsm->request, |
265 ec_sdo_request_address(&fsm->request, |
262 0x1C10 + fsm->sync->index, fsm->pdo_count); |
266 0x1C10 + fsm->sync_index, fsm->pdo_count); |
263 ecrt_sdo_request_write(&fsm->request); |
267 ecrt_sdo_request_write(&fsm->request); |
264 if (fsm->slave->master->debug_level) |
268 if (fsm->slave->master->debug_level) |
265 EC_DBG("Assigning Pdo 0x%04X at position %u.\n", |
269 EC_DBG("Assigning Pdo 0x%04X at position %u.\n", |
266 fsm->pdo->index, fsm->pdo_count); |
270 fsm->pdo->index, fsm->pdo_count); |
267 |
271 |
291 |
295 |
292 // find first Pdo |
296 // find first Pdo |
293 if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdos->list))) { |
297 if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdos->list))) { |
294 if (fsm->slave->master->debug_level) |
298 if (fsm->slave->master->debug_level) |
295 EC_DBG("No Pdos to assign for SM%u of slave %u.\n", |
299 EC_DBG("No Pdos to assign for SM%u of slave %u.\n", |
296 fsm->sync->index, fsm->slave->ring_position); |
300 fsm->sync_index, fsm->slave->ring_position); |
297 ec_fsm_pdo_assign_next_dir(fsm); |
301 ec_fsm_pdo_assign_next_sync(fsm); |
298 return; |
302 return; |
299 } |
303 } |
300 |
304 |
301 // assign first Pdo |
305 // assign first Pdo |
302 fsm->pdo_count = 1; |
306 fsm->pdo_count = 1; |
313 { |
317 { |
314 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
318 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
315 |
319 |
316 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
320 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
317 EC_ERR("Failed to map Pdo 0x%04X for SM%u of slave %u.\n", |
321 EC_ERR("Failed to map Pdo 0x%04X for SM%u of slave %u.\n", |
318 fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position); |
322 fsm->pdo->index, fsm->sync_index, fsm->slave->ring_position); |
319 fsm->state = ec_fsm_pdo_assign_state_error; |
323 fsm->state = ec_fsm_pdo_assign_state_error; |
320 return; |
324 return; |
321 } |
325 } |
322 |
326 |
323 // find next Pdo |
327 // find next Pdo |
324 if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdo->list))) { |
328 if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdo->list))) { |
325 // no more Pdos to map. write Pdo count |
329 // no more Pdos to map. write Pdo count |
326 EC_WRITE_U8(fsm->request.data, fsm->pdo_count); |
330 EC_WRITE_U8(fsm->request.data, fsm->pdo_count); |
327 fsm->request.data_size = 1; |
331 fsm->request.data_size = 1; |
328 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0); |
332 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0); |
329 ecrt_sdo_request_write(&fsm->request); |
333 ecrt_sdo_request_write(&fsm->request); |
330 if (fsm->slave->master->debug_level) |
334 if (fsm->slave->master->debug_level) |
331 EC_DBG("Setting number of assigned Pdos to %u.\n", |
335 EC_DBG("Setting number of assigned Pdos to %u.\n", |
332 fsm->pdo_count); |
336 fsm->pdo_count); |
333 |
337 |
359 return; |
363 return; |
360 } |
364 } |
361 |
365 |
362 if (fsm->slave->master->debug_level) |
366 if (fsm->slave->master->debug_level) |
363 EC_DBG("Successfully configured Pdo assignment for SM%u of" |
367 EC_DBG("Successfully configured Pdo assignment for SM%u of" |
364 " slave %u.\n", fsm->sync->index, fsm->slave->ring_position); |
368 " slave %u.\n", fsm->sync_index, fsm->slave->ring_position); |
365 |
369 |
366 // assignment for this direction finished |
370 // assignment for this sync manager finished |
367 ec_fsm_pdo_assign_next_dir(fsm); |
371 ec_fsm_pdo_assign_next_sync(fsm); |
368 } |
372 } |
369 |
373 |
370 /****************************************************************************** |
374 /****************************************************************************** |
371 * Common state functions |
375 * Common state functions |
372 *****************************************************************************/ |
376 *****************************************************************************/ |