btconfig.c

/*
 * Copyright (c) 2008-2009 Atheros Corporation. All rights reserved.
 * 
 * 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;
 *
 * Software distributed under the License is distributed on an "AS
 * IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
 * implied. See the License for the specific language governing
 * rights and limitations under the License.
 *
 * <Configuration utility for AR3001>
 *
 * <btconfig.c>
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdio.h>
#include <errno.h>
#include <ctype.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>
#include <signal.h>
#include <time.h>
#include <sys/param.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/types.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>

#include "compat/getline.h"
#include "btconfig.h"
#include "masterblaster.h"

#define BT_PORT 2398
/* Global Variables */
static int sid, cid, aid;
static int Tag_Count = 0;
static int Patch_Count = 0;
static unsigned short DynMem_Count = 0;
static int Total_tag_lenght = 0;
static BOOL CtrlCBreak = FALSE;
bdaddr_t BdAddr;
/* Function Declarations */
static int LoadConfFile(const char *path, int basetag, int format);
static int ParseFiles(FILE *fpt, int basetag, int format);
static void LoadPSHeader(UCHAR *HCI_PS_Command,UCHAR opcode,int length,int index);
static BOOL PSOperations(int dd, UCHAR Opcode, int Param1, UINT32 *out);
static BOOL SU_LERxTest(int dev_id, UCHAR channel);
static BOOL SU_LETxTest(int dev_id, UCHAR channel, UCHAR length, UCHAR payload);
static int PSInit(int dd);
static void usage(void);
static int writeHciCommand(int dd, uint16_t ogf, uint16_t ocf, uint8_t plen, UCHAR *buf);
static int MemBlkRead(int dd, UINT32 Address,UCHAR *pBuffer, UINT32 Length);
static int MemBlkwrite(int dd, UINT32 Address,UCHAR *pBuffer, UINT32 Length);
static int Dut(int dd);
static int ReadAudioStats(int dd);
static int ReadGlobalDMAStats(int dd);
static int ResetGlobalDMAStats(int dd);
static int ReadTpcTable(int dd);
static int ReadHostInterest(int dd,tBtHostInterest *pHostInt);
static int ReadMemoryBlock(int dd, int StartAddress,UCHAR *pBufToWrite, int Length );
static int WriteMemoryBlock(int dd, int StartAddress,UCHAR *pBufToWrite, int Length );
static int write_otpRaw(int dev_id, int address, int length, UCHAR *data);
static int read_otpRaw(int dev_id, int address, int length, UCHAR *data);
static void dumpHex(UCHAR *buf, int length, int col);
static void sig_term(int sig);
static UCHAR LEMode = 0;

static struct option main_options[] = {
	{ "help",	0, 0, 'h' },
	{ "device",	1, 0, 'i' },
	{ 0, 0, 0, 0 }
};
//Read the configuration files, file paths are hardcoded in btconfig.h
static int LoadConfFile(const char *path, int basetag, int format){

	FILE *fp;
	//printf("\nOpening file :%s\n",path);
	fp = fopen(path,"r");
	if(fp == NULL){
	//	perror("File open error");
		return FALSE;
	}
	// Parse file
	if(!ParseFiles(fp,basetag,format)){
		printf("\nError :Invalid file format\n");
		return FALSE;
	}	
	// Load conf data to PS
	
	fclose(fp);
	return TRUE;
}


unsigned int uGetInputDataFormat(char **str, struct ST_PS_DATA_FORMAT *pstFormat) 
{
	char *pCharLine = *str;
	if(pCharLine[0] != '[') {
		pstFormat->eDataType = eHex;
		pstFormat->bIsArray = TRUE;
		return TRUE;
	}
	switch(pCharLine[1]) {
		case 'H':
		case 'h':
        if(pCharLine[2]==':') {
			if((pCharLine[3]== 'a') || (pCharLine[3]== 'A')) {
				if(pCharLine[4] == ']') {
					pstFormat->eDataType = eHex;
					pstFormat->bIsArray = TRUE;
					//pCharLine += 5;
					*str += 5;
					return TRUE;
				}
				else {
				   	printf("\nuGetInputDataFormat - Invalid Data Format \r\n"); //[H:A
					return FALSE;
				}
			}
			if((pCharLine[3]== 'S') || (pCharLine[3]== 's')) {
				if(pCharLine[4] == ']') {
					pstFormat->eDataType = eHex;
					pstFormat->bIsArray = FALSE;
					//pCharLine += 5;
					*str += 5;
					//printf("\nDEBUG H-1:%s\n",pCharLine);
					return TRUE;
				}
				else {
				   	printf("\nuGetInputDataFormat - Invalid Data Format \r\n"); //[H:A
					return FALSE;
				}
			}
			else if(pCharLine[3] == ']') {         //[H:]
				pstFormat->eDataType = eHex;
				pstFormat->bIsArray = TRUE;
				//pCharLine += 4;
				*str += 4;
				return TRUE;
			}
			else {                            //[H:
				printf("\nuGetInputDataFormat - Invalid Data Format \r\n");
				return FALSE;					
			}
		}
		else if(pCharLine[2]==']') {    //[H]
			pstFormat->eDataType = eHex;
			pstFormat->bIsArray = TRUE;
			//pCharLine += 3;
			*str += 5;
			return TRUE;
		}
		else {                      //[H
			printf("\nuGetInputDataFormat - Invalid Data Format \r\n");
			return FALSE;			
		}
		break;

		case 'A':
		case 'a':
        if(pCharLine[2]==':') {
			if((pCharLine[3]== 'h') || (pCharLine[3]== 'H')) {
				if(pCharLine[4] == ']') {
					pstFormat->eDataType = eHex;
					pstFormat->bIsArray = TRUE;
					//pCharLine += 5;
					*str += 5;
					return TRUE;
				}
				else {
					printf("\nuGetInputDataFormat - Invalid Data Format \r\n"); //[A:H
					return FALSE;
				}
		 	}
			else if(pCharLine[3]== ']') {         //[A:]
				pstFormat->eDataType = eHex;
				pstFormat->bIsArray = TRUE;
				//pCharLine += 4;
				*str += 5;
				return TRUE;
			}
			else {                            //[A:
				printf("\nuGetInputDataFormat - Invalid Data Format \r\n");
				return FALSE;					
			}
        }
		else if(pCharLine[2]==']') {    //[H]
			pstFormat->eDataType = eHex;
			pstFormat->bIsArray = TRUE;
			//pCharLine += 3;
			*str += 5;
			return TRUE;
		}
		else {                      //[H
			printf("\nuGetInputDataFormat - Invalid Data Format \r\n");
			return FALSE;			
		}
		break;

		case 'S':
		case 's':
        if(pCharLine[2]==':') {
			if((pCharLine[3]== 'h') || (pCharLine[3]== 'H')) {
				if(pCharLine[4] == ']') {
					pstFormat->eDataType = eHex;
					pstFormat->bIsArray = TRUE;
					//pCharLine += 5;
					*str += 5;
					return TRUE;
				}
				else {
					printf("\nuGetInputDataFormat - Invalid Data Format \r\n");//[A:H
					return FALSE;
				}
		 	}
			else if(pCharLine[3]== ']') {         //[A:]
				pstFormat->eDataType = eHex;
				pstFormat->bIsArray = TRUE;
				//pCharLine += 4;
				*str += 5;
				return TRUE;
			}
			else {                            //[A:
				printf("\nuGetInputDataFormat - Invalid Data Format \r\n");
				return FALSE;					
			}
        }
		else if(pCharLine[2]==']') {    //[H]
			pstFormat->eDataType = eHex;
			pstFormat->bIsArray = TRUE;
			//pCharLine += 3;
			*str += 5;
			return TRUE;
		}
		else {                      //[H
			printf("\nuGetInputDataFormat - Invalid Data Format \r\n");
			return FALSE;			
		}
		break;
	
		default:
		printf("\nuGetInputDataFormat - Invalid Data Format \r\n");
		return FALSE;
	}
}

unsigned int uReadDataInSection(char *pCharLine, struct ST_PS_DATA_FORMAT stPS_DataFormat)
{
	if(stPS_DataFormat.eDataType == eHex) {
		if(stPS_DataFormat.bIsArray == TRUE) {
            //Not implemented
            		printf("\nNO IMP\n");
            		return (0x0FFF);
		}
		else {
			//printf("\nDEBUG H-2 %d\n",strtol(pCharLine, NULL, 16));
			return (strtol(pCharLine, NULL, 16));
		}
	}
	else {
        //Not implemented
        	printf("\nNO IMP-1\n");
        	return (0x0FFF);
	}
}


