diff options
Diffstat (limited to 'usr/ble_tst/src/lld_re.c')
| -rw-r--r-- | usr/ble_tst/src/lld_re.c | 505 |
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; + +} |