spyderta/linux/1394ethbridge.c

#include <stdio.h>
#include <stdarg.h>
#include <errno.h>
#include <stdint.h>
#include <stdbool.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <poll.h>
#include <libraw1394/raw1394.h>

enum {
	SERVER_FD = 0,
	IEEE_FD = 1,
	CLIENT_FD = 2,
	DESCRIPTORS = 3
};

static struct msg_state client_state;
static raw1394handle_t ieee_handle;
static struct pollfd pollfds[DESCRIPTORS] = {{0}};

static void verr(char *fmt, va_list va) {
	int n = 0;
	if (fmt)
		n = vfprintf(stderr, fmt, va);

	if (errno)
		fprintf(stderr, "%s%s\n", n ? ": " : "", strerror(errno));
	else
		fprintf(stderr, "\n");
	exit(1);
}

static void err(char *fmt, ...) {
	va_list va;
	va_start(va, fmt);
	verr(fmt, va);
	va_end(va);
}

static void server_accept(void) {
	int fd = accept(pollfds[SERVER_FD], NULL, NULL);
	if (pollfds[CLIENT_FD].fd >= 0) {
		close(fd);
		return;
	}

	if (fd < 0) {
		if (errno != ECONNABORTED)
			err("accept");
		else
			return;
	}

	pollfds[CLIENT_FD].fd = fd;
	client_state.state = PARSE_VERSION_MESSAGE;
	client_state.type = MESSAGE_INVALID;
	if (client_state.buf) {
		sdsclear(client_state.buf);
	} else {
		client_state.buf = sdsempty();
	}
}

static void iterate(void) {
	if (raw1394_loop_iterate(ieee_handle) < 0)
		err("raw1394_loop_iterate");
}

/* 0b111111 = 0x3F
 * if tags[x] < 0, then tag is not allocated
 */
#define TAG_NUM 0x40
static long tag_to_handle[TAG_NUM];

static void handle_send_async(union msg *msg) {
	switch (msg->send_async.tcode) {
	case TCODE_WRITE_QUADLET_REQUEST:
	case TCODE_WRITE_BLOCK_REQUEST:
		raw1394_start_write(ieee_handle, msg->send_async.node,
		                        msg->send_async.addr, msg->send_async.len,
		                        msg->send_async.buf, msg->send_async.tlabel);
		break;
	case TCODE_READ_QUADLET_REQUEST:
	case TCODE_READ_BLOCK_REQUEST:
		raw1394_start_read(ieee_handle, msg->send_async.node,
		                       msg->send_async.addr, msg->send_async.len,
		                       msg->send_async.buf, msg->send_async.tlabel);
		break;
	case TCODE_WRITE_RESONSE: case TCODE_READ_QUADLET_RESPONSE:
	case TCODE_READ_BLOCK_RESPONSE:
		if (msg->send_async.tlabel < TAG_NUM &&
		    tags_to_handle[msg->send_async.tlabel] >= 0) {
			raw1394_send_rw_response(ieee_handle, msg->send_async.tcode,
		                             msg->send_async.buf, msg->send_async.len,
		                             tags_to_handle[msg->send_async.tlabel]);
			tags_to_handle[msg->send_async.tlabel] = 0;
		}
		break;
	/* FIXME: implement lock request */
	}
}

static void handle_send_iso(union msg *msg) {
	/* do nothing */
}

static int arm_handler(raw1394handle_t handle, raw1394_arm_allocation_t *arm,
                        int tcode, unsigned long long offset, int source_node_id,
                        int card, unsigned kernel_handle, size_t length, void *data)
{
	(void)handle;
	uint8_t tag;

	/* Allocate tags.
	 * Tags are temporarily allocated since the real tag is stored in the
	 * kernel in the kernel_handle.
	 *
	 * firewire-cdev: bus is only 1023?
	 */
	for (tag = 0; tag < MAX_TAG; tag++) {
		if (tag_to_handle[tag] < 0)
			break;
	}
	if (tag == MAX_TAG)
		return -1;

	tag_to_handle[tag] = kernel_handle;
	if (msg_send_async(pollfds[SERVER_FD].fd, 1023, source_node_id, offset,
	               tcode, tag, length, data) < 0)
		tag_to_handle[tag] = -1;
	/* TODO: return an error response */
	return 0;
}

