1254 */ |
1254 */ |
1255 static int ec_master_idle_thread(void *priv_data) |
1255 static int ec_master_idle_thread(void *priv_data) |
1256 { |
1256 { |
1257 ec_master_t *master = (ec_master_t *) priv_data; |
1257 ec_master_t *master = (ec_master_t *) priv_data; |
1258 ec_slave_t *slave = NULL; |
1258 ec_slave_t *slave = NULL; |
1259 int fsm_exec; |
|
1260 size_t sent_bytes; |
1259 size_t sent_bytes; |
1261 |
1260 |
1262 // send interval in IDLE phase |
1261 // send interval in IDLE phase |
1263 ec_master_set_send_interval(master, 1000000 / HZ); |
1262 ec_master_set_send_interval(master, 1000000 / HZ); |
1264 |
1263 |
1273 down(&master->io_sem); |
1272 down(&master->io_sem); |
1274 ecrt_master_receive(master); |
1273 ecrt_master_receive(master); |
1275 up(&master->io_sem); |
1274 up(&master->io_sem); |
1276 |
1275 |
1277 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1276 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1278 fsm_exec = 0; |
|
1279 // execute master & slave state machines |
1277 // execute master & slave state machines |
1280 if (down_interruptible(&master->master_sem)) |
1278 if (down_interruptible(&master->master_sem)) |
1281 break; |
1279 break; |
1282 fsm_exec = ec_fsm_master_exec(&master->fsm); |
1280 if (ec_fsm_master_exec(&master->fsm)) |
|
1281 master->injection_seq_fsm++; |
1283 for (slave = master->slaves; |
1282 for (slave = master->slaves; |
1284 slave < master->slaves + master->slave_count; |
1283 slave < master->slaves + master->slave_count; |
1285 slave++) { |
1284 slave++) { |
1286 ec_fsm_slave_exec(&slave->fsm); |
1285 if (ec_fsm_slave_exec(&slave->fsm)) |
|
1286 master->injection_seq_fsm++; |
1287 } |
1287 } |
1288 up(&master->master_sem); |
1288 up(&master->master_sem); |
1289 |
1289 } |
1290 // queue and send |
1290 // queue and send |
1291 down(&master->io_sem); |
1291 down(&master->io_sem); |
1292 if (fsm_exec) { |
|
1293 master->injection_seq_rt++; |
|
1294 } |
|
1295 } |
|
1296 ecrt_master_send(master); |
1292 ecrt_master_send(master); |
1297 sent_bytes = master->main_device.tx_skb[ |
1293 sent_bytes = master->main_device.tx_skb[ |
1298 master->main_device.tx_ring_index]->len; |
1294 master->main_device.tx_ring_index]->len; |
1299 up(&master->io_sem); |
1295 up(&master->io_sem); |
1300 |
1296 |
1325 */ |
1321 */ |
1326 static int ec_master_operation_thread(void *priv_data) |
1322 static int ec_master_operation_thread(void *priv_data) |
1327 { |
1323 { |
1328 ec_master_t *master = (ec_master_t *) priv_data; |
1324 ec_master_t *master = (ec_master_t *) priv_data; |
1329 ec_slave_t *slave = NULL; |
1325 ec_slave_t *slave = NULL; |
1330 int fsm_exec; |
|
1331 |
1326 |
1332 EC_MASTER_DBG(master, 1, "Operation thread running" |
1327 EC_MASTER_DBG(master, 1, "Operation thread running" |
1333 " with fsm interval = %u us, max data size=%zu\n", |
1328 " with fsm interval = %u us, max data size=%zu\n", |
1334 master->send_interval, master->max_queue_size); |
1329 master->send_interval, master->max_queue_size); |
1335 |
1330 |
1338 |
1333 |
1339 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1334 if (master->injection_seq_rt == master->injection_seq_fsm) { |
1340 // output statistics |
1335 // output statistics |
1341 ec_master_output_stats(master); |
1336 ec_master_output_stats(master); |
1342 |
1337 |
1343 fsm_exec = 0; |
|
1344 // execute master & slave state machines |
1338 // execute master & slave state machines |
1345 if (down_interruptible(&master->master_sem)) |
1339 if (down_interruptible(&master->master_sem)) |
1346 break; |
1340 break; |
1347 fsm_exec = ec_fsm_master_exec(&master->fsm); |
1341 if (ec_fsm_master_exec(&master->fsm)) |
|
1342 master->injection_seq_fsm++; |
1348 for (slave = master->slaves; |
1343 for (slave = master->slaves; |
1349 slave < master->slaves + master->slave_count; |
1344 slave < master->slaves + master->slave_count; |
1350 slave++) { |
1345 slave++) { |
1351 ec_fsm_slave_exec(&slave->fsm); |
1346 if (ec_fsm_slave_exec(&slave->fsm)) |
|
1347 master->injection_seq_fsm++; |
1352 } |
1348 } |
1353 up(&master->master_sem); |
1349 up(&master->master_sem); |
1354 |
|
1355 // inject datagrams (let the rt thread queue them, see ecrt_master_send) |
|
1356 if (fsm_exec) |
|
1357 master->injection_seq_fsm++; |
|
1358 } |
1350 } |
1359 |
1351 |
1360 #ifdef EC_USE_HRTIMER |
1352 #ifdef EC_USE_HRTIMER |
1361 // the op thread should not work faster than the sending RT thread |
1353 // the op thread should not work faster than the sending RT thread |
1362 ec_master_nanosleep(master->send_interval * 1000); |
1354 ec_master_nanosleep(master->send_interval * 1000); |