1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
|
/*
* Licensed to the Apache Software Foundation (ASF) under one
* or more contributor license agreements. See the NOTICE file
* distributed with this work for additional information
* regarding copyright ownership. The ASF licenses this file
* to you 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.
*/
#include <string.h>
#include "ble_hs_priv.h"
int
ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom)
{
struct ble_l2cap_chan *chan;
struct ble_hs_conn *conn;
int rc;
ble_hs_lock();
rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
&conn, &chan);
if (rc == 0) {
rc = ble_l2cap_tx(conn, chan, txom);
}
ble_hs_unlock();
return rc;
}
void
ble_l2cap_sig_hdr_parse(void *payload, uint16_t len,
struct ble_l2cap_sig_hdr *dst)
{
struct ble_l2cap_sig_hdr *src = payload;
BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_HDR_SZ);
dst->op = src->op;
dst->identifier = src->identifier;
dst->length = le16toh(src->length);
}
int
ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason,
void *data, int data_len)
{
struct ble_l2cap_sig_reject *cmd;
struct os_mbuf *txom;
cmd = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_REJECT, id,
sizeof(*cmd) + data_len, &txom);
if (!cmd) {
return BLE_HS_ENOMEM;
}
cmd->reason = htole16(reason);
memcpy(cmd->data, data, data_len);
STATS_INC(ble_l2cap_stats, sig_rx);
return ble_l2cap_sig_tx(conn_handle, txom);
}
int
ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id,
uint16_t src_cid, uint16_t dst_cid)
{
struct {
uint16_t local_cid;
uint16_t remote_cid;
} data = {
.local_cid = dst_cid,
.remote_cid = src_cid,
};
return ble_l2cap_sig_reject_tx(conn_handle, id,
BLE_L2CAP_SIG_ERR_INVALID_CID,
&data, sizeof data);
}
void *
ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len,
struct os_mbuf **txom)
{
struct ble_l2cap_sig_hdr *hdr;
*txom = ble_hs_mbuf_l2cap_pkt();
if (*txom == NULL) {
return NULL;
}
if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) {
os_mbuf_free_chain(*txom);
return NULL;
}
hdr = (struct ble_l2cap_sig_hdr *)(*txom)->om_data;
hdr->op = opcode;
hdr->identifier = id;
hdr->length = htole16(len);
return hdr->data;
}
|