static void handle_allocation(union msg *msg) {
	/* may fail due to overlap: ignore */
	raw1394_arm_register(ieee_handle, msg->allocate.start, msg->allocate.length,
	                     ARM_READ | ARM_WRITE | ARM_LOCK,
	                     ARM_READ | ARM_WRITE | ARM_LOCK,
	                     0 /* ignored */);
}

static bool read_client(void) {
	char buf[512];
	ssize_t len;
	int msgtype;

	len = read(pollfds[CLIENT_FD].fd, buf, sizeof(buf));
	if (len <= 0)
		return false;

	client_state.buf = sdscatlen(client_state.buf, buf, len);
	while (!parse_packet(fdstate));

	return true;
}

static bool client_process(void) {
	if (fd->revents & POLLERR)
		return false;

	if (fd->revents & POLLIN)
		if (!read_client(fd, fdstate))
			return false;

	if (!(fd->revents & POLLIN) && fd->revents & POLLHUP)
		return false;
	return true;
}

static int open_server(const char *port) {
	int r;
	struct addrinfo *serv;
	int sock;

	if ((r = getaddrinfo(NULL, port, &(struct addrinfo){
			.ai_family = AF_INET,
			.ai_socktype = SOCK_STREAM,
			.ai_flags = AI_PASSIVE
		}, &serv) != 0) {
		err("getaddrinfo: %s", gai_strerror(r));
	}

	for (struct addrinfo *p = serv; p; p = p->ai_next) {
		sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
		if (sock < 0)
			continue;
		if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR) < 0)
			continue;
		if (bind(sock, p->ai_addr, p->ai_addrlen) < 0)
			continue;
		break;
	}
	if (!p)
		err("socket");

	freeaddrinfo(serv);

	if (listen(p, 5) < 0)
		err("listen");

	return sock;
}

static int setup_port(int port) {
	int num;
	int fd;
	static struct raw1394_portinfo ports[PORTSNUM] = {0};

	ieee_handle = raw1394_new_handle();
	if (!ieee_handle)
		err("raw1394_new_handle");

	num = raw1394_get_port_info(ieee_handle, ports, PORTSNUM);

	do {
		for (int i = 0; i < num && i < PORTSNUM; i++)
			fprintf(stderr, "[%d/%d]: %s w/ %d\n", i + 1, num,
			        ports[i].name, ports[i].nodes);
		errno = 0;
		raw1394_set_port(ieee_handle, port);
	} while (errno != ESTALE);

	if (errno)
		die("raw1394_set_port");

	fd = raw1394_get_fd(ieee_handle);
	if (fd < 0)
		die("raw1394_get_fd");

	return fd;
}

int main(void) {
	pollfds[IEEE_FD].fd = setup_port(0);
	pollfds[IEEE_FD].events = POLLIN | POLLPRI;
	pollfds[SERVER_FD].fd = open_server("1394");
	pollfds[SERVER_FD].events = POLLIN | POLLPRI;
	pollfds[CLIENT_FD].fd = -1;

	for (int i = 0; i < TAG_NUM; i++)
		tag_to_handle[i] = -1;

	client_state.on_read[PARSE_SEND_ASYNC] = ieee_process;

	while (1) {
		if (poll(pollfds, DESCRIPTORS, -1) <= 0)
			err("poll");

		if (pollfds[SERVER_FD].revents & (POLLIN | POLLPRI)) {
			server_accept();
		} else if (pollfds[IEEE_FD].revents & (POLLIN | POLLPRI)) {
			ieee_process();
		}
		if (!client_process()) {
			close(pollfds[CLIENT_FD].fd);
			pollfds[CLIENT_FD].fd = -1;

			// Reset Firewire
			raw1394_destroy_handle(ieee_handle);
			pollfds[IEEE_FD].fd = setup_port(0);
		}
	}
}