s98 player for SPFM Light

s98_to_spfm.c

#define _XOPEN_SOURCE 600
#include <errno.h>
#include <fcntl.h>
#include <signal.h>
#include <stdarg.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/select.h>
#include <sys/stat.h>
#include <termios.h>
#include <unistd.h>

enum misc_t {
	VERBOSE        = true,
	BUFSIZE        = 16,
	SELECT_TIMEOUT = 15000, /* usec */
	SLEEP_TIME     = 30000, /* sleep time at EAGAIN, EWOULDBLOCK (usec) */
};

enum fd_state_t {
	FD_IS_BUSY     = -1,
	FD_IS_READABLE = 0,
	FD_IS_WRITABLE = 1,
};

enum check_type_t {
	CHECK_READ_FD  = 0,
	CHECK_WRITE_FD = 1,
};

static const char *serial_dev      = "/dev/ttyUSB0";
volatile sig_atomic_t catch_sigint = false;

/* error functions */
enum loglevel_t {
	DEBUG = 0,
	WARN,
	ERROR,
	FATAL,
};

void logging(enum loglevel_t loglevel, char *format, ...)
{
	va_list arg;
	static const char *loglevel2str[] = {
		[DEBUG] = "DEBUG",
		[WARN]  = "WARN",
		[ERROR] = "ERROR",
		[FATAL] = "FATAL",
	};

	/* debug message is available on verbose mode */
	if ((loglevel == DEBUG) && (VERBOSE == false))
		return;

	fprintf(stderr, ">>%s<<\t", loglevel2str[loglevel]);

	va_start(arg, format);
	vfprintf(stderr, format, arg);
	va_end(arg);
}

/* wrapper of C functions */
int eopen(const char *path, int flag)
{
	int fd;
	errno = 0;

	if ((fd = open(path, flag)) < 0) {
		logging(ERROR, "couldn't open \"%s\"\n", path);
		logging(ERROR, "open: %s\n", strerror(errno));
	}
	return fd;
}

int eclose(int fd)
{
	int ret;
	errno = 0;

	if ((ret = close(fd)) < 0)
		logging(ERROR, "close: %s\n", strerror(errno));

	return ret;
}

FILE *efopen(const char *path, char *mode)
{
	FILE *fp;
	errno = 0;

	if ((fp = fopen(path, mode)) == NULL) {
		logging(ERROR, "couldn't open \"%s\"\n", path);
		logging(ERROR, "fopen: %s\n", strerror(errno));
	}
	return fp;
}

int efclose(FILE *fp)
{
	int ret;
	errno = 0;

	if ((ret = fclose(fp)) < 0)
		logging(ERROR, "fclose: %s\n", strerror(errno));

	return ret;
}

int eselect(int maxfd, fd_set *readfds, fd_set *writefds, fd_set *errorfds, struct timeval *tv)
{
	int ret;
	errno = 0;

	if ((ret = select(maxfd, readfds, writefds, errorfds, tv)) < 0) {
		if (errno == EINTR)
			return eselect(maxfd, readfds, writefds, errorfds, tv);
		else
			logging(ERROR, "select: %s\n", strerror(errno));
	}
	return ret;
}

ssize_t eread(int fd, void *buf, size_t size)
{
	ssize_t ret;
	errno = 0;

	if ((ret = read(fd, buf, size)) < 0) {
		if (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK) {
			logging(ERROR, "read(): interrupt! (EINTR or EAGAIN or EWOULDBLOCK occured), sleep %d usec\n", SLEEP_TIME);
			usleep(SLEEP_TIME);
			return eread(fd, buf, size);
		} else {
			logging(ERROR, "read(): %s\n", strerror(errno));
			return ret;
		}
	}
	/*
	else if (ret < (ssize_t) size) {
		logging(WARN, "request size:%zu read size:%zd, try to read again\n", size, ret);
		return eread(fd, (char *) buf + ret, size - ret);
	}
	*/
	return ret;
}

ssize_t ewrite(int fd, const void *buf, size_t size)
{
	ssize_t ret;
	errno = 0;

	if ((ret = write(fd, buf, size)) < 0) {
		if (errno == EINTR) {
			logging(ERROR, "write: EINTR occurred\n");
			return ewrite(fd, buf, size);
		} else if (errno == EAGAIN || errno == EWOULDBLOCK) {
			logging(ERROR, "write: EAGAIN or EWOULDBLOCK occurred, sleep %d usec\n", SLEEP_TIME);
			usleep(SLEEP_TIME);
			return ewrite(fd, buf, size);
		} else {
			logging(ERROR, "write: %s\n", strerror(errno));
			return ret;
		}
	} else if (ret < (ssize_t) size) {
		logging(ERROR, "data size:%zu write size:%zd\n", size, ret);
		return ewrite(fd, (char *) buf + ret, size - ret);
	}
	return ret;
}