static int ParseFiles(FILE *fpt, int basetag, int format){
	int i,j,k,linecnt,ByteCount=0,ByteCount_Org =0,data,Cnt;
	char *str,line[LINE_SIZE_MAX],byte[3];
	int ParseSelection=RAM_PS_SECTION;
	struct ST_PS_DATA_FORMAT stPS_DataFormat;
	struct ST_READ_STATUS   stReadStatus = {0, 0, 0,0};
	unsigned int uReadCount;

	switch(format){	
		case MB_FILEFORMAT_PS:
			linecnt = 0;
			j=0;

			while ((str = fgets(line, LINE_SIZE_MAX, fpt)) != NULL) {
			SKIP_BLANKS(str);
			//Comment line
			if ((str[0]== '/') && (str[1]== '/'))
				continue;
		
			if (str[0]== '#'){
				if (stReadStatus.uSection != 0){
			 		printf("\nParseFiles - Invalid file format %d\r\n",ParseSelection);
			 		return FALSE;
				 }
			 	else {
					 stReadStatus.uSection = 1;
					 continue;
			 	}
			
			}
			if ((str[0]== '/') && (str[1]== '*'))
			{
				str+=2;
				SKIP_BLANKS(str);
				if(!strncmp(str,"PA",2)||!strncmp(str,"Pa",2)||!strncmp(str,"pa",2)){
					
					ParseSelection=RAM_PATCH_SECTION;
				}
				if(!strncmp(str,"DY",2)||!strncmp(str,"Dy",2)||!strncmp(str,"dy",2)){
					
					ParseSelection=RAM_DYN_MEM_SECTION;
				}
				if(!strncmp(str,"PS",2)||!strncmp(str,"Ps",2)||!strncmp(str,"ps",2)){
					
					ParseSelection=RAM_PS_SECTION;
				}
				linecnt = 0;
				stReadStatus.uSection = 0;
				continue;
			}
		
			switch(ParseSelection){

				case RAM_PS_SECTION:
					if (stReadStatus.uSection == 1){ //TagID
		         			SKIP_BLANKS(str);	
						if(!uGetInputDataFormat(&str, &stPS_DataFormat)) {
							return FALSE;
                    				}	
						PsTagEntry[Tag_Count].TagId = uReadDataInSection(str, stPS_DataFormat);
						stReadStatus.uSection = 2;
					}
					else if (stReadStatus.uSection == 2){ //TagLength
						if(!uGetInputDataFormat(&str, &stPS_DataFormat)) {
							return FALSE;
						}
						ByteCount = uReadDataInSection(str, stPS_DataFormat);

		            			if (ByteCount > RAMPS_MAX_PS_DATA_PER_TAG){
			                		printf("\nParseFiles - INVALID %d: One of the table exceeds maximum table size of %d\r\n",ParseSelection, MAX_RADIO_CFG_TABLE_SIZE);
		        				return FALSE;
		        	    		}
		           			 PsTagEntry[Tag_Count].TagLen = (ByteCount & 0xFF);
						 stReadStatus.uSection = 3;
						 stReadStatus.uLineCount = 0;
		         		}
					else if( stReadStatus.uSection == 3) {  //Data
				        	if(stReadStatus.uLineCount == 0) {
							if(!uGetInputDataFormat(&str,&stPS_DataFormat)) {
								return FALSE;
							}
        		            		}
					   	SKIP_BLANKS(str);
                    				stReadStatus.uCharCount = 0;
						uReadCount = (ByteCount > BYTES_OF_PS_DATA_PER_LINE)? BYTES_OF_PS_DATA_PER_LINE: ByteCount;
						if((stPS_DataFormat.eDataType == eHex) && stPS_DataFormat.bIsArray == TRUE) {
                       					while(uReadCount > 0) {
		                 				PsTagEntry[Tag_Count].TagData[stReadStatus.uByteCount] = (UCHAR)(CONV_HEX_DIGIT_TO_VALUE(str[stReadStatus.uCharCount]) << 4) | (UCHAR)(CONV_HEX_DIGIT_TO_VALUE(str[stReadStatus.uCharCount + 1]));
		                   				PsTagEntry[Tag_Count].TagData[stReadStatus.uByteCount+1] = (UCHAR)(CONV_HEX_DIGIT_TO_VALUE(str[stReadStatus.uCharCount + 3]) << 4) | (UCHAR)(CONV_HEX_DIGIT_TO_VALUE(str[stReadStatus.uCharCount + 4]));
					       			stReadStatus.uCharCount += 6; // read two bytes, plus a space;
					       			stReadStatus.uByteCount += 2;
					       			uReadCount -= 2;
                       					}

					   if(ByteCount > BYTES_OF_PS_DATA_PER_LINE) {
					   	   ByteCount -= BYTES_OF_PS_DATA_PER_LINE;
					   }
					   else {
						  ByteCount = 0;
					   }
					}
					else {
						//to be implemented
						printf("\nParseFiles - To be implemented");
					}

					stReadStatus.uLineCount++;
					
					if(ByteCount == 0) {
						stReadStatus.uSection = 0;
						stReadStatus.uCharCount = 0;
						stReadStatus.uLineCount = 0;
						stReadStatus.uByteCount = 0;
					}
					else { 
					    stReadStatus.uCharCount = 0;
					}
					
		            		if((stReadStatus.uSection == 0)&&(++Tag_Count == RAMPS_MAX_PS_TAGS_PER_FILE))
		            		{
						printf("\n ParseFiles - INVALID %d: Number of tables exceeds %d\r\n",ParseSelection, RAMPS_MAX_PS_TAGS_PER_FILE);
		            			return FALSE;
					}
		
		         
				}
				break;
				default:
	    	 		{
					printf("\nParseFiles - Invalid file format %d\r\n",ParseSelection);
				        return FALSE;
				}
				break;
	
					
		}
		linecnt++;	
	}
		break;
	case MB_FILEFORMAT_DY:
	{
		linecnt = 0;
		while ((str = fgets(line, LINE_SIZE_MAX, fpt)) != NULL) {
			SKIP_BLANKS(str);
			//Comment line
			if ((str[0]== '/') && (str[1]== '/'))
				continue;
		
			if ((str[0]== '/') && (str[1]== '*'))
			{
				continue;
			}
		
	
			if((linecnt % 2) == 0)
			{
	  			ByteCount = (UINT16)strtol(str, NULL, 16);
		  		RamDynMemOverride.Len= (ByteCount & 0xFF);
			}
	  		else
			{
				for (i=0,k=0; k < ByteCount; i += 2,k++) {
					memcpy(byte, &str[i], 2);
					byte[2] = '\0';
					data = strtoul(byte, NULL, 16);
					RamDynMemOverride.Data[k] = (data & 0xFF);
				}
				DynMem_Count = TRUE;
			}
			linecnt++;
		}
	}
		break;
	case MB_FILEFORMAT_PATCH:
	{
		j=0;
		Cnt=0;
		linecnt = 0;
		while ((str = fgets(line, LINE_SIZE_MAX, fpt)) != NULL) {
			SKIP_BLANKS(str);
			//Comment line
			if ((str[0]== '/') && (str[1]== '/'))
				continue;
		
			if ((str[0]== '/') && (str[1]== '*'))
			{
				continue;
			}
	
			if(linecnt==0)
			{
				ByteCount = (UINT16)strtol(str, NULL, 16);
				ByteCount_Org = ByteCount;
				while(ByteCount > MAX_BYTE_LENGTH){
					RamPatch[Patch_Count].Len= MAX_BYTE_LENGTH;
					Patch_Count ++;
					ByteCount= ByteCount - MAX_BYTE_LENGTH;
				}
				RamPatch[Patch_Count].Len= (ByteCount & 0xFF);
				Patch_Count ++;
	
			}
			else
			{
				while(ByteCount_Org > MAX_BYTE_LENGTH){
					for (i = 0, k=0; i < MAX_BYTE_LENGTH*2; i += 2,k++) {
						memcpy(byte, &str[Cnt], 2);
						byte[2] = '\0';
						data = strtoul(byte, NULL, 16);
						RamPatch[j].Data[k] = (data & 0xFF);
						Cnt += 2;
					}
					j++;
					ByteCount_Org = ByteCount_Org - MAX_BYTE_LENGTH;
				}
				if(j == 0){
					j++;
				}
				for (k=0; k < ByteCount_Org;k++) {
					memcpy(byte, &str[Cnt], 2);
					byte[2] = '\0';
					data = strtoul(byte, NULL, 16);
					RamPatch[j].Data[k] = (data & 0xFF);
					Cnt += 2;
				}
			}
	
			linecnt++;	
		}
			
	}
	break;


	
	}
	return TRUE;
}

static void LoadPSHeader(UCHAR *HCI_PS_Command,UCHAR opcode,int length,int index) {
	
	HCI_PS_Command[0]= opcode;
	HCI_PS_Command[1]= (index & 0xFF);
	HCI_PS_Command[2]= ((index>>8) & 0xFF);	
	HCI_PS_Command[3]= length;	

}


static BOOL PSOperations(int dd, UCHAR Opcode, int Param1, UINT32 *out) {
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	int Length,i,j,iRet;
	memset(&buf,0,sizeof(buf));
	switch(Opcode){
		case WRITE_PATCH:
			for(i=0;i< Param1;i++){
				LoadPSHeader(buf,Opcode,RamPatch[i].Len,i);
				for(j=0;j<RamPatch[i].Len;j++){
					buf[4+j]=RamPatch[i].Data[j];
				}
				iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_PS,RamPatch[i].Len + PS_COMMAND_HEADER, buf);
				if(buf[iRet-1] != 0){
					return FALSE;
				}
			}
			break;

		case ENABLE_PATCH:
			Length =0;
			i=0;
			LoadPSHeader(buf,Opcode,Length,i);
			iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_PS,PS_COMMAND_HEADER, buf);
			if(buf[iRet-1] != 0){
				return FALSE;
			}
			break;

		case PS_RESET:
			Length =0;
			i=0;
			LoadPSHeader(buf,Opcode,Length,i);
			buf[8] = (Param1 & 0xFF);
			buf[9] = ((Param1 >>  8) & 0xFF);
			Length = 6;
			iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_PS,Length + PS_COMMAND_HEADER, buf);
			if(buf[iRet-1] != 0){
				return FALSE;
			}
			break;

		case PS_READ: {
			UCHAR *len = (UCHAR *)out;
			ssize_t plen = 0;
			Length = len[0] | ( len[1] << 8);
			LoadPSHeader(buf,Opcode,Length,Param1);
			iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_PS, Length + PS_COMMAND_HEADER, buf);
			if(buf[iRet-1] != 0) {
				return FALSE;
			}
			do {
				plen = read(dd, buf, HCI_MAX_EVENT_SIZE);
				if (plen < 0)
					return FALSE;
			} while (buf[HCI_EVENT_HEADER_SIZE] != DEBUG_EVENT_TYPE_PS);
			memcpy((UCHAR *)out, buf + HCI_EVENT_HEADER_SIZE + 1, plen - HCI_EVENT_HEADER_SIZE - 1);
			break;
		}

		case PS_WRITE:
			for(i=0;i< Param1;i++){
				LoadPSHeader(buf,Opcode,PsTagEntry[i].TagLen,PsTagEntry[i].TagId);
				for(j=0;j<PsTagEntry[i].TagLen;j++){
					buf[4+j]=PsTagEntry[i].TagData[j];
				}
				iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_PS,PsTagEntry[i].TagLen + PS_COMMAND_HEADER, buf);
				if(buf[iRet-1] != 0){
					return FALSE;
				}
			}
			break;

		case PS_DYNMEM_OVERRIDE:
			LoadPSHeader(buf,Opcode,RamDynMemOverride.Len,RamDynMemOverride.Len);
			for(j=0;j<RamDynMemOverride.Len;j++){
				buf[4+j]=RamDynMemOverride.Data[j];
			}
			iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_PS,RamDynMemOverride.Len + PS_COMMAND_HEADER, buf);
			if(buf[iRet-1] != 0){
				return FALSE;
			}
			break;

		case PS_VERIFY_CRC:
			//printf("PSOperations - PS_VERIFY_CRC:VALUE of CRC:%d\r\n",Param1); 
			Length =0;
			LoadPSHeader(buf,Opcode,Length,Param1);
			iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_PS, PS_COMMAND_HEADER, buf);
			if(buf[iRet-1] != 0){
				return FALSE;
			}
			break;

		case PS_GET_LENGTH: {
			ssize_t plen = 0;
			LoadPSHeader(buf,Opcode,0,Param1);
			iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_PS, PS_COMMAND_HEADER, buf);
			if(buf[iRet-1] != 0){
				return FALSE;
			}
			do {
				plen = read(dd, buf, HCI_MAX_EVENT_SIZE);
				if (plen < 0)
					return FALSE;
			} while (buf[HCI_EVENT_HEADER_SIZE] != DEBUG_EVENT_TYPE_PS);
			*((UINT16 *)out) = (buf[HCI_EVENT_HEADER_SIZE + 2] << 8) | buf[HCI_EVENT_HEADER_SIZE + 1];
			break;
		}
	}
		return TRUE;
}


