C++ STREAM_TO_UINT16函数代码示例

/*******************************************************************************
**
** Function         gatt_process_prep_write_rsp
**
** Description      This function is called to handle the read response
**
**
** Returns          void
**
*******************************************************************************/
void gatt_process_prep_write_rsp (tGATT_TCB *p_tcb, tGATT_CLCB *p_clcb, UINT8 op_code,
                                  UINT16 len, UINT8 *p_data)
{
    tGATT_VALUE  value = {0};
    UINT8        *p= p_data;

    GATT_TRACE_ERROR2("value resp op_code = %s len = %d", gatt_dbg_op_name(op_code), len);

    STREAM_TO_UINT16 (value.handle, p);
    STREAM_TO_UINT16 (value.offset, p);

    value.len = len - 4;

    memcpy (value.value, p, value.len);

    if (p_clcb->op_subtype == GATT_WRITE_PREPARE)
    {
        p_clcb->status = GATT_SUCCESS;
        /* application should verify handle offset
           and value are matched or not */

        gatt_end_operation(p_clcb, p_clcb->status, &value);
    }
    else if (p_clcb->op_subtype == GATT_WRITE )
    {
        if (!gatt_check_write_long_terminate(p_tcb, p_clcb, &value))
            gatt_send_prepare_write(p_tcb, p_clcb);
    }

}

//*****************************************************************************
//
//!  hci_event_unsol_flowcontrol_handler
//!
//!  @param  pEvent  pointer to the string contains parameters for IPERF
//!  @return         ESUCCESS if successful, EFAIL if an error occurred
//!
//!  @brief  Called in case unsolicited event from type
//!          HCI_EVNT_DATA_UNSOL_FREE_BUFF has received.
//!				   Keep track on the number of packets transmitted and update the
//!					 number of free buffer in the SL device.
//
//*****************************************************************************
INT32 hci_event_unsol_flowcontrol_handler(CHAR *pEvent)
{

	INT32 temp, value;
	UINT16 i;
	UINT16  pusNumberOfHandles=0;
	CHAR *pReadPayload;

	STREAM_TO_UINT16((CHAR *)pEvent,HCI_EVENT_HEADER_SIZE,pusNumberOfHandles);
	pReadPayload = ((CHAR *)pEvent +
		HCI_EVENT_HEADER_SIZE + sizeof(pusNumberOfHandles));	
	temp = 0;

	for(i = 0; i < pusNumberOfHandles ; i++)
	{
		STREAM_TO_UINT16(pReadPayload, FLOW_CONTROL_EVENT_FREE_BUFFS_OFFSET, value);
		temp += value;
		pReadPayload += FLOW_CONTROL_EVENT_SIZE;  
	}

	tSLInformation.usNumberOfFreeBuffers += temp;
	tSLInformation.NumberOfReleasedPackets += temp;

	return(ESUCCESS);
}

/*******************************************************************************
**
** Function         gatt_process_find_type_value_rsp
**
** Description      This function is called to handle find by type value response.
**
**
** Returns          void
**
*******************************************************************************/
void gatt_process_find_type_value_rsp (tGATT_TCB *p_tcb, tGATT_CLCB *p_clcb, UINT16 len, UINT8 *p_data)
{
    tGATT_DISC_RES      result;
    tGATT_DISC_VALUE    record_value;
    UINT8               *p = p_data;

    GATT_TRACE_DEBUG0("gatt_process_find_type_value_rsp ");
    /* unexpected response */
    if (p_clcb->operation != GATTC_OPTYPE_DISCOVERY || p_clcb->op_subtype != GATT_DISC_SRVC_BY_UUID)
        return;

    memset (&record_value, 0, sizeof(tGATT_DISC_VALUE));
    result.type.len = 2;
    result.type.uu.uuid16 = GATT_UUID_PRI_SERVICE;

    /* returns a series of handle ranges */
    while (len >= 4)
    {
        STREAM_TO_UINT16 (result.handle, p);
        STREAM_TO_UINT16 (record_value.handle, p);
        len -= 4;

        memcpy (&result.value, &record_value, sizeof (result.value));;

        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);
    }

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

