← Back to branch summary

~ubuntu-branches/ubuntu/wily/bluez/wily

« back to all changes in this revision

Viewing changes to src/attrib-server.c

Import upstream version 4.81

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/attrib-server.c
 
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/sdp.h>
 
37
#include <bluetooth/sdp_lib.h>
 
38
 
 
39
#include "log.h"
 
40
#include "glib-helper.h"
 
41
#include "btio.h"
 
42
#include "sdpd.h"
 
43
#include "hcid.h"
 
44
#include "att.h"
 
45
#include "gattrib.h"
 
46
 
 
47
#include "attrib-server.h"
 
48
 
 
49
#define GATT_PSM 0x1f
 
50
#define GATT_CID 4
 
51
 
 
52
static GSList *database = NULL;
 
53
 
 
54
struct gatt_channel {
 
55
        bdaddr_t src;
 
56
        bdaddr_t dst;
 
57
        GAttrib *attrib;
 
58
        guint mtu;
 
59
        guint id;
 
60
};
 
61
 
 
62
struct group_elem {
 
63
        uint16_t handle;
 
64
        uint16_t end;
 
65
        uint8_t *data;
 
66
        uint16_t len;
 
67
};
 
68
 
 
69
static GIOChannel *l2cap_io = NULL;
 
70
static GIOChannel *le_io = NULL;
 
71
static GSList *clients = NULL;
 
72
static uint32_t sdp_handle = 0;
 
73
 
 
74
static uuid_t prim_uuid = { .type = SDP_UUID16, .value.uuid16 = GATT_PRIM_SVC_UUID };
 
75
static uuid_t snd_uuid = { .type = SDP_UUID16, .value.uuid16 = GATT_SND_SVC_UUID };
 
76
 
 
77
static sdp_record_t *server_record_new(void)
 
78
{
 
79
        sdp_list_t *svclass_id, *apseq, *proto[2], *profiles, *root, *aproto;
 
80
        uuid_t root_uuid, proto_uuid, gatt_uuid, l2cap;
 
81
        sdp_profile_desc_t profile;
 
82
        sdp_record_t *record;
 
83
        sdp_data_t *psm, *sh, *eh;
 
84
        uint16_t lp = GATT_PSM, start = 0x0001, end = 0xffff;
 
85
 
 
86
        record = sdp_record_alloc();
 
87
        if (record == NULL)
 
88
                return NULL;
 
89
 
 
90
        sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
 
91
        root = sdp_list_append(NULL, &root_uuid);
 
92
        sdp_set_browse_groups(record, root);
 
93
        sdp_list_free(root, NULL);
 
94
 
 
95
        sdp_uuid16_create(&gatt_uuid, GENERIC_ATTRIB_SVCLASS_ID);
 
96
        svclass_id = sdp_list_append(NULL, &gatt_uuid);
 
97
        sdp_set_service_classes(record, svclass_id);
 
98
        sdp_list_free(svclass_id, NULL);
 
99
 
 
100
        sdp_uuid16_create(&profile.uuid, GENERIC_ATTRIB_PROFILE_ID);
 
101
        profile.version = 0x0100;
 
102
        profiles = sdp_list_append(NULL, &profile);
 
103
        sdp_set_profile_descs(record, profiles);
 
104
        sdp_list_free(profiles, NULL);
 
105
 
 
106
        sdp_uuid16_create(&l2cap, L2CAP_UUID);
 
107
        proto[0] = sdp_list_append(NULL, &l2cap);
 
108
        psm = sdp_data_alloc(SDP_UINT16, &lp);
 
109
        proto[0] = sdp_list_append(proto[0], psm);
 
110
        apseq = sdp_list_append(NULL, proto[0]);
 
111
 
 
112
        sdp_uuid16_create(&proto_uuid, ATT_UUID);
 
113
        proto[1] = sdp_list_append(NULL, &proto_uuid);
 
114
        sh = sdp_data_alloc(SDP_UINT16, &start);
 
115
        proto[1] = sdp_list_append(proto[1], sh);
 
116
        eh = sdp_data_alloc(SDP_UINT16, &end);
 
117
        proto[1] = sdp_list_append(proto[1], eh);
 
118
        apseq = sdp_list_append(apseq, proto[1]);
 
119
 
 
120
        aproto = sdp_list_append(NULL, apseq);
 
121
        sdp_set_access_protos(record, aproto);
 
122
 
 
123
        sdp_set_info_attr(record, "Generic Attribute Profile", "BlueZ", NULL);
 
124
 
 
125
        sdp_set_url_attr(record, "http://www.bluez.org/",
 
126
                        "http://www.bluez.org/", "http://www.bluez.org/");
 
127
 
 
128
        sdp_set_service_id(record, gatt_uuid);
 
129
 
 
130
        sdp_data_free(psm);
 
131
        sdp_data_free(sh);
 
132
        sdp_data_free(eh);
 
133
        sdp_list_free(proto[0], NULL);
 
134
        sdp_list_free(proto[1], NULL);
 
135
        sdp_list_free(apseq, NULL);
 
136
        sdp_list_free(aproto, NULL);
 
137
 
 
138
        return record;
 
139
}
 