int etcgetattr(int fd, struct termios *tm)
{
	int ret;
	errno = 0;

	if ((ret = tcgetattr(fd, tm)) < 0)
		logging(ERROR, "tcgetattr: %s\n", strerror(errno));

	return ret;
}

int etcsetattr(int fd, int action, const struct termios *tm)
{
	int ret;
	errno = 0;

	if ((ret = tcsetattr(fd, action, tm)) < 0)
		logging(ERROR, "tcgetattr: %s\n", strerror(errno));

	return ret;
}

int ecfsetispeed(struct termios *tm, speed_t speed)
{
	int ret;
	errno = 0;

	if ((ret = cfsetispeed(tm, speed)) < 0)
		logging(ERROR, "cfsetispeed: %s\n", strerror(errno));

	return ret;
}

int esigaction(int signo, struct sigaction *act, struct sigaction *oact)
{
    int ret;
    errno = 0;

    if ((ret = sigaction(signo, act, oact)) < 0)
        logging(ERROR, "sigaction: %s\n", strerror(errno));

    return ret;
}

/* spfm functions */
int serial_init(struct termios *old_termio)
{
	int fd = -1;
	bool get_old_termio = false;
	struct termios cur_termio;

	if ((fd = eopen(serial_dev, O_RDWR | O_NOCTTY | O_NDELAY)) < 0
		|| etcgetattr(fd, old_termio) < 0)
		goto err;

	get_old_termio = true;
	cur_termio = *old_termio;

	/*
	 * SPFM light serial:
	 *
	 * baud        : 1500000
	 * data size   : 8bit
	 * parity      : none
	 * flow control: disable
	 */

	//cur_termio.c_iflag = 0;
	//cur_termio.c_oflag = 0;

	cur_termio.c_cflag &= ~CSIZE;
	cur_termio.c_cflag |= (CS8 | CREAD | CLOCAL);
	cur_termio.c_lflag &= ~(ECHO | ISIG | ICANON);

	cur_termio.c_cc[VMIN]  = 1;
	cur_termio.c_cc[VTIME] = 0;

	if (ecfsetispeed(&cur_termio, B1500000) < 0
		|| etcsetattr(fd, TCSAFLUSH, &cur_termio) < 0)
		goto err;

	return fd;

err:
	if (get_old_termio)
		etcsetattr(fd, TCSAFLUSH, old_termio);

	if (fd != -1)
		eclose(fd);

	return -1;
}

void serial_die(int fd, struct termios *old_termio)
{
	etcsetattr(fd, TCSAFLUSH, old_termio);
	eclose(fd);
}

/*
 * SPFM light protocol:
 * <client>                <light>
 *
 * check interface:
 *         --> 0xFF    -->
 *         <-- 'L' 'T' <--
 * reset:
 *         --> 0xFE    -->
 *         <-- 'O' 'K' <--
 * nop:
 *         --> 0x80    -->
 *         <-- (none)  <--
 *
 * send register data:
 *
 * 1st byte: module number (0x00 or 0x01)
 * 2nd byte: command byte  (0x0n, n: set A0-A3 bit)
 * 3rd byte: register address
 * 4th byte: register data
 *
 * command byte:
 *
 * bit 0: A0 bit
 * bit 1: A1 bit
 * bit 2: A2 bit
 * bit 3: A3 bit
 * bit 4: CS1 bit
 * bit 5: CS2 bit
 * bit 6: CS3 bit
 * bit 7: (on: check/reset/nop, off: other commands)
 *
 * send data:
 *
 * 1st byte: module number (0x00 or 0x01)
 * 2nd byte: command byte  (0x8n, n: set A0-A3 bit)
 * 3rd byte: data
 *
 * SN76489 send data:
 *
 * 1st byte: module number (0x00 or 0x01)
 * 2nd byte: command byte  (0x20)
 * 3rd byte: data
 * 4th byte: 0x00          (dummy)
 * 5th byte: 0x00          (dummy)
 * 6th byte: 0x00          (dummy)
 *
 * read data (not implemented):
 *
 * 1st byte: module number (0x00 or 0x01)
 * 2nd byte: command byte  (0x4n, n: set A0-A3 bit?)
 * 3rd byte: register address
 *
 */

enum fd_state_t check_fds(int fd, enum check_type_t type)
{
	struct timeval tv;
	fd_set rfds, wfds;

	FD_ZERO(&rfds);
	FD_ZERO(&wfds);

	FD_SET(fd, &rfds);
	FD_SET(fd, &wfds);

	tv.tv_sec  = 0;
	tv.tv_usec = SELECT_TIMEOUT;

	eselect(fd + 1, &rfds, &wfds, NULL, &tv);

	if (type == CHECK_READ_FD && FD_ISSET(fd, &rfds))
		return FD_IS_READABLE;
	else if (type == CHECK_WRITE_FD && FD_ISSET(fd, &wfds))
		return FD_IS_WRITABLE;
	else
		return FD_IS_BUSY;
}