static int GetDeviceType(int dd){
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	int iRet;
	unsigned int Reg = 0;

	memset(&buf,0,sizeof(buf));
	buf[0] = (FPGA_REGISTER & 0xFF);
	buf[1] = ((FPGA_REGISTER >> 8) & 0xFF);
	buf[2] = ((FPGA_REGISTER >> 16) & 0xFF);
	buf[3] = ((FPGA_REGISTER >> 24) & 0xFF); 
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_READ_MEMORY,4, buf);
	if(buf[6] != 0){
		return FALSE;
	}

	Reg = buf[10];
	Reg = ((Reg << 8) | buf[9]);
	Reg = ((Reg << 8) | buf[8]);
	Reg = ((Reg << 8) | buf[7]);
	return Reg;
}
/* PS Operations */
static int PSInit(int dd){
	int i,Crc=0,DevType =0; 
   	BOOL BDADDR_Present = 0;	
	BOOL File_Present = 0;
	
	//DevType = GetDeviceType(dd);
	//printf("\nDevice Type:%x\n",DevType);
	if(DevType){
		if(DevType == 0xdeadc0de){
		
			if(!LoadConfFile(PS_ASIC_FILENAME,0xFFFFFFFF,MB_FILEFORMAT_PS)){
				printf("\nPlease copy PS file to :%s\n",PS_ASIC_FILENAME);
				//return FALSE;
			}
			else
				File_Present = 1;
		}
		else{
			if(!LoadConfFile(PS_FPGA_FILENAME,0xFFFFFFFF,MB_FILEFORMAT_PS)){
				printf("\nPlease copy PS file to :%s\n",PS_FPGA_FILENAME);
				File_Present = 1;
				//return FALSE;
			}
			else
				File_Present = 1;
		}
	}
	else{
		if(!LoadConfFile(PS_ASIC_FILENAME,0xFFFFFFFF,MB_FILEFORMAT_PS)){
				printf("\nPlease copy PS file to :%s\n",PS_ASIC_FILENAME);
				File_Present = 1;
				//return FALSE;
		}
		else
			File_Present = 1;
	}

	if(!LoadConfFile(PATCH_FILENAME,0xFFFFFFFF,MB_FILEFORMAT_PATCH)){
		printf("\nPlease copy Patch file to :%s\n",PATCH_FILENAME);
		File_Present = 1;
	}
	else
		File_Present = 1;
	
	if(!File_Present){
		printf("\nPS and Patch files are not present\n");
		return FALSE;
	}
	if(Tag_Count == 0){
		Total_tag_lenght = 10;

	}
	else{
		for(i=0; i<Tag_Count; i++){
			if(PsTagEntry[i].TagId == 1){
				BDADDR_Present = TRUE;
				//printf("ReadPSFiles - BD ADDR is present in Patch File \r\n");
			}
		
			if(PsTagEntry[i].TagLen % 2 == 1){
				Total_tag_lenght = Total_tag_lenght + PsTagEntry[i].TagLen + 1;
			}
			else{
				Total_tag_lenght = Total_tag_lenght + PsTagEntry[i].TagLen;
			}
		
		}
	}
	
	if(Tag_Count > 0 && !BDADDR_Present){
		//printf("\nReadPSFiles - BD ADDR is not present adding 10 extra bytes \r\n");
		Total_tag_lenght=Total_tag_lenght + 10;
	}
	Total_tag_lenght = Total_tag_lenght+ 10 + (Tag_Count*4);

//	printf("\nPSInitialize - PATCH:%d, DYN:%d, TAG:%d Total_tag_lenght:%d\n",Patch_Count,DynMem_Count,Tag_Count,Total_tag_lenght);
	
	if(Patch_Count > 0)
		Crc |= RAM_PATCH_REGION;
	if(DynMem_Count)
		Crc |= RAM_DYN_MEM_REGION;
	if(Tag_Count > 0)
		Crc |= RAM_PS_REGION;

	if(Patch_Count || DynMem_Count || Tag_Count ){
		if(Patch_Count > 0){
			if(!PSOperations(dd,WRITE_PATCH,Patch_Count, NULL)){
				printf("\nPSInitialize - *** WRITE_PATCH FAILED**** \r\n");
				return FALSE;
			}
			if(!PSOperations(dd,ENABLE_PATCH,0, NULL)){
				printf("\nPSInitialize - *** ENABLE_PATCH FAILED**** \r\n");
				return FALSE;
			}
		}
		if(DynMem_Count){
			if(!PSOperations(dd,PS_DYNMEM_OVERRIDE,DynMem_Count, NULL)){
				printf("\nPSInitialize - *** PS_DYNMEM_OVERRIDE FAILED**** \r\n");
				return FALSE;
			}
		}
		if(!PSOperations(dd,PS_RESET,Total_tag_lenght, NULL)){
			printf("\nPSInitialize - *** PS RESET FAILED**** \r\n");
			return FALSE;
		}
		if(Tag_Count > 0){
			if(!PSOperations(dd,PS_WRITE,Tag_Count, NULL)){
				printf("\nPSInitialize - *** PS_WRITE FAILED**** \r\n");
				return FALSE;
			}
		}	
		
	}	
	
	if(!PSOperations(dd,PS_VERIFY_CRC,Crc, NULL)){
		printf("\nVerify CRC failed\n");	
		return FALSE;
	}
	return TRUE;

}

static int writeHciCommand(int dd, uint16_t ogf, uint16_t ocf, uint8_t plen, UCHAR *buf){
#ifdef DUMP_DEBUG
#define cmd_opcode_pack(ogf, ocf) (uint16_t)((ocf & 0x03ff)|(ogf << 10))
#if __BYTE_ORDER == __LITTLE_ENDIAN
#define htobs(d)	(d)
#elif __BYTE_ORDER == __BIG_ENDIAN
#define htobs(d)	bswap_16(d);
#else
#error "Unknown byte order"
#endif
	int i = 0, j = 0;
	uint16_t opcode = htobs(cmd_opcode_pack(ogf, ocf));
	
	printf("\nDump:\n");
	printf("0x%02X ", opcode & 0xff);
	i++;
	printf("0x%02X ", (opcode & 0xff00) >> 8);
	i++;
	printf("0x%02X ", plen);
	i++;
	for (j = 0; j < plen; i++, j++) {
		printf("0x%02X ", buf[j]);
		if (((i+1) % 8) == 0 && i != 0)
			printf("\n");
	}
	if (((i+1) % 8) != 0) printf("\n");
	buf[6] = 0;
	return plen;
#else
	struct hci_filter flt;
        uint16_t opcode, topcode;

	/* Setup filter */
	hci_filter_clear(&flt);
	hci_filter_set_ptype(HCI_EVENT_PKT, &flt);
	hci_filter_all_events(&flt);
	if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
		perror("HCI filter setup failed");
		exit(EXIT_FAILURE);
	}

	//printf("< HCI Command: ogf 0x%02x, ocf 0x%04x, plen %d\n", ogf, ocf, plen);
	if (hci_send_cmd(dd, ogf, ocf, plen, buf) < 0) {
		perror("Send failed");
		hci_close_dev(dd);
		exit(EXIT_FAILURE);
	}
	sleep(0.4);
        opcode = (ogf << 10 | ocf);
        do {
	    plen = read(dd, buf,HCI_MAX_EVENT_SIZE);
	    if (plen < 0) {
		    perror("Read failed");
		    hci_close_dev(dd);
		    exit(EXIT_FAILURE);
	    }
	    topcode=(uint16_t)(buf[4] | (buf[5] << 8));
        }while(topcode != opcode);
	return plen;
#endif
}
static int MemBlkRead(int dd,UINT32 Address,UCHAR *pBuffer, UINT32 Length){
	UINT32         Size, ByteLeft,IntCnt;
   	UCHAR          *pData,*pTemp=pBuffer;
	int iRet;	
       	int TempVal;

	IntCnt =0;
       	TempVal = (Length % 4);
	if (TempVal !=0)
       	{
          Length = Length + (4- (Length%4));
       	}
        ByteLeft = Length;
	while (ByteLeft > 0)
   	{
		Size = (ByteLeft > MEM_BLK_DATA_MAX) ? MEM_BLK_DATA_MAX : ByteLeft;
	//	printf("\nMemBlkwrite : Size :%x   Address :%x\n",Size, Address);
		pData = (UCHAR *) malloc(Size + 6);
		pData[0]= 0x00;//depot/esw/projects/azure/AR3001_3_0/src/hci/Hci_Vsc_Proc.c
		pData[1]= (Address & 0xFF);
		pData[2]= ((Address >> 8) & 0xFF);
		pData[3]= ((Address >> 16) & 0xFF);
		pData[4]= ((Address >> 24) & 0xFF);
		pData[5]= Size;
		iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_MEMOP,6,pData);
		if(pData[6]!= 0){
			printf("\nwrite memory command failed due to reason 0x%X\n",pData[6]);	
			free(pData);
			return FALSE;
		}
		if ((read(dd, pData,HCI_MAX_EVENT_SIZE)) < 0) {
			perror("Read failed");
			exit(EXIT_FAILURE);
	    	}

	    	if(pData[3]!=3) {
  			perror("Read failed");
			exit(EXIT_FAILURE);
	    	}
	    	memcpy(pTemp,(pData+4),Size);
		pTemp+=Size;
	        IntCnt = Size;
     		ByteLeft -= Size;
	    	Address += Size;
		free(pData);
   	} 
  	return TRUE;
}


static int MemBlkwrite(int dd,UINT32 Address,UCHAR *pBuffer, UINT32 Length){
	UINT32         Size, ByteLeft,IntCnt;
   	UCHAR          *pData;
	int iRet;	
	ByteLeft = Length;
	IntCnt =0;
	while (ByteLeft > 0)
   	{
	      	
		Size = (ByteLeft > MEM_BLK_DATA_MAX) ? MEM_BLK_DATA_MAX : ByteLeft;
	//	printf("\nMemBlkwrite : Size :%x   Address :%x\n",Size, Address);
		pData = (UCHAR *) malloc(Size + 6);
		pData[0]= 0x01;
		pData[1]= (Address & 0xFF);
		pData[2]= ((Address >> 8) & 0xFF);
		pData[3]= ((Address >> 16) & 0xFF);
		pData[4]= ((Address >> 24) & 0xFF);
		pData[5]= Size;
		memcpy(&pData[6],&pBuffer[IntCnt],Size);
		iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_MEMOP,Size+6,pData);
		if(pData[6]!= 0){
			printf("\nwrite memory command faileddue to reason 0x%X\n",pData[6]);	
			free(pData);
			return FALSE;
		}
	      	IntCnt = Size;
      		ByteLeft -= Size;
	      	Address += Size;
		free(pData);
   	}
  	return TRUE;
}

static int Dut(int dd){
	int iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = 3; //All scan enabled
	iRet = writeHciCommand(dd, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE, 1, buf);	
	if(buf[6] != 0){
		printf("\nWrite scan mode command failed due to reason 0x%X\n",buf[6]);
		return FALSE;
	}
	sleep(1);
	memset(&buf,0,HCI_MAX_EVENT_SIZE);	
	iRet = writeHciCommand(dd, OGF_TEST_CMD, OCF_ENABLE_DEVICE_UNDER_TEST_MODE, 0, buf);	
	if(buf[6] != 0){
		printf("\nDUT mode command failed due to reason 0x%X\n",buf[6]);
		return FALSE;
	}
	memset(&buf,0,HCI_MAX_EVENT_SIZE);	
	buf[0] = 0; //SEQN Track enable =0
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_TEST_MODE_SEQN_TRACKING , 1, buf);	
	if(buf[6] != 0){
		printf("\nTest Mode seqn Tracking failed due to reason 0x%X\n",buf[6]);
		return FALSE;
	}
	return TRUE;
}


void Audio_DumpStats(tAudioStat *AudioStats)
{
  printf("\n\n");
  printf("    Audio Statistics\n");
  printf(">RxCmplt:                 %d\n",AudioStats->RxCmplt);
  printf(">TxCmplt:                 %d\n",AudioStats->TxCmplt);
  printf(">RxSilenceInsert:         %d\n",AudioStats->RxSilenceInsert);
  printf(">RxAirPktDump:            %d\n",AudioStats->RxAirPktDump);
  printf(">MaxPLCGenInterval:       %d\n",AudioStats->MaxPLCGenInterval);
  printf(">RxAirPktStatusGood:      %d\n",AudioStats->RxAirPktStatusGood);
  printf(">RxAirPktStatusError:     %d\n",AudioStats->RxAirPktStatusError);
  printf(">RxAirPktStatusLost:      %d\n",AudioStats->RxAirPktStatusLost);
  printf(">RxAirPktStatusPartial:   %d\n",AudioStats->RxAirPktStatusPartial);
  printf(">SampleMin:               %d\n",AudioStats->SampleMin);
  printf(">SampleMax:               %d\n",AudioStats->SampleMax);
  printf(">SampleCounter:           %d\n",AudioStats->SampleCounter);
  printf("\n\n");

  memset((UCHAR *)AudioStats, 0, sizeof(tAudioStat));
  AudioStats->SampleMax =SHRT_MIN;
  AudioStats->SampleMin =SHRT_MAX;
}

