71 void ec_fsm_slaveconf_state_start(ec_fsm_t *); |
71 void ec_fsm_slaveconf_state_start(ec_fsm_t *); |
72 void ec_fsm_slaveconf_state_init(ec_fsm_t *); |
72 void ec_fsm_slaveconf_state_init(ec_fsm_t *); |
73 void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *); |
73 void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *); |
74 void ec_fsm_slaveconf_state_sync(ec_fsm_t *); |
74 void ec_fsm_slaveconf_state_sync(ec_fsm_t *); |
75 void ec_fsm_slaveconf_state_preop(ec_fsm_t *); |
75 void ec_fsm_slaveconf_state_preop(ec_fsm_t *); |
|
76 void ec_fsm_slaveconf_state_sync2(ec_fsm_t *); |
76 void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *); |
77 void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *); |
77 void ec_fsm_slaveconf_state_sdoconf(ec_fsm_t *); |
78 void ec_fsm_slaveconf_state_sdoconf(ec_fsm_t *); |
78 void ec_fsm_slaveconf_state_saveop(ec_fsm_t *); |
79 void ec_fsm_slaveconf_state_saveop(ec_fsm_t *); |
79 void ec_fsm_slaveconf_state_op(ec_fsm_t *); |
80 void ec_fsm_slaveconf_state_op(ec_fsm_t *); |
80 |
81 |
81 void ec_fsm_slaveconf_enter_sync(ec_fsm_t *); |
82 void ec_fsm_slaveconf_enter_sync(ec_fsm_t *); |
82 void ec_fsm_slaveconf_enter_preop(ec_fsm_t *); |
83 void ec_fsm_slaveconf_enter_preop(ec_fsm_t *); |
|
84 void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *); |
|
85 void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *); |
83 void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *); |
86 void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *); |
84 void ec_fsm_slaveconf_enter_saveop(ec_fsm_t *); |
87 void ec_fsm_slaveconf_enter_saveop(ec_fsm_t *); |
85 |
88 |
86 void ec_fsm_slave_state_end(ec_fsm_t *); |
89 void ec_fsm_slave_state_end(ec_fsm_t *); |
87 void ec_fsm_slave_state_error(ec_fsm_t *); |
90 void ec_fsm_slave_state_error(ec_fsm_t *); |
1359 mbox_sync.est_length = 0; |
1362 mbox_sync.est_length = 0; |
1360 ec_sync_config(&mbox_sync, slave, |
1363 ec_sync_config(&mbox_sync, slave, |
1361 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
1364 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
1362 } |
1365 } |
1363 } |
1366 } |
1364 else { |
1367 else if (slave->sii_mailbox_protocols) { // mailboxes present |
1365 list_for_each_entry(sync, &slave->sii_syncs, list) { |
1368 list_for_each_entry(sync, &slave->sii_syncs, list) { |
1366 if (sync->index >= slave->base_sync_count) { |
1369 // only configure mailbox sync-managers |
1367 EC_ERR("Invalid sync manager configuration found!"); |
1370 if (sync->index != 0 && sync->index != 1) continue; |
1368 fsm->slave->error_flag = 1; |
|
1369 fsm->slave_state = ec_fsm_slave_state_error; |
|
1370 return; |
|
1371 } |
|
1372 ec_sync_config(sync, slave, |
1371 ec_sync_config(sync, slave, |
1373 datagram->data + EC_SYNC_SIZE * sync->index); |
1372 datagram->data + EC_SYNC_SIZE * sync->index); |
1374 } |
1373 } |
1375 } |
1374 } |
1376 |
1375 |
1421 |
1420 |
1422 void ec_fsm_slaveconf_state_preop(ec_fsm_t *fsm /**< finite state machine */) |
1421 void ec_fsm_slaveconf_state_preop(ec_fsm_t *fsm /**< finite state machine */) |
1423 { |
1422 { |
1424 ec_slave_t *slave = fsm->slave; |
1423 ec_slave_t *slave = fsm->slave; |
1425 ec_master_t *master = fsm->master; |
1424 ec_master_t *master = fsm->master; |
1426 ec_datagram_t *datagram = &fsm->datagram; |
|
1427 unsigned int j; |
|
1428 |
1425 |
1429 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
1426 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
1430 |
1427 |
1431 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
1428 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
1432 slave->error_flag = 1; |
1429 slave->error_flag = 1; |
1448 slave->ring_position); |
1445 slave->ring_position); |
1449 } |
1446 } |
1450 return; |
1447 return; |
1451 } |
1448 } |
1452 |
1449 |
|
1450 ec_fsm_slaveconf_enter_sync2(fsm); |
|
1451 } |
|
1452 |
|
1453 /*****************************************************************************/ |
|
1454 |
|
1455 /** |
|
1456 */ |
|
1457 |
|
1458 void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *fsm /**< finite state machine */) |
|
1459 { |
|
1460 ec_slave_t *slave = fsm->slave; |
|
1461 ec_datagram_t *datagram = &fsm->datagram; |
|
1462 ec_sii_sync_t *sync; |
|
1463 |
|
1464 if (list_empty(&slave->sii_syncs)) { |
|
1465 ec_fsm_slaveconf_enter_fmmu(fsm); |
|
1466 return; |
|
1467 } |
|
1468 |
|
1469 // configure sync managers for process data |
|
1470 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
|
1471 EC_SYNC_SIZE * slave->base_sync_count); |
|
1472 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
|
1473 |
|
1474 list_for_each_entry(sync, &slave->sii_syncs, list) { |
|
1475 ec_sync_config(sync, slave, |
|
1476 datagram->data + EC_SYNC_SIZE * sync->index); |
|
1477 } |
|
1478 |
|
1479 ec_master_queue_datagram(fsm->master, datagram); |
|
1480 fsm->slave_state = ec_fsm_slaveconf_state_sync2; |
|
1481 } |
|
1482 |
|
1483 /*****************************************************************************/ |
|
1484 |
|
1485 /** |
|
1486 Slave configuration state: SYNC2. |
|
1487 */ |
|
1488 |
|
1489 void ec_fsm_slaveconf_state_sync2(ec_fsm_t *fsm /**< finite state machine */) |
|
1490 { |
|
1491 ec_datagram_t *datagram = &fsm->datagram; |
|
1492 ec_slave_t *slave = fsm->slave; |
|
1493 |
|
1494 if (datagram->state != EC_DATAGRAM_RECEIVED |
|
1495 || datagram->working_counter != 1) { |
|
1496 slave->error_flag = 1; |
|
1497 fsm->slave_state = ec_fsm_slave_state_error; |
|
1498 EC_ERR("Failed to set process data sync managers on slave %i.\n", |
|
1499 slave->ring_position); |
|
1500 return; |
|
1501 } |
|
1502 |
|
1503 ec_fsm_slaveconf_enter_fmmu(fsm); |
|
1504 } |
|
1505 |
|
1506 /*****************************************************************************/ |
|
1507 |
|
1508 /** |
|
1509 */ |
|
1510 |
|
1511 void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *fsm /**< finite state machine */) |
|
1512 { |
|
1513 ec_slave_t *slave = fsm->slave; |
|
1514 ec_master_t *master = slave->master; |
|
1515 ec_datagram_t *datagram = &fsm->datagram; |
|
1516 unsigned int j; |
|
1517 |
1453 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1518 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1454 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
|
1455 ec_fsm_slaveconf_enter_saveop(fsm); |
|
1456 return; |
|
1457 } |
|
1458 |
|
1459 ec_fsm_slaveconf_enter_sdoconf(fsm); |
1519 ec_fsm_slaveconf_enter_sdoconf(fsm); |
1460 return; |
1520 return; |
1461 } |
1521 } |
1462 |
1522 |
1463 // configure FMMUs |
1523 // configure FMMUs |
1507 /** |
1567 /** |
1508 */ |
1568 */ |
1509 |
1569 |
1510 void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *fsm /**< finite state machine */) |
1570 void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *fsm /**< finite state machine */) |
1511 { |
1571 { |
|
1572 ec_slave_t *slave = fsm->slave; |
|
1573 |
|
1574 if (list_empty(&slave->sdo_confs)) { // skip SDO configuration |
|
1575 ec_fsm_slaveconf_enter_saveop(fsm); |
|
1576 return; |
|
1577 } |
|
1578 |
1512 // start SDO configuration |
1579 // start SDO configuration |
1513 fsm->slave_state = ec_fsm_slaveconf_state_sdoconf; |
1580 fsm->slave_state = ec_fsm_slaveconf_state_sdoconf; |
1514 fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list); |
1581 fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list); |
1515 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
1582 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
1516 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
1583 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |