1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2010 Nokia Corporation
6 * Copyright (C) 2010 Marcel Holtmann <marcel@holtmann.org>
7 *
8 *
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
22 *
23 */
24
25 #include <errno.h>
26 #include <stdint.h>
27 #include <stdlib.h>
28
29 #include <bluetooth/bluetooth.h>
30 #include <bluetooth/uuid.h>
31
32 #include <glib.h>
33
34 #include "att.h"
35
att_ecode2str(uint8_t status)36 const char *att_ecode2str(uint8_t status)
37 {
38 switch (status) {
39 case ATT_ECODE_INVALID_HANDLE:
40 return "Invalid handle";
41 case ATT_ECODE_READ_NOT_PERM:
42 return "Atribute can't be read";
43 case ATT_ECODE_WRITE_NOT_PERM:
44 return "Attribute can't be written";
45 case ATT_ECODE_INVALID_PDU:
46 return "Attribute PDU was invalid";
47 case ATT_ECODE_AUTHENTICATION:
48 return "Attribute requires authentication before read/write";
49 case ATT_ECODE_REQ_NOT_SUPP:
50 return "Server doesn't support the request received";
51 case ATT_ECODE_INVALID_OFFSET:
52 return "Offset past the end of the attribute";
53 case ATT_ECODE_AUTHORIZATION:
54 return "Attribute requires authorization before read/write";
55 case ATT_ECODE_PREP_QUEUE_FULL:
56 return "Too many prepare writes have been queued";
57 case ATT_ECODE_ATTR_NOT_FOUND:
58 return "No attribute found within the given range";
59 case ATT_ECODE_ATTR_NOT_LONG:
60 return "Attribute can't be read/written using Read Blob Req";
61 case ATT_ECODE_INSUFF_ENCR_KEY_SIZE:
62 return "Encryption Key Size is insufficient";
63 case ATT_ECODE_INVAL_ATTR_VALUE_LEN:
64 return "Attribute value length is invalid";
65 case ATT_ECODE_UNLIKELY:
66 return "Request attribute has encountered an unlikely error";
67 case ATT_ECODE_INSUFF_ENC:
68 return "Encryption required before read/write";
69 case ATT_ECODE_UNSUPP_GRP_TYPE:
70 return "Attribute type is not a supported grouping attribute";
71 case ATT_ECODE_INSUFF_RESOURCES:
72 return "Insufficient Resources to complete the request";
73 case ATT_ECODE_IO:
74 return "Internal application error: I/O";
75 default:
76 return "Unexpected error code";
77 }
78 }
79
att_data_list_free(struct att_data_list * list)80 void att_data_list_free(struct att_data_list *list)
81 {
82 if (list == NULL)
83 return;
84
85 if (list->data) {
86 int i;
87 for (i = 0; i < list->num; i++)
88 g_free(list->data[i]);
89 }
90
91 g_free(list->data);
92 g_free(list);
93 }
94
att_data_list_alloc(uint16_t num,uint16_t len)95 struct att_data_list *att_data_list_alloc(uint16_t num, uint16_t len)
96 {
97 struct att_data_list *list;
98 int i;
99
100 list = g_new0(struct att_data_list, 1);
101 list->len = len;
102 list->num = num;
103
104 list->data = g_malloc0(sizeof(uint8_t *) * num);
105
106 for (i = 0; i < num; i++)
107 list->data[i] = g_malloc0(sizeof(uint8_t) * len);
108
109 return list;
110 }
111
enc_read_by_grp_req(uint16_t start,uint16_t end,bt_uuid_t * uuid,uint8_t * pdu,int len)112 uint16_t enc_read_by_grp_req(uint16_t start, uint16_t end, bt_uuid_t *uuid,
113 uint8_t *pdu, int len)
114 {
115 const uint16_t min_len = sizeof(pdu[0]) + sizeof(start) + sizeof(end);
116 uint16_t length;
117
118 if (!uuid)
119 return 0;
120
121 if (uuid->type == BT_UUID16)
122 length = 2;
123 else if (uuid->type == BT_UUID128)
124 length = 16;
125 else
126 return 0;
127
128 if (len < min_len + length)
129 return 0;
130
131 pdu[0] = ATT_OP_READ_BY_GROUP_REQ;
132 att_put_u16(start, &pdu[1]);
133 att_put_u16(end, &pdu[3]);
134
135 att_put_uuid(*uuid, &pdu[5]);
136
137 return min_len + length;
138 }
139
dec_read_by_grp_req(const uint8_t * pdu,int len,uint16_t * start,uint16_t * end,bt_uuid_t * uuid)140 uint16_t dec_read_by_grp_req(const uint8_t *pdu, int len, uint16_t *start,
141 uint16_t *end, bt_uuid_t *uuid)
142 {
143 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*start) + sizeof(*end);
144
145 if (pdu == NULL)
146 return 0;
147
148 if (start == NULL || end == NULL || uuid == NULL)
149 return 0;
150
151 if (pdu[0] != ATT_OP_READ_BY_GROUP_REQ)
152 return 0;
153
154 if (len < min_len + 2)
155 return 0;
156
157 *start = att_get_u16(&pdu[1]);
158 *end = att_get_u16(&pdu[3]);
159 if (len == min_len + 2)
160 *uuid = att_get_uuid16(&pdu[5]);
161 else
162 *uuid = att_get_uuid128(&pdu[5]);
163
164 return len;
165 }
166
enc_read_by_grp_resp(struct att_data_list * list,uint8_t * pdu,int len)167 uint16_t enc_read_by_grp_resp(struct att_data_list *list, uint8_t *pdu,
168 int len)
169 {
170 int i;
171 uint16_t w;
172 uint8_t *ptr;
173
174 if (list == NULL)
175 return 0;
176
177 if (len < list->len + 2)
178 return 0;
179
180 pdu[0] = ATT_OP_READ_BY_GROUP_RESP;
181 pdu[1] = list->len;
182
183 ptr = &pdu[2];
184
185 for (i = 0, w = 2; i < list->num && w + list->len <= len; i++) {
186 memcpy(ptr, list->data[i], list->len);
187 ptr += list->len;
188 w += list->len;
189 }
190
191 return w;
192 }
193
dec_read_by_grp_resp(const uint8_t * pdu,int len)194 struct att_data_list *dec_read_by_grp_resp(const uint8_t *pdu, int len)
195 {
196 struct att_data_list *list;
197 const uint8_t *ptr;
198 uint16_t elen, num;
199 int i;
200
201 if (pdu[0] != ATT_OP_READ_BY_GROUP_RESP)
202 return NULL;
203
204 elen = pdu[1];
205 num = (len - 2) / elen;
206 list = att_data_list_alloc(num, elen);
207
208 ptr = &pdu[2];
209
210 for (i = 0; i < num; i++) {
211 memcpy(list->data[i], ptr, list->len);
212 ptr += list->len;
213 }
214
215 return list;
216 }
217
enc_find_by_type_req(uint16_t start,uint16_t end,bt_uuid_t * uuid,const uint8_t * value,int vlen,uint8_t * pdu,int len)218 uint16_t enc_find_by_type_req(uint16_t start, uint16_t end, bt_uuid_t *uuid,
219 const uint8_t *value, int vlen, uint8_t *pdu, int len)
220 {
221 uint16_t min_len = sizeof(pdu[0]) + sizeof(start) + sizeof(end) +
222 sizeof(uint16_t);
223
224 if (pdu == NULL)
225 return 0;
226
227 if (!uuid)
228 return 0;
229
230 if (uuid->type != BT_UUID16)
231 return 0;
232
233 if (len < min_len)
234 return 0;
235
236 if (vlen > len - min_len)
237 vlen = len - min_len;
238
239 pdu[0] = ATT_OP_FIND_BY_TYPE_REQ;
240 att_put_u16(start, &pdu[1]);
241 att_put_u16(end, &pdu[3]);
242 att_put_uuid16(*uuid, &pdu[5]);
243
244 if (vlen > 0) {
245 memcpy(&pdu[7], value, vlen);
246 return min_len + vlen;
247 }
248
249 return min_len;
250 }
251
dec_find_by_type_req(const uint8_t * pdu,int len,uint16_t * start,uint16_t * end,bt_uuid_t * uuid,uint8_t * value,int * vlen)252 uint16_t dec_find_by_type_req(const uint8_t *pdu, int len, uint16_t *start,
253 uint16_t *end, bt_uuid_t *uuid, uint8_t *value, int *vlen)
254 {
255 int valuelen;
256 uint16_t min_len = sizeof(pdu[0]) + sizeof(*start) +
257 sizeof(*end) + sizeof(uint16_t);
258
259 if (pdu == NULL)
260 return 0;
261
262 if (len < min_len)
263 return 0;
264
265 if (pdu[0] != ATT_OP_FIND_BY_TYPE_REQ)
266 return 0;
267
268 /* First requested handle number */
269 if (start)
270 *start = att_get_u16(&pdu[1]);
271
272 /* Last requested handle number */
273 if (end)
274 *end = att_get_u16(&pdu[3]);
275
276 /* Always UUID16 */
277 if (uuid)
278 *uuid = att_get_uuid16(&pdu[5]);
279
280 valuelen = len - min_len;
281
282 /* Attribute value to find */
283 if (valuelen > 0 && value)
284 memcpy(value, pdu + min_len, valuelen);
285
286 if (vlen)
287 *vlen = valuelen;
288
289 return len;
290 }
291
enc_find_by_type_resp(GSList * matches,uint8_t * pdu,int len)292 uint16_t enc_find_by_type_resp(GSList *matches, uint8_t *pdu, int len)
293 {
294 GSList *l;
295 uint16_t offset;
296
297 if (pdu == NULL || len < 5)
298 return 0;
299
300 pdu[0] = ATT_OP_FIND_BY_TYPE_RESP;
301
302 for (l = matches, offset = 1; l && len >= (offset + 4);
303 l = l->next, offset += 4) {
304 struct att_range *range = l->data;
305
306 att_put_u16(range->start, &pdu[offset]);
307 att_put_u16(range->end, &pdu[offset + 2]);
308 }
309
310 return offset;
311 }
312
dec_find_by_type_resp(const uint8_t * pdu,int len)313 GSList *dec_find_by_type_resp(const uint8_t *pdu, int len)
314 {
315 struct att_range *range;
316 GSList *matches;
317 int offset;
318
319 if (pdu == NULL || len < 5)
320 return NULL;
321
322 if (pdu[0] != ATT_OP_FIND_BY_TYPE_RESP)
323 return NULL;
324
325 for (offset = 1, matches = NULL; len >= (offset + 4); offset += 4) {
326 range = g_new0(struct att_range, 1);
327 range->start = att_get_u16(&pdu[offset]);
328 range->end = att_get_u16(&pdu[offset + 2]);
329
330 matches = g_slist_append(matches, range);
331 }
332
333 return matches;
334 }
335
enc_read_by_type_req(uint16_t start,uint16_t end,bt_uuid_t * uuid,uint8_t * pdu,int len)336 uint16_t enc_read_by_type_req(uint16_t start, uint16_t end, bt_uuid_t *uuid,
337 uint8_t *pdu, int len)
338 {
339 const uint16_t min_len = sizeof(pdu[0]) + sizeof(start) + sizeof(end);
340 uint16_t length;
341
342 if (!uuid)
343 return 0;
344
345 if (uuid->type == BT_UUID16)
346 length = 2;
347 else if (uuid->type == BT_UUID128)
348 length = 16;
349 else
350 return 0;
351
352 if (len < min_len + length)
353 return 0;
354
355 pdu[0] = ATT_OP_READ_BY_TYPE_REQ;
356 att_put_u16(start, &pdu[1]);
357 att_put_u16(end, &pdu[3]);
358
359 att_put_uuid(*uuid, &pdu[5]);
360
361 return min_len + length;
362 }
363
dec_read_by_type_req(const uint8_t * pdu,int len,uint16_t * start,uint16_t * end,bt_uuid_t * uuid)364 uint16_t dec_read_by_type_req(const uint8_t *pdu, int len, uint16_t *start,
365 uint16_t *end, bt_uuid_t *uuid)
366 {
367 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*start) + sizeof(*end);
368
369 if (pdu == NULL)
370 return 0;
371
372 if (start == NULL || end == NULL || uuid == NULL)
373 return 0;
374
375 if (len < min_len + 2)
376 return 0;
377
378 if (pdu[0] != ATT_OP_READ_BY_TYPE_REQ)
379 return 0;
380
381 *start = att_get_u16(&pdu[1]);
382 *end = att_get_u16(&pdu[3]);
383
384 if (len == min_len + 2)
385 *uuid = att_get_uuid16(&pdu[5]);
386 else
387 *uuid = att_get_uuid128(&pdu[5]);
388
389 return len;
390 }
391
enc_read_by_type_resp(struct att_data_list * list,uint8_t * pdu,int len)392 uint16_t enc_read_by_type_resp(struct att_data_list *list, uint8_t *pdu, int len)
393 {
394 uint8_t *ptr;
395 int i, w, l;
396
397 if (list == NULL)
398 return 0;
399
400 if (pdu == NULL)
401 return 0;
402
403 l = MIN(len - 2, list->len);
404
405 pdu[0] = ATT_OP_READ_BY_TYPE_RESP;
406 pdu[1] = l;
407 ptr = &pdu[2];
408
409 for (i = 0, w = 2; i < list->num && w + l <= len; i++) {
410 memcpy(ptr, list->data[i], l);
411 ptr += l;
412 w += l;
413 }
414
415 return w;
416 }
417
dec_read_by_type_resp(const uint8_t * pdu,int len)418 struct att_data_list *dec_read_by_type_resp(const uint8_t *pdu, int len)
419 {
420 struct att_data_list *list;
421 const uint8_t *ptr;
422 uint16_t elen, num;
423 int i;
424
425 if (pdu[0] != ATT_OP_READ_BY_TYPE_RESP)
426 return NULL;
427
428 elen = pdu[1];
429 num = (len - 2) / elen;
430 list = att_data_list_alloc(num, elen);
431
432 ptr = &pdu[2];
433
434 for (i = 0; i < num; i++) {
435 memcpy(list->data[i], ptr, list->len);
436 ptr += list->len;
437 }
438
439 return list;
440 }
441
enc_write_cmd(uint16_t handle,const uint8_t * value,int vlen,uint8_t * pdu,int len)442 uint16_t enc_write_cmd(uint16_t handle, const uint8_t *value, int vlen,
443 uint8_t *pdu, int len)
444 {
445 const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle);
446
447 if (pdu == NULL)
448 return 0;
449
450 if (len < min_len)
451 return 0;
452
453 if (vlen > len - min_len)
454 vlen = len - min_len;
455
456 pdu[0] = ATT_OP_WRITE_CMD;
457 att_put_u16(handle, &pdu[1]);
458
459 if (vlen > 0) {
460 memcpy(&pdu[3], value, vlen);
461 return min_len + vlen;
462 }
463
464 return min_len;
465 }
466
dec_write_cmd(const uint8_t * pdu,int len,uint16_t * handle,uint8_t * value,int * vlen)467 uint16_t dec_write_cmd(const uint8_t *pdu, int len, uint16_t *handle,
468 uint8_t *value, int *vlen)
469 {
470 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle);
471
472 if (pdu == NULL)
473 return 0;
474
475 if (value == NULL || vlen == NULL || handle == NULL)
476 return 0;
477
478 if (len < min_len)
479 return 0;
480
481 if (pdu[0] != ATT_OP_WRITE_CMD)
482 return 0;
483
484 *handle = att_get_u16(&pdu[1]);
485 memcpy(value, pdu + min_len, len - min_len);
486 *vlen = len - min_len;
487
488 return len;
489 }
490
enc_write_req(uint16_t handle,const uint8_t * value,int vlen,uint8_t * pdu,int len)491 uint16_t enc_write_req(uint16_t handle, const uint8_t *value, int vlen,
492 uint8_t *pdu, int len)
493 {
494 const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle);
495
496 if (pdu == NULL)
497 return 0;
498
499 if (len < min_len)
500 return 0;
501
502 if (vlen > len - min_len)
503 vlen = len - min_len;
504
505 pdu[0] = ATT_OP_WRITE_REQ;
506 att_put_u16(handle, &pdu[1]);
507
508 if (vlen > 0) {
509 memcpy(&pdu[3], value, vlen);
510 return min_len + vlen;
511 }
512
513 return min_len;
514 }
515
dec_write_req(const uint8_t * pdu,int len,uint16_t * handle,uint8_t * value,int * vlen)516 uint16_t dec_write_req(const uint8_t *pdu, int len, uint16_t *handle,
517 uint8_t *value, int *vlen)
518 {
519 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle);
520
521 if (pdu == NULL)
522 return 0;
523
524 if (value == NULL || vlen == NULL || handle == NULL)
525 return 0;
526
527 if (len < min_len)
528 return 0;
529
530 if (pdu[0] != ATT_OP_WRITE_REQ)
531 return 0;
532
533 *handle = att_get_u16(&pdu[1]);
534 *vlen = len - min_len;
535 if (*vlen > 0)
536 memcpy(value, pdu + min_len, *vlen);
537
538 return len;
539 }
540
enc_write_resp(uint8_t * pdu,int len)541 uint16_t enc_write_resp(uint8_t *pdu, int len)
542 {
543 if (pdu == NULL)
544 return 0;
545
546 pdu[0] = ATT_OP_WRITE_RESP;
547
548 return sizeof(pdu[0]);
549 }
550
dec_write_resp(const uint8_t * pdu,int len)551 uint16_t dec_write_resp(const uint8_t *pdu, int len)
552 {
553 if (pdu == NULL)
554 return 0;
555
556 if (pdu[0] != ATT_OP_WRITE_RESP)
557 return 0;
558
559 return len;
560 }
561
enc_read_req(uint16_t handle,uint8_t * pdu,int len)562 uint16_t enc_read_req(uint16_t handle, uint8_t *pdu, int len)
563 {
564 const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle);
565
566 if (pdu == NULL)
567 return 0;
568
569 if (len < min_len)
570 return 0;
571
572 pdu[0] = ATT_OP_READ_REQ;
573 att_put_u16(handle, &pdu[1]);
574
575 return min_len;
576 }
577
enc_read_blob_req(uint16_t handle,uint16_t offset,uint8_t * pdu,int len)578 uint16_t enc_read_blob_req(uint16_t handle, uint16_t offset, uint8_t *pdu,
579 int len)
580 {
581 const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle) +
582 sizeof(offset);
583
584 if (pdu == NULL)
585 return 0;
586
587 if (len < min_len)
588 return 0;
589
590 pdu[0] = ATT_OP_READ_BLOB_REQ;
591 att_put_u16(handle, &pdu[1]);
592 att_put_u16(offset, &pdu[3]);
593
594 return min_len;
595 }
596
dec_read_req(const uint8_t * pdu,int len,uint16_t * handle)597 uint16_t dec_read_req(const uint8_t *pdu, int len, uint16_t *handle)
598 {
599 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle);
600
601 if (pdu == NULL)
602 return 0;
603
604 if (handle == NULL)
605 return 0;
606
607 if (len < min_len)
608 return 0;
609
610 if (pdu[0] != ATT_OP_READ_REQ)
611 return 0;
612
613 *handle = att_get_u16(&pdu[1]);
614
615 return min_len;
616 }
617
dec_read_blob_req(const uint8_t * pdu,int len,uint16_t * handle,uint16_t * offset)618 uint16_t dec_read_blob_req(const uint8_t *pdu, int len, uint16_t *handle,
619 uint16_t *offset)
620 {
621 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle) +
622 sizeof(*offset);
623
624 if (pdu == NULL)
625 return 0;
626
627 if (handle == NULL)
628 return 0;
629
630 if (offset == NULL)
631 return 0;
632
633 if (len < min_len)
634 return 0;
635
636 if (pdu[0] != ATT_OP_READ_BLOB_REQ)
637 return 0;
638
639 *handle = att_get_u16(&pdu[1]);
640 *offset = att_get_u16(&pdu[3]);
641
642 return min_len;
643 }
644
enc_read_resp(uint8_t * value,int vlen,uint8_t * pdu,int len)645 uint16_t enc_read_resp(uint8_t *value, int vlen, uint8_t *pdu, int len)
646 {
647 if (pdu == NULL)
648 return 0;
649
650 /* If the attribute value length is longer than the allowed PDU size,
651 * send only the octets that fit on the PDU. The remaining octets can
652 * be requested using the Read Blob Request. */
653 if (vlen > len - 1)
654 vlen = len - 1;
655
656 pdu[0] = ATT_OP_READ_RESP;
657
658 memcpy(pdu + 1, value, vlen);
659
660 return vlen + 1;
661 }
662
enc_read_blob_resp(uint8_t * value,int vlen,uint16_t offset,uint8_t * pdu,int len)663 uint16_t enc_read_blob_resp(uint8_t *value, int vlen, uint16_t offset,
664 uint8_t *pdu, int len)
665 {
666 if (pdu == NULL)
667 return 0;
668
669 vlen -= offset;
670 if (vlen > len - 1)
671 vlen = len - 1;
672
673 pdu[0] = ATT_OP_READ_BLOB_RESP;
674
675 memcpy(pdu + 1, &value[offset], vlen);
676
677 return vlen + 1;
678 }
679
dec_read_resp(const uint8_t * pdu,int len,uint8_t * value,int * vlen)680 uint16_t dec_read_resp(const uint8_t *pdu, int len, uint8_t *value, int *vlen)
681 {
682 if (pdu == NULL)
683 return 0;
684
685 if (value == NULL || vlen == NULL)
686 return 0;
687
688 if (pdu[0] != ATT_OP_READ_RESP)
689 return 0;
690
691 memcpy(value, pdu + 1, len - 1);
692
693 *vlen = len - 1;
694
695 return len;
696 }
697
enc_error_resp(uint8_t opcode,uint16_t handle,uint8_t status,uint8_t * pdu,int len)698 uint16_t enc_error_resp(uint8_t opcode, uint16_t handle, uint8_t status,
699 uint8_t *pdu, int len)
700 {
701 const uint16_t min_len = sizeof(pdu[0]) + sizeof(opcode) +
702 sizeof(handle) + sizeof(status);
703 uint16_t u16;
704
705 if (len < min_len)
706 return 0;
707
708 u16 = htobs(handle);
709 pdu[0] = ATT_OP_ERROR;
710 pdu[1] = opcode;
711 memcpy(&pdu[2], &u16, sizeof(u16));
712 pdu[4] = status;
713
714 return min_len;
715 }
716
enc_find_info_req(uint16_t start,uint16_t end,uint8_t * pdu,int len)717 uint16_t enc_find_info_req(uint16_t start, uint16_t end, uint8_t *pdu, int len)
718 {
719 const uint16_t min_len = sizeof(pdu[0]) + sizeof(start) + sizeof(end);
720
721 if (pdu == NULL)
722 return 0;
723
724 if (len < min_len)
725 return 0;
726
727 pdu[0] = ATT_OP_FIND_INFO_REQ;
728 att_put_u16(start, &pdu[1]);
729 att_put_u16(end, &pdu[3]);
730
731 return min_len;
732 }
733
dec_find_info_req(const uint8_t * pdu,int len,uint16_t * start,uint16_t * end)734 uint16_t dec_find_info_req(const uint8_t *pdu, int len, uint16_t *start,
735 uint16_t *end)
736 {
737 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*start) + sizeof(*end);
738
739 if (pdu == NULL)
740 return 0;
741
742 if (len < min_len)
743 return 0;
744
745 if (start == NULL || end == NULL)
746 return 0;
747
748 if (pdu[0] != ATT_OP_FIND_INFO_REQ)
749 return 0;
750
751 *start = att_get_u16(&pdu[1]);
752 *end = att_get_u16(&pdu[3]);
753
754 return min_len;
755 }
756
enc_find_info_resp(uint8_t format,struct att_data_list * list,uint8_t * pdu,int len)757 uint16_t enc_find_info_resp(uint8_t format, struct att_data_list *list,
758 uint8_t *pdu, int len)
759 {
760 uint8_t *ptr;
761 int i, w;
762
763 if (pdu == NULL)
764 return 0;
765
766 if (list == NULL)
767 return 0;
768
769 if (len < list->len + 2)
770 return 0;
771
772 pdu[0] = ATT_OP_FIND_INFO_RESP;
773 pdu[1] = format;
774 ptr = (void *) &pdu[2];
775
776 for (i = 0, w = 2; i < list->num && w + list->len <= len; i++) {
777 memcpy(ptr, list->data[i], list->len);
778 ptr += list->len;
779 w += list->len;
780 }
781
782 return w;
783 }
784
dec_find_info_resp(const uint8_t * pdu,int len,uint8_t * format)785 struct att_data_list *dec_find_info_resp(const uint8_t *pdu, int len,
786 uint8_t *format)
787 {
788 struct att_data_list *list;
789 uint8_t *ptr;
790 uint16_t elen, num;
791 int i;
792
793 if (pdu == NULL)
794 return 0;
795
796 if (format == NULL)
797 return 0;
798
799 if (pdu[0] != ATT_OP_FIND_INFO_RESP)
800 return 0;
801
802 *format = pdu[1];
803 elen = sizeof(pdu[0]) + sizeof(*format);
804 if (*format == 0x01)
805 elen += 2;
806 else if (*format == 0x02)
807 elen += 16;
808
809 num = (len - 2) / elen;
810
811 ptr = (void *) &pdu[2];
812
813 list = att_data_list_alloc(num, elen);
814
815 for (i = 0; i < num; i++) {
816 memcpy(list->data[i], ptr, list->len);
817 ptr += list->len;
818 }
819
820 return list;
821 }
822
enc_notification(struct attribute * a,uint8_t * pdu,int len)823 uint16_t enc_notification(struct attribute *a, uint8_t *pdu, int len)
824 {
825 const uint16_t min_len = sizeof(pdu[0]) + sizeof(uint16_t);
826
827 if (pdu == NULL)
828 return 0;
829
830 if (len < (a->len + min_len))
831 return 0;
832
833 pdu[0] = ATT_OP_HANDLE_NOTIFY;
834 att_put_u16(a->handle, &pdu[1]);
835 memcpy(&pdu[3], a->data, a->len);
836
837 return a->len + min_len;
838 }
839
enc_indication(struct attribute * a,uint8_t * pdu,int len)840 uint16_t enc_indication(struct attribute *a, uint8_t *pdu, int len)
841 {
842 const uint16_t min_len = sizeof(pdu[0]) + sizeof(uint16_t);
843
844 if (pdu == NULL)
845 return 0;
846
847 if (len < (a->len + min_len))
848 return 0;
849
850 pdu[0] = ATT_OP_HANDLE_IND;
851 att_put_u16(a->handle, &pdu[1]);
852 memcpy(&pdu[3], a->data, a->len);
853
854 return a->len + min_len;
855 }
856
dec_indication(const uint8_t * pdu,int len)857 struct attribute *dec_indication(const uint8_t *pdu, int len)
858 {
859 const uint16_t min_len = sizeof(pdu[0]) + sizeof(uint16_t);
860
861 struct attribute *a;
862
863 if (pdu == NULL)
864 return NULL;
865
866 if (pdu[0] != ATT_OP_HANDLE_IND)
867 return NULL;
868
869 if (len < min_len)
870 return NULL;
871
872 a = g_malloc0(sizeof(struct attribute) + len - min_len);
873 a->len = len - min_len;
874
875 a->handle = att_get_u16(&pdu[1]);
876 memcpy(a->data, &pdu[3], a->len);
877
878 return a;
879 }
880
enc_confirmation(uint8_t * pdu,int len)881 uint16_t enc_confirmation(uint8_t *pdu, int len)
882 {
883 const uint16_t min_len = sizeof(pdu[0]);
884
885 if (pdu == NULL)
886 return 0;
887
888 if (len < min_len)
889 return 0;
890
891 pdu[0] = ATT_OP_HANDLE_CNF;
892
893 return min_len;
894 }
895
enc_mtu_req(uint16_t mtu,uint8_t * pdu,int len)896 uint16_t enc_mtu_req(uint16_t mtu, uint8_t *pdu, int len)
897 {
898 const uint16_t min_len = sizeof(pdu[0]) + sizeof(mtu);
899
900 if (pdu == NULL)
901 return 0;
902
903 if (len < min_len)
904 return 0;
905
906 pdu[0] = ATT_OP_MTU_REQ;
907 att_put_u16(mtu, &pdu[1]);
908
909 return min_len;
910 }
911
dec_mtu_req(const uint8_t * pdu,int len,uint16_t * mtu)912 uint16_t dec_mtu_req(const uint8_t *pdu, int len, uint16_t *mtu)
913 {
914 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*mtu);
915
916 if (pdu == NULL)
917 return 0;
918
919 if (mtu == NULL)
920 return 0;
921
922 if (len < min_len)
923 return 0;
924
925 if (pdu[0] != ATT_OP_MTU_REQ)
926 return 0;
927
928 *mtu = att_get_u16(&pdu[1]);
929
930 return min_len;
931 }
932
enc_mtu_resp(uint16_t mtu,uint8_t * pdu,int len)933 uint16_t enc_mtu_resp(uint16_t mtu, uint8_t *pdu, int len)
934 {
935 const uint16_t min_len = sizeof(pdu[0]) + sizeof(mtu);
936
937 if (pdu == NULL)
938 return 0;
939
940 if (len < min_len)
941 return 0;
942
943 pdu[0] = ATT_OP_MTU_RESP;
944 att_put_u16(mtu, &pdu[1]);
945
946 return min_len;
947 }
948
dec_mtu_resp(const uint8_t * pdu,int len,uint16_t * mtu)949 uint16_t dec_mtu_resp(const uint8_t *pdu, int len, uint16_t *mtu)
950 {
951 const uint16_t min_len = sizeof(pdu[0]) + sizeof(*mtu);
952
953 if (pdu == NULL)
954 return 0;
955
956 if (mtu == NULL)
957 return 0;
958
959 if (len < min_len)
960 return 0;
961
962 if (pdu[0] != ATT_OP_MTU_RESP)
963 return 0;
964
965 *mtu = att_get_u16(&pdu[1]);
966
967 return min_len;
968 }
969