static int ReadAudioStats(int dd){

	tBtHostInterest	HostInt;
	tAudioStat Stats;

	ReadHostInterest(dd, &HostInt);
	if(!HostInt.AudioStatAddr || (HostInt.Version < 0x0300)){
		printf("\nAudio Stat not present\n");
		return FALSE;
	}
	ReadMemoryBlock(dd,HostInt.AudioStatAddr,(UCHAR *)&Stats,sizeof(tAudioStat));
	Audio_DumpStats(&Stats);
	return TRUE;
}

void BRM_DumpStats(tBRM_Stats *Stats)
{
   printf("\n  Link Controller Voice DMA Statistics\n");
   printf("  %22s: %u\n", "VoiceTxDmaIntrs", Stats->VoiceTxDmaIntrs);
   printf("  %22s: %u\n", "VoiceTxPktAvail", Stats->VoiceTxPktAvail);
   printf("  %22s: %u\n", "VoiceTxPktDumped", Stats->VoiceTxPktDumped);
   printf("  %22s: %u\n", "VoiceTxErrors", Stats->VoiceTxErrorIntrs);
   printf("  %22s: %u\n", "VoiceTxDmaErrors", Stats->VoiceTxDmaErrorIntrs);
   printf("  %22s: %u\n", "VoiceTxSilenceInserts", Stats->VoiceTxDmaSilenceInserts);
   printf("\n");
   printf("  %22s: %u\n", "VoiceRxDmaIntrs", Stats->VoiceRxDmaIntrs);
   printf("  %22s: %u\n", "VoiceRxGoodPkts", Stats->VoiceRxGoodPkts);
   printf("  %22s: %u\n", "VoiceRxPktDumped", Stats->VoiceRxPktDumped);
   printf("  %22s: %u\n", "VoiceRxErrors", Stats->VoiceRxErrorIntrs);
   printf("  %22s: %u\n", "VoiceRxCRC", Stats->VoiceRxErrCrc);
   printf("  %22s: %u\n", "VoiceRxUnderOverFlow", Stats->VoiceRxErrUnderOverFlow);
   printf("\n");
   printf("  %22s: %u\n", "SchedOnVoiceError", Stats->SchedOnVoiceError);
   printf("  %22s: %u\n", "VoiceTxReapOnError", Stats->VoiceTxReapOnError);
   printf("  %22s: %u\n", "VoiceRxReapOnError", Stats->VoiceRxReapOnError);
   printf("  %22s: %u\n", "VoiceSchedulingError", Stats->VoiceSchedulingError);

   printf("\n  Link Controller ACL DMA Statistics\n");
   printf("  %22s: %u\n", "DmaIntrs", Stats->DmaIntrs);
   printf("  %22s: %u\n", "ErrWrongLlid", Stats->ErrWrongLlid);
   printf("  %22s: %u\n", "ErrL2CapLen", Stats->ErrL2CapLen);
   printf("  %22s: %u\n", "ErrUnderOverFlow", Stats->ErrUnderOverFlow);
   printf("  %22s: %u\n", "RxBufferDumped", Stats->RxBufferDumped);
   printf("  %22s: %u\n", "ErrWrongLmpPktType", Stats->ErrWrongLmpPktType);
   printf("  %22s: %u\n", "ErrWrongL2CapPktType", Stats->ErrWrongL2CapPktType);
   printf("  %22s: %u\n", "IgnoredPkts", Stats->IgnoredPkts);
   printf("\n");
   printf("  %22s: %u\n", "Data TxBuffers", Stats->DataTxBuffers);
   printf("  %22s: %u\n", "Data RxBuffers", Stats->DataRxBuffers);
   printf("  %22s: %u\n", "LMP TxBuffers", Stats->LmpTxBuffers);
   printf("  %22s: %u\n", "LMP RxBuffers", Stats->LmpRxBuffers);
   printf("  %22s: %u\n", "HEC Errors", Stats->HecFailPkts);
   printf("  %22s: %u\n", "CRC Errors", Stats->CrcFailPkts);

   // Buffer Management
   printf("\n  Buffer Management Statistics\n");
   printf("  %22s: %u\n", "CtrlErrNoLmpBufs", Stats->CtrlErrNoLmpBufs);

   printf("\n  Sniff Statistics\n");
   printf("  %22s: %u\n", "SniffSchedulingError", Stats->SniffSchedulingError);
   printf("  %22s: %u\n", "SniffIntervalNoCorr", Stats->SniffIntervalNoCorr);

   // Other stats
   printf("\n  Other Statistics\n");
   printf("  %22s: %u\n", "ForceOverQosJob", Stats->ForceOverQosJob);
   //printf("  %22s: %u\n", "Temp 1", Stats->Temp1);
   //printf("  %22s: %u\n", "Temp 2", Stats->Temp2);

   // Test Mode Stats
   printf("\n  Test Mode Statistics\n");
   printf("  %22s: %u\n", "TestModeDroppedTxPkts", Stats->TestModeDroppedTxPkts);
   printf("  %22s: %u\n", "TestModeDroppedLmps", Stats->TestModeDroppedLmps);

   // Error Stats
   printf("\n  General Error Statistics\n");
   printf("  %22s: %u\n", "TimePassedIntrs", Stats->TimePassedIntrs);
   printf("  %22s: %u\n", "NoCommandIntrs", Stats->NoCommandIntrs);
}

static int ReadGlobalDMAStats(int dd){
	tBtHostInterest	HostInt;
	tBRM_Stats  Stats;
	
	ReadHostInterest(dd, &HostInt);
	if(!HostInt.GlobalDmaStats || (HostInt.Version < 0x0100)){
		printf("\nGlobal DMA stats not present\n");
		return FALSE;
	}
	ReadMemoryBlock(dd,HostInt.GlobalDmaStats,(UCHAR *)&Stats,sizeof(tBRM_Stats));
	BRM_DumpStats(&Stats);
	return TRUE;
}

static int ResetGlobalDMAStats(int dd){
	tBtHostInterest	HostInt;
	tBRM_Stats  Stats;
	
	ReadHostInterest(dd, &HostInt);
	if(!HostInt.GlobalDmaStats || (HostInt.Version < 0x0100)){
		printf("\nGlobal DMA stats not present\n");
		return FALSE;
	}
	memset(&Stats,0,sizeof(Stats));
	printf("\nHarry\n");
	WriteMemoryBlock(dd,HostInt.GlobalDmaStats,(UCHAR *)&Stats,sizeof(tBRM_Stats));
	printf("\nDMA stattestics has been reset\n");
	return TRUE;
}

static int ReadTpcTable(int dd){
	tBtHostInterest	HostInt;
	tPsSysCfgTransmitPowerControlTable  TpcTable;
	int i;
	
	ReadHostInterest(dd, &HostInt);
	if(!HostInt.TpcTableAddr || (HostInt.Version < 0x0100)){
		printf("\nTPC table not present\n");
		return FALSE;
	}
	ReadMemoryBlock(dd,HostInt.TpcTableAddr,(UCHAR *)&TpcTable,sizeof(TpcTable));
	for(i=0;i< TpcTable.NumOfEntries; i++){
		printf("Level [%d] represents %3d dBm\n",i,TpcTable.t[i].TxPowerLevel);
	}
	return TRUE;
}

/*
static void dump_conf_data(){
	printf("\nTAG_COUNT %d\n",Tag_Count);
	int i=0,j=0;
	for(i=0;i<Tag_Count;i++){
		printf("\nTAG ID :%X    LEN:%X\n",PsTagEntry[i].TagId,PsTagEntry[i].TagLen);
		for(j=0;j<PsTagEntry[i].TagLen;j++)
			printf("\t %x",PsTagEntry[i].TagData[j]);
	}
	printf("\n");
	printf("\nPATCH_COUNT %d\n",Patch_Count);
	for(i=0;i<Patch_Count;i++){
		printf("\tPATCH LEN:%X\t",RamPatch[i].Len);
		for(j=0;j<RamPatch[i].Len;j++)
			printf("\t %x",RamPatch[i].Data[j]);
	}
	printf("\n");

	printf("\tDYMA LEN:%X\t",RamDynMemOverride.Len);
	for(j=0;j<RamDynMemOverride.Len;j++)
		printf("\t %x",RamDynMemOverride.Data[j]);
	printf("\n");

}
*/
static const char *psreset_help =
	"Usage:\n"
	"\n psreset\n";

static void cmd_psreset(int dev_id, int argc, char **argv){
	int dd,Length =0,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	
	if(argc > 1){
		printf("\n%s\n",psreset_help);	
		return;
	}
	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}
	PSInit(dd);
	memset(&buf,0,sizeof(buf));
	iRet = writeHciCommand(dd, OGF_HOST_CTL,OCF_RESET,Length,buf);
	if(buf[6] != 0){
		printf("\nError: HCI RESET failed due to reason 0x%X\n",buf[6]);
		return;
	}
 
    // Bttest work around for external 32k


    int IsForeverRepeat=0;
    int IsCmdIdle=0;
    int address=0, width = 0, value=0;
    int loop=0,Reg=0;
    do
    {

        address = 0x00020024;
        width = 4;
        buf[0] = (address & 0xFF);
        buf[1] = ((address >>8) & 0xFF);
        buf[2] = ((address>>16) & 0xFF);
        buf[3] = ((address>>24) & 0xFF);
        buf[4] = (UCHAR)width;  //Memory width
        iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_READ_MEMORY, 5, buf);	
        if(buf[6] != 0){
            printf("\nRead Memory address failed due to reason 0x%X\n",buf[6]);
            hci_close_dev(dd);	
            return;
        }
        value = buf[10];
        value = ((value << 8) | buf[9]);
        value = ((value << 8) | buf[8]);
        value = ((value << 8) | buf[7]);


        if(value&0x40000000)
            IsForeverRepeat=1;
        else
            IsForeverRepeat=0;


        address = 0x00020020;
        width = 4;
        buf[0] = (address & 0xFF);
        buf[1] = ((address >>8) & 0xFF);
        buf[2] = ((address>>16) & 0xFF);
        buf[3] = ((address>>24) & 0xFF);
        buf[4] = (UCHAR)width;  //Memory width
        iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_READ_MEMORY, 5, buf);	
        if(buf[6] != 0){
            printf("\nRead Memory address failed due to reason 0x%X\n",buf[6]);
            hci_close_dev(dd);	
            return;
        }
        value = buf[10];
        value = ((value << 8) | buf[9]);
        value = ((value << 8) | buf[8]);
        value = ((value << 8) | buf[7]);

        if((value&0x0000000f)==0x8)
            IsCmdIdle=1;
        else
            IsCmdIdle=0;

    }
    while(!(IsForeverRepeat&IsCmdIdle));//Wait til brm issues forever idle


    address = 0x000200a8;
    width = 4;
    buf[0] = (address & 0xFF);
    buf[1] = ((address >>8) & 0xFF);
    buf[2] = ((address>>16) & 0xFF);
    buf[3] = ((address>>24) & 0xFF);
    buf[4] = (UCHAR)width;  //Memory width
    iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_READ_MEMORY, 5, buf);	
    if(buf[6] != 0){
        printf("\nRead Memory address failed due to reason 0x%X\n",buf[6]);
        hci_close_dev(dd);	
        return;
    }
    value = buf[10];
    value = ((value << 8) | buf[9]);
    value = ((value << 8) | buf[8]);
    value = ((value << 8) | buf[7]);


    value |= 0x1;

    loop = 0;
    while ( loop < 10 ) {
  
            loop++;

	    address = 0x000200a8;
	    width = 4;

	    buf[0] = (address & 0xFF);
	    buf[1] = ((address >>8) & 0xFF);
	    buf[2] = ((address>>16) & 0xFF);
	    buf[3] = ((address>>24) & 0xFF);
	    buf[4] = width;  //Memory width
	    buf[5] = (value & 0xFF);
	    buf[6] = ((value >> 8) & 0xFF);
	    buf[7] = ((value >> 16) & 0xFF);
	    buf[8] = ((value >> 24) & 0xFF);
	    buf[9] =  0xFF;
	    buf[10] = 0xFF;
	    buf[11] = 0xFF;
	    buf[12] = 0xFF;
	    iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_WRITE_MEMORY, 13, buf);	
	    if(buf[6] != 0){
		printf("\nWrite memory address failed\n");
		hci_close_dev(dd);	
		return;
	    }

	address = 0x00004064;
	width = 4;
	buf[0] = (address & 0xFF);
	buf[1] = ((address >>8) & 0xFF);
	buf[2] = ((address>>16) & 0xFF);
	buf[3] = ((address>>24) & 0xFF);
	buf[4] = (UCHAR)width;  //Memory width
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_READ_MEMORY, 5, buf);	
	if(buf[6] != 0){
	    printf("\nRead Memory address failed\n");
	    hci_close_dev(dd);	
	    return;
	}
	Reg = buf[10];
	Reg = ((Reg << 8) | buf[9]);
	Reg = ((Reg << 8) | buf[8]);
	Reg = ((Reg << 8) | buf[7]);

	if(Reg & 0x04) {
           break;
        }
    }

