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 "ble_hs_priv.h"
22
23 int
ble_l2cap_sig_tx(uint16_t conn_handle,struct os_mbuf * txom)24 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
30 ble_hs_lock();
31 ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
32 &conn, &chan);
33 rc = ble_l2cap_tx(conn, chan, txom);
34 ble_hs_unlock();
35
36 return rc;
37 }
38
39 void
ble_l2cap_sig_hdr_parse(void * payload,uint16_t len,struct ble_l2cap_sig_hdr * dst)40 ble_l2cap_sig_hdr_parse(void *payload, uint16_t len,
41 struct ble_l2cap_sig_hdr *dst)
42 {
43 struct ble_l2cap_sig_hdr *src = payload;
44
45 BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_HDR_SZ);
46
47 dst->op = src->op;
48 dst->identifier = src->identifier;
49 dst->length = le16toh(src->length);
50 }
51
52 int
ble_l2cap_sig_reject_tx(uint16_t conn_handle,uint8_t id,uint16_t reason,void * data,int data_len)53 ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason,
54 void *data, int data_len)
55 {
56 struct ble_l2cap_sig_reject *cmd;
57 struct os_mbuf *txom;
58
59 cmd = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_REJECT, id,
60 sizeof(*cmd) + data_len, &txom);
61 if (!cmd) {
62 return BLE_HS_ENOMEM;
63 }
64
65 cmd->reason = htole16(reason);
66 memcpy(cmd->data, data, data_len);
67
68 STATS_INC(ble_l2cap_stats, sig_rx);
69 return ble_l2cap_sig_tx(conn_handle, txom);
70 }
71
72 int
ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle,uint8_t id,uint16_t src_cid,uint16_t dst_cid)73 ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id,
74 uint16_t src_cid, uint16_t dst_cid)
75 {
76 struct {
77 uint16_t local_cid;
78 uint16_t remote_cid;
79 } data = {
80 .local_cid = dst_cid,
81 .remote_cid = src_cid,
82 };
83
84 return ble_l2cap_sig_reject_tx(conn_handle, id,
85 BLE_L2CAP_SIG_ERR_INVALID_CID,
86 &data, sizeof data);
87 }
88
89 void *
ble_l2cap_sig_cmd_get(uint8_t opcode,uint8_t id,uint16_t len,struct os_mbuf ** txom)90 ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len,
91 struct os_mbuf **txom)
92 {
93 struct ble_l2cap_sig_hdr *hdr;
94
95 *txom = ble_hs_mbuf_l2cap_pkt();
96 if (*txom == NULL) {
97 return NULL;
98 }
99
100 if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) {
101 os_mbuf_free_chain(*txom);
102 return NULL;
103 }
104
105 hdr = (struct ble_l2cap_sig_hdr *)(*txom)->om_data;
106
107 hdr->op = opcode;
108 hdr->identifier = id;
109 hdr->length = htole16(len);
110
111 return hdr->data;
112 }
113