master/master.c
changeset 2039 629d3cf05180
parent 2038 770e86a39c98
child 2040 45706e3273fd
equal deleted inserted replaced
2038:770e86a39c98 2039:629d3cf05180
    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