793 void ec_master_queue_external_datagram( |
793 void ec_master_queue_external_datagram( |
794 ec_master_t *master, /**< EtherCAT master */ |
794 ec_master_t *master, /**< EtherCAT master */ |
795 ec_datagram_t *datagram /**< datagram */ |
795 ec_datagram_t *datagram /**< datagram */ |
796 ) |
796 ) |
797 { |
797 { |
798 ec_datagram_t *queued_datagram; |
798 ec_datagram_t *queued_datagram; |
799 |
799 |
800 down(&master->io_sem); |
800 down(&master->io_sem); |
801 |
801 |
802 // check, if the datagram is already queued |
802 // check, if the datagram is already queued |
803 list_for_each_entry(queued_datagram, &master->external_datagram_queue, |
803 list_for_each_entry(queued_datagram, &master->external_datagram_queue, |
804 queue) { |
804 queue) { |
805 if (queued_datagram == datagram) { |
805 if (queued_datagram == datagram) { |
806 datagram->state = EC_DATAGRAM_QUEUED; |
806 datagram->state = EC_DATAGRAM_QUEUED; |
807 return; |
807 return; |
808 } |
808 } |
809 } |
809 } |
810 |
810 |
811 #if DEBUG_INJECT |
811 #if DEBUG_INJECT |
812 if (master->debug_level) { |
812 if (master->debug_level) { |
813 EC_DBG("Requesting external datagram %p size=%u\n", |
813 EC_DBG("Requesting external datagram %p size=%u\n", |
814 datagram, datagram->data_size); |
814 datagram, datagram->data_size); |
815 } |
815 } |
816 #endif |
816 #endif |
817 |
817 |
818 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
818 list_add_tail(&datagram->queue, &master->external_datagram_queue); |
819 datagram->state = EC_DATAGRAM_QUEUED; |
819 datagram->state = EC_DATAGRAM_QUEUED; |
820 #ifdef EC_HAVE_CYCLES |
820 #ifdef EC_HAVE_CYCLES |
821 datagram->cycles_sent = get_cycles(); |
821 datagram->cycles_sent = get_cycles(); |
822 #endif |
822 #endif |
823 datagram->jiffies_sent = jiffies; |
823 datagram->jiffies_sent = jiffies; |
824 |
824 |
825 master->fsm.idle = 0; |
825 master->fsm.idle = 0; |
826 up(&master->io_sem); |
826 up(&master->io_sem); |
827 } |
827 } |
828 |
828 |
829 /*****************************************************************************/ |
829 /*****************************************************************************/ |
830 |
830 |
831 /** Places a datagram in the datagram queue. |
831 /** Places a datagram in the datagram queue. |
874 * |
874 * |
875 */ |
875 */ |
876 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
876 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */) |
877 { |
877 { |
878 ec_datagram_t *datagram, *next; |
878 ec_datagram_t *datagram, *next; |
879 size_t datagram_size; |
879 size_t datagram_size; |
880 uint8_t *frame_data, *cur_data; |
880 uint8_t *frame_data, *cur_data; |
881 void *follows_word; |
881 void *follows_word; |
882 #ifdef EC_HAVE_CYCLES |
882 #ifdef EC_HAVE_CYCLES |
883 cycles_t cycles_start, cycles_sent, cycles_end; |
883 cycles_t cycles_start, cycles_sent, cycles_end; |
884 #endif |
884 #endif |
1220 static int ec_master_idle_thread(void *priv_data) |
1220 static int ec_master_idle_thread(void *priv_data) |
1221 { |
1221 { |
1222 ec_master_t *master = (ec_master_t *) priv_data; |
1222 ec_master_t *master = (ec_master_t *) priv_data; |
1223 ec_slave_t *slave = NULL; |
1223 ec_slave_t *slave = NULL; |
1224 int fsm_exec; |
1224 int fsm_exec; |
1225 size_t sent_bytes; |
1225 size_t sent_bytes; |
1226 |
1226 |
1227 // send interval in IDLE phase |
1227 // send interval in IDLE phase |
1228 ec_master_set_send_interval(master, 1000000 / HZ); |
1228 ec_master_set_send_interval(master, 1000000 / HZ); |
1229 |
1229 |
1230 if (master->debug_level) |
1230 if (master->debug_level) |
1231 EC_DBG("Idle thread running with send interval = %d us," |
1231 EC_DBG("Idle thread running with send interval = %d us," |
1232 " max data size=%d\n", master->send_interval, |
1232 " max data size=%d\n", master->send_interval, |
1233 master->max_queue_size); |
1233 master->max_queue_size); |
1234 |
1234 |
1235 while (!kthread_should_stop()) { |
1235 while (!kthread_should_stop()) { |
1236 ec_datagram_output_stats(&master->fsm_datagram); |
1236 ec_datagram_output_stats(&master->fsm_datagram); |
1257 if (fsm_exec) { |
1257 if (fsm_exec) { |
1258 ec_master_queue_datagram(master, &master->fsm_datagram); |
1258 ec_master_queue_datagram(master, &master->fsm_datagram); |
1259 } |
1259 } |
1260 ec_master_inject_external_datagrams(master); |
1260 ec_master_inject_external_datagrams(master); |
1261 ecrt_master_send(master); |
1261 ecrt_master_send(master); |
1262 sent_bytes = master->main_device.tx_skb[ |
1262 sent_bytes = master->main_device.tx_skb[ |
1263 master->main_device.tx_ring_index]->len; |
1263 master->main_device.tx_ring_index]->len; |
1264 up(&master->io_sem); |
1264 up(&master->io_sem); |
1265 |
1265 |
1266 if (ec_fsm_master_idle(&master->fsm)) { |
1266 if (ec_fsm_master_idle(&master->fsm)) { |
1267 #ifdef EC_USE_HRTIMER |
1267 #ifdef EC_USE_HRTIMER |
1268 ec_master_nanosleep(master->send_interval * 1000); |
1268 ec_master_nanosleep(master->send_interval * 1000); |
1269 #else |
1269 #else |
1270 set_current_state(TASK_INTERRUPTIBLE); |
1270 set_current_state(TASK_INTERRUPTIBLE); |
1271 schedule_timeout(1); |
1271 schedule_timeout(1); |
1272 #endif |
1272 #endif |
1273 } else { |
1273 } else { |
1274 #ifdef EC_USE_HRTIMER |
1274 #ifdef EC_USE_HRTIMER |
1275 ec_master_nanosleep(sent_bytes * EC_BYTE_TRANSMISSION_TIME_NS); |
1275 ec_master_nanosleep(sent_bytes * EC_BYTE_TRANSMISSION_TIME_NS); |
1276 #else |
1276 #else |
1277 schedule(); |
1277 schedule(); |
1278 #endif |
1278 #endif |
1279 } |
1279 } |
1280 } |
1280 } |
1294 ec_master_t *master = (ec_master_t *) priv_data; |
1294 ec_master_t *master = (ec_master_t *) priv_data; |
1295 ec_slave_t *slave = NULL; |
1295 ec_slave_t *slave = NULL; |
1296 int fsm_exec; |
1296 int fsm_exec; |
1297 |
1297 |
1298 if (master->debug_level) |
1298 if (master->debug_level) |
1299 EC_DBG("Operation thread running with fsm interval = %d us," |
1299 EC_DBG("Operation thread running with fsm interval = %d us," |
1300 " max data size=%d\n", |
1300 " max data size=%d\n", |
1301 master->send_interval, |
1301 master->send_interval, |
1302 master->max_queue_size); |
1302 master->max_queue_size); |
1303 |
1303 |
1304 while (!kthread_should_stop()) { |
1304 while (!kthread_should_stop()) { |
1324 if (fsm_exec) |
1324 if (fsm_exec) |
1325 master->injection_seq_fsm++; |
1325 master->injection_seq_fsm++; |
1326 } |
1326 } |
1327 |
1327 |
1328 #ifdef EC_USE_HRTIMER |
1328 #ifdef EC_USE_HRTIMER |
1329 // the op thread should not work faster than the sending RT thread |
1329 // the op thread should not work faster than the sending RT thread |
1330 ec_master_nanosleep(master->send_interval * 1000); |
1330 ec_master_nanosleep(master->send_interval * 1000); |
1331 #else |
1331 #else |
1332 if (ec_fsm_master_idle(&master->fsm)) { |
1332 if (ec_fsm_master_idle(&master->fsm)) { |
1333 set_current_state(TASK_INTERRUPTIBLE); |
1333 set_current_state(TASK_INTERRUPTIBLE); |
1334 schedule_timeout(1); |
1334 schedule_timeout(1); |
1335 } |
1335 } |
1336 else { |
1336 else { |
1337 schedule(); |
1337 schedule(); |
1338 } |
1338 } |
1339 #endif |
1339 #endif |
1340 } |
1340 } |
1341 |
1341 |
1342 if (master->debug_level) |
1342 if (master->debug_level) |
1343 EC_DBG("Master OP thread exiting...\n"); |
1343 EC_DBG("Master OP thread exiting...\n"); |
1344 return 0; |
1344 return 0; |
1345 } |
1345 } |