C++ STREAM_TO_UINT8函数代码示例

/*******************************************************************************
**
** Function         btm_ble_batchscan_filter_track_adv_vse_cback
**
** Description      VSE callback for batch scan, filter, and tracking events.
**
** Returns          None
**
*******************************************************************************/
void btm_ble_batchscan_filter_track_adv_vse_cback(UINT8 len, UINT8 *p)
{
    UINT8   sub_event = 0, filt_index = 0, addr_type = 0, adv_state = 0;
    BD_ADDR bd_addr;
    STREAM_TO_UINT8(sub_event, p);

    BTM_TRACE_EVENT("btm_ble_batchscan_filter_track_adv_vse_cback called with event:%x", sub_event);
    if (HCI_VSE_SUBCODE_BLE_THRESHOLD_SUB_EVT == sub_event &&
        NULL != ble_batchscan_cb.p_thres_cback)
    {
        ble_batchscan_cb.p_thres_cback(ble_batchscan_cb.ref_value);
        return;
    }

    if (HCI_VSE_SUBCODE_BLE_TRACKING_SUB_EVT == sub_event && NULL != ble_advtrack_cb.p_track_cback)
    {
        if (len < 10)
            return;
        STREAM_TO_UINT8(filt_index, p);
        STREAM_TO_UINT8(addr_type, p);
        STREAM_TO_BDADDR(bd_addr, p);
        STREAM_TO_UINT8(adv_state, p);
        BTM_TRACE_EVENT("track_adv_vse_cback called: %d, %d, %d", filt_index, addr_type, adv_state);
        ble_advtrack_cb.p_track_cback(filt_index, addr_type, bd_addr, adv_state,
            ble_advtrack_cb.ref_value);
        return;
    }
}

// Returns true if the event was intercepted and should not proceed to
// higher layers. Also inspects an incoming event for interesting
// information, like how many commands are now able to be sent.
static bool filter_incoming_event(BT_HDR *packet) {
  waiting_command_t *wait_entry = NULL;
  uint8_t *stream = packet->data;
  uint8_t event_code;
  command_opcode_t opcode;

  STREAM_TO_UINT8(event_code, stream);
  STREAM_SKIP_UINT8(stream); // Skip the parameter total length field

  if (event_code == HCI_COMMAND_COMPLETE_EVT) {
    STREAM_TO_UINT8(command_credits, stream);
    STREAM_TO_UINT16(opcode, stream);

    wait_entry = get_waiting_command(opcode);
    if (!wait_entry)
      LOG_WARN(LOG_TAG, "%s command complete event with no matching command. opcode: 0x%x.", __func__, opcode);
    else if (wait_entry->complete_callback)
      wait_entry->complete_callback(packet, wait_entry->context);
    else if (wait_entry->complete_future)
      future_ready(wait_entry->complete_future, packet);

    goto intercepted;
  } else if (event_code == HCI_COMMAND_STATUS_EVT) {
    uint8_t status;
    STREAM_TO_UINT8(status, stream);
    STREAM_TO_UINT8(command_credits, stream);
    STREAM_TO_UINT16(opcode, stream);

    // If a command generates a command status event, it won't be getting a command complete event

    wait_entry = get_waiting_command(opcode);
    if (!wait_entry)
      LOG_WARN(LOG_TAG, "%s command status event with no matching command. opcode: 0x%x", __func__, opcode);
    else if (wait_entry->status_callback)
      wait_entry->status_callback(status, wait_entry->command, wait_entry->context);

    goto intercepted;
  }

  return false;
intercepted:;
  non_repeating_timer_restart_if(command_response_timer, !list_is_empty(commands_pending_response));

  if (wait_entry) {
    // If it has a callback, it's responsible for freeing the packet
    if (event_code == HCI_COMMAND_STATUS_EVT || (!wait_entry->complete_callback && !wait_entry->complete_future))
      buffer_allocator->free(packet);

    // If it has a callback, it's responsible for freeing the command
    if (event_code == HCI_COMMAND_COMPLETE_EVT || !wait_entry->status_callback)
      buffer_allocator->free(wait_entry->command);

    osi_free(wait_entry);
  } else {
    buffer_allocator->free(packet);
  }

  return true;
}

