/*
 *  bthidproxy - proxy two Bluetooth HID connections
 *
 *  version 1.0
 *
 *  Copyright (C) 2009-2010  Michael Ossmann <mike@ossmann.com>
 *  Copyright (C) 2003-2009  Marcel Holtmann <marcel@holtmann.org>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <stdio.h>
#include <stdlib.h>
#include <getopt.h>
#include <sys/poll.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/l2cap.h>
#include <bluetooth/hidp.h>

#define L2CAP_PSM_HIDP_CTRL 0x11
#define L2CAP_PSM_HIDP_INTR 0x13
#define BUFLEN 65536
#define NUMFDS 4

static bdaddr_t bdaddr_a; /* remote HID host */
static bdaddr_t bdaddr_b; /* local interface that connects (as device) to host */
static bdaddr_t bdaddr_c; /* local interface that connects (as host) to device */
static bdaddr_t bdaddr_d; /* remote HID device */

static const char* FD_NAMES[] = {
	"control out",
	"interrupt out",
	"control in",
	"interrupt in"
};

int listen_for_host = 0;

/* copied from BlueZ's hidd.c */
static int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm)
{
	struct sockaddr_l2 addr;
	struct l2cap_options opts;
	int sk;

	if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
		return -1;

	memset(&addr, 0, sizeof(addr));
	addr.l2_family  = AF_BLUETOOTH;
	bacpy(&addr.l2_bdaddr, src);

	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		close(sk);
		return -1;
	}

	memset(&opts, 0, sizeof(opts));
	opts.imtu = HIDP_DEFAULT_MTU;
	opts.omtu = HIDP_DEFAULT_MTU;
	opts.flush_to = 0xffff;

	setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts));

	memset(&addr, 0, sizeof(addr));
	addr.l2_family  = AF_BLUETOOTH;
	bacpy(&addr.l2_bdaddr, dst);
	addr.l2_psm = htobs(psm);

	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		close(sk);
		return -1;
	}

	return sk;
}

/* copied from BlueZ's hidd.c */
static int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm, int lm, int backlog)
{
	struct sockaddr_l2 addr;
	struct l2cap_options opts;
	int sk;

	if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
		return -1;

	memset(&addr, 0, sizeof(addr));
	addr.l2_family = AF_BLUETOOTH;
	bacpy(&addr.l2_bdaddr, bdaddr);
	addr.l2_psm = htobs(psm);

	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		close(sk);
		return -1;
	}

	setsockopt(sk, SOL_L2CAP, L2CAP_LM, &lm, sizeof(lm));

	memset(&opts, 0, sizeof(opts));
	opts.imtu = HIDP_DEFAULT_MTU;
	opts.omtu = HIDP_DEFAULT_MTU;
	opts.flush_to = 0xffff;

	setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts));

	if (listen(sk, backlog) < 0) {
		close(sk);
		return -1;
	}

	return sk;
}

/* copied from BlueZ's hidd.c */
static int l2cap_accept(int sk, bdaddr_t *bdaddr)
{
	struct sockaddr_l2 addr;
	socklen_t addrlen;
	int nsk;

	memset(&addr, 0, sizeof(addr));
	addrlen = sizeof(addr);

	if ((nsk = accept(sk, (struct sockaddr *) &addr, &addrlen)) < 0)
		return -1;

	if (bdaddr)
		bacpy(bdaddr, &addr.l2_bdaddr);

	return nsk;
}

