#include "RomCallFlash.h"
#include "ble_reg_access.h"
#include "common_hci.h"
#include "common_llcp.h"
#include "common_utils.h"
#include "ea.h"
#include "ecc_p256.h"
#include "em_buf.h"
#include "em_map_ble.h"
#include "lld.h"
#include "lld_evt.h"
#include "lld_pdu.h"
#include "lld_util.h"
#include "llm.h"
#include "llm_util.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_buffer_data.h"
#include "reg_ble_em_tx_desc.h"
#include "reg_blecore.h"
#include "rwip.h"
#include "rwip_config.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;
}