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 #ifdef HAVE_CONFIG_H
26 #include <config.h>
27 #endif
28
29 #include <errno.h>
30 #include <stdint.h>
31 #include <string.h>
32 #include <unistd.h>
33 #include <glib.h>
34
35 #include <bluetooth/bluetooth.h>
36 #include <bluetooth/uuid.h>
37 #include <bluetooth/sdp.h>
38 #include <bluetooth/sdp_lib.h>
39
40 #include "log.h"
41 #include "glib-helper.h"
42 #include "btio.h"
43 #include "sdpd.h"
44 #include "hcid.h"
45 #include "att.h"
46 #include "gattrib.h"
47
48 #include "attrib-server.h"
49
50 static GSList *database = NULL;
51
52 struct gatt_channel {
53 bdaddr_t src;
54 bdaddr_t dst;
55 GSList *configs;
56 GSList *notify;
57 GSList *indicate;
58 GAttrib *attrib;
59 guint mtu;
60 gboolean le;
61 guint id;
62 gboolean encrypted;
63 };
64
65 struct group_elem {
66 uint16_t handle;
67 uint16_t end;
68 uint8_t *data;
69 uint16_t len;
70 };
71
72 static GIOChannel *l2cap_io = NULL;
73 static GIOChannel *le_io = NULL;
74 static GSList *clients = NULL;
75 static uint32_t gatt_sdp_handle = 0;
76 static uint32_t gap_sdp_handle = 0;
77
78 /* GAP attribute handles */
79 static uint16_t name_handle = 0x0000;
80 static uint16_t appearance_handle = 0x0000;
81
82 static bt_uuid_t prim_uuid = {
83 .type = BT_UUID16,
84 .value.u16 = GATT_PRIM_SVC_UUID
85 };
86 static bt_uuid_t snd_uuid = {
87 .type = BT_UUID16,
88 .value.u16 = GATT_SND_SVC_UUID
89 };
90
server_record_new(uuid_t * uuid,uint16_t start,uint16_t end)91 static sdp_record_t *server_record_new(uuid_t *uuid, uint16_t start, uint16_t end)
92 {
93 sdp_list_t *svclass_id, *apseq, *proto[2], *root, *aproto;
94 uuid_t root_uuid, proto_uuid, l2cap;
95 sdp_record_t *record;
96 sdp_data_t *psm, *sh, *eh;
97 uint16_t lp = ATT_PSM;
98
99 if (uuid == NULL)
100 return NULL;
101
102 if (start > end)
103 return NULL;
104
105 record = sdp_record_alloc();
106 if (record == NULL)
107 return NULL;
108
109 sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
110 root = sdp_list_append(NULL, &root_uuid);
111 sdp_set_browse_groups(record, root);
112 sdp_list_free(root, NULL);
113
114 svclass_id = sdp_list_append(NULL, uuid);
115 sdp_set_service_classes(record, svclass_id);
116 sdp_list_free(svclass_id, NULL);
117
118 sdp_uuid16_create(&l2cap, L2CAP_UUID);
119 proto[0] = sdp_list_append(NULL, &l2cap);
120 psm = sdp_data_alloc(SDP_UINT16, &lp);
121 proto[0] = sdp_list_append(proto[0], psm);
122 apseq = sdp_list_append(NULL, proto[0]);
123
124 sdp_uuid16_create(&proto_uuid, ATT_UUID);
125 proto[1] = sdp_list_append(NULL, &proto_uuid);
126 sh = sdp_data_alloc(SDP_UINT16, &start);
127 proto[1] = sdp_list_append(proto[1], sh);
128 eh = sdp_data_alloc(SDP_UINT16, &end);
129 proto[1] = sdp_list_append(proto[1], eh);
130 apseq = sdp_list_append(apseq, proto[1]);
131
132 aproto = sdp_list_append(NULL, apseq);
133 sdp_set_access_protos(record, aproto);
134
135 sdp_data_free(psm);
136 sdp_data_free(sh);
137 sdp_data_free(eh);
138 sdp_list_free(proto[0], NULL);
139 sdp_list_free(proto[1], NULL);
140 sdp_list_free(apseq, NULL);
141 sdp_list_free(aproto, NULL);
142
143 return record;
144 }
145
handle_cmp(gconstpointer a,gconstpointer b)146 static int handle_cmp(gconstpointer a, gconstpointer b)
147 {
148 const struct attribute *attrib = a;
149 uint16_t handle = GPOINTER_TO_UINT(b);
150
151 return attrib->handle - handle;
152 }
153
attribute_cmp(gconstpointer a1,gconstpointer a2)154 static int attribute_cmp(gconstpointer a1, gconstpointer a2)
155 {
156 const struct attribute *attrib1 = a1;
157 const struct attribute *attrib2 = a2;
158
159 return attrib1->handle - attrib2->handle;
160 }
161
att_check_reqs(struct gatt_channel * channel,uint8_t opcode,int reqs)162 static uint8_t att_check_reqs(struct gatt_channel *channel, uint8_t opcode,
163 int reqs)
164 {
165 /* FIXME: currently, it is assumed an encrypted link is enough for
166 * authentication. This will allow to enable the SMP negotiation once
167 * it is on upstream kernel. High security level should be mapped
168 * to authentication and medium to encryption permission. */
169 if (!channel->encrypted)
170 channel->encrypted = g_attrib_is_encrypted(channel->attrib);
171 if (reqs == ATT_AUTHENTICATION && !channel->encrypted)
172 return ATT_ECODE_AUTHENTICATION;
173 else if (reqs == ATT_AUTHORIZATION)
174 return ATT_ECODE_AUTHORIZATION;
175
176 switch (opcode) {
177 case ATT_OP_READ_BY_GROUP_REQ:
178 case ATT_OP_READ_BY_TYPE_REQ:
179 case ATT_OP_READ_REQ:
180 case ATT_OP_READ_BLOB_REQ:
181 case ATT_OP_READ_MULTI_REQ:
182 if (reqs == ATT_NOT_PERMITTED)
183 return ATT_ECODE_READ_NOT_PERM;
184 break;
185 case ATT_OP_PREP_WRITE_REQ:
186 case ATT_OP_WRITE_REQ:
187 case ATT_OP_WRITE_CMD:
188 if (reqs == ATT_NOT_PERMITTED)
189 return ATT_ECODE_WRITE_NOT_PERM;
190 break;
191 }
192
193 return 0;
194 }
195
client_set_notifications(struct attribute * attr,gpointer user_data)196 static uint8_t client_set_notifications(struct attribute *attr,
197 gpointer user_data)
198 {
199 struct gatt_channel *channel = user_data;
200 struct attribute *last_chr_val = NULL;
201 uint16_t cfg_val;
202 uint8_t props;
203 bt_uuid_t uuid;
204 GSList *l;
205
206 cfg_val = att_get_u16(attr->data);
207
208 bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
209 for (l = database, props = 0; l != NULL; l = l->next) {
210 struct attribute *a = l->data;
211 static uint16_t handle = 0;
212
213 if (a->handle >= attr->handle)
214 break;
215
216 if (bt_uuid_cmp(&a->uuid, &uuid) == 0) {
217 props = att_get_u8(&a->data[0]);
218 handle = att_get_u16(&a->data[1]);
219 continue;
220 }
221
222 if (handle && a->handle == handle)
223 last_chr_val = a;
224 }
225
226 if (last_chr_val == NULL)
227 return 0;
228
229 if ((cfg_val & 0x0001) && !(props & ATT_CHAR_PROPER_NOTIFY))
230 return ATT_ECODE_WRITE_NOT_PERM;
231
232 if ((cfg_val & 0x0002) && !(props & ATT_CHAR_PROPER_INDICATE))
233 return ATT_ECODE_WRITE_NOT_PERM;
234
235 if (cfg_val & 0x0001)
236 channel->notify = g_slist_append(channel->notify, last_chr_val);
237 else
238 channel->notify = g_slist_remove(channel->notify, last_chr_val);
239
240 if (cfg_val & 0x0002)
241 channel->indicate = g_slist_append(channel->indicate,
242 last_chr_val);
243 else
244 channel->indicate = g_slist_remove(channel->indicate,
245 last_chr_val);
246
247 return 0;
248 }
249
client_cfg_attribute(struct gatt_channel * channel,struct attribute * orig_attr,const uint8_t * value,int vlen)250 static struct attribute *client_cfg_attribute(struct gatt_channel *channel,
251 struct attribute *orig_attr,
252 const uint8_t *value, int vlen)
253 {
254 guint handle = orig_attr->handle;
255 bt_uuid_t uuid;
256 GSList *l;
257
258 bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
259 if (bt_uuid_cmp(&orig_attr->uuid, &uuid) != 0)
260 return NULL;
261
262 /* Value is unchanged, not need to create a private copy yet */
263 if (vlen == orig_attr->len && memcmp(orig_attr->data, value, vlen) == 0)
264 return orig_attr;
265
266 l = g_slist_find_custom(channel->configs, GUINT_TO_POINTER(handle),
267 handle_cmp);
268 if (!l) {
269 struct attribute *a;
270
271 /* Create a private copy of the Client Characteristic
272 * Configuration attribute */
273 a = g_malloc0(sizeof(*a) + vlen);
274 memcpy(a, orig_attr, sizeof(*a));
275 memcpy(a->data, value, vlen);
276 a->write_cb = client_set_notifications;
277 a->cb_user_data = channel;
278
279 channel->configs = g_slist_insert_sorted(channel->configs, a,
280 attribute_cmp);
281
282 return a;
283 }
284
285 return l->data;
286 }
287
read_by_group(struct gatt_channel * channel,uint16_t start,uint16_t end,bt_uuid_t * uuid,uint8_t * pdu,int len)288 static uint16_t read_by_group(struct gatt_channel *channel, uint16_t start,
289 uint16_t end, bt_uuid_t *uuid,
290 uint8_t *pdu, int len)
291 {
292 struct att_data_list *adl;
293 struct attribute *a;
294 struct group_elem *cur, *old = NULL;
295 GSList *l, *groups;
296 uint16_t length, last_handle, last_size = 0;
297 uint8_t status;
298 int i;
299
300 if (start > end || start == 0x0000)
301 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
302 ATT_ECODE_INVALID_HANDLE, pdu, len);
303
304 /*
305 * Only <<Primary Service>> and <<Secondary Service>> grouping
306 * types may be used in the Read By Group Type Request.
307 */
308
309 if (bt_uuid_cmp(uuid, &prim_uuid) != 0 &&
310 bt_uuid_cmp(uuid, &snd_uuid) != 0)
311 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, 0x0000,
312 ATT_ECODE_UNSUPP_GRP_TYPE, pdu, len);
313
314 last_handle = end;
315 for (l = database, groups = NULL, cur = NULL; l; l = l->next) {
316 struct attribute *client_attr;
317
318 a = l->data;
319
320 if (a->handle < start)
321 continue;
322
323 if (a->handle >= end)
324 break;
325
326 /* The old group ends when a new one starts */
327 if (old && (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
328 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)) {
329 old->end = last_handle;
330 old = NULL;
331 }
332
333 if (bt_uuid_cmp(&a->uuid, uuid) != 0) {
334 /* Still inside a service, update its last handle */
335 if (old)
336 last_handle = a->handle;
337 continue;
338 }
339
340 if (last_size && (last_size != a->len))
341 break;
342
343 status = att_check_reqs(channel, ATT_OP_READ_BY_GROUP_REQ,
344 a->read_reqs);
345
346 client_attr = client_cfg_attribute(channel, a, a->data, a->len);
347 if (client_attr)
348 a = client_attr;
349
350 if (status == 0x00 && a->read_cb)
351 status = a->read_cb(a, a->cb_user_data);
352
353 if (status) {
354 g_slist_foreach(groups, (GFunc) g_free, NULL);
355 g_slist_free(groups);
356 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ,
357 a->handle, status, pdu, len);
358 }
359
360 cur = g_new0(struct group_elem, 1);
361 cur->handle = a->handle;
362 cur->data = a->data;
363 cur->len = a->len;
364
365 /* Attribute Grouping Type found */
366 groups = g_slist_append(groups, cur);
367
368 last_size = a->len;
369 old = cur;
370 last_handle = cur->handle;
371 }
372
373 if (groups == NULL)
374 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
375 ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
376
377 if (l == NULL)
378 cur->end = a->handle;
379 else
380 cur->end = last_handle;
381
382 length = g_slist_length(groups);
383
384 adl = att_data_list_alloc(length, last_size + 4);
385
386 for (i = 0, l = groups; l; l = l->next, i++) {
387 uint8_t *value;
388
389 cur = l->data;
390
391 value = (void *) adl->data[i];
392
393 att_put_u16(cur->handle, value);
394 att_put_u16(cur->end, &value[2]);
395 /* Attribute Value */
396 memcpy(&value[4], cur->data, cur->len);
397 }
398
399 length = enc_read_by_grp_resp(adl, pdu, len);
400
401 att_data_list_free(adl);
402 g_slist_foreach(groups, (GFunc) g_free, NULL);
403 g_slist_free(groups);
404
405 return length;
406 }
407
read_by_type(struct gatt_channel * channel,uint16_t start,uint16_t end,bt_uuid_t * uuid,uint8_t * pdu,int len)408 static uint16_t read_by_type(struct gatt_channel *channel, uint16_t start,
409 uint16_t end, bt_uuid_t *uuid,
410 uint8_t *pdu, int len)
411 {
412 struct att_data_list *adl;
413 GSList *l, *types;
414 struct attribute *a;
415 uint16_t num, length;
416 uint8_t status;
417 int i;
418
419 if (start > end || start == 0x0000)
420 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
421 ATT_ECODE_INVALID_HANDLE, pdu, len);
422
423 for (l = database, length = 0, types = NULL; l; l = l->next) {
424 struct attribute *client_attr;
425
426 a = l->data;
427
428 if (a->handle < start)
429 continue;
430
431 if (a->handle > end)
432 break;
433
434 if (bt_uuid_cmp(&a->uuid, uuid) != 0)
435 continue;
436
437 status = att_check_reqs(channel, ATT_OP_READ_BY_TYPE_REQ,
438 a->read_reqs);
439
440 client_attr = client_cfg_attribute(channel, a, a->data, a->len);
441 if (client_attr)
442 a = client_attr;
443
444 if (status == 0x00 && a->read_cb)
445 status = a->read_cb(a, a->cb_user_data);
446
447 if (status) {
448 g_slist_free(types);
449 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ,
450 a->handle, status, pdu, len);
451 }
452
453 /* All elements must have the same length */
454 if (length == 0)
455 length = a->len;
456 else if (a->len != length)
457 break;
458
459 types = g_slist_append(types, a);
460 }
461
462 if (types == NULL)
463 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
464 ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
465
466 num = g_slist_length(types);
467
468 /* Handle length plus attribute value length */
469 length += 2;
470
471 adl = att_data_list_alloc(num, length);
472
473 for (i = 0, l = types; l; i++, l = l->next) {
474 uint8_t *value;
475
476 a = l->data;
477
478 value = (void *) adl->data[i];
479
480 att_put_u16(a->handle, value);
481
482 /* Attribute Value */
483 memcpy(&value[2], a->data, a->len);
484 }
485
486 length = enc_read_by_type_resp(adl, pdu, len);
487
488 att_data_list_free(adl);
489 g_slist_free(types);
490
491 return length;
492 }
493
find_info(uint16_t start,uint16_t end,uint8_t * pdu,int len)494 static int find_info(uint16_t start, uint16_t end, uint8_t *pdu, int len)
495 {
496 struct attribute *a;
497 struct att_data_list *adl;
498 GSList *l, *info;
499 uint8_t format, last_type = BT_UUID_UNSPEC;
500 uint16_t length, num;
501 int i;
502
503 if (start > end || start == 0x0000)
504 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
505 ATT_ECODE_INVALID_HANDLE, pdu, len);
506
507 for (l = database, info = NULL, num = 0; l; l = l->next) {
508 a = l->data;
509
510 if (a->handle < start)
511 continue;
512
513 if (a->handle > end)
514 break;
515
516 if (last_type == BT_UUID_UNSPEC)
517 last_type = a->uuid.type;
518
519 if (a->uuid.type != last_type)
520 break;
521
522 info = g_slist_append(info, a);
523 num++;
524
525 last_type = a->uuid.type;
526 }
527
528 if (info == NULL)
529 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
530 ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
531
532 if (last_type == BT_UUID16) {
533 length = 2;
534 format = 0x01;
535 } else if (last_type == BT_UUID128) {
536 length = 16;
537 format = 0x02;
538 } else {
539 g_slist_free(info);
540 return 0;
541 }
542
543 adl = att_data_list_alloc(num, length + 2);
544
545 for (i = 0, l = info; l; i++, l = l->next) {
546 uint8_t *value;
547
548 a = l->data;
549
550 value = (void *) adl->data[i];
551
552 att_put_u16(a->handle, value);
553
554 /* Attribute Value */
555 att_put_uuid(a->uuid, &value[2]);
556 }
557
558 length = enc_find_info_resp(format, adl, pdu, len);
559
560 att_data_list_free(adl);
561 g_slist_free(info);
562
563 return length;
564 }
565
find_by_type(uint16_t start,uint16_t end,bt_uuid_t * uuid,const uint8_t * value,int vlen,uint8_t * opdu,int mtu)566 static int find_by_type(uint16_t start, uint16_t end, bt_uuid_t *uuid,
567 const uint8_t *value, int vlen, uint8_t *opdu, int mtu)
568 {
569 struct attribute *a;
570 struct att_range *range;
571 GSList *l, *matches;
572 int len;
573
574 if (start > end || start == 0x0000)
575 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
576 ATT_ECODE_INVALID_HANDLE, opdu, mtu);
577
578 /* Searching first requested handle number */
579 for (l = database, matches = NULL, range = NULL; l; l = l->next) {
580 a = l->data;
581
582 if (a->handle < start)
583 continue;
584
585 if (a->handle > end)
586 break;
587
588 /* Primary service? Attribute value matches? */
589 if ((bt_uuid_cmp(&a->uuid, uuid) == 0) && (a->len == vlen) &&
590 (memcmp(a->data, value, vlen) == 0)) {
591
592 range = g_new0(struct att_range, 1);
593 range->start = a->handle;
594 /* It is allowed to have end group handle the same as
595 * start handle, for groups with only one attribute. */
596 range->end = a->handle;
597
598 matches = g_slist_append(matches, range);
599 } else if (range) {
600 /* Update the last found handle or reset the pointer
601 * to track that a new group started: Primary or
602 * Secondary service. */
603 if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
604 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)
605 range = NULL;
606 else
607 range->end = a->handle;
608 }
609 }
610
611 if (matches == NULL)
612 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
613 ATT_ECODE_ATTR_NOT_FOUND, opdu, mtu);
614
615 len = enc_find_by_type_resp(matches, opdu, mtu);
616
617 g_slist_foreach(matches, (GFunc) g_free, NULL);
618 g_slist_free(matches);
619
620 return len;
621 }
622
find_primary_range(uint16_t start,uint16_t * end)623 static struct attribute *find_primary_range(uint16_t start, uint16_t *end)
624 {
625 struct attribute *attrib;
626 guint h = start;
627 GSList *l;
628
629 if (end == NULL)
630 return NULL;
631
632 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
633 if (!l)
634 return NULL;
635
636 attrib = l->data;
637
638 if (bt_uuid_cmp(&attrib->uuid, &prim_uuid) != 0)
639 return NULL;
640
641 *end = start;
642
643 for (l = l->next; l; l = l->next) {
644 struct attribute *a = l->data;
645
646 if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
647 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)
648 break;
649
650 *end = a->handle;
651 }
652
653 return attrib;
654 }
655
read_value(struct gatt_channel * channel,uint16_t handle,uint8_t * pdu,int len)656 static uint16_t read_value(struct gatt_channel *channel, uint16_t handle,
657 uint8_t *pdu, int len)
658 {
659 struct attribute *a, *client_attr;
660 uint8_t status;
661 GSList *l;
662 guint h = handle;
663
664 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
665 if (!l)
666 return enc_error_resp(ATT_OP_READ_REQ, handle,
667 ATT_ECODE_INVALID_HANDLE, pdu, len);
668
669 a = l->data;
670
671 status = att_check_reqs(channel, ATT_OP_READ_REQ, a->read_reqs);
672
673 client_attr = client_cfg_attribute(channel, a, a->data, a->len);
674 if (client_attr)
675 a = client_attr;
676
677 if (status == 0x00 && a->read_cb)
678 status = a->read_cb(a, a->cb_user_data);
679
680 if (status)
681 return enc_error_resp(ATT_OP_READ_REQ, handle, status, pdu,
682 len);
683
684 return enc_read_resp(a->data, a->len, pdu, len);
685 }
686
read_blob(struct gatt_channel * channel,uint16_t handle,uint16_t offset,uint8_t * pdu,int len)687 static uint16_t read_blob(struct gatt_channel *channel, uint16_t handle,
688 uint16_t offset, uint8_t *pdu, int len)
689 {
690 struct attribute *a, *client_attr;
691 uint8_t status;
692 GSList *l;
693 guint h = handle;
694
695 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
696 if (!l)
697 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle,
698 ATT_ECODE_INVALID_HANDLE, pdu, len);
699
700 a = l->data;
701
702 if (a->len <= offset)
703 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle,
704 ATT_ECODE_INVALID_OFFSET, pdu, len);
705
706 status = att_check_reqs(channel, ATT_OP_READ_BLOB_REQ, a->read_reqs);
707
708 client_attr = client_cfg_attribute(channel, a, a->data, a->len);
709 if (client_attr)
710 a = client_attr;
711
712 if (status == 0x00 && a->read_cb)
713 status = a->read_cb(a, a->cb_user_data);
714
715 if (status)
716 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle, status,
717 pdu, len);
718
719 return enc_read_blob_resp(a->data, a->len, offset, pdu, len);
720 }
721
write_value(struct gatt_channel * channel,uint16_t handle,const uint8_t * value,int vlen,uint8_t * pdu,int len)722 static uint16_t write_value(struct gatt_channel *channel, uint16_t handle,
723 const uint8_t *value, int vlen,
724 uint8_t *pdu, int len)
725 {
726 struct attribute *a, *client_attr;
727 uint8_t status;
728 GSList *l;
729 guint h = handle;
730
731 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
732 if (!l)
733 return enc_error_resp(ATT_OP_WRITE_REQ, handle,
734 ATT_ECODE_INVALID_HANDLE, pdu, len);
735
736 a = l->data;
737
738 status = att_check_reqs(channel, ATT_OP_WRITE_REQ, a->write_reqs);
739 if (status)
740 return enc_error_resp(ATT_OP_WRITE_REQ, handle, status, pdu,
741 len);
742
743 client_attr = client_cfg_attribute(channel, a, value, vlen);
744 if (client_attr)
745 a = client_attr;
746 else
747 attrib_db_update(a->handle, NULL, value, vlen, &a);
748
749 if (a->write_cb) {
750 status = a->write_cb(a, a->cb_user_data);
751 if (status)
752 return enc_error_resp(ATT_OP_WRITE_REQ, handle, status,
753 pdu, len);
754 }
755
756 DBG("Notifications: %d, indications: %d",
757 g_slist_length(channel->notify),
758 g_slist_length(channel->indicate));
759
760 return enc_write_resp(pdu, len);
761 }
762
mtu_exchange(struct gatt_channel * channel,uint16_t mtu,uint8_t * pdu,int len)763 static uint16_t mtu_exchange(struct gatt_channel *channel, uint16_t mtu,
764 uint8_t *pdu, int len)
765 {
766 guint old_mtu = channel->mtu;
767
768 if (mtu < ATT_DEFAULT_LE_MTU)
769 channel->mtu = ATT_DEFAULT_LE_MTU;
770 else
771 channel->mtu = MIN(mtu, channel->mtu);
772
773 bt_io_set(le_io, BT_IO_L2CAP, NULL,
774 BT_IO_OPT_OMTU, channel->mtu,
775 BT_IO_OPT_INVALID);
776
777 return enc_mtu_resp(old_mtu, pdu, len);
778 }
779
channel_disconnect(void * user_data)780 static void channel_disconnect(void *user_data)
781 {
782 struct gatt_channel *channel = user_data;
783
784 g_attrib_unref(channel->attrib);
785 clients = g_slist_remove(clients, channel);
786
787 g_slist_free(channel->notify);
788 g_slist_free(channel->indicate);
789 g_slist_foreach(channel->configs, (GFunc) g_free, NULL);
790 g_slist_free(channel->configs);
791
792 g_free(channel);
793 }
794
channel_handler(const uint8_t * ipdu,uint16_t len,gpointer user_data)795 static void channel_handler(const uint8_t *ipdu, uint16_t len,
796 gpointer user_data)
797 {
798 struct gatt_channel *channel = user_data;
799 uint8_t opdu[ATT_MAX_MTU], value[ATT_MAX_MTU];
800 uint16_t length, start, end, mtu, offset;
801 bt_uuid_t uuid;
802 uint8_t status = 0;
803 int vlen;
804
805 DBG("op 0x%02x", ipdu[0]);
806
807 switch (ipdu[0]) {
808 case ATT_OP_READ_BY_GROUP_REQ:
809 length = dec_read_by_grp_req(ipdu, len, &start, &end, &uuid);
810 if (length == 0) {
811 status = ATT_ECODE_INVALID_PDU;
812 goto done;
813 }
814
815 length = read_by_group(channel, start, end, &uuid, opdu,
816 channel->mtu);
817 break;
818 case ATT_OP_READ_BY_TYPE_REQ:
819 length = dec_read_by_type_req(ipdu, len, &start, &end, &uuid);
820 if (length == 0) {
821 status = ATT_ECODE_INVALID_PDU;
822 goto done;
823 }
824
825 length = read_by_type(channel, start, end, &uuid, opdu,
826 channel->mtu);
827 break;
828 case ATT_OP_READ_REQ:
829 length = dec_read_req(ipdu, len, &start);
830 if (length == 0) {
831 status = ATT_ECODE_INVALID_PDU;
832 goto done;
833 }
834
835 length = read_value(channel, start, opdu, channel->mtu);
836 break;
837 case ATT_OP_READ_BLOB_REQ:
838 length = dec_read_blob_req(ipdu, len, &start, &offset);
839 if (length == 0) {
840 status = ATT_ECODE_INVALID_PDU;
841 goto done;
842 }
843
844 length = read_blob(channel, start, offset, opdu, channel->mtu);
845 break;
846 case ATT_OP_MTU_REQ:
847 if (!channel->le) {
848 status = ATT_ECODE_REQ_NOT_SUPP;
849 goto done;
850 }
851
852 length = dec_mtu_req(ipdu, len, &mtu);
853 if (length == 0) {
854 status = ATT_ECODE_INVALID_PDU;
855 goto done;
856 }
857
858 length = mtu_exchange(channel, mtu, opdu, channel->mtu);
859 break;
860 case ATT_OP_FIND_INFO_REQ:
861 length = dec_find_info_req(ipdu, len, &start, &end);
862 if (length == 0) {
863 status = ATT_ECODE_INVALID_PDU;
864 goto done;
865 }
866
867 length = find_info(start, end, opdu, channel->mtu);
868 break;
869 case ATT_OP_WRITE_REQ:
870 length = dec_write_req(ipdu, len, &start, value, &vlen);
871 if (length == 0) {
872 status = ATT_ECODE_INVALID_PDU;
873 goto done;
874 }
875
876 length = write_value(channel, start, value, vlen, opdu,
877 channel->mtu);
878 break;
879 case ATT_OP_WRITE_CMD:
880 length = dec_write_cmd(ipdu, len, &start, value, &vlen);
881 if (length > 0)
882 write_value(channel, start, value, vlen, opdu,
883 channel->mtu);
884 return;
885 case ATT_OP_FIND_BY_TYPE_REQ:
886 length = dec_find_by_type_req(ipdu, len, &start, &end,
887 &uuid, value, &vlen);
888 if (length == 0) {
889 status = ATT_ECODE_INVALID_PDU;
890 goto done;
891 }
892
893 length = find_by_type(start, end, &uuid, value, vlen,
894 opdu, channel->mtu);
895 break;
896 case ATT_OP_HANDLE_CNF:
897 return;
898 case ATT_OP_READ_MULTI_REQ:
899 case ATT_OP_PREP_WRITE_REQ:
900 case ATT_OP_EXEC_WRITE_REQ:
901 default:
902 DBG("Unsupported request 0x%02x", ipdu[0]);
903 status = ATT_ECODE_REQ_NOT_SUPP;
904 goto done;
905 }
906
907 if (length == 0)
908 status = ATT_ECODE_IO;
909
910 done:
911 if (status)
912 length = enc_error_resp(ipdu[0], 0x0000, status, opdu,
913 channel->mtu);
914
915 g_attrib_send(channel->attrib, 0, opdu[0], opdu, length,
916 NULL, NULL, NULL);
917 }
918
connect_event(GIOChannel * io,GError * err,void * user_data)919 static void connect_event(GIOChannel *io, GError *err, void *user_data)
920 {
921 struct gatt_channel *channel;
922 uint16_t cid;
923 GError *gerr = NULL;
924
925 if (err) {
926 error("%s", err->message);
927 return;
928 }
929
930 channel = g_new0(struct gatt_channel, 1);
931
932 bt_io_get(io, BT_IO_L2CAP, &gerr,
933 BT_IO_OPT_SOURCE_BDADDR, &channel->src,
934 BT_IO_OPT_DEST_BDADDR, &channel->dst,
935 BT_IO_OPT_CID, &cid,
936 BT_IO_OPT_OMTU, &channel->mtu,
937 BT_IO_OPT_INVALID);
938 if (gerr) {
939 error("bt_io_get: %s", gerr->message);
940 g_error_free(gerr);
941 g_free(channel);
942 g_io_channel_shutdown(io, TRUE, NULL);
943 return;
944 }
945
946 if (channel->mtu > ATT_MAX_MTU)
947 channel->mtu = ATT_MAX_MTU;
948
949 if (cid != ATT_CID)
950 channel->le = FALSE;
951 else
952 channel->le = TRUE;
953
954 channel->attrib = g_attrib_new(io);
955 g_io_channel_unref(io);
956
957 channel->id = g_attrib_register(channel->attrib, GATTRIB_ALL_EVENTS,
958 channel_handler, channel, NULL);
959
960 g_attrib_set_disconnect_function(channel->attrib, channel_disconnect,
961 channel);
962
963 clients = g_slist_append(clients, channel);
964 }
965
confirm_event(GIOChannel * io,void * user_data)966 static void confirm_event(GIOChannel *io, void *user_data)
967 {
968 GError *gerr = NULL;
969
970 if (bt_io_accept(io, connect_event, user_data, NULL, &gerr) == FALSE) {
971 error("bt_io_accept: %s", gerr->message);
972 g_error_free(gerr);
973 g_io_channel_unref(io);
974 }
975
976 return;
977 }
978
attrib_notify_clients(struct attribute * attr)979 static void attrib_notify_clients(struct attribute *attr)
980 {
981 guint handle = attr->handle;
982 GSList *l;
983
984 for (l = clients; l; l = l->next) {
985 struct gatt_channel *channel = l->data;
986
987 /* Notification */
988 if (g_slist_find_custom(channel->notify,
989 GUINT_TO_POINTER(handle), handle_cmp)) {
990 uint8_t pdu[ATT_MAX_MTU];
991 uint16_t len;
992
993 len = enc_notification(attr, pdu, channel->mtu);
994 if (len == 0)
995 continue;
996
997 g_attrib_send(channel->attrib, 0, pdu[0], pdu, len,
998 NULL, NULL, NULL);
999 }
1000
1001 /* Indication */
1002 if (g_slist_find_custom(channel->indicate,
1003 GUINT_TO_POINTER(handle), handle_cmp)) {
1004 uint8_t pdu[ATT_MAX_MTU];
1005 uint16_t len;
1006
1007 len = enc_indication(attr, pdu, channel->mtu);
1008 if (len == 0)
1009 return;
1010
1011 g_attrib_send(channel->attrib, 0, pdu[0], pdu, len,
1012 NULL, NULL, NULL);
1013 }
1014 }
1015 }
1016
register_core_services(void)1017 static gboolean register_core_services(void)
1018 {
1019 uint8_t atval[256];
1020 bt_uuid_t uuid;
1021 uint16_t appearance = 0x0000;
1022
1023 /* GAP service: primary service definition */
1024 bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
1025 att_put_u16(GENERIC_ACCESS_PROFILE_ID, &atval[0]);
1026 attrib_db_add(0x0001, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 2);
1027
1028 /* GAP service: device name characteristic */
1029 name_handle = 0x0006;
1030 bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
1031 atval[0] = ATT_CHAR_PROPER_READ;
1032 att_put_u16(name_handle, &atval[1]);
1033 att_put_u16(GATT_CHARAC_DEVICE_NAME, &atval[3]);
1034 attrib_db_add(0x0004, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 5);
1035
1036 /* GAP service: device name attribute */
1037 bt_uuid16_create(&uuid, GATT_CHARAC_DEVICE_NAME);
1038 attrib_db_add(name_handle, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1039 NULL, 0);
1040
1041 /* GAP service: device appearance characteristic */
1042 appearance_handle = 0x0008;
1043 bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
1044 atval[0] = ATT_CHAR_PROPER_READ;
1045 att_put_u16(appearance_handle, &atval[1]);
1046 att_put_u16(GATT_CHARAC_APPEARANCE, &atval[3]);
1047 attrib_db_add(0x0007, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 5);
1048
1049 /* GAP service: device appearance attribute */
1050 bt_uuid16_create(&uuid, GATT_CHARAC_APPEARANCE);
1051 att_put_u16(appearance, &atval[0]);
1052 attrib_db_add(appearance_handle, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1053 atval, 2);
1054 gap_sdp_handle = attrib_create_sdp(0x0001, "Generic Access Profile");
1055 if (gap_sdp_handle == 0) {
1056 error("Failed to register GAP service record");
1057 goto failed;
1058 }
1059
1060 /* GATT service: primary service definition */
1061 bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
1062 att_put_u16(GENERIC_ATTRIB_PROFILE_ID, &atval[0]);
1063 attrib_db_add(0x0010, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 2);
1064
1065 gatt_sdp_handle = attrib_create_sdp(0x0010,
1066 "Generic Attribute Profile");
1067 if (gatt_sdp_handle == 0) {
1068 error("Failed to register GATT service record");
1069 goto failed;
1070 }
1071
1072 return TRUE;
1073
1074 failed:
1075 if (gap_sdp_handle)
1076 remove_record_from_server(gap_sdp_handle);
1077
1078 return FALSE;
1079 }
1080
attrib_server_init(void)1081 int attrib_server_init(void)
1082 {
1083 GError *gerr = NULL;
1084
1085 /* BR/EDR socket */
1086 l2cap_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
1087 NULL, NULL, &gerr,
1088 BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY,
1089 BT_IO_OPT_PSM, ATT_PSM,
1090 BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
1091 BT_IO_OPT_INVALID);
1092 if (l2cap_io == NULL) {
1093 error("%s", gerr->message);
1094 g_error_free(gerr);
1095 return -1;
1096 }
1097
1098 if (!register_core_services())
1099 goto failed;
1100
1101 if (!main_opts.le)
1102 return 0;
1103
1104 /* LE socket */
1105 le_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
1106 &le_io, NULL, &gerr,
1107 BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY,
1108 BT_IO_OPT_CID, ATT_CID,
1109 BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
1110 BT_IO_OPT_INVALID);
1111 if (le_io == NULL) {
1112 error("%s", gerr->message);
1113 g_error_free(gerr);
1114 /* Doesn't have LE support, continue */
1115 }
1116
1117 return 0;
1118
1119 failed:
1120 g_io_channel_unref(l2cap_io);
1121 l2cap_io = NULL;
1122
1123 if (le_io) {
1124 g_io_channel_unref(le_io);
1125 le_io = NULL;
1126 }
1127
1128 return -1;
1129 }
1130
attrib_server_exit(void)1131 void attrib_server_exit(void)
1132 {
1133 GSList *l;
1134
1135 g_slist_foreach(database, (GFunc) g_free, NULL);
1136 g_slist_free(database);
1137
1138 if (l2cap_io) {
1139 g_io_channel_unref(l2cap_io);
1140 g_io_channel_shutdown(l2cap_io, FALSE, NULL);
1141 }
1142
1143 if (le_io) {
1144 g_io_channel_unref(le_io);
1145 g_io_channel_shutdown(le_io, FALSE, NULL);
1146 }
1147
1148 for (l = clients; l; l = l->next) {
1149 struct gatt_channel *channel = l->data;
1150
1151 g_slist_free(channel->notify);
1152 g_slist_free(channel->indicate);
1153 g_slist_foreach(channel->configs, (GFunc) g_free, NULL);
1154 g_slist_free(channel->configs);
1155
1156 g_attrib_unref(channel->attrib);
1157 g_free(channel);
1158 }
1159
1160 g_slist_free(clients);
1161
1162 if (gatt_sdp_handle)
1163 remove_record_from_server(gatt_sdp_handle);
1164
1165 if (gap_sdp_handle)
1166 remove_record_from_server(gap_sdp_handle);
1167 }
1168
attrib_create_sdp(uint16_t handle,const char * name)1169 uint32_t attrib_create_sdp(uint16_t handle, const char *name)
1170 {
1171 sdp_record_t *record;
1172 struct attribute *a;
1173 uint16_t end = 0;
1174 uuid_t svc, gap_uuid;
1175
1176 a = find_primary_range(handle, &end);
1177
1178 if (a == NULL)
1179 return 0;
1180
1181 if (a->len == 2)
1182 sdp_uuid16_create(&svc, att_get_u16(a->data));
1183 else if (a->len == 16)
1184 sdp_uuid128_create(&svc, a->data);
1185 else
1186 return 0;
1187
1188 record = server_record_new(&svc, handle, end);
1189 if (record == NULL)
1190 return 0;
1191
1192 if (name)
1193 sdp_set_info_attr(record, name, "BlueZ", NULL);
1194
1195 sdp_uuid16_create(&gap_uuid, GENERIC_ACCESS_PROFILE_ID);
1196 if (sdp_uuid_cmp(&svc, &gap_uuid) == 0) {
1197 sdp_set_url_attr(record, "http://www.bluez.org/",
1198 "http://www.bluez.org/",
1199 "http://www.bluez.org/");
1200 }
1201
1202 if (add_record_to_server(BDADDR_ANY, record) < 0)
1203 sdp_record_free(record);
1204 else
1205 return record->handle;
1206
1207 return 0;
1208 }
1209
attrib_free_sdp(uint32_t sdp_handle)1210 void attrib_free_sdp(uint32_t sdp_handle)
1211 {
1212 remove_record_from_server(sdp_handle);
1213 }
1214
attrib_db_find_avail(uint16_t nitems)1215 uint16_t attrib_db_find_avail(uint16_t nitems)
1216 {
1217 uint16_t handle;
1218 GSList *l;
1219
1220 g_assert(nitems > 0);
1221
1222 for (l = database, handle = 0; l; l = l->next) {
1223 struct attribute *a = l->data;
1224
1225 if (handle && (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
1226 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0) &&
1227 a->handle - handle >= nitems)
1228 /* Note: the range above excludes the current handle */
1229 return handle;
1230
1231 if (a->handle == 0xffff)
1232 return 0;
1233
1234 handle = a->handle + 1;
1235 }
1236
1237 if (0xffff - handle + 1 >= nitems)
1238 return handle;
1239
1240 return 0;
1241 }
1242
attrib_db_add(uint16_t handle,bt_uuid_t * uuid,int read_reqs,int write_reqs,const uint8_t * value,int len)1243 struct attribute *attrib_db_add(uint16_t handle, bt_uuid_t *uuid, int read_reqs,
1244 int write_reqs, const uint8_t *value, int len)
1245 {
1246 struct attribute *a;
1247 guint h = handle;
1248
1249 DBG("handle=0x%04x", handle);
1250
1251 if (g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp))
1252 return NULL;
1253
1254 a = g_malloc0(sizeof(struct attribute) + len);
1255 a->handle = handle;
1256 memcpy(&a->uuid, uuid, sizeof(bt_uuid_t));
1257 a->read_reqs = read_reqs;
1258 a->write_reqs = write_reqs;
1259 a->len = len;
1260 memcpy(a->data, value, len);
1261
1262 database = g_slist_insert_sorted(database, a, attribute_cmp);
1263
1264 return a;
1265 }
1266
attrib_db_update(uint16_t handle,bt_uuid_t * uuid,const uint8_t * value,int len,struct attribute ** attr)1267 int attrib_db_update(uint16_t handle, bt_uuid_t *uuid, const uint8_t *value,
1268 int len, struct attribute **attr)
1269 {
1270 struct attribute *a;
1271 GSList *l;
1272 guint h = handle;
1273
1274 DBG("handle=0x%04x", handle);
1275
1276 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
1277 if (!l)
1278 return -ENOENT;
1279
1280 a = g_try_realloc(l->data, sizeof(struct attribute) + len);
1281 if (a == NULL)
1282 return -ENOMEM;
1283
1284 l->data = a;
1285 if (uuid != NULL)
1286 memcpy(&a->uuid, uuid, sizeof(bt_uuid_t));
1287 a->len = len;
1288 memcpy(a->data, value, len);
1289
1290 attrib_notify_clients(a);
1291
1292 if (attr)
1293 *attr = a;
1294
1295 return 0;
1296 }
1297
attrib_db_del(uint16_t handle)1298 int attrib_db_del(uint16_t handle)
1299 {
1300 struct attribute *a;
1301 GSList *l;
1302 guint h = handle;
1303
1304 DBG("handle=0x%04x", handle);
1305
1306 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
1307 if (!l)
1308 return -ENOENT;
1309
1310 a = l->data;
1311 database = g_slist_remove(database, a);
1312 g_free(a);
1313
1314 return 0;
1315 }
1316
attrib_gap_set(uint16_t uuid,const uint8_t * value,int len)1317 int attrib_gap_set(uint16_t uuid, const uint8_t *value, int len)
1318 {
1319 uint16_t handle;
1320
1321 /* FIXME: Missing Privacy and Reconnection Address */
1322
1323 switch (uuid) {
1324 case GATT_CHARAC_DEVICE_NAME:
1325 handle = name_handle;
1326 break;
1327 case GATT_CHARAC_APPEARANCE:
1328 handle = appearance_handle;
1329 break;
1330 default:
1331 return -ENOSYS;
1332 }
1333
1334 return attrib_db_update(handle, NULL, value, len, NULL);
1335 }
1336