//*****************************************************************************
//
//!  SpiReadDataCont
//!
//!  @param  None
//!
//!  @return None
//!
//!  @brief  This function processes received SPI Header and in accordance with 
//!	         it - continues reading the packet
//
//*****************************************************************************
long
SpiReadDataCont(void)
{
	long data_to_recv;
	unsigned char *evnt_buff, type;
	
	//determine what type of packet we have
	evnt_buff =  sSpiInformation.pRxPacket;
	data_to_recv = 0;
	STREAM_TO_UINT8((char *)(evnt_buff + SPI_HEADER_SIZE), HCI_PACKET_TYPE_OFFSET,
									type);
	
	switch(type)
	{
	case HCI_TYPE_DATA:
		{
			// We need to read the rest of data..
			STREAM_TO_UINT16((char *)(evnt_buff + SPI_HEADER_SIZE), 
											 HCI_DATA_LENGTH_OFFSET, data_to_recv);
			if (!((HEADERS_SIZE_EVNT + data_to_recv) & 1))
			{	
				data_to_recv++;
			}
			
			if (data_to_recv)
			{
				SpiReadDataSynchronous(evnt_buff + 10, data_to_recv);
			}
			break;
		}
	case HCI_TYPE_EVNT:
		{
			// Calculate the rest length of the data
			STREAM_TO_UINT8((char *)(evnt_buff + SPI_HEADER_SIZE), 
											HCI_EVENT_LENGTH_OFFSET, data_to_recv);
			data_to_recv -= 1;
			
			// Add padding byte if needed
			if ((HEADERS_SIZE_EVNT + data_to_recv) & 1)
			{
				
				data_to_recv++;
			}
			
			if (data_to_recv)
			{
				SpiReadDataSynchronous(evnt_buff + 10, data_to_recv);
			}
			
			sSpiInformation.ulSpiState = eSPI_STATE_READ_EOT;
			break;
		}
	}
	
	return (0);
}

static bt_status_t btif_mp_test_evt(void* msg)
{
    BT_HDR *p_msg = (BT_HDR *)msg;
    UINT8   *p = (UINT8 *)(p_msg + 1) + p_msg->offset;
    UINT8   hci_evt_len;
    UINT8   hci_evt_code;
    STREAM_TO_UINT8  (hci_evt_code, p);
    STREAM_TO_UINT8  (hci_evt_len, p);
    BTIF_TRACE_DEBUG1("%s: notify btif_mp_test_evt", __FUNCTION__);
    if(btif_dut_mode)
    {
        btif_mp_rx_data_ind(hci_evt_code, (uint8_t*)p, hci_evt_len);
    }

    return BT_STATUS_SUCCESS;
}

void gatt_verify_signature(tGATT_TCB *p_tcb, BT_HDR *p_buf)
{
    UINT16  cmd_len;
#if (GATTS_INCLUDED == TRUE)
    UINT8   op_code;
#endif  ///GATTS_INCLUDED == TRUE
    UINT8   *p, *p_orig = (UINT8 *)(p_buf + 1) + p_buf->offset;
    UINT32  counter;

    if (p_buf->len < GATT_AUTH_SIGN_LEN + 4) {
        GATT_TRACE_ERROR("%s: Data length %u less than expected %u",
                         __func__, p_buf->len, GATT_AUTH_SIGN_LEN + 4);
        return;
    }
    cmd_len = p_buf->len - GATT_AUTH_SIGN_LEN + 4;
    p =  p_orig + cmd_len - 4;
    STREAM_TO_UINT32(counter, p);

    if (BTM_BleVerifySignature(p_tcb->peer_bda, p_orig, cmd_len, counter, p)) {
#if (GATTS_INCLUDED == TRUE)
        STREAM_TO_UINT8(op_code, p_orig);
        gatt_server_handle_client_req (p_tcb, op_code, (UINT16)(p_buf->len - 1), p_orig);
#endif  ///GATTS_INCLUDED == TRUE
    } else {
        /* if this is a bad signature, assume from attacker, ignore it  */
        GATT_TRACE_ERROR("Signature Verification Failed, data ignored");
    }

    return;
}

