equal
deleted
inserted
replaced
54 #endif |
54 #endif |
55 #include "master.h" |
55 #include "master.h" |
56 |
56 |
57 /*****************************************************************************/ |
57 /*****************************************************************************/ |
58 |
58 |
59 /** Set to 1 to enable external datagram injection debugging. |
59 /** Set to 1 to enable fsm datagram injection debugging. |
60 */ |
60 */ |
61 #ifdef USE_TRACE_PRINTK |
61 #ifdef USE_TRACE_PRINTK |
62 #define DEBUG_INJECT 1 |
62 #define DEBUG_INJECT 1 |
63 #else |
63 #else |
64 #define DEBUG_INJECT 0 |
64 #define DEBUG_INJECT 0 |
668 */ |
668 */ |
669 void ec_master_inject_fsm_datagrams( |
669 void ec_master_inject_fsm_datagrams( |
670 ec_master_t *master /**< EtherCAT master */ |
670 ec_master_t *master /**< EtherCAT master */ |
671 ) |
671 ) |
672 { |
672 { |
673 ec_datagram_t *datagram, *n; |
673 ec_datagram_t *datagram, *next; |
674 size_t queue_size = 0; |
674 size_t queue_size = 0; |
675 |
675 |
676 if (master->fsm_queue_lock_cb) |
676 if (master->fsm_queue_lock_cb) |
677 master->fsm_queue_lock_cb(master->fsm_queue_locking_data); |
677 master->fsm_queue_lock_cb(master->fsm_queue_locking_data); |
678 if (ec_mutex_trylock(&master->fsm_queue_mutex) == 0) { |
678 if (ec_mutex_trylock(&master->fsm_queue_mutex) == 0) { |
688 } |
688 } |
689 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
689 list_for_each_entry(datagram, &master->datagram_queue, queue) { |
690 queue_size += datagram->data_size; |
690 queue_size += datagram->data_size; |
691 } |
691 } |
692 |
692 |
693 list_for_each_entry_safe(datagram, n, &master->fsm_datagram_queue, |
693 list_for_each_entry_safe(datagram, next, &master->fsm_datagram_queue, |
694 fsm_queue) { |
694 fsm_queue) { |
695 queue_size += datagram->data_size; |
695 queue_size += datagram->data_size; |
696 if (queue_size <= master->max_queue_size) { |
696 if (queue_size <= master->max_queue_size) { |
697 list_del_init(&datagram->fsm_queue); |
697 list_del_init(&datagram->fsm_queue); |
698 #if DEBUG_INJECT |
698 #if DEBUG_INJECT |
1283 ec_master_queue_fsm_datagram(master, &master->fsm_datagram); |
1283 ec_master_queue_fsm_datagram(master, &master->fsm_datagram); |
1284 } |
1284 } |
1285 for (slave = master->slaves; |
1285 for (slave = master->slaves; |
1286 slave < master->slaves + master->slave_count; |
1286 slave < master->slaves + master->slave_count; |
1287 slave++) { |
1287 slave++) { |
1288 ec_fsm_slave_exec(&slave->fsm); // may queue datagram in external queue |
1288 ec_fsm_slave_exec(&slave->fsm); // may queue datagram in fsm queue |
1289 } |
1289 } |
1290 #if defined(EC_EOE) |
1290 #if defined(EC_EOE) |
1291 if (!ec_master_eoe_processing(master)) |
1291 if (!ec_master_eoe_processing(master)) |
1292 master->fsm.idle = 0; // pump the bus as fast as possible |
1292 master->fsm.idle = 0; // pump the bus as fast as possible |
1293 #endif |
1293 #endif |
1346 if (ec_fsm_master_exec(&master->fsm)) |
1346 if (ec_fsm_master_exec(&master->fsm)) |
1347 ec_master_queue_fsm_datagram(master, &master->fsm_datagram); |
1347 ec_master_queue_fsm_datagram(master, &master->fsm_datagram); |
1348 for (slave = master->slaves; |
1348 for (slave = master->slaves; |
1349 slave < master->slaves + master->slave_count; |
1349 slave < master->slaves + master->slave_count; |
1350 slave++) { |
1350 slave++) { |
1351 ec_fsm_slave_exec(&slave->fsm); // may queue datagram in external queue |
1351 ec_fsm_slave_exec(&slave->fsm); // may queue datagram in fsm queue |
1352 } |
1352 } |
1353 #if defined(EC_EOE) |
1353 #if defined(EC_EOE) |
1354 ec_master_eoe_processing(master); |
1354 ec_master_eoe_processing(master); |
1355 #endif |
1355 #endif |
1356 ec_mutex_unlock(&master->master_mutex); |
1356 ec_mutex_unlock(&master->master_mutex); |
2037 |
2037 |
2038 /*****************************************************************************/ |
2038 /*****************************************************************************/ |
2039 |
2039 |
2040 void ecrt_master_send(ec_master_t *master) |
2040 void ecrt_master_send(ec_master_t *master) |
2041 { |
2041 { |
2042 ec_datagram_t *datagram, *n; |
2042 ec_datagram_t *datagram, *next; |
2043 |
2043 |
2044 ec_master_inject_fsm_datagrams(master); |
2044 ec_master_inject_fsm_datagrams(master); |
2045 |
2045 |
2046 if (unlikely(!master->main_device.link_state)) { |
2046 if (unlikely(!master->main_device.link_state)) { |
2047 // link is down, no datagram can be sent |
2047 // link is down, no datagram can be sent |
2048 list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) { |
2048 list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) { |
2049 datagram->state = EC_DATAGRAM_ERROR; |
2049 datagram->state = EC_DATAGRAM_ERROR; |
2050 list_del_init(&datagram->queue); |
2050 list_del_init(&datagram->queue); |
2051 } |
2051 } |
2052 |
2052 |
2053 // query link state |
2053 // query link state |