main.cpp

blue-usu: (Search, Connect, Playback, Record) with bluetooth devices like headset. Rely on usu dr

/*
 * main.cpp
 *
 * Author : Lionetti Salvatore <[e-mail]>
 * License: GPL
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */
#include <stdio.h>
#include <unistd.h>
#include <time.h> 	/* nanosleep(), timespec.*/
#include <string.h>	/* memset().*/
#include <fcntl.h>  	/* open().*/
#include <unistd.h> 	/* read()/close().*/
#include <errno.h>
#include "blue.h"
HciBus hciBus;
int startConn(char cmd='C', char* mac=NULL, char* pin=NULL, char* lk=NULL) {
	int ritorno=0;
	HciConn hci(hciBus);
	hciBus.clear();

	/* 
	 * w	write configuration,
	 * 
	 * c	connect hci level,
	 * l	connect l2cap,
	 * s    connect sco
	 * 
	 * d	disconnect sco, l2cap, hci level
	 *
	 */
	if (cmd=='C' || cmd=='c') {
//		hci.setMac("\x44\xf0\x01\x5b\x02\x00");
//		linkKeyRes.compileReq("\xE4\xA1\xCA\x92\x03\x49\x18\xF9\x14\x0A\x95\x84\x4A\x0C\x51\xF5",16);
		ritorno = -EINVAL;
		if (!isEmpty(mac,6)) {
			ritorno = 0;
			hci.setMac(mac);
			if (!isEmpty(pin,16)) hci.setPin(pin);
			else {
				char stdpin[16];
				if (isEmpty(lk,16)) {
					memset(stdpin,0,16);
					write(1,"Inserisci il pin[Invio]: ",25);
					read(0,stdpin,16);
					stdpin[strlen(stdpin)-1]='\x0';
					printf("\npin inserito(%s)",stdpin);
					memcpy(pin,stdpin,6);
				} else
					memcpy(stdpin,"\x31\x32\x33\x34",4);
				hci.setPin(stdpin);
			}
			printf("\npin inserito(%s)",pin);

			hci.setLinkKey(lk);
		}
	}

	if (!ritorno && (cmd=='\x0' || cmd=='w' || cmd=='C'))
		if (ritorno==0) ritorno=hci.writeConf();
	hciBus.clear();
	if (!ritorno && (cmd=='\x0' || cmd=='c' || cmd=='C')) {
			/*ritorno=hci.connect(mac,DEVICE);
			if (ritorno<0)*/
		ritorno=hci.connectHci(HOST);
		if (ritorno==0 && lk) hci.getLinkKey(lk);
	}
	hciBus.clear();
	if (!ritorno && (cmd=='\x0' || cmd=='l' || cmd=='C'))
		ritorno=hci.connectL2cap();
	hciBus.clear();
	if (!ritorno && (cmd=='\x0' || cmd=='s' || cmd=='C'))
		ritorno=hci.connectSco();
	hciBus.clear();
	if (!ritorno && (cmd=='\x0' || cmd=='d'))
		if (ritorno==0) ritorno=hci.disconnect();

	hciBus.clear();
//	hciBus.Dump();
	return ritorno;
}
struct Device {
	unsigned char mac[6];
	char nome[256];
	unsigned char pin[16];
	unsigned char link[16];
	Device() { reset(); }
	void operator=(const Device& id) {memcpy(this,&id,sizeof(Device));}
	void reset() { memset(this,0,sizeof(Device)); }
};
class DeviceStore {
	static char nomeFile[];
	static FILE* fp;
public:
	typedef enum WorkT {LOAD,SAVE}; /* One device at time.*/
	DeviceStore();
	~DeviceStore();
	int DeviceStore::workOnStore(WorkT whatWork, Device &device);
};
char DeviceStore::nomeFile[]="devices.txt";
FILE* DeviceStore::fp = NULL;

DeviceStore::DeviceStore() {memcpy(nomeFile,"devices.txt",sizeof(nomeFile));}
DeviceStore::~DeviceStore() {if (fp) fclose(fp);}
#define MAC_FMT "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x"
#define MAC_ARG(a) a[0],a[1],a[2],a[3],a[4],a[5]

#define LINK_FMT "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x"
#define LINK_ARG(a) a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7],a[8],a[9],a[10],a[11],a[12],a[13],a[14],a[15]

#define MAC_FMT_SCAN "%2x:%2x:%2x:%2x:%2x:%2x"
#define MAC_ARG_SCAN(a) a,a+1,a+2,a+3,a+4,a+5
	
#define LINK_FMT_SCAN "%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x:%2x"
#define LINK_ARG_SCAN(a) a,a+1,a+2,a+3,a+4,a+5,a+6,a+7,a+8,a+9,a+10,a+11,a+12,a+13,a+14,a+15

/* 
 * Now we read all device then write all device.
 * LOAD: load first device found (exist internal state)
 * SAVE: append current device to file.
 *
 * ret: 0 mean no error.
 */