// 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;
}

/*******************************************************************************
**
** Function         gap_ble_c_cmpl_cback
**
** Description      Client operation complete callback.
**
** Returns          void
**
*******************************************************************************/
static void gap_ble_c_cmpl_cback (UINT16 conn_id, tGATTC_OPTYPE op, tGATT_STATUS status, tGATT_CL_COMPLETE *p_data)

{
    tGAP_CLCB   *p_clcb = gap_ble_find_clcb_by_conn_id(conn_id);
    UINT16      op_type;
    UINT16      min, max, latency, tout;
    UINT16      len;
    UINT8       *pp;

    if (p_clcb == NULL)
        return;

    op_type = p_clcb->cl_op_uuid;

    GAP_TRACE_EVENT ("gap_ble_c_cmpl_cback() - op_code: 0x%02x  status: 0x%02x  read_type: 0x%04x", op, status, op_type);
    /* Currently we only issue read commands */
    if (op != GATTC_OPTYPE_READ)
        return;

    if (status != GATT_SUCCESS)
    {
        gap_ble_cl_op_cmpl(p_clcb, FALSE, 0, NULL);
        return;
    }

    pp = p_data->att_value.value;

    switch (op_type)
    {
        case GATT_UUID_GAP_PREF_CONN_PARAM:
            GAP_TRACE_EVENT ("GATT_UUID_GAP_PREF_CONN_PARAM");
            /* Extract the peripheral preferred connection parameters and save them */

            STREAM_TO_UINT16 (min, pp);
            STREAM_TO_UINT16 (max, pp);
            STREAM_TO_UINT16 (latency, pp);
            STREAM_TO_UINT16 (tout, pp);

            BTM_BleSetPrefConnParams (p_clcb->bda, min, max, latency, tout);
            /* release the connection here */
            gap_ble_cl_op_cmpl(p_clcb, TRUE, 0, NULL);
            break;

        case GATT_UUID_GAP_DEVICE_NAME:
            GAP_TRACE_EVENT ("GATT_UUID_GAP_DEVICE_NAME");
            len = (UINT16)strlen((char *)pp);
            if (len > GAP_CHAR_DEV_NAME_SIZE)
                len = GAP_CHAR_DEV_NAME_SIZE;
            gap_ble_cl_op_cmpl(p_clcb, TRUE, len, pp);
            break;
        case GATT_UUID_GAP_ICON:
            break;
    }
}

/*******************************************************************************
**
** 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         smp_rand_back
**
** Description      This function is to process the rand command finished,
**                  process the random/encrypted number for further action.
**
** Returns          void
**
*******************************************************************************/
static void smp_rand_back(tBTM_RAND_ENC *p)
{
    tSMP_CB *p_cb = &smp_cb;
    UINT8   *pp = p->param_buf;
    UINT8   failure = SMP_PAIR_FAIL_UNKNOWN;
    UINT8   state = p_cb->rand_enc_proc & ~0x80;

    SMP_TRACE_DEBUG1 ("smp_rand_back state=0x%x", state);
    if (p && p->status == HCI_SUCCESS)
    {
        switch (state)
        {

            case SMP_GEN_SRAND_MRAND:
                memcpy((void *)p_cb->rand, p->param_buf, p->param_len);
                smp_genenrate_rand_cont(p_cb, NULL);
                break;

            case SMP_GEN_SRAND_MRAND_CONT:
                memcpy((void *)&p_cb->rand[8], p->param_buf, p->param_len);
                smp_genenrate_confirm(p_cb, NULL);
                break;

            case SMP_GEN_DIV_LTK:
                STREAM_TO_UINT16(p_cb->div, pp);
                smp_genenrate_ltk_cont(p_cb, NULL);
                break;

            case SMP_GEN_DIV_CSRK:
                STREAM_TO_UINT16(p_cb->div, pp);
                smp_compute_csrk(p_cb, NULL);
                break;

            case SMP_GEN_TK:
                smp_proc_passkey(p_cb, p);
                break;

            case SMP_GEN_RAND_V:
                memcpy(p_cb->enc_rand, p->param_buf, BT_OCTET8_LEN);
                smp_generate_y(p_cb, NULL);
                break;

        }

        return;
    }

    SMP_TRACE_ERROR1("smp_rand_back Key generation failed: (%d)", p_cb->rand_enc_proc);

    smp_sm_event(p_cb, SMP_AUTH_CMPL_EVT, &failure);

}

