133 Destructor. |
125 Destructor. |
134 */ |
126 */ |
135 |
127 |
136 void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */) |
128 void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */) |
137 { |
129 { |
|
130 // clear sub-state machines |
|
131 ec_fsm_sii_clear(&fsm->fsm_sii); |
|
132 ec_fsm_change_clear(&fsm->fsm_change); |
|
133 ec_fsm_coe_clear(&fsm->fsm_coe); |
|
134 |
138 ec_datagram_clear(&fsm->datagram); |
135 ec_datagram_clear(&fsm->datagram); |
139 } |
136 } |
140 |
137 |
141 /*****************************************************************************/ |
138 /*****************************************************************************/ |
142 |
139 |
143 /** |
140 /** |
144 Resets the state machine. |
|
145 */ |
|
146 |
|
147 void ec_fsm_reset(ec_fsm_t *fsm /**< finite state machine */) |
|
148 { |
|
149 fsm->master_state = ec_fsm_master_start; |
|
150 fsm->master_slaves_responding = 0; |
|
151 fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN; |
|
152 } |
|
153 |
|
154 /*****************************************************************************/ |
|
155 |
|
156 /** |
|
157 Executes the current state of the state machine. |
141 Executes the current state of the state machine. |
158 */ |
142 \return false, if state machine has terminated |
159 |
143 */ |
160 void ec_fsm_execute(ec_fsm_t *fsm /**< finite state machine */) |
144 |
|
145 int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */) |
161 { |
146 { |
162 fsm->master_state(fsm); |
147 fsm->master_state(fsm); |
163 } |
148 |
164 |
149 return ec_fsm_running(fsm); |
165 /*****************************************************************************/ |
150 } |
166 |
151 |
167 /** |
152 /*****************************************************************************/ |
168 Initializes the master startup state machine. |
153 |
169 */ |
154 /** |
170 |
155 \return false, if state machine has terminated |
171 void ec_fsm_startup(ec_fsm_t *fsm) |
156 */ |
172 { |
157 |
173 fsm->master_state = ec_fsm_startup_start; |
158 int ec_fsm_running(ec_fsm_t *fsm /**< finite state machine */) |
174 } |
159 { |
175 |
160 return fsm->master_state != ec_fsm_master_end |
176 /*****************************************************************************/ |
161 && fsm->master_state != ec_fsm_master_error; |
177 |
162 } |
178 /** |
163 |
179 Returns the running state of the master startup state machine. |
164 /*****************************************************************************/ |
180 \return non-zero if not terminated yet. |
165 |
181 */ |
166 /** |
182 |
167 \return true, if the master state machine terminated gracefully |
183 int ec_fsm_startup_running(ec_fsm_t *fsm /**< Finite state machine */) |
168 */ |
184 { |
169 |
185 return fsm->master_state != ec_fsm_end && |
170 int ec_fsm_success(ec_fsm_t *fsm /**< finite state machine */) |
186 fsm->master_state != ec_fsm_error; |
171 { |
187 } |
172 return fsm->master_state == ec_fsm_master_end; |
188 |
|
189 /*****************************************************************************/ |
|
190 |
|
191 /** |
|
192 Returns, if the master startup state machine terminated with success. |
|
193 \return non-zero if successful. |
|
194 */ |
|
195 |
|
196 int ec_fsm_startup_success(ec_fsm_t *fsm /**< Finite state machine */) |
|
197 { |
|
198 return fsm->master_state == ec_fsm_end; |
|
199 } |
|
200 |
|
201 /*****************************************************************************/ |
|
202 |
|
203 /** |
|
204 Initializes the master configuration state machine. |
|
205 */ |
|
206 |
|
207 void ec_fsm_configuration(ec_fsm_t *fsm) |
|
208 { |
|
209 fsm->master_state = ec_fsm_configuration_start; |
|
210 } |
|
211 |
|
212 /*****************************************************************************/ |
|
213 |
|
214 /** |
|
215 Returns the running state of the master configuration state machine. |
|
216 \return non-zero if not terminated yet. |
|
217 */ |
|
218 |
|
219 int ec_fsm_configuration_running(ec_fsm_t *fsm /**< Finite state machine */) |
|
220 { |
|
221 return fsm->master_state != ec_fsm_end && |
|
222 fsm->master_state != ec_fsm_error; |
|
223 } |
|
224 |
|
225 /*****************************************************************************/ |
|
226 |
|
227 /** |
|
228 Returns, if the master confuguration state machine terminated with success. |
|
229 \return non-zero if successful. |
|
230 */ |
|
231 |
|
232 int ec_fsm_configuration_success(ec_fsm_t *fsm /**< Finite state machine */) |
|
233 { |
|
234 return fsm->master_state == ec_fsm_end; |
|
235 } |
173 } |
236 |
174 |
237 /****************************************************************************** |
175 /****************************************************************************** |
238 * master startup state machine |
176 * operation/idle state machine |
239 *****************************************************************************/ |
|
240 |
|
241 /** |
|
242 Master state: START. |
|
243 Starts with getting slave count and slave states. |
|
244 */ |
|
245 |
|
246 void ec_fsm_startup_start(ec_fsm_t *fsm) |
|
247 { |
|
248 ec_datagram_brd(&fsm->datagram, 0x0130, 2); |
|
249 ec_master_queue_datagram(fsm->master, &fsm->datagram); |
|
250 fsm->master_state = ec_fsm_startup_broadcast; |
|
251 } |
|
252 |
|
253 /*****************************************************************************/ |
|
254 |
|
255 /** |
|
256 Master state: BROADCAST. |
|
257 Processes the broadcast read slave count and slaves states. |
|
258 */ |
|
259 |
|
260 void ec_fsm_startup_broadcast(ec_fsm_t *fsm /**< finite state machine */) |
|
261 { |
|
262 ec_datagram_t *datagram = &fsm->datagram; |
|
263 unsigned int i; |
|
264 ec_slave_t *slave; |
|
265 ec_master_t *master = fsm->master; |
|
266 |
|
267 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
268 EC_ERR("Failed tor receive broadcast datagram.\n"); |
|
269 fsm->master_state = ec_fsm_error; |
|
270 return; |
|
271 } |
|
272 |
|
273 EC_INFO("Scanning bus.\n"); |
|
274 |
|
275 ec_master_clear_slaves(master); |
|
276 |
|
277 master->slave_count = datagram->working_counter; |
|
278 |
|
279 if (!master->slave_count) { |
|
280 // no slaves present -> finish state machine. |
|
281 fsm->master_state = ec_fsm_end; |
|
282 return; |
|
283 } |
|
284 |
|
285 // init slaves |
|
286 for (i = 0; i < master->slave_count; i++) { |
|
287 if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t), |
|
288 GFP_KERNEL))) { |
|
289 EC_ERR("Failed to allocate slave %i!\n", i); |
|
290 fsm->master_state = ec_fsm_error; |
|
291 return; |
|
292 } |
|
293 |
|
294 if (ec_slave_init(slave, master, i, i + 1)) { |
|
295 fsm->master_state = ec_fsm_error; |
|
296 return; |
|
297 } |
|
298 |
|
299 if (kobject_add(&slave->kobj)) { |
|
300 EC_ERR("Failed to add kobject.\n"); |
|
301 kobject_put(&slave->kobj); // free |
|
302 fsm->master_state = ec_fsm_error; |
|
303 return; |
|
304 } |
|
305 |
|
306 list_add_tail(&slave->list, &master->slaves); |
|
307 } |
|
308 |
|
309 // begin scanning of slaves |
|
310 fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); |
|
311 fsm->slave_state = ec_fsm_slavescan_start; |
|
312 fsm->master_state = ec_fsm_startup_scan; |
|
313 fsm->master_state(fsm); // execute immediately |
|
314 return; |
|
315 } |
|
316 |
|
317 /*****************************************************************************/ |
|
318 |
|
319 /** |
|
320 Master state: SCAN. |
|
321 Executes the sub-statemachine for the scanning of a slave. |
|
322 */ |
|
323 |
|
324 void ec_fsm_startup_scan(ec_fsm_t *fsm /**< finite state machine */) |
|
325 { |
|
326 ec_master_t *master = fsm->master; |
|
327 ec_slave_t *slave = fsm->slave; |
|
328 |
|
329 fsm->slave_state(fsm); // execute slave state machine |
|
330 |
|
331 if (fsm->slave_state == ec_fsm_error) { |
|
332 EC_ERR("Slave scanning failed.\n"); |
|
333 fsm->master_state = ec_fsm_error; |
|
334 return; |
|
335 } |
|
336 |
|
337 if (fsm->slave_state != ec_fsm_end) return; |
|
338 |
|
339 // another slave to scan? |
|
340 if (slave->list.next != &master->slaves) { |
|
341 fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); |
|
342 fsm->slave_state = ec_fsm_slavescan_start; |
|
343 fsm->slave_state(fsm); // execute immediately |
|
344 return; |
|
345 } |
|
346 |
|
347 EC_INFO("Bus scanning completed.\n"); |
|
348 |
|
349 ec_master_calc_addressing(master); |
|
350 |
|
351 fsm->master_state = ec_fsm_end; |
|
352 } |
|
353 |
|
354 /****************************************************************************** |
|
355 * master configuration state machine |
|
356 *****************************************************************************/ |
|
357 |
|
358 /** |
|
359 Master configuration state machine: START. |
|
360 */ |
|
361 |
|
362 void ec_fsm_configuration_start(ec_fsm_t *fsm /**< finite state machine */) |
|
363 { |
|
364 ec_master_t *master = fsm->master; |
|
365 |
|
366 if (list_empty(&master->slaves)) { |
|
367 fsm->master_state = ec_fsm_end; |
|
368 return; |
|
369 } |
|
370 |
|
371 // begin configuring slaves |
|
372 fsm->slave = list_entry(master->slaves.next, ec_slave_t, list); |
|
373 fsm->slave_state = ec_fsm_slaveconf_init; |
|
374 fsm->change_new = EC_SLAVE_STATE_INIT; |
|
375 fsm->change_state = ec_fsm_change_start; |
|
376 fsm->master_state = ec_fsm_configuration_conf; |
|
377 fsm->master_state(fsm); // execute immediately |
|
378 } |
|
379 |
|
380 /*****************************************************************************/ |
|
381 |
|
382 /** |
|
383 Master state: CONF. |
|
384 */ |
|
385 |
|
386 void ec_fsm_configuration_conf(ec_fsm_t *fsm /**< finite state machine */) |
|
387 { |
|
388 ec_master_t *master = fsm->master; |
|
389 ec_slave_t *slave = fsm->slave; |
|
390 |
|
391 fsm->slave_state(fsm); // execute slave's state machine |
|
392 |
|
393 if (fsm->slave_state == ec_fsm_error) { |
|
394 fsm->master_state = ec_fsm_error; |
|
395 return; |
|
396 } |
|
397 |
|
398 if (fsm->slave_state != ec_fsm_end) return; |
|
399 |
|
400 // another slave to configure? |
|
401 if (slave->list.next != &master->slaves) { |
|
402 fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list); |
|
403 fsm->slave_state = ec_fsm_slaveconf_init; |
|
404 fsm->change_new = EC_SLAVE_STATE_INIT; |
|
405 fsm->change_state = ec_fsm_change_start; |
|
406 fsm->master_state(fsm); // execute immediately |
|
407 return; |
|
408 } |
|
409 |
|
410 fsm->master_state = ec_fsm_end; |
|
411 } |
|
412 |
|
413 /****************************************************************************** |
|
414 * operation / idle state machine |
|
415 *****************************************************************************/ |
177 *****************************************************************************/ |
416 |
178 |
417 /** |
179 /** |
418 Master state: START. |
180 Master state: START. |
419 Starts with getting slave count and slave states. |
181 Starts with getting slave count and slave states. |
1279 if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2), |
1179 if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2), |
1280 cat_size, EC_RX_PDO)) |
1180 cat_size, EC_RX_PDO)) |
1281 goto end; |
1181 goto end; |
1282 break; |
1182 break; |
1283 default: |
1183 default: |
1284 EC_WARN("Unknown category type 0x%04X in slave %i.\n", |
1184 if (fsm->master->debug_level) |
1285 cat_type, slave->ring_position); |
1185 EC_WARN("Unknown category type 0x%04X in slave %i.\n", |
|
1186 cat_type, slave->ring_position); |
1286 } |
1187 } |
1287 |
1188 |
1288 cat_word += cat_size + 2; |
1189 cat_word += cat_size + 2; |
1289 } |
1190 } |
1290 |
1191 |
1291 fsm->slave_state = ec_fsm_end; |
1192 fsm->slave_state = ec_fsm_slave_state_end; |
1292 return; |
1193 return; |
1293 |
1194 |
1294 end: |
1195 end: |
1295 EC_ERR("Failed to analyze category data.\n"); |
1196 EC_ERR("Failed to analyze category data.\n"); |
1296 fsm->slave->error_flag = 1; |
1197 fsm->slave->error_flag = 1; |
1297 fsm->slave_state = ec_fsm_error; |
1198 fsm->slave_state = ec_fsm_slave_state_error; |
1298 } |
1199 } |
1299 |
1200 |
1300 /****************************************************************************** |
1201 /****************************************************************************** |
1301 * slave configuration state machine |
1202 * slave configuration state machine |
1302 *****************************************************************************/ |
1203 *****************************************************************************/ |
1303 |
1204 |
1304 /** |
1205 /** |
1305 Slave state: INIT. |
1206 Slave configuration state: START. |
1306 */ |
1207 */ |
1307 |
1208 |
1308 void ec_fsm_slaveconf_init(ec_fsm_t *fsm /**< finite state machine */) |
1209 void ec_fsm_slaveconf_state_start(ec_fsm_t *fsm /**< finite state machine */) |
1309 { |
1210 { |
|
1211 if (fsm->master->debug_level) { |
|
1212 EC_DBG("Configuring slave %i...\n", fsm->slave->ring_position); |
|
1213 } |
|
1214 |
|
1215 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); |
|
1216 ec_fsm_change_exec(&fsm->fsm_change); |
|
1217 fsm->slave_state = ec_fsm_slaveconf_state_init; |
|
1218 } |
|
1219 |
|
1220 /*****************************************************************************/ |
|
1221 |
|
1222 /** |
|
1223 Slave configuration state: INIT. |
|
1224 */ |
|
1225 |
|
1226 void ec_fsm_slaveconf_state_init(ec_fsm_t *fsm /**< finite state machine */) |
|
1227 { |
|
1228 ec_master_t *master = fsm->master; |
1310 ec_slave_t *slave = fsm->slave; |
1229 ec_slave_t *slave = fsm->slave; |
1311 ec_datagram_t *datagram = &fsm->datagram; |
1230 ec_datagram_t *datagram = &fsm->datagram; |
1312 const ec_sii_sync_t *sync; |
1231 |
1313 |
1232 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
1314 fsm->change_state(fsm); // execute state change state machine |
1233 |
1315 |
1234 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
1316 if (fsm->change_state == ec_fsm_error) { |
|
1317 slave->error_flag = 1; |
1235 slave->error_flag = 1; |
1318 fsm->slave_state = ec_fsm_error; |
1236 fsm->slave_state = ec_fsm_slave_state_error; |
1319 return; |
1237 return; |
1320 } |
1238 } |
1321 |
1239 |
1322 if (fsm->change_state != ec_fsm_end) return; |
1240 slave->configured = 1; |
1323 |
1241 |
1324 // slave is now in INIT |
1242 if (master->debug_level) { |
1325 if (slave->current_state == slave->requested_state) { |
1243 EC_DBG("Slave %i is now in INIT.\n", slave->ring_position); |
1326 fsm->slave_state = ec_fsm_end; // successful |
|
1327 return; |
|
1328 } |
1244 } |
1329 |
1245 |
1330 // check and reset CRC fault counters |
1246 // check and reset CRC fault counters |
1331 //ec_slave_check_crc(slave); |
1247 //ec_slave_check_crc(slave); |
1332 // TODO: Implement state machine for CRC checking. |
1248 // TODO: Implement state machine for CRC checking. |
1333 |
1249 |
|
1250 if (!slave->base_fmmu_count) { // skip FMMU configuration |
|
1251 ec_fsm_slaveconf_enter_sync(fsm); |
|
1252 return; |
|
1253 } |
|
1254 |
|
1255 if (master->debug_level) |
|
1256 EC_DBG("Clearing FMMU configurations of slave %i...\n", |
|
1257 slave->ring_position); |
|
1258 |
|
1259 // clear FMMU configurations |
|
1260 ec_datagram_npwr(datagram, slave->station_address, |
|
1261 0x0600, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1262 memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count); |
|
1263 ec_master_queue_datagram(master, datagram); |
|
1264 fsm->slave_state = ec_fsm_slaveconf_state_clear_fmmus; |
|
1265 } |
|
1266 |
|
1267 /*****************************************************************************/ |
|
1268 |
|
1269 /** |
|
1270 Slave configuration state: CLEAR FMMU. |
|
1271 */ |
|
1272 |
|
1273 void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *fsm |
|
1274 /**< finite state machine */) |
|
1275 { |
|
1276 ec_datagram_t *datagram = &fsm->datagram; |
|
1277 |
|
1278 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1279 || datagram->working_counter != 1) { |
|
1280 fsm->slave->error_flag = 1; |
|
1281 fsm->slave_state = ec_fsm_slave_state_error; |
|
1282 EC_ERR("Failed to clear FMMUs on slave %i.\n", |
|
1283 fsm->slave->ring_position); |
|
1284 return; |
|
1285 } |
|
1286 |
|
1287 ec_fsm_slaveconf_enter_sync(fsm); |
|
1288 } |
|
1289 |
|
1290 /*****************************************************************************/ |
|
1291 |
|
1292 /** |
|
1293 */ |
|
1294 |
|
1295 void ec_fsm_slaveconf_enter_sync(ec_fsm_t *fsm /**< finite state machine */) |
|
1296 { |
|
1297 ec_master_t *master = fsm->master; |
|
1298 ec_slave_t *slave = fsm->slave; |
|
1299 ec_datagram_t *datagram = &fsm->datagram; |
|
1300 const ec_sii_sync_t *sync; |
|
1301 ec_sii_sync_t mbox_sync; |
|
1302 |
|
1303 // slave is now in INIT |
|
1304 if (slave->current_state == slave->requested_state) { |
|
1305 fsm->slave_state = ec_fsm_slave_state_end; // successful |
|
1306 if (master->debug_level) { |
|
1307 EC_DBG("Finished configuration of slave %i.\n", |
|
1308 slave->ring_position); |
|
1309 } |
|
1310 return; |
|
1311 } |
|
1312 |
1334 if (!slave->base_sync_count) { // no sync managers |
1313 if (!slave->base_sync_count) { // no sync managers |
1335 fsm->slave_state = ec_fsm_slaveconf_preop; |
1314 ec_fsm_slaveconf_enter_preop(fsm); |
1336 fsm->change_new = EC_SLAVE_STATE_PREOP; |
1315 return; |
1337 fsm->change_state = ec_fsm_change_start; |
1316 } |
1338 fsm->change_state(fsm); // execute immediately |
1317 |
1339 return; |
1318 if (master->debug_level) { |
|
1319 EC_DBG("Configuring sync managers of slave %i.\n", |
|
1320 slave->ring_position); |
1340 } |
1321 } |
1341 |
1322 |
1342 // configure sync managers |
1323 // configure sync managers |
1343 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
1324 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
1344 EC_SYNC_SIZE * slave->base_sync_count); |
1325 EC_SYNC_SIZE * slave->base_sync_count); |
1345 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
1326 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
1346 |
1327 |
1347 list_for_each_entry(sync, &slave->sii_syncs, list) { |
1328 if (list_empty(&slave->sii_syncs)) { |
1348 if (sync->index >= slave->base_sync_count) { |
1329 if (slave->sii_rx_mailbox_offset && slave->sii_tx_mailbox_offset) { |
1349 EC_ERR("Invalid sync manager configuration found!"); |
1330 if (slave->master->debug_level) |
1350 fsm->slave->error_flag = 1; |
1331 EC_DBG("Guessing sync manager settings for slave %i.\n", |
1351 fsm->slave_state = ec_fsm_error; |
1332 slave->ring_position); |
1352 return; |
1333 mbox_sync.index = 0; |
1353 } |
1334 mbox_sync.physical_start_address = slave->sii_tx_mailbox_offset; |
1354 ec_sync_config(sync, slave, |
1335 mbox_sync.length = slave->sii_tx_mailbox_size; |
1355 datagram->data + EC_SYNC_SIZE * sync->index); |
1336 mbox_sync.control_register = 0x26; |
|
1337 mbox_sync.enable = 0x01; |
|
1338 mbox_sync.est_length = 0; |
|
1339 ec_sync_config(&mbox_sync, slave, |
|
1340 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
|
1341 mbox_sync.index = 1; |
|
1342 mbox_sync.physical_start_address = slave->sii_rx_mailbox_offset; |
|
1343 mbox_sync.length = slave->sii_rx_mailbox_size; |
|
1344 mbox_sync.control_register = 0x22; |
|
1345 mbox_sync.enable = 0x01; |
|
1346 mbox_sync.est_length = 0; |
|
1347 ec_sync_config(&mbox_sync, slave, |
|
1348 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
|
1349 } |
|
1350 } |
|
1351 else if (slave->sii_mailbox_protocols) { // mailboxes present |
|
1352 list_for_each_entry(sync, &slave->sii_syncs, list) { |
|
1353 // only configure mailbox sync-managers |
|
1354 if (sync->index != 0 && sync->index != 1) continue; |
|
1355 ec_sync_config(sync, slave, |
|
1356 datagram->data + EC_SYNC_SIZE * sync->index); |
|
1357 } |
1356 } |
1358 } |
1357 |
1359 |
1358 ec_master_queue_datagram(fsm->master, datagram); |
1360 ec_master_queue_datagram(fsm->master, datagram); |
1359 fsm->slave_state = ec_fsm_slaveconf_sync; |
1361 fsm->slave_state = ec_fsm_slaveconf_state_sync; |
1360 } |
1362 } |
1361 |
1363 |
1362 /*****************************************************************************/ |
1364 /*****************************************************************************/ |
1363 |
1365 |
1364 /** |
1366 /** |
1365 Slave state: SYNC. |
1367 Slave configuration state: SYNC. |
1366 */ |
1368 */ |
1367 |
1369 |
1368 void ec_fsm_slaveconf_sync(ec_fsm_t *fsm /**< finite state machine */) |
1370 void ec_fsm_slaveconf_state_sync(ec_fsm_t *fsm /**< finite state machine */) |
1369 { |
1371 { |
1370 ec_datagram_t *datagram = &fsm->datagram; |
1372 ec_datagram_t *datagram = &fsm->datagram; |
1371 ec_slave_t *slave = fsm->slave; |
1373 ec_slave_t *slave = fsm->slave; |
1372 |
1374 |
1373 if (datagram->state != EC_DATAGRAM_RECEIVED |
1375 if (datagram->state != EC_DATAGRAM_RECEIVED |
1374 || datagram->working_counter != 1) { |
1376 || datagram->working_counter != 1) { |
1375 slave->error_flag = 1; |
1377 slave->error_flag = 1; |
1376 fsm->slave_state = ec_fsm_error; |
1378 fsm->slave_state = ec_fsm_slave_state_error; |
1377 EC_ERR("Failed to set sync managers on slave %i.\n", |
1379 EC_ERR("Failed to set sync managers on slave %i.\n", |
1378 slave->ring_position); |
1380 slave->ring_position); |
1379 return; |
1381 return; |
1380 } |
1382 } |
1381 |
1383 |
1382 fsm->slave_state = ec_fsm_slaveconf_preop; |
1384 ec_fsm_slaveconf_enter_preop(fsm); |
1383 fsm->change_new = EC_SLAVE_STATE_PREOP; |
1385 } |
1384 fsm->change_state = ec_fsm_change_start; |
1386 |
1385 fsm->change_state(fsm); // execute immediately |
1387 /*****************************************************************************/ |
1386 } |
1388 |
1387 |
1389 /** |
1388 /*****************************************************************************/ |
1390 */ |
1389 |
1391 |
1390 /** |
1392 void ec_fsm_slaveconf_enter_preop(ec_fsm_t *fsm /**< finite state machine */) |
1391 Slave state: PREOP. |
1393 { |
1392 */ |
1394 fsm->slave_state = ec_fsm_slaveconf_state_preop; |
1393 |
1395 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
1394 void ec_fsm_slaveconf_preop(ec_fsm_t *fsm /**< finite state machine */) |
1396 ec_fsm_change_exec(&fsm->fsm_change); // execute immediately |
|
1397 } |
|
1398 |
|
1399 /*****************************************************************************/ |
|
1400 |
|
1401 /** |
|
1402 Slave configuration state: PREOP. |
|
1403 */ |
|
1404 |
|
1405 void ec_fsm_slaveconf_state_preop(ec_fsm_t *fsm /**< finite state machine */) |
1395 { |
1406 { |
1396 ec_slave_t *slave = fsm->slave; |
1407 ec_slave_t *slave = fsm->slave; |
1397 ec_master_t *master = fsm->master; |
1408 ec_master_t *master = fsm->master; |
|
1409 |
|
1410 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
|
1411 |
|
1412 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
|
1413 slave->error_flag = 1; |
|
1414 fsm->slave_state = ec_fsm_slave_state_error; |
|
1415 return; |
|
1416 } |
|
1417 |
|
1418 // slave is now in PREOP |
|
1419 slave->jiffies_preop = fsm->datagram.jiffies_received; |
|
1420 |
|
1421 if (master->debug_level) { |
|
1422 EC_DBG("Slave %i is now in PREOP.\n", slave->ring_position); |
|
1423 } |
|
1424 |
|
1425 if (slave->current_state == slave->requested_state) { |
|
1426 fsm->slave_state = ec_fsm_slave_state_end; // successful |
|
1427 if (master->debug_level) { |
|
1428 EC_DBG("Finished configuration of slave %i.\n", |
|
1429 slave->ring_position); |
|
1430 } |
|
1431 return; |
|
1432 } |
|
1433 |
|
1434 ec_fsm_slaveconf_enter_sync2(fsm); |
|
1435 } |
|
1436 |
|
1437 /*****************************************************************************/ |
|
1438 |
|
1439 /** |
|
1440 */ |
|
1441 |
|
1442 void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *fsm /**< finite state machine */) |
|
1443 { |
|
1444 ec_slave_t *slave = fsm->slave; |
|
1445 ec_datagram_t *datagram = &fsm->datagram; |
|
1446 ec_sii_sync_t *sync; |
|
1447 |
|
1448 if (list_empty(&slave->sii_syncs)) { |
|
1449 ec_fsm_slaveconf_enter_fmmu(fsm); |
|
1450 return; |
|
1451 } |
|
1452 |
|
1453 // configure sync managers for process data |
|
1454 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
|
1455 EC_SYNC_SIZE * slave->base_sync_count); |
|
1456 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
|
1457 |
|
1458 list_for_each_entry(sync, &slave->sii_syncs, list) { |
|
1459 ec_sync_config(sync, slave, |
|
1460 datagram->data + EC_SYNC_SIZE * sync->index); |
|
1461 } |
|
1462 |
|
1463 ec_master_queue_datagram(fsm->master, datagram); |
|
1464 fsm->slave_state = ec_fsm_slaveconf_state_sync2; |
|
1465 } |
|
1466 |
|
1467 /*****************************************************************************/ |
|
1468 |
|
1469 /** |
|
1470 Slave configuration state: SYNC2. |
|
1471 */ |
|
1472 |
|
1473 void ec_fsm_slaveconf_state_sync2(ec_fsm_t *fsm /**< finite state machine */) |
|
1474 { |
|
1475 ec_datagram_t *datagram = &fsm->datagram; |
|
1476 ec_slave_t *slave = fsm->slave; |
|
1477 |
|
1478 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1479 || datagram->working_counter != 1) { |
|
1480 slave->error_flag = 1; |
|
1481 fsm->slave_state = ec_fsm_slave_state_error; |
|
1482 EC_ERR("Failed to set process data sync managers on slave %i.\n", |
|
1483 slave->ring_position); |
|
1484 return; |
|
1485 } |
|
1486 |
|
1487 ec_fsm_slaveconf_enter_fmmu(fsm); |
|
1488 } |
|
1489 |
|
1490 /*****************************************************************************/ |
|
1491 |
|
1492 /** |
|
1493 */ |
|
1494 |
|
1495 void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *fsm /**< finite state machine */) |
|
1496 { |
|
1497 ec_slave_t *slave = fsm->slave; |
|
1498 ec_master_t *master = slave->master; |
1398 ec_datagram_t *datagram = &fsm->datagram; |
1499 ec_datagram_t *datagram = &fsm->datagram; |
1399 unsigned int j; |
1500 unsigned int j; |
1400 |
1501 |
1401 fsm->change_state(fsm); // execute state change state machine |
|
1402 |
|
1403 if (fsm->change_state == ec_fsm_error) { |
|
1404 slave->error_flag = 1; |
|
1405 fsm->slave_state = ec_fsm_error; |
|
1406 return; |
|
1407 } |
|
1408 |
|
1409 if (fsm->change_state != ec_fsm_end) return; |
|
1410 |
|
1411 // slave is now in PREOP |
|
1412 if (slave->current_state == slave->requested_state) { |
|
1413 fsm->slave_state = ec_fsm_end; // successful |
|
1414 return; |
|
1415 } |
|
1416 |
|
1417 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1502 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1418 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
1503 ec_fsm_slaveconf_enter_sdoconf(fsm); |
1419 fsm->slave_state = ec_fsm_slaveconf_saveop; |
|
1420 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
|
1421 fsm->change_state = ec_fsm_change_start; |
|
1422 fsm->change_state(fsm); // execute immediately |
|
1423 return; |
|
1424 } |
|
1425 fsm->slave_state = ec_fsm_slaveconf_sdoconf; |
|
1426 fsm->sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); |
|
1427 fsm->coe_state = ec_fsm_coe_down_start; |
|
1428 fsm->coe_state(fsm); // execute immediately |
|
1429 return; |
1504 return; |
1430 } |
1505 } |
1431 |
1506 |
1432 // configure FMMUs |
1507 // configure FMMUs |
1433 ec_datagram_npwr(datagram, slave->station_address, |
1508 ec_datagram_npwr(datagram, slave->station_address, |
1437 ec_fmmu_config(&slave->fmmus[j], slave, |
1512 ec_fmmu_config(&slave->fmmus[j], slave, |
1438 datagram->data + EC_FMMU_SIZE * j); |
1513 datagram->data + EC_FMMU_SIZE * j); |
1439 } |
1514 } |
1440 |
1515 |
1441 ec_master_queue_datagram(master, datagram); |
1516 ec_master_queue_datagram(master, datagram); |
1442 fsm->slave_state = ec_fsm_slaveconf_fmmu; |
1517 fsm->slave_state = ec_fsm_slaveconf_state_fmmu; |
1443 } |
1518 } |
1444 |
1519 |
1445 /*****************************************************************************/ |
1520 /*****************************************************************************/ |
1446 |
1521 |
1447 /** |
1522 /** |
1448 Slave state: FMMU. |
1523 Slave configuration state: FMMU. |
1449 */ |
1524 */ |
1450 |
1525 |
1451 void ec_fsm_slaveconf_fmmu(ec_fsm_t *fsm /**< finite state machine */) |
1526 void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *fsm /**< finite state machine */) |
1452 { |
1527 { |
1453 ec_datagram_t *datagram = &fsm->datagram; |
1528 ec_datagram_t *datagram = &fsm->datagram; |
1454 ec_slave_t *slave = fsm->slave; |
1529 ec_slave_t *slave = fsm->slave; |
1455 |
1530 |
1456 if (datagram->state != EC_DATAGRAM_RECEIVED |
1531 if (datagram->state != EC_DATAGRAM_RECEIVED |
1457 || datagram->working_counter != 1) { |
1532 || datagram->working_counter != 1) { |
1458 fsm->slave->error_flag = 1; |
1533 fsm->slave->error_flag = 1; |
1459 fsm->slave_state = ec_fsm_error; |
1534 fsm->slave_state = ec_fsm_slave_state_error; |
1460 EC_ERR("Failed to set FMMUs on slave %i.\n", |
1535 EC_ERR("Failed to set FMMUs on slave %i.\n", |
1461 fsm->slave->ring_position); |
1536 fsm->slave->ring_position); |
1462 return; |
1537 return; |
1463 } |
1538 } |
1464 |
1539 |
1465 // No CoE configuration to be applied? Jump to SAVEOP state. |
1540 // No CoE configuration to be applied? Jump to SAVEOP state. |
1466 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
1541 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
1467 // set state to SAVEOP |
1542 ec_fsm_slaveconf_enter_saveop(fsm); |
1468 fsm->slave_state = ec_fsm_slaveconf_saveop; |
1543 return; |
1469 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
1544 } |
1470 fsm->change_state = ec_fsm_change_start; |
1545 |
1471 fsm->change_state(fsm); // execute immediately |
1546 ec_fsm_slaveconf_enter_sdoconf(fsm); |
1472 return; |
1547 } |
1473 } |
1548 |
1474 |
1549 /*****************************************************************************/ |
1475 fsm->slave_state = ec_fsm_slaveconf_sdoconf; |
1550 |
1476 fsm->sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list); |
1551 /** |
1477 fsm->coe_state = ec_fsm_coe_down_start; |
1552 */ |
1478 fsm->coe_state(fsm); // execute immediately |
1553 |
1479 } |
1554 void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *fsm /**< finite state machine */) |
1480 |
1555 { |
1481 /*****************************************************************************/ |
1556 ec_slave_t *slave = fsm->slave; |
1482 |
1557 |
1483 /** |
1558 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
1484 Slave state: SDOCONF. |
1559 ec_fsm_slaveconf_enter_saveop(fsm); |
1485 */ |
1560 return; |
1486 |
1561 } |
1487 void ec_fsm_slaveconf_sdoconf(ec_fsm_t *fsm /**< finite state machine */) |
1562 |
1488 { |
1563 // start SDO configuration |
1489 fsm->coe_state(fsm); // execute CoE state machine |
1564 fsm->slave_state = ec_fsm_slaveconf_state_sdoconf; |
1490 |
1565 fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list); |
1491 if (fsm->coe_state == ec_fsm_error) { |
1566 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
|
1567 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
1568 } |
|
1569 |
|
1570 /*****************************************************************************/ |
|
1571 |
|
1572 /** |
|
1573 Slave configuration state: SDOCONF. |
|
1574 */ |
|
1575 |
|
1576 void ec_fsm_slaveconf_state_sdoconf(ec_fsm_t *fsm /**< finite state machine */) |
|
1577 { |
|
1578 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
|
1579 |
|
1580 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1492 fsm->slave->error_flag = 1; |
1581 fsm->slave->error_flag = 1; |
1493 fsm->slave_state = ec_fsm_error; |
1582 fsm->slave_state = ec_fsm_slave_state_error; |
1494 return; |
1583 return; |
1495 } |
1584 } |
1496 |
|
1497 if (fsm->coe_state != ec_fsm_end) return; |
|
1498 |
1585 |
1499 // Another SDO to configure? |
1586 // Another SDO to configure? |
1500 if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) { |
1587 if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) { |
1501 fsm->sdodata = list_entry(fsm->sdodata->list.next, |
1588 fsm->sdodata = list_entry(fsm->sdodata->list.next, |
1502 ec_sdo_data_t, list); |
1589 ec_sdo_data_t, list); |
1503 fsm->coe_state = ec_fsm_coe_down_start; |
1590 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
1504 fsm->coe_state(fsm); // execute immediately |
1591 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
1505 return; |
1592 return; |
1506 } |
1593 } |
1507 |
1594 |
1508 // All SDOs are now configured. |
1595 // All SDOs are now configured. |
1509 |
1596 |
1510 // set state to SAVEOP |
1597 // set state to SAVEOP |
1511 fsm->slave_state = ec_fsm_slaveconf_saveop; |
1598 ec_fsm_slaveconf_enter_saveop(fsm); |
1512 fsm->change_new = EC_SLAVE_STATE_SAVEOP; |
1599 } |
1513 fsm->change_state = ec_fsm_change_start; |
1600 |
1514 fsm->change_state(fsm); // execute immediately |
1601 /*****************************************************************************/ |
1515 } |
1602 |
1516 |
1603 /** |
1517 /*****************************************************************************/ |
1604 */ |
1518 |
1605 |
1519 /** |
1606 void ec_fsm_slaveconf_enter_saveop(ec_fsm_t *fsm /**< finite state machine */) |
1520 Slave state: SAVEOP. |
1607 { |
1521 */ |
1608 fsm->slave_state = ec_fsm_slaveconf_state_saveop; |
1522 |
1609 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAVEOP); |
1523 void ec_fsm_slaveconf_saveop(ec_fsm_t *fsm /**< finite state machine */) |
1610 ec_fsm_change_exec(&fsm->fsm_change); // execute immediately |
1524 { |
1611 } |
1525 fsm->change_state(fsm); // execute state change state machine |
1612 |
1526 |
1613 /*****************************************************************************/ |
1527 if (fsm->change_state == ec_fsm_error) { |
1614 |
|
1615 /** |
|
1616 Slave configuration state: SAVEOP. |
|
1617 */ |
|
1618 |
|
1619 void ec_fsm_slaveconf_state_saveop(ec_fsm_t *fsm /**< finite state machine */) |
|
1620 { |
|
1621 ec_master_t *master = fsm->master; |
|
1622 ec_slave_t *slave = fsm->slave; |
|
1623 |
|
1624 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
|
1625 |
|
1626 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
1528 fsm->slave->error_flag = 1; |
1627 fsm->slave->error_flag = 1; |
1529 fsm->slave_state = ec_fsm_error; |
1628 fsm->slave_state = ec_fsm_slave_state_error; |
1530 return; |
1629 return; |
1531 } |
1630 } |
1532 |
|
1533 if (fsm->change_state != ec_fsm_end) return; |
|
1534 |
1631 |
1535 // slave is now in SAVEOP |
1632 // slave is now in SAVEOP |
|
1633 |
|
1634 if (master->debug_level) { |
|
1635 EC_DBG("Slave %i is now in SAVEOP.\n", slave->ring_position); |
|
1636 } |
|
1637 |
1536 if (fsm->slave->current_state == fsm->slave->requested_state) { |
1638 if (fsm->slave->current_state == fsm->slave->requested_state) { |
1537 fsm->slave_state = ec_fsm_end; // successful |
1639 fsm->slave_state = ec_fsm_slave_state_end; // successful |
|
1640 if (master->debug_level) { |
|
1641 EC_DBG("Finished configuration of slave %i.\n", |
|
1642 slave->ring_position); |
|
1643 } |
1538 return; |
1644 return; |
1539 } |
1645 } |
1540 |
1646 |
1541 // set state to OP |
1647 // set state to OP |
1542 fsm->slave_state = ec_fsm_slaveconf_op; |
1648 fsm->slave_state = ec_fsm_slaveconf_state_op; |
1543 fsm->change_new = EC_SLAVE_STATE_OP; |
1649 ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP); |
1544 fsm->change_state = ec_fsm_change_start; |
1650 ec_fsm_change_exec(&fsm->fsm_change); // execute immediately |
1545 fsm->change_state(fsm); // execute immediately |
1651 } |
1546 } |
1652 |
1547 |
1653 /*****************************************************************************/ |
1548 /*****************************************************************************/ |
1654 |
1549 |
1655 /** |
1550 /** |
1656 Slave configuration state: OP |
1551 Slave state: OP |
1657 */ |
1552 */ |
1658 |
1553 |
1659 void ec_fsm_slaveconf_state_op(ec_fsm_t *fsm /**< finite state machine */) |
1554 void ec_fsm_slaveconf_op(ec_fsm_t *fsm /**< finite state machine */) |
1660 { |
1555 { |
1661 ec_master_t *master = fsm->master; |
1556 fsm->change_state(fsm); // execute state change state machine |
1662 ec_slave_t *slave = fsm->slave; |
1557 |
1663 |
1558 if (fsm->change_state == ec_fsm_error) { |
1664 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
1559 fsm->slave->error_flag = 1; |
1665 |
1560 fsm->slave_state = ec_fsm_error; |
1666 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
1561 return; |
1667 slave->error_flag = 1; |
1562 } |
1668 fsm->slave_state = ec_fsm_slave_state_error; |
1563 |
1669 return; |
1564 if (fsm->change_state != ec_fsm_end) return; |
1670 } |
1565 |
1671 |
1566 // slave is now in OP |
1672 // slave is now in OP |
1567 fsm->slave_state = ec_fsm_end; // successful |
1673 |
1568 } |
1674 if (master->debug_level) { |
1569 |
1675 EC_DBG("Slave %i is now in OP.\n", slave->ring_position); |
1570 /****************************************************************************** |
1676 EC_DBG("Finished configuration of slave %i.\n", slave->ring_position); |
1571 * SII state machine |
1677 } |
1572 *****************************************************************************/ |
1678 |
1573 |
1679 fsm->slave_state = ec_fsm_slave_state_end; // successful |
1574 /** |
|
1575 SII state: START_READING. |
|
1576 Starts reading the slave information interface. |
|
1577 */ |
|
1578 |
|
1579 void ec_fsm_sii_start_reading(ec_fsm_t *fsm /**< finite state machine */) |
|
1580 { |
|
1581 ec_datagram_t *datagram = &fsm->datagram; |
|
1582 |
|
1583 // initiate read operation |
|
1584 if (fsm->sii_mode) { |
|
1585 ec_datagram_npwr(datagram, fsm->slave->station_address, 0x502, 4); |
|
1586 } |
|
1587 else { |
|
1588 ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4); |
|
1589 } |
|
1590 |
|
1591 EC_WRITE_U8 (datagram->data, 0x00); // read-only access |
|
1592 EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation |
|
1593 EC_WRITE_U16(datagram->data + 2, fsm->sii_offset); |
|
1594 ec_master_queue_datagram(fsm->master, datagram); |
|
1595 fsm->sii_state = ec_fsm_sii_read_check; |
|
1596 } |
|
1597 |
|
1598 /*****************************************************************************/ |
|
1599 |
|
1600 /** |
|
1601 SII state: READ_CHECK. |
|
1602 Checks, if the SII-read-datagram has been sent and issues a fetch datagram. |
|
1603 */ |
|
1604 |
|
1605 void ec_fsm_sii_read_check(ec_fsm_t *fsm /**< finite state machine */) |
|
1606 { |
|
1607 ec_datagram_t *datagram = &fsm->datagram; |
|
1608 |
|
1609 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1610 || datagram->working_counter != 1) { |
|
1611 EC_ERR("SII: Reception of read datagram failed.\n"); |
|
1612 fsm->sii_state = ec_fsm_error; |
|
1613 return; |
|
1614 } |
|
1615 |
|
1616 fsm->sii_start = get_cycles(); |
|
1617 |
|
1618 // issue check/fetch datagram |
|
1619 if (fsm->sii_mode) { |
|
1620 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); |
|
1621 } |
|
1622 else { |
|
1623 ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10); |
|
1624 } |
|
1625 |
|
1626 ec_master_queue_datagram(fsm->master, datagram); |
|
1627 fsm->sii_state = ec_fsm_sii_read_fetch; |
|
1628 } |
|
1629 |
|
1630 /*****************************************************************************/ |
|
1631 |
|
1632 /** |
|
1633 SII state: READ_FETCH. |
|
1634 Fetches the result of an SII-read datagram. |
|
1635 */ |
|
1636 |
|
1637 void ec_fsm_sii_read_fetch(ec_fsm_t *fsm /**< finite state machine */) |
|
1638 { |
|
1639 ec_datagram_t *datagram = &fsm->datagram; |
|
1640 |
|
1641 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1642 || datagram->working_counter != 1) { |
|
1643 EC_ERR("SII: Reception of check/fetch datagram failed.\n"); |
|
1644 fsm->sii_state = ec_fsm_error; |
|
1645 return; |
|
1646 } |
|
1647 |
|
1648 // check "busy bit" |
|
1649 if (EC_READ_U8(datagram->data + 1) & 0x81) { |
|
1650 // still busy... timeout? |
|
1651 if (get_cycles() - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { |
|
1652 EC_ERR("SII: Timeout.\n"); |
|
1653 fsm->sii_state = ec_fsm_error; |
|
1654 #if 0 |
|
1655 EC_DBG("SII busy: %02X %02X %02X %02X\n", |
|
1656 EC_READ_U8(datagram->data + 0), |
|
1657 EC_READ_U8(datagram->data + 1), |
|
1658 EC_READ_U8(datagram->data + 2), |
|
1659 EC_READ_U8(datagram->data + 3)); |
|
1660 #endif |
|
1661 } |
|
1662 |
|
1663 // issue check/fetch datagram again |
|
1664 if (fsm->sii_mode) { |
|
1665 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 10); |
|
1666 } |
|
1667 else { |
|
1668 ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10); |
|
1669 } |
|
1670 ec_master_queue_datagram(fsm->master, datagram); |
|
1671 return; |
|
1672 } |
|
1673 |
|
1674 #if 0 |
|
1675 EC_DBG("SII rec: %02X %02X %02X %02X - %02X %02X %02X %02X\n", |
|
1676 EC_READ_U8(datagram->data + 0), EC_READ_U8(datagram->data + 1), |
|
1677 EC_READ_U8(datagram->data + 2), EC_READ_U8(datagram->data + 3), |
|
1678 EC_READ_U8(datagram->data + 6), EC_READ_U8(datagram->data + 7), |
|
1679 EC_READ_U8(datagram->data + 8), EC_READ_U8(datagram->data + 9)); |
|
1680 #endif |
|
1681 |
|
1682 // SII value received. |
|
1683 memcpy(fsm->sii_value, datagram->data + 6, 4); |
|
1684 fsm->sii_state = ec_fsm_end; |
|
1685 } |
|
1686 |
|
1687 /*****************************************************************************/ |
|
1688 |
|
1689 /** |
|
1690 SII state: START_WRITING. |
|
1691 Starts reading the slave information interface. |
|
1692 */ |
|
1693 |
|
1694 void ec_fsm_sii_start_writing(ec_fsm_t *fsm /**< finite state machine */) |
|
1695 { |
|
1696 ec_datagram_t *datagram = &fsm->datagram; |
|
1697 |
|
1698 // initiate write operation |
|
1699 ec_datagram_npwr(datagram, fsm->slave->station_address, 0x502, 8); |
|
1700 EC_WRITE_U8 (datagram->data, 0x01); // enable write access |
|
1701 EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation |
|
1702 EC_WRITE_U32(datagram->data + 2, fsm->sii_offset); |
|
1703 memcpy(datagram->data + 6, fsm->sii_value, 2); |
|
1704 ec_master_queue_datagram(fsm->master, datagram); |
|
1705 fsm->sii_state = ec_fsm_sii_write_check; |
|
1706 } |
|
1707 |
|
1708 /*****************************************************************************/ |
|
1709 |
|
1710 /** |
|
1711 SII state: WRITE_CHECK. |
|
1712 */ |
|
1713 |
|
1714 void ec_fsm_sii_write_check(ec_fsm_t *fsm /**< finite state machine */) |
|
1715 { |
|
1716 ec_datagram_t *datagram = &fsm->datagram; |
|
1717 |
|
1718 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1719 || datagram->working_counter != 1) { |
|
1720 EC_ERR("SII: Reception of write datagram failed.\n"); |
|
1721 fsm->sii_state = ec_fsm_error; |
|
1722 return; |
|
1723 } |
|
1724 |
|
1725 fsm->sii_start = get_cycles(); |
|
1726 |
|
1727 // issue check/fetch datagram |
|
1728 ec_datagram_nprd(datagram, fsm->slave->station_address, 0x502, 2); |
|
1729 ec_master_queue_datagram(fsm->master, datagram); |
|
1730 fsm->sii_state = ec_fsm_sii_write_check2; |
|
1731 } |
|
1732 |
|
1733 /*****************************************************************************/ |
|
1734 |
|
1735 /** |
|
1736 SII state: WRITE_CHECK2. |
|
1737 */ |
|
1738 |
|
1739 void ec_fsm_sii_write_check2(ec_fsm_t *fsm /**< finite state machine */) |
|
1740 { |
|
1741 ec_datagram_t *datagram = &fsm->datagram; |
|
1742 |
|
1743 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1744 || datagram->working_counter != 1) { |
|
1745 EC_ERR("SII: Reception of write check datagram failed.\n"); |
|
1746 fsm->sii_state = ec_fsm_error; |
|
1747 return; |
|
1748 } |
|
1749 |
|
1750 if (EC_READ_U8(datagram->data + 1) & 0x82) { |
|
1751 // still busy... timeout? |
|
1752 if (get_cycles() - fsm->sii_start >= (cycles_t) 10 * cpu_khz) { |
|
1753 EC_ERR("SII: Write timeout.\n"); |
|
1754 fsm->sii_state = ec_fsm_error; |
|
1755 } |
|
1756 |
|
1757 // issue check/fetch datagram again |
|
1758 ec_master_queue_datagram(fsm->master, datagram); |
|
1759 } |
|
1760 else if (EC_READ_U8(datagram->data + 1) & 0x40) { |
|
1761 EC_ERR("SII: Write operation failed!\n"); |
|
1762 fsm->sii_state = ec_fsm_error; |
|
1763 } |
|
1764 else { // success |
|
1765 fsm->sii_state = ec_fsm_end; |
|
1766 } |
|
1767 } |
|
1768 |
|
1769 /****************************************************************************** |
|
1770 * state change state machine |
|
1771 *****************************************************************************/ |
|
1772 |
|
1773 /** |
|
1774 Change state: START. |
|
1775 */ |
|
1776 |
|
1777 void ec_fsm_change_start(ec_fsm_t *fsm /**< finite state machine */) |
|
1778 { |
|
1779 ec_datagram_t *datagram = &fsm->datagram; |
|
1780 ec_slave_t *slave = fsm->slave; |
|
1781 |
|
1782 fsm->change_jiffies = jiffies; |
|
1783 |
|
1784 // write new state to slave |
|
1785 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
|
1786 EC_WRITE_U16(datagram->data, fsm->change_new); |
|
1787 ec_master_queue_datagram(fsm->master, datagram); |
|
1788 fsm->change_state = ec_fsm_change_check; |
|
1789 } |
|
1790 |
|
1791 /*****************************************************************************/ |
|
1792 |
|
1793 /** |
|
1794 Change state: CHECK. |
|
1795 */ |
|
1796 |
|
1797 void ec_fsm_change_check(ec_fsm_t *fsm /**< finite state machine */) |
|
1798 { |
|
1799 ec_datagram_t *datagram = &fsm->datagram; |
|
1800 ec_slave_t *slave = fsm->slave; |
|
1801 |
|
1802 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1803 fsm->change_state = ec_fsm_error; |
|
1804 EC_ERR("Failed to send state datagram to slave %i!\n", |
|
1805 fsm->slave->ring_position); |
|
1806 return; |
|
1807 } |
|
1808 |
|
1809 if (datagram->working_counter != 1) { |
|
1810 if (jiffies - fsm->change_jiffies >= 3 * HZ) { |
|
1811 fsm->change_state = ec_fsm_error; |
|
1812 EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not" |
|
1813 " respond.\n", fsm->change_new, fsm->slave->ring_position); |
|
1814 return; |
|
1815 } |
|
1816 |
|
1817 // repeat writing new state to slave |
|
1818 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
|
1819 EC_WRITE_U16(datagram->data, fsm->change_new); |
|
1820 ec_master_queue_datagram(fsm->master, datagram); |
|
1821 return; |
|
1822 } |
|
1823 |
|
1824 fsm->change_jiffies = jiffies; |
|
1825 |
|
1826 // read AL status from slave |
|
1827 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
|
1828 ec_master_queue_datagram(fsm->master, datagram); |
|
1829 fsm->change_state = ec_fsm_change_status; |
|
1830 } |
|
1831 |
|
1832 /*****************************************************************************/ |
|
1833 |
|
1834 /** |
|
1835 Change state: STATUS. |
|
1836 */ |
|
1837 |
|
1838 void ec_fsm_change_status(ec_fsm_t *fsm /**< finite state machine */) |
|
1839 { |
|
1840 ec_datagram_t *datagram = &fsm->datagram; |
|
1841 ec_slave_t *slave = fsm->slave; |
|
1842 |
|
1843 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1844 || datagram->working_counter != 1) { |
|
1845 fsm->change_state = ec_fsm_error; |
|
1846 EC_ERR("Failed to check state 0x%02X on slave %i.\n", |
|
1847 fsm->change_new, slave->ring_position); |
|
1848 return; |
|
1849 } |
|
1850 |
|
1851 slave->current_state = EC_READ_U8(datagram->data); |
|
1852 |
|
1853 if (slave->current_state == fsm->change_new) { |
|
1854 // state has been set successfully |
|
1855 fsm->change_state = ec_fsm_end; |
|
1856 return; |
|
1857 } |
|
1858 |
|
1859 if (slave->current_state & 0x10) { |
|
1860 // state change error |
|
1861 fsm->change_new = slave->current_state & 0x0F; |
|
1862 EC_ERR("Failed to set state 0x%02X - Slave %i refused state change" |
|
1863 " (code 0x%02X)!\n", fsm->change_new, slave->ring_position, |
|
1864 slave->current_state); |
|
1865 // fetch AL status error code |
|
1866 ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); |
|
1867 ec_master_queue_datagram(fsm->master, datagram); |
|
1868 fsm->change_state = ec_fsm_change_code; |
|
1869 return; |
|
1870 } |
|
1871 |
|
1872 if (jiffies - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms |
|
1873 // timeout while checking |
|
1874 fsm->change_state = ec_fsm_error; |
|
1875 EC_ERR("Timeout while setting state 0x%02X on slave %i.\n", |
|
1876 fsm->change_new, slave->ring_position); |
|
1877 return; |
|
1878 } |
|
1879 |
|
1880 // still old state: check again |
|
1881 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
|
1882 ec_master_queue_datagram(fsm->master, datagram); |
|
1883 } |
|
1884 |
|
1885 /*****************************************************************************/ |
|
1886 |
|
1887 /** |
|
1888 Application layer status messages. |
|
1889 */ |
|
1890 |
|
1891 const ec_code_msg_t al_status_messages[] = { |
|
1892 {0x0001, "Unspecified error"}, |
|
1893 {0x0011, "Invalud requested state change"}, |
|
1894 {0x0012, "Unknown requested state"}, |
|
1895 {0x0013, "Bootstrap not supported"}, |
|
1896 {0x0014, "No valid firmware"}, |
|
1897 {0x0015, "Invalid mailbox configuration"}, |
|
1898 {0x0016, "Invalid mailbox configuration"}, |
|
1899 {0x0017, "Invalid sync manager configuration"}, |
|
1900 {0x0018, "No valid inputs available"}, |
|
1901 {0x0019, "No valid outputs"}, |
|
1902 {0x001A, "Synchronisation error"}, |
|
1903 {0x001B, "Sync manager watchdog"}, |
|
1904 {0x001C, "Invalid sync manager types"}, |
|
1905 {0x001D, "Invalid output configuration"}, |
|
1906 {0x001E, "Invalid input configuration"}, |
|
1907 {0x001F, "Invalid watchdog configuration"}, |
|
1908 {0x0020, "Slave needs cold start"}, |
|
1909 {0x0021, "Slave needs INIT"}, |
|
1910 {0x0022, "Slave needs PREOP"}, |
|
1911 {0x0023, "Slave needs SAVEOP"}, |
|
1912 {0x0030, "Invalid DC SYNCH configuration"}, |
|
1913 {0x0031, "Invalid DC latch configuration"}, |
|
1914 {0x0032, "PLL error"}, |
|
1915 {0x0033, "Invalid DC IO error"}, |
|
1916 {0x0034, "Invalid DC timeout error"}, |
|
1917 {0x0042, "MBOX EOE"}, |
|
1918 {0x0043, "MBOX COE"}, |
|
1919 {0x0044, "MBOX FOE"}, |
|
1920 {0x0045, "MBOX SOE"}, |
|
1921 {0x004F, "MBOX VOE"}, |
|
1922 {} |
|
1923 }; |
|
1924 |
|
1925 /*****************************************************************************/ |
|
1926 |
|
1927 /** |
|
1928 Change state: CODE. |
|
1929 */ |
|
1930 |
|
1931 void ec_fsm_change_code(ec_fsm_t *fsm /**< finite state machine */) |
|
1932 { |
|
1933 ec_datagram_t *datagram = &fsm->datagram; |
|
1934 ec_slave_t *slave = fsm->slave; |
|
1935 uint32_t code; |
|
1936 const ec_code_msg_t *al_msg; |
|
1937 |
|
1938 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1939 || datagram->working_counter != 1) { |
|
1940 fsm->change_state = ec_fsm_error; |
|
1941 EC_ERR("Reception of AL status code datagram failed.\n"); |
|
1942 return; |
|
1943 } |
|
1944 |
|
1945 if ((code = EC_READ_U16(datagram->data))) { |
|
1946 for (al_msg = al_status_messages; al_msg->code; al_msg++) { |
|
1947 if (al_msg->code != code) continue; |
|
1948 EC_ERR("AL status message 0x%04X: \"%s\".\n", |
|
1949 al_msg->code, al_msg->message); |
|
1950 break; |
|
1951 } |
|
1952 if (!al_msg->code) |
|
1953 EC_ERR("Unknown AL status code 0x%04X.\n", code); |
|
1954 } |
|
1955 |
|
1956 // acknowledge "old" slave state |
|
1957 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
|
1958 EC_WRITE_U16(datagram->data, slave->current_state); |
|
1959 ec_master_queue_datagram(fsm->master, datagram); |
|
1960 fsm->change_state = ec_fsm_change_ack; |
|
1961 } |
|
1962 |
|
1963 /*****************************************************************************/ |
|
1964 |
|
1965 /** |
|
1966 Change state: ACK. |
|
1967 */ |
|
1968 |
|
1969 void ec_fsm_change_ack(ec_fsm_t *fsm /**< finite state machine */) |
|
1970 { |
|
1971 ec_datagram_t *datagram = &fsm->datagram; |
|
1972 ec_slave_t *slave = fsm->slave; |
|
1973 |
|
1974 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1975 || datagram->working_counter != 1) { |
|
1976 fsm->change_state = ec_fsm_error; |
|
1977 EC_ERR("Reception of state ack datagram failed.\n"); |
|
1978 return; |
|
1979 } |
|
1980 |
|
1981 fsm->change_jiffies = jiffies; |
|
1982 |
|
1983 // read new AL status |
|
1984 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
|
1985 ec_master_queue_datagram(fsm->master, datagram); |
|
1986 fsm->change_state = ec_fsm_change_check_ack; |
|
1987 } |
|
1988 |
|
1989 /*****************************************************************************/ |
|
1990 |
|
1991 /** |
|
1992 Change state: CHECK ACK. |
|
1993 */ |
|
1994 |
|
1995 void ec_fsm_change_check_ack(ec_fsm_t *fsm /**< finite state machine */) |
|
1996 { |
|
1997 ec_datagram_t *datagram = &fsm->datagram; |
|
1998 ec_slave_t *slave = fsm->slave; |
|
1999 ec_slave_state_t ack_state; |
|
2000 |
|
2001 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
2002 || datagram->working_counter != 1) { |
|
2003 fsm->change_state = ec_fsm_error; |
|
2004 EC_ERR("Reception of state ack check datagram failed.\n"); |
|
2005 return; |
|
2006 } |
|
2007 |
|
2008 ack_state = EC_READ_U8(datagram->data); |
|
2009 |
|
2010 if (ack_state == slave->current_state) { |
|
2011 fsm->change_state = ec_fsm_error; |
|
2012 EC_INFO("Acknowleged state 0x%02X on slave %i.\n", |
|
2013 slave->current_state, slave->ring_position); |
|
2014 return; |
|
2015 } |
|
2016 |
|
2017 if (jiffies - fsm->change_jiffies >= 100 * HZ / 1000) { // 100ms |
|
2018 // timeout while checking |
|
2019 slave->current_state = EC_SLAVE_STATE_UNKNOWN; |
|
2020 fsm->change_state = ec_fsm_error; |
|
2021 EC_ERR("Timeout while acknowleging state 0x%02X on slave %i.\n", |
|
2022 fsm->change_new, slave->ring_position); |
|
2023 return; |
|
2024 } |
|
2025 |
|
2026 // reread new AL status |
|
2027 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
|
2028 ec_master_queue_datagram(fsm->master, datagram); |
|
2029 } |
|
2030 |
|
2031 /****************************************************************************** |
|
2032 * CoE state machine |
|
2033 *****************************************************************************/ |
|
2034 |
|
2035 /** |
|
2036 CoE state: DOWN_START. |
|
2037 */ |
|
2038 |
|
2039 void ec_fsm_coe_down_start(ec_fsm_t *fsm /**< finite state machine */) |
|
2040 { |
|
2041 ec_datagram_t *datagram = &fsm->datagram; |
|
2042 ec_slave_t *slave = fsm->slave; |
|
2043 ec_sdo_data_t *sdodata = fsm->sdodata; |
|
2044 uint8_t *data; |
|
2045 |
|
2046 EC_INFO("Downloading SDO 0x%04X:%i to slave %i.\n", |
|
2047 sdodata->index, sdodata->subindex, slave->ring_position); |
|
2048 |
|
2049 if (slave->sii_rx_mailbox_size < 6 + 10 + sdodata->size) { |
|
2050 EC_ERR("SDO fragmenting not supported yet!\n"); |
|
2051 fsm->coe_state = ec_fsm_error; |
|
2052 return; |
|
2053 } |
|
2054 |
|
2055 if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
|
2056 sdodata->size + 10))) { |
|
2057 fsm->coe_state = ec_fsm_error; |
|
2058 return; |
|
2059 } |
|
2060 |
|
2061 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
2062 EC_WRITE_U8 (data + 2, (0x1 // size specified |
|
2063 | 0x1 << 5)); // Download request |
|
2064 EC_WRITE_U16(data + 3, sdodata->index); |
|
2065 EC_WRITE_U8 (data + 5, sdodata->subindex); |
|
2066 EC_WRITE_U32(data + 6, sdodata->size); |
|
2067 memcpy(data + 10, sdodata->data, sdodata->size); |
|
2068 |
|
2069 ec_master_queue_datagram(fsm->master, datagram); |
|
2070 fsm->coe_state = ec_fsm_coe_down_request; |
|
2071 } |
|
2072 |
|
2073 /*****************************************************************************/ |
|
2074 |
|
2075 /** |
|
2076 CoE state: DOWN_REQUEST. |
|
2077 */ |
|
2078 |
|
2079 void ec_fsm_coe_down_request(ec_fsm_t *fsm /**< finite state machine */) |
|
2080 { |
|
2081 ec_datagram_t *datagram = &fsm->datagram; |
|
2082 ec_slave_t *slave = fsm->slave; |
|
2083 |
|
2084 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
2085 || datagram->working_counter != 1) { |
|
2086 fsm->coe_state = ec_fsm_error; |
|
2087 EC_ERR("Reception of CoE download request failed.\n"); |
|
2088 return; |
|
2089 } |
|
2090 |
|
2091 fsm->coe_start = get_cycles(); |
|
2092 |
|
2093 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
2094 ec_master_queue_datagram(fsm->master, datagram); |
|
2095 fsm->coe_state = ec_fsm_coe_down_check; |
|
2096 } |
|
2097 |
|
2098 /*****************************************************************************/ |
|
2099 |
|
2100 /** |
|
2101 CoE state: DOWN_CHECK. |
|
2102 */ |
|
2103 |
|
2104 void ec_fsm_coe_down_check(ec_fsm_t *fsm /**< finite state machine */) |
|
2105 { |
|
2106 ec_datagram_t *datagram = &fsm->datagram; |
|
2107 ec_slave_t *slave = fsm->slave; |
|
2108 |
|
2109 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
2110 || datagram->working_counter != 1) { |
|
2111 fsm->coe_state = ec_fsm_error; |
|
2112 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
|
2113 return; |
|
2114 } |
|
2115 |
|
2116 if (!ec_slave_mbox_check(datagram)) { |
|
2117 if (get_cycles() - fsm->coe_start >= (cycles_t) 100 * cpu_khz) { |
|
2118 fsm->coe_state = ec_fsm_error; |
|
2119 EC_ERR("Timeout while checking SDO configuration on slave %i.\n", |
|
2120 slave->ring_position); |
|
2121 return; |
|
2122 } |
|
2123 |
|
2124 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
2125 ec_master_queue_datagram(fsm->master, datagram); |
|
2126 return; |
|
2127 } |
|
2128 |
|
2129 // Fetch response |
|
2130 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
|
2131 ec_master_queue_datagram(fsm->master, datagram); |
|
2132 fsm->coe_state = ec_fsm_coe_down_response; |
|
2133 } |
|
2134 |
|
2135 /*****************************************************************************/ |
|
2136 |
|
2137 /** |
|
2138 CoE state: DOWN_RESPONSE. |
|
2139 */ |
|
2140 |
|
2141 void ec_fsm_coe_down_response(ec_fsm_t *fsm /**< finite state machine */) |
|
2142 { |
|
2143 ec_datagram_t *datagram = &fsm->datagram; |
|
2144 ec_slave_t *slave = fsm->slave; |
|
2145 uint8_t *data, mbox_prot; |
|
2146 size_t rec_size; |
|
2147 ec_sdo_data_t *sdodata = fsm->sdodata; |
|
2148 |
|
2149 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
2150 || datagram->working_counter != 1) { |
|
2151 fsm->coe_state = ec_fsm_error; |
|
2152 EC_ERR("Reception of CoE download response failed.\n"); |
|
2153 return; |
|
2154 } |
|
2155 |
|
2156 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
|
2157 &mbox_prot, &rec_size))) { |
|
2158 fsm->coe_state = ec_fsm_error; |
|
2159 return; |
|
2160 } |
|
2161 |
|
2162 if (mbox_prot != 0x03) { // CoE |
|
2163 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
|
2164 fsm->coe_state = ec_fsm_error; |
|
2165 return; |
|
2166 } |
|
2167 |
|
2168 if (rec_size < 6) { |
|
2169 fsm->coe_state = ec_fsm_error; |
|
2170 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |
|
2171 ec_print_data(data, rec_size); |
|
2172 return; |
|
2173 } |
|
2174 |
|
2175 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
|
2176 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
|
2177 fsm->coe_state = ec_fsm_error; |
|
2178 EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n", |
|
2179 sdodata->index, sdodata->subindex, sdodata->size, |
|
2180 slave->ring_position); |
|
2181 if (rec_size < 10) { |
|
2182 EC_ERR("Incomplete Abort command:\n"); |
|
2183 ec_print_data(data, rec_size); |
|
2184 } |
|
2185 else |
|
2186 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
2187 return; |
|
2188 } |
|
2189 |
|
2190 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
|
2191 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
|
2192 EC_READ_U16(data + 3) != sdodata->index || // index |
|
2193 EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex |
|
2194 fsm->coe_state = ec_fsm_error; |
|
2195 EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n", |
|
2196 sdodata->index, sdodata->subindex, sdodata->size); |
|
2197 EC_ERR("Invalid SDO download response at slave %i!\n", |
|
2198 slave->ring_position); |
|
2199 ec_print_data(data, rec_size); |
|
2200 return; |
|
2201 } |
|
2202 |
|
2203 fsm->coe_state = ec_fsm_end; // success |
|
2204 } |
|
2205 |
|
2206 /*****************************************************************************/ |
|
2207 |
|
2208 /** |
|
2209 SDO abort messages. |
|
2210 The "abort SDO transfer request" supplies an abort code, |
|
2211 which can be translated to clear text. This table does |
|
2212 the mapping of the codes and messages. |
|
2213 */ |
|
2214 |
|
2215 const ec_code_msg_t sdo_abort_messages[] = { |
|
2216 {0x05030000, "Toggle bit not changed"}, |
|
2217 {0x05040000, "SDO protocol timeout"}, |
|
2218 {0x05040001, "Client/Server command specifier not valid or unknown"}, |
|
2219 {0x05040005, "Out of memory"}, |
|
2220 {0x06010000, "Unsupported access to an object"}, |
|
2221 {0x06010001, "Attempt to read a write-only object"}, |
|
2222 {0x06010002, "Attempt to write a read-only object"}, |
|
2223 {0x06020000, "This object does not exist in the object directory"}, |
|
2224 {0x06040041, "The object cannot be mapped into the PDO"}, |
|
2225 {0x06040042, "The number and length of the objects to be mapped would" |
|
2226 " exceed the PDO length"}, |
|
2227 {0x06040043, "General parameter incompatibility reason"}, |
|
2228 {0x06040047, "Gerneral internal incompatibility in device"}, |
|
2229 {0x06060000, "Access failure due to a hardware error"}, |
|
2230 {0x06070010, "Data type does not match, length of service parameter does" |
|
2231 " not match"}, |
|
2232 {0x06070012, "Data type does not match, length of service parameter too" |
|
2233 " high"}, |
|
2234 {0x06070013, "Data type does not match, length of service parameter too" |
|
2235 " low"}, |
|
2236 {0x06090011, "Subindex does not exist"}, |
|
2237 {0x06090030, "Value range of parameter exceeded"}, |
|
2238 {0x06090031, "Value of parameter written too high"}, |
|
2239 {0x06090032, "Value of parameter written too low"}, |
|
2240 {0x06090036, "Maximum value is less than minimum value"}, |
|
2241 {0x08000000, "General error"}, |
|
2242 {0x08000020, "Data cannot be transferred or stored to the application"}, |
|
2243 {0x08000021, "Data cannot be transferred or stored to the application" |
|
2244 " because of local control"}, |
|
2245 {0x08000022, "Data cannot be transferred or stored to the application" |
|
2246 " because of the present device state"}, |
|
2247 {0x08000023, "Object dictionary dynamic generation fails or no object" |
|
2248 " dictionary is present"}, |
|
2249 {} |
|
2250 }; |
|
2251 |
|
2252 /*****************************************************************************/ |
|
2253 |
|
2254 /** |
|
2255 Outputs an SDO abort message. |
|
2256 */ |
|
2257 |
|
2258 void ec_canopen_abort_msg(uint32_t abort_code) |
|
2259 { |
|
2260 const ec_code_msg_t *abort_msg; |
|
2261 |
|
2262 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
|
2263 if (abort_msg->code == abort_code) { |
|
2264 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
|
2265 abort_msg->code, abort_msg->message); |
|
2266 return; |
|
2267 } |
|
2268 } |
|
2269 |
|
2270 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
|
2271 } |
1680 } |
2272 |
1681 |
2273 /****************************************************************************** |
1682 /****************************************************************************** |
2274 * Common state functions |
1683 * Common state functions |
2275 *****************************************************************************/ |
1684 *****************************************************************************/ |
2276 |
1685 |
2277 /** |
1686 /** |
2278 State: ERROR. |
1687 State: ERROR. |
2279 */ |
1688 */ |
2280 |
1689 |
2281 void ec_fsm_error(ec_fsm_t *fsm /**< finite state machine */) |
1690 void ec_fsm_slave_state_error(ec_fsm_t *fsm /**< finite state machine */) |
2282 { |
1691 { |
2283 } |
1692 } |
2284 |
1693 |
2285 /*****************************************************************************/ |
1694 /*****************************************************************************/ |
2286 |
1695 |
2287 /** |
1696 /** |
2288 State: END. |
1697 State: END. |
2289 */ |
1698 */ |
2290 |
1699 |
2291 void ec_fsm_end(ec_fsm_t *fsm /**< finite state machine */) |
1700 void ec_fsm_slave_state_end(ec_fsm_t *fsm /**< finite state machine */) |
2292 { |
1701 { |
2293 } |
1702 } |
2294 |
1703 |
2295 /*****************************************************************************/ |
1704 /*****************************************************************************/ |