140
 
 
141
static uint16_t read_by_group(uint16_t start, uint16_t end, uuid_t *uuid,
 
142
                                                        uint8_t *pdu, int len)
 
143
{
 
144
        struct att_data_list *adl;
 
145
        struct attribute *a;
 
146
        struct group_elem *cur, *old = NULL;
 
147
        GSList *l, *groups;
 
148
        uint16_t length, last_handle, last_size = 0;
 
149
        int i;
 
150
 
 
151
        if (start > end || start == 0x0000)
 
152
                return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
 
153
                                        ATT_ECODE_INVALID_HANDLE, pdu, len);
 
154
 
 
155
        /*
 
156
         * Only <<Primary Service>> and <<Secondary Service>> grouping
 
157
         * types may be used in the Read By Group Type Request.
 
158
         */
 
159
 
 
160
        if (sdp_uuid_cmp(uuid, &prim_uuid) != 0 &&
 
161
                sdp_uuid_cmp(uuid, &snd_uuid) != 0)
 
162
                return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, 0x0000,
 
163
                                        ATT_ECODE_UNSUPP_GRP_TYPE, pdu, len);
 
164
 
 
165
        last_handle = end;
 
166
        for (l = database, groups = NULL; l; l = l->next) {
 
167
                a = l->data;
 
168
 
 
169
                if (a->handle < start)
 
170
                        continue;
 
171
 
 
172
                if (a->handle >= end)
 
173
                        break;
 
174
 
 
175
                /* The old group ends when a new one starts */
 
176
                if (old && (sdp_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 
177
                                sdp_uuid_cmp(&a->uuid, &snd_uuid) == 0)) {
 
178
                        old->end = last_handle;
 
179
                        old = NULL;
 
180
                }
 
181
 
 
182
                if (sdp_uuid_cmp(&a->uuid, uuid) != 0) {
 
183
                        /* Still inside a service, update its last handle */
 
184
                        if (old)
 
185
                                last_handle = a->handle;
 
186
                        continue;
 
187
                }
 
188
 
 
189
                if (last_size && (last_size != a->len))
 
190
                        break;
 
191
 
 
192
                cur = g_new0(struct group_elem, 1);
 
193
                cur->handle = a->handle;
 
194
                cur->data = a->data;
 
195
                cur->len = a->len;
 
196
 
 
197
                /* Attribute Grouping Type found */
 
198
                groups = g_slist_append(groups, cur);
 
199
 
 
200
                last_size = a->len;
 
201
                old = cur;
 
202
        }
 
203
 
 
204
        if (groups == NULL)
 
205
                return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
 
206
                                        ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 
207
 
 
208
        if (l == NULL)
 
209
                cur->end = a->handle;
 
