41 |
41 |
42 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
42 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
43 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
43 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
44 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
44 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
45 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
45 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
46 void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *); |
46 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
47 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
47 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
48 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
48 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
49 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
49 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
50 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
50 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
51 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
51 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
53 |
53 |
54 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
54 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
56 void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *); |
56 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
59 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
59 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
60 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); |
60 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); |
287 if (!slave->sii.mailbox_protocols) { |
287 if (!slave->sii.mailbox_protocols) { |
288 // no mailbox protocols supported |
288 // no mailbox protocols supported |
289 if (master->debug_level) |
289 if (master->debug_level) |
290 EC_DBG("Slave %u does not support mailbox communication.\n", |
290 EC_DBG("Slave %u does not support mailbox communication.\n", |
291 slave->ring_position); |
291 slave->ring_position); |
292 ec_fsm_slave_config_enter_preop(fsm); |
292 ec_fsm_slave_config_enter_boot_preop(fsm); |
293 return; |
293 return; |
294 } |
294 } |
295 |
295 |
296 if (master->debug_level) { |
296 if (master->debug_level) { |
297 EC_DBG("Configuring mailbox sync managers of slave %u.\n", |
297 EC_DBG("Configuring mailbox sync managers of slave %u.\n", |
298 slave->ring_position); |
298 slave->ring_position); |
299 } |
299 } |
300 |
300 |
301 if (slave->sii.sync_count >= 2) { // mailbox configuration provided |
301 if (slave->requested_state == EC_SLAVE_STATE_BOOT) { |
|
302 ec_sync_t sync; |
|
303 |
|
304 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
|
305 EC_SYNC_PAGE_SIZE * 2); |
|
306 memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * 2); |
|
307 |
|
308 ec_sync_init(&sync, slave); |
|
309 sync.physical_start_address = slave->sii.boot_rx_mailbox_offset; |
|
310 sync.control_register = 0x26; |
|
311 sync.enable = 1; |
|
312 ec_sync_page(&sync, 0, slave->sii.boot_rx_mailbox_size, |
|
313 EC_DIR_INVALID, // use default direction |
|
314 datagram->data); |
|
315 |
|
316 ec_sync_init(&sync, slave); |
|
317 sync.physical_start_address = slave->sii.boot_tx_mailbox_offset; |
|
318 sync.control_register = 0x22; |
|
319 sync.enable = 1; |
|
320 ec_sync_page(&sync, 1, slave->sii.boot_tx_mailbox_size, |
|
321 EC_DIR_INVALID, // use default direction |
|
322 datagram->data + EC_SYNC_PAGE_SIZE); |
|
323 } else if (slave->sii.sync_count >= 2) { // mailbox configuration provided |
302 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
324 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
303 EC_SYNC_PAGE_SIZE * slave->sii.sync_count); |
325 EC_SYNC_PAGE_SIZE * slave->sii.sync_count); |
304 ec_datagram_zero(datagram); |
326 ec_datagram_zero(datagram); |
305 |
327 |
306 for (i = 0; i < 2; i++) { |
328 for (i = 0; i < 2; i++) { |
320 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
342 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
321 EC_SYNC_PAGE_SIZE * 2); |
343 EC_SYNC_PAGE_SIZE * 2); |
322 ec_datagram_zero(datagram); |
344 ec_datagram_zero(datagram); |
323 |
345 |
324 ec_sync_init(&sync, slave); |
346 ec_sync_init(&sync, slave); |
325 sync.physical_start_address = slave->sii.rx_mailbox_offset; |
347 sync.physical_start_address = slave->sii.std_rx_mailbox_offset; |
326 sync.control_register = 0x26; |
348 sync.control_register = 0x26; |
327 sync.enable = 1; |
349 sync.enable = 1; |
328 ec_sync_page(&sync, 0, slave->sii.rx_mailbox_size, |
350 ec_sync_page(&sync, 0, slave->sii.std_rx_mailbox_size, |
329 EC_DIR_INVALID, // use default direction |
351 EC_DIR_INVALID, // use default direction |
330 datagram->data); |
352 datagram->data); |
331 |
353 |
332 ec_sync_init(&sync, slave); |
354 ec_sync_init(&sync, slave); |
333 sync.physical_start_address = slave->sii.tx_mailbox_offset; |
355 sync.physical_start_address = slave->sii.std_tx_mailbox_offset; |
334 sync.control_register = 0x22; |
356 sync.control_register = 0x22; |
335 sync.enable = 1; |
357 sync.enable = 1; |
336 ec_sync_page(&sync, 1, slave->sii.tx_mailbox_size, |
358 ec_sync_page(&sync, 1, slave->sii.std_tx_mailbox_size, |
337 EC_DIR_INVALID, // use default direction |
359 EC_DIR_INVALID, // use default direction |
338 datagram->data + EC_SYNC_PAGE_SIZE); |
360 datagram->data + EC_SYNC_PAGE_SIZE); |
339 } |
361 } |
340 |
362 |
341 fsm->retries = EC_FSM_RETRIES; |
363 fsm->retries = EC_FSM_RETRIES; |
371 slave->ring_position); |
393 slave->ring_position); |
372 ec_datagram_print_wc_error(datagram); |
394 ec_datagram_print_wc_error(datagram); |
373 return; |
395 return; |
374 } |
396 } |
375 |
397 |
376 ec_fsm_slave_config_enter_preop(fsm); |
398 ec_fsm_slave_config_enter_boot_preop(fsm); |
377 } |
399 } |
378 |
400 |
379 /*****************************************************************************/ |
401 /*****************************************************************************/ |
380 |
402 |
381 /** Request PREOP state. |
403 /** Request PREOP state. |
382 */ |
404 */ |
383 void ec_fsm_slave_config_enter_preop( |
405 void ec_fsm_slave_config_enter_boot_preop( |
384 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
406 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
385 ) |
407 ) |
386 { |
408 { |
387 fsm->state = ec_fsm_slave_config_state_preop; |
409 fsm->state = ec_fsm_slave_config_state_boot_preop; |
388 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
410 |
|
411 if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) { |
|
412 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
|
413 } else { // BOOT |
|
414 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_BOOT); |
|
415 } |
|
416 |
389 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
417 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
390 } |
418 } |
391 |
419 |
392 /*****************************************************************************/ |
420 /*****************************************************************************/ |
393 |
421 |
394 /** Slave configuration state: PREOP. |
422 /** Slave configuration state: BOOT/PREOP. |
395 */ |
423 */ |
396 void ec_fsm_slave_config_state_preop( |
424 void ec_fsm_slave_config_state_boot_preop( |
397 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
425 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
398 ) |
426 ) |
399 { |
427 { |
400 ec_slave_t *slave = fsm->slave; |
428 ec_slave_t *slave = fsm->slave; |
401 ec_master_t *master = fsm->slave->master; |
429 ec_master_t *master = fsm->slave->master; |
407 slave->error_flag = 1; |
435 slave->error_flag = 1; |
408 fsm->state = ec_fsm_slave_config_state_error; |
436 fsm->state = ec_fsm_slave_config_state_error; |
409 return; |
437 return; |
410 } |
438 } |
411 |
439 |
412 // slave is now in PREOP |
440 // slave is now in BOOT/PREOP |
413 slave->jiffies_preop = fsm->datagram->jiffies_received; |
441 slave->jiffies_preop = fsm->datagram->jiffies_received; |
414 |
442 |
415 if (master->debug_level) { |
443 if (master->debug_level) { |
416 EC_DBG("Slave %u is now in PREOP.\n", slave->ring_position); |
444 EC_DBG("Slave %u is now in %s.\n", slave->ring_position, |
|
445 slave->requested_state != EC_SLAVE_STATE_BOOT |
|
446 ? "PREOP" : "BOOT"); |
417 } |
447 } |
418 |
448 |
419 if (slave->current_state == slave->requested_state) { |
449 if (slave->current_state == slave->requested_state) { |
420 fsm->state = ec_fsm_slave_config_state_end; // successful |
450 fsm->state = ec_fsm_slave_config_state_end; // successful |
421 if (master->debug_level) { |
451 if (master->debug_level) { |