Import bk_ble from elektroda forum

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

1 files changed, 505 insertions, 0 deletions

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;
+
+}