210
        else
 
211
                cur->end = last_handle;
 
212
 
 
213
        length = g_slist_length(groups);
 
214
 
 
215
        adl = g_new0(struct att_data_list, 1);
 
216
        adl->len = last_size + 4;       /* Length of each element */
 
217
        adl->num = length;      /* Number of primary or secondary services */
 
218
        adl->data = g_malloc(length * sizeof(uint8_t *));
 
219
 
 
220
        for (i = 0, l = groups; l; l = l->next, i++) {
 
221
                uint8_t *value;
 
222
 
 
223
                cur = l->data;
 
224
 
 
225
                adl->data[i] = g_malloc(adl->len);
 
226
                value = (void *) adl->data[i];
 
227
 
 
228
                att_put_u16(cur->handle, value);
 
229
                att_put_u16(cur->end, &value[2]);
 
230
                /* Attribute Value */
 
231
                memcpy(&value[4], cur->data, cur->len);
 
232
        }
 
233
 
 
234
        length = enc_read_by_grp_resp(adl, pdu, len);
 
235
 
 
236
        att_data_list_free(adl);
 
237
        g_slist_foreach(groups, (GFunc) g_free, NULL);
 
238
        g_slist_free(groups);
 
239
 
 
240
        return length;
 
241
}
 
242
 
 
243
static uint16_t read_by_type(uint16_t start, uint16_t end, uuid_t *uuid,
 
244
                                                        uint8_t *pdu, int len)
 
245
{
 
246
        struct att_data_list *adl;
 
247
        GSList *l, *types;
 
248
        struct attribute *a;
 
249
        uint16_t num, length;
 
250
        int i;
 
251
 
 
252
        if (start > end || start == 0x0000)
 
253
                return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
 
254
                                        ATT_ECODE_INVALID_HANDLE, pdu, len);
 
255
 
 
256
        for (l = database, length = 0, types = NULL; l; l = l->next) {
 
257
                a = l->data;
 
258
 
 
259
                if (a->handle < start)
 
260
                        continue;
 
261
 
 
262
                if (a->handle >= end)
 
263
                        break;
 
264
 
 
265
                if (sdp_uuid_cmp(&a->uuid, uuid)  != 0)
 
266
                        continue;
 
267
 
 
268
                /* All elements must have the same length */
 
269
                if (length == 0)
 
270
                        length = a->len;
 
271
                else if (a->len != length)
 
272
                        break;
 
273
 
 
274
                types = g_slist_append(types, a);
 
275
        }
 
276
 
 
277
        if (types == NULL)
 
278
                return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
 
279
                                        ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 
280
 
 
281
        num = g_slist_length(types);
 
282
 
 
283
        /* Handle length plus attribute value length */
 
284
        length += 2;
 
285
 
 
286
        adl = g_new0(struct att_data_list, 1);
 
287
        adl->len = length;      /* Length of each element */
 
288
        adl->num = num;         /* Number of primary or secondary services */
 
289
        adl->data = g_malloc(num * sizeof(uint8_t *));
 
290
 
 
291
        for (i = 0, l = types; l; i++, l = l->next) {
 
292
                uint8_t *value;
 
293
 
 
294
                a = l->data;
 
295
                adl->data[i] = g_malloc(length);
 
296
 
 
297
                value = (void *) adl->data[i];
 
298
 
 
299
                att_put_u16(a->handle, value);
 
300
 
 
301
                /* Attribute Value */
 
302
                memcpy(&value[2], a->data, a->len);
 
303
        }
 
304
 
 
305
        length = enc_read_by_type_resp(adl, pdu, len);
 
306
 
 
307
        att_data_list_free(adl);
 
308
        g_slist_free(types);
 
309
 
 
310
        return length;
 
311
}
 
312
 
 
313
static int find_info(uint16_t start, uint16_t end, uint8_t *pdu, int len)
 
