| /****************************************************************************** |
| * |
| * Copyright (C) 2003-2012 Broadcom Corporation |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at: |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| * |
| ******************************************************************************/ |
| |
| /****************************************************************************** |
| * |
| * This module contains action functions of the link control state machine. |
| * |
| ******************************************************************************/ |
| |
| #include <string.h> |
| #include "bt_types.h" |
| #include "bt_target.h" |
| #include "bt_utils.h" |
| #include "avct_api.h" |
| #include "avct_int.h" |
| #include "bt_common.h" |
| #include "btm_api.h" |
| #include "osi/include/osi.h" |
| |
| /* packet header length lookup table */ |
| const uint8_t avct_lcb_pkt_type_len[] = { |
| AVCT_HDR_LEN_SINGLE, |
| AVCT_HDR_LEN_START, |
| AVCT_HDR_LEN_CONT, |
| AVCT_HDR_LEN_END |
| }; |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_msg_asmbl |
| * |
| * Description Reassemble incoming message. |
| * |
| * |
| * Returns Pointer to reassembled message; NULL if no message |
| * available. |
| * |
| ******************************************************************************/ |
| static BT_HDR *avct_lcb_msg_asmbl(tAVCT_LCB *p_lcb, BT_HDR *p_buf) |
| { |
| uint8_t *p; |
| uint8_t pkt_type; |
| BT_HDR *p_ret; |
| |
| /* parse the message header */ |
| p = (uint8_t *)(p_buf + 1) + p_buf->offset; |
| pkt_type = AVCT_PKT_TYPE(p); |
| |
| /* quick sanity check on length */ |
| if (p_buf->len < avct_lcb_pkt_type_len[pkt_type]) |
| { |
| osi_free(p_buf); |
| AVCT_TRACE_WARNING("Bad length during reassembly"); |
| p_ret = NULL; |
| } |
| /* single packet */ |
| else if (pkt_type == AVCT_PKT_TYPE_SINGLE) |
| { |
| /* if reassembly in progress drop message and process new single */ |
| if (p_lcb->p_rx_msg != NULL) |
| AVCT_TRACE_WARNING("Got single during reassembly"); |
| |
| osi_free_and_reset((void **)&p_lcb->p_rx_msg); |
| |
| p_ret = p_buf; |
| } |
| /* start packet */ |
| else if (pkt_type == AVCT_PKT_TYPE_START) |
| { |
| /* if reassembly in progress drop message and process new start */ |
| if (p_lcb->p_rx_msg != NULL) |
| AVCT_TRACE_WARNING("Got start during reassembly"); |
| |
| osi_free(p_lcb->p_rx_msg); |
| |
| /* |
| * Allocate bigger buffer for reassembly. As lower layers are |
| * not aware of possible packet size after reassembly, they |
| * would have allocated smaller buffer. |
| */ |
| p_lcb->p_rx_msg = (BT_HDR *)osi_malloc(BT_DEFAULT_BUFFER_SIZE); |
| memcpy(p_lcb->p_rx_msg, p_buf, |
| sizeof(BT_HDR) + p_buf->offset + p_buf->len); |
| |
| /* Free original buffer */ |
| osi_free(p_buf); |
| |
| /* update p to point to new buffer */ |
| p = (uint8_t *)(p_lcb->p_rx_msg + 1) + p_lcb->p_rx_msg->offset; |
| |
| /* copy first header byte over nosp */ |
| *(p + 1) = *p; |
| |
| /* set offset to point to where to copy next */ |
| p_lcb->p_rx_msg->offset += p_lcb->p_rx_msg->len; |
| |
| /* adjust length for packet header */ |
| p_lcb->p_rx_msg->len -= 1; |
| |
| p_ret = NULL; |
| } |
| /* continue or end */ |
| else |
| { |
| /* if no reassembly in progress drop message */ |
| if (p_lcb->p_rx_msg == NULL) |
| { |
| osi_free(p_buf); |
| AVCT_TRACE_WARNING("Pkt type=%d out of order", pkt_type); |
| p_ret = NULL; |
| } |
| else |
| { |
| /* get size of buffer holding assembled message */ |
| /* |
| * NOTE: The buffer is allocated above at the beginning of the |
| * reassembly, and is always of size BT_DEFAULT_BUFFER_SIZE. |
| */ |
| uint16_t buf_len = BT_DEFAULT_BUFFER_SIZE - sizeof(BT_HDR); |
| |
| /* adjust offset and len of fragment for header byte */ |
| p_buf->offset += AVCT_HDR_LEN_CONT; |
| p_buf->len -= AVCT_HDR_LEN_CONT; |
| |
| /* verify length */ |
| if ((p_lcb->p_rx_msg->offset + p_buf->len) > buf_len) { |
| /* won't fit; free everything */ |
| AVCT_TRACE_WARNING("%s: Fragmented message too big!", __func__); |
| osi_free_and_reset((void **)&p_lcb->p_rx_msg); |
| osi_free(p_buf); |
| p_ret = NULL; |
| } else { |
| /* copy contents of p_buf to p_rx_msg */ |
| memcpy((uint8_t *)(p_lcb->p_rx_msg + 1) + p_lcb->p_rx_msg->offset, |
| (uint8_t *)(p_buf + 1) + p_buf->offset, p_buf->len); |
| |
| if (pkt_type == AVCT_PKT_TYPE_END) |
| { |
| p_lcb->p_rx_msg->offset -= p_lcb->p_rx_msg->len; |
| p_lcb->p_rx_msg->len += p_buf->len; |
| p_ret = p_lcb->p_rx_msg; |
| p_lcb->p_rx_msg = NULL; |
| } |
| else |
| { |
| p_lcb->p_rx_msg->offset += p_buf->len; |
| p_lcb->p_rx_msg->len += p_buf->len; |
| p_ret = NULL; |
| } |
| osi_free(p_buf); |
| } |
| } |
| } |
| return p_ret; |
| } |
| |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_chnl_open |
| * |
| * Description Open L2CAP channel to peer |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_chnl_open(tAVCT_LCB *p_lcb, |
| UNUSED_ATTR tAVCT_LCB_EVT *p_data) |
| { |
| uint16_t result = AVCT_RESULT_FAIL; |
| |
| BTM_SetOutService(p_lcb->peer_addr, BTM_SEC_SERVICE_AVCTP, 0); |
| /* call l2cap connect req */ |
| p_lcb->ch_state = AVCT_CH_CONN; |
| p_lcb->ch_lcid = L2CA_ConnectReq(AVCT_PSM, p_lcb->peer_addr); |
| if (p_lcb->ch_lcid == 0) |
| { |
| /* if connect req failed, send ourselves close event */ |
| avct_lcb_event(p_lcb, AVCT_LCB_LL_CLOSE_EVT, (tAVCT_LCB_EVT *) &result); |
| } |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_unbind_disc |
| * |
| * Description Deallocate ccb and call callback with disconnect event. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_unbind_disc(UNUSED_ATTR tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| avct_ccb_dealloc(p_data->p_ccb, AVCT_DISCONNECT_CFM_EVT, 0, NULL); |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_open_ind |
| * |
| * Description Handle an LL_OPEN event. For each allocated ccb already |
| * bound to this lcb, send a connect event. For each |
| * unbound ccb with a new PID, bind that ccb to this lcb and |
| * send a connect event. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_open_ind(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| tAVCT_CCB *p_ccb = &avct_cb.ccb[0]; |
| int i; |
| bool bind = false; |
| |
| for (i = 0; i < AVCT_NUM_CONN; i++, p_ccb++) |
| { |
| /* if ccb allocated and */ |
| if (p_ccb->allocated) |
| { |
| /* if bound to this lcb send connect confirm event */ |
| if (p_ccb->p_lcb == p_lcb) |
| { |
| bind = true; |
| L2CA_SetTxPriority(p_lcb->ch_lcid, L2CAP_CHNL_PRIORITY_HIGH); |
| p_ccb->cc.p_ctrl_cback(avct_ccb_to_idx(p_ccb), AVCT_CONNECT_CFM_EVT, |
| 0, p_lcb->peer_addr); |
| } |
| /* if unbound acceptor and lcb doesn't already have a ccb for this PID */ |
| else if ((p_ccb->p_lcb == NULL) && (p_ccb->cc.role == AVCT_ACP) && |
| (avct_lcb_has_pid(p_lcb, p_ccb->cc.pid) == NULL)) |
| { |
| /* bind ccb to lcb and send connect ind event */ |
| bind = true; |
| p_ccb->p_lcb = p_lcb; |
| L2CA_SetTxPriority(p_lcb->ch_lcid, L2CAP_CHNL_PRIORITY_HIGH); |
| p_ccb->cc.p_ctrl_cback(avct_ccb_to_idx(p_ccb), AVCT_CONNECT_IND_EVT, |
| 0, p_lcb->peer_addr); |
| } |
| } |
| } |
| |
| /* if no ccbs bound to this lcb, disconnect */ |
| if (bind == false) |
| { |
| avct_lcb_event(p_lcb, AVCT_LCB_INT_CLOSE_EVT, p_data); |
| } |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_open_fail |
| * |
| * Description L2CAP channel open attempt failed. Deallocate any ccbs |
| * on this lcb and send connect confirm event with failure. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_open_fail(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| tAVCT_CCB *p_ccb = &avct_cb.ccb[0]; |
| int i; |
| |
| for (i = 0; i < AVCT_NUM_CONN; i++, p_ccb++) |
| { |
| if (p_ccb->allocated && (p_ccb->p_lcb == p_lcb)) |
| { |
| avct_ccb_dealloc(p_ccb, AVCT_CONNECT_CFM_EVT, |
| p_data->result, p_lcb->peer_addr); |
| } |
| } |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_close_ind |
| * |
| * Description L2CAP channel closed by peer. Deallocate any initiator |
| * ccbs on this lcb and send disconnect ind event. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_close_ind(tAVCT_LCB *p_lcb, |
| UNUSED_ATTR tAVCT_LCB_EVT *p_data) |
| { |
| tAVCT_CCB *p_ccb = &avct_cb.ccb[0]; |
| int i; |
| |
| for (i = 0; i < AVCT_NUM_CONN; i++, p_ccb++) |
| { |
| if (p_ccb->allocated && (p_ccb->p_lcb == p_lcb)) |
| { |
| if (p_ccb->cc.role == AVCT_INT) |
| { |
| avct_ccb_dealloc(p_ccb, AVCT_DISCONNECT_IND_EVT, |
| 0, p_lcb->peer_addr); |
| } |
| else |
| { |
| p_ccb->p_lcb = NULL; |
| (*p_ccb->cc.p_ctrl_cback)(avct_ccb_to_idx(p_ccb), AVCT_DISCONNECT_IND_EVT, |
| 0, p_lcb->peer_addr); |
| } |
| } |
| } |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_close_cfm |
| * |
| * Description L2CAP channel closed by us. Deallocate any initiator |
| * ccbs on this lcb and send disconnect ind or cfm event. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_close_cfm(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| tAVCT_CCB *p_ccb = &avct_cb.ccb[0]; |
| int i; |
| uint8_t event; |
| |
| for (i = 0; i < AVCT_NUM_CONN; i++, p_ccb++) |
| { |
| if (p_ccb->allocated && (p_ccb->p_lcb == p_lcb)) |
| { |
| /* if this ccb initiated close send disconnect cfm otherwise ind */ |
| if (p_ccb->ch_close) |
| { |
| p_ccb->ch_close = false; |
| event = AVCT_DISCONNECT_CFM_EVT; |
| } |
| else |
| { |
| event = AVCT_DISCONNECT_IND_EVT; |
| } |
| |
| if (p_ccb->cc.role == AVCT_INT) |
| { |
| avct_ccb_dealloc(p_ccb, event, p_data->result, p_lcb->peer_addr); |
| } |
| else |
| { |
| p_ccb->p_lcb = NULL; |
| (*p_ccb->cc.p_ctrl_cback)(avct_ccb_to_idx(p_ccb), event, |
| p_data->result, p_lcb->peer_addr); |
| } |
| } |
| } |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_bind_conn |
| * |
| * Description Bind ccb to lcb and send connect cfm event. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_bind_conn(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| p_data->p_ccb->p_lcb = p_lcb; |
| (*p_data->p_ccb->cc.p_ctrl_cback)(avct_ccb_to_idx(p_data->p_ccb), |
| AVCT_CONNECT_CFM_EVT, 0, p_lcb->peer_addr); |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_chk_disc |
| * |
| * Description A ccb wants to close; if it is the last ccb on this lcb, |
| * close channel. Otherwise just deallocate and call |
| * callback. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_chk_disc(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| AVCT_TRACE_WARNING("%s", __func__); |
| |
| avct_close_bcb(p_lcb, p_data); |
| if (avct_lcb_last_ccb(p_lcb, p_data->p_ccb)) |
| { |
| AVCT_TRACE_WARNING("%s: closing", __func__); |
| p_data->p_ccb->ch_close = true; |
| avct_lcb_event(p_lcb, AVCT_LCB_INT_CLOSE_EVT, p_data); |
| } |
| else |
| { |
| AVCT_TRACE_WARNING("%s: dealloc ccb", __func__); |
| avct_lcb_unbind_disc(p_lcb, p_data); |
| } |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_chnl_disc |
| * |
| * Description Disconnect L2CAP channel. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_chnl_disc(tAVCT_LCB *p_lcb, |
| UNUSED_ATTR tAVCT_LCB_EVT *p_data) |
| { |
| L2CA_DisconnectReq(p_lcb->ch_lcid); |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_bind_fail |
| * |
| * Description Deallocate ccb and call callback with connect event |
| * with failure result. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_bind_fail(UNUSED_ATTR tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| avct_ccb_dealloc(p_data->p_ccb, AVCT_CONNECT_CFM_EVT, AVCT_RESULT_FAIL, NULL); |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_cong_ind |
| * |
| * Description Handle congestion indication from L2CAP. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_cong_ind(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| tAVCT_CCB *p_ccb = &avct_cb.ccb[0]; |
| int i; |
| uint8_t event; |
| BT_HDR *p_buf; |
| |
| /* set event */ |
| event = (p_data->cong) ? AVCT_CONG_IND_EVT : AVCT_UNCONG_IND_EVT; |
| p_lcb->cong = p_data->cong; |
| if (p_lcb->cong == false && !fixed_queue_is_empty(p_lcb->tx_q)) |
| { |
| while (!p_lcb->cong && |
| (p_buf = (BT_HDR *)fixed_queue_try_dequeue(p_lcb->tx_q)) != NULL) |
| { |
| if (L2CA_DataWrite(p_lcb->ch_lcid, p_buf) == L2CAP_DW_CONGESTED) |
| { |
| p_lcb->cong = true; |
| } |
| } |
| } |
| |
| /* send event to all ccbs on this lcb */ |
| for (i = 0; i < AVCT_NUM_CONN; i++, p_ccb++) |
| { |
| if (p_ccb->allocated && (p_ccb->p_lcb == p_lcb)) |
| { |
| (*p_ccb->cc.p_ctrl_cback)(avct_ccb_to_idx(p_ccb), event, 0, p_lcb->peer_addr); |
| } |
| } |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_discard_msg |
| * |
| * Description Discard a message sent in from the API. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_discard_msg(UNUSED_ATTR tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| AVCT_TRACE_WARNING("%s Dropping message", __func__); |
| osi_free_and_reset((void **)&p_data->ul_msg.p_buf); |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_send_msg |
| * |
| * Description Build and send an AVCTP message. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_send_msg(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| uint16_t curr_msg_len; |
| uint8_t pkt_type; |
| uint8_t hdr_len; |
| uint8_t *p; |
| uint8_t nosp = 0; /* number of subsequent packets */ |
| uint16_t temp; |
| uint16_t buf_size = p_lcb->peer_mtu + L2CAP_MIN_OFFSET + BT_HDR_SIZE; |
| |
| |
| /* store msg len */ |
| curr_msg_len = p_data->ul_msg.p_buf->len; |
| |
| /* initialize packet type and other stuff */ |
| if (curr_msg_len <= (p_lcb->peer_mtu - AVCT_HDR_LEN_SINGLE)) |
| { |
| pkt_type = AVCT_PKT_TYPE_SINGLE; |
| } |
| else |
| { |
| pkt_type = AVCT_PKT_TYPE_START; |
| temp = (curr_msg_len + AVCT_HDR_LEN_START - p_lcb->peer_mtu); |
| nosp = temp / (p_lcb->peer_mtu - 1) + 1; |
| if ( (temp % (p_lcb->peer_mtu - 1)) != 0) |
| nosp++; |
| } |
| |
| /* while we haven't sent all packets */ |
| while (curr_msg_len != 0) { |
| BT_HDR *p_buf; |
| |
| /* set header len */ |
| hdr_len = avct_lcb_pkt_type_len[pkt_type]; |
| |
| /* if remaining msg must be fragmented */ |
| if (p_data->ul_msg.p_buf->len > (p_lcb->peer_mtu - hdr_len)) |
| { |
| /* get a new buffer for fragment we are sending */ |
| p_buf = (BT_HDR *)osi_malloc(buf_size); |
| |
| /* copy portion of data from current message to new buffer */ |
| p_buf->offset = L2CAP_MIN_OFFSET + hdr_len; |
| p_buf->len = p_lcb->peer_mtu - hdr_len; |
| |
| memcpy((uint8_t *)(p_buf + 1) + p_buf->offset, |
| (uint8_t *)(p_data->ul_msg.p_buf + 1) + p_data->ul_msg.p_buf->offset, p_buf->len); |
| |
| p_data->ul_msg.p_buf->offset += p_buf->len; |
| p_data->ul_msg.p_buf->len -= p_buf->len; |
| } |
| else |
| { |
| p_buf = p_data->ul_msg.p_buf; |
| } |
| |
| curr_msg_len -= p_buf->len; |
| |
| /* set up to build header */ |
| p_buf->len += hdr_len; |
| p_buf->offset -= hdr_len; |
| p = (uint8_t *)(p_buf + 1) + p_buf->offset; |
| |
| /* build header */ |
| AVCT_BUILD_HDR(p, p_data->ul_msg.label, pkt_type, p_data->ul_msg.cr); |
| if (pkt_type == AVCT_PKT_TYPE_START) |
| { |
| UINT8_TO_STREAM(p, nosp); |
| } |
| if ((pkt_type == AVCT_PKT_TYPE_START) || (pkt_type == AVCT_PKT_TYPE_SINGLE)) |
| { |
| UINT16_TO_BE_STREAM(p, p_data->ul_msg.p_ccb->cc.pid); |
| } |
| |
| if (p_lcb->cong == true) |
| { |
| fixed_queue_enqueue(p_lcb->tx_q, p_buf); |
| } |
| |
| /* send message to L2CAP */ |
| else |
| { |
| if (L2CA_DataWrite(p_lcb->ch_lcid, p_buf) == L2CAP_DW_CONGESTED) |
| { |
| p_lcb->cong = true; |
| } |
| } |
| |
| /* update pkt type for next packet */ |
| if (curr_msg_len > (p_lcb->peer_mtu - AVCT_HDR_LEN_END)) |
| { |
| pkt_type = AVCT_PKT_TYPE_CONT; |
| } |
| else |
| { |
| pkt_type = AVCT_PKT_TYPE_END; |
| } |
| } |
| AVCT_TRACE_DEBUG ("%s tx_q_count:%d", __func__, |
| fixed_queue_length(p_lcb->tx_q)); |
| return; |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_free_msg_ind |
| * |
| * Description Discard an incoming AVCTP message. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_free_msg_ind(UNUSED_ATTR tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| if (p_data == NULL) |
| return; |
| |
| osi_free_and_reset((void **)&p_data->p_buf); |
| } |
| |
| /******************************************************************************* |
| * |
| * Function avct_lcb_msg_ind |
| * |
| * Description Handle an incoming AVCTP message. |
| * |
| * |
| * Returns Nothing. |
| * |
| ******************************************************************************/ |
| void avct_lcb_msg_ind(tAVCT_LCB *p_lcb, tAVCT_LCB_EVT *p_data) |
| { |
| uint8_t *p; |
| uint8_t label, type, cr_ipid; |
| uint16_t pid; |
| tAVCT_CCB *p_ccb; |
| |
| /* this p_buf is to be reported through p_msg_cback. The layer_specific |
| * needs to be set properly to indicate that it is received through |
| * control channel */ |
| p_data->p_buf->layer_specific = AVCT_DATA_CTRL; |
| |
| /* reassemble message; if no message available (we received a fragment) return */ |
| p_data->p_buf = avct_lcb_msg_asmbl(p_lcb, p_data->p_buf); |
| if (p_data->p_buf == NULL) |
| { |
| return; |
| } |
| |
| p = (uint8_t *)(p_data->p_buf + 1) + p_data->p_buf->offset; |
| |
| /* parse header byte */ |
| AVCT_PARSE_HDR(p, label, type, cr_ipid); |
| |
| /* check for invalid cr_ipid */ |
| if (cr_ipid == AVCT_CR_IPID_INVALID) |
| { |
| AVCT_TRACE_WARNING("Invalid cr_ipid", cr_ipid); |
| osi_free_and_reset((void **)&p_data->p_buf); |
| return; |
| } |
| |
| /* parse and lookup PID */ |
| BE_STREAM_TO_UINT16(pid, p); |
| p_ccb = avct_lcb_has_pid(p_lcb, pid); |
| if (p_ccb) |
| { |
| /* PID found; send msg up, adjust bt hdr and call msg callback */ |
| p_data->p_buf->offset += AVCT_HDR_LEN_SINGLE; |
| p_data->p_buf->len -= AVCT_HDR_LEN_SINGLE; |
| (*p_ccb->cc.p_msg_cback)(avct_ccb_to_idx(p_ccb), label, cr_ipid, p_data->p_buf); |
| return; |
| } |
| |
| /* PID not found; drop message */ |
| AVCT_TRACE_WARNING("No ccb for PID=%x", pid); |
| osi_free_and_reset((void **)&p_data->p_buf); |
| |
| /* if command send reject */ |
| if (cr_ipid == AVCT_CMD) |
| { |
| BT_HDR *p_buf = (BT_HDR *)osi_malloc(AVCT_CMD_BUF_SIZE); |
| p_buf->len = AVCT_HDR_LEN_SINGLE; |
| p_buf->offset = AVCT_MSG_OFFSET - AVCT_HDR_LEN_SINGLE; |
| p = (uint8_t *)(p_buf + 1) + p_buf->offset; |
| AVCT_BUILD_HDR(p, label, AVCT_PKT_TYPE_SINGLE, AVCT_REJ); |
| UINT16_TO_BE_STREAM(p, pid); |
| L2CA_DataWrite(p_lcb->ch_lcid, p_buf); |
| } |
| } |