/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2000-2001  Qualcomm Incorporated
 *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
 *  Copyright (C) 2002-2005  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 version 2 as
 *  published by the Free Software Foundation;
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 *  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
 *  IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
 *  CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *  ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 
 *  COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 
 *  SOFTWARE IS DISCLAIMED.
 *
 *
 *  $Id: l2test.c,v 1.16 2005/01/11 21:42:38 holtmann Exp $
 */
                                                                                                      

			/*Send program*/
#include <stdio.h>
#include <string.h>
#include <sys/socket.h>

#include <sys/ioctl.h>
#include <fcntl.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/l2cap.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/l2cap.h>
#include <errno.h>
#include "csr.h"


unsigned char packet[256];
unsigned char buf[256]={'a','b','c','d','e','f',0x7c,'g',0x7c,'i'};

static unsigned char *rbuf;


static int master = 0;
static int auth = 0;
static int encrypt = 0;
static int secure = 0;

static int reliable = 0;
static int socktype = SOCK_SEQPACKET;

/* Default mtu */
static int imtu = 672;
static int omtu = 0;
static int linger = 0;


/* Default data size */
static long data_size = 672;

/* Default addr and psm */
static bdaddr_t bdaddr;
static unsigned short psm = 10;


//const char str[18]="00:0B:0D:33:DA:F9";


void dump_mode(int);
static int listen_connection(void);
void accept_connection(int);
static void do_listen(void);





unsigned char formpacket(unsigned char device, unsigned char len )
{
	unsigned char temp=0x00;
	unsigned char i,j,pdlen;
	
	packet[0]=0x7c;
	switch(device)
	{
		case 1:
			packet[1]=0x01;
			break;
		case 2:
			packet[1]=0x02;
			break;
		case 3:
			packet[1]=0x03;
			break;
		case 4:
			packet[1]=0x04;
			break;
	}
		
	pdlen=len;
	for(i=3,j=0;j<len;i++,j++)
	{
		/*if(buf[j]==0x7c)
		{
			packet[i]=0x7c;
			packet[++i]=buf[j];
			pdlen++;			
		}
		else*/
			packet[i]=buf[j];
		
	}
	
	packet[2]=pdlen;

	temp=temp ^ packet[0];
	temp=temp ^ packet[1];
	temp=temp ^ packet[2];
	for(i=0;i<pdlen;i++)
		temp=temp ^ packet[i+3];

	packet[pdlen+3]=temp;
	
	return pdlen;	
	
}

void send_data(int s)
{
	size_t datlen;
	int i,nb;
	datlen=formpacket(0x01,0x0a);

	if ((nb=send(s,&packet[0],(datlen+4),MSG_DONTWAIT))< 0)
			perror("\nCan't send data.\n");
	else
	{
			printf("\nNo. of bytes sent=%d\n",nb);
			printf("\nPacket:\n");
			for(i=0;i<datlen+4;i++)
				printf(" %x ",packet[i]);	
				printf("\n\n");

			//return 0;
	}
}	