314
{
 
315
        struct attribute *a;
 
316
        struct att_data_list *adl;
 
317
        GSList *l, *info;
 
318
        uint8_t format, last_type = SDP_UUID_UNSPEC;
 
319
        uint16_t length, num;
 
320
        int i;
 
321
 
 
322
        if (start > end || start == 0x0000)
 
323
                return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
 
324
                                        ATT_ECODE_INVALID_HANDLE, pdu, len);
 
325
 
 
326
        for (l = database, info = NULL, num = 0; l; l = l->next) {
 
327
                a = l->data;
 
328
 
 
329
                if (a->handle < start)
 
330
                        continue;
 
331
 
 
332
                if (a->handle > end)
 
333
                        break;
 
334
 
 
335
                if (last_type == SDP_UUID_UNSPEC)
 
336
                        last_type = a->uuid.type;
 
337
 
 
338
                if (a->uuid.type != last_type)
 
339
                        break;
 
340
 
 
341
                info = g_slist_append(info, a);
 
342
                num++;
 
343
 
 
344
                last_type = a->uuid.type;
 
345
        }
 
346
 
 
347
        if (info == NULL)
 
348
                return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
 
349
                                        ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 
350
 
 
351
        if (last_type == SDP_UUID16) {
 
352
                length = 2;
 
353
                format = 0x01;
 
354
        } else if (last_type == SDP_UUID128) {
 
355
                length = 16;
 
356
                format = 0x02;
 
357
        }
 
358
 
 
359
        adl = g_new0(struct att_data_list, 1);
 
360
        adl->len = length + 2;  /* Length of each element */
 
361
        adl->num = num;         /* Number of primary or secondary services */
 
362
        adl->data = g_malloc(num * sizeof(uint8_t *));
 
363
 
 
364
        for (i = 0, l = info; l; i++, l = l->next) {
 
365
                uint8_t *value;
 
366
 
 
367
                a = l->data;
 
368
                adl->data[i] = g_malloc(adl->len);
 
369
 
 
370
                value = (void *) adl->data[i];
 
371
 
 
372
                att_put_u16(a->handle, value);
 
373
 
 
374
                /* Attribute Value */
 
375
                memcpy(&value[2], &a->uuid.value, length);
 
376
        }
 
377
 
 
378
        length = enc_find_info_resp(format, adl, pdu, len);
 
379
 
 
380
        att_data_list_free(adl);
 
381
        g_slist_free(info);
 
382
 
 
383
        return length;
 
384
}
 
385
 
 
386
static int find_by_type(uint16_t start, uint16_t end, uuid_t *uuid,
 
387
                        const uint8_t *value, int vlen, uint8_t *opdu, int mtu)
 
388
{
 
389
        struct attribute *a;
 
390
        struct att_range *range;
 
391
        GSList *l, *matches;
 
392
        int len;
 
393
 
 
394
        if (start > end || start == 0x0000)
 
395
                return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
 
396
                                        ATT_ECODE_INVALID_HANDLE, opdu, mtu);
 
397
 
 
398
        /* Searching first requested handle number */
 
399
        for (l = database, matches = NULL, range = NULL; l; l = l->next) {
 
400
                a = l->data;
 
401
 
 
402
                if (a->handle < start)
 
403
                        continue;
 
404
 
 
405
                if (a->handle > end)
 
406
                        break;
 
407
 
 
408
                /* Primary service? Attribute value matches? */
 
409
                if ((sdp_uuid_cmp(&a->uuid, uuid) == 0) && (a->len == vlen) &&
 
410
                                        (memcmp(a->data, value, vlen) == 0)) {
 
411
 
 
412
                        range = g_new0(struct att_range, 1);
 
413
                        range->start = a->handle;
 
414
 
 
415
                        matches = g_slist_append(matches, range);
 
416
                } else if (range) {
 
417
                        /*
 
418
                         * Update the last found handle or reset the pointer
 
419
                         * to track that a new group started: Primary or
 
420
                         * Secondary service.
 
421
                         */
 
422
                        if (sdp_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 
423
                                        sdp_uuid_cmp(&a->uuid, &snd_uuid) == 0)
 
424
                                range = NULL;
 
425
                        else
 
426
                                range->end = a->handle;
 
427
                }
 
428
        }
 
429
 
 
430
        if (range) {
 
431
                if (l == NULL) {
 
432
                        /* Avoids another iteration */
 
433
                        range->end = 0xFFFF;
 
434
                } else if (range->end == 0) {
 
435
                        /*
 
436
                         * Broken requests: requested End Handle is not 0xFFFF.
 
437
                         * Given handle is in the middle of a service definition.
 
438
                         */
 
439
                        matches = g_slist_remove(matches, range);
 
440
                        g_free(range);
 
441
                }
 
442
        }
 
443
 
 
444
        if (matches == NULL)
 
445
                return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
 
446
                                ATT_ECODE_ATTR_NOT_FOUND, opdu, mtu);
 
447
 
 
448
        len = enc_find_by_type_resp(matches, opdu, mtu);
 
449
 
 
450
        g_slist_foreach(matches, (GFunc) g_free, NULL);
 
451
        g_slist_free(matches);
 
452
 
 
453
        return len;
 
454
}
 
455
 
 
456
static int handle_cmp(gconstpointer a, gconstpointer b)
 
457
{
 
458
        const struct attribute *attrib = a;
 
459
        uint16_t handle = GPOINTER_TO_UINT(b);
 
460
 
 
461
        return attrib->handle - handle;
 
462
}
 
463
 
 
464
static int attribute_cmp(gconstpointer a1, gconstpointer a2)
 
465
{
 
466
        const struct attribute *attrib1 = a1;
 
467
        const struct attribute *attrib2 = a2;
 
468
 
 
469
        return attrib1->handle - attrib2->handle;
 
470
}
 
471
 
 
472
static uint16_t read_value(uint16_t handle, uint8_t *pdu, int len)
 
473
{
 
474
        struct attribute *a;
 
475
        GSList *l;
 
476
        guint h = handle;
 
477
 
 
478
        l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 
479
        if (!l)
 
480
                return enc_error_resp(ATT_OP_READ_REQ, handle,
 
481
                                        ATT_ECODE_INVALID_HANDLE, pdu, len);
 
482
 
 
483
        a = l->data;
 
484
 
 
485
        return enc_read_resp(a->data, a->len, pdu, len);
 
486
}
 
487
 
 
488
static void write_value(uint16_t handle, const uint8_t *value, int vlen)
 
489
{
 
490
        struct attribute *a;
 
491
        GSList *l;
 
492
        guint h = handle;
 
493
        uuid_t uuid;
 
494
 
 
495
        l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 
496
        if (!l)
 
497
                return;
 
498
 
 
499
        a = l->data;
 
500
        memcpy(&uuid, &a->uuid, sizeof(uuid_t));
 
501
        attrib_db_update(handle, &uuid, value, vlen);
 
502
}
 
503
 
 
504
static uint16_t mtu_exchange(struct gatt_channel *channel, uint16_t mtu,
 
505
                uint8_t *pdu, int len)
 
506
{
 
507
        channel->mtu = MIN(mtu, ATT_MAX_MTU);
 
508
 
 
509
        return enc_mtu_resp(channel->mtu, pdu, len);
 
510
}
 
511
 
 
512
static void channel_disconnect(void *user_data)
 
513
{
 
514
        struct gatt_channel *channel = user_data;
 
515
 
 
516
        g_attrib_unref(channel->attrib);
 
517
        clients = g_slist_remove(clients, channel);
 
518
 
 
519
        g_free(channel);
 
520
}
 
521
 
 
522
static void channel_handler(const uint8_t *ipdu, uint16_t len,
 
523
                                                        gpointer user_data)
 
