Import bk_ble from elektroda forum

https://www.elektroda.com/rtvforum/topic3989434.html\#20742145

10 files changed, 1588 insertions, 0 deletions

diff --git a/usr/ble_tst/src/device.c b/usr/ble_tst/src/device.c
new file mode 100644
index 0000000..6aa15d5
--- /dev/null
+++ b/usr/ble_tst/src/device.c
@@ -0,0 +1,420 @@
+
+#include "ble.h"
+#include "ble_pub.h"
+#include "ble_api.h"
+#include "common_bt.h"
+#include "comm_task.h"
+#include "comm.h"
+#include "gattc.h"
+#include "gattc_task.h"
+#include "prf.h"
+
+#include "application.h"
+#include "generic.h"
+#include <string.h>
+
+#include "include.h"
+#include "driver_pub.h"
+#include "func_pub.h"
+#include "app.h"
+#include "start_type_pub.h"
+#include "mcu_ps_pub.h"
+
+#define ATT_DB_LENGTH 6
+#define BK_ATT_DECL_PRIMARY_SERVICE_128     {0x00,0x28,0,0,0,0,0,0,0,0,0,0,0,0,0,0}
+#define BK_ATT_DECL_CHARACTERISTIC_128      {0x03,0x28,0,0,0,0,0,0,0,0,0,0,0,0,0,0}
+#define BK_ATT_DESC_CLIENT_CHAR_CFG_128     {0x02,0x29,0,0,0,0,0,0,0,0,0,0,0,0,0,0}
+#define WRITE_REQ_CHARACTERISTIC        {0xD0,0x07,0x9B,0x5F,0x80,0x00,  0x01,0x80,  0x01,0x10,  0,0,  0x01,0,0,0}
+#define NOTIFY_CHARACTERISTIC           {0xD0,0x07,0x9B,0x5F,0x80,0x00,  0x01,0x80,  0x01,0x10,  0,0,  0x02,0,0,0}
+
+typedef struct ble_cfg_st
+{
+  struct bd_addr mac;
+  char name[APP_DEVICE_NAME_LENGTH_MAX];
+} BLE_CFG_ST, *BLE_CFG_PTR;
+
+uint32_t app_stack_size = 4096;
+uint32_t ext_init_stack_size = 2048;
+char canary[2] = { 'N', '\0' };
+
+extern uint32_t _bootloader_world_flag;
+
+volatile uint32_t *bootloader_world_flag = &_bootloader_world_flag;
+extern BLE_CFG_ST ble_cfg;
+extern struct bd_addr common_default_bdaddr;
+
+static void update_thread (void *arg);
+
+extern struct prf_env_tag prf_env;
+
+extern uint8_t bk_ble_get_prf_by_task (kernel_task_id_t task,
+				       struct prf_task_env **env);
+static void notify_val (void);
+
+static void *pull_stubs (void);
+
+bk_attm_desc_t btl_att_db[ATT_DB_LENGTH] = {
+  [0] = {BK_ATT_DECL_PRIMARY_SERVICE_128, BK_PERM_SET (RD, ENABLE), 0, 0},
+  [1] = {BK_ATT_DECL_CHARACTERISTIC_128, BK_PERM_SET (RD, ENABLE), 0, 0},
+  [2] = {WRITE_REQ_CHARACTERISTIC,
+	 BK_PERM_SET (WRITE_REQ, ENABLE) | BK_PERM_SET (WRITE_COMMAND,
+							ENABLE),
+	 BK_PERM_SET (RI, ENABLE) | BK_PERM_SET (UUID_LEN, UUID_128), 256},
+  [3] = {BK_ATT_DECL_CHARACTERISTIC_128, BK_PERM_SET (RD, ENABLE), 0, 0},
+  [4] = {NOTIFY_CHARACTERISTIC, BK_PERM_SET (NTF, ENABLE),
+	 BK_PERM_SET (RI, ENABLE) | BK_PERM_SET (UUID_LEN, UUID_128), 256},
+  [5] =
+    {BK_ATT_DESC_CLIENT_CHAR_CFG_128,
+     BK_PERM_SET (RD, ENABLE) | BK_PERM_SET (WRITE_REQ, ENABLE), 0, 0},
+};
+
+beken_semaphore_t app_sema = NULL;
+beken_semaphore_t ext_init_sema = NULL;
+
+static void
+ble_event_callback (ble_event_t event, void *param)
+{
+  switch (event)
+    {
+    case BLE_STACK_OK:
+      bk_printf ("STACK INIT OK\r\n");
+
+      {
+	struct bk_ble_db_cfg ble_db_cfg;
+
+	ble_db_cfg.att_db = btl_att_db;
+	ble_db_cfg.att_db_nb = ATT_DB_LENGTH;
+	ble_db_cfg.prf_task_id = 0;
+	ble_db_cfg.start_hdl = 0;
+	ble_db_cfg.svc_perm = BK_PERM_SET (SVC_UUID_LEN, UUID_16);
+#if 0
+	memcpy (&(ble_db_cfg.uuid[0]), &custom_uuid[0], 16);
+#endif
+	bk_ble_create_db (&ble_db_cfg);
+      }
+
+      break;
+
+    case BLE_CREATE_DB_OK:
+      bk_printf ("CREATE DB SUCCESS\r\n");
+
+      appm_start_advertising ();
+
+      break;
+
+    case BLE_CONNECT:
+      bk_printf ("BLE CONNECT\r\n");
+
+      if (((struct gapc_connection_req_ind *) param)->sup_to < 200)
+	{
+	  struct gapc_conn_param conn_param;
+
+	  conn_param.intv_max =
+	    ((struct gapc_connection_req_ind *) param)->con_interval;
+	  conn_param.intv_min =
+	    ((struct gapc_connection_req_ind *) param)->con_interval;
+	  conn_param.latency =
+	    ((struct gapc_connection_req_ind *) param)->con_latency;
+	  conn_param.time_out = 600;
+	  appm_update_param (&conn_param);
+	}
+
+      rtos_create_thread (NULL,
+			  THD_APPLICATION_PRIORITY,
+			  "update thread",
+			  (beken_thread_function_t) update_thread,
+			  ext_init_stack_size, (beken_thread_arg_t) 0);
+
+      mcu_prevent_set (MCU_PS_BLE_FROBID);
+
+      break;
+
+    case BLE_MTU_CHANGE:
+      bk_printf ("BLE_MTU_CHANGE:%d\r\n", *(unsigned short *) param);
+      break;
+
+    case BLE_DISCONNECT:
+      bk_printf ("BLE DISCONNECTed\r\n");
+      break;
+
+    case BLE_CFG_NOTIFY:
+      bk_printf ("BLE_CFG_NOTIFY:%d\r\n", *(unsigned short *) param);
+      break;
+
+    case BLE_CFG_INDICATE:
+      bk_printf ("BLE_CFG_INDICATE:%d\r\n", *(unsigned short *) param);
+      break;
+
+    case BLE_TX_DONE:
+      bk_printf ("BLE_TX_DONE\r\n");
+      break;
+
+    case BLE_GEN_DH_KEY:
+      bk_printf ("BLE_GEN_DH_KEY\r\n");
+      break;
+
+    case BLE_GET_KEY:
+      bk_printf ("BLE_GET_KEY\r\n");
+      break;
+
+    case BLE_STACK_FAIL:
+      bk_printf ("STACK INIT FAIL\r\n");
+      break;
+
+    case BLE_HW_ERROR:
+      bk_printf ("BLE_HW_ERROR\r\n");
+      break;
+
+    default:
+      bk_printf ("UNKNOWN EVENT:%d\r\n", event);
+      break;
+
+    }
+}
+
+static void
+ble_write_callback (write_req_t * write_req)
+{
+
+  bk_printf ("write req");
+#if 0
+  tuya_ble_data_buf_t data;
+
+  if (write_req->att_idx == 5)
+    {
+      ntf_enable = (write_req->value[0]) | (write_req->value[1] << 8);
+    }
+  else
+    {
+      data.data = write_req->value;
+      data.len = write_req->len;
+
+      if (ty_bt_msg_cb != NULL)
+	{
+	  ty_bt_msg_cb (0, TY_BT_EVENT_RX_DATA, &data);
+	}
+    }
+#endif
+}
+
+static unsigned char
+ble_read_callback (read_req_t * read_req)
+{
+  bk_printf ("read req");
+#if 0
+  if (read_req->att_idx == 5)
+    {
+      memcpy (read_req->value, &ntf_enable, sizeof (ntf_enable));
+    }
+#endif
+  return 2;
+}
+
+static void
+ble_recv_adv_callback (recv_adv_t * recv_adv)
+{
+}
+
+static void
+init_ble_thread (void *arg)
+{
+
+  ble_set_event_cb (ble_event_callback);
+
+  ble_set_write_cb (ble_write_callback);
+  ble_set_read_cb (ble_read_callback);
+  ble_set_recv_adv_cb (ble_recv_adv_callback);
+
+  ble_clk_power_up ();
+  ble_switch_rf_to_ble ();
+  ble_init ();
+  ble_activate ("TESTER");
+
+  uint8_t *mac = (uint8_t *) & ble_cfg.mac;
+
+  bk_printf ("ble name:%s, %02x:%02x:%02x:%02x:%02x:%02x\r\n",
+	     &ble_cfg.name, mac[5], mac[4], mac[3], mac[2], mac[1], mac[0]);
+
+  rtos_delete_thread (NULL);
+}
+
+static void
+update_thread (void *arg)
+{
+  while (1)
+    {
+#if 0
+      notify_val ();
+#else
+      bk_ble_send_ntf_value (5, (uint8_t *) "\x27\x18\x28\x18\x28", 0, 4);
+      bk_printf ("BLE control[8]: 0x%x\n",
+		 *((volatile uint16_t *) 0x0081436e));
+      bk_printf ("Hop control[8]: 0x%x\n",
+		 *((volatile uint16_t *) 0x00814380));
+      bk_printf ("SYNCWL[8]: 0x%x\n", *((volatile uint16_t *) 0x814376));
+
+#endif
+      rtos_delay_milliseconds (3000);
+    }
+}
+
+static void
+notify_val (void)
+{
+  bk_printf (".");
+  struct gattc_send_evt_cmd *ntf_value =
+    KERNEL_MSG_ALLOC_DYN (GATTC_SEND_EVT_CMD,
+			  KERNEL_BUILD_ID (TASK_GATTC, 0),
+			  prf_src_task_get (&(prf_env_t) { TASK_APP,
+					    TASK_ID_COMMON }, 0),
+			  gattc_send_evt_cmd, 5);
+
+  ntf_value->operation = GATTC_NOTIFY;
+
+  struct bk_ble_env_tag *ble_env = NULL;
+
+#if 0
+
+  struct prf_task_env *prf_env = NULL;
+
+  bk_ble_get_prf_by_task (TASK_GAPC + 1, &prf_env);
+  ble_env = (struct bk_ble_env_tag *) (prf_env->env);
+#else
+  ble_env = (struct bk_ble_env_tag *) prf_env_get (TASK_ID_COMMON);
+#endif
+  ntf_value->handle = ble_env->start_hdl + 4;
+
+  ntf_value->length = 5;
+  ntf_value->seq_num = 4;
+
+  memcpy (&ntf_value->value[0], (uint8_t *) "\x31\x41\x59\x26\x53", 5);
+
+  kernel_msg_send (ntf_value);
+}
+
+static void
+ext_init_task_handler (void *arg)
+{
+  OSStatus ret;
+
+  func_init_extended ();
+  ret = rtos_set_semaphore (&ext_init_sema);
+
+  ASSERT (kNoErr == ret);
+
+  rtos_delete_thread (NULL);
+}
+
+void
+app_set_sema (void)
+{
+  OSStatus ret;
+
+  ret = rtos_set_semaphore (&app_sema);
+
+  (void) ret;
+}
+
+__attribute__((noinline))
+     static void *pull_stubs (void)
+{
+  int a = 0;
+  extern uint32_t _vector_start;
+  uint32_t *vecs = &_vector_start;
+  extern uint8_t wifi_read_efuse (uint8_t addr);
+  uint8_t ef = 0;
+
+  ef = wifi_read_efuse (31);
+  bk_printf ("start: %x, ef: %x, a: %d\n", *vecs, ef, a);
+#if 0
+
+  extern void rwble_hl_init_re (void);
+  extern void rwble_reset_re (void);
+
+  void *func_list[] = {
+    rwble_hl_init_re,
+    rwble_reset_re
+  };
+  return func_list[!!ef];
+#endif
+  return NULL;
+}
+
+void
+entry_main (void)
+{
+
+  OSStatus ret;
+
+  canary[0] = 'm';
+  *bootloader_world_flag = 1;
+
+  bk_misc_init_start_type ();
+  driver_init ();
+  bk_misc_check_start_type ();
+  func_init_basic ();
+
+  pull_stubs ();
+
+  uint32_t my_stack = 0;
+  __asm__ __volatile__ ("mov %0, sp":"=r" (my_stack)::);
+
+  bk_printf ("current stack top: 0x%x\n", my_stack);
+
+  int a = 0x7FFFFFFF;
+  int b[3] = { 1, 2, 3 };
+
+  if (a + 2[b] >= 0)
+    {
+      bk_printf ("condition (%d >= 0) is true\n", a + 2[b]);
+    }
+
+  union u_t
+  {
+    uint32_t a;
+    struct s_t
+    {
+      uint16_t b;
+      uint16_t c;
+    } d;
+  } u;
+
+  u.d = (struct s_t)
+  { 0, 1 };
+
+  bk_printf ("value: 0x%x, system is %s endian\n", u.a,
+	     u.a == 1 ? "big" : "little");
+
+#define IS_ARM() do{ \
+    uint32_t is_arm =  0; \
+    __asm__ __volatile__( \
+    "mov %0, pc\n" \
+    "mov r1, pc\n" \
+    "sub %0, r1, %0\n" \
+    "sub %0, #2\n" \
+    : "=r" (is_arm) :: "r1"); \
+    bk_printf("current state is %s\n", is_arm ? "ARM" : "Thumb"); \
+} while(0)
+
+  extern uint32_t is_arm (void);
+
+  bk_printf ("is_arm() runs in %s state\n", is_arm ()? "ARM" : "Thumb");
+  IS_ARM ();
+
+  ret = rtos_create_thread (NULL,
+			    THD_INIT_PRIORITY,
+			    "main",
+			    (beken_thread_function_t) init_ble_thread,
+			    app_stack_size, (beken_thread_arg_t) 0);
+  ASSERT (kNoErr == ret);
+
+  rtos_init_semaphore (&ext_init_sema, 1);
+  ret = rtos_create_thread (NULL,
+			    THD_EXTENDED_APP_PRIORITY,
+			    "ext_init",
+			    (beken_thread_function_t) ext_init_task_handler,
+			    ext_init_stack_size, (beken_thread_arg_t) 0);
+  ASSERT (kNoErr == ret);
+
+  vTaskStartScheduler ();
+}
diff --git a/usr/ble_tst/src/diagnostic.c b/usr/ble_tst/src/diagnostic.c
new file mode 100644
index 0000000..90afaae
--- /dev/null
+++ b/usr/ble_tst/src/diagnostic.c
@@ -0,0 +1,73 @@
+
+#include "generic.h"
+
+#include "rwip_config.h"
+#include "rwip.h"
+#include "ble_reg_access.h"
+#include "reg_blecore.h"
+#include "em_map_ble.h"
+#include "reg_ble_em_cs.h"
+#include "reg_ble_em_rx_buffer.h"
+#include "reg_ble_em_rx_desc.h"
+#include "reg_ble_em_tx_buffer_cntl.h"
+#include "reg_ble_em_tx_desc.h"
+#include "reg_ble_em_tx_buffer_data.h"
+#include "ecc_p256.h"
+#include "llm.h"
+#include "llm_util.h"
+#include "lld.h"
+#include "lld_evt.h"
+#include "lld_pdu.h"
+#include "lld_util.h"
+#include "RomCallFlash.h"
+#include "em_buf.h"
+#include "ea.h"
+#include "common_utils.h"
+#include "common_hci.h"
+#include "common_llcp.h"
+
+void output_ble_registers (void);
+
+uint32_t assembly_test0 (uint32_t input);
+
+void
+output_ble_registers (void)
+{
+
+  uint8_t rxpwrup0 = 0;
+  uint8_t txpwrdn0 = 0;
+  uint8_t txpwrup0 = 0;
+
+  bk_printf ("BLE control[8]: 0x%x\n", ble_cntl_get (8));
+
+  ble_radiopwrupdn0_unpack (&rxpwrup0, &txpwrdn0, &txpwrup0);
+  bk_printf
+    ("Power down control 0: rxpwrup0::0x%x, txpwrdn0::0x%x, txpwrup0::0x%x\n",
+     rxpwrup0, txpwrdn0, txpwrup0);
+  bk_printf ("Power down control 0: 0x%x\n", ble_radiopwrupdn0_get ());
+  bk_printf ("Interrupt control: 0x%x\n", ble_intcntl_get ());
+  bk_printf ("Upper BDADDR: 0x%x\n", ble_bdaddru_getf ());
+  bk_printf ("Lower BDADDR: 0x%x\n", ble_bdaddrl_getf ());
+}
+
+uint32_t
+assembly_test0 (uint32_t input)
+{
+  uint32_t output = 0xABCDEFEF;
+  __asm__ __volatile__ ("        sub r3, %0, #1\n"
+			"        sbc %0, r3\n":"=r" (output):"0" (input):"cc",
+			"r3");
+  return output;
+}
+
+void
+output_diagnostics (void)
+{
+
+  output_ble_registers ();
+
+  bk_printf ("Assembly test 0, input=0, output=0x%x\n", assembly_test0 (0));
+  bk_printf ("Assembly test 0, input=0x800, output=0x%x\n",
+	     assembly_test0 (0x800));
+
+}
diff --git a/usr/ble_tst/src/experimental.c b/usr/ble_tst/src/experimental.c
new file mode 100644
index 0000000..fdc2581
--- /dev/null
+++ b/usr/ble_tst/src/experimental.c
@@ -0,0 +1,62 @@
+
+#include "rwip_config.h"
+#include "rwip.h"
+#include "ble_reg_access.h"
+#include "reg_blecore.h"
+#include "em_map_ble.h"
+#include "reg_ble_em_cs.h"
+#include "reg_ble_em_rx_buffer.h"
+#include "reg_ble_em_rx_desc.h"
+#include "reg_ble_em_tx_buffer_cntl.h"
+#include "reg_ble_em_tx_desc.h"
+#include "reg_ble_em_tx_buffer_data.h"
+#include "ecc_p256.h"
+#include "llm.h"
+#include "llm_util.h"
+#include "lld.h"
+#include "lld_evt.h"
+#include "lld_pdu.h"
+#include "lld_util.h"
+#include "RomCallFlash.h"
+#include "em_buf.h"
+#include "ea.h"
+#include "common_utils.h"
+#include "common_hci.h"
+#include "common_llcp.h"
+
+int func1 (int);
+int func2 (int);
+
+static int (*(farray[2])) (int) = { func1, func2 };
+
+struct st
+{
+  void *str;
+  uint32_t a;
+};
+
+struct st arr[2] = { {"A1", 0}, {"A2", 0} };
+
+int
+func1 (int inp)
+{
+  return inp * inp;
+}
+
+int
+func2 (int inp)
+{
+  uint32_t clk2, clk1;
+
+  clk1 = 2 * inp;
+  clk2 = inp + 1;
+
+  clk1 = CLK_ADD_3 (clk1, 3, clk2) & SYNC_TRAIN_TO_MAX;
+  return clk1 + arr[inp].a;
+}
+
+int
+func3 (int i)
+{
+  return farray[i] (11);
+}
diff --git a/usr/ble_tst/src/is_arm_test.S b/usr/ble_tst/src/is_arm_test.S
new file mode 100644
index 0000000..fabc72c
--- /dev/null
+++ b/usr/ble_tst/src/is_arm_test.S
@@ -0,0 +1,31 @@
+/*60:*/
+#line 1367 "/home/nemo/projects/bk_ble/cweb/notes.x"
+
+/*59:*/
+#line 1344 "/home/nemo/projects/bk_ble/cweb/notes.x"
+
+
+
+.code 32
+.globl is_arm
+.type is_arm, %function
+
+
+
+
+
+is_arm:
+    push {r1, lr}
+        mov r0, pc
+        mov r1, pc
+        sub r0, r1, r0
+        sub r0, #2
+    pop {r1, pc}
+
+
+
+/*:59*/
+#line 1368 "/home/nemo/projects/bk_ble/cweb/notes.x"
+
+
+/*:60*/
diff --git a/usr/ble_tst/src/lld_re.c b/usr/ble_tst/src/lld_re.c
new file mode 100644
index 0000000..fedea3e
--- /dev/null
+++ b/usr/ble_tst/src/lld_re.c
@@ -0,0 +1,505 @@
+
+#include "rwip_config.h"
+#include "rwip.h"
+#include "ble_reg_access.h"
+#include "reg_blecore.h"
+#include "em_map_ble.h"
+#include "reg_ble_em_cs.h"
+#include "reg_ble_em_rx_buffer.h"
+#include "reg_ble_em_rx_desc.h"
+#include "reg_ble_em_tx_buffer_cntl.h"
+#include "reg_ble_em_tx_desc.h"
+#include "reg_ble_em_tx_buffer_data.h"
+#include "ecc_p256.h"
+#include "llm.h"
+#include "llm_util.h"
+#include "lld.h"
+#include "lld_evt.h"
+#include "lld_pdu.h"
+#include "lld_util.h"
+#include "RomCallFlash.h"
+#include "em_buf.h"
+#include "ea.h"
+#include "common_utils.h"
+#include "common_hci.h"
+#include "common_llcp.h"
+
+extern struct kernel_task_desc TASK_DESC_LLD;
+
+void
+lld_init (bool reset)
+{
+
+  if (reset)
+    {
+      kernel_task_create (TASK_LLD, &TASK_DESC_LLD);
+    }
+
+  ble_rxwinszdef_setf (LLD_EVT_DEFAULT_RX_WIN_SIZE);
+
+  ble_prefetch_time_setf (LLD_EVT_PREFETCH_TIME_US);
+  ble_prefetchabort_time_setf (LLD_EVT_ABORT_CNT_DURATION_US);
+
+  ble_radiopwrupdn0_pack (122, 0, 120);
+  ble_radiopwrupdn1_pack (122, 0, 120);
+  ble_intcntl_set (BLE_RXINTMSK_BIT | BLE_EVENTINTMSK_BIT |
+		   BLE_CRYPTINTMSK_BIT | BLE_ERRORINTMSK_BIT |
+		   BLE_SWINTMSK_BIT | BLE_EVENTAPFAINTMSK_BIT);
+
+  ble_sn_dsb_setf (0);
+  ble_nesn_dsb_setf (0);
+
+  ecc_gen_new_secret_key (&llm_le_env.secret_key256[0], 0);
+
+#define m(idx,pos) ((uint32_t)(common_default_bdaddr.addr[idx]))<<(pos*8)
+  ble_bdaddrl_setf (m (0, 0) | m (1, 1) | m (2, 2) | m (3, 3));
+  ble_bdaddru_pack (0, m (4, 0) | m (5, 1));
+
+#undef mac
+
+  ble_advertfilt_en_setf (0);
+
+  ble_advchmap_setf (0x7);
+  ble_wlpubaddptr_setf (EM_BLE_WPB_OFFSET);
+  ble_wlprivaddptr_setf (EM_BLE_WPV_OFFSET);
+  ble_wlnbdev_pack (BLE_WHITELIST_MAX, BLE_WHITELIST_MAX);
+
+  ble_ralptr_setf (EM_BLE_RAL_OFFSET);
+  ble_nbraldev_setf (BLE_RESOL_ADDR_LIST_MAX);
+
+  ble_syncwordl_setf (LLD_ADV_HDL, (uint16_t) (0x0000FFFF & LLM_LE_ADV_AA));
+  ble_syncwordh_setf (LLD_ADV_HDL, (uint16_t) (LLM_LE_ADV_AA >> 16));
+  ble_crcinit0_setf (LLD_ADV_HDL, 0x5555);
+  ble_crcinit1_setf (LLD_ADV_HDL, 0x55);
+  ble_llchmap0_setf (LLD_ADV_HDL, 0);
+  ble_llchmap1_setf (LLD_ADV_HDL, 0);
+  ble_chmap2_pack (LLD_ADV_HDL, 0, 0);
+  ble_rxmaxbuf_setf (LLD_ADV_HDL, 0);
+  ble_rxmaxtime_setf (LLD_ADV_HDL, 0);
+  ble_et_currentrxdescptr_pack (0, EM_BLE_RX_DESC_OFFSET);
+  ble_txrxcntl_pack (LLD_ADV_HDL, 0, 0, 0, 0, LLM_ADV_CHANNEL_TXPWR);
+
+  if (system_mode == 1)
+    {
+      ble_thrcntl_ratecntl_pack (LLD_ADV_HDL, 1, 0, 0);
+    }
+  else
+    {
+      ble_thrcntl_ratecntl_pack (LLD_ADV_HDL, 4, 0, 0);
+    }
+
+  ble_conflict_setf (LLD_ADV_HDL, 0);
+  ble_filtpol_ralcntl_pack (LLD_ADV_HDL, 0, 0, 0, 0);
+  em_set (0, 0, 0x40);
+
+  lld_evt_init (reset);
+
+  extern bool system_sleep_flag;
+
+  if (system_sleep_flag)
+    {
+      lld_sleep_init ();
+    }
+
+  ble_txpldsrc_setf (0);
+
+  ble_rwble_en_setf (1);
+
+}
+
+struct ea_elt_tag *
+lld_adv_start (struct advertising_pdu_params *adv_par,
+	       struct em_desc_node *adv_pdu,
+	       struct em_desc_node *scan_rsp_pdu, uint8_t adv_pwr)
+{
+
+  uint32_t adv_time;
+  uint8_t position;
+  uint8_t priv_info;
+  uint32_t ral_pos;
+  struct ea_elt_tag *elt = NULL;
+  struct lld_evt_tag *env;
+
+  if (adv_par->type == ADV_CONN_DIR && !adv_par->adv_ldc_flag)
+    {
+      adv_par->intervalmax = LLM_LE_ADV_INTERV_DFLT;
+      adv_time = LLD_EVT_FRAME_DURATION;
+      elt =
+	lld_evt_adv_create (LLD_ADV_HDL, adv_par->intervalmin,
+			    adv_par->intervalmax, false);
+    }
+  else
+    {
+      adv_time = LLD_EVT_FRAME_DURATION + DRIFT_BT_DFT;
+
+      elt =
+	lld_evt_adv_create (LLD_ADV_HDL, adv_par->intervalmin,
+			    adv_par->intervalmax, true);
+    }
+
+  if (!elt)
+    {
+      return NULL;
+    }
+
+  env = LLD_EVT_ENV_ADDR_GET (elt);
+
+  priv_info = llm_le_env.enh_priv_info & LLM_PRIV_ENABLE_MASK;
+  ral_pos = 0;
+
+  if (priv_info)
+    {
+      ral_pos = 0;
+      if (adv_par->own_addr_type & ADDR_RPA_MASK)
+	{
+	  if (llm_util_bd_addr_in_ral
+	      (&(adv_par->peer_addr), adv_par->peer_addr_type, &position))
+	    {
+	      ral_pos = EM_BLE_RAL_OFFSET + position * REG_BLE_EM_RAL_SIZE;
+	    }
+
+	  priv_info = true;
+
+	  if (adv_par->type == ADV_CONN_DIR)
+	    {
+	      priv_info = !!ral_pos;
+	    }
+	}
+    }
+
+  ble_linklbl_setf (LLD_ADV_HDL, 8);
+
+  if (adv_par->type == ADV_CONN_DIR)
+    {
+      ble_cntl_pack (LLD_ADV_HDL, 0xff, 0x1, 0, 0,
+		     adv_par->
+		     adv_ldc_flag ? LLD_LD_ADVERTISER : LLD_HD_ADVERTISER);
+    }
+  else
+    {
+      ble_cntl_pack (LLD_ADV_HDL, 0xff, 0x1, 0, 0, LLD_LD_ADVERTISER);
+    }
+
+  uint8_t localrpasel = 0;
+
+  if (adv_par->own_addr_type > ADDR_RAND)
+    {
+      localrpasel = !!ral_pos;
+    }
+
+  ble_filtpol_ralcntl_pack (LLD_ADV_HDL, adv_par->filterpolicy, localrpasel,
+			    !!ral_pos, priv_info);
+
+  ble_conflict_setf (LLD_ADV_HDL, 0);
+
+  if (ral_pos)
+    {
+      ble_peer_ralptr_set (LLD_ADV_HDL, ral_pos);
+    }
+  else
+    {
+      em_wr ((adv_par->peer_addr).addr,
+	     2 * BLE_ADV_BD_ADDR_INDEX +
+	     EM_BLE_CS_OFFSET +
+	     LLD_ADV_HDL * REG_BLE_EM_CS_SIZE, BD_ADDR_LEN);
+      ble_adv_bd_addr_type_set (LLD_ADV_HDL, adv_par->peer_addr_type);
+    }
+
+  ble_hopcntl_pack (LLD_ADV_HDL, 1, 0x0, 0x27);
+  ble_crcinit1_set (LLD_ADV_HDL, 0x55);
+  ble_rxwincntl_set (LLD_ADV_HDL, 0);
+  ble_txrxcntl_set (LLD_ADV_HDL, adv_pwr);
+  ble_thrcntl_ratecntl_pack (LLD_ADV_HDL, 0x4, 0, 0);
+  ble_fcntoffset_set (LLD_ADV_HDL, 0x00);
+  ble_rxmaxbuf_set (LLD_ADV_HDL, 0x00);
+  ble_rxmaxtime_set (LLD_ADV_HDL, 0x00);
+  ble_maxevtime_set (LLD_ADV_HDL, 0x08);
+  ble_advchmap_set (adv_par->channelmap);
+  ble_advtim_set (adv_time);
+
+  env->tx_pwr = adv_pwr;
+  lld_pdu_tx_push (elt, adv_pdu);
+
+  if (scan_rsp_pdu)
+    {
+      lld_pdu_tx_push (elt, scan_rsp_pdu);
+    }
+
+  lld_pdu_tx_loop (env);
+
+  void lld_abort_scan_evt (void);
+
+  GLOBAL_INT_DIS ();
+
+  lld_evt_elt_insert (elt, 1);
+  lld_abort_scan_evt ();
+  (env->evt).non_conn.anchor = elt->timestamp;
+  (env->evt).non_conn.end_ts =
+    CLK_ADD_2 (elt->timestamp, LLM_LE_ADV_INTERV_DFLT);
+  (env->evt).non_conn.initiate = 0;
+  (env->evt).non_conn.connect_req_sent = 0;
+
+  GLOBAL_INT_RES ();
+
+  return elt;
+}
+
+struct ea_elt_tag *
+lld_con_start (struct hci_le_create_con_cmd const *con_par,
+	       struct em_desc_node *con_req_pdu, uint16_t conhdl)
+{
+
+  struct ea_elt_tag *element1, *element2;
+  struct lld_evt_tag *environ1, *environ2;
+
+  uint32_t rand_val;
+  struct ea_param_input in_param;
+  struct ea_param_output out_param = { 0 };
+
+  struct llm_pdu_con_req_tx connreq_pdu;
+  uint8_t hop_increment = 0;
+  uint8_t pdu_len = 0;
+
+  uint8_t ral_idx = 0;
+  uint16_t ral_ptr = 0;
+  bool ral_en = false;
+
+  element1 = ea_elt_create (sizeof (struct lld_evt_tag));
+
+  if (!element1)
+    {
+      return NULL;
+    }
+
+  element2 = ea_elt_create (sizeof (struct lld_evt_tag));
+
+  if (!element2)
+    {
+      kernel_free (element1);
+      return NULL;
+    }
+
+  environ1 = LLD_EVT_ENV_ADDR_GET (element1);
+  environ2 = LLD_EVT_ENV_ADDR_GET (element2);
+
+  element1->start_latency = 2;
+  element1->duration_min = LLD_EVT_FRAME_DURATION;
+  element1->stop_latency1 = 0;
+  element1->stop_latency2 = 0;
+
+  environ1->conhdl = LLD_ADV_HDL;
+  environ1->mode = LLD_EVT_SCAN_MODE;
+  environ1->interval = con_par->scan_intv;
+  environ1->cs_ptr = EM_BLE_CS_OFFSET + LLD_ADV_HDL * REG_BLE_EM_CS_SIZE;
+
+  element2->start_latency = 2;
+  element2->stop_latency1 = 0;
+  element2->stop_latency2 = 0;
+
+  (environ1->evt).non_conn.window = SLOT_SIZE * con_par->scan_window;
+
+  lld_evt_init_evt (environ1);
+  lld_util_priority_set (element1, RWIP_PRIO_INIT_IDX);
+
+  element1->ea_cb_start = lld_evt_schedule;
+  element1->ea_cb_cancel = lld_evt_canceled;
+  element1->ea_cb_stop = lld_evt_prevent_stop;
+  element1->asap_settings = (EA_FLAG_ASAP_NO_LIMIT | EA_NO_PARITY) << 12;
+
+  element1->timestamp = CLK_ADD_3 (element1->start_latency,
+				   3, ea_time_get_halfslot_rounded ()) &
+    SYNC_TRAIN_TO_MAX;
+
+  in_param.interval_min = con_par->con_intv_min * 2;
+  in_param.interval_max = con_par->con_intv_max * 2;
+  in_param.duration_min = con_par->ce_len_min;
+  in_param.duration_max = con_par->ce_len_max;
+  in_param.pref_period = 0;
+  in_param.action = 0;
+  in_param.conhdl = conhdl;
+  in_param.role = MASTER_ROLE;
+  in_param.linkid = conhdl * REG_BLE_EM_CS_SIZE + EM_BLE_CS_OFFSET;
+
+  ea_interval_duration_req (&in_param, &out_param);
+
+  in_param.offset = 0;
+  in_param.odd_offset = 0;
+
+  if (ea_offset_req (&in_param, &out_param) == 1)
+    {
+      kernel_free (element1);
+      kernel_free (element2);
+
+      return NULL;
+    }
+
+  ble_txphadv_pack (con_req_pdu->idx, LLCP_CON_REQ_LEN,
+		    con_par->peer_addr_type & ADDR_MASK,
+		    ADDR_PUBLIC, LL_CONNECT_REQ);
+
+  llm_util_aa_gen (connreq_pdu.aa.addr);
+  llm_util_get_channel_map (&connreq_pdu.chm);
+
+  rand_val = rand ();
+
+  connreq_pdu.crcinit.crc[2] = (uint8_t) ((rand_val & 0x00FF0000) >> 16);
+  connreq_pdu.crcinit.crc[1] = (uint8_t) ((rand_val & 0x0000FF00) >> 8);
+  connreq_pdu.crcinit.crc[0] = (uint8_t) (rand_val & 0x000000FF);
+
+  connreq_pdu.timeout = con_par->superv_to;
+  connreq_pdu.latency = con_par->con_latency;
+  connreq_pdu.winsize = 2;
+  connreq_pdu.interval = out_param.interval * 2;
+
+  hop_increment = (uint8_t) (rand_val & 0x0000000F);
+
+  if (hop_increment < 5)
+    {
+
+      hop_increment += 5;
+    }
+
+  connreq_pdu.hop_sca = ((lld_evt_env.sca) << 5) | hop_increment;
+
+  lld_pdu_adv_pack (LL_CONNECT_REQ, (uint8_t *) (&connreq_pdu), &pdu_len);
+  em_wr (&connreq_pdu, ble_txdataptr_get (LLD_ADV_HDL), pdu_len);
+  llm_util_apply_bd_addr (con_par->own_addr_type);
+
+  if (!(con_par->peer_addr_type & ADDR_RPA_MASK)
+      && con_par->own_addr_type & ADDR_RPA_MASK)
+    {
+      ral_en = true;
+      if (!(con_par->init_filt_policy) &&
+	  llm_util_bd_addr_in_ral (&(con_par->peer_addr),
+				   con_par->peer_addr_type, &ral_idx))
+	{
+	  ral_ptr = EM_BLE_RAL_OFFSET + ral_idx * REG_BLE_EM_RAL_SIZE;
+	}
+    }
+
+  if ((con_par->peer_addr_type & ADDR_RPA_MASK)
+      && !(con_par->init_filt_policy))
+    {
+      ral_en = true;
+      if (llm_util_bd_addr_in_ral (&(con_par->peer_addr),
+				   con_par->peer_addr_type, &ral_idx))
+	{
+	  ral_ptr = EM_BLE_RAL_OFFSET + ral_idx * REG_BLE_EM_RAL_SIZE;
+	}
+    }
+
+  if ((con_par->peer_addr_type & ADDR_RPA_MASK) && con_par->init_filt_policy)
+    {
+      ral_en = !!(con_par->own_addr_type & ADDR_RPA_MASK);
+    }
+
+  element1->linked_element = element2;
+  lld_pdu_tx_push (element1, con_req_pdu);
+  lld_util_connection_param_set (element1, &out_param);
+
+  ble_linklbl_setf (LLD_ADV_HDL, 8);
+  ble_cntl_pack (LLD_ADV_HDL, 0xF, 1, 1, 0, LLD_INITIATING);
+
+  ble_filtpol_ralcntl_pack (LLD_ADV_HDL, con_par->init_filt_policy,
+			    !!(con_par->own_addr_type & ADDR_RPA_MASK),
+			    !!(ral_ptr), ral_en);
+
+  ble_conflict_setf (LLD_ADV_HDL, 0);
+  ble_hopcntl_pack (LLD_ADV_HDL, 1, 0x0, 0x27);
+  ble_fcntoffset_set (LLD_ADV_HDL, 0);
+
+  if (ral_ptr)
+    {
+      ble_peer_ralptr_set (LLD_ADV_HDL, ral_ptr);
+    }
+  else
+    {
+      ble_adv_bd_addr_set (LLD_ADV_HDL, 0, (con_par->peer_addr).addr[0] |
+			   (con_par->peer_addr).addr[1] << 8);
+      ble_adv_bd_addr_set (LLD_ADV_HDL, 1, (con_par->peer_addr).addr[2] |
+			   (con_par->peer_addr).addr[3] << 8);
+      ble_adv_bd_addr_set (LLD_ADV_HDL, 2, (con_par->peer_addr).addr[4] |
+			   (con_par->peer_addr).addr[5] << 8);
+      ble_adv_bd_addr_type_setf (LLD_ADV_HDL,
+				 con_par->peer_addr_type & ADDR_MASK);
+    }
+
+  ble_crcinit0_set (LLD_ADV_HDL, 0x5555);
+
+  ble_crcinit1_set (LLD_ADV_HDL, 0x55);
+
+  if (out_param.offset <= 4)
+    {
+      out_param.offset = connreq_pdu.interval * 2 + out_param.offset;
+    }
+
+  ble_winoffset_set (conhdl, (out_param.offset / 2 - 1));
+  ble_conninterval_set (LLD_ADV_HDL, out_param.interval / 2);
+
+  lld_evt_init_evt (environ2);
+
+  environ2->conhdl = conhdl;
+  environ2->interval = connreq_pdu.interval * 2;
+  (environ2->evt).conn.latency = con_par->con_latency + 1;
+  environ2->mode = LLD_EVT_MST_MODE;
+  environ2->cs_ptr = conhdl * REG_BLE_EM_CS_SIZE + EM_BLE_CS_OFFSET;
+
+  element2->timestamp =
+    CLK_ADD_2 (CLK_SUB (out_param.offset % environ2->interval,
+			environ2->interval), element1->timestamp);
+
+  if (con_par->ce_len_min)
+    {
+      element2->duration_min = SLOT_SIZE * con_par->ce_len_min;
+    }
+  else
+    {
+      element2->duration_min = LLD_EVT_FRAME_DURATION;
+    }
+
+  element2->current_prio = rwip_priority[RWIP_PRIO_LE_CON_IDLE_IDX].value;
+  element2->ea_cb_start = lld_evt_schedule;
+  element2->ea_cb_cancel = lld_evt_canceled;
+  element2->ea_cb_stop = lld_evt_prevent_stop;
+
+  ble_cntl_pack (conhdl, 0, 0, 0, 0, LLD_MASTER_CONNECTED);
+  ble_fcntoffset_set (conhdl, 0);
+  ble_txcrypt_en_setf (conhdl, 0);
+  ble_rxcrypt_en_setf (conhdl, 0);
+  ble_thrcntl_ratecntl_pack (conhdl, LLD_RX_IRQ_THRES, 0, 0);
+  ble_syncwordl_setf (conhdl,
+		      connreq_pdu.aa.addr[0] | connreq_pdu.aa.addr[1] << 8);
+  ble_syncwordh_setf (conhdl,
+		      connreq_pdu.aa.addr[2] | connreq_pdu.aa.addr[3] << 8);
+  ble_crcinit0_set (conhdl,
+		    connreq_pdu.crcinit.crc[0] | connreq_pdu.crcinit.
+		    crc[1] << 8);
+  ble_crcinit1_set (conhdl, connreq_pdu.crcinit.crc[2]);
+  ble_hopcntl_pack (conhdl, 1, hop_increment, 0);
+  ble_txrxcntl_pack (conhdl, 0, 0, 0, 0, rwip_rf.txpwr_max);
+  ble_maxevtime_set (conhdl, environ2->interval - element2->start_latency);
+  ble_linklbl_setf (conhdl, conhdl);
+  ble_rxmaxbuf_set (conhdl, 0);
+  ble_rxmaxtime_set (conhdl, 0);
+  ble_llchmap0_setf (conhdl,
+		     connreq_pdu.chm.map[0] | connreq_pdu.chm.map[1] << 8);
+  ble_llchmap1_setf (conhdl,
+		     connreq_pdu.chm.map[2] | connreq_pdu.chm.map[3] << 8);
+  ble_chmap2_pack (conhdl,
+		   llm_util_check_map_validity (connreq_pdu.chm.map,
+						LE_CHNL_MAP_LEN),
+		   connreq_pdu.chm.map[4]);
+
+  GLOBAL_INT_DIS ();
+  (environ1->evt).non_conn.anchor = element1->timestamp;
+  (environ1->evt).non_conn.end_ts =
+    CLK_ADD_2 (element1->timestamp,
+	       (environ1->evt).non_conn.window / SLOT_SIZE);
+  (environ1->evt).non_conn.initiate = true;
+  (environ1->evt).non_conn.connect_req_sent = false;
+  element1->duration_min = LLD_EVT_FRAME_DURATION;
+  GLOBAL_INT_RES ();
+
+  return element1;
+
+}
diff --git a/usr/ble_tst/src/stubs/ke_stubs.c b/usr/ble_tst/src/stubs/ke_stubs.c
new file mode 100644
index 0000000..4a329b6
--- /dev/null
+++ b/usr/ble_tst/src/stubs/ke_stubs.c
@@ -0,0 +1,86 @@
+/* 
+Various functions and data structures required by the KE kernel
+*/
+
+#include "mm_task.h"
+
+const struct ke_state_handler mm_default_handler = {0};
+const struct ke_state_handler mm_state_handler[MM_STATE_MAX] = {{0}};
+ke_state_t mm_state[MM_IDX_MAX] = {0};
+
+#include "scan_task.h"
+
+const struct ke_state_handler scan_default_handler = {0};
+ke_state_t scan_state[SCAN_IDX_MAX] = {0};
+
+#include "scanu_task.h"
+
+const struct ke_state_handler scanu_state_handler[SCANU_STATE_MAX] = {{0}};
+const struct ke_state_handler scanu_default_handler = {0};
+ke_state_t scanu_state[SCANU_IDX_MAX] = {0};
+
+#include "me_task.h"
+
+const struct ke_state_handler me_state_handler[ME_STATE_MAX] = {{0}};
+const struct ke_state_handler me_default_handler = {0};
+ke_state_t me_state[ME_IDX_MAX] = {0};
+
+#include "sm_task.h"
+
+const struct ke_state_handler sm_default_handler = {0};
+ke_state_t sm_state[SM_IDX_MAX] = {0};
+
+#include "apm_task.h"
+
+const struct ke_state_handler apm_default_handler = {0};
+ke_state_t apm_state[APM_IDX_MAX] = {0};
+
+#include "bam_task.h"
+
+const struct ke_state_handler bam_default_handler = {0};
+ke_state_t bam_state[BAM_IDX_MAX] = {0};
+
+//from txl_cntrl.h
+void txl_payload_handle(int access_category) {
+};
+
+//from txl_cfm.h
+void txl_cfm_evt(int dummy) {
+};
+
+//from mm.h
+void mm_tbtt_evt(int dummy){
+};
+
+void mm_hw_idle_evt(int dummy){
+};
+
+//from mm_timer.h
+void mm_timer_schedule(int dummy){
+};
+
+#include "rwnx.h"
+
+RW_CONNECTOR_T g_rwnx_connector;
+
+void rwnxl_reset_evt(int dummy){
+};
+
+//from reg_mac_pl.h
+void rwnxl_set_nxmac_timer_value_set_bit(uint32_t bit){
+};
+
+void rwnxl_set_nxmac_timer_value_clear_bit(uint32_t bit){
+};
+
+//from rxu_cntrl.h
+void rxu_cntrl_evt(int dummy){
+};
+
+//from txl_frame.h
+void txl_frame_evt(int dummy){
+};
+
+//from hal_dma.h
+void hal_dma_evt(int dma_queue){
+};
diff --git a/usr/ble_tst/src/stubs/other_stubs.c b/usr/ble_tst/src/stubs/other_stubs.c
new file mode 100644
index 0000000..3d79d13
--- /dev/null
+++ b/usr/ble_tst/src/stubs/other_stubs.c
@@ -0,0 +1,132 @@
+#include "rw_pub.h"
+
+unsigned char mhdr_get_station_status(){
+  return RW_EVT_STA_IDLE;
+}
+
+#include "td.h"
+void td_pck_ps_ind(uint8_t vif_index, bool rx){
+};
+
+void td_timer_end(void *env){
+};
+
+struct td_env_tag td_env[NX_VIRT_DEV_MAX] = {{{{0}}}};
+
+#include "vif_mgmt.h"
+
+struct vif_info_tag vif_info_tab[NX_VIRT_DEV_MAX] = {{{0}}};
+struct vif_mgmt_env_tag vif_mgmt_env = {{0}};
+
+void rwnx_printf( char *fmt, ...) {
+
+}
+
+#include "rwnx.h"
+bool rwnxl_sleep(IDLE_FUNC wait_func,IDLE_FUNC do_func) {
+  return true;
+};
+
+void rwnxl_wakeup(IDLE_FUNC wait_func){
+};
+
+bool rwnxl_get_status_in_doze(void){
+  return true;
+};
+
+void tpc_init(void){
+};
+
+void tpc_deinit(void){
+};
+
+#include "chan.h"
+bool chan_is_on_channel(struct vif_info_tag *vif_entry){
+  return false;
+}
+
+#include "mm_timer.h"
+void mm_timer_set(struct mm_timer_tag *timer, uint32_t value){
+};
+
+
+//from app_bk.c
+#include "include.h"
+#include "mem_pub.h"
+#include "rwnx_config.h"
+#include "app.h"
+
+#if (NX_POWERSAVE)
+#include "ps.h"
+#endif //(NX_POWERSAVE)
+
+#include "sa_ap.h"
+#include "sa_station.h"
+#include "main_none.h"
+#include "sm.h"
+#include "uart_pub.h"
+
+#include "rtos_pub.h"
+#include "rtos_error.h"
+#include "param_config.h"
+#include "rxl_cntrl.h"
+#include "lwip/pbuf.h"
+#include "rw_pub.h"
+#include "rw_msg_rx.h"
+#include "hostapd_intf_pub.h"
+#include "wlan_ui_pub.h"
+#include "ps_debug_pub.h"
+#include "power_save_pub.h"
+#include "mcu_ps_pub.h"
+#include "rw_msdu.h"
+#include "txu_cntrl.h"
+
+#include "app_music_pub.h"
+#include "bk7011_cal_pub.h"
+
+WIFI_CORE_T g_wifi_core = {0};
+
+void bmsg_null_sender(void)
+{
+    OSStatus ret;
+    BUS_MSG_T msg;
+
+    msg.type = BMSG_NULL_TYPE;
+    msg.arg = 0;
+    msg.len = 0;
+    msg.sema = NULL;
+
+    if(!rtos_is_queue_empty(&g_wifi_core.io_queue))
+    {
+        return;
+    }
+
+    ret = rtos_push_to_queue(&g_wifi_core.io_queue, &msg, BEKEN_NO_WAIT);
+    if(kNoErr != ret)
+    {
+        os_printf("bmsg_null_sender_failed\r\n");
+    }
+}
+
+void bmsg_ps_sender(uint8_t arg)
+{
+    OSStatus ret;
+    BUS_MSG_T msg;
+    if(g_wifi_core.io_queue)
+    {
+        msg.type = BMSG_STA_PS_TYPE;
+        msg.arg = (uint32_t)arg;
+        msg.len = 0;
+        msg.sema = NULL;
+
+        ret = rtos_push_to_queue(&g_wifi_core.io_queue, &msg, BEKEN_NO_WAIT);
+        if(kNoErr != ret)
+        {
+            os_printf("bmsg_ps_sender failed\r\n");
+        }
+    }
+    else
+    {
+        os_printf("g_wifi_core.io_queue null\r\n");
+    }
+}
diff --git a/usr/ble_tst/src/stubs/re_stubs.S b/usr/ble_tst/src/stubs/re_stubs.S
new file mode 100644
index 0000000..774a390
--- /dev/null
+++ b/usr/ble_tst/src/stubs/re_stubs.S
@@ -0,0 +1,55 @@
+        .thumb
+        .globl rwble_reset
+        .thumb_func
+        
+rwble_reset:
+        push	{r3, r4, r5, lr}
+        bl	portDISABLE_FIQ
+        add	r5, r0, #0
+        bl	portDISABLE_IRQ
+        movs	r2, #128	// 0x80
+        add	r4, r0, #0
+        ldr	r3, BLE_BASETIMECNT_ADDR	
+        lsl	r2, r2, #24
+        str	r2, [r3, #0]
+2:     
+        ldr	r2, [r3, #0]
+        cmp	r2, #0
+        blt	2b
+        ldr	r2, BLE_FINETIMECNT_ADDR	
+        ldr	r3, BLE_BASETIMECNT_ADDR	
+        ldr	r3, [r3, #0]
+        ldr	r0, [r2, #0]
+        add	r0, r0, r3
+        ldr	r3, BLE_BDADDRL_ADDR	
+        ldr	r3, [r3, #0]
+        add	r0, r0, r3
+        bl	srand
+        bl	lld_core_reset
+        movs	r0, #1
+        bl	lld_init
+        movs	r0, #1
+        bl	lld_adv_test_init
+        bl	llc_reset
+        movs	r0, #1
+        bl	llm_init
+        bl	em_buf_init
+        cmp	r5, #0
+        bne	0f
+        bl	portENABLE_FIQ
+0:     
+        cmp	r4, #0
+        bne	1f
+        bl	portENABLE_IRQ
+1:       
+        pop	{r3, r4, r5, pc}
+
+        .balign 4
+        
+BLE_BASETIMECNT_ADDR:   
+        .word	0x0081001c   // BLE_BASETIMECNT_ADDR
+BLE_FINETIMECNT_ADDR:   
+        .word	0x00810020   // BLE_FINETIMECNT_ADDR
+BLE_BDADDRL_ADDR:
+        .word	0x00810024   // BLE_BDADDRL_ADDR
+        
diff --git a/usr/ble_tst/src/stubs/re_stubs.c b/usr/ble_tst/src/stubs/re_stubs.c
new file mode 100644
index 0000000..f9dabb5
--- /dev/null
+++ b/usr/ble_tst/src/stubs/re_stubs.c
@@ -0,0 +1,51 @@
+#include "include.h"
+
+#include "rwble_hl.h"
+#include "gattm.h"
+#include "gapm.h"
+#include "attm.h"
+#include "l2cm.h"
+
+void rwble_hl_init(void) {
+  bk_printf("BLE stack init: %s\n", __func__);
+  attm_init(0);
+  gapm_init(0);
+  gattm_init(0);
+  l2cm_init(0);
+}
+
+/*
+0003dca0 <rwble_hl_init>:
+   3dca0:	b508      	push	{r3, lr}
+   3dca2:	2000      	movs	r0, #0
+   3dca4:	f7f0 fc86 	bl	2e5b4 <attm_init>
+   3dca8:	2000      	movs	r0, #0
+   3dcaa:	f7ec fe3d 	bl	2a928 <gapm_init>
+   3dcae:	2000      	movs	r0, #0
+   3dcb0:	f7ef fdda 	bl	2d868 <gattm_init>
+   3dcb4:	2000      	movs	r0, #0
+   3dcb6:	f006 fa3b 	bl	44130 <l2cm_init>
+   3dcba:	bd08      	pop	{r3, pc}
+*/
+
+void rwble_hl_reset(void) {
+  bk_printf("BLE stack reset: %s\n", __func__);
+  attm_init(1);
+  gapm_init(1);
+  gattm_init(1);
+  l2cm_init(1);
+}
+
+/*
+0003dcc8 <rwble_hl_reset>:
+   3dcc8:	b508      	push	{r3, lr}
+   3dcca:	2001      	movs	r0, #1
+   3dccc:	f7f0 fc86 	bl	2e5dc <attm_init>
+   3dcd0:	2001      	movs	r0, #1
+   3dcd2:	f7ec fe3d 	bl	2a950 <gapm_init>
+   3dcd6:	2001      	movs	r0, #1
+   3dcd8:	f7ef fdda 	bl	2d890 <gattm_init>
+   3dcdc:	2001      	movs	r0, #1
+   3dcde:	f006 fa2d 	bl	4413c <l2cm_init>
+   3dce2:	bd08      	pop	{r3, pc}
+*/
diff --git a/usr/ble_tst/src/stubs/stubs.c b/usr/ble_tst/src/stubs/stubs.c
new file mode 100644
index 0000000..9dadf75
--- /dev/null
+++ b/usr/ble_tst/src/stubs/stubs.c
@@ -0,0 +1,173 @@
+//stubs
+#include "wlan_ui_pub.h"
+#include "sk_intf.h"
+
+//from wlan_ui.c
+static monitor_data_cb_t g_bcn_cb = 0;
+monitor_data_cb_t g_monitor_cb = 0;
+int g_set_channel_postpone_num = 0;
+
+int bk_wlan_is_monitor_mode(void)
+{
+  return (0 == g_monitor_cb) ? false : true;
+}
+
+monitor_data_cb_t bk_wlan_get_bcn_cb(void)
+{
+    return g_bcn_cb;
+}
+
+monitor_data_cb_t bk_wlan_get_monitor_cb(void)
+{
+    if (g_monitor_cb)
+    {
+    	return g_monitor_cb;
+    }
+    else
+	{
+        return NULL;
+    }
+}
+
+int bk_wlan_is_general_sniffer_type(void)
+{
+  return false;//(MTR_GENERAL_SNIFFER_TYPE == g_monitor_type);
+}
+
+uint32_t bk_sta_cipher_is_open(void)
+{
+  //    ASSERT(g_sta_param_ptr);
+  return false;//(SECURITY_TYPE_NONE == g_sta_param_ptr->cipher_suite);
+}
+
+
+int bk_wlan_dtim_rf_ps_mode_do_wakeup()
+{
+  //    void *sem_list = NULL;
+
+	UINT32 ret = 0;
+  #if 0
+	sem_list = power_save_rf_ps_wkup_semlist_create();
+
+	if (!sem_list)
+    {
+        os_printf("err ---- NULL\r\n");
+        ASSERT(0);
+    }
+
+    GLOBAL_INT_DECLARATION();
+    GLOBAL_INT_DISABLE();
+
+    if((power_save_if_ps_rf_dtim_enabled()
+            && power_save_if_rf_sleep()) || ble_switch_mac_sleeped)
+    {
+        power_save_rf_ps_wkup_semlist_wait(sem_list);
+    }
+    else
+    {
+        power_save_rf_ps_wkup_semlist_destroy(sem_list);
+        os_free(sem_list);
+	sem_list = NULL;
+    }
+
+    GLOBAL_INT_RESTORE();
+
+    power_save_rf_ps_wkup_semlist_get(sem_list);
+#endif
+    return ret;
+}
+
+
+void bk_wlan_ap_set_channel_config(uint8_t channel)
+{
+  //    g_ap_param_ptr->chann = channel;
+}
+
+UINT32 if_other_mode_rf_sleep(void){
+  return false;
+}
+
+int bk_wlan_mcu_suppress_and_sleep(UINT32 sleep_ticks ){
+  return 0;
+}
+
+// from wpa_ie.c
+struct wpa_ie_data;
+
+int wpa_parse_wpa_ie(const u8 *wpa_ie, size_t wpa_ie_len,
+		     struct wpa_ie_data *data)
+{
+#if 0
+  if (wpa_ie_len >= 1 && wpa_ie[0] == WLAN_EID_RSN)
+    return wpa_parse_wpa_ie_rsn(wpa_ie, wpa_ie_len, data);
+  if (wpa_ie_len >= 6 && wpa_ie[0] == WLAN_EID_VENDOR_SPECIFIC &&
+      wpa_ie[1] >= 4 && WPA_GET_BE32(&wpa_ie[2]) == OSEN_IE_VENDOR_TYPE)
+    return wpa_parse_wpa_ie_rsn(wpa_ie, wpa_ie_len, data);
+  else
+    return wpa_parse_wpa_ie_wpa(wpa_ie, wpa_ie_len, data);
+#endif
+  return 0;
+}
+
+//from sk_intf.c 
+int ke_mgmt_packet_tx(unsigned char *buf, int len, int flag)
+{
+  int ret = 0;
+  
+#if 0
+  int ret, poll_flag = 0;
+  SOCKET sk = mgmt_get_socket_num(flag);
+  
+  ret = ke_sk_send(sk, buf, len, flag);
+  if(ret)
+    {
+      poll_flag = wpa_hostapd_queue_poll((uint32_t)flag);
+    }
+  
+  if(poll_flag)
+    {
+      handle_dummy_read(sk, NULL, NULL);
+    }
+#endif
+  return ret;
+}
+
+//from pbuf.c
+u8_t
+pbuf_free(struct pbuf *p)
+{
+  return 0;
+}
+
+void
+pbuf_chain(struct pbuf *h, struct pbuf *t)
+{
+}
+
+struct pbuf *
+pbuf_dechain(struct pbuf *p)
+{
+  return NULL;
+}
+
+struct pbuf *
+pbuf_alloc(pbuf_layer layer, u16_t length, pbuf_type type)
+{
+  return NULL;
+}
+
+
+//from rwnx.h
+void rwnxl_init(void){}
+
+// compiler experiments: can be removed
+
+int secret_int = 0;
+
+int *get_that_int(void) {
+  return &secret_int;
+}
+
+void change_that_int(int a) {
+  secret_int = a;
+}