/*
 *  aaplfwpush - pushes firmware to keyboard
 *
 *  version 1.0
 *
 *  THIS WILL PROBABLY DESTROY YOUR KEYBOARD
 *
 *  tested at least once with A1255
 *
 *  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>

/*
 * you provide the firmware
 * 
 * should define DATA and DATALEN
 *
 */
//#include "320.h" 

#define L2CAP_PSM_FIRMWARE 62221
#define BUFLEN 65536

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 int dsk = -1; // socket to d (device)
static int timeout = 20000;
unsigned char *buf;

void error() {
	if (dsk >= 0) {
		printf("aborting\n");
		printf("FW out : d3 00\n");
		buf[0] = 0xd3;
		buf[1] = 0x00;
		if (send(dsk, buf, 2, 0) <= 0)
			perror("send failed");
		close(dsk);
	}
	free(buf);
	exit(1);
}

/* 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 = 185;
	//opts.omtu = 32767;
	opts.omtu = 185;
	opts.imtu = 32767;
	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;
}

void tx(int fd, ssize_t len) {
	int i;

	printf("FW out :");
	for (i = 0; i < len; i++)
		printf(" %02x", buf[i]);
	printf("\n");
	if (send(fd, buf, len, 0) <= 0) {
		perror("send failed");
		error();
	}
}

unsigned char rx(struct pollfd* pf) {
	ssize_t len;
	int numrdy;
	int i;

	numrdy = poll(pf, 1, timeout);
	if (numrdy < 0) {
		perror("poll failed");
		error();
	} else if (numrdy == 0) {
		return 0xff;
	}

	if (pf[0].revents & (POLLIN | POLLPRI)) {

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

		/* print to screen */
		printf("FW in  :");
		for (i = 0; i < len; i++)
			printf(" %02x", buf[i]);
		printf("\n");
	}
	if (pf[0].revents & (POLLERR | POLLHUP | POLLNVAL)) {
		printf("event: %x\n", pf[0].revents);
		error();
	}

	return buf[0];
}

static void push()
{
	char local[18];
	char remote[18];
	struct pollfd pf[1];
	unsigned char response;
	int i, j;
	int len;

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

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

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

	pf[0].fd = dsk;
	pf[0].events = POLLIN | POLLERR | POLLHUP | POLLNVAL | POLLPRI;

	while (1) {
		buf[0] = 0xd1;
		buf[1] = 0x01;
		buf[2] = 0x00; //version 320 0x00, 321 0x01
		tx(dsk, 3);
		response = rx(pf);

		if (response == 0xd2)
			break;
		if (response != 0xdc)
			error();
	}

	buf[0] = 0xd4;
	buf[1] = 0x00;
	tx(dsk, 2);
	if (rx(pf) != 0xd5)
		error();

	sleep(3);

	buf[0] = 0xd6;
	buf[1] = 0x01;
	buf[2] = 0x03;
	tx(dsk, 3);
	if (rx(pf) != 0xd7)
		error();

	timeout = 100;
	for (i = 0; i < DATALEN; i++) {
		len = DATA[i][1] + 2;
		for (j = 0; j < len; j++)
			buf[j] = DATA[i][j];
		tx(dsk, len);
		response = rx(pf);
		if ((response != 0xd9) && (response != 0xff))
			error();
	}

	buf[0] = 0xda;
	buf[1] = 0x01;
	/* crude checksum */
	buf[2] = 0x56; //0x56 for 320, 0x54 for 321
	tx(dsk, 3);
	timeout = 60000;
	response = rx(pf);
	if ((response != 0xdb) && (response != 0xd9))
		error();

	if (dsk >= 0) close(dsk);
	free(buf);
	return;
}

static void usage(void)
{
	printf("aaplfwpush - push firmware to bluetooth keyboard\n");
	printf("Usage:\n");
	printf("\taaplfwpush -c bdaddr_c -d bdaddr_d\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("\n\tbdaddr_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,"c:d:")) != EOF) {
		switch(opt) {
		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;
		default:
			usage();
			exit(1);
		}
	}

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

	push();

	return 0;
}