123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622 |
- #include <string.h>
- #include "nrf_ble_bms.h"
- #include "ble_srv_common.h"
- #define NRF_LOG_MODULE_NAME ble_bms
- #include "nrf_log.h"
- NRF_LOG_MODULE_REGISTER();
- static void error_check(ret_code_t err_code, ble_srv_error_handler_t err_handler)
- {
- if (err_code != NRF_SUCCESS)
- {
- err_handler(err_code);
- }
- }
- static ret_code_t ctrlpt_char_add(nrf_ble_bms_t * p_bms, nrf_ble_bms_init_t const * p_bms_init)
- {
- ble_add_char_params_t add_char_params;
- memset(&add_char_params, 0, sizeof(add_char_params));
- add_char_params.uuid = BLE_UUID_BMS_CTRLPT;
- add_char_params.char_props.write = 1;
- add_char_params.is_defered_write = true;
- add_char_params.is_var_len = true;
- add_char_params.max_len = NRF_BLE_BMS_CTRLPT_MAX_LEN;
- add_char_params.read_access = SEC_NO_ACCESS;
- add_char_params.write_access = p_bms_init->bms_ctrlpt_sec_req;
- if (p_bms_init->p_qwr != NULL)
- {
- add_char_params.char_ext_props.reliable_wr = 1;
- }
- return characteristic_add(p_bms->service_handle, &add_char_params, &p_bms->ctrlpt_handles);
- }
- static void ctrlpt_auth(nrf_ble_bms_t * p_bms, nrf_ble_bms_ctrlpt_t * p_ctrlpt)
- {
- nrf_ble_bms_features_t * p_feature = &p_bms->feature;
- p_bms->auth_status = NRF_BLE_BMS_AUTH_STATUS_ALLOWED;
-
- if ((
- (p_ctrlpt->op_code == NRF_BLE_BMS_OP_DEL_BOND_REQ_DEVICE_LE_ONLY) &&
- (p_feature->delete_requesting_auth)
- ) ||
- (
- (p_ctrlpt->op_code == NRF_BLE_BMS_OP_DEL_ALL_BONDS_ON_SERVER_LE_ONLY) &&
- (p_feature->delete_all_auth)
- ) ||
- (
- (p_ctrlpt->op_code == NRF_BLE_BMS_OP_DEL_ALL_BUT_ACTIVE_BOND_LE_ONLY) &&
- (p_feature->delete_all_but_requesting_auth)
- )
- )
- {
- if (p_bms->evt_handler != NULL)
- {
- nrf_ble_bms_evt_t bms_evt;
- memset(&bms_evt, 0, sizeof(bms_evt));
- bms_evt.evt_type = NRF_BLE_BMS_EVT_AUTH;
- bms_evt.auth_code.len = p_ctrlpt->auth_code.len;
- memcpy(bms_evt.auth_code.code, p_ctrlpt->auth_code.code, p_ctrlpt->auth_code.len);
- p_bms->auth_status = NRF_BLE_BMS_AUTH_STATUS_PENDING;
- p_bms->evt_handler(p_bms, &bms_evt);
- }
- else
- {
- p_bms->auth_status = NRF_BLE_BMS_AUTH_STATUS_DENIED;
- }
- }
- }
- static ret_code_t ctrlpt_decode(uint8_t const * p_rcvd_val,
- uint16_t len,
- nrf_ble_bms_ctrlpt_t * p_ctrlpt)
- {
- uint16_t pos = 0;
- VERIFY_TRUE(len >= NRF_BLE_BMS_CTRLPT_MIN_LEN, NRF_ERROR_INVALID_LENGTH);
- VERIFY_TRUE(len <= NRF_BLE_BMS_CTRLPT_MAX_LEN, NRF_ERROR_INVALID_LENGTH);
- p_ctrlpt->op_code = (nrf_ble_bms_op_t) p_rcvd_val[pos++];
- p_ctrlpt->auth_code.len = (len - pos);
- memcpy(p_ctrlpt->auth_code.code, &(p_rcvd_val[pos]), p_ctrlpt->auth_code.len);
- return NRF_SUCCESS;
- }
- static void ctrlpt_execute(nrf_ble_bms_t * p_bms, nrf_ble_bms_op_t op_code)
- {
- switch (op_code)
- {
- case NRF_BLE_BMS_OP_DEL_BOND_REQ_DEVICE_LE_ONLY:
-
- p_bms->bond_callbacks.delete_requesting(p_bms);
- break;
- case NRF_BLE_BMS_OP_DEL_ALL_BONDS_ON_SERVER_LE_ONLY:
-
- p_bms->bond_callbacks.delete_all(p_bms);
- break;
- case NRF_BLE_BMS_OP_DEL_ALL_BUT_ACTIVE_BOND_LE_ONLY:
-
- p_bms->bond_callbacks.delete_all_except_requesting(p_bms);
- break;
- default:
-
- break;
- }
- }
- static bool ctrlpt_validate(nrf_ble_bms_ctrlpt_t * p_ctrlpt, nrf_ble_bms_features_t * p_feature)
- {
- switch (p_ctrlpt->op_code)
- {
- case NRF_BLE_BMS_OP_DEL_BOND_REQ_DEVICE_LE_ONLY:
- if (p_feature->delete_requesting || p_feature->delete_requesting_auth)
- {
- return true;
- }
- break;
- case NRF_BLE_BMS_OP_DEL_ALL_BONDS_ON_SERVER_LE_ONLY:
- if (p_feature->delete_all || p_feature->delete_all_auth)
- {
- return true;
- }
- break;
- case NRF_BLE_BMS_OP_DEL_ALL_BUT_ACTIVE_BOND_LE_ONLY:
- if (p_feature->delete_all_but_requesting || p_feature->delete_all_but_requesting_auth)
- {
- return true;
- }
- break;
- default:
-
- break;
- }
- return false;
- }
- static uint16_t ctrlpt_process(nrf_ble_bms_t * p_bms,
- uint8_t const * p_rcvd_val,
- uint16_t len,
- nrf_ble_bms_ctrlpt_t * p_ctrlpt)
- {
- ret_code_t err_code;
-
- err_code = ctrlpt_decode(p_rcvd_val, len, p_ctrlpt);
- if (err_code != NRF_SUCCESS)
- {
- NRF_LOG_ERROR("Control point write: Operation failed.");
- return NRF_BLE_BMS_OPERATION_FAILED;
- }
-
- if (!ctrlpt_validate(p_ctrlpt, &p_bms->feature))
- {
- NRF_LOG_ERROR("Control point write: Invalid op code.");
- return NRF_BLE_BMS_OPCODE_NOT_SUPPORTED;
- }
-
- ctrlpt_auth(p_bms, p_ctrlpt);
- if (p_bms->auth_status != NRF_BLE_BMS_AUTH_STATUS_ALLOWED)
- {
- NRF_LOG_ERROR("Control point long write: Invalid auth.");
- return BLE_GATT_STATUS_ATTERR_INSUF_AUTHORIZATION;
- }
- return BLE_GATT_STATUS_SUCCESS;
- }
- static uint8_t feature_encode(nrf_ble_bms_features_t const * p_feature, uint8_t * p_encoded_feature)
- {
- uint32_t data = 0;
- if (p_feature->delete_all_auth)
- {
- data |= NRF_BLE_BMS_ALL_BONDS_LE_AUTH_CODE;
- }
- if (p_feature->delete_all_but_requesting_auth)
- {
- data |= NRF_BLE_BMS_ALL_EXCEPT_REQUESTING_DEVICE_LE_AUTH_CODE;
- }
- if (p_feature->delete_all_but_requesting)
- {
- data |= NRF_BLE_BMS_ALL_EXCEPT_REQUESTING_DEVICE_LE;
- }
- if (p_feature->delete_all)
- {
- data |= NRF_BLE_BMS_ALL_BONDS_LE;
- }
- if (p_feature->delete_requesting_auth)
- {
- data |= NRF_BLE_BMS_REQUESTING_DEVICE_LE_AUTH_CODE;
- }
- if (p_feature->delete_requesting)
- {
- data |= NRF_BLE_BMS_REQUESTING_DEVICE_LE;
- }
- return (uint24_encode(data, p_encoded_feature));
- }
- static ret_code_t feature_char_add(nrf_ble_bms_t * p_bms, nrf_ble_bms_init_t const * p_bms_init)
- {
- uint8_t encoded_feature[NRF_BLE_BMS_FEATURE_LEN];
- uint8_t init_value_len;
- ble_add_char_params_t add_char_params;
- nrf_ble_bms_features_t * p_feature = &p_bms->feature;
- if ((p_feature->delete_all_auth) ||
- (p_feature->delete_all_but_requesting_auth) ||
- (p_feature->delete_requesting_auth))
- {
- VERIFY_PARAM_NOT_NULL(p_bms_init->evt_handler);
- }
- if ((p_feature->delete_requesting_auth) ||
- (p_feature->delete_requesting))
- {
- VERIFY_PARAM_NOT_NULL(p_bms_init->bond_callbacks.delete_requesting);
- }
- if ((p_feature->delete_all) ||
- (p_feature->delete_all_auth))
- {
- VERIFY_PARAM_NOT_NULL(p_bms_init->bond_callbacks.delete_all);
- }
- if ((p_feature->delete_all_but_requesting) ||
- (p_feature->delete_all_but_requesting_auth))
- {
- VERIFY_PARAM_NOT_NULL(p_bms_init->bond_callbacks.delete_all_except_requesting);
- }
- init_value_len = feature_encode(&p_bms->feature, encoded_feature);
- memset(&add_char_params, 0, sizeof(add_char_params));
- add_char_params.uuid = BLE_UUID_BMS_FEATURE;
- add_char_params.char_props.read = true;
- add_char_params.max_len = init_value_len;
- add_char_params.p_init_value = encoded_feature;
- add_char_params.init_len = init_value_len;
- add_char_params.read_access = p_bms_init->bms_feature_sec_req;
- add_char_params.write_access = SEC_NO_ACCESS;
- return characteristic_add(p_bms->service_handle, &add_char_params, &p_bms->feature_handles);
- }
- static void on_ctrlpt_write(nrf_ble_bms_t * p_bms,
- ble_gatts_evt_write_t const * p_evt_write,
- ble_gatts_authorize_params_t * p_auth_params)
- {
- ret_code_t err_code;
- nrf_ble_bms_ctrlpt_t ctrlpt;
- err_code = ctrlpt_process(p_bms, p_evt_write->data, p_evt_write->len, &ctrlpt);
- if (err_code != NRF_SUCCESS)
- {
- p_auth_params->gatt_status = err_code;
- p_auth_params->update = 0;
- return;
- }
- p_auth_params->gatt_status = BLE_GATT_STATUS_SUCCESS;
- p_auth_params->update = 1;
- NRF_LOG_INFO("Control point write: Success");
-
- ctrlpt_execute(p_bms, ctrlpt.op_code);
- }
- static void on_rw_auth_req(nrf_ble_bms_t * p_bms, ble_gatts_evt_t const * p_gatts_evt)
- {
- ret_code_t err_code;
- ble_gatts_rw_authorize_reply_params_t auth_reply;
- ble_gatts_evt_rw_authorize_request_t const * p_auth_req =
- &p_gatts_evt->params.authorize_request;
- memset(&auth_reply, 0, sizeof(auth_reply));
- if ((p_auth_req->type == BLE_GATTS_AUTHORIZE_TYPE_WRITE) &&
- (p_auth_req->request.write.op == BLE_GATTS_OP_WRITE_REQ) &&
- (p_auth_req->request.write.handle == p_bms->ctrlpt_handles.value_handle))
- {
- auth_reply.type = BLE_GATTS_AUTHORIZE_TYPE_WRITE;
- on_ctrlpt_write(p_bms, &p_auth_req->request.write, &auth_reply.params.write);
-
- err_code = sd_ble_gatts_rw_authorize_reply(p_bms->conn_handle, &auth_reply);
- error_check(err_code, p_bms->error_handler);
- }
- }
- uint16_t on_qwr_auth_req(nrf_ble_bms_t * p_bms, nrf_ble_qwr_t * p_qwr, nrf_ble_qwr_evt_t * p_evt)
- {
- ret_code_t err_code;
- uint16_t len = NRF_BLE_BMS_CTRLPT_MAX_LEN;
- uint8_t mem_buffer[NRF_BLE_BMS_CTRLPT_MAX_LEN];
- nrf_ble_bms_ctrlpt_t ctrlpt;
- err_code = nrf_ble_qwr_value_get(p_qwr, p_evt->attr_handle, mem_buffer, &len);
- if (err_code != NRF_SUCCESS)
- {
- NRF_LOG_ERROR("Control point write: Operation failed.");
- return NRF_BLE_BMS_OPERATION_FAILED;
- }
- return ctrlpt_process(p_bms, mem_buffer, len, &ctrlpt);
- }
- uint16_t on_qwr_exec_write(nrf_ble_bms_t * p_bms, nrf_ble_qwr_t * p_qwr, nrf_ble_qwr_evt_t * p_evt)
- {
- ret_code_t err_code;
- uint16_t len = NRF_BLE_BMS_CTRLPT_MAX_LEN;
- uint8_t mem_buffer[NRF_BLE_BMS_CTRLPT_MAX_LEN];
- nrf_ble_bms_ctrlpt_t ctrlpt;
- ble_gatts_value_t ctrlpt_value;
- ctrlpt_value.len = NRF_BLE_BMS_CTRLPT_MAX_LEN;
- ctrlpt_value.offset = 0;
- ctrlpt_value.p_value = mem_buffer;
- const uint16_t ctrlpt_handle = p_bms->ctrlpt_handles.value_handle;
- err_code = sd_ble_gatts_value_get(p_bms->conn_handle, ctrlpt_handle, &ctrlpt_value);
- if (err_code != NRF_SUCCESS)
- {
- NRF_LOG_ERROR("Control point write: Operation failed.");
- return NRF_BLE_BMS_OPERATION_FAILED;
- }
-
- err_code = ctrlpt_decode(ctrlpt_value.p_value, len, &ctrlpt);
- if (err_code != NRF_SUCCESS)
- {
- NRF_LOG_ERROR("Control point write: Operation failed.");
- return NRF_BLE_BMS_OPERATION_FAILED;
- }
-
- ctrlpt_execute(p_bms, ctrlpt.op_code);
-
- p_bms->auth_status = NRF_BLE_BMS_AUTH_STATUS_DENIED;
- return BLE_GATT_STATUS_SUCCESS;
- }
- uint16_t nrf_ble_bms_on_qwr_evt(nrf_ble_bms_t * p_bms,
- nrf_ble_qwr_t * p_qwr,
- nrf_ble_qwr_evt_t * p_evt)
- {
- VERIFY_TRUE(p_bms != NULL, (NRF_BLE_QWR_REJ_REQUEST_ERR_CODE));
- VERIFY_TRUE(p_qwr != NULL, (NRF_BLE_QWR_REJ_REQUEST_ERR_CODE));
- VERIFY_TRUE(p_evt != NULL, (NRF_BLE_QWR_REJ_REQUEST_ERR_CODE));
- VERIFY_TRUE(p_evt->attr_handle == p_bms->ctrlpt_handles.value_handle,
- (NRF_BLE_QWR_REJ_REQUEST_ERR_CODE));
- if (p_evt->evt_type == NRF_BLE_QWR_EVT_AUTH_REQUEST)
- {
- return on_qwr_auth_req(p_bms, p_qwr, p_evt);
- }
- else if ((p_evt->evt_type == NRF_BLE_QWR_EVT_EXECUTE_WRITE) &&
- (p_bms->auth_status == NRF_BLE_BMS_AUTH_STATUS_ALLOWED))
- {
- return on_qwr_exec_write(p_bms, p_qwr, p_evt);
- }
- return BLE_GATT_STATUS_SUCCESS;
- }
- void nrf_ble_bms_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
- {
- VERIFY_PARAM_NOT_NULL_VOID(p_context);
- VERIFY_PARAM_NOT_NULL_VOID(p_ble_evt);
- nrf_ble_bms_t * p_bms = (nrf_ble_bms_t *)p_context;
- switch (p_ble_evt->header.evt_id)
- {
- case BLE_GATTS_EVT_RW_AUTHORIZE_REQUEST:
- on_rw_auth_req(p_bms, &p_ble_evt->evt.gatts_evt);
- break;
- default:
- break;
- }
- }
- ret_code_t nrf_ble_bms_set_conn_handle(nrf_ble_bms_t * p_bms, uint16_t conn_handle)
- {
- VERIFY_PARAM_NOT_NULL(p_bms);
- p_bms->conn_handle = conn_handle;
- return NRF_SUCCESS;
- }
- ret_code_t nrf_ble_bms_init(nrf_ble_bms_t * p_bms, nrf_ble_bms_init_t * p_bms_init)
- {
- ret_code_t err_code;
- ble_uuid_t ble_uuid;
- VERIFY_PARAM_NOT_NULL(p_bms_init);
- VERIFY_PARAM_NOT_NULL(p_bms);
-
- BLE_UUID_BLE_ASSIGN(ble_uuid, BLE_UUID_BMS_SERVICE);
- p_bms->evt_handler = p_bms_init->evt_handler;
- p_bms->error_handler = p_bms_init->error_handler;
- p_bms->feature = p_bms_init->feature;
- p_bms->bond_callbacks = p_bms_init->bond_callbacks;
- p_bms->conn_handle = BLE_CONN_HANDLE_INVALID;
- err_code = sd_ble_gatts_service_add(BLE_GATTS_SRVC_TYPE_PRIMARY,
- &ble_uuid,
- &p_bms->service_handle);
- VERIFY_SUCCESS(err_code);
- err_code = feature_char_add(p_bms, p_bms_init);
- VERIFY_SUCCESS(err_code);
- err_code = ctrlpt_char_add(p_bms, p_bms_init);
- VERIFY_SUCCESS(err_code);
- if (p_bms_init->p_qwr != NULL)
- {
- return nrf_ble_qwr_attr_register(p_bms_init->p_qwr, p_bms->ctrlpt_handles.value_handle);
- }
- NRF_LOG_INFO("Init complete.");
- return NRF_SUCCESS;
- }
- ret_code_t nrf_ble_bms_auth_response(nrf_ble_bms_t * p_bms, bool authorize)
- {
- VERIFY_PARAM_NOT_NULL(p_bms);
- VERIFY_TRUE(p_bms->auth_status == NRF_BLE_BMS_AUTH_STATUS_PENDING, NRF_ERROR_INVALID_STATE);
- if (authorize)
- {
- p_bms->auth_status = NRF_BLE_BMS_AUTH_STATUS_ALLOWED;
- }
- else
- {
- p_bms->auth_status = NRF_BLE_BMS_AUTH_STATUS_DENIED;
- }
- return NRF_SUCCESS;
- }
|