static void transmit_command(
    BT_HDR *command,
    command_complete_cb complete_callback,
    command_status_cb status_callback,
    void *context)
{
    uint8_t *stream;
    waiting_command_t *wait_entry = osi_calloc(sizeof(waiting_command_t));
    if (!wait_entry) {
        LOG_ERROR("%s couldn't allocate space for wait entry.", __func__);
        return;
    }

    stream = command->data + command->offset;
    STREAM_TO_UINT16(wait_entry->opcode, stream);
    wait_entry->complete_callback = complete_callback;
    wait_entry->status_callback = status_callback;
    wait_entry->command = command;
    wait_entry->context = context;

    // Store the command message type in the event field
    // in case the upper layer didn't already
    command->event = MSG_STACK_TO_HC_HCI_CMD;
    LOG_DEBUG("HCI Enqueue Comamnd opcode=0x%x\n", wait_entry->opcode);
    BTTRC_DUMP_BUFFER(NULL, command->data + command->offset, command->len);

    fixed_queue_enqueue(hci_host_env.command_queue, wait_entry);
    hci_host_task_post();
}

//*****************************************************************************
//
//!  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);
}

/*******************************************************************************
**
** Function         gatt_process_mtu_rsp
**
** Description      This function is called to process the configure MTU response.
**
**
** Returns          void
**
*******************************************************************************/
void gatt_process_mtu_rsp(tGATT_TCB *p_tcb, tGATT_CLCB *p_clcb, UINT16 len, UINT8 *p_data)
{
    UINT16 mtu;

    STREAM_TO_UINT16(mtu, p_data);

    if (mtu < p_tcb->payload_size && mtu >= GATT_DEF_BLE_MTU_SIZE)
        p_tcb->payload_size = mtu;

    gatt_end_operation(p_clcb, p_clcb->status, NULL);
}

void SpiReceiveHandler(void *pvBuffer)
{
  tSLInformation.pucReceivedData = (uint8_t *)pvBuffer;
  tSLInformation.usEventOrDataReceived = 1;

  uint16_t event_type;
  STREAM_TO_UINT16((char *)tSLInformation.pucReceivedData, HCI_EVENT_OPCODE_OFFSET,event_type);
  nllvdbg("Evtn:0x%x\n", event_type);

  hci_unsolicited_event_handler();
}

/*******************************************************************************
**
** Function         gatt_process_handle_rsp
**
** Description      This function is called to handle the write response
**
**
** Returns          void
**
*******************************************************************************/
void gatt_process_handle_rsp(tGATT_TCB *p_tcb, tGATT_CLCB *p_clcb, UINT8 op_code, UINT16 len, UINT8 *p_data)
{
    UINT16      handle;
    UINT8       * p= p_data;

    STREAM_TO_UINT16(handle, p);
    len -= 2;

    if (op_code == GATT_RSP_WRITE)
        gatt_end_operation(p_clcb, GATT_SUCCESS, NULL);
}