void send_data(int fd, uint8_t *buf, int size)
{
	ssize_t wsize;
	
	while (check_fds(fd, CHECK_WRITE_FD) != FD_IS_WRITABLE);

	wsize = ewrite(fd, buf, size);
	//logging(DEBUG, "%ld byte(s) wrote\n", wsize);
}

void recv_data(int fd, uint8_t *buf, int size)
{
	ssize_t rsize;

	while (check_fds(fd, CHECK_READ_FD) != FD_IS_READABLE);

	rsize = eread(fd, buf, size);
	if (0 < rsize&& rsize < size) {
		buf[rsize] = '\0';
		//logging(DEBUG, "rsize:%ld buf:%s\n", rsize, buf);
	}
}

bool spfm_reset(int fd)
{
	uint8_t buf[BUFSIZE];

	send_data(fd, &(uint8_t){0xFF}, 1);
	recv_data(fd, buf, BUFSIZE);

	if (strncmp((char *) buf, "LT", 2) != 0)
		return false;

	send_data(fd, &(uint8_t){0xFE}, 1);
	recv_data(fd, buf, BUFSIZE);

	if (strncmp((char *) buf, "OK", 2) != 0)
		return false;

	return true;
}

void spfm_send(int fd, int device, uint8_t *buf, int size)
{
	if (size == 2) {
		send_data(fd, &(uint8_t){0x00}, 1);
		if (device == 0x00)
			send_data(fd, &(uint8_t){0x00}, 1);
		else
			send_data(fd, &(uint8_t){0x02}, 1);

		send_data(fd, buf, size);

		logging(DEBUG, "divice:%d buf:0x%.2X 0x%.2X size:%d\n",
			device, buf[0], buf[1], size);
	}
}

void sync_wait(int n, double step)
{
	logging(DEBUG, "nsync:%d step:%lf\n", n, step);
	usleep(step * n * 1000000);
}

double s98_header(FILE *fp)
{
	unsigned int num, denom;

	fseek(fp, 4, SEEK_SET);
	if (fread(&num, 1, 4, fp) != 4
		|| fread(&denom, 1, 4, fp) != 4)
		return -1.0;

	logging(DEBUG, "num:%u denom:%u\n", num, denom);

	fseek(fp, 80, SEEK_SET);

	if (num == 0 || denom == 0)
		return (double) 10 / 10000;
	else
		return (double) num / denom;
}

bool s98_parse(int fd, FILE *fp)
{
	uint8_t buf[BUFSIZE], op;
	int nbyte;
	long nsync = 0L;
	double step;
	extern volatile sig_atomic_t catch_sigint;

	if ((step = s98_header(fp)) == -1.0)
		return false;

	while (catch_sigint == false) {
		if (fread(buf, 1, 1, fp) != 1)
			return feof(fp) ? true: false;

		op = buf[0];

		switch (op) {
		case 0x00: /* device 1 (normal) */
		case 0x01: /* device 1 (extended) */
			if (fread(buf, 1, 2, fp) == 2)
				spfm_send(fd, op, buf, 2);
			break;
		case 0xFD: /* END/LOOP */
			logging(DEBUG, "end of s98 data\n");
			return true;
		case 0xFE: /* n sync */
			nbyte = 0;
			do {
				if (fread(buf, 1, 1, fp) != 1)
					return false;

				nsync |= (buf[0] & 0x7F) << (7 * nbyte);
				nbyte++;
			} while (buf[0] & 0x80);
			sync_wait(nsync, step);
			break;
		case 0xFF: /* 1 sync */
			sync_wait(1, step);
			break;
		}
	}
	return true;
}

void sig_handler(int signo)
{
	extern volatile sig_atomic_t catch_sigint;

	if (signo == SIGINT)
		catch_sigint = true;
}

void set_signal(int signo, void (*sig_handler)(int signo))
{
	struct sigaction sigact;

	memset(&sigact, 0, sizeof(struct sigaction));
	sigact.sa_handler = sig_handler;
	sigact.sa_flags   = SA_RESTART;
	esigaction(signo, &sigact, NULL);
}

int main(int argc, char *argv[])
{
	int fd;
	FILE *fp;
	struct termios old_termio;

	if ((fd = serial_init(&old_termio)) < 0) {
		logging(FATAL, "serial_init() failed\n");
		return EXIT_FAILURE;
	}

	if (spfm_reset(fd) == false) {
		logging(FATAL, "spfm_reset() failed\n");
		goto err;
	}

	set_signal(SIGINT, sig_handler);

	fp = (argc > 1) ? efopen(argv[1], "r"): stdin;

	if (s98_parse(fd, fp) == false)
		logging(WARN, "s98_parse() failed\n");

	fclose(fp);

	spfm_reset(fd);

	serial_die(fd, &old_termio);

	return EXIT_SUCCESS;

err:
	spfm_reset(fd);
	serial_die(fd, &old_termio);
	return EXIT_FAILURE;
}