#include "os_config.h"

#include "c_def.h"
#include "debug.h"

#include "regmap.h"

#include "hw_timer.h"
#include "hw_dma.h" 

//#include "types.h"
//#include "prj.h"
//#include "gpio.h"
//#include "test.h"

#include "interrupt.h"
#include "app_uart.h" 
#include "flash_boot.h" 

/// Features definitions
#define FEAT_3_SLOT_BIT_POS           0
#define FEAT_5_SLOT_BIT_POS           1
#define FEAT_ENC_BIT_POS              2
#define FEAT_SLOT_OFFSET_BIT_POS      3
#define FEAT_TIMING_ACC_BIT_POS       4
#define FEAT_SWITCH_BIT_POS           5
#define FEAT_HOLD_BIT_POS             6
#define FEAT_SNIFF_BIT_POS            7

#define FEAT_PARK_BIT_POS             8
#define FEAT_RSSI_BIT_POS             9
#define FEAT_QUALITY_BIT_POS          10
#define FEAT_SCO_BIT_POS              11
#define FEAT_HV2_BIT_POS              12
#define FEAT_HV3_BIT_POS              13
#define FEAT_ULAW_BIT_POS             14
#define FEAT_ALAW_BIT_POS             15

#define FEAT_CVSD_BIT_POS             16
#define FEAT_PAGING_BIT_POS           17
#define FEAT_POWER_BIT_POS            18
#define FEAT_TRANSP_SCO_BIT_POS       19
#define FEAT_BCAST_ENCRYPT_BIT_POS    23

#define FEAT_EDR_2MB_BIT_POS          25
#define FEAT_EDR_3MB_BIT_POS          26
#define FEAT_ENH_INQSCAN_BIT_POS      27
#define FEAT_INT_INQSCAN_BIT_POS      28
#define FEAT_INT_PAGESCAN_BIT_POS     29
#define FEAT_RSSI_INQRES_BIT_POS      30
#define FEAT_EV3_BIT_POS              31

#define FEAT_EV4_BIT_POS              32
#define FEAT_EV5_BIT_POS              33
#define FEAT_AFH_CAPABLE_S_BIT_POS    35
#define FEAT_AFH_CLASS_S_BIT_POS      36
#define FEAT_BR_EDR_NO_SUPP_BIT_POS   37
#define FEAT_LE_BIT_POS               38
#define FEAT_3_SLOT_EDR_BIT_POS       39
#define FEAT_5_SLOT_EDR_BIT_POS       40
#define FEAT_SNIFF_SUBRAT_BIT_POS     41
#define FEAT_PAUSE_ENCRYPT_BIT_POS    42
#define FEAT_AFH_CAPABLE_M_BIT_POS    43
#define FEAT_AFH_CLASS_M_BIT_POS      44
#define FEAT_EDR_ESCO_2MB_BIT_POS     45
#define FEAT_EDR_ESCO_3MB_BIT_POS     46
#define FEAT_3_SLOT_EDR_ESCO_BIT_POS  47
#define FEAT_EIR_BIT_POS              48
#define FEAT_LE_BR_EDR_BIT_POS        49
#define FEAT_SSP_BIT_POS              51
#define FEAT_ENCAP_PDU_BIT_POS        52
#define FEAT_ERRO_DATA_REP_BIT_POS    53
#define FEAT_NFLUSH_PBF_BIT_POS       54
#define FEAT_LSTO_CHG_EVT_BIT_POS     56
#define FEAT_INQ_TXPWR_BIT_POS        57
#define FEAT_EPC_BIT_POS              58
#define FEAT_EXT_FEATS_BIT_POS        63
#define FEAT_SSP_HOST_BIT_POS         64
#define FEAT_LE_HOST_BIT_POS          65
#define FEAT_LE_BR_EDR_HOST_BIT_POS   66
#define FEAT_SEC_CON_HOST_BIT_POS     67