524
{
 
525
        struct gatt_channel *channel = user_data;
 
526
        uint8_t opdu[ATT_MAX_MTU], value[ATT_MAX_MTU];
 
527
        uint16_t length, start, end, mtu;
 
528
        uuid_t uuid;
 
529
        uint8_t status = 0;
 
530
        int vlen;
 
531
 
 
532
        switch(ipdu[0]) {
 
533
        case ATT_OP_READ_BY_GROUP_REQ:
 
534
                length = dec_read_by_grp_req(ipdu, len, &start, &end, &uuid);
 
535
                if (length == 0) {
 
536
                        status = ATT_ECODE_INVALID_PDU;
 
537
                        goto done;
 
538
                }
 
539
 
 
540
                length = read_by_group(start, end, &uuid, opdu, channel->mtu);
 
541
                break;
 
542
        case ATT_OP_READ_BY_TYPE_REQ:
 
543
                length = dec_read_by_type_req(ipdu, len, &start, &end, &uuid);
 
544
                if (length == 0) {
 
545
                        status = ATT_ECODE_INVALID_PDU;
 
546
                        goto done;
 
547
                }
 
548
 
 
549
                length = read_by_type(start, end, &uuid, opdu, channel->mtu);
 
550
                break;
 
551
        case ATT_OP_READ_REQ:
 
552
                length = dec_read_req(ipdu, len, &start);
 
553
                if (length == 0) {
 
554
                        status = ATT_ECODE_INVALID_PDU;
 
555
                        goto done;
 
556
                }
 
557
 
 
558
                length = read_value(start, opdu, channel->mtu);
 
559
                break;
 
560
        case ATT_OP_MTU_REQ:
 
561
                length = dec_mtu_req(ipdu, len, &mtu);
 
562
                if (length == 0) {
 
563
                        status = ATT_ECODE_INVALID_PDU;
 
564
                        goto done;
 
565
                }
 
566
 
 
567
                length = mtu_exchange(channel, mtu, opdu, channel->mtu);
 
568
                break;
 
569
        case ATT_OP_FIND_INFO_REQ:
 
570
                length = dec_find_info_req(ipdu, len, &start, &end);
 
571
                if (length == 0) {
 
572
                        status = ATT_ECODE_INVALID_PDU;
 
573
                        goto done;
 
574
                }
 
575
 
 
576
                length = find_info(start, end, opdu, channel->mtu);
 
577
                break;
 
578
        case ATT_OP_WRITE_REQ:
 
579
                length = dec_write_req(ipdu, len, &start, value, &vlen);
 
580
                if (length == 0) {
 
581
                        status = ATT_ECODE_INVALID_PDU;
 
582
                        goto done;
 
583
                }
 
584
 
 
585
                write_value(start, value, vlen);
 
586
                opdu[0] = ATT_OP_WRITE_RESP;
 
587
                length = sizeof(opdu[0]);
 
588
                break;
 
589
        case ATT_OP_WRITE_CMD:
 
590
                length = dec_write_cmd(ipdu, len, &start, value, &vlen);
 
591
                if (length > 0)
 
592
                        write_value(start, value, vlen);
 
593
                return;
 
594
        case ATT_OP_FIND_BY_TYPE_REQ:
 
595
                length = dec_find_by_type_req(ipdu, len, &start, &end,
 
596
                                                        &uuid, value, &vlen);
 
597
                if (length == 0) {
 
598
                        status = ATT_ECODE_INVALID_PDU;
 
599
                        goto done;
 
600
                }
 
601
 
 
602
                length = find_by_type(start, end, &uuid, value, vlen,
 
603
                                                        opdu, channel->mtu);
 
604
                break;
 
605
        case ATT_OP_READ_BLOB_REQ:
 
606
        case ATT_OP_READ_MULTI_REQ:
 
607
        case ATT_OP_PREP_WRITE_REQ:
 
608
        case ATT_OP_EXEC_WRITE_REQ:
 
609
        default:
 
610
                status = ATT_ECODE_REQ_NOT_SUPP;
 
611
                goto done;
 
612
        }
 
613
 
 
614
        if (length == 0)
 
615
                status = ATT_ECODE_IO;
 
616
 
 
617
done:
 
618
        if (status)
 
619
                length = enc_error_resp(ipdu[0], 0x0000, status, opdu, channel->mtu);
 
620
 
 
621
        g_attrib_send(channel->attrib, opdu[0], opdu, length,
 
622
                                                        NULL, NULL, NULL);
 
623
}
 