static void proxy()
{
	struct sockaddr_l2 addr;
	socklen_t optlen;
	unsigned char *buf;
	char local[18];
	char remote[18];
	int i, j;
	int acs = -1; /* control channel socket to a (host) */
	int ais = -1; /* interrupt channel socket to a (host) */
	int dcs = -1; /* control channel socket to d (device) */
	int dis = -1; /* interrupt channel socket to d (device) */
	int acs_listener = -1;
	int ais_listener = -1;
	struct pollfd pf[NUMFDS];

	buf = malloc(BUFLEN);
	if (!buf) {
		perror("can't allocate buffer");
		exit(1);
	}

	printf("connecting HID control channel to device\n");
	while (dcs < 0) {
		dcs = l2cap_connect(&bdaddr_c, &bdaddr_d, L2CAP_PSM_HIDP_CTRL);
		if (dcs < 0) {
			perror("connect failed");
			sleep(1);
		}
	}

	printf("connecting HID interrupt channel to device\n");
	while (dis < 0) {
		dis = l2cap_connect(&bdaddr_c, &bdaddr_d, L2CAP_PSM_HIDP_INTR);
		if (dis < 0) {
			perror("connect failed");
			sleep(1);
		}
	}

	ba2str(&bdaddr_d, remote);
	ba2str(&bdaddr_c, local);
	printf("connected to device %s from %s\n", remote, local);

	if (listen_for_host) {

		printf("starting control listener\n");
		while (acs_listener < 0) {
			acs_listener = l2cap_listen(&bdaddr_b, L2CAP_PSM_HIDP_CTRL, 0, 10);
			if (acs_listener < 0) {
				perror("listen failed");
				sleep(1);
			}
		}
		printf("starting interrupt listener\n");
		while (ais_listener < 0) {
			ais_listener = l2cap_listen(&bdaddr_b, L2CAP_PSM_HIDP_INTR, 0, 10);
			if (ais_listener < 0) {
				perror("listen failed");
				sleep(1);
			}
		}

		printf("waiting for HID control connection from host\n");
		while (acs < 0) {
			acs = l2cap_accept(acs_listener, NULL);
			if (acs < 0) {
				perror("accept failed");
				sleep(1);
			}
			printf("accepted\n");
		}

		printf("waiting for HID interrupt connection from host\n");
		while (ais < 0) {
			ais = l2cap_accept(ais_listener, NULL);
			if (ais < 0) {
				perror("accept failed");
				sleep(1);
			}
			printf("accepted\n");
		}

	} else {

		printf("connecting HID control channel to host\n");
		while (acs < 0) {
			acs = l2cap_connect(&bdaddr_b, &bdaddr_a, L2CAP_PSM_HIDP_CTRL);
			if (acs < 0) {
				perror("connect failed");
				sleep(1);
			}
		}

		printf("connecting HID interrupt channel to host\n");
		while (ais < 0) {
			ais = l2cap_connect(&bdaddr_b, &bdaddr_a, L2CAP_PSM_HIDP_INTR);
			if (ais < 0) {
				perror("connect failed");
				sleep(1);
			}
		}
	}

	ba2str(&bdaddr_a, remote);
	ba2str(&bdaddr_b, local);
	printf("connected to host %s from %s\n", remote, local);

	pf[0].fd = acs;
	pf[0].events = POLLIN | POLLERR | POLLHUP | POLLNVAL | POLLPRI;
	pf[1].fd = ais;
	pf[1].events = POLLIN | POLLERR | POLLHUP | POLLNVAL | POLLPRI;
	pf[2].fd = dcs;
	pf[2].events = POLLIN | POLLERR | POLLHUP | POLLNVAL | POLLPRI;
	pf[3].fd = dis;
	pf[3].events = POLLIN | POLLERR | POLLHUP | POLLNVAL | POLLPRI;

	while (1) {
		ssize_t len;
		int numrdy;

		numrdy = poll(pf, NUMFDS, -1);
		if (numrdy < 0) {
			perror("poll failed");
			goto error;
		}

		for (i = 0; i < NUMFDS; i++) {
			if (pf[i].revents & (POLLIN | POLLPRI)) {

				/* receive data */
				len = recv(pf[i].fd, buf, BUFLEN, 0);
				if (len < 0) {
					perror("recv failed");
					goto error;
				} else if (len == 0) {
					printf("disconnected\n");
					goto error;
				}

				/* print to screen */
				printf("%-13s:", FD_NAMES[i]);
				for (j = 0; j < len; j++)
					printf(" %02x", buf[j]);
				printf("\n");

				/* forward it */
				if (send(pf[(i+2)%4].fd, buf, len, 0) <= 0) {
					perror("send failed");
					goto error;
				}
			}
			if (pf[i].revents & (POLLERR | POLLHUP | POLLNVAL)) {
				printf("lost %s\n", FD_NAMES[i]);
				goto error;
			}
		}
	}

error:
	if (ais >= 0) close(ais);
	if (acs >= 0) close(acs);
	if (dis >= 0) close(dis);
	if (dcs >= 0) close(dcs);
	free(buf);
	exit(1);
}

static void usage(void)
{
	printf("bthidproxy - proxy two Bluetooth HID connections\n");
	printf("Usage:\n");
	printf("\tbthidproxy [-l] -a bdaddr_a -b bdaddr_b -c bdaddr_c -d bdaddr_d\n");
	printf("\n\tbdaddr_a is the address of the remote host\n");
	printf("\tbdaddr_b is the local interface that connects (as a device) to host\n");
	printf("\tbdaddr_c is the local interface that connects (as a host) to device\n");
	printf("\tbdaddr_d is the address of the remote device\n");
	printf("\t-l listen mode: wait for host to connect\n");
	printf("\n\tbdaddr_b and bdaddr_c may be specified by interface (hciX) or bdaddr\n");
}

int main(int argc, char *argv[])
{
	int opt;
	int count = 0;

	while ((opt=getopt(argc,argv,"la:b:c:d:")) != EOF) {
		switch(opt) {
		case 'a':
			count++;
			str2ba(optarg, &bdaddr_a);
			break;
		case 'b':
			count++;
			if (!strncasecmp(optarg, "hci", 3))
				hci_devba(atoi(optarg + 3), &bdaddr_b);
			else
				str2ba(optarg, &bdaddr_b);
			break;
		case 'c':
			count++;
			if (!strncasecmp(optarg, "hci", 3))
				hci_devba(atoi(optarg + 3), &bdaddr_c);
			else
				str2ba(optarg, &bdaddr_c);
			break;
		case 'd':
			count++;
			str2ba(optarg, &bdaddr_d);
			break;
		case 'l':
			listen_for_host = 1;
			break;
		default:
			usage();
			exit(1);
		}
	}

	if (count < 4) {
		usage();
		exit(1);
	}

	proxy();

	return 0;
}