#define FEAT_CSB_MASTER_BIT_POS        128
#define FEAT_CSB_SLAVE_BIT_POS         129
#define FEAT_SYNC_TRAIN_BIT_POS        130
#define FEAT_SYNC_SCAN_BIT_POS         131
#define FEAT_INQ_RES_NOTIF_EVT_BIT_POS 132
#define FEAT_GEN_INTERL_SCAN_BIT_POS   133
#define FEAT_COARSE_CLK_ADJ_BIT_POS    134
#define FEAT_SEC_CON_CTRL_BIT_POS      136
#define FEAT_PING_BIT_POS              137
#define FEAT_SAM_BIT_POS               138
#define FEAT_TRAIN_NUDGING_BIT_POS     139


#define FEATURE_PAGE_MAX   3
#define FEATS_LEN           0x08
extern uint8_t lm_local_supp_feats[FEATURE_PAGE_MAX][FEATS_LEN];

/*
feature_bit:whitch bit to be update
con:0(disable) or 1(enable)
*/
void device_feature_bit_update(U8 feature_bit,U8 con)
{
	U8 page = feature_bit/64;
	U8 byte = (feature_bit%64)/8;
	U8 bit = feature_bit%8;

	if (con)
		lm_local_supp_feats[page][byte] |= (0x1<<bit);
	else
		lm_local_supp_feats[page][byte] &= ~(0x1<<bit);
}

void hci_uart_read_check (void);

void NVDS_write_callback(void)
{

}

void rwip_schedule_callback(void) __attribute__ ((section (".internal_ram_1_text")));
void rwip_schedule_callback(void)
{
	SOFT7_INT_SET;
}

void audio_com_init(void);

void rwip_schedule_enter(void) __attribute__ ((section (".internal_ram_1_text")));

void rwip_schedule_enter(void)
{
	unsigned long uxBits;

	while(1)                        
	{	
		uxBits  = xEventGroupWaitBits(event_grop, BT_CONTROLLER_EVENT|BT_CONTROLLER_RX_EVENT, pdTRUE, pdFALSE, portMAX_DELAY);

		if ( (uxBits & BT_CONTROLLER_RX_EVENT) == BT_CONTROLLER_RX_EVENT) 
		{
			hci_uart_read_check();
		}
		
		if ( (uxBits & BT_CONTROLLER_EVENT) == BT_CONTROLLER_EVENT) 
		{
			rwip_schedule();	
		}
	}    

}

void app_bt_tx_pwr_set (U8 max_tx_pwr);

U8 tx_gain;
void app_bt_tx_pwr_up(void)
{
	if (tx_gain < 0x17)
	{
		tx_gain++;
		app_bt_tx_pwr_set(tx_gain);
		DBG_Printf("tx up,gain:0x%x\n\r",tx_gain);
	}
	else
	{
		DBG_Printf("tx gain is max!gain:0x%x\n\r",tx_gain);
	}
}



void app_bt_tx_pwr_down(void)
{
	if (tx_gain > 0)
	{
		tx_gain--;
		app_bt_tx_pwr_set(tx_gain);
		DBG_Printf("tx up,gain:0x%x\n\r",tx_gain);
	}
	else
	{
		DBG_Printf("tx gain is min!gain:0x%x\n\r",tx_gain);
	}
}

#define BT_WR_RG(addr, value)        *((volatile U32 *)(addr | 0x17000400)) = (value)
#define BT_RD_RG(addr)               *((volatile U32 *)(addr | 0x17000400))

#define BLE_WR_RG(addr, value)        *((volatile U32 *)(addr | 0x17000800)) = (value)
#define BLE_RD_RG(addr)               *((volatile U32 *)(addr | 0x17000800))


#define DM_WR_RG(addr, value)        *((volatile U32 *)(addr | 0x17000000)) = (value)
#define DM_RD_RG(addr)               *((volatile U32 *)(addr | 0x17000000))


