1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one
3 * or more contributor license agreements. See the NOTICE file
4 * distributed with this work for additional information
5 * regarding copyright ownership. The ASF licenses this file
6 * to you under the Apache License, Version 2.0 (the
7 * "License"); you may not use this file except in compliance
8 * with the License. You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing,
13 * software distributed under the License is distributed on an
14 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
15 * KIND, either express or implied. See the License for the
16 * specific language governing permissions and limitations
17 * under the License.
18 */
19
20 #include <string.h>
21 #include "securec.h"
22 #include "ble_hs_priv.h"
23
ble_l2cap_sig_tx(uint16_t conn_handle,struct os_mbuf * txom)24 int ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom)
25 {
26 struct ble_l2cap_chan *chan;
27 struct ble_hs_conn *conn;
28 int rc;
29 ble_hs_lock();
30 ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
31 &conn, &chan);
32 rc = ble_l2cap_tx(conn, chan, txom);
33 ble_hs_unlock();
34 return rc;
35 }
36
ble_l2cap_sig_hdr_parse(void * payload,uint16_t len,struct ble_l2cap_sig_hdr * dst)37 void ble_l2cap_sig_hdr_parse(void *payload, uint16_t len,
38 struct ble_l2cap_sig_hdr *dst)
39 {
40 struct ble_l2cap_sig_hdr *src = payload;
41 BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_HDR_SZ);
42 dst->op = src->op;
43 dst->identifier = src->identifier;
44 dst->length = le16toh(src->length);
45 }
46
ble_l2cap_sig_reject_tx(uint16_t conn_handle,uint8_t id,uint16_t reason,void * data,int data_len)47 int ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason,
48 void *data, int data_len)
49 {
50 struct ble_l2cap_sig_reject *cmd;
51 struct os_mbuf *txom;
52 cmd = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_REJECT, id,
53 sizeof(*cmd) + data_len, &txom);
54 if (!cmd) {
55 return BLE_HS_ENOMEM;
56 }
57
58 cmd->reason = htole16(reason);
59 memcpy_s(cmd->data, sizeof(cmd->data), data, data_len);
60 STATS_INC(ble_l2cap_stats, sig_rx);
61 return ble_l2cap_sig_tx(conn_handle, txom);
62 }
63
ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle,uint8_t id,uint16_t src_cid,uint16_t dst_cid)64 int ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id,
65 uint16_t src_cid, uint16_t dst_cid)
66 {
67 struct {
68 uint16_t local_cid;
69 uint16_t remote_cid;
70 } data = {
71 .local_cid = dst_cid,
72 .remote_cid = src_cid,
73 };
74 return ble_l2cap_sig_reject_tx(conn_handle, id,
75 BLE_L2CAP_SIG_ERR_INVALID_CID,
76 &data, sizeof data);
77 }
78
ble_l2cap_sig_cmd_get(uint8_t opcode,uint8_t id,uint16_t len,struct os_mbuf ** txom)79 void *ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len,
80 struct os_mbuf **txom)
81 {
82 struct ble_l2cap_sig_hdr *hdr;
83 *txom = ble_hs_mbuf_l2cap_pkt();
84 if (*txom == NULL) {
85 return NULL;
86 }
87
88 if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) {
89 os_mbuf_free_chain(*txom);
90 return NULL;
91 }
92
93 hdr = (struct ble_l2cap_sig_hdr *)(*txom)->om_data;
94 hdr->op = opcode;
95 hdr->identifier = id;
96 hdr->length = htole16(len);
97 return hdr->data;
98 }