//--------------------------------------------------------------


	hci_close_dev(dd);
	printf("\nReset Done\n");
}
static const char *reset_help =
	"Usage:\n"
	"\n reset\n";

static void cmd_reset(int dev_id, int argc, char **argv){
	int dd,Length =0,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	
	if(argc > 1) {
		printf("\n%s\n",reset_help);	
		return;
	}
	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}
	memset(&buf,0,sizeof(buf));
	iRet = writeHciCommand(dd, OGF_HOST_CTL,OCF_RESET,Length,buf);
	if(buf[6] != 0){
		printf("\nError: HCI RESET failed due to reason 0x%X\n",buf[6]);
		return;
	}
 
	hci_close_dev(dd);
	printf("\nReset Done\n");
}
static const char *rba_help =
	"Usage:\n"
	"\n rba\n";

static void cmd_rba(int dev_id, int argc, char **argv){
	int dd,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	
	if(argc > 1){
		printf("\n%s\n",rba_help);	
		return;
	}
	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	iRet = writeHciCommand(dd, OGF_INFO_PARAM, OCF_READ_BD_ADDR, 0, buf);	
	if(buf[6] != 0){
		printf("\nread bdaddr command failed due to reason 0x%X\n",buf[6] );
		return;
	}
	printf("\nBD ADDRESS: \n");
	int i;
	for(i=iRet-1;i > 7;i--){
		printf("%02X:",buf[i]);
	}
	printf("%X \n\n",buf[7]);
	hci_close_dev(dd);

}

static const char *dtx_help =
	"Usage:\n"
	"\n dtx\n";

static void cmd_dtx(int dev_id, int argc, char **argv){
	int dd,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	
	if(argc > 1){
		printf("\n%s\n",dtx_help);	
		return;
	}


	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}

        memset(&buf,0,HCI_MAX_EVENT_SIZE);
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_DISABLE_TX, 0, buf);	
	if(buf[6] != 0){
		printf("\nDisable TX command failed due to reason 0x%X\n",buf[6]);
		return;
	}
        else {
             printf("\nDisable TX command passed\n");
        }
	hci_close_dev(dd);

}

static const char *ssm_help =
	"Usage:\n"
	"\n ssm [0|1]\n"
	"\nExample:\n"
	"\tssm 0\t(Sleep disabled)\n"
	"\tssm 1\t(Sleep enabled)\n";

static void cmd_ssm(int dev_id, int argc, char **argv){
	int dd,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	
	if(argc != 2){
		printf("\n%s\n",ssm_help);	
		return;
	}

	if(atoi(argv[1]) > 1){
		printf("\nInvalid sleep mode :%d\n",atoi(argv[1]));
		return;
	}


	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}

        memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = atoi(argv[1]);;
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_SLEEP_MODE, 1, buf);	
	if(buf[6] != 0){
		printf("\nSet sleep mode command failed due to reason 0x%X\n",buf[6]);
		return;
	}
        else {
             printf("\nSet sleep mode command passed\n");
        }
	hci_close_dev(dd);

}

static const char *wba_help =
	"Usage:\n"
	"\n wba <bdaddr>\n"
	"\nExample:\n"
	"\n wba 00:03:ff:56:23:89\n";

static void cmd_wba(int dev_id, int argc, char **argv){
	//printf("\nFeature not implemented\n");	
	int dd,iRet;
	bdaddr_t bdaddr;
	
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	if(argc < 2){
		printf("\n%s\n",wba_help);	
		return;
	}

	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}
	str2ba(argv[1],&bdaddr);	
	if((strlen(argv[1]) < 17)||(strlen(argv[1]) > 17)){
		printf("\nInvalid BD address : %s\n",argv[1]);	
		printf("\n%s\n",wba_help);
		hci_close_dev(dd);	
		return;
	}
	LoadPSHeader(buf,PS_WRITE,BD_ADDR_SIZE,BD_ADDR_PSTAG);
	int i,j=0;
	for(i= 0,j=4;i< BD_ADDR_SIZE;i++,j++){
		buf[j] = bdaddr.b[i];
        }
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_PS,BD_ADDR_SIZE + PS_COMMAND_HEADER, buf);
	if(buf[6] != 0){
		printf("\n Write BD address failed due to reason 0x%X\n",buf[6]);
		hci_close_dev(dd);	
		return;
	}
	memset(&buf,0,sizeof(buf));
	iRet = writeHciCommand(dd, OGF_HOST_CTL,OCF_RESET,0,buf);
	if(buf[iRet-1] != 0){
		printf("\nError: HCI RESET failed\n");
		hci_close_dev(dd);	
		return;
	} 
	memset(&buf,0,sizeof(buf));
	iRet = writeHciCommand(dd, OGF_INFO_PARAM, OCF_READ_BD_ADDR, 0, buf);	
	if(buf[6] != 0){
		printf("\nread bdaddr command failed due to reason 0x%X\n",buf[6]);
		hci_close_dev(dd);	
		return;
	}
	printf("\nBD address changed successfully\n");	
	hci_close_dev(dd);	
}

static const char *edutm_help =
	"Usage:\n"
	"\n edutm\n";

static void cmd_edutm(int dev_id, int argc, char **argv){
	int Crc = 0;	
	int dd;
	//UCHAR buf[HCI_MAX_EVENT_SIZE];
	UCHAR ZeroBuf[MEM_BLK_DATA_MAX*2] = {0};	
	if(argc > 1){
		printf("\n%s\n",edutm_help);	
		return;
	}
	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}
/*
	Patch_Count = 20;
	for(i=0; i < Patch_Count; i++){
		RamPatch[i].Len = MAX_BYTE_LENGTH;
		memset(&RamPatch[i].Data,0,MAX_BYTE_LENGTH);
	}
	printf("\nCMD DUT MODE\n");
*/
//When Patch file is present write the patch, if not present just enter DUT mode
	if(!LoadConfFile(TESTPATCH_FILENAME,0xFFFFFFFF,MB_FILEFORMAT_PATCH)){
		if(!Dut(dd)){
			hci_close_dev(dd);	
			return;	
		}
		printf("\nDevice is in test mode ...\n");
		hci_close_dev(dd);
		return;
	}
	//dump_conf_data();
	Crc |= RAM_PATCH_REGION;
	if(!MemBlkwrite(dd,(UINT32)MC_BCAM_COMPARE_ADDRESS, ZeroBuf, HCI_3_PATCH_SPACE_LENGTH_1)){
  		printf("\nError in clearing the patch space 1\n"); 
		return;
   	}

   	if(!MemBlkwrite(dd,(UINT32)MC_BCAM_VALID_ADDRESS, ZeroBuf, HCI_3_PATCH_SPACE_LENGTH_1)){
  		printf("\nError in clearing the patch space 2\n"); 
		return;
   	}
	printf("\nLoading Patch from file :%s\n",TESTPATCH_FILENAME);
	if(!PSOperations(dd,WRITE_PATCH,Patch_Count, NULL)){
		printf("EnterDUT_HCI_3 : patch write failed \r\n");
		return;
	}
	if(!PSOperations(dd,ENABLE_PATCH,0, NULL)){
		printf("EnterDUT_HCI_3 : patch enable failed \r\n");
		return;
	}	
	if(!PSOperations(dd,PS_VERIFY_CRC,Crc, NULL)){
		printf("EnterDUT_HCI_3 : verify crc failed \r\n");
		return;
	}
	if(!Dut(dd)){
		hci_close_dev(dd);	
		return;	
	}
	printf("\nDevice is in test mode ...\n");
	hci_close_dev(dd);	
}


static int ReadMemorySmallBlock(int dd, int StartAddress,UCHAR *pBufToWrite, int Length ){
	int iRet;
   	UCHAR *pData;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	pData = (UCHAR *) malloc(Length + 6);
	memset(pData,0,Length+6);	
	pData[0]= 0x00;  //Memory Read Opcode
	pData[1]= (StartAddress & 0xFF);
	pData[2]= ((StartAddress >> 8) & 0xFF);
	pData[3]= ((StartAddress >> 16) & 0xFF);
	pData[4]= ((StartAddress >> 24) & 0xFF);
	pData[5]= Length;
	
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_MEMOP,Length+6,pData);
	if(pData[6]!= 0){
		printf("\nwrite memory command failed due to reason 0x%X\n",pData[6]);	
		free(pData);
		return FALSE;
	}
	int plen =0;
	do{
		plen = read(dd, buf,HCI_MAX_EVENT_SIZE);
		if (plen < 0) {
			free(pData);
			perror("Read failed");
			exit(EXIT_FAILURE);
		}
	}while (buf[HCI_EVENT_HEADER_SIZE] != DEBUG_EVENT_TYPE_MEMBLK);	
	memcpy(pBufToWrite,(buf+HCI_EVENT_HEADER_SIZE+1),Length);
	free(pData);
	return TRUE;
}

static int ReadMemoryBlock(int dd, int StartAddress,UCHAR *pBufToWrite, int Length ){
			
	int ModResult,i;
	
	if(Length > MEM_BLK_DATA_MAX){
		ModResult = Length % MEM_BLK_DATA_MAX;	
		for(i=0;i < (Length - ModResult);i += MEM_BLK_DATA_MAX) {
			ReadMemorySmallBlock(dd, (StartAddress + i),(pBufToWrite + i), MEM_BLK_DATA_MAX);
		}
		if(ModResult){
			ReadMemorySmallBlock(dd, (StartAddress + i),(pBufToWrite + i), ModResult);
		}
	}
	else{

		ReadMemorySmallBlock(dd, StartAddress, pBufToWrite, Length);
	}
	return TRUE;
}

static int WriteMemorySmallBlock(int dd, int StartAddress,UCHAR *pBufToWrite, int Length ){
	int iRet;
   	UCHAR *pData;
	
	printf("\nStart Address:%x Length:%x  %x\n",StartAddress,Length,MEM_BLK_DATA_MAX);
	/*if(Length <= MEM_BLK_DATA_MAX)
		return FALSE; */
	pData = (UCHAR *) malloc(Length + 6);
	memset(pData,0,Length+6);	
	pData[0]= 0x01;  //Write Read Opcode
	pData[1]= (StartAddress & 0xFF);
	pData[2]= ((StartAddress >> 8) & 0xFF);
	pData[3]= ((StartAddress >> 16) & 0xFF);
	pData[4]= ((StartAddress >> 24) & 0xFF);
	pData[5]= Length;
	
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD,OCF_MEMOP,Length+6,pData);
	if(pData[6]!= 0){
		printf("\nwrite memory command failed due to reason 0x%X\n",pData[6]);	
		free(pData);
		return FALSE;
	}
	free(pData);
	return TRUE;
}


