Quick and dirty TTY ping

ttyping.cc

/* Simple ping-pong test for TTY
 *
 * Depends on: Boost.ASIO and docopt.cpp
 * https://github.com/docopt/docopt.cpp.git
 *
 * Complie:

      g++ -std=c++14 -pthread ttyping.cc -o ttyping -lboost_system -static -I/usr/local/include -ldocopt

 * Testing socat:

      socat -v PTY,link=/tmp/ttyA PTY,link=/tmp/ttyB

 */

#include <iostream>
#include <thread>
#include <chrono>
#include <unistd.h>
#include <cstring>
#include <deque>
#include <boost/asio.hpp>
#include <docopt/docopt.h>
#include <csignal>

using spb = boost::asio::serial_port_base;
using boost::asio::buffer;
using boost::system::error_code;
using std::chrono::nanoseconds;
using std::chrono::steady_clock;
using std::chrono::time_point_cast;

constexpr size_t PACKET_SIZE = 4 + 2 * 8;
constexpr size_t BUF_SIZE = 256;
uint8_t rx_buf[BUF_SIZE];
uint8_t tx_buf[BUF_SIZE];
boost::asio::io_service io_service;
boost::asio::serial_port serial_dev(io_service);
std::deque<uint8_t> rx_accu;

// calculate avearge
uint64_t dt_sum = 0;
size_t dt_cnt = 0;
uint64_t dt_min = std::numeric_limits<uint64_t>::max();
uint64_t dt_max = 0;

static inline uint64_t now_ns()
{
	return time_point_cast<nanoseconds>(steady_clock::now()).time_since_epoch().count();
}

// -*- PING mode -*-

void parse_rx(uint8_t *buf, size_t len, const char *stx_exp, void (*f)(uint64_t ,uint64_t, uint64_t))
{
	if (buf != nullptr)
		rx_accu.insert(rx_accu.end(), buf, buf + len);

	for (; rx_accu.size() >= 4; ) {
		auto it = rx_accu.cbegin();

		std::string stx(it, it + 4);
		if (stx == stx_exp) {
			break;
		}

		rx_accu.erase(it, it + 4);

		//std::cout << "RX: STX: " << stx << std::endl;
	}

	if (rx_accu.size() >= PACKET_SIZE) {
		uint64_t rx_ts = now_ns();
		uint64_t ping_ts, pong_ts;

		auto it = rx_accu.cbegin();

		std::string stx(it, it + 4); it += 4;
		std::copy(it, it + 8, (uint8_t*) &ping_ts); it += 8;
		std::copy(it, it + 8, (uint8_t*) &pong_ts); it += 8;

		rx_accu.erase(rx_accu.cbegin(), it);

		std::cout << "RX: " << stx << " " << ping_ts << " " << pong_ts << std::endl;

		f(rx_ts, ping_ts, pong_ts);
	}

	if (rx_accu.size() >= PACKET_SIZE)
		parse_rx(nullptr, 0, stx_exp, f);
}

void ping_do_read()
{
	serial_dev.async_read_some(
			buffer(rx_buf, sizeof(rx_buf)),
			[] (error_code error, size_t bytes_tr) {
				if (error) {
					std::cout << "rx error: " << error << std::endl;
					return;
				}

				parse_rx(rx_buf, bytes_tr, "PONG",
						[](auto now, auto ping, auto pong) {
							auto dT = now - ping;

							dt_sum += dT;
							dt_cnt++;
							dt_max = std::max(dt_max, dT);
							dt_min = std::min(dt_min, dT);

							std::cout << "PING: dT: " << dT << " ns, Tx dT: " << (pong - ping) << " ns seq: " << dt_cnt << std::endl;
						});
				ping_do_read();
			});
}

void ping_do_write()
{
	uint64_t ping_ts = now_ns();
	auto *p = tx_buf;

	memcpy(p, "PING", 4);	p += 4;
	memcpy(p, &ping_ts, 8);	p += 8;
	memset(p, 0, 8);	p += 8;

	// does several async_write_some if needed
	boost::asio::async_write(
			serial_dev,
			buffer(tx_buf, p - tx_buf),
			[](error_code error, size_t bytes_tr) {
				if (error) {
					std::cout << "tx error: " << error << std::endl;
					return;
				}
			});
}

// -*- PONG mode -*-

void pong_do_write(uint64_t ping_ts, uint64_t pong_ts)
{
	auto *p = tx_buf;
	memcpy(p, "PONG", 4);	p += 4;
	memcpy(p, &ping_ts, 8);	p += 8;
	memcpy(p, &pong_ts, 8);	p += 8;

	boost::asio::async_write(
			serial_dev,
			buffer(tx_buf, p - tx_buf),
			[](error_code error, size_t bytes_tr) {
				if (error) {
					std::cout << "tx error: " << error << std::endl;
					return;
				}
			});
}

void pong_do_read()
{
	serial_dev.async_read_some(
			buffer(rx_buf, sizeof(rx_buf)),
			[] (error_code error, size_t bytes_tr) {
				if (error) {
					std::cout << "rx error: " << error << std::endl;
					return;
				}

				parse_rx(rx_buf, bytes_tr, "PING",
						[](auto now, auto ping, auto pong) {
							pong_do_write(ping, now);

							std::cout << "PING: dT: " << (now - ping) << " ns" << std::endl;
						});

				pong_do_read();
			});
}

// -*- main -*-

static const char USAGE[] =
R"(TTY Ping-Pong

Usage:
  ttyping [options] <tty>
  ttyping -h

Options:
  -h --help           Show this help
  -b --baud=<br>      Baud rate [default: 115200]
  -i --interval=<ms>  Ping interval [default: 1000]
  --pong              PONG mode
)";

void sigint_handler(int signal)
{
	double avg = (dt_sum > 0) ? double(dt_sum) / dt_cnt : 0.0;

	std::cout << "--- ping statistics ---" << std::endl;
	std::cout << dt_cnt << " packets transmitted" << std::endl;
	std::cout << "min/avg/max: " << dt_min << "/" << avg << "/" << dt_max << " ns" << std::endl;

	std::exit(0);
}

int main(int argc, char **argv)
{
	auto args = docopt::docopt(USAGE, { argv + 1, argv + argc });

	for (auto const &arg : args) {
		std::cout << arg.first << ": " << arg.second << std::endl;
	}

	auto ttyport = args["<tty>"].asString();
	auto baudrate = args["--baud"].asLong();
	auto pong_mode = args["--pong"].asBool();
	auto interval = std::chrono::milliseconds(args["--interval"].asLong());

	std::cout << "Opening: " << ttyport << " Mode: " << ((pong_mode) ? "PONG" : "PING") << std::endl;

	serial_dev.open(ttyport);
	// set 115200 8N1
	serial_dev.set_option(spb::baud_rate(baudrate));
	serial_dev.set_option(spb::character_size(8));
	serial_dev.set_option(spb::parity(spb::parity::none));
	serial_dev.set_option(spb::stop_bits(spb::stop_bits::one));
	serial_dev.set_option(spb::flow_control(spb::flow_control::none));

	if (pong_mode)
		io_service.post(pong_do_read);
	else {
		io_service.post(ping_do_read);
		std::signal(SIGINT, sigint_handler);
	}

	std::thread t(
		[&]() {
			std::cout << "io_service running" << std::endl;
			io_service.run();
			std::cout << "io_service stopped" << std::endl;
		});

	if (pong_mode) {
		t.join();
	}
	else {
		for (; t.joinable() ;) {
			std::this_thread::sleep_for(interval);
			io_service.post(ping_do_write);
		}
	}

	return 0;
}