123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433 |
- #include <stdint.h>
- #include <string.h>
- #include "es_slot.h"
- #include "es_flash.h"
- #include "es_security.h"
- #include "es_slot_reg.h"
- #include "es_tlm.h"
- #include "fds.h"
- static es_slot_reg_t m_reg;
- static bool m_eid_loaded_from_flash;
- #define RANGING_DATA_INDEX (1)
- #define RANGING_DATA_LENGTH (1)
- static void slot_boundary_check(uint8_t * p_slot)
- {
- if (*p_slot > (APP_MAX_ADV_SLOTS - 1))
- {
- *p_slot = (APP_MAX_ADV_SLOTS - 1);
- }
- }
- static void load_slot_from_flash(uint8_t slot_no)
- {
- ret_code_t err_code;
- err_code = es_flash_access_slot_configs(slot_no, &m_reg.slots[slot_no], ES_FLASH_ACCESS_READ);
- if (err_code != FDS_ERR_NOT_FOUND)
- {
- APP_ERROR_CHECK(err_code);
- if (m_reg.slots[slot_no].adv_frame.type == ES_FRAME_TYPE_EID)
- {
- m_eid_loaded_from_flash = true;
- es_security_eid_slots_restore(slot_no,
- m_reg.slots[slot_no].k_scaler,
- m_reg.slots[slot_no].seconds,
- (const uint8_t *)m_reg.slots[slot_no].ik);
- }
- else
- {
-
- es_slot_reg_update_slot_list_info_on_add(&m_reg, slot_no, m_reg.slots[slot_no].adv_frame.type, true);
- }
- }
- }
- static void set_ranging_data_for_slot(uint8_t slot_no, nrf_ble_escs_radio_tx_pwr_t tx_power)
- {
- int8_t ranging_data_array[ESCS_NUM_OF_SUPPORTED_TX_POWER] = APP_CONFIG_CALIBRATED_RANGING_DATA;
- nrf_ble_escs_radio_tx_pwr_t supported_tx[ESCS_NUM_OF_SUPPORTED_TX_POWER] =
- ESCS_SUPPORTED_TX_POWER;
- int8_t ranging_data = 0;
- if (m_reg.slots[slot_no].adv_custom_tx_power)
- {
- ranging_data = m_reg.slots[slot_no].custom_tx_power;
- }
- else
- {
- for (uint32_t i = 0; i < ESCS_NUM_OF_SUPPORTED_TX_POWER; ++i)
- {
- if (supported_tx[i] >= tx_power)
- {
- ranging_data = ranging_data_array[i];
- break;
- }
- }
- }
- es_adv_frame_t * frame = &m_reg.slots[slot_no].adv_frame;
- switch (frame->type)
- {
- case ES_FRAME_TYPE_UID:
- {
- es_uid_frame_t * uid = &frame->frame.uid;
- uid->ranging_data = ranging_data;
- break;
- }
- case ES_FRAME_TYPE_URL:
- {
- es_url_frame_t * url = &frame->frame.url;
- url->ranging_data = ranging_data;
- break;
- }
- case ES_FRAME_TYPE_EID:
- {
- es_eid_frame_t * eid = &frame->frame.eid;
- eid->ranging_data = ranging_data;
- break;
- }
- case ES_FRAME_TYPE_TLM:
- APP_ERROR_CHECK(NRF_ERROR_INVALID_PARAM);
- break;
- }
- }
- static void configure_slot(uint8_t slot_no, uint8_t length, uint8_t const * p_frame_data)
- {
-
- if ((es_frame_type_t)p_frame_data[0] == ES_FRAME_TYPE_TLM && m_reg.tlm_configured)
- {
- return;
- }
- es_slot_reg_update_slot_list_info_on_add(&m_reg, slot_no, (es_frame_type_t)p_frame_data[0], false);
-
- m_reg.slots[slot_no].adv_frame.type = (es_frame_type_t)p_frame_data[0];
- memcpy(&m_reg.slots[slot_no].adv_frame.frame, &m_reg.slots[slot_no].adv_frame.type, 1);
- uint8_t * p_data_after_ranging_data = ((uint8_t *)(&m_reg.slots[slot_no].adv_frame.frame) +
- RANGING_DATA_INDEX + RANGING_DATA_LENGTH);
- switch (m_reg.slots[slot_no].adv_frame.type)
- {
- case ES_FRAME_TYPE_UID:
-
- case ES_FRAME_TYPE_URL:
- memcpy(p_data_after_ranging_data, &p_frame_data[1], length - 1);
- set_ranging_data_for_slot(slot_no, APP_CFG_DEFAULT_RADIO_TX_POWER);
- m_reg.slots[slot_no].adv_frame.length = length + 1;
- break;
- case ES_FRAME_TYPE_TLM:
- es_tlm_tlm_get(&m_reg.slots[slot_no].adv_frame.frame.tlm);
- m_reg.slots[slot_no].adv_frame.length = ES_TLM_LENGTH;
- break;
- default:
- break;
- }
- }
- static void configure_eid_slot(uint8_t slot_no, uint8_t length, uint8_t const * p_frame_data)
- {
-
-
-
-
- (void)es_slot_reg_clear_slot(&m_reg, slot_no);
- if (p_frame_data[0] != ES_FRAME_TYPE_EID)
- {
- APP_ERROR_CHECK(NRF_ERROR_INVALID_STATE);
- }
- if (length == ESCS_EID_WRITE_ECDH_LENGTH)
- {
- es_security_client_pub_ecdh_receive(slot_no,
- (uint8_t*)&p_frame_data[ESCS_EID_WRITE_PUB_KEY_INDEX],
- p_frame_data[ESCS_EID_WRITE_ECDH_LENGTH -1]);
- }
- else if (length == ESCS_EID_WRITE_IDK_LENGTH)
- {
- es_security_shared_ik_receive(slot_no,
- (uint8_t*)&p_frame_data[ESCS_EID_WRITE_ENC_ID_KEY_INDEX],
- p_frame_data[ESCS_EID_WRITE_IDK_LENGTH - 1]);
- }
- else
- {
-
- APP_ERROR_CHECK(NRF_ERROR_INVALID_PARAM);
- }
- }
- ret_code_t es_slot_write_to_flash(uint8_t slot_no)
- {
- if (m_reg.slots[slot_no].configured)
- {
-
- if (m_reg.slots[slot_no].adv_frame.type == ES_FRAME_TYPE_EID)
- {
- m_reg.slots[slot_no].seconds = es_security_clock_get(slot_no);
- m_reg.slots[slot_no].k_scaler = es_security_scaler_get(slot_no);
- es_security_plain_eid_id_key_get(slot_no, m_reg.slots[slot_no].ik);
- }
- return es_flash_access_slot_configs(slot_no, &m_reg.slots[slot_no], ES_FLASH_ACCESS_WRITE);
- }
- else
- {
- return es_flash_access_slot_configs(slot_no, NULL, ES_FLASH_ACCESS_CLEAR);
- }
- }
- void es_slot_radio_tx_pwr_set(uint8_t slot_no, nrf_ble_escs_radio_tx_pwr_t radio_tx_pwr)
- {
- slot_boundary_check(&slot_no);
- m_reg.slots[slot_no].radio_tx_pwr = radio_tx_pwr;
- if (!m_reg.slots[slot_no].adv_custom_tx_power)
- {
- set_ranging_data_for_slot(slot_no, radio_tx_pwr);
- }
- }
- void es_slot_set_adv_custom_tx_power(uint8_t slot_no, nrf_ble_escs_adv_tx_pwr_t tx_pwr)
- {
- slot_boundary_check(&slot_no);
- m_reg.slots[slot_no].adv_custom_tx_power = true;
- m_reg.slots[slot_no].custom_tx_power = tx_pwr;
- set_ranging_data_for_slot(slot_no, tx_pwr);
- }
- void es_slot_on_write(uint8_t slot_no, uint8_t length, uint8_t const * p_frame_data)
- {
- slot_boundary_check(&slot_no);
- if (p_frame_data == NULL)
- {
- APP_ERROR_CHECK(NRF_ERROR_NULL);
- }
-
- if (length == 0 || (length == 1 && p_frame_data[0] == 0))
- {
- (void)es_slot_reg_clear_slot(&m_reg, slot_no);
- }
-
- else if (p_frame_data[0] == ES_FRAME_TYPE_EID &&
- (length == ESCS_EID_WRITE_ECDH_LENGTH || length == ESCS_EID_WRITE_IDK_LENGTH))
- {
- if (m_reg.slots[slot_no].configured)
- (void)es_slot_reg_clear_slot(&m_reg, slot_no);
- configure_eid_slot(slot_no, length, p_frame_data);
- }
-
- else
- {
- if (m_reg.slots[slot_no].configured)
- (void)es_slot_reg_clear_slot(&m_reg, slot_no);
- configure_slot(slot_no, length, p_frame_data);
- }
- }
- void es_slot_encrypted_eid_id_key_set(uint8_t slot_no, nrf_ble_escs_eid_id_key_t * p_eid_id_key)
- {
- slot_boundary_check(&slot_no);
- if (p_eid_id_key != NULL)
- {
- memcpy(&(m_reg.slots[slot_no].encrypted_eid_id_key), p_eid_id_key,
- sizeof(nrf_ble_escs_eid_id_key_t));
- }
- }
- void es_slot_eid_ready(uint8_t slot_no)
- {
- m_reg.slots[slot_no].adv_frame.type = ES_FRAME_TYPE_EID;
- m_reg.slots[slot_no].adv_frame.length = ES_EID_LENGTH;
- es_security_eid_get(slot_no, (uint8_t *)m_reg.slots[slot_no].adv_frame.frame.eid.eid);
- m_reg.slots[slot_no].adv_frame.frame.eid.frame_type = ES_FRAME_TYPE_EID;
- set_ranging_data_for_slot(slot_no, m_reg.slots[slot_no].radio_tx_pwr);
- if (m_eid_loaded_from_flash)
- {
- es_slot_reg_update_slot_list_info_on_add(&m_reg, slot_no, ES_FRAME_TYPE_EID, true);
- m_eid_loaded_from_flash = false;
- }
- else
- {
- es_slot_reg_update_slot_list_info_on_add(&m_reg, slot_no, ES_FRAME_TYPE_EID, false);
- }
- }
- static bool slot_is_eid(uint8_t eid_slot_no)
- {
- for (uint32_t i = 0; i < m_reg.num_configured_eid_slots; ++i)
- {
- if (m_reg.eid_slots_configured[i] == eid_slot_no)
- {
- return true;
- }
- }
- return false;
- }
- void es_slot_tlm_update(void)
- {
- if (m_reg.tlm_configured)
- {
- es_tlm_tlm_get(&m_reg.slots[m_reg.tlm_slot].adv_frame.frame.tlm);
- }
- }
- void es_slot_etlm_update(uint8_t eid_slot_no)
- {
- es_tlm_frame_t tlm;
- es_etlm_frame_t etlm;
-
- if (!es_slot_reg_etlm_required(&m_reg) || !slot_is_eid(eid_slot_no))
- {
- return;
- }
- es_tlm_tlm_get(&tlm);
- es_security_tlm_to_etlm(eid_slot_no, &tlm, &etlm);
- memcpy(&m_reg.slots[m_reg.tlm_slot].adv_frame.frame.etlm, &etlm, sizeof(es_etlm_frame_t));
- m_reg.slots[m_reg.tlm_slot].adv_frame.length = sizeof(es_etlm_frame_t);
- }
- const es_slot_reg_t * es_slot_get_registry(void)
- {
- return (const es_slot_reg_t *)&m_reg;
- }
- void es_slots_init(const es_slot_t * p_default_slot)
- {
- ret_code_t err_code;
- es_flash_flags_t flash_flags = {{0}};
- es_slot_reg_init(&m_reg);
- m_eid_loaded_from_flash = false;
-
- err_code = es_flash_access_flags(&flash_flags, ES_FLASH_ACCESS_READ);
- if (err_code == FDS_ERR_NOT_FOUND)
- {
-
- memcpy(&m_reg.slots[0], p_default_slot, sizeof(*p_default_slot));
- es_slot_reg_update_slot_list_info_on_add(&m_reg, 0, p_default_slot->adv_frame.type, true);
- }
- else
- {
- APP_ERROR_CHECK(err_code);
- for (uint32_t i = 0; i < APP_MAX_ADV_SLOTS; ++i)
- {
- if (!flash_flags.slot_is_empty[i])
- {
- load_slot_from_flash(i);
- }
- }
- }
- }
|