static int WriteMemoryBlock(int dd, int StartAddress,UCHAR *pBufToWrite, int Length ){
			
	int ModResult,i;
	
	if(Length > MEM_BLK_DATA_MAX){
		ModResult = Length % MEM_BLK_DATA_MAX;	
		for(i=0;i < (Length - ModResult);i += MEM_BLK_DATA_MAX) {
			WriteMemorySmallBlock(dd, (StartAddress + i),(pBufToWrite + i), MEM_BLK_DATA_MAX);
		}
		if(ModResult){
			WriteMemorySmallBlock(dd, (StartAddress + i),(pBufToWrite + i), ModResult);
		}
	}
	else{

		WriteMemorySmallBlock(dd, StartAddress, pBufToWrite, Length);
	}
	return TRUE;
}


static int ReadHostInterest(int dd,tBtHostInterest *pHostInt){
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	int iRet;
	int HostInterestAddress;	
	memset(&buf,0,HCI_MAX_EVENT_SIZE);	
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_HOST_INTEREST, 0, buf);	
	if(buf[6] != 0){
		printf("\nhost interest command failed due to reason 0x%X\n",buf[6]);
		return FALSE;
	}
	HostInterestAddress = buf[iRet-1];
	HostInterestAddress = ((HostInterestAddress << 8)|buf[iRet-2]);	
	HostInterestAddress = ((HostInterestAddress << 8)|buf[iRet-3]);	
	HostInterestAddress = ((HostInterestAddress << 8)|buf[iRet-4]);	
	ReadMemoryBlock(dd, HostInterestAddress,(UCHAR*)pHostInt, sizeof(tBtHostInterest));
	
	if(pHostInt->MagicNumber != HI_MAGIC_NUMBER){
		if((pHostInt->MagicNumber != 0xFBAD)|| (pHostInt->Version != 0xDECA))
			return 0;
	}
	return TRUE;
	
}

static int contRxAtGivenChannel(int dd, UCHAR *pString){
	int Address, Mask, Reg, RxFreq,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	//1. Disable all scans and set intervals and scan windows eually
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = 0; //All scan disabled
	iRet = writeHciCommand(dd, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE, 1, buf);	
	if(buf[6] != 0){
		printf("\nWrite scan mode command failed due to reason 0x%X\n",buf[6]);
		return 0;
	}	
	short int inq_scan = 0x1000;		
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = (inq_scan&0xFF);
	buf[1] = ((inq_scan >> 8)& 0xFF);
	buf[2] = (inq_scan&0xFF);
	buf[3] = ((inq_scan >> 8)& 0xFF);
	iRet = writeHciCommand(dd, OGF_HOST_CTL, OCF_WRITE_INQ_ACTIVITY, 4, buf);	
	if(buf[6] != 0){
		printf("\nWrite inquiry scan activity command failed due to reason 0x%X\n",buf[6]);
		return 0;
	}	
	
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = (inq_scan&0xFF);
	buf[1] = ((inq_scan >> 8)& 0xFF);
	buf[2] = (inq_scan&0xFF);
	buf[3] = ((inq_scan >> 8)& 0xFF);
	iRet = writeHciCommand(dd, OGF_HOST_CTL, OCF_WRITE_PAGE_ACTIVITY, 4, buf);	
	if(buf[6] != 0){
		printf("\nWrite page scan activity command failed due to reason 0x%X\n",buf[6]);
		return 0;
	}
	//2. Disbable AGC
	Address = LC_JTAG_MODEM_REGS_ADDRESS + AGC_BYPASS_ADDRESS;
	Mask = AGC_BYPASS_ENABLE_MASK;
	Reg = AGC_BYPASS_ENABLE_SET(1);
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = (Address & 0xFF);
	buf[1] = ((Address >>8) & 0xFF);
	buf[2] = ((Address>>16) & 0xFF);
	buf[3] = ((Address>>24) & 0xFF);
	buf[4] = 0x04;  //Memory width
	buf[5] = (Reg & 0xFF);
	buf[6] = ((Reg >> 8) & 0xFF);
	buf[7] = ((Reg >> 16) & 0xFF);
	buf[8] = ((Reg >> 24) & 0xFF);
	buf[9] = (Mask & 0xFF);
	buf[10] = ((Mask >>8) & 0xFF);
	buf[11] = ((Mask>>16) & 0xFF);
	buf[12] = ((Mask>>24) & 0xFF);
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_WRITE_MEMORY, 13, buf);	
	if(buf[6] != 0){
		printf("\nWrite to AGC bypass register failed due to reason 0x%X\n",buf[6]);
		return 0;
	}
	// 3. Disable frequency hoping and set rx frequency

	RxFreq = (int)pString;
	Address = LC_DEV_PARAM_CTL_ADDRESS;
   	Mask = 	LC_DEV_PARAM_CTL_FREQ_HOP_EN_MASK |
          	LC_DEV_PARAM_CTL_RX_FREQ_MASK     |
          	LC_DEV_PARAM_CTL_WHITEN_EN_MASK;
	Reg = LC_DEV_PARAM_CTL_RX_FREQ_SET(RxFreq);
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = (Address & 0xFF);
	buf[1] = ((Address >>8) & 0xFF);
	buf[2] = ((Address>>16) & 0xFF);
	buf[3] = ((Address>>24) & 0xFF);
	buf[4] = 0x04;  //Memory width
	buf[5] = (Reg & 0xFF);
	buf[6] = ((Reg >> 8) & 0xFF);
	buf[7] = ((Reg >> 16) & 0xFF);
	buf[8] = ((Reg >> 24) & 0xFF);
	buf[9] = (Mask & 0xFF);
	buf[10] = ((Mask >>8) & 0xFF);
	buf[11] = ((Mask>>16) & 0xFF);
	buf[12] = ((Mask>>24) & 0xFF);
	
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_WRITE_MEMORY, 13, buf);	
	if(buf[6] != 0){
		printf("\nWrite to Rx Freq register failed due to reason 0x%X\n",buf[6]);
		return 0;
	}
	// 4. Enable page scan only (Note: the old way puts device into inq scan mode only ???)
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = 2; //Page scan enabled
	iRet = writeHciCommand(dd, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE, 1, buf);	
	if(buf[6] != 0){
		printf("\nPage scan enable command failed due to reason 0x%X\n",buf[6]);
		return 0;
	}
	// 5. Increase correlator
	Address = LC_JTAG_MODEM_REGS_ADDRESS + CORR_PARAM1_ADDRESS;
	Mask = CORR_PARAM1_TIM_THR_MASK;
	Reg = CORR_PARAM1_TIM_THR_SET(0x3f);
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = (Address & 0xFF);
	buf[1] = ((Address >>8) & 0xFF);
	buf[2] = ((Address>>16) & 0xFF);
	buf[3] = ((Address>>24) & 0xFF);
	buf[4] = 0x04;  //Memory width
	buf[5] = (Reg & 0xFF);
	buf[6] = ((Reg >> 8) & 0xFF);
	buf[7] = ((Reg >> 16) & 0xFF);
	buf[8] = ((Reg >> 24) & 0xFF);
	buf[9] = (Mask & 0xFF);
	buf[10] = ((Mask >>8) & 0xFF);
	buf[11] = ((Mask>>16) & 0xFF);
	buf[12] = ((Mask>>24) & 0xFF);
	
	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_WRITE_MEMORY, 12, buf);	
	if(buf[6] != 0){
		printf("\nWrite to Correlator register failed due to reason 0x%X\n",buf[6]);
		return 0;
	}

	return TRUE;
}
static const char *cwrx_help =
	"Usage:\n"
	"\n cwrx <Channel>\n";

static void cmd_cwrx(int dev_id, int argc, char **argv){

	int dd,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	UCHAR channel;
	BOOL Ok = TRUE;
	if(argc != 2){
		printf("\n%s\n",cwrx_help);	
		return;
	}
	
	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	channel = atoi(argv[1]);
	if(channel > 78 || channel < 0){
		printf("\nPlease enter channel 0-78!\n");
		return;	
	}

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}

	// Disable sleep mode
	memset(&buf,0,sizeof(buf));
	buf[0] = 0;
	iRet = writeHciCommand(dd,OGF_VENDOR_CMD,OCF_SLEEP_MODE,1,buf);
	if(buf[6] != 0){
		printf("\nError: Sleep mode failed due to reason 0x%X\n",buf[6]);
		Ok = 0;
	}
       	printf (" Continuoux Rx at channel %d\n",channel);
	Ok = contRxAtGivenChannel(dd, &channel);

         // All modes come here
	if (Ok)
       	{
       		printf (" Continuoux Rx at channel %d Done...\n",channel);
       	}
       	else
       	{
        	printf ("\nERROR ---> Could not enter continuous Rx mode\n");
  	}

	hci_close_dev(dd);
}
static const char *mb_help =
	"Usage:\n"
	"\n mb\n";

