1385 { |
1385 { |
1386 ec_master_t *master = fsm->master; |
1386 ec_master_t *master = fsm->master; |
1387 ec_slave_t *slave = fsm->slave; |
1387 ec_slave_t *slave = fsm->slave; |
1388 ec_datagram_t *datagram = &fsm->datagram; |
1388 ec_datagram_t *datagram = &fsm->datagram; |
1389 const ec_sii_sync_t *sync; |
1389 const ec_sii_sync_t *sync; |
|
1390 ec_sii_sync_t mbox_sync; |
1390 |
1391 |
1391 fsm->change_state(fsm); // execute state change state machine |
1392 fsm->change_state(fsm); // execute state change state machine |
1392 |
1393 |
1393 if (fsm->change_state == ec_fsm_error) { |
1394 if (fsm->change_state == ec_fsm_error) { |
1394 slave->error_flag = 1; |
1395 slave->error_flag = 1; |
1432 // configure sync managers |
1433 // configure sync managers |
1433 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
1434 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
1434 EC_SYNC_SIZE * slave->base_sync_count); |
1435 EC_SYNC_SIZE * slave->base_sync_count); |
1435 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
1436 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
1436 |
1437 |
1437 list_for_each_entry(sync, &slave->sii_syncs, list) { |
1438 if (list_empty(&slave->sii_syncs)) { |
1438 if (sync->index >= slave->base_sync_count) { |
1439 if (slave->sii_rx_mailbox_offset && slave->sii_tx_mailbox_offset) { |
1439 EC_ERR("Invalid sync manager configuration found!"); |
1440 if (slave->master->debug_level) |
1440 fsm->slave->error_flag = 1; |
1441 EC_DBG("Guessing sync manager settings for slave %i.\n", |
1441 fsm->slave_state = ec_fsm_error; |
1442 slave->ring_position); |
1442 return; |
1443 mbox_sync.index = 0; |
1443 } |
1444 mbox_sync.physical_start_address = slave->sii_tx_mailbox_offset; |
1444 ec_sync_config(sync, slave, |
1445 mbox_sync.length = slave->sii_tx_mailbox_size; |
1445 datagram->data + EC_SYNC_SIZE * sync->index); |
1446 mbox_sync.control_register = 0x26; |
|
1447 mbox_sync.enable = 0x01; |
|
1448 mbox_sync.est_length = 0; |
|
1449 ec_sync_config(&mbox_sync, slave, |
|
1450 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
|
1451 mbox_sync.index = 1; |
|
1452 mbox_sync.physical_start_address = slave->sii_rx_mailbox_offset; |
|
1453 mbox_sync.length = slave->sii_rx_mailbox_size; |
|
1454 mbox_sync.control_register = 0x22; |
|
1455 mbox_sync.enable = 0x01; |
|
1456 mbox_sync.est_length = 0; |
|
1457 ec_sync_config(&mbox_sync, slave, |
|
1458 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
|
1459 } |
|
1460 } |
|
1461 else { |
|
1462 list_for_each_entry(sync, &slave->sii_syncs, list) { |
|
1463 if (sync->index >= slave->base_sync_count) { |
|
1464 EC_ERR("Invalid sync manager configuration found!"); |
|
1465 fsm->slave->error_flag = 1; |
|
1466 fsm->slave_state = ec_fsm_error; |
|
1467 return; |
|
1468 } |
|
1469 ec_sync_config(sync, slave, |
|
1470 datagram->data + EC_SYNC_SIZE * sync->index); |
|
1471 } |
1446 } |
1472 } |
1447 |
1473 |
1448 ec_master_queue_datagram(fsm->master, datagram); |
1474 ec_master_queue_datagram(fsm->master, datagram); |
1449 fsm->slave_state = ec_fsm_slaveconf_sync; |
1475 fsm->slave_state = ec_fsm_slaveconf_sync; |
1450 } |
1476 } |