static int remote_connect(char *svr)
{
	struct sockaddr_l2 rem_addr, loc_addr;
	struct l2cap_options opts;
	struct l2cap_conninfo conn;
	int s, opt;

	if ((s = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP)) < 0) {
		printf("\nCan't create socket: %s (%d)\n", strerror(errno), errno);
		return -1;
	}
	else 
		printf("\nL2CAP socket opened successfully.\n");


	memset(&loc_addr, 0, sizeof(loc_addr));
	loc_addr.l2_family = AF_BLUETOOTH;
	bacpy(&loc_addr.l2_bdaddr, &bdaddr);
	if (bind(s, (struct sockaddr *) &loc_addr, sizeof(loc_addr)) < 0) {
		printf("\nCan't bind socket: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	/* Get default options */
	memset(&opts, 0, sizeof(opts));
	opt = sizeof(opts);
	if (getsockopt(s, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		printf("\nCan't get default L2CAP options: %s (%d)\n", strerror(errno), errno);
		return -1;	
	}

	/* Set new options */
	opts.omtu = omtu;
	opts.imtu = imtu;
	if (setsockopt(s, SOL_L2CAP, L2CAP_OPTIONS, &opts, opt) < 0) {
		printf("\nCan't set L2CAP options: %s (%d)\n", strerror(errno), errno);
		return -1;
	}

	/* Enable SO_LINGER */
	if (linger) {
		struct linger l = { .l_onoff = 1, .l_linger = linger };
		if (setsockopt(s, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
			printf("\nCan't enable SO_LINGER: %s (%d)\n",
				strerror(errno), errno);
			return -1;
		}
	}

	/* Set link mode */
	opt = 0;
	if (reliable)
		opt |= L2CAP_LM_RELIABLE;

	if (setsockopt(s, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {
		printf("\nCan't set L2CAP link mode: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	memset(&rem_addr, 0, sizeof(rem_addr));
	rem_addr.l2_family = AF_BLUETOOTH;
	str2ba(svr, &rem_addr.l2_bdaddr);
	rem_addr.l2_psm = htobs(psm);
	if (connect(s, (struct sockaddr *) &rem_addr, sizeof(rem_addr)) < 0 ) {
		printf("\nCan't connect: %s (%d)\n", strerror(errno), errno);
		close(s);
		return -1;
	}

	memset(&opts, 0, sizeof(opts));
	opt = sizeof(opts);
	if (getsockopt(s, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		printf("\nCan't get L2CAP options: %s (%d)\n", strerror(errno), errno);
		close(s);
		return -1;
	}

	memset(&conn, 0, sizeof(conn));
	opt = sizeof(conn);
	if (getsockopt(s, SOL_L2CAP, L2CAP_CONNINFO, &conn, &opt) < 0) {
		printf("\nCan't get L2CAP connection information: %s (%d)\n", strerror(errno), errno);
		close(s);
		return -1;
	}

	printf("\nConnected [imtu %d, omtu %d, flush_to %d, mode %d, handle %d, class 0x%02x%02x%02x]\n",
		opts.imtu, opts.omtu, opts.flush_to, opts.mode, conn.hci_handle,
		conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);

	return s;
}

// Recevie part at the sending end

#if 0
static int listen_connection(void)
{
	struct sockaddr_l2 loc_addr, rem_addr;
	struct l2cap_options opts;
	int s, s1, opt;
	char ba[18];

	
	bacpy(&bdaddr, BDADDR_ANY);


	if ((s = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP)) < 0) {
		printf("\nCan't create socket: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	loc_addr.l2_family = AF_BLUETOOTH;
	bacpy(&loc_addr.l2_bdaddr, &bdaddr);
	loc_addr.l2_psm = htobs(psm);
	
	if (bind(s, (struct sockaddr *) &loc_addr, sizeof(loc_addr)) < 0) {
		printf("\nCan't bind socket: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	/* Set link mode */
	opt = 0;
	if (reliable)
		opt |= L2CAP_LM_RELIABLE;
	if (master)
		opt |= L2CAP_LM_MASTER;
	if (auth)
		opt |= L2CAP_LM_AUTH;
	if (encrypt)
		opt |= L2CAP_LM_ENCRYPT;
	if (secure)
		opt |= L2CAP_LM_SECURE;

	if (setsockopt(s, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {
		printf("\nCan't set L2CAP link mode: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	/* Get default options */
	opt = sizeof(opts);
	if (getsockopt(s, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		printf("\nCan't get default L2CAP options: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	/* Set new options */
	opts.imtu = imtu;
	if (setsockopt(s, SOL_L2CAP, L2CAP_OPTIONS, &opts, opt) < 0) {
		printf("\nCan't set L2CAP options: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

		
	return s;

}


void accept_connection(int s)
{
	struct sockaddr_l2 loc_addr, rem_addr;
	struct l2cap_options opts;
	int s1, opt;
	char ba[18];

		if (listen(s, 10)) {
		printf("\nCan not listen on the socket: %s (%d)\n", strerror(errno), errno);
		exit(1);
		}

		printf("\nWaiting for connection on psm %d ...\n", psm);
	

		while(1) {
		opt = sizeof(rem_addr);
		if ((s1 = accept(s, (struct sockaddr *) &rem_addr, &opt)) < 0) {
			printf("\nAccept failed: %s (%d)\n", strerror(errno), errno);
			exit(1);
		}

		opt = sizeof(opts);
		if (getsockopt(s1, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
			printf("\nCan't get L2CAP options: %s (%d)\n", strerror(errno), errno);
			exit(1);
		}

		ba2str(&rem_addr.l2_bdaddr, ba);
		printf("\nConnect from %s [imtu %d, omtu %d, flush_to %d]\n",ba, opts.imtu, opts.omtu, opts.flush_to);

		/* Enable SO_LINGER */
		if (linger) {
			struct linger l = { .l_onoff = 1, .l_linger = linger };
			if (setsockopt(s, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
				printf("\nCan't enable SO_LINGER: %s (%d)\n",
					strerror(errno), errno);
				exit(1);
			}
		}

	
		
		dump_mode(s1);
		close(s1);

		printf("\nDisconnect: %m\n");
	}
}
#endif

static void do_listen(void)
{
	struct sockaddr_l2 loc_addr, rem_addr;
	struct l2cap_options opts;
	int s, s1, opt;
	char ba[18];

	if ((s = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP)) < 0) {
		printf("\nCan't create socket: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	loc_addr.l2_family = AF_BLUETOOTH;
	bacpy(&loc_addr.l2_bdaddr, &bdaddr);
	loc_addr.l2_psm = htobs(psm);
	
	if (bind(s, (struct sockaddr *) &loc_addr, sizeof(loc_addr)) < 0) {
		printf("\nCan't bind socket: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	/* Set link mode */
	opt = 0;
	if (reliable)
		opt |= L2CAP_LM_RELIABLE;
	if (master)
		opt |= L2CAP_LM_MASTER;
	if (auth)
		opt |= L2CAP_LM_AUTH;
	if (encrypt)
		opt |= L2CAP_LM_ENCRYPT;
	if (secure)
		opt |= L2CAP_LM_SECURE;

	if (setsockopt(s, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {
		printf("\nCan't set L2CAP link mode: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	/* Get default options */
	opt = sizeof(opts);
	if (getsockopt(s, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		printf("\nCan't get default L2CAP options: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	/* Set new options */
	opts.imtu = imtu;
	if (setsockopt(s, SOL_L2CAP, L2CAP_OPTIONS, &opts, opt) < 0) {
		printf("\nCan't set L2CAP options: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

/*	if (socktype == SOCK_DGRAM) {
		handler(s);
		return;
	}*/

	if (listen(s, 10)) {
		printf("\nCan not listen on the socket: %s (%d)\n", strerror(errno), errno);
		exit(1);
	}

	printf("\nWaiting for connection on psm %d ...\n", psm);

	while(1) {
		opt = sizeof(rem_addr);
		if ((s1 = accept(s, (struct sockaddr *) &rem_addr, &opt)) < 0) {
			printf("\nAccept failed: %s (%d)\n", strerror(errno), errno);
			exit(1);
		}

		opt = sizeof(opts);
		if (getsockopt(s1, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
			printf("\nCan't get L2CAP options: %s (%d)\n", strerror(errno), errno);
			exit(1);
		}

		ba2str(&rem_addr.l2_bdaddr, ba);
		printf("\nConnect from %s [imtu %d, omtu %d, flush_to %d]\n",ba, opts.imtu, opts.omtu, opts.flush_to);

		/* Enable SO_LINGER */
		if (linger) {
			struct linger l = { .l_onoff = 1, .l_linger = linger };
			if (setsockopt(s, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
				printf("\nCan't enable SO_LINGER: %s (%d)\n",
					strerror(errno), errno);
				exit(1);
			}
		}

	
		
		dump_mode(s1);
		close(s1);

		printf("\nDisconnect: %m\n");
	}
}



static void hexdump(unsigned char *s, unsigned long l)
{
	unsigned long i;
	unsigned char j,k;
	unsigned char temp=0x00;

	/*for(i=0;i<l;i++)
	{
		printf("\r\n0x");
		printf("%x",s[i]);
	}
	printf("\r\n");*/
	
	for(i=0;i<l;i++)
	{
		if((s[i]== 0x7C)) 	//&& (s[i+1] != 0x7C))
		{
			temp=temp ^ s[i];
			temp=temp ^ s[i+1];
			temp=temp ^ s[i+2];
			
			for(k=i;k<(i+s[i+2]);k++)
				temp=temp ^ s[k+3];

					
				if(s[s[i+2]+3]==temp)
					printf("\nPacket recevied correctly\n");
				else
				{
					printf("\nPacket corrupted\n");
					return;
				}

				
			if(s[i+1]==0x01)
			{
				printf("\nKeyboard data packet\n");
				printf("\nNo. of bytes in Keyboard packet:%x\n",s[2]);
				for(j=3;j<(s[i+2]+3);j++)
					printf(" %x ",s[j]);
			}
				
			if(s[i+1]==0x02)
			{
				printf("\nMouse data packet\n");
				printf("\nNo. of bytes in Mouse packet:%x\n",s[2]);
				for(j=3;j<(s[i+2]+3);j++)
					printf(" %x ",s[j]);
			}
			
			if(s[i+1]==0x03)
			{
				printf("\nFinger print data packet\n");
				printf("\nNo. of bytes in Finger print packet:%x\n",s[2]);
				for(j=3;j<(s[i+2]+3);j++)
					printf(" %x ",s[j]);
			}
			
			if(s[i+1]==0x04)
			{
				printf("\nControl packet\n");
				printf("\nNo. of bytes in Control packet:%x\n",s[2]);
				for(j=3;j<(s[i+2]+3);j++)
					printf(" %x ",s[j]);
			}
		break;
		}
	}
			

	


}


void dump_mode(int s)
{
	int len;
	int opt, optl;


	printf("\r\nReceiving ...\r\n");

	while (1) {
		fd_set rset;

		FD_ZERO(&rset);
		FD_SET(s, &rset);

		if (select(s + 1, &rset, NULL, NULL, NULL) < 0)
			return;

		if (!FD_ISSET(s, &rset))
			continue;

		len = read(s, rbuf, data_size);
		printf("\nRecevied %d bytes\n", len);

		if (len <= 0) {
			if (len < 0) {
				if (reliable && (errno == ECOMM)) {
					printf("\r\nL2CAP Error ECOMM - clearing error and continuing.\r\n");
					optl = sizeof(opt);
					if (getsockopt(s, SOL_SOCKET, SO_ERROR, &opt, &optl ) < 0) { // Clear error
						printf("\r\nCouldn't getsockopt(SO_ERROR): %s (%d)\r\n",
							strerror(errno), errno);
						return;
					}
					continue;
				} else {
					printf("\r\nRead error: %s(%d)\n", strerror(errno), errno);
				}
			}
		//	return;
			break;
		//	continue;
		}
	//	else if(len > 0)
		{			
			printf("\nRecevied %d bytes\n", len);
			hexdump(rbuf,len);
		}
	}
	close(s);
}


int main(void)
{
	int fd;
	fd=remote_connect("00:0B:0D:33:DA:F9");
	send_data(fd);
	//fd=listen_connection();
	//accept_connection(fd);
//	do_listen();
	
	

}