static void cmd_mb(int dev_id, int argc, char **argv){

	int newfd, FieldNum,dd,iRet,need_raw, iDataSize, address,width,value,mask, fdmax, k, l;
	bdaddr_t bdaddr;
	tBRM_Control_packet MasterBlaster;
	UCHAR	SkipRxSlot; 	
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	char FieldAlias;
	BOOL TestEnabled = 0,Ok = TRUE;
	UINT8 setContTxType;
	tBtHostInterest	HostInt;
	fd_set master, read_fds;
    uint32_t m_BerTotalBits, m_BerGoodBits;
    uint8_t m_pattern[16];
    uint16_t m_pPatternlength;
    struct hci_filter flt;
    struct hci_dev_info di;
    struct timeval timeout;

	if(argc > 1) {
		printf("\n%s\n",mb_help);	
		return;
	}
	
	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	iRet = writeHciCommand(dd, OGF_INFO_PARAM, OCF_READ_BD_ADDR, 0, buf);	
	if (buf[6] != 0) {
		printf("\nread bdaddr command failed due to reason 0x%X\n",buf[6]);
		hci_close_dev(dd);
		return;
	}
	int i,j;
	char bda[18];
	for (i=iRet-1,j=0;i>7;i--,j+=3) {
		sprintf(&bda[j],"%X",((buf[i]>>4)&0xFF));
		sprintf(&bda[j+1],"%X",(buf[i]&0x0F));
		sprintf(&bda[j+2],":");
	}
	sprintf(&bda[15],"%X",((buf[7]>>4)&0xFF));
	sprintf(&bda[16],"%X",(buf[7]&0x0F));
	bda[18] ='\0';
	str2ba(bda,&bdaddr);
	
	InitMasterBlaster(&MasterBlaster, &bdaddr, &SkipRxSlot);
#ifndef DUMP_DEBUG
	Ok = ReadHostInterest(dd,&HostInt);	
	if(Ok) {
		if (HostInt.TpcTableAddr && (HostInt.Version >= 0x0100)) {	
         		Ok = ReadMemoryBlock(dd, HostInt.TpcTableAddr, (UCHAR *)&TpcTable, sizeof(TpcTable));
	         	MasterBlaster.testCtrl.Power = TpcTable.NumOfEntries - 1;
      		}
   	}
   	if(!Ok) {
      		printf ("\nCould not load TPC table.\n");
		sleep (2);
      		Ok = TRUE;
   	}
#endif

	PrintMasterBlasterMenu (&MasterBlaster);
    m_BerGoodBits = 0;
    m_BerTotalBits = 0;
    m_pattern[0] = 0x0f;
    m_pPatternlength = 1;

	FD_ZERO(&master);
	FD_ZERO(&read_fds);
	FD_SET(0, &master);
	fdmax = 0;
	newfd = -1;

   while (1) {
        read_fds = master;
        timeout.tv_sec = 5;
        timeout.tv_usec = 0;
        iRet = select(fdmax+1, &read_fds, NULL, NULL, &timeout);
        if (iRet == -1) {
            perror("cmd_mb select() error!");
            goto exits_mb;
        }
        if (iRet == 0) continue;

        for(i = 0; i <= fdmax; i++) {
	    if(FD_ISSET(i, &read_fds)) {
	    	if (i==0) {	// input 
		    scanf("%s",buf);
		    FieldAlias = (char)buf[0];
		    FieldNum = CheckField(MasterBlaster, &FieldAlias);
		    if (FieldNum == INVALID_MASTERBLASTER_FIELD) {
			printf ("\nERROR ---> Invalid command. Try again.\n");
		        printf ("mb>");
			continue;
      		    }

		    if (!strncmp(&FieldAlias, MasterBlasterMenu[EXX].Alias, 1)) {
			printf("\nExit the Master Blaster Mode without reset\n");
			goto exits_mb;
		    }

	            // if the test is in rx and the key is neither 'd' nor 'g', then stop the test, renew the option, and procced
		    // if the test is in tx and the key is not 'e', then stop the test, renew the option, and procced
		    // if the test is in (LE) continuous rx/tx and the key is not 'j' , then stop the test, renew the option, and procced
		    if (((TestEnabled == MB_RX_TEST) && strncmp(&FieldAlias, MasterBlasterMenu[RX].Alias, 1) && strncmp(&FieldAlias, MasterBlasterMenu[GB].Alias, 1))
		    	|| ((TestEnabled == MB_TX_TEST) && strncmp(&FieldAlias, MasterBlasterMenu[TX].Alias, 1))
		    	|| ((TestEnabled == MB_CONT_RX_TEST || TestEnabled == MB_CONT_TX_TEST
		        || TestEnabled == MB_LE_RX_TEST || TestEnabled == MB_LE_TX_TEST) &&
		       	(strncmp(&FieldAlias, MasterBlasterMenu[EN].Alias, 1)))) {
			printf (" ... Please wait ...");
         		if (MasterBlaster.ContTxMode) {
		        	memset(&buf,0,HCI_MAX_EVENT_SIZE);
				buf[0] = 255;
				iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_CONT_TX_TESTER, 14, buf);	
				if(buf[6] != 0) {
					printf("\nContinious Tx Tester command failed due to reason 0x%X\n",buf[6]);
					Ok = 0;
				} else
					Ok = TRUE;
         		}
			if (TestEnabled == MB_RX_TEST) {
                       	    if (need_raw) {
                            	if (ioctl(dd, HCISETRAW, 0) < 0)
                                    perror("Can't clear raw mode \n");
			    }
                       	    FD_CLR(newfd, &master);
                            newfd = -1;
			    fdmax = 0;
			}
		        // Ok = Diag::Reset (Unit, "");
			//PSInit(dd);
			// The default setting is sleep mode enabled.
                        memset(&buf,0,sizeof(buf));
                        buf[0] = 1;
                        iRet = writeHciCommand(dd,OGF_VENDOR_CMD,OCF_SLEEP_MODE,1,buf);
                        if(buf[6] != 0) {
                                printf("\nError: Sleep mode failed due to reason 0x%X\n",buf[6]);
                        }

			memset(&buf,0,sizeof(buf));
			iRet = writeHciCommand(dd,OGF_HOST_CTL,OCF_RESET,0,buf);
			if (buf[6] != 0) {
				printf("\nError: HCI RESET failed due to reason 0x%X\n",buf[6]);
				Ok = FALSE;
			} else
				Ok = TRUE;
         		if (!Ok) {
		            printf ("\nERROR ---> Could not stop test mode\n");
         		} else if (!strncmp(&FieldAlias, MasterBlasterMenu[RX].Alias, 1)
				|| !strncmp(&FieldAlias, MasterBlasterMenu[TX].Alias, 1)
				|| ((TestEnabled != MB_NO_TEST) &&
				(!strncmp(&FieldAlias, MasterBlasterMenu[CR].Alias, 1) ||
				 !strncmp(&FieldAlias, MasterBlasterMenu[CT].Alias, 1) ||
				 !strncmp(&FieldAlias, MasterBlasterMenu[LR].Alias, 1) ||
				 !strncmp(&FieldAlias, MasterBlasterMenu[LT].Alias, 1))) ||
				 !strncmp(&FieldAlias, MasterBlasterMenu[EN].Alias, 1)) {
				TestEnabled = MB_NO_TEST;
			}
	      		sleep(1);
		    } 
		    if (!strncmp(&FieldAlias,MasterBlasterMenu[EX].Alias,1)) {			// Exit
		        TestEnabled = MB_NO_TEST;
		        printf ("\n Exit ..\n");
		    	goto exits_mb;
      		    } else if (!strncmp(&FieldAlias,MasterBlasterMenu[ST].Alias,1)) {		// Stop Test
			TestEnabled = MB_NO_TEST;
		        PrintMasterBlasterMenu (&MasterBlaster);
			continue;
		    } else if (!strncmp(&FieldAlias,MasterBlasterMenu[GB].Alias,1)) {		// get BER
		        printf("\n\tGoodBits %d, total is %d\n", m_BerGoodBits, m_BerTotalBits);
		        printf("mb>\n");
		        continue;
		    } else if (!strncmp(&FieldAlias,MasterBlasterMenu[PO].Alias,1)) {		// set Power
			MasterBlasterMenu[FieldNum].pFunc (&MasterBlaster, &FieldAlias);
      		    } else if (!MasterBlasterMenu[FieldNum].pFunc (&MasterBlaster, MasterBlasterMenu[FieldNum].Options)) {
			printf ("\nERROR ---> Invalid option. Try again.\n");
		        printf ("mb>");
		        continue;
      		    }
		    PrintMasterBlasterMenu(&MasterBlaster);

                // Enable RX test mode
                    if ((!strncmp(&FieldAlias, MasterBlasterMenu[RX].Alias, 1) && (TestEnabled == MB_NO_TEST))		
                    	|| (strncmp(&FieldAlias, MasterBlasterMenu[RX].Alias, 1) && (TestEnabled == MB_RX_TEST))) {
                        Ok = Dut(dd);
                        if (!Ok) {
                                printf("\nERROR ---> Could not enter DUT mode\n");
                                printf("mb>");
                                continue;
                        }

                        printf(".");
    			if (hci_devinfo(dev_id, &di) < 0) {
        			printf("Can't get device info\n");
				printf("mb>");
				continue;
			}

			need_raw = !hci_test_bit(HCI_RAW, &di.flags);

                        memset(&buf,0,HCI_MAX_EVENT_SIZE);
                        buf[0] = eBRM_TestMode_Rx;
                        buf[1] = MasterBlaster.testCtrl.Packet;
                        buf[2] = MasterBlaster.testCtrl.DataLen & 0xFF;
                        buf[3] = ((MasterBlaster.testCtrl.DataLen>>8) & 0xFF);
                        buf[4] = MasterBlaster.testCtrl.HopMode;
                        buf[5] = MasterBlaster.testCtrl.TxFreq;
                        buf[6] = MasterBlaster.testCtrl.Power;
                        buf[7] = MasterBlaster.testCtrl.RxFreq;
                        buf[8] = MasterBlaster.bdaddr[0];
                        buf[9] = MasterBlaster.bdaddr[1];
                        buf[10] = MasterBlaster.bdaddr[2];
                        buf[11] = MasterBlaster.bdaddr[3];
                        buf[12] = MasterBlaster.bdaddr[4];
                        buf[13] = MasterBlaster.bdaddr[5];
                        buf[14] = SkipRxSlot;
                        iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_RX_TESTER, 15, buf);
                        if (buf[6] != 0) {
                                printf("\nRx Tester command failed due to reason 0x%X\n",buf[6]);
                                printf("\nERROR --> Could not enable master blaster mode\n");
                                TestEnabled = MB_NO_TEST;
                                Ok = 0;
                        } else {
                            printf(" rx test is in progress. Press 's' to stop the test\n");
                            TestEnabled = MB_RX_TEST;
			    hci_filter_clear(&flt);
                            hci_filter_all_ptypes(&flt);
                            hci_filter_all_events(&flt);

                            if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
                                printf("Can't set filter for hci\n");
                                printf("mb>");
                                continue;
                            }

                            if (need_raw) {
                                if (ioctl(dd, HCISETRAW, 1) < 0) {
                                    printf("Can't set raw mode on hci\n");
                                    printf("mb>");
				    continue;
                            	}
                            }
                            newfd = dd;
			    FD_SET(newfd, &master);
                            fdmax = dd;
                        }
                        printf("mb>");
                        continue;
                    } else if ((!strncmp(&FieldAlias, MasterBlasterMenu[RX].Alias, 1)) && (TestEnabled == MB_RX_TEST)) {
                        printf(" rx test is in progress. Press 's' to stop the test\n");
                        printf("mb>");
                        continue;
                    }

      		// Enable TX test mode
		    if ((!strncmp(&FieldAlias, MasterBlasterMenu[TX].Alias, 1) && (TestEnabled == MB_NO_TEST))
		    	|| (strncmp(&FieldAlias, MasterBlasterMenu[TX].Alias, 1) && (TestEnabled == MB_TX_TEST))) {
		        // Disable sleep mode
	        	printf (".");
			Ok = TRUE;
			memset(&buf,0,sizeof(buf));
			buf[0] = 0;
			iRet = writeHciCommand(dd,OGF_VENDOR_CMD,OCF_SLEEP_MODE,1,buf);
			if(buf[6] != 0) {
				printf("\nError: Sleep mode failed due to reason 0x%X\n",buf[6]);
				Ok = 0;
			}
	
			if (!Ok) {
            			printf ("\nERROR ---> Could not disable sleep mode\n");
			        printf ("mb>");
        	    		continue;
         		}

	        	printf (".");
			Ok = Dut(dd);
			if (!Ok) {
				printf("\nERROR ---> Could not enter DUT mode\n");
				printf("mb>");
				continue;
			}

			printf(".");
			memset(&buf,0,HCI_MAX_EVENT_SIZE);
			buf[0] = MasterBlaster.testCtrl.Mode ;
			buf[1] = MasterBlaster.testCtrl.Packet;
			buf[2] = MasterBlaster.testCtrl.DataLen & 0xFF; 
			buf[3] = ((MasterBlaster.testCtrl.DataLen>>8) & 0xFF); 
			buf[4] = MasterBlaster.testCtrl.HopMode;
			buf[5] = MasterBlaster.testCtrl.TxFreq;
			buf[6] = MasterBlaster.testCtrl.Power;
			buf[7] = MasterBlaster.testCtrl.RxFreq;
			buf[8] = MasterBlaster.bdaddr[0]; 
			buf[9] = MasterBlaster.bdaddr[1]; 
			buf[10] = MasterBlaster.bdaddr[2]; 
			buf[11] = MasterBlaster.bdaddr[3]; 
			buf[12] = MasterBlaster.bdaddr[4]; 
			buf[13] = MasterBlaster.bdaddr[5]; 
			buf[14] = SkipRxSlot;
			iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_TX_TESTER, 15, buf);	
			if (buf[6] != 0) {
				printf("\nTx Tester command failed due to reason 0x%X\n",buf[6]);
				printf("\nERROR --> Could not enable master blaster mode\n");
				TestEnabled = MB_NO_TEST;
				Ok = 0;
			} else {
				printf(" tx test is in progress. Press 's' to stop the test\n");
				TestEnabled = MB_TX_TEST;
			}
			printf("mb>");
			continue;
		    } else if ((!strncmp(&FieldAlias, MasterBlasterMenu[TX].Alias, 1)) && TestEnabled == MB_TX_TEST) {
			printf(" tx test is in progress. Press 's' to stop the test\n");
			printf("mb>");
			continue;
		    }

		/* Enable (LE) continuous tx/rx test modes */
		    if (((!strncmp(&FieldAlias, MasterBlasterMenu[EN].Alias, 1)) && (TestEnabled == MB_NO_TEST))
		    	|| (strncmp(&FieldAlias, MasterBlasterMenu[EN].Alias, 1) && (TestEnabled == MB_CONT_RX_TEST
			|| TestEnabled == MB_CONT_TX_TEST || TestEnabled == MB_LE_RX_TEST || TestEnabled == MB_LE_TX_TEST))) {
		        // Disable sleep mode
	        	printf (".");
			Ok = TRUE;
			memset(&buf,0,sizeof(buf));
			buf[0] = 0;
			iRet = writeHciCommand(dd,OGF_VENDOR_CMD,OCF_SLEEP_MODE,1,buf);
			if (buf[6] != 0) {
				printf("\nError: Sleep mode failed due to reason 0x%X\n",buf[6]);
				Ok = 0;
			}
	
			if (!Ok) {
            			printf ("\nERROR ---> Could not disable sleep mode\n");
			        printf ("mb>");
        	    		continue;
         		}

			/*  LE Rx Mode */
			if (MasterBlaster.LERxMode == ENABLE) {
				Ok = SU_LERxTest(dd, MasterBlaster.testCtrl.RxFreq);
				TestEnabled = MB_LE_RX_TEST;
			} else if (MasterBlaster.LETxMode == ENABLE) {
				Ok = SU_LETxTest(dd, MasterBlaster.testCtrl.TxFreq, MasterBlaster.testCtrl.DataLen,
						MasterBlaster.LETxParms.PktPayload);
				TestEnabled = MB_LE_TX_TEST;
			} else if (MasterBlaster.ContRxMode == ENABLE) {
				UCHAR RxFreq = MasterBlaster.testCtrl.RxFreq;
				Ok = contRxAtGivenChannel(dd, &RxFreq);
				TestEnabled = MB_CONT_RX_TEST;
			} else /* Continous TX mode */ {
				printf(".");
				Ok = Dut(dd);
				if (!Ok) {
					printf("\nERROR ---> Could not enter DUT mode\n");
					printf("mb>");
					continue;
				}
				/* Enable master blaster mode */
				printf(".");
				if (CW_Single_Tone == MasterBlaster.ContTxType)
					setContTxType = Cont_Tx_Raw_1MHz;
				else
					setContTxType = MasterBlaster.ContTxType;
					
				memset(&buf, 0, HCI_MAX_EVENT_SIZE);
				buf[0] = MasterBlaster.testCtrl.Mode ;
				buf[1] = MasterBlaster.testCtrl.Packet;
				buf[2] = MasterBlaster.testCtrl.DataLen & 0xFF; 
				buf[3] = ((MasterBlaster.testCtrl.DataLen>>8) & 0xFF); 
				buf[4] = MasterBlaster.ContTxType;
				buf[5] = MasterBlaster.testCtrl.TxFreq;
				buf[6] = MasterBlaster.testCtrl.Power;
				buf[7] = MasterBlaster.testCtrl.RxFreq;
				buf[8] = MasterBlaster.bdaddr[0]; 
				buf[9] = MasterBlaster.bdaddr[1]; 
				buf[10] = MasterBlaster.bdaddr[2]; 
				buf[11] = MasterBlaster.bdaddr[3]; 
				buf[12] = MasterBlaster.bdaddr[4]; 
				buf[13] = MasterBlaster.bdaddr[5]; 
				iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_CONT_TX_TESTER, 14, buf);	
				if(buf[6] != 0){
					printf("\nContinious Tx Tester command failed due to reason 0x%X\n",buf[6]);
					Ok = FALSE;
				} else
					Ok = TRUE;

                                memset(&buf,0,HCI_MAX_EVENT_SIZE);
                        	address = 0x00022914;
                        	value = 0x00200000;
                        	mask = 0x00200000;
                    		width = 4;
                        	buf[0] = (address & 0xFF);
                        	buf[1] = ((address >>8) & 0xFF);
                        	buf[2] = ((address>>16) & 0xFF);
                        	buf[3] = ((address>>24) & 0xFF);
                        	buf[4] = width;  //Memory width
                        	buf[5] = (value & 0xFF);
                        	buf[6] = ((value >> 8) & 0xFF);
                        	buf[7] = ((value >> 16) & 0xFF);
                        	buf[8] = ((value >> 24) & 0xFF);
                        	buf[9] = (mask & 0xFF);
                        	buf[10] = ((mask >>8) & 0xFF);
                        	buf[11] = ((mask>>16) & 0xFF);
                        	buf[12] = ((mask>>24) & 0xFF);
                        	iRet = writeHciCommand(dd, OGF_VENDOR_CMD, OCF_WRITE_MEMORY, 13, buf);	
                        	if(buf[6] != 0){
                        		printf("\nWrite memory address failed due to reason 0x%X\n",buf[6]);
                        		Ok = FALSE;
				} else 
					Ok = TRUE;

				TestEnabled = MB_CONT_TX_TEST;
			}
			if (Ok) {
				printf("Test is in progress. Press 's' to stop the test\n");
			} else {
				printf("\nERROR ---> Could not enable master blaster mode\n");
				TestEnabled = MB_NO_TEST;
			}
			printf("mb>");
			continue;
      		    } else if ((!strncmp(&FieldAlias, MasterBlasterMenu[EN].Alias, 1)) && TestEnabled) {
		        printf (" Test mode is in progress. Press 's' to stop the test\n");
         		printf ("mb>");
         		continue;
      		    }
	        }
	    	else if (i == newfd) {
                    iRet = read(i, buf, sizeof(buf));
                    iDataSize = iRet - 5;
//		    printf("first:%x,nbyte:%d, packet:%d, pattern:%x\n",buf[0], iRet, (uint16_t)(buf[3] | (buf[4] << 8)), buf[5]);
                    if (buf[0] == 0x2) {        // ACL data
                            m_BerTotalBits = m_BerTotalBits + iDataSize * 8;
                            for(j=0,l=0;j<iDataSize;j++,l++) {
                                if (l == m_pPatternlength) l = 0;
                                for(k=0;k<8;k++){
                                    if((m_pattern[l]&(1<<k)) == (buf[8+j]&(1<<k)))
                                    m_BerGoodBits++;
                                }
                            }
                    }
	    	}
	    }
	}
   }