static bt_status_t btif_mp_notify_evt(void* msg)
{
    BT_HDR *p_msg = (BT_HDR *)msg;
    char   *p = (char *)(p_msg + 1);
    UINT8 mp_op_code;
    UINT8 mp_op_paraLen;
    STREAM_TO_UINT8  (mp_op_code, p);
    STREAM_TO_UINT8  (mp_op_paraLen, p);
    
    BTIF_TRACE_DEBUG2("%s: notify1 :%s", __FUNCTION__, p);
    
    p[mp_op_paraLen] = NULL;
    BTIF_TRACE_DEBUG2("%s: notify2 :%s", __FUNCTION__, p);
    
    HAL_CBACK(bt_hal_cbacks, dut_mode_recv_cb, mp_op_code, p);

    
    return BT_STATUS_SUCCESS;
}

/*******************************************************************************
**
** Function         gatt_process_error_rsp
**
** Description      This function is called to handle the error response
**
**
** Returns          void
**
*******************************************************************************/
void gatt_process_error_rsp(tGATT_TCB *p_tcb, tGATT_CLCB *p_clcb, UINT8 op_code,
                            UINT16 len, UINT8 *p_data)
{
    UINT8   opcode, reason, * p= p_data;
    UINT16  handle;
    tGATT_VALUE  *p_attr = (tGATT_VALUE *)p_clcb->p_attr_buf;

    UNUSED(op_code);
    UNUSED(len);

    GATT_TRACE_DEBUG("gatt_process_error_rsp ");
    STREAM_TO_UINT8(opcode, p);
    STREAM_TO_UINT16(handle, p);
    STREAM_TO_UINT8(reason, p);

    if (p_clcb->operation == GATTC_OPTYPE_DISCOVERY)
    {
        gatt_proc_disc_error_rsp(p_tcb, p_clcb, opcode, handle, reason);
    }
    else
    {
        if ( (p_clcb->operation == GATTC_OPTYPE_WRITE) &&
             (p_clcb->op_subtype == GATT_WRITE) &&
             (opcode == GATT_REQ_PREPARE_WRITE) &&
             (p_attr) &&
             (handle == p_attr->handle)  )
        {
            p_clcb->status = reason;
            gatt_send_queue_write_cancel(p_tcb, p_clcb, GATT_PREP_WRITE_CANCEL);
        }
        else if ((p_clcb->operation == GATTC_OPTYPE_READ) &&
                 ((p_clcb->op_subtype == GATT_READ_CHAR_VALUE_HDL) ||
                  (p_clcb->op_subtype == GATT_READ_BY_HANDLE)) &&
                 (opcode == GATT_REQ_READ_BLOB) &&
                 p_clcb->first_read_blob_after_read &&
                 (reason == GATT_NOT_LONG))
        {
            gatt_end_operation(p_clcb, GATT_SUCCESS, (void *)p_clcb->p_attr_buf);
        }
        else
            gatt_end_operation(p_clcb, reason, NULL);
    }
}

/*******************************************************************************
**
** Function         btm_ble_clear_white_list_complete
**
** Description      This function clears the white list complete.
*******************************************************************************/
void btm_ble_clear_white_list_complete(UINT8 *p_data, UINT16 evt_len)
{
    tBTM_BLE_CB *p_cb = &btm_cb.ble_ctr_cb;
    UINT8       status;
    BTM_TRACE_EVENT0 ("btm_ble_clear_white_list_complete");
    STREAM_TO_UINT8  (status, p_data);

    if (status == HCI_SUCCESS)
        p_cb->num_empty_filter = p_cb->max_filter_entries;

}