624
 
 
625
static void connect_event(GIOChannel *io, GError *err, void *user_data)
 
626
{
 
627
        struct gatt_channel *channel;
 
628
        GError *gerr = NULL;
 
629
 
 
630
        if (err) {
 
631
                error("%s", err->message);
 
632
                return;
 
633
        }
 
634
 
 
635
        channel = g_new0(struct gatt_channel, 1);
 
636
 
 
637
        bt_io_get(io, BT_IO_L2CAP, &gerr,
 
638
                        BT_IO_OPT_SOURCE_BDADDR, &channel->src,
 
639
                        BT_IO_OPT_DEST_BDADDR, &channel->dst,
 
640
                        BT_IO_OPT_INVALID);
 
641
        if (gerr) {
 
642
                error("bt_io_get: %s", gerr->message);
 
643
                g_error_free(gerr);
 
644
                g_free(channel);
 
645
                g_io_channel_shutdown(io, TRUE, NULL);
 
646
                return;
 
647
        }
 
648
 
 
649
        channel->attrib = g_attrib_new(io);
 
650
        channel->mtu = ATT_DEFAULT_MTU;
 
651
 
 
652
        channel->id = g_attrib_register(channel->attrib, GATTRIB_ALL_EVENTS,
 
653
                                channel_handler, channel, NULL);
 
654
 
 
655
        g_attrib_set_disconnect_function(channel->attrib, channel_disconnect,
 
656
                                                                channel);
 
657
 
 
658
        clients = g_slist_append(clients, channel);
 
659
}
 
660
 
 
661
static void confirm_event(GIOChannel *io, void *user_data)
 
662
{
 
663
        GError *gerr = NULL;
 
664
 
 
665
        if (bt_io_accept(io, connect_event, NULL, NULL, &gerr) == FALSE) {
 
666
                error("bt_io_accept: %s", gerr->message);
 
667
                g_error_free(gerr);
 
668
                g_io_channel_unref(io);
 
669
        }
 
670
 
 
671
        return;
 
672
}
 
673
 
 
674
int attrib_server_init(void)
 
675
{
 
676
        GError *gerr = NULL;
 
677
        sdp_record_t *record;
 
678
 
 
679
        /* BR/EDR socket */
 
680
        l2cap_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
 
681
                                        NULL, NULL, &gerr,
 
682
                                        BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY,
 
683
                                        BT_IO_OPT_PSM, GATT_PSM,
 
684
                                        BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
 
685
                                        BT_IO_OPT_INVALID);
 
686
 
 
687
        if (l2cap_io == NULL) {
 
688
                error("%s", gerr->message);
 
689
                g_error_free(gerr);
 
690
                return -1;
 
691
        }
 
692
 
 
693
        record = server_record_new();
 
694
        if (record == NULL) {
 
695
                error("Unable to create GATT service record");
 
696
                goto failed;
 
697
        }
 
698
 
 
699
        if (add_record_to_server(BDADDR_ANY, record) < 0) {
 
700
                error("Failed to register GATT service record");
 
701
                sdp_record_free(record);
 
702
                goto failed;
 
703
        }
 
704
 
 
705
        sdp_handle = record->handle;
 
706
 
 
707
        if (!main_opts.le)
 
708
                return 0;
 
709
 
 
710
        /* LE socket */
 
711
        le_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
 
712
                                        NULL, NULL, &gerr,
 
713
                                        BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY,
 
714
                                        BT_IO_OPT_CID, GATT_CID,
 