exits_mb:
        if (need_raw) {
                if (ioctl(dd, HCISETRAW, 0) < 0)
                        perror("Can't clear raw mode \n");
        }
	hci_close_dev(dd);
}
/*
static void cmd_gid(int dev_id, int argc, char **argv){
	printf("\nFeature not implemented\n");
}
*/
static const char *wsm_help =
	"Usage:\n"
	"\n wsm [0|1|2|3]\n"
	"\nExample:\n"
	"\twsm 0\t(Scan disabled)\n"
	"\twsm 1\t(Inquiry scan enabled)\n"
	"\twsm 2\t(Page scan enabled)\n"
	"\twsm 3\t(Inquiry and Page scan enabled)\n";

static void cmd_wsm(int dev_id, int argc, char **argv){
	int dd,iRet;
	UCHAR buf[HCI_MAX_EVENT_SIZE];
	if(argc < 2){
		printf("\n%s\n",wsm_help);	
		return;
	}
	if(atoi(argv[1]) > 3){
		printf("\nInvalid scan mode :%d\n",atoi(argv[1]));
		return;
	}

	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}
	memset(&buf,0,HCI_MAX_EVENT_SIZE);
	buf[0] = atoi(argv[1]);
	iRet = writeHciCommand(dd, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE, 1, buf);	
	if(buf[6] != 0){
		printf("\nWrite scan mode command failed due to reason 0x%X\n",buf[6]);
		return;
	}
	hci_close_dev(dd);
	printf("\nScan Mode set to :%d\n",atoi(argv[1]));
}

static void dumpHex(UCHAR *buf, int length, int col)
{
	int i;
	for (i = 0; i < length; i++) {
		printf("0x%02x ", buf[i]);
		if (((i+1) % col) == 0 && i != 0)
			printf("\n");
	}
	if (((i+1) % col) != 0) printf("\n");
}

static void ReverseHexString(char *pStr)
{
	int i, j;
	char temp;
	int len = strlen(pStr);

	for (i = 0; pStr[i] == ' ' || pStr[i] == '\t'; i++);

	if (pStr[i] == '0' && pStr[i+1] == 'x')
		i += 2;
	
	for (j = len - 1; i < j - 2; i += 2, j -= 2) {
		temp = pStr[i];
		pStr[i] = pStr[j - 1];
		pStr[j - 1] = temp;
		temp = pStr[i + 1];
		pStr[i + 1] = pStr[j];
		pStr[j] = temp;
	}
}

static void GetByteSeq(UCHAR *pDst, UCHAR *pSrc, int Size)
{
	UCHAR LowNibble, Nibble = 0;
	UCHAR *pLastHex;
	UCHAR *pStr = pSrc;

	while (*pStr == ' ' || *pStr == '\t') pStr++;

	if ((pStr[0] == '0') && (pStr[1] == 'x'))
		pStr += 2;

	pLastHex = pStr - 1;
	while (IS_HEX(*(pLastHex + 1)))
		pLastHex++;

	LowNibble = 0;

	while (Size > 0) {
		if (pStr <= pLastHex) {
			Nibble = CONV_HEX_DIGIT_TO_VALUE(*pStr);
			pStr++;
		} else {
			Nibble = 0;
		}

		if (LowNibble) {
			*pDst |= (UCHAR)(Nibble & 0x0F);
			LowNibble = 0;
			pDst++;
			Size--;
		} else {
			*pDst = (UCHAR)((Nibble << 4) & 0xF0);
			LowNibble = 1;
		}
	}
}

unsigned int GetUInt(char **ppLine, unsigned int DefaultValue)
{
  char *pStr = *ppLine;
  unsigned int Value = 0;

  // Is it a hex value?
  if ((*pStr == '0') && (*(pStr+1) == 'x'))
  {
    // We have a hex value

    pStr += 2;

    while (IS_HEX(*pStr))
    {
      Value = CONV_HEX_DIGIT_TO_VALUE(*pStr) + (Value*16);
      pStr++;
    }

  }
  else if (IS_DIGIT(*pStr))
  {
    // We have a decimal value
    while (IS_DIGIT(*pStr))
    {
      Value = CONV_DEC_DIGIT_TO_VALUE(*pStr) + (Value*10);
      pStr++;
    }
  }
  else
  {
    // We don't have a value at all - return default value
    return DefaultValue;
  }

  // Update the BtString ptr
  *ppLine = pStr;
  return Value;
}

static const char *mbr_help =
	"Usage:\n"
	"\n mbr <address> <length> \n"
	"\n Example \n"
	"\n mbr 0x00004FFC 10 \n"
	"\n mbr 0x00004FFC 0x10 \n";

static void cmd_mbr(int dev_id, int argc, char **argv){
	int dd;
	UCHAR buf[HCI_MAX_EVENT_SIZE*20];

	if(argc != 3){
		printf("\n%s\n",mbr_help);	
		return;
	}
	if (dev_id < 0)
		dev_id = hci_get_route(NULL);

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("\nERROR: Can not open HCI device\n");
		return;
	}

        int length =