/*******************************************************************************
**
** Function         nfc_enabled
**
** Description      NFCC enabled, proceed with stack start up.
**
** Returns          void
**
*******************************************************************************/
void nfc_enabled (tNFC_STATUS nfc_status, BT_HDR *p_init_rsp_msg)
{
    tNFC_RESPONSE evt_data;
    tNFC_CONN_CB  *p_cb = &nfc_cb.conn_cb[NFC_RF_CONN_ID];
    UINT8   *p;
    UINT8   num_interfaces = 0, xx;
    int     yy = 0;

    memset (&evt_data, 0, sizeof (tNFC_RESPONSE));

    if (nfc_status == NCI_STATUS_OK)
    {
        nfc_set_state (NFC_STATE_IDLE);

        p = (UINT8 *) (p_init_rsp_msg + 1) + p_init_rsp_msg->offset + NCI_MSG_HDR_SIZE + 1;
        /* we currently only support NCI of the same version.
        * We may need to change this, when we support multiple version of NFCC */
        evt_data.enable.nci_version = NCI_VERSION;
        STREAM_TO_UINT32 (evt_data.enable.nci_features, p);
        STREAM_TO_UINT8 (num_interfaces, p);

        evt_data.enable.nci_interfaces = 0;
        for (xx = 0; xx < num_interfaces; xx++)
        {
            if ((*p) <= NCI_INTERFACE_MAX)
                evt_data.enable.nci_interfaces |= (1 << (*p));
            #if(NFC_NXP_NOT_OPEN_INCLUDED == TRUE)
            else if (((*p) >= NCI_INTERFACE_FIRST_VS) && (yy < NFC_NFCC_MAX_NUM_VS_INTERFACE))
            #else
            else if (((*p) > NCI_INTERFACE_FIRST_VS) && (yy < NFC_NFCC_MAX_NUM_VS_INTERFACE))
            #endif
            {
                /* save the VS RF interface in control block, if there's still room */
                nfc_cb.vs_interface[yy++] = *p;
            }
            p++;
        }
        nfc_cb.nci_interfaces    = evt_data.enable.nci_interfaces;
        memcpy (evt_data.enable.vs_interface, nfc_cb.vs_interface, NFC_NFCC_MAX_NUM_VS_INTERFACE);
        evt_data.enable.max_conn = *p++;
        STREAM_TO_UINT16 (evt_data.enable.max_ce_table, p);
#if (NFC_RW_ONLY == FALSE)
        nfc_cb.max_ce_table          = evt_data.enable.max_ce_table;
        nfc_cb.nci_features          = evt_data.enable.nci_features;
        nfc_cb.max_conn              = evt_data.enable.max_conn;
#endif
        nfc_cb.nci_ctrl_size         = *p++; /* Max Control Packet Payload Length */
        p_cb->init_credits           = p_cb->num_buff = 0;
        STREAM_TO_UINT16 (evt_data.enable.max_param_size, p);
        nfc_set_conn_id (p_cb, NFC_RF_CONN_ID);
        evt_data.enable.manufacture_id   = *p++;
        STREAM_TO_ARRAY (evt_data.enable.nfcc_info, p, NFC_NFCC_INFO_LEN);
        NFC_DiscoveryMap (nfc_cb.num_disc_maps, (tNCI_DISCOVER_MAPS *) nfc_cb.p_disc_maps, NULL);
    }

/*******************************************************************************
**
** Function         btm_ble_clear_white_list_complete
**
** Description      Indicates white list cleared.
**
*******************************************************************************/
void btm_ble_clear_white_list_complete(UINT8 *p_data, UINT16 evt_len)
{
    tBTM_BLE_CB *p_cb = &btm_cb.ble_ctr_cb;
    UINT8       status;
    UNUSED(evt_len);

    BTM_TRACE_EVENT ("btm_ble_clear_white_list_complete");
    STREAM_TO_UINT8  (status, p_data);

    if (status == HCI_SUCCESS)
        p_cb->white_list_avail_size = controller_get_interface()->get_ble_white_list_size();
}

/*******************************************************************************
**
** Function         gatt_le_data_ind
**
** Description      This function is called when data is received from L2CAP.
**                  if we are the originator of the connection, we are the ATT
**                  client, and the received message is queued up for the client.
**
**                  If we are the destination of the connection, we are the ATT
**                  server, so the message is passed to the server processing
**                  function.
**
** Returns          void
**
*******************************************************************************/
void gatt_data_process (tGATT_TCB *p_tcb, BT_HDR *p_buf)
{
    GATT_TRACE_DEBUG0("gatt_data_process");
    UINT8   *p = (UINT8 *)(p_buf + 1) + p_buf->offset;
    UINT8   op_code, pseudo_op_code;
    UINT16  msg_len;


    if (p_buf->len > 0)
    {
        msg_len = p_buf->len - 1;
        STREAM_TO_UINT8(op_code, p);
        GATT_TRACE_DEBUG1("op_code = %d", op_code);

        /* remove the two MSBs associated with sign write and write cmd */
        pseudo_op_code = op_code & (~GATT_WRITE_CMD_MASK);

        if (pseudo_op_code < GATT_OP_CODE_MAX)
        {
            if (op_code == GATT_SIGN_CMD_WRITE)
            {
                GATT_TRACE_DEBUG0("op_code == GATT_SIGN_CMD_WRITE");
                gatt_verify_signature(p_tcb, p_buf);
                return;
            }
            else
            {
                /* message from client */
                if ((op_code % 2) == 0)
                {
                    GATT_TRACE_DEBUG0("gatt_server_handle_client_req");
                    gatt_server_handle_client_req (p_tcb, op_code, msg_len, p);
                }
                else
                {
                    GATT_TRACE_DEBUG0("gatt_client_handle_server_rsp");
                    gatt_client_handle_server_rsp (p_tcb, op_code, msg_len, p);
                }
            }
        }
        else
        {
            GATT_TRACE_ERROR1 ("ATT - Rcvd L2CAP data, unknown cmd: 0x%x", op_code);
        }
    }
    else
    {
        GATT_TRACE_ERROR0 ("invalid data length, ignore");
    }

    GKI_freebuf (p_buf);
}

/*******************************************************************************
 **
 ** Function         btusb_isoc_check_hdr
 **
 ** Description      Check the packet header
 **
 ** Parameters       p_dev: device instance control block
 **
 ** Returns          void
 **
 *******************************************************************************/
static bool btusb_isoc_check_hdr(struct btusb *p_dev)
{
    unsigned char *p_data = p_dev->voice_rx.hdr;
    int idx;
    unsigned short sco_handle;
    unsigned char size;
    struct btusb_voice_pkt *p_pkt;
    BT_HDR *p_hdr;
    struct btusb_voice_channel *p_chan;

    STREAM_TO_UINT16(sco_handle, p_data);
    sco_handle &= 0x0fff;
    STREAM_TO_UINT8(size, p_data);

    for (idx = 0; idx < ARRAY_SIZE(p_dev->voice_rx.channels); idx++)
    {
        p_chan = &p_dev->voice_rx.channels[idx];
        if ((p_chan->used) &&
            (sco_handle == p_chan->handle) &&
            (size <= (2 * p_chan->burst)))
        {
            /* check if there is already a message being consolidated */
            if (unlikely(!p_chan->p_pkt))
            {
                if (!btusb_cq_get(&p_dev->voice_rx_list, &p_pkt))
                {
                    BTUSB_ERR("No buffer available for SCO defragmentation\n");
                    return false;
                }
                p_hdr = &p_pkt->bt_hdr;
                p_hdr->len = BTUSB_VOICE_HEADER_SIZE;
                p_hdr->offset = 0;
                p_hdr->layer_specific = 0;

                p_data = (unsigned char *) (p_hdr + 1);

                /* add sco handle and buffer size */
                UINT16_TO_STREAM(p_data, sco_handle);
                UINT8_TO_STREAM(p_data, BTUSB_SCO_RX_LEN);

                p_chan->p_pkt = p_pkt;
            }
            p_dev->voice_rx.remaining = size;
            p_dev->voice_rx.pp_pkt = &p_chan->p_pkt;

            return true;
        }
    }
    return false;
}

/*******************************************************************************
**
** Function         gatt_process_read_info_rsp
**
** Description      This function is called to handle the read information
**                  response.
**
**
** Returns          void
**
*******************************************************************************/
void gatt_process_read_info_rsp(tGATT_TCB *p_tcb, tGATT_CLCB *p_clcb, UINT8 op_code,
                                UINT16 len, UINT8 *p_data)
{
    tGATT_DISC_RES  result;
    UINT8   *p = p_data, uuid_len = 0, type;

    UNUSED(p_tcb);
    UNUSED(op_code);

    if (len < GATT_INFO_RSP_MIN_LEN) {
        GATT_TRACE_ERROR("invalid Info Response PDU received, discard.");
        gatt_end_operation(p_clcb, GATT_INVALID_PDU, NULL);
        return;
    }
    /* unexpected response */
    if (p_clcb->operation != GATTC_OPTYPE_DISCOVERY || p_clcb->op_subtype != GATT_DISC_CHAR_DSCPT) {
        return;
    }

    STREAM_TO_UINT8(type, p);
    len -= 1;

    if (type == GATT_INFO_TYPE_PAIR_16) {
        uuid_len = LEN_UUID_16;
    } else if (type == GATT_INFO_TYPE_PAIR_128) {
        uuid_len = LEN_UUID_128;
    }

    while (len >= uuid_len + 2) {
        STREAM_TO_UINT16 (result.handle, p);

        if (uuid_len > 0) {
            if (!gatt_parse_uuid_from_cmd(&result.type, uuid_len, &p)) {
                break;
            }
        } else {
            memcpy (&result.type, &p_clcb->uuid, sizeof(tBT_UUID));
        }

        len -= (uuid_len + 2);

        if (p_clcb->p_reg->app_cb.p_disc_res_cb) {
            (*p_clcb->p_reg->app_cb.p_disc_res_cb)(p_clcb->conn_id, p_clcb->op_subtype, &result);
        }
    }

    p_clcb->s_handle = (result.handle == 0) ? 0 : (result.handle + 1);
    /* initiate another request */
    gatt_act_discovery(p_clcb) ;
}

void SpiReadPacket(void)
{
    int length;

    /* read SPI header */
    SpiReadDataSynchronous(sSpiInformation.pRxPacket, SPI_HEADER_SIZE);

    /* parse data length  */
    STREAM_TO_UINT8(sSpiInformation.pRxPacket, SPI_HEADER_SIZE-1, length);

    /* read the remainder of the packet */
    SpiReadDataSynchronous(sSpiInformation.pRxPacket + SPI_HEADER_SIZE, length);

    sSpiInformation.ulSpiState = eSPI_STATE_READ_EOT;
}

/*******************************************************************************
**
** Function         smp_br_data_received
**
** Description      This function is called when data is received from L2CAP on
**                  SMP BR channel.
**
** Returns          void
**
*******************************************************************************/
static void smp_br_data_received(UINT16 channel, BD_ADDR bd_addr, BT_HDR *p_buf)
{
    tSMP_CB *p_cb = &smp_cb;
    UINT8   *p = (UINT8 *)(p_buf + 1) + p_buf->offset;
    UINT8   cmd ;
    SMP_TRACE_EVENT ("SMDBG l2c %s", __func__);

    STREAM_TO_UINT8(cmd, p);

    /* sanity check */
    if ((SMP_OPCODE_MAX < cmd) || (SMP_OPCODE_MIN > cmd))
    {
        SMP_TRACE_WARNING( "Ignore received command with RESERVED code 0x%02x", cmd);
        GKI_freebuf(p_buf);
        return;
    }

    /* reject the pairing request if there is an on-going SMP pairing */
    if (SMP_OPCODE_PAIRING_REQ == cmd)
    {
        if ((p_cb->state == SMP_STATE_IDLE) && (p_cb->br_state == SMP_BR_STATE_IDLE))
        {
            p_cb->role = HCI_ROLE_SLAVE;
            p_cb->smp_over_br = TRUE;
            memcpy(&p_cb->pairing_bda[0], bd_addr, BD_ADDR_LEN);
        }
        else if (memcmp(&bd_addr[0], p_cb->pairing_bda, BD_ADDR_LEN))
        {
            GKI_freebuf (p_buf);
            smp_reject_unexpected_pairing_command(bd_addr);
            return;
        }
        /* else, out of state pairing request received, passed into State Machine */
    }

    if (memcmp(&bd_addr[0], p_cb->pairing_bda, BD_ADDR_LEN) == 0)
    {
        btu_stop_timer (&p_cb->rsp_timer_ent);
        btu_start_timer (&p_cb->rsp_timer_ent, BTU_TTYPE_SMP_PAIRING_CMD,
                             SMP_WAIT_FOR_RSP_TOUT);

        p_cb->rcvd_cmd_code = cmd;
        p_cb->rcvd_cmd_len = (UINT8) p_buf->len;
        smp_br_state_machine_event(p_cb, cmd, p);
    }

    GKI_freebuf (p_buf);
}