1 | /*
|
---|
2 | * Copyright (C) 2009-2012 by Matthias Ringwald
|
---|
3 | *
|
---|
4 | * Redistribution and use in source and binary forms, with or without
|
---|
5 | * modification, are permitted provided that the following conditions
|
---|
6 | * are met:
|
---|
7 | *
|
---|
8 | * 1. Redistributions of source code must retain the above copyright
|
---|
9 | * notice, this list of conditions and the following disclaimer.
|
---|
10 | * 2. Redistributions in binary form must reproduce the above copyright
|
---|
11 | * notice, this list of conditions and the following disclaimer in the
|
---|
12 | * documentation and/or other materials provided with the distribution.
|
---|
13 | * 3. Neither the name of the copyright holders nor the names of
|
---|
14 | * contributors may be used to endorse or promote products derived
|
---|
15 | * from this software without specific prior written permission.
|
---|
16 | * 4. Any redistribution, use, or modification is done solely for
|
---|
17 | * personal benefit and not for any commercial purpose or for
|
---|
18 | * monetary gain.
|
---|
19 | *
|
---|
20 | * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
|
---|
21 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
---|
22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
---|
23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
|
---|
24 | * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
---|
25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
---|
26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
---|
27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
---|
28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
---|
29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
|
---|
30 | * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
---|
31 | * SUCH DAMAGE.
|
---|
32 | *
|
---|
33 | * Please inquire about commercial licensing options at btstack@ringwald.ch
|
---|
34 | *
|
---|
35 | */
|
---|
36 |
|
---|
37 | /*
|
---|
38 | * l2cap_signaling.h
|
---|
39 | *
|
---|
40 | * Created by Matthias Ringwald on 7/23/09.
|
---|
41 | */
|
---|
42 |
|
---|
43 | #include "l2cap_signaling.h"
|
---|
44 | #include "btstack-config.h"
|
---|
45 | #include "hci.h"
|
---|
46 |
|
---|
47 | #include <string.h>
|
---|
48 |
|
---|
49 | static const char *l2cap_signaling_commands_format[] = {
|
---|
50 | "2D", // 0x01 command reject: reason {cmd not understood (0), sig MTU exceeded (2:max sig MTU), invalid CID (4:req CID)}, data len, data
|
---|
51 | "22", // 0x02 connection request: PSM, Source CID
|
---|
52 | "2222", // 0x03 connection response: Dest CID, Source CID, Result, Status
|
---|
53 | "22D", // 0x04 config request: Dest CID, Flags, Configuration options
|
---|
54 | "222D", // 0x05 config response: Source CID, Flags, Result, Configuration options
|
---|
55 | "22", // 0x06 disconection request: Dest CID, Source CID
|
---|
56 | "22", // 0x07 disconection response: Dest CID, Source CID
|
---|
57 | "D", // 0x08 echo request: Data
|
---|
58 | "D", // 0x09 echo response: Data
|
---|
59 | "2", // 0x0a information request: InfoType {1=Connectionless MTU, 2=Extended features supported}
|
---|
60 | "22D", // 0x0b information response: InfoType, Result, Data
|
---|
61 | #ifdef HAVE_BLE
|
---|
62 | // skip 6 not supported signaling pdus, see below
|
---|
63 | "2222", // 0x12 connection parameter update request: interval min, interval max, slave latency, timeout multipler
|
---|
64 | "2", // 0x13 connection parameter update response: result
|
---|
65 | #endif
|
---|
66 | };
|
---|
67 |
|
---|
68 | uint8_t sig_seq_nr = 0xff;
|
---|
69 | uint16_t source_cid = 0x40;
|
---|
70 |
|
---|
71 | uint8_t l2cap_next_sig_id(void){
|
---|
72 | if (sig_seq_nr == 0xff) {
|
---|
73 | sig_seq_nr = 1;
|
---|
74 | } else {
|
---|
75 | sig_seq_nr++;
|
---|
76 | }
|
---|
77 | return sig_seq_nr;
|
---|
78 | }
|
---|
79 |
|
---|
80 | uint16_t l2cap_next_local_cid(void){
|
---|
81 | return source_cid++;
|
---|
82 | }
|
---|
83 |
|
---|
84 | uint16_t l2cap_create_signaling_internal(uint8_t * acl_buffer, hci_con_handle_t handle, uint16_t cid, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr){
|
---|
85 |
|
---|
86 | int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02;
|
---|
87 | uint16_t pos;
|
---|
88 | const char *format;
|
---|
89 | uint16_t word;
|
---|
90 | uint8_t * ptr;
|
---|
91 |
|
---|
92 | // 0 - Connection handle : PB=pb : BC=00
|
---|
93 | little_endian_store_16(acl_buffer, 0, handle | (pb << 12) | (0 << 14));
|
---|
94 | // 6 - L2CAP channel = 1
|
---|
95 | little_endian_store_16(acl_buffer, 6, cid);
|
---|
96 | // 8 - Code
|
---|
97 | acl_buffer[8] = cmd;
|
---|
98 | // 9 - id (!= 0 sequentially)
|
---|
99 | acl_buffer[9] = identifier;
|
---|
100 |
|
---|
101 | // 12 - L2CAP signaling parameters
|
---|
102 | pos = 12;
|
---|
103 | // skip AMP commands
|
---|
104 | if (cmd >= CONNECTION_PARAMETER_UPDATE_REQUEST){
|
---|
105 | cmd -= 6;
|
---|
106 | }
|
---|
107 | format = l2cap_signaling_commands_format[cmd-1];
|
---|
108 | while (*format) {
|
---|
109 | switch(*format) {
|
---|
110 | case '1': // 8 bit value
|
---|
111 | case '2': // 16 bit value
|
---|
112 | word = va_arg(argptr, int);
|
---|
113 | // minimal va_arg is int: 2 bytes on 8+16 bit CPUs
|
---|
114 | acl_buffer[pos++] = word & 0xff;
|
---|
115 | if (*format == '2') {
|
---|
116 | acl_buffer[pos++] = word >> 8;
|
---|
117 | }
|
---|
118 | break;
|
---|
119 | case 'D': // variable data. passed: len, ptr
|
---|
120 | word = va_arg(argptr, int);
|
---|
121 | ptr = va_arg(argptr, uint8_t *);
|
---|
122 | memcpy(&acl_buffer[pos], ptr, word);
|
---|
123 | pos += word;
|
---|
124 | break;
|
---|
125 | default:
|
---|
126 | break;
|
---|
127 | }
|
---|
128 | format++;
|
---|
129 | };
|
---|
130 | va_end(argptr);
|
---|
131 |
|
---|
132 | // Fill in various length fields: it's the number of bytes following for ACL lenght and l2cap parameter length
|
---|
133 | // - the l2cap payload length is counted after the following channel id (only payload)
|
---|
134 |
|
---|
135 | // 2 - ACL length
|
---|
136 | little_endian_store_16(acl_buffer, 2, pos - 4);
|
---|
137 | // 4 - L2CAP packet length
|
---|
138 | little_endian_store_16(acl_buffer, 4, pos - 6 - 2);
|
---|
139 | // 10 - L2CAP signaling parameter length
|
---|
140 | little_endian_store_16(acl_buffer, 10, pos - 12);
|
---|
141 |
|
---|
142 | return pos;
|
---|
143 | }
|
---|
144 |
|
---|
145 | uint16_t l2cap_create_signaling_classic(uint8_t * acl_buffer, hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr){
|
---|
146 | return l2cap_create_signaling_internal(acl_buffer, handle, 1, cmd, identifier, argptr);
|
---|
147 | }
|
---|
148 |
|
---|
149 | #ifdef HAVE_BLE
|
---|
150 |
|
---|
151 | uint16_t l2cap_create_signaling_le(uint8_t * acl_buffer, hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr){
|
---|
152 | return l2cap_create_signaling_internal(acl_buffer, handle, 5, cmd, identifier, argptr);
|
---|
153 | }
|
---|
154 |
|
---|
155 | uint16_t l2cap_le_create_connection_parameter_update_request(uint8_t * acl_buffer, uint16_t handle, uint16_t interval_min, uint16_t interval_max, uint16_t slave_latency, uint16_t timeout_multiplier){
|
---|
156 |
|
---|
157 | int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02;
|
---|
158 |
|
---|
159 | // 0 - Connection handle : PB=pb : BC=00
|
---|
160 | little_endian_store_16(acl_buffer, 0, handle | (pb << 12) | (0 << 14));
|
---|
161 | // 6 - L2CAP LE Signaling channel = 5
|
---|
162 | little_endian_store_16(acl_buffer, 6, 5);
|
---|
163 | // 8 - Code
|
---|
164 | acl_buffer[8] = CONNECTION_PARAMETER_UPDATE_REQUEST;
|
---|
165 | // 9 - id (!= 0 sequentially)
|
---|
166 | acl_buffer[9] = 1;
|
---|
167 | uint16_t pos = 12;
|
---|
168 | little_endian_store_16(acl_buffer, pos, interval_min);
|
---|
169 | pos += 2;
|
---|
170 | little_endian_store_16(acl_buffer, pos, interval_max);
|
---|
171 | pos += 2;
|
---|
172 | little_endian_store_16(acl_buffer, pos, slave_latency);
|
---|
173 | pos += 2;
|
---|
174 | little_endian_store_16(acl_buffer, pos, timeout_multiplier);
|
---|
175 | pos += 2;
|
---|
176 | // 2 - ACL length
|
---|
177 | little_endian_store_16(acl_buffer, 2, pos - 4);
|
---|
178 | // 4 - L2CAP packet length
|
---|
179 | little_endian_store_16(acl_buffer, 4, pos - 6 - 2);
|
---|
180 | // 10 - L2CAP signaling parameter length
|
---|
181 | little_endian_store_16(acl_buffer, 10, pos - 12);
|
---|
182 | return pos;
|
---|
183 | }
|
---|
184 |
|
---|
185 | uint16_t l2cap_le_create_connection_parameter_update_response(uint8_t * acl_buffer, uint16_t handle, uint16_t response){
|
---|
186 |
|
---|
187 | int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02;
|
---|
188 |
|
---|
189 | // 0 - Connection handle : PB=pb : BC=00
|
---|
190 | little_endian_store_16(acl_buffer, 0, handle | (pb << 12) | (0 << 14));
|
---|
191 | // 6 - L2CAP LE Signaling channel = 5
|
---|
192 | little_endian_store_16(acl_buffer, 6, 5);
|
---|
193 | // 8 - Code
|
---|
194 | acl_buffer[8] = CONNECTION_PARAMETER_UPDATE_REQUEST;
|
---|
195 | // 9 - id (!= 0 sequentially)
|
---|
196 | acl_buffer[9] = 1;
|
---|
197 | uint16_t pos = 12;
|
---|
198 | little_endian_store_16(acl_buffer, pos, response);
|
---|
199 | pos += 2;
|
---|
200 | // 2 - ACL length
|
---|
201 | little_endian_store_16(acl_buffer, 2, pos - 4);
|
---|
202 | // 4 - L2CAP packet length
|
---|
203 | little_endian_store_16(acl_buffer, 4, pos - 6 - 2);
|
---|
204 | // 10 - L2CAP signaling parameter length
|
---|
205 | little_endian_store_16(acl_buffer, 10, pos - 12);
|
---|
206 | return pos;
|
---|
207 | }
|
---|
208 |
|
---|
209 |
|
---|
210 | #endif
|
---|