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 memset(datagram->data, 0x00, |
326 memset(datagram->data, 0x00, |
305 EC_SYNC_PAGE_SIZE * slave->sii.sync_count); |
327 EC_SYNC_PAGE_SIZE * slave->sii.sync_count); |
306 |
328 |
321 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
343 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
322 EC_SYNC_PAGE_SIZE * 2); |
344 EC_SYNC_PAGE_SIZE * 2); |
323 memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * 2); |
345 memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * 2); |
324 |
346 |
325 ec_sync_init(&sync, slave); |
347 ec_sync_init(&sync, slave); |
326 sync.physical_start_address = slave->sii.rx_mailbox_offset; |
348 sync.physical_start_address = slave->sii.std_rx_mailbox_offset; |
327 sync.control_register = 0x26; |
349 sync.control_register = 0x26; |
328 sync.enable = 1; |
350 sync.enable = 1; |
329 ec_sync_page(&sync, 0, slave->sii.rx_mailbox_size, |
351 ec_sync_page(&sync, 0, slave->sii.std_rx_mailbox_size, |
330 EC_DIR_INVALID, // use default direction |
352 EC_DIR_INVALID, // use default direction |
331 datagram->data); |
353 datagram->data); |
332 |
354 |
333 ec_sync_init(&sync, slave); |
355 ec_sync_init(&sync, slave); |
334 sync.physical_start_address = slave->sii.tx_mailbox_offset; |
356 sync.physical_start_address = slave->sii.std_tx_mailbox_offset; |
335 sync.control_register = 0x22; |
357 sync.control_register = 0x22; |
336 sync.enable = 1; |
358 sync.enable = 1; |
337 ec_sync_page(&sync, 1, slave->sii.tx_mailbox_size, |
359 ec_sync_page(&sync, 1, slave->sii.std_tx_mailbox_size, |
338 EC_DIR_INVALID, // use default direction |
360 EC_DIR_INVALID, // use default direction |
339 datagram->data + EC_SYNC_PAGE_SIZE); |
361 datagram->data + EC_SYNC_PAGE_SIZE); |
340 } |
362 } |
341 |
363 |
342 fsm->retries = EC_FSM_RETRIES; |
364 fsm->retries = EC_FSM_RETRIES; |
372 slave->ring_position); |
394 slave->ring_position); |
373 ec_datagram_print_wc_error(datagram); |
395 ec_datagram_print_wc_error(datagram); |
374 return; |
396 return; |
375 } |
397 } |
376 |
398 |
377 ec_fsm_slave_config_enter_preop(fsm); |
399 ec_fsm_slave_config_enter_boot_preop(fsm); |
378 } |
400 } |
379 |
401 |
380 /*****************************************************************************/ |
402 /*****************************************************************************/ |
381 |
403 |
382 /** Request PREOP state. |
404 /** Request PREOP state. |
383 */ |
405 */ |
384 void ec_fsm_slave_config_enter_preop( |
406 void ec_fsm_slave_config_enter_boot_preop( |
385 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
407 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
386 ) |
408 ) |
387 { |
409 { |
388 fsm->state = ec_fsm_slave_config_state_preop; |
410 fsm->state = ec_fsm_slave_config_state_boot_preop; |
389 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
411 |
|
412 if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) { |
|
413 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
|
414 } else { // BOOT |
|
415 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_BOOT); |
|
416 } |
|
417 |
390 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
418 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
391 } |
419 } |
392 |
420 |
393 /*****************************************************************************/ |
421 /*****************************************************************************/ |
394 |
422 |
395 /** Slave configuration state: PREOP. |
423 /** Slave configuration state: BOOT/PREOP. |
396 */ |
424 */ |
397 void ec_fsm_slave_config_state_preop( |
425 void ec_fsm_slave_config_state_boot_preop( |
398 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
426 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
399 ) |
427 ) |
400 { |
428 { |
401 ec_slave_t *slave = fsm->slave; |
429 ec_slave_t *slave = fsm->slave; |
402 ec_master_t *master = fsm->slave->master; |
430 ec_master_t *master = fsm->slave->master; |
408 slave->error_flag = 1; |
436 slave->error_flag = 1; |
409 fsm->state = ec_fsm_slave_config_state_error; |
437 fsm->state = ec_fsm_slave_config_state_error; |
410 return; |
438 return; |
411 } |
439 } |
412 |
440 |
413 // slave is now in PREOP |
441 // slave is now in BOOT/PREOP |
414 slave->jiffies_preop = fsm->datagram->jiffies_received; |
442 slave->jiffies_preop = fsm->datagram->jiffies_received; |
415 |
443 |
416 if (master->debug_level) { |
444 if (master->debug_level) { |
417 EC_DBG("Slave %u is now in PREOP.\n", slave->ring_position); |
445 EC_DBG("Slave %u is now in %s.\n", slave->ring_position, |
|
446 slave->requested_state != EC_SLAVE_STATE_BOOT |
|
447 ? "PREOP" : "BOOT"); |
418 } |
448 } |
419 |
449 |
420 if (slave->current_state == slave->requested_state) { |
450 if (slave->current_state == slave->requested_state) { |
421 fsm->state = ec_fsm_slave_config_state_end; // successful |
451 fsm->state = ec_fsm_slave_config_state_end; // successful |
422 if (master->debug_level) { |
452 if (master->debug_level) { |