int DeviceStore::workOnStore(WorkT whatWork, Device& indevice) {
	/* sizeof(mac) return 4*
	 * 1) Do a tour in file:
	 * 	LOAD: memo value
	 * 	STOR: memo from value (excluded) to eof, 
	 * 	      overwrite area on file, value are merged (isEmpty()).
	 */
	int ritorno = -ENODATA;

	Device curDevice;
	if (fp || (fp=fopen("devices.txt",whatWork==LOAD?"r":"w+"))) {
		switch (whatWork) {
			case LOAD: {
				char riga[80];
				enum LoadStatus {NONE, FOUND, DONE} stato=NONE, statokm1=NONE;
				while ((stato!=DONE) && fgets(riga,80,fp)) {
					char ts[80];
					int tl,l;
					unsigned int *ti=(unsigned int*)ts;
					memset(ts,0,80);
					statokm1=stato;
					if (sscanf(riga,"\tmac=\"" MAC_FMT_SCAN "\"",MAC_ARG_SCAN(ti))==6) {
					      if (riga[6]!=' ') {curDevice.reset(); stato = FOUND;}
					      for (l=0; l<6; l++) curDevice.mac[l]=(unsigned char)ti[l];
					}else if(sscanf(riga,"\tnome=\"%s\"",ts)==1) {
						memcpy(curDevice.nome,ts,strlen(ts)-1);
					}else if((tl=sscanf(riga,"\tpin=\"" LINK_FMT_SCAN "\"",LINK_ARG_SCAN(ti)))>=4) {
						for (l=0; l<tl; l++) curDevice.pin[l]=(unsigned char)ti[l];
					}else if((tl=sscanf(riga,"\tpin=\"%s\"",ts))==1) {
						memcpy(curDevice.pin,ts,strlen(ts)-1);
					}else if((tl=sscanf(riga,"\tlinkkey=\"" LINK_FMT_SCAN "\"",LINK_ARG_SCAN(ti)))==16) {
						for (l=0; l<tl; l++) curDevice.link[l]=(unsigned char)ti[l];
					}else if(sscanf(riga,"</device%c",ts))
						stato = DONE;
				}
				if (statokm1==FOUND)	{ritorno = 0; indevice=curDevice;}
				else 			{ritorno = -ENODATA; fclose(fp); fp=NULL;}
				break;
			}
			case SAVE: {
				ritorno=fprintf(fp,"<device>\n");
				if (ritorno>0) ritorno=fprintf(fp,"\tmac=\"" MAC_FMT "\"\n", MAC_ARG(indevice.mac));
				if (ritorno>0) ritorno=fprintf(fp,"\tnome=\"%s\"\n",indevice.nome);
				if (ritorno>0) ritorno=fprintf(fp,"\tpin=\"%s\"\n",indevice.pin);
				if (ritorno>0) ritorno=fprintf(fp,"\tlinkkey=\"" LINK_FMT "\"\n",
										 LINK_ARG(indevice.link));
				if (ritorno>0) ritorno=fprintf(fp,"</device>\n");
				if (ritorno>0) ritorno=0;
				break;
			}
		}
	}
	return ritorno;
}
# define HOWMAC 10
int main(int argc, char* argv[]) {
	int ritorno;
//	return startConn('d');
	char cmd;
	/* Local */
	unsigned char mac[6];
	/* Remote */
	Device devices[HOWMAC];
	char opts[HOWMAC][8];
	int stato[HOWMAC];
	int nMac;
	int l,k;

	nMac=0; cmd=0; ritorno=0;
	memset(opts,0,sizeof(opts));
	memset(stato,0,sizeof(stato));

	/* Load saved device list.*/
	DeviceStore ds;
	while (nMac<HOWMAC && ds.workOnStore(DeviceStore::LOAD, devices[nMac])==0)
		nMac++;

	{
		int fd=open("currmac.bin",O_RDONLY);
		if (fd>0 && read(fd,mac,6)==6)
			for (l=0; l<nMac; l++)
				if (memcmp(devices[l].mac,mac,6)==0)
					stato[l]=!stato[l];
		if (fd>0) close(fd);
	}
	{ /* Read host device mac.*/
		memset(mac,0,6);
		ritorno = hciBus.readMac((char*)mac);
		if (ritorno<0)
			return ritorno;
	}

	while (cmd!=nMac+3) {
		printf(" === Blue@" MAC_FMT "=== \n", MAC_ARG(mac));
		printf(" 1) Search for reachable peripheral.\n");
		printf(" 2) Write Local configuration (audio mode...)\n");

		for (l=0; l<nMac; l++)
			printf(" %d) %s " MAC_FMT " %s\n",l+3, stato[l]?"Disconnect from":"Connect to",MAC_ARG(devices[l].mac),devices[l].nome);
		printf(" %d) quit(Saving current state).",nMac+3);
		
		while ((cmd=getchar())<='0') {}
	       	cmd -= '0';
//		printf("got char=%x",cmd+'0');
		
		hciBus.clear();
		if (cmd==1) {
			char newMacs[HOWMAC][6];
			char newName[256];
			int newnMac=0;
			memset(newMacs,0,HOWMAC*6);
			write(1,"Searching...",13);
			printf("(ret=%d)\n",(ritorno=hciBus.searchDevice(newMacs,opts,HOWMAC)));
			if (ritorno>0)
			       newnMac = ritorno;
			
			for (k=0; k<newnMac; k++) {
				printf("device %d) " MAC_FMT " clock off: %2.2x:%2.2x\n",k,
						MAC_ARG((unsigned char)newMacs[k]),
						(unsigned char)opts[k][6],
						(unsigned char)opts[k][7]);
			}
						
			/* New device where merged with old one.*/
			for (k=0; (k<newnMac)/* && (ritorno>=0)*/; k++) {
#if 0
				SONO DIVERSI O NO PER LO STESSO MAC!!!
				Belkin dongle: puo dare 4 eventi sullo stesso mac.
					       In genere a regime converge.
						Es 2 eventi
						A B (1o search)
						B B
						...
						Es 3 eventi
						A B A
						B B B
						...
						
				We choice the last value, perhaps the most probable2be steady state.
#endif
						
				
				int n;
				for (n=0; n<newnMac; n++)
					if (n!=k && !isEmpty(newMacs[k],6) && memcmp(newMacs[k],newMacs[n],6)==0) {
						int min=n;
						if (k<n) min=k;
						printf("delete device %d\n",min);
						memset(newMacs[min],0,6);
					}
					
					
				/* However update mac, name, if mac!empty, name readed without problem.*/
				ritorno = -EINVAL;
				memset(newName,0,256);
				if (!isEmpty(newMacs[k],6)) {
					printf("device %d) " MAC_FMT " (rep,mode)=(%d,%d) clock off: %2.2x:%2.2x\n",k,
							MAC_ARG((unsigned char)newMacs[k]),
							(unsigned char)opts[k][0],
							(unsigned char)opts[k][2],
							(unsigned char)opts[k][6],
							(unsigned char)opts[k][7]);
					/*opts[k][0]=1;
					opts[k][7]|=0x80;
					opts[k][0]=2;
					opts[k][2]=0;
					opts[k][6]=0;
					opts[k][7]=0;*/
#define READ_REM_NAME
#ifdef READ_REM_NAME
					if ((ritorno=hciBus.readRemoteName(newMacs[k],opts[k],newName,256))>=0)
#endif
					{
						/* Get un index l onto write, updating nMac id new device.*/
						for (l=0; l<nMac; l++)
							if (memcmp(newMacs[k],devices[l].mac,6)==0)
								break;
						
						if (l==nMac) nMac++;
						if (nMac<HOWMAC) {
							memcpy(devices[l].mac,newMacs[k],6);
#ifdef READ_REM_NAME
							memcpy(devices[l].nome,newName,256);
#endif
						}
					}
				}
			}
		}
		if (cmd==2) {
			ritorno = startConn('w');
		}
		if (cmd>2 && cmd<nMac+3) {
			int k;
			bool alreadyConnected=false;
			l=cmd-3;
			for (k=0; k<nMac; k++)
				if (stato[k] && k!=l) {
					printf("Already connected with device %d " MAC_FMT ".\n",k,MAC_ARG(devices[k].mac));
					alreadyConnected=true;
				}
			if (!alreadyConnected) {
				printf("trying %sconnection with device %d " MAC_FMT ".",stato[l]?"dis":"",cmd, MAC_ARG(devices[l].mac));
				ritorno = startConn(stato[l]?'d':'C',
						(char*)devices[l].mac,
						(char*)devices[l].pin,
						(char*)devices[l].link);
				printf("ritorno(%d)",ritorno);
				if (ritorno>=0) stato[l]=!stato[l];
			}
		}
		hciBus.clear();
	}
	/* Flush all data on device learned in this session.*/
	l=0;
	while (l<nMac && ds.workOnStore(DeviceStore::SAVE,devices[l])==0) l++;
	{
		for (l=0; l<nMac; l++)
			if (stato[l])
				break;
		if (l<nMac) memcpy(mac,devices[l].mac,6);
		else memset(mac,0,6);
		
		int fd=open("currmac.bin",O_CREAT|O_WRONLY);
		if (fd<=0 || write(fd,mac,6)!=6)
			fprintf(stderr,"Non riesco a salvare su file che il device " MAC_FMT " e' connesso!",MAC_ARG(mac));
		if (fd>0) close(fd);
	}

#if 0
	char cmd='\x0';
	if (argc>1)
		cmd = argv[1][0];
#endif
	return ritorno;
	/*startConn(cmd);*/
}