715
                                        BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
 
716
                                        BT_IO_OPT_INVALID);
 
717
 
 
718
        if (le_io == NULL) {
 
719
                error("%s", gerr->message);
 
720
                g_error_free(gerr);
 
721
                /* Doesn't have LE support, continue */
 
722
        }
 
723
 
 
724
        return 0;
 
725
 
 
726
failed:
 
727
        g_io_channel_unref(l2cap_io);
 
728
        l2cap_io = NULL;
 
729
 
 
730
        return -1;
 
731
}
 
732
 
 
733
void attrib_server_exit(void)
 
734
{
 
735
        GSList *l;
 
736
 
 
737
        g_slist_foreach(database, (GFunc) g_free, NULL);
 
738
        g_slist_free(database);
 
739
 
 
740
        if (l2cap_io) {
 
741
                g_io_channel_unref(l2cap_io);
 
742
                g_io_channel_shutdown(l2cap_io, FALSE, NULL);
 
743
        }
 
744
 
 
745
        if (le_io) {
 
746
                g_io_channel_unref(le_io);
 
747
                g_io_channel_shutdown(le_io, FALSE, NULL);
 
748
        }
 
749
 
 
750
        for (l = clients; l; l = l->next) {
 
751
                struct gatt_channel *channel = l->data;
 
752
 
 
753
                g_attrib_unref(channel->attrib);
 
754
                g_free(channel);
 
755
        }
 
756
 
 
757
        g_slist_free(clients);
 
758
 
 
759
        if (sdp_handle)
 
760
                remove_record_from_server(sdp_handle);
 
761
}
 
762
 
 
763
int attrib_db_add(uint16_t handle, uuid_t *uuid, const uint8_t *value, int len)
 
764
{
 
765
        struct attribute *a;
 
766
 
 
767
        /* FIXME: handle conflicts */
 
768
 
 
769
        a = g_malloc0(sizeof(struct attribute) + len);
 
770
        a->handle = handle;
 
771
        memcpy(&a->uuid, uuid, sizeof(uuid_t));
 
772
        a->len = len;
 
773
        memcpy(a->data, value, len);
 
774
 
 
775
        database = g_slist_insert_sorted(database, a, attribute_cmp);
 
776
 
 
777
        return 0;
 
778
}
 
779
 
 
780
int attrib_db_update(uint16_t handle, uuid_t *uuid, const uint8_t *value,
 
781
                                                                int len)
 
782
{
 
783
        struct attribute *a;
 
784
        GSList *l;
 
785
        guint h = handle;
 
786
 
 
787
        l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 
788
        if (!l)
 
789
                return -ENOENT;
 
790
 
 
791
        a = g_try_realloc(l->data, sizeof(struct attribute) + len);
 
792
        if (a == NULL)
 
793
                return -ENOMEM;
 
794
 
 
795
        l->data = a;
 
796
        a->handle = handle;
 
797
        memcpy(&a->uuid, uuid, sizeof(uuid_t));
 
798
        a->len = len;
 
799
        memcpy(a->data, value, len);
 
800
 
 
801
        /*
 
802
         * <<Client/Server Characteristic Configuration>> descriptors are
 
803
         * not supported yet. If a given attribute changes, the attribute
 
804
         * server shall report the new values using the mechanism selected
 
805
         * by the client. Notification/Indication shall not be automatically
 
806
         * sent if the client didn't request them.
 
807
         */
 
808
 
 
809
        return 0;
 
810
}
 
811
 
 
812
int attrib_db_del(uint16_t handle)
 
813
{
 
814
        struct attribute *a;
 
815
        GSList *l;
 
816
        guint h = handle;
 
817
 
 
818
        l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 
819
        if (!l)
 
820
                return -ENOENT;
 
821
 
 
822
        a = l->data;
 
823
        database = g_slist_remove(database, a);
 
824
        g_free(a);
 
825
 
 
826
        return 0;
 
827
}