void task_bt_controller(void *pvParameters)
{  
	//enable uart0 for hci
//	DBG_Printf("controller init\n\r");

const char *bt_lmp_data_get(void);
const char *bt_lmp_time_get(void);
const char *bt_lmp_version_get(void);

	DBG_Printf("lmp ver:%s,build time:%s %s\r\n",bt_lmp_version_get(),bt_lmp_time_get(),bt_lmp_data_get());

	void uart_read_set(U8 *bufptr, U32 size);
	void uart_write_set(U8 *bufptr, U32 size);

	hw_uart_function_set(uart_read_set,uart_write_set);

	void lc_table_init (void* table);
	lc_table_init(NULL);
	
	void lb_table_init (void* table);
	lb_table_init(NULL);

	void lm_table_init (void* table);
	lm_table_init(NULL);

	void llc_table_init (void* table);
	llc_table_init(NULL);

	void llm_table_init (void* table);
	llm_table_init(NULL);

	void DBG_table_init (void* table);
	DBG_table_init(NULL);

	void lm_local_supp_feats_table_init (void* table);
	lm_local_supp_feats_table_init(NULL);
	device_feature_bit_update(FEAT_EDR_3MB_BIT_POS,0);

	//tx_gain = 0x0e;
	tx_gain = 0x0F;
	//tx_gain = 0x15;
	
	app_bt_tx_pwr_set(tx_gain);

	app_bt_csb_tx_pwr_set(tx_gain);

	extern 	U8  NVDS_buffer[0x800];

	memset(NVDS_buffer,0xff,sizeof(NVDS_buffer));
	write_callback_function_set(NVDS_write_callback);

	/* BT ADDR TEST */
	NVDS_buffer[0] = 'N';
	NVDS_buffer[1] = 'V';
	NVDS_buffer[2] = 'D';
	NVDS_buffer[3] = 'S';
	NVDS_buffer[4] = 0x01;
	NVDS_buffer[5] = 0x06;
	NVDS_buffer[6] = 0x06;

	NVDS_buffer[7] = 0x55;
	NVDS_buffer[8] = 0x56;
	NVDS_buffer[9] = 0x57;
	NVDS_buffer[10] = 0xFF;
	NVDS_buffer[11] = 0xFE;
	NVDS_buffer[12] = 0x04;

	rw_lib_init();

#if 1
	REG32(MDM_BASE_ADDR + (0xF8<<2)) = 0x700;
	//MDM_Data = REG32(MDM_BASE_ADDR + (0xF8<<2));
	//DBG_Printf("MDM 0xF8:0x%x\n", MDM_Data);

	REG32(MDM_BASE_ADDR + (0xB1<<2)) = 0x003F03E2;//for asic
	//MDM_Data = REG32(MDM_BASE_ADDR + (0xB1<<2));
	//DBG_Printf("MDM 0xB1:0x%x\n", MDM_Data);

	REG32(MDM_BASE_ADDR + (0xDA<<2)) = 0x80000003;//0x8000000C;
	//MDM_Data = REG32(MDM_BASE_ADDR + (0xDA<<2));
	//DBG_Printf("MDM 0xDA:0x%x\n", MDM_Data);


	//debug port
	//BT_WR_RG(0x00000050, 0x000000A4);    
	//BT_WR_RG(0x00000050, 0x000000A3);   
	//BT_WR_RG(0x00000050, 0x0000009A);
	

	//BT_WR_RG(0x00000050, 0x00000082);//Scan Status
	//BT_WR_RG(0x00000050, 0x000000A7);
	//BT_WR_RG(0x00000050, 0x00000099);//tx/rx len     
	//BT_WR_RG(0x00000050, 0x00000092);//Scan data path
	//BT_WR_RG(0x00000050, 0x00000098);//Scan data Len
	//BT_WR_RG(0x00000050, 0x0000008E);//Scan ET_CURRENTRXDESCPTR
	//BT_WR_RG(0x00000050, 0x000000A0);//Scan intr
	//BT_WR_RG(0x00000050, 0x000000CF);//Scan intr


   
	//BT_WR_RG(0x00000050, 0x000000A6);	//RF Interface, TX_EN, RX_EN, SPI


	//BT_WR_RG(0x00000050, 0x0000009B);//Scan Err  	
	//BT_WR_RG(0x00000050, 0x000000A9);	//RF Interface, TX_EN, RX_EN, Rate


	//Master
	BT_WR_RG(0x00000050, 0x000083A6);	//RF Interface, TX_EN, RX_EN, spi

#else
#if 0//for filter
	U32 tmp;
	BTRF_AGC_CONTROL0 *agc_config0;
	tmp = TEST_REG_AGC0;
	agc_config0 = (BTRF_AGC_CONTROL0 *) &tmp;
	
	agc_config0->mac2rfdac_mode = 0x0;//bypass
	agc_config0->rfadc2mac_mode = 0x0;//bypass
	
	TEST_REG_AGC0 = tmp;
#endif


#if 0//for mac
	volatile U32 val;
	MISC_BTDM_CONTROL_2s *btdm_config_2;
	val = REG_MISC_BTDM_2;
	btdm_config_2 = (MISC_BTDM_CONTROL_2s *) &val;
	btdm_config_2->bt_clksel = 16;		//BT MAC 16M
	//btdm_config_2->bt_clksel = 12;	  //BT MAC 12M
	//btdm_config_2->bt_rf_dc_offset_en = 1; 
	//btdm_config_2->bt_if_adc_filter_mode = 2; 
	//btdm_config_2->bt_if_adc_filter_mode = 1; 

	btdm_config_2->bt_rf_dc_block_filter_en = 0;
	REG_MISC_BTDM_2 = val;	


	delayus(10);
	btdm_config_2->bt_rf_dc_offset_en = 1; 
	//btdm_config_2->bt_rf_dc_block_filter_en = 0;
	REG_MISC_BTDM_2 = val;	
#endif

#if   1//for MDM 
	U32 MDM_Data;

/*	MDM_Data = REG32(MDM_BASE_ADDR + (0x91<<2));
	DBG_Printf("MDM 0x91:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xDA<<2));
	DBG_Printf("MDM 0xDA:0x%x\n", MDM_Data);

	REG32(MDM_BASE_ADDR + (0x91<<2)) = 0x1c72;//0x16b0;//0x1c72;
	REG32(MDM_BASE_ADDR + (0xDA<<2)) = 0x80000307;
	
	MDM_Data = REG32(MDM_BASE_ADDR + (0x91<<2));
	DBG_Printf("MDM 0x91:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xDA<<2));
	DBG_Printf("MDM 0xDA:0x%x\n", MDM_Data);
*/


	MDM_Data = REG32(MDM_BASE_ADDR + (0xE0<<2));
	DBG_Printf("MDM 0xE0:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE1<<2));
	DBG_Printf("MDM 0xE1:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE2<<2));
	DBG_Printf("MDM 0xE2:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE3<<2));
	DBG_Printf("MDM 0xE3:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE4<<2));
	DBG_Printf("MDM 0xE4:0x%x\n", MDM_Data);


	MDM_Data = REG32(MDM_BASE_ADDR + (0xEA<<2));
	DBG_Printf("MDM 0xEA:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xEB<<2));
	DBG_Printf("MDM 0xEB:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xEC<<2));
	DBG_Printf("MDM 0xEC:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xED<<2));
	DBG_Printf("MDM 0xED:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xEE<<2));
	DBG_Printf("MDM 0xEE:0x%x\n", MDM_Data);



	/*RX LPF 650k * 1.3*/
	//REG32(MDM_BASE_ADDR + (0xE0<<2)) = 0x82007FFD;
	//REG32(MDM_BASE_ADDR + (0xE1<<2)) = 0x941CC518;


#if 0
	/*RX LR LPF 1.35M*/
	/*
	REG32(MDM_BASE_ADDR + (0xEA<<2)) = 0xFFFF8001;
	REG32(MDM_BASE_ADDR + (0xEB<<2)) = 0x73146107;
	REG32(MDM_BASE_ADDR + (0xEC<<2)) = 0x0E;
	REG32(MDM_BASE_ADDR + (0xEE<<2)) = 0x700;
	*/

	/*RX LPF 1.35M*/ //for BLE 1M

	REG32(MDM_BASE_ADDR + (0xE0<<2)) = 0xFFFF8001;
	REG32(MDM_BASE_ADDR + (0xE1<<2)) = 0x73146107;
	REG32(MDM_BASE_ADDR + (0xE2<<2)) = 0x0E;
	REG32(MDM_BASE_ADDR + (0xE4<<2)) = 0x700;


	/*RX LR SEARCH LPF 1.35M*/
	/*
	REG32(MDM_BASE_ADDR + (0xEF<<2)) = 0xFFFF8001;
	REG32(MDM_BASE_ADDR + (0xF0<<2)) = 0x73146107;
	REG32(MDM_BASE_ADDR + (0xF1<<2)) = 0x0E;
	REG32(MDM_BASE_ADDR + (0xF3<<2)) = 0x700;
	*/
#else
	MDM_Data = REG32(MDM_BASE_ADDR + (0xF4<<2));
	DBG_Printf("MDM 0xF4:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xF5<<2));
	DBG_Printf("MDM 0xF5:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xF6<<2));
	DBG_Printf("MDM 0xF6:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xF8<<2));
	DBG_Printf("MDM 0xF8:0x%x\n", MDM_Data);

	REG32(MDM_BASE_ADDR + (0xF8<<2)) = 0x700;
	MDM_Data = REG32(MDM_BASE_ADDR + (0xF8<<2));
	DBG_Printf("MDM 0xF8:0x%x\n", MDM_Data);
#endif

	//MDM_Data = REG32(MDM_BASE_ADDR + (0xDB<<2));		
	//DBG_Printf("MDM 0xDB:0x%x\n", MDM_Data);
	//REG32(MDM_BASE_ADDR + (0xDB<<2)) = 0x04801600;//0x03E81600;
	//MDM_Data = REG32(MDM_BASE_ADDR + (0xDB<<2));		
	//DBG_Printf("MDM 0xDB:0x%x\n", MDM_Data);

	MDM_Data = REG32(MDM_BASE_ADDR + (0xE0<<2));
	DBG_Printf("MDM 0xE0:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE1<<2));
	DBG_Printf("MDM 0xE1:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE2<<2));
	DBG_Printf("MDM 0xE2:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE3<<2));
	DBG_Printf("MDM 0xE3:0x%x\n", MDM_Data);
	MDM_Data = REG32(MDM_BASE_ADDR + (0xE4<<2));
	DBG_Printf("MDM 0xE4:0x%x\n", MDM_Data);

	REG32(MDM_BASE_ADDR + (0xB1<<2)) = 0x003F03E2;//for asic

	MDM_Data = REG32(MDM_BASE_ADDR + (0xB1<<2));
	DBG_Printf("MDM 0xB1:0x%x\n", MDM_Data);

#if 1
MDM_Data = REG32(MDM_BASE_ADDR + (0xDA<<2));
DBG_Printf("MDM 0xDA:0x%x\n", MDM_Data);
REG32(MDM_BASE_ADDR + (0xDA<<2)) = 0x80000003;//0x8000000C;

MDM_Data = REG32(MDM_BASE_ADDR + (0xDA<<2));
DBG_Printf("MDM 0xDA:0x%x\n", MDM_Data);
#endif
#endif

	//debug port
	//BT_WR_RG(0x00000050, 0x000000A4);    
	//BT_WR_RG(0x00000050, 0x000000A3);   
	//BT_WR_RG(0x00000050, 0x0000009A);
	

	//BT_WR_RG(0x00000050, 0x00000082);//Scan Status
	//BT_WR_RG(0x00000050, 0x000000A7);
	//BT_WR_RG(0x00000050, 0x00000099);//tx/rx len     
	//BT_WR_RG(0x00000050, 0x00000092);//Scan data path
	//BT_WR_RG(0x00000050, 0x00000098);//Scan data Len
	//BT_WR_RG(0x00000050, 0x0000008E);//Scan ET_CURRENTRXDESCPTR
	//BT_WR_RG(0x00000050, 0x000000A0);//Scan intr
	//BT_WR_RG(0x00000050, 0x000000CF);//Scan intr


   
	//BT_WR_RG(0x00000050, 0x000000A6);	//RF Interface, TX_EN, RX_EN, SPI


	//BT_WR_RG(0x00000050, 0x0000009B);//Scan Err  	
	//BT_WR_RG(0x00000050, 0x000000A9);	//RF Interface, TX_EN, RX_EN, Rate


	//Master
	BT_WR_RG(0x00000050, 0x000083A6);	//RF Interface, TX_EN, RX_EN, spi

	//BT_WR_RG(0x00000050, 0x0000839b);	//RF Interface, TX_EN, RX_EN, eRR


	//BT_WR_RG(0x00000050, 0x00008382);	//RF Interface, TX_EN, RX_EN, packet main state
	
	//BT_WR_RG(0x00000050, 0x00008399);	//RF Interface, TX_EN, RX_EN, tx/rxlen

	//BT_WR_RG(0x00000050, 0x0000839A);	//RF Interface, TX_EN, RX_EN, RX TYPE/TX TYPE

	//BT_WR_RG(0x00000050, 0x00008393);	//RF Interface, TX_EN, RX_EN, abort

	//BT_WR_RG(0x00000050, 0x0000839B);	//RF Interface, TX_EN, RX_EN, err

	

	//data = BT_RD_RG(0x50);
	//DBG_Printf("bt diag 0x50:0x%x\n", data);

		                               
#if 0//for orca
			U32 data;
			 
			data = BT_RD_RG(0x28);      
			DBG_Printf("0x28:0x%x\n", data);      
			//BT_WR_RG(0x28,(data&0xffff0000)|0x00ff);
			//BT_WR_RG(0x28,(data&0xffff0000)|0x006A);
			//BT_WR_RG(0x28,(data&0xffff0000)|0x41ff);	//tx swap
			//BT_WR_RG(0x28,(data&0xffff0000)|0x21ff);	//rx swap
			//BT_WR_RG(0x28,(data&0xffff0000)|0x01ff);
			BT_WR_RG(0x28,(data&0xffff0000)|0x053f);       
			data = BT_RD_RG(0x28);           
			DBG_Printf("0x28:0x%x\n", data);                   
 
		 	//BT_WR_RG(0x00000050, 0x000000A4);    
		 	//BT_WR_RG(0x00000050, 0x000000A3);   
			//BT_WR_RG(0x00000050, 0x0000009A);
			//BT_WR_RG(0x00000050, 0x0000009B);//Scan Err     
			//BT_WR_RG(0x00000050, 0x00000082);//Scan Status
			//BT_WR_RG(0x00000050, 0x000000A7);
			//BT_WR_RG(0x00000050, 0x00000099);//tx/rx len     
			//BT_WR_RG(0x00000050, 0x00000092);//Scan data path
			//BT_WR_RG(0x00000050, 0x00000098);//Scan data Len
			//BT_WR_RG(0x00000050, 0x0000008E);//Scan ET_CURRENTRXDESCPTR
			//BT_WR_RG(0x00000050, 0x000000A0);//Scan intr
			//BT_WR_RG(0x00000050, 0x000000CF);//Scan intr
			BT_WR_RG(0x00000050, 0x000000A6);	//RF Interface, TX_EN, RX_EN, SPI
			data = BT_RD_RG(0x50);
			DBG_Printf("bt diag 0x50:0x%x\n", data);
			
			//data = BT_RD_RG(0xE0);
			//DBG_Printf("0xE0H:0x%x\n", data);
			//BT_WR_RG(0xE0,(data-0x10));
			//data = BT_RD_RG(0xE0); 
			//DBG_Printf("0xE0H:0x%x\n", data);

    /*** BLE ***/
	//BLE_WR_RG(0x00000050, 0x83);
	data = BLE_RD_RG(0x50);
	DBG_Printf("BLE diag 0x50:0x%x\n", data);

	//data = BLE_RD_RG(0xE0);
	//DBG_Printf("BLE diag 0xE0:0x%x\n", data);

	//BLE_RD_RG(0xE0) = ((data&0xFFFF0000) | 0x1FE);

	data = BLE_RD_RG(0xE0);
	DBG_Printf("BLE diag 0xE0:0x%x\n", data);
#endif

#endif

#ifdef BTDM5_DIV_3INT
	XT_INTS_ON(LEVEL2_INT18_MASK);
	XT_INTS_ON(LEVEL2_INT19_MASK);
	XT_INTS_ON(LEVEL2_INT20_MASK);
#endif

	SOFT7_INT_ENABLE;
	rwip_schedule_enter();
}