819 void ec_master_queue_datagram( |
819 void ec_master_queue_datagram( |
820 ec_master_t *master, /**< EtherCAT master */ |
820 ec_master_t *master, /**< EtherCAT master */ |
821 ec_datagram_t *datagram /**< datagram */ |
821 ec_datagram_t *datagram /**< datagram */ |
822 ) |
822 ) |
823 { |
823 { |
824 ec_datagram_t *queued_datagram; |
824 switch (datagram->state) { |
825 |
825 case EC_DATAGRAM_QUEUED: |
826 if (datagram->state == EC_DATAGRAM_SENT) |
|
827 return; |
|
828 // check, if the datagram is already queued |
|
829 list_for_each_entry(queued_datagram, &master->datagram_queue, queue) { |
|
830 if (queued_datagram == datagram) { |
|
831 datagram->skip_count++; |
826 datagram->skip_count++; |
832 EC_MASTER_DBG(master, 1, "skipping datagram %p.\n", datagram); |
827 EC_MASTER_DBG(master, 1, "Skipping already queued datagram %p.\n", |
|
828 datagram); |
|
829 break; |
|
830 |
|
831 case EC_DATAGRAM_SENT: |
|
832 datagram->skip_count++; |
|
833 EC_MASTER_DBG(master, 1, "Skipping already sent datagram %p.\n", |
|
834 datagram); |
|
835 break; |
|
836 |
|
837 default: |
|
838 list_add_tail(&datagram->queue, &master->datagram_queue); |
833 datagram->state = EC_DATAGRAM_QUEUED; |
839 datagram->state = EC_DATAGRAM_QUEUED; |
834 return; |
840 break; |
835 } |
841 } |
836 } |
|
837 |
|
838 list_add_tail(&datagram->queue, &master->datagram_queue); |
|
839 datagram->state = EC_DATAGRAM_QUEUED; |
|
840 } |
842 } |
841 |
843 |
842 /*****************************************************************************/ |
844 /*****************************************************************************/ |
843 |
845 |
844 /** Places a datagram in the non-application datagram queue. |
846 /** Places a datagram in the non-application datagram queue. |
1301 slave++) { |
1303 slave++) { |
1302 ec_fsm_slave_exec(&slave->fsm); |
1304 ec_fsm_slave_exec(&slave->fsm); |
1303 } |
1305 } |
1304 up(&master->master_sem); |
1306 up(&master->master_sem); |
1305 |
1307 |
1306 // inject datagrams (let the rt thread queue them, see ecrt_master_send) |
1308 // inject datagrams (let the rt thread queue them, see |
|
1309 // ecrt_master_send) |
1307 if (fsm_exec) |
1310 if (fsm_exec) |
1308 master->injection_seq_fsm++; |
1311 master->injection_seq_fsm++; |
1309 } |
1312 } |
1310 |
1313 |
1311 #ifdef EC_USE_HRTIMER |
1314 #ifdef EC_USE_HRTIMER |