自学教程：C++ uint16_decode函数代码示例

/**@brief Function for decoding a command packet with RPC_SD_BLE_GAP_PPCP_SET opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gap_ppcp_set_handle(uint8_t * p_command, uint32_t command_len){    uint32_t              err_code;    ble_gap_conn_params_t conn_params;    uint32_t                index        = 0;    ble_gap_conn_params_t * p_conn_param = NULL;    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        conn_params.min_conn_interval   = uint16_decode(&p_command[index]);        index += sizeof(uint16_t);        conn_params.max_conn_interval   = uint16_decode(&p_command[index]);        index += sizeof(uint16_t);        conn_params.slave_latency       = uint16_decode(&p_command[index]);        index += sizeof(uint16_t);        conn_params.conn_sup_timeout    = uint16_decode(&p_command[index]);        p_conn_param = &conn_params;    }        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_PPCP_SET);    err_code = sd_ble_gap_ppcp_set(p_conn_param);    return ble_rpc_cmd_resp_send(SD_BLE_GAP_PPCP_SET, err_code);}

/**@brief Function for decoding a command packet with RPC_SD_BLE_GAP_SEC_INFO_REPLY opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gap_sec_info_reply_handle(uint8_t * const p_command, uint32_t command_len){    bool     use_sign;    uint16_t conn_handle;    uint32_t err_code;        uint32_t            index     = 0;    ble_gap_enc_info_t  enc_info  = {0};    ble_gap_sign_info_t sign_info = {{0}};        conn_handle  = uint16_decode(&p_command[index]);    index       += sizeof(uint16_t);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_SEC_INFO_REPLY);    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {                enc_info.div     = uint16_decode(&p_command[index]);        index           += sizeof(uint16_t);        memcpy(enc_info.ltk, &p_command[index], sizeof(enc_info.ltk));        index           += sizeof(enc_info.ltk);        enc_info.auth    = (p_command[index] & 0x01);        enc_info.ltk_len = ((p_command[index] & 0xfe) >> 1);        index++;        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_SEC_INFO_REPLY);    }

/**@brief Function for decoding a command packet with RPC_SD_BLE_GAP_CONN_PARAM_UPDATE opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gap_conn_param_update_handle(const uint8_t * const p_command, uint32_t command_len){    uint16_t                conn_handle;    uint32_t                err_code;    ble_gap_conn_params_t   conn_params;    ble_gap_conn_params_t * p_conn_params = NULL;    uint32_t                index = 0;    conn_handle  = uint16_decode(&p_command[index]);    index       += sizeof(uint16_t);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_CONN_PARAM_UPDATE);    // Check if the Connection Parameters field is present.    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        conn_params.min_conn_interval   = uint16_decode(&p_command[index]);        index                           += sizeof(uint16_t);        conn_params.max_conn_interval   = uint16_decode(&p_command[index]);        index                           += sizeof(uint16_t);        conn_params.slave_latency       = uint16_decode(&p_command[index]);        index                           += sizeof(uint16_t);        conn_params.conn_sup_timeout    = uint16_decode(&p_command[index]);        p_conn_params                   = &conn_params;    }    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_CONN_PARAM_UPDATE);    err_code = sd_ble_gap_conn_param_update(conn_handle, p_conn_params);    return ble_rpc_cmd_resp_send(SD_BLE_GAP_CONN_PARAM_UPDATE, err_code);}

/**@brief Function for decoding a command packet with RPC_SD_BLE_GATTS_SYS_ATTR_SET opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gatts_sys_attr_set_handle(uint8_t const * const p_command, uint32_t command_len){    uint16_t conn_handle;    uint32_t err_code;    uint32_t        index            = 0;    uint16_t        attr_data_length = 0;    const uint8_t * p_attr_data      = NULL;    conn_handle  = uint16_decode(&p_command[index]);    index       += sizeof(uint16_t);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_SYS_ATTR_SET);    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        attr_data_length  = uint16_decode(&p_command[index]);        index            += sizeof(uint16_t);        p_attr_data       = &(p_command[index]);        index            += attr_data_length;    }    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_SYS_ATTR_SET);    err_code = sd_ble_gatts_sys_attr_set(conn_handle, p_attr_data, attr_data_length);    return ble_rpc_cmd_resp_send(SD_BLE_GATTS_SYS_ATTR_SET, err_code);}

static void decode_ad_manuf_spec_data(uint8_t *buffer, uint8_t len, adv_packet_t *adv_packet){    int i = 1;    adv_packet->adv_data.manuf_id = uint16_decode(&buffer[i]);    i += sizeof(uint16_t);    adv_packet->adv_data.beacon_dev_type = buffer[i++];    uint8_t data_len = buffer[i++];    if (data_len == BEACON_DATA_LEN)    {        memcpy(&adv_packet->adv_data.uuid, &buffer[i], sizeof(ble_uuid128_t));        i += sizeof(ble_uuid128_t);        adv_packet->adv_data.major = uint16_decode(&buffer[i]);        i += sizeof(uint16_t);        adv_packet->adv_data.minor = uint16_decode(&buffer[i]);        i += sizeof(uint16_t);        adv_packet->adv_data.rssi = uint16_decode(&buffer[i]);    }    else    {        memset(&adv_packet->adv_data.uuid, 0, sizeof(ble_uuid128_t));        adv_packet->adv_data.major = 0;        adv_packet->adv_data.minor = 0;        adv_packet->adv_data.rssi = 0;    }}

/**@brief Function for handling the rx packets comming from hal_transport. * * @details *  This function is called in serial peripheral interrupt context. Response packets are handled in *  this context. Events are passed to the application and it is up to application in which context *  they are handled. * * @param[in]   p_data   Pointer to received data. * @param[in]   length   Size of data. */static void ser_sd_transport_rx_packet_handler(uint8_t * p_data, uint16_t length){    if (p_data && (length >= SER_PKT_TYPE_SIZE))    {        const uint8_t packet_type = p_data[SER_PKT_TYPE_POS];        p_data += SER_PKT_TYPE_SIZE;        length -= SER_PKT_TYPE_SIZE;        switch (packet_type)        {            case SER_PKT_TYPE_RESP:            case SER_PKT_TYPE_DTM_RESP:#ifdef ANT_STACK_SUPPORT_REQD            case SER_PKT_TYPE_ANT_RESP:#endif // ANT_STACK_SUPPORT_REQD                if (m_rsp_wait)                {                    m_return_value = m_rsp_dec_handler(p_data, length);                    (void)ser_sd_transport_rx_free(p_data);                    /* Reset response flag - cmd_write function is pending on it.*/                    m_rsp_wait = false;                    /* If os handler is set, signal os that response has arrived.*/                    if (m_os_rsp_set_handler)                    {                        m_os_rsp_set_handler();                    }                }                else                {                    /* Unexpected packet. */                    (void)ser_sd_transport_rx_free(p_data);                    APP_ERROR_HANDLER(packet_type);                }                break;#ifdef BLE_STACK_SUPPORT_REQD            case SER_PKT_TYPE_EVT:                /* It is ensured during opening that handler is not NULL. No check needed. */                NRF_LOG_DEBUG("[EVT]: %s ", (uint32_t)ser_dbg_sd_evt_str_get(uint16_decode(&p_data[SER_EVT_ID_POS]))); // p_data points to EVT_ID                m_ble_evt_handler(p_data, length);                break;#endif // BLE_STACK_SUPPORT_REQD#ifdef ANT_STACK_SUPPORT_REQD            case SER_PKT_TYPE_ANT_EVT:                /* It is ensured during opening that handler is not NULL. No check needed. */                NRF_LOG_DEBUG("[ANT_EVT_ID]: %s ", (uint32_t)ser_dbg_sd_evt_str_get(uint16_decode(&p_data[SER_EVT_ID_POS]))); // p_data points to EVT_ID                m_ant_evt_handler(p_data, length);                break;#endif // ANT_STACK_SUPPORT_REQD            default:                (void)ser_sd_transport_rx_free(p_data);                APP_ERROR_HANDLER(packet_type);                break;        }    }}

/**@brief Function for decoding a command packet with RPC_SD_BLE_GAP_ADV_START opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gap_adv_start_handle(uint8_t * p_command, uint32_t command_len){    ble_gap_adv_params_t adv_params;    ble_gap_addr_t       directed_peer_address;    ble_gap_whitelist_t  white_list;    uint32_t index = 0;    adv_params.type = p_command[index++];    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_ADV_START);    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        // Peer Address Present. Decode the peer address.        index                  += addr_decode(&directed_peer_address, &(p_command[index]));        adv_params.p_peer_addr  = &(directed_peer_address);        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_ADV_START);    }    else    {        adv_params.p_peer_addr = NULL;    }    adv_params.fp = p_command[index++];    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_ADV_START);    // Whitelist present.    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        uint8_t  decoded_length = 0;        uint32_t err_code       = whitelist_decode(&white_list,                                                   &(p_command[index]),                                                   &decoded_length);        if (err_code != NRF_SUCCESS)        {            return ble_rpc_cmd_resp_send(SD_BLE_GAP_ADV_START, err_code);        }        index = index + decoded_length;        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_ADV_START);        adv_params.p_whitelist = &white_list;        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_ADV_START);    }    else    {        adv_params.p_whitelist = NULL;    }    adv_params.interval = uint16_decode(&p_command[index]);    index              += sizeof(uint16_t);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_ADV_START);    adv_params.timeout  = uint16_decode(&p_command[index]);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_ADV_START);    uint32_t err_code   = sd_ble_gap_adv_start(&adv_params);    return ble_rpc_cmd_resp_send(SD_BLE_GAP_ADV_START, err_code);}

/**@brief Handle received ANT data message. *  * @param[in]  p_evt_buffer   The buffer containing received data.  */static void ant_data_messages_handle(uint8_t * p_evt_buffer){    static uint32_t s_previous_beat_count = 0;    // Heart beat count from previously received page    uint32_t        err_code;    uint32_t        current_page;    uint8_t         beat_count;    uint8_t         computed_heart_rate;    uint16_t        beat_time;    // Decode the default page data present in all pages    beat_time           = uint16_decode(&p_evt_buffer[ANT_BUFFER_INDEX_MESG_DATA + 4]);    beat_count          = (uint8_t)p_evt_buffer[ANT_BUFFER_INDEX_MESG_DATA + 6];    computed_heart_rate = (uint8_t)p_evt_buffer[ANT_BUFFER_INDEX_MESG_DATA + 7];    // Decode page specific data    current_page = p_evt_buffer[ANT_BUFFER_INDEX_MESG_DATA];    switch (current_page & ~ANT_HRM_TOGGLE_MASK)    {        case ANT_HRM_PAGE_4:            // Ensure that there is only one beat between time intervals.            if ((beat_count - s_previous_beat_count) == 1)            {                uint16_t prev_beat = uint16_decode(&p_evt_buffer[ANT_BUFFER_INDEX_MESG_DATA + 2]);                                // Subtracting the event time gives the R-R interval                ble_hrs_rr_interval_add(&m_hrs, beat_time - prev_beat);            }            s_previous_beat_count = beat_count;            break;                  case ANT_HRM_PAGE_0:        case ANT_HRM_PAGE_1:        case ANT_HRM_PAGE_2:        case ANT_HRM_PAGE_3:        default:            // No implementation needed.            break;    }        // Notify the received heart rate measurement    err_code = ble_hrs_heart_rate_measurement_send(&m_hrs, computed_heart_rate);    if (        (err_code != NRF_SUCCESS)        &&        (err_code != NRF_ERROR_INVALID_STATE)        &&        (err_code != BLE_ERROR_NO_TX_BUFFERS)        &&        (err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)    )    {        APP_ERROR_HANDLER(err_code);    }}

void ant_bpwr_page_torque_decode(uint8_t const               * p_page_buffer,                                 ant_bpwr_page_torque_data_t * p_page_data){    ant_bpwr_page_torque_data_layout_t const * p_incoming_data =        (ant_bpwr_page_torque_data_layout_t *)p_page_buffer;    p_page_data->update_event_count    = p_incoming_data->update_event_count;    p_page_data->tick                  = p_incoming_data->tick;    p_page_data->period                = uint16_decode(p_incoming_data->period);    p_page_data->accumulated_torque    = uint16_decode(p_incoming_data->accumulated_torque);}

/**@brief Function for decoding a command packet with RPC_SD_BLE_GATTS_VALUE_SET opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gatts_value_set_handle(uint8_t const * const p_command, uint32_t command_len){    uint16_t handle;    uint16_t offset;    uint32_t err_code;    uint32_t        index    = 0;    uint16_t        length   = 0;    uint16_t *      p_length = NULL;    const uint8_t * p_value  = NULL;    handle  = uint16_decode(&p_command[index]);    index  += sizeof(uint16_t);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_VALUE_SET);    offset  = uint16_decode(&p_command[index]);    index  += sizeof(uint16_t);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_VALUE_SET);    // Value length present.    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        length    = uint16_decode(&p_command[index]);        p_length  = &length;        index    += sizeof(uint16_t);    }    RPC_DECODER_LENGTH_CHECK(command_len, (index + length), SD_BLE_GATTS_VALUE_SET);    // Value present.    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        p_value = &p_command[index];    }    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_VALUE_SET);    err_code = sd_ble_gatts_value_set(handle, offset, p_length, p_value);    if (err_code == NRF_SUCCESS)    {        uint8_t resp_data[sizeof(uint16_t)];        UNUSED_VARIABLE(uint16_encode(*p_length, resp_data));        return ble_rpc_cmd_resp_data_send(SD_BLE_GATTS_VALUE_SET,                                          err_code,                                          resp_data,                                          sizeof(resp_data));    }    else    {        return ble_rpc_cmd_resp_send(SD_BLE_GATTS_VALUE_SET, err_code);    }}

void ant_bpwr_page_16_decode(uint8_t const          * p_page_buffer,                             ant_bpwr_page16_data_t * p_page_data){    ant_bpwr_page16_data_layout_t const * p_incoming_data =        (ant_bpwr_page16_data_layout_t *)p_page_buffer;    p_page_data->update_event_count    = p_incoming_data->update_event_count;    p_page_data->pedal_power.byte      = p_incoming_data->pedal_power;    p_page_data->accumulated_power     = uint16_decode(p_incoming_data->accumulated_power);    p_page_data->instantaneous_power   = uint16_decode(p_incoming_data->instantaneous_power);    page16_data_log(p_page_data);}

void ant_common_page_80_decode(uint8_t const                     * p_page_buffer,                               volatile ant_common_page80_data_t * p_page_data){    ant_common_page80_data_layout_t const * p_incoming_data =        (ant_common_page80_data_layout_t *)p_page_buffer;    p_page_data->hw_revision = p_incoming_data->hw_revision;    p_page_data->manufacturer_id = uint16_decode(p_incoming_data->manufacturer_id);    p_page_data->model_number    = uint16_decode(p_incoming_data->model_number);    page80_data_log(p_page_data);}

static bool is_cccd_configured(uint16_t conn_handle){    uint32_t          err_code;    uint8_t           cccd_value_buf[BLE_CCCD_VALUE_LEN];    ble_gatts_value_t value;    bool              is_ctrlpt_notif_enabled = false;    value.len     = BLE_CCCD_VALUE_LEN;    value.offset  = 0;    value.p_value = cccd_value_buf;    err_code = sd_ble_gatts_value_get(conn_handle,                                      m_char_ctrlpt_handles.cccd_handle,                                      &value);    // TODO: Error codes should be sent back to application indicating that the    // read of CCCD did not work. No application error handler is currently    // implemented.    (void)err_code;    uint16_t cccd_value = uint16_decode(cccd_value_buf);    is_ctrlpt_notif_enabled = ((cccd_value & BLE_GATT_HVX_NOTIFICATION) != 0);    return is_ctrlpt_notif_enabled;}

uint32_t sd_ble_gap_appearance_get(uint16_t * const p_appearance){    uint32_t index = 0;    g_cmd_buffer[index++] = BLE_RPC_PKT_CMD;    g_cmd_buffer[index++] = SD_BLE_GAP_APPEARANCE_GET;    if (p_appearance != NULL)    {        g_cmd_buffer[index++] = RPC_BLE_FIELD_PRESENT;       }    else    {       g_cmd_buffer[index++] = RPC_BLE_FIELD_NOT_PRESENT;     }        uint32_t err_code = hci_transport_pkt_write(g_cmd_buffer, index);        if (err_code != NRF_SUCCESS)    {        return err_code;    }                err_code = ble_rpc_cmd_resp_wait(SD_BLE_GAP_APPEARANCE_GET);    if (p_appearance != NULL)    {        *p_appearance = uint16_decode(&(g_cmd_response_buf[BLE_OP_CODE_SIZE  +                                                           BLE_PKT_TYPE_SIZE +                                                           RPC_ERR_CODE_SIZE]));    }            UNUSED_VARIABLE(hci_transport_rx_pkt_consume(g_cmd_response_buf));    return err_code;}

/** @brief Function for decoding a GAP connection parameter update event. * * @param[in]       p_evt_data      The pointer to the encoded event (excluding the event header). * @param[in, out]  p_decoded_evt   The pointer to the decoded event. */static void gap_conn_param_update_evt_decode(const uint8_t * const                    p_evt_data,                                             ble_gap_evt_conn_param_update_t * const  p_decoded_evt){    uint8_t index = 0;    p_decoded_evt->conn_params.min_conn_interval = uint16_decode(&p_evt_data[index]);    index += sizeof(uint16_t);    p_decoded_evt->conn_params.max_conn_interval = uint16_decode(&p_evt_data[index]);    index += sizeof(uint16_t);    p_decoded_evt->conn_params.slave_latency     = uint16_decode(&p_evt_data[index]);    index += sizeof(uint16_t);    p_decoded_evt->conn_params.conn_sup_timeout  = uint16_decode(&p_evt_data[index]);}

/**@brief     Function for handling Handle Value Notification received from the SoftDevice. * * @details   This function will uses the Handle Value Notification received from the SoftDevice *            and checks if it is a notification of the heart rate measurement from the peer. If *            it is, this function will decode the heart rate measurement and send it to the *            application. * * @param[in] p_ble_hrs_c Pointer to the Heart Rate Client structure. * @param[in] p_ble_evt   Pointer to the BLE event received. */static void on_hvx(ble_hrs_c_t * p_ble_hrs_c, const ble_evt_t * p_ble_evt){    // Check if this is a heart rate notification.    if (p_ble_evt->evt.gattc_evt.params.hvx.handle == p_ble_hrs_c->hrm_handle)    {        ble_hrs_c_evt_t ble_hrs_c_evt;        uint32_t        index = 0;        ble_hrs_c_evt.evt_type = BLE_HRS_C_EVT_HRM_NOTIFICATION;        if (!(p_ble_evt->evt.gattc_evt.params.hvx.data[index++] & HRM_FLAG_MASK_HR_16BIT))        {            // 8 Bit heart rate value received.            ble_hrs_c_evt.params.hrm.hr_value = p_ble_evt->evt.gattc_evt.params.hvx.data[index++];  //lint !e415 suppress Lint Warning 415: Likely access out of bond        }        else        {            // 16 bit heart rate value received.            ble_hrs_c_evt.params.hrm.hr_value =                uint16_decode(&(p_ble_evt->evt.gattc_evt.params.hvx.data[index]));        }        p_ble_hrs_c->evt_handler(p_ble_hrs_c, &ble_hrs_c_evt);    }}

uint32_t dfu_init_postvalidate(uint8_t * p_image, uint32_t image_len){#if NEED_CRC_CHECK /* disabled for now */    uint16_t image_crc;    uint16_t received_crc;        // In order to support hashing (and signing) then the (decrypted) hash should be fetched and    // the corresponding hash should be calculated over the image at this location.    // If hashing (or signing) is added to the system then the CRC validation should be removed.    // calculate CRC from active block.    image_crc = crc16_compute(p_image, image_len, NULL);    // Decode the received CRC from extended data.        received_crc = uint16_decode((uint8_t *)&m_extended_packet[0]);    // Compare the received and calculated CRC.    if (image_crc != received_crc)    {        return NRF_ERROR_INVALID_DATA;    }#endif /* NEED_CRC_CHECK */    return NRF_SUCCESS;}

/**@brief Function for decoding a command packet with RPC_SD_BLE_GAP_SEC_PARAMS_REPLY opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gap_sec_params_reply_handle(const uint8_t * const p_command, uint32_t command_len){    uint8_t              sec_status;    uint16_t             conn_handle;    uint32_t             err_code;    ble_gap_sec_params_t sec_params;    uint32_t               index       = 0;    ble_gap_sec_params_t * p_sec_param = NULL;    conn_handle  = uint16_decode(&p_command[index]);    index       += sizeof(uint16_t);    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_SEC_PARAMS_REPLY);    sec_status = p_command[index++];    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_SEC_PARAMS_REPLY);        if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        index       += sec_params_decode(&p_command[index], &sec_params);        p_sec_param  = &sec_params;    }    else    {        p_sec_param = NULL;    }    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_SEC_PARAMS_REPLY);    err_code = sd_ble_gap_sec_params_reply(conn_handle, sec_status, p_sec_param);    return ble_rpc_cmd_resp_send(SD_BLE_GAP_SEC_PARAMS_REPLY, err_code);}

/**@brief Function for handling the Write event. * * @param[in] p_si7021  si7021 Service structure. * @param[in] p_ble_evt  Event received from the BLE stack. */static void on_write(ble_si7021_t * p_si7021, ble_evt_t * p_ble_evt) {    ble_gatts_evt_write_t * p_evt_write = &p_ble_evt->evt.gatts_evt.params.write;	NRF_LOG_DEBUG("on_write %x/n/r",p_evt_write->handle);    if (p_evt_write->handle == p_si7021->value_char_handles.cccd_handle) {        if (p_evt_write->len == 2 && p_si7021->evt_handler != NULL) {        	ble_si7021_evt_t evt;            if (ble_srv_is_notification_enabled(p_evt_write->data)) {                evt.evt_type = BLE_si7021_EVT_NOTIFICATION_ENABLED;            	NRF_LOG_DEBUG("BLE_si7021_EVT_NOTIFICATION_ENABLED/n/r");            } else {                evt.evt_type = BLE_si7021_EVT_NOTIFICATION_DISABLED;            	NRF_LOG_DEBUG("BLE_si7021_EVT_NOTIFICATION_DISABLED/n/r");            }            p_si7021->evt_handler(p_si7021, &evt);        }    } else if (p_evt_write->handle == p_si7021->com_char_handles.value_handle) {    	p_si7021->interval = uint16_decode(&p_evt_write->data[0]);    	if (p_si7021->interval<100) {    		p_si7021->interval=100; // min 100ms    	}    	ble_si7021_evt_t evt;    	evt.evt_type = BLE_si7021_EVT_INTERVAL_CHANGE;    	NRF_LOG_DEBUG("BLE_si7021_EVT_INTERVAL_CHANGE %d/n/r",p_si7021->interval);        p_si7021->evt_handler(p_si7021, &evt);	} else {			// No implementation needed.	}}

/**@brief Function for decoding a command packet with RPC_SD_BLE_GATTS_SERVICE_ADD opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gatts_service_add_handle(const uint8_t * const p_command, uint32_t command_len){    uint8_t      type;    uint16_t *   p_handle;    uint32_t     err_code;    ble_uuid_t   uuid;    ble_uuid_t * p_uuid;    uint8_t      resp_data[sizeof(uint16_t)];    uint32_t index  = 0;    uint16_t handle = 0;    type = p_command[index++];    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_SERVICE_ADD);    // Service UUID field is present.    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        uuid.uuid = uint16_decode(&p_command[index]);        index    += sizeof(uint16_t);        uuid.type = p_command[index++];        p_uuid    = &uuid;        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_SERVICE_ADD);    }    else    {        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_SERVICE_ADD);        p_uuid = NULL;    }    // Handle present.    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        p_handle = &handle;    }    else    {        p_handle = NULL;    }    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GATTS_SERVICE_ADD);    err_code = sd_ble_gatts_service_add(type, p_uuid, p_handle);    if (err_code == NRF_SUCCESS)    {        UNUSED_VARIABLE(uint16_encode(handle, resp_data));        return ble_rpc_cmd_resp_data_send(SD_BLE_GATTS_SERVICE_ADD,                                          err_code,                                          resp_data,                                          sizeof(resp_data));    }    else    {        return ble_rpc_cmd_resp_send(SD_BLE_GATTS_SERVICE_ADD, err_code);    }}

/** @brief Function for decoding the event header from an encoded event. * * @param[in]       p_evt       The pointer to the encoded event. * @param[in, out]  p_evt_hdr   The pointer to the decoded event header. * * @return          Number of bytes decoded. */static uint8_t evt_header_decode(const uint8_t * const p_evt, ble_evt_hdr_t * const p_evt_hdr){    uint8_t index = 0;    p_evt_hdr->evt_id = uint16_decode(&(p_evt[index]));    index += sizeof(uint16_t);    return index;}

void uint16_dec(uint8_t const * const p_buf,                uint32_t              buf_len,                uint32_t * const      index,                uint16_t * const      value){    SER_ASSERT_VOID_RETURN(*index + 2 <= buf_len);    *value  = uint16_decode(&p_buf[*index]);    *index += 2;}

/** *@brief Callback for requesting from upper layer memory for an incomming packet. */static __INLINE void callback_mem_request(void){    m_rx_stream_length          = uint16_decode(m_rx_length_buf) + SER_PHY_HEADER_SIZE;    m_ser_phy_rx_event.evt_type = SER_PHY_EVT_RX_BUF_REQUEST;    m_ser_phy_rx_event.evt_params.rx_buf_request.num_of_bytes =        m_rx_stream_length - SER_PHY_HEADER_SIZE;    callback_ser_phy_event(m_ser_phy_rx_event);}

void bootloader_util_settings_get(const bootloader_settings_t ** pp_bootloader_settings){    static bootloader_settings_t s_converted_boot_settings;    // Depending on the configuration / version the compiler used the enums used in bootloader    // settings might have different padding (half word / word padded).    // Half word padding would be, bc = Bank Code enum, crc (half word), xx = byte padding:    // bc0 xx  crc crc - bc1 xx  xx  xx  - rest of data    // Word padding would be, bc = Bank Code enum, crc (half word), xx = byte padding:    // bc0 xx  xx  xx  - crc crc xx  xx  - bc1 xx  xx  xx - rest of data    // To ensure full compability when updating from older bootloader to new, the padding must    // be checked and conversion must be done accordingly.    // In case of empty flash, 0xFFFFFFFF it is unimportant how settings are loaded.    uint32_t pad_test = *((uint32_t *)&m_boot_settings[SIZE_TEST_INDEX]) & 0xFFFFFF00;    if (pad_test == ZERO)    {        // Bank code enum is word sized, thus it is safe to cast without conversion.        // Read only pointer to bootloader settings in flash.        bootloader_settings_t const * const p_bootloader_settings =            (bootloader_settings_t *)&m_boot_settings[0];        *pp_bootloader_settings = p_bootloader_settings;    }    else    {        // Bank code enum is half word sized, thus crc is following the bank code in same word.        uint32_t index                           = 0;        s_converted_boot_settings.bank_0         =            (bootloader_bank_code_t)((uint16_t)(m_boot_settings[index]) & 0xFFFF);        index                                   += sizeof(uint16_t);        s_converted_boot_settings.bank_0_crc     = uint16_decode(&m_boot_settings[index]);        index                                   += sizeof(uint16_t);        s_converted_boot_settings.bank_1         =            (bootloader_bank_code_t)((uint16_t)(m_boot_settings[index]) & 0xFFFF);        // Rest of settings are word sized values and thus will be aligned to next word.        index                                   += sizeof(uint32_t);        s_converted_boot_settings.bank_0_size    = uint32_decode(&m_boot_settings[index]);;        index                                   += sizeof(uint32_t);        s_converted_boot_settings.sd_image_size  = uint32_decode(&m_boot_settings[index]);;        index                                   += sizeof(uint32_t);        s_converted_boot_settings.bl_image_size  = uint32_decode(&m_boot_settings[index]);;        index                                   += sizeof(uint32_t);        s_converted_boot_settings.app_image_size = uint32_decode(&m_boot_settings[index]);;        index                                   += sizeof(uint32_t);        s_converted_boot_settings.sd_image_start = uint32_decode(&m_boot_settings[index]);;        *pp_bootloader_settings = &s_converted_boot_settings;    }}

/** @brief Function for decoding a GAP authentication status request event. * * @param[in]       p_evt_data      The pointer to the encoded event (excluding the event header). * @param[in, out]  p_decoded_evt   The pointer to the decoded event. */static void gap_auth_status_evt_decode(const uint8_t * const             p_evt_data,                                      ble_gap_evt_auth_status_t * const  p_decoded_evt){    uint8_t index = 0;    p_decoded_evt->auth_status = p_evt_data[index++];    p_decoded_evt->error_src = p_evt_data[index++];    p_decoded_evt->sm1_levels.lv3 = (p_evt_data[index] >> 5) & 0x01;    p_decoded_evt->sm1_levels.lv2 = (p_evt_data[index] >> 4) & 0x01;    p_decoded_evt->sm1_levels.lv1 = (p_evt_data[index] >> 3) & 0x01;    p_decoded_evt->sm2_levels.lv3 = (p_evt_data[index] >> 2) & 0x01;    p_decoded_evt->sm2_levels.lv2 = (p_evt_data[index] >> 1) & 0x01;    p_decoded_evt->sm2_levels.lv1 = (p_evt_data[index] >> 0) & 0x01;    index += sizeof(uint8_t);          p_decoded_evt->periph_kex.csrk      = (p_evt_data[index] >> 4) & 0x01;    p_decoded_evt->periph_kex.address   = (p_evt_data[index] >> 3) & 0x01;    p_decoded_evt->periph_kex.irk       = (p_evt_data[index] >> 2) & 0x01;    p_decoded_evt->periph_kex.ediv_rand = (p_evt_data[index] >> 1) & 0x01;    p_decoded_evt->periph_kex.ltk       = (p_evt_data[index] >> 0) & 0x01;    index += sizeof(uint8_t);          p_decoded_evt->central_kex.ltk        = (p_evt_data[index] >> 4) & 0x01;    p_decoded_evt->central_kex.ediv_rand  = (p_evt_data[index] >> 3) & 0x01;    p_decoded_evt->central_kex.irk        = (p_evt_data[index] >> 2) & 0x01;    p_decoded_evt->central_kex.address    = (p_evt_data[index] >> 1) & 0x01;    p_decoded_evt->central_kex.csrk       = (p_evt_data[index] >> 0) & 0x01;    index += sizeof(uint8_t);          p_decoded_evt->periph_keys.enc_info.div = uint16_decode(&p_evt_data[index]);    index += sizeof(uint16_t);        uint32_t i = 0;    for (i = 0; i < BLE_GAP_SEC_KEY_LEN; i++)    {        p_decoded_evt->periph_keys.enc_info.ltk[i] = p_evt_data[index++];    }        p_decoded_evt->periph_keys.enc_info.ltk_len = (p_evt_data[index] >> 1);    p_decoded_evt->periph_keys.enc_info.auth = (p_evt_data[index] >> 0) & 0x01;    index += sizeof(uint8_t);         for (i = 0; i < BLE_GAP_SEC_KEY_LEN; i++)    {        p_decoded_evt->central_keys.irk.irk[i] = p_evt_data[index++];    }        p_decoded_evt->central_keys.id_info.addr_type = p_evt_data[index++];    for (i = 0; i < BLE_GAP_ADDR_LEN; i++)    {        p_decoded_evt->central_keys.id_info.addr[i] = p_evt_data[index++];    }    }

/**@brief Function for decoding a command packet with RPC_SD_BLE_GAP_APPEARANCE_SET opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gap_appearance_set_handle(uint8_t * p_command, uint32_t command_len){    uint16_t appearance = uint16_decode(&(p_command[0]));    RPC_DECODER_LENGTH_CHECK(command_len, sizeof(appearance), SD_BLE_GAP_APPEARANCE_SET);    uint32_t err_code = sd_ble_gap_appearance_set(appearance);    return ble_rpc_cmd_resp_send(SD_BLE_GAP_APPEARANCE_SET, err_code);}

/**@brief Function for decoding a command packet with RPC_SD_BLE_UUID_ENCODE opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t uuid_encode_handle(uint8_t * p_command, uint32_t command_len){    ble_uuid_t uuid_data;    uint32_t   err_code;    // uuid can be up to 16 bytes, and 1 byte length field    uint8_t      response_buffer[sizeof(ble_uuid128_t) + sizeof(uint8_t)];    uint32_t     index           = 0;    ble_uuid_t * p_uuid_data     = &uuid_data;    uint8_t *    p_length        = &(response_buffer[0]);    uint8_t *    p_result_buffer = &(response_buffer[1]);    // UUID field present.    if (p_command[index++] == RPC_BLE_FIELD_PRESENT)    {        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_UUID_ENCODE);        uuid_data.uuid  = uint16_decode(&(p_command[index]));        index          += sizeof(uint16_t);        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_UUID_ENCODE);        uuid_data.type  = p_command[index++];    }    else    {        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_UUID_ENCODE);        p_uuid_data = NULL;    }    // Length field not present.    if (p_command[index++] == RPC_BLE_FIELD_NOT_PRESENT)    {        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_UUID_ENCODE);        p_length = NULL;    }    // Result buffer not present.    if (p_command[index++] == RPC_BLE_FIELD_NOT_PRESENT)    {        p_result_buffer = NULL;    }    RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_UUID_ENCODE);    err_code = sd_ble_uuid_encode(p_uuid_data, p_length, p_result_buffer);    if (err_code == NRF_SUCCESS)    {        return ble_rpc_cmd_resp_data_send(SD_BLE_UUID_ENCODE,                                          NRF_SUCCESS,                                          response_buffer,                                          response_buffer[0] + sizeof(uint8_t));    }    return ble_rpc_cmd_resp_send(SD_BLE_UUID_ENCODE, err_code);}

/** @brief Function for decoding a connected event. * * @param[in]       p_evt_data      The pointer to the encoded event (excluding the event *                                  header). * @param[in, out]  p_decoded_evt   The pointer to the decoded event. */static void connected_evt_decode(const uint8_t * const           p_evt_data,                                 ble_gap_evt_connected_t * const p_decoded_evt){    uint8_t index = 0;    index += ble_addr_decode(p_evt_data, &(p_decoded_evt->peer_addr));    p_decoded_evt->irk_match      = (p_evt_data[index] & 0x01);    p_decoded_evt->irk_match_idx  = (p_evt_data[index] & 0xFE) >> 1;    index++;    p_decoded_evt->conn_params.min_conn_interval = uint16_decode(&p_evt_data[index]);    index += sizeof(uint16_t);    p_decoded_evt->conn_params.max_conn_interval = uint16_decode(&p_evt_data[index]);    index += sizeof(uint16_t);    p_decoded_evt->conn_params.slave_latency     = uint16_decode(&p_evt_data[index]);    index += sizeof(uint16_t);    p_decoded_evt->conn_params.conn_sup_timeout  = uint16_decode(&p_evt_data[index]);}

uint32_t uint16_t_dec(uint8_t const * const p_buf,                      uint32_t              buf_len,                      uint32_t * const      p_index,                      void *                p_field){    uint16_t * p_u16 = (uint16_t *)p_field;    SER_ASSERT_LENGTH_LEQ(2, ((int32_t)buf_len - *p_index));    *p_u16    = uint16_decode(&p_buf[*p_index]);    *p_index += 2;    return NRF_SUCCESS;}

/**@brief Function for handling the rx packets comming from hal_transport. * * @details *  This function is called in serial peripheral interrupt context. Response packets are handled in *  this context. Events are passed to the application and it is up to application in which context *  they are handled. * * @param[in]   p_data   Pointer to received data. * @param[in]   length   Size of data. */static void ser_sd_transport_rx_packet_handler(uint8_t * p_data, uint16_t length){    if (p_data && (length >= SER_PKT_TYPE_SIZE))    {        const uint8_t packet_type = p_data[SER_PKT_TYPE_POS];        p_data += SER_PKT_TYPE_SIZE;        length -= SER_PKT_TYPE_SIZE;        switch (packet_type)        {            case SER_PKT_TYPE_RESP:            case SER_PKT_TYPE_DTM_RESP:                if (m_rsp_wait)                {                    m_return_value = m_rsp_dec_handler(p_data, length);                    (void)ser_sd_transport_rx_free(p_data);                    /* Reset response flag - cmd_write function is pending on it.*/                    m_rsp_wait = false;                    /* If os handler is set, signal os that response has arrived.*/                    if (m_os_rsp_set_handler)                    {                        m_os_rsp_set_handler();                    }                }                else                {                    /* Unexpected packet. */                    (void)ser_sd_transport_rx_free(p_data);                    APP_ERROR_HANDLER(packet_type);                }                break;            case SER_PKT_TYPE_EVT:                /* It is ensured during opening that handler is not NULL. No check needed. */                APPL_LOG("/r/n[EVT_ID]: 0x%X /r/n", uint16_decode(&p_data[SER_EVT_ID_POS])); // p_data points to EVT_ID                m_evt_handler(p_data, length);                break;            default:                (void)ser_sd_transport_rx_free(p_data);                APP_ERROR_HANDLER(packet_type);                break;        }    }}