int32_t cc3000_event::hci_event_unsol_flowcontrol_handler(uint8_t *event) {
    int32_t temp, value;
    uint16_t i;
    uint16_t pusNumberOfHandles=0;
    uint8_t *pReadPayload;

    STREAM_TO_UINT16((uint8_t *)event,HCI_EVENT_HEADER_SIZE,pusNumberOfHandles);
    pReadPayload = ((uint8_t *)event + HCI_EVENT_HEADER_SIZE + sizeof(pusNumberOfHandles));
    temp = 0;

    for(i = 0; i < pusNumberOfHandles; i++) {
        STREAM_TO_UINT16(pReadPayload, FLOW_CONTROL_EVENT_FREE_BUFFS_OFFSET, value);
        temp += value;
        pReadPayload += FLOW_CONTROL_EVENT_SIZE;
    }

    _simple_link.set_number_free_buffers(_simple_link.get_number_free_buffers() + temp);
    _simple_link.set_number_of_released_packets(_simple_link.get_released_packets() + temp);

    return(ESUCCESS);
}

/*******************************************************************************
**
** Function         hw_sco_cfg_cback
**
** Description      Callback function for SCO configuration rquest
**
** Returns          None
**
*******************************************************************************/
void hw_sco_cfg_cback(void *p_mem)
{
#if 0
    HC_BT_HDR *p_evt_buf = (HC_BT_HDR *) p_mem;
    uint8_t     *p;
    uint16_t    opcode;
    HC_BT_HDR  *p_buf=NULL;

    p = (uint8_t *)(p_evt_buf + 1) + HCI_EVT_CMD_CMPL_OPCODE;
    STREAM_TO_UINT16(opcode,p);

    /* Free the RX event buffer */
    if (bt_vendor_cbacks)
        bt_vendor_cbacks->dealloc(p_evt_buf);

#if (!defined(SCO_USE_I2S_INTERFACE) || (SCO_USE_I2S_INTERFACE == FALSE))
    if (opcode == HCI_VSC_WRITE_SCO_PCM_INT_PARAM)
    {
        uint8_t ret = FALSE;

        /* Ask a new buffer to hold WRITE_PCM_DATA_FORMAT_PARAM command */
        if (bt_vendor_cbacks)
            p_buf = (HC_BT_HDR *) bt_vendor_cbacks->alloc(BT_HC_HDR_SIZE + \
                                                HCI_CMD_PREAMBLE_SIZE + \
                                                PCM_DATA_FORMAT_PARAM_SIZE);
        if (p_buf)
        {
            p_buf->event = MSG_STACK_TO_HC_HCI_CMD;
            p_buf->offset = 0;
            p_buf->layer_specific = 0;
            p_buf->len = HCI_CMD_PREAMBLE_SIZE + PCM_DATA_FORMAT_PARAM_SIZE;

            p = (uint8_t *) (p_buf + 1);
            UINT16_TO_STREAM(p, HCI_VSC_WRITE_PCM_DATA_FORMAT_PARAM);
            *p++ = PCM_DATA_FORMAT_PARAM_SIZE;
            memcpy(p, &bt_pcm_data_fmt_param, PCM_DATA_FORMAT_PARAM_SIZE);

            if ((ret = bt_vendor_cbacks->xmit_cb(HCI_VSC_WRITE_PCM_DATA_FORMAT_PARAM,\
                                           p_buf, hw_sco_cfg_cback)) == FALSE)
            {
                bt_vendor_cbacks->dealloc(p_buf);
            }
            else
                return;
        }
    }
#endif  // !SCO_USE_I2S_INTERFACE

if (bt_vendor_cbacks)
    bt_vendor_cbacks->scocfg_cb(BT_VND_OP_RESULT_SUCCESS);
#endif
}

uint16_t get_uuid16(tBT_UUID *p_uuid)
{
    if (p_uuid->len == LEN_UUID_16) {
        return p_uuid->uu.uuid16;
    } else if (p_uuid->len == LEN_UUID_128) {
        UINT16 u16;
        UINT8 *p = &p_uuid->uu.uuid128[LEN_UUID_128 - 4];
        STREAM_TO_UINT16(u16, p);
        return u16;
    } else { /* p_uuid->len == LEN_UUID_32 */
        return (UINT16) p_uuid->uu.uuid32;
    }
}