← Back to branch summary

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

« back to all changes in this revision

Viewing changes to test/ipctest.c

Import upstream version 4.81

Show diffs side-by-side

added added

removed removed

Lines of Context:
test/ipctest.c
 
1
/*
 
2
 *
 
3
 *  BlueZ - Bluetooth protocol stack for Linux
 
4
 *
 
5
 *  Copyright (C) 2006-2010  Nokia Corporation
 
6
 *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
 
7
 *  Copyright (C) 2009  Lennart Poettering
 
8
 *  Copyright (C) 2008  Joao Paulo Rechi Vita
 
9
 *
 
10
 *
 
11
 *  This library is free software; you can redistribute it and/or
 
12
 *  modify it under the terms of the GNU Lesser General Public
 
13
 *  License as published by the Free Software Foundation; either
 
14
 *  version 2.1 of the License, or (at your option) any later version.
 
15
 *
 
16
 *  This library is distributed in the hope that it will be useful,
 
17
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 
18
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 
19
 *  Lesser General Public License for more details.
 
20
 *
 
21
 *  You should have received a copy of the GNU Lesser General Public
 
22
 *  License along with this library; if not, write to the Free Software
 
23
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
24
 *
 
25
 */
 
26
 
 
27
#ifdef HAVE_CONFIG_H
 
28
#include <config.h>
 
29
#endif
 
30
 
 
31
#include <stdlib.h>
 
32
#include <stdio.h>
 
33
#include <errno.h>
 
34
#include <string.h>
 
35
#include <assert.h>
 
36
#include <libgen.h>
 
37
#include <unistd.h>
 
38
#include <fcntl.h>
 
39
#include <signal.h>
 
40
 
 
41
#include <glib.h>
 
42
 
 
43
#include "ipc.h"
 
44
#include "sbc.h"
 
45
 
 
46
#define DBG(fmt, arg...)                                \
 
47
        printf("debug %s: " fmt "\n" , __FUNCTION__ , ## arg)
 
48
#define ERR(fmt, arg...)                                \
 
49
        fprintf(stderr, "ERROR %s: " fmt "\n" , __FUNCTION__ , ## arg)
 
50
 
 
51
#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
 
52
 
 
53
#ifndef MIN
 
54
# define MIN(x, y) ((x) < (y) ? (x) : (y))
 
55
#endif
 
56
 
 
57
#ifndef MAX
 
58
# define MAX(x, y) ((x) > (y) ? (x) : (y))
 
59
#endif
 
60
 
 
61
#ifndef TRUE
 
62
# define TRUE (1)
 
63
#endif
 
64
 
 
65
#ifndef FALSE
 
66
# define FALSE (0)
 
67
#endif
 
68
 
 
69
#define YES_NO(t) ((t) ? "yes" : "no")
 
70
 
 
71
#define BUFFER_SIZE 2048
 
72
#define MAX_BITPOOL 64
 
73
#define MIN_BITPOOL 2
 
74
 
 
75
struct a2dp_info {
 
76
        sbc_capabilities_t sbc_capabilities;
 
77
        sbc_t sbc; /* Codec data */
 
78
        int sbc_initialized; /* Keep track if the encoder is initialized */
 
79
        size_t codesize; /* SBC codesize */
 
80
 
 
81
        void* buffer; /* Codec transfer buffer */
 
82
        size_t buffer_size; /* Size of the buffer */
 
83
 
 
84
        uint16_t seq_num; /* Cumulative packet sequence */
 
85
};
 
86
 
 
87
struct hsp_info {
 
88
        pcm_capabilities_t pcm_capabilities;
 
89
};
 
90
 
 
91
struct userdata {
 
92
        int service_fd;
 
93
        int stream_fd;
 
94
        GIOChannel *stream_channel;
 
95
        guint stream_watch;
 
96
        GIOChannel *gin; /* dude, I am thirsty now */
 
97
        guint gin_watch;
 
98
        int transport;
 
99
        uint32_t rate;
 
100
        int channels;
 
101
        char *address;
 
102
        struct a2dp_info a2dp;
 
103
        struct hsp_info hsp;
 
104
        size_t link_mtu;
 
105
        size_t block_size;
 
106
        gboolean debug_stream_read : 1;
 
107
        gboolean debug_stream_write : 1;
 
108
};
 
109
 
 
110
static struct userdata data = {
 
111
        .service_fd = -1,
 
112
        .stream_fd = -1,
 
113
        .transport = BT_CAPABILITIES_TRANSPORT_A2DP,
 
114
        .rate = 48000,
 
115
        .channels = 2,
 
116
        .address = NULL
 
117
};
 
118
 
 
119
static int start_stream(struct userdata *u);
 
120
static int stop_stream(struct userdata *u);
 
121
static gboolean input_cb(GIOChannel *gin, GIOCondition condition, gpointer data);
 
122
 
 
123
static GMainLoop *main_loop;
 
124
 
 
125
static int service_send(struct userdata *u, const bt_audio_msg_header_t *msg)
 
126
{
 
127
        int err;
 
128
        uint16_t length;
 
129
 
 
130
        assert(u);
 
131
 
 
132
        length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE;
 
133
 
 
134
        DBG("sending %s:%s", bt_audio_strtype(msg->type),
 
135
                bt_audio_strname(msg->name));
 
136
 
 
137
        if (send(u->service_fd, msg, length, 0) > 0)
 
138
                err = 0;
 
139
        else {
 
140
                err = -errno;
 
141
                ERR("Error sending data to audio service: %s(%d)",
 
142
                        strerror(errno), errno);
 
143
        }
 
144
 
 
145
        return err;
 
146
}
 
147
 
 
148
static int service_recv(struct userdata *u, bt_audio_msg_header_t *rsp)
 
149
{
 
150
        int err;
 
151
        const char *type, *name;
 
152
        uint16_t length;
 
153
 
 
154
        assert(u);
 
155
 
 
156
        length = rsp->length ? : BT_SUGGESTED_BUFFER_SIZE;
 
157
 
 
158
        DBG("trying to receive msg from audio service...");
 
159
        if (recv(u->service_fd, rsp, length, 0) > 0) {
 
160
                type = bt_audio_strtype(rsp->type);
 
161
                name = bt_audio_strname(rsp->name);
 
162
                if (type && name) {
 
163
                        DBG("Received %s - %s", type, name);
 
164
                        err = 0;
 
165
                } else {
 
166
                        err = -EINVAL;
 
167
                        ERR("Bogus message type %d - name %d"
 
168
                                "received from audio service",
 
169
                                rsp->type, rsp->name);
 
170
                }
 
171
        } else {
 
172
                err = -errno;
 
173
                ERR("Error receiving data from audio service: %s(%d)",
 
174
                        strerror(errno), errno);
 
175
        }
 
176
 
 
177
        return err;
 
178
}
 
179
 
 
180
static ssize_t service_expect(struct userdata *u, bt_audio_msg_header_t *rsp,
 
181
                                uint8_t expected_name)
 
182
{
 
183
        int r;
 
184
 
 
185
        assert(u);
 
186
        assert(u->service_fd >= 0);
 
187
        assert(rsp);
 
188
 
 
189
        if ((r = service_recv(u, rsp)) < 0)
 
190
                return r;
 
191
 
 
192
        if ((rsp->type != BT_INDICATION && rsp->type != BT_RESPONSE) ||
 
193
                        (rsp->name != expected_name)) {
 
194
                if (rsp->type == BT_ERROR && rsp->length == sizeof(bt_audio_error_t))
 
195
                        ERR("Received error condition: %s",
 
196
                                strerror(((bt_audio_error_t*) rsp)->posix_errno));
 
197
                else
 
198
                        ERR("Bogus message %s received while %s was expected",
 
199
                                bt_audio_strname(rsp->name),
 
200
                                bt_audio_strname(expected_name));
 
201
                return -1;
 
202
        }
 
203
 
 
204
        return 0;
 
205
}
 
206
 
 
207
static int init_bt(struct userdata *u)
 
208
{
 
209
        assert(u);
 
210
 
 
211
        if (u->service_fd != -1)
 
212
                return 0;
 
213
 
 
214
        DBG("bt_audio_service_open");
 
215
 
 
216
        u->service_fd = bt_audio_service_open();
 
217
        if (u->service_fd <= 0) {
 
218
                perror(strerror(errno));
 
219
                return errno;
 
220
        }
 
221
 
 
222
        return 0;
 
223
}
 
224
 
 
225
static int parse_caps(struct userdata *u, const struct bt_get_capabilities_rsp *rsp)
 
226
{
 
227
        unsigned char *ptr;
 
228
        uint16_t bytes_left;
 
229
        codec_capabilities_t codec;
 
230
 
 
231
        assert(u);
 
232
        assert(rsp);
 
233
 
 
234
        bytes_left = rsp->h.length - sizeof(*rsp);
 
235
 
 
236
        if (bytes_left < sizeof(codec_capabilities_t)) {
 
237
                ERR("Packet too small to store codec information.");
 
238
                return -1;
 
239
        }
 
240
 
 
241
        ptr = ((void *) rsp) + sizeof(*rsp);
 
242
 
 
243
        memcpy(&codec, ptr, sizeof(codec)); /** ALIGNMENT? **/
 
244
 
 
245
        DBG("Payload size is %lu %lu",
 
246
                (unsigned long) bytes_left, (unsigned long) sizeof(codec));
 
247
 
 
248
        if (u->transport != codec.transport) {
 
249
                ERR("Got capabilities for wrong codec.");
 
250
                return -1;
 
251
        }
 
252
 
 
253
        if (u->transport == BT_CAPABILITIES_TRANSPORT_SCO) {
 
254
 
 
255
                if (bytes_left <= 0 ||
 
256
                                codec.length != sizeof(u->hsp.pcm_capabilities))
 
257
                        return -1;
 
258
 
 
259
                assert(codec.type == BT_HFP_CODEC_PCM);
 
260
 
 
261
                memcpy(&u->hsp.pcm_capabilities,
 
262
                                &codec, sizeof(u->hsp.pcm_capabilities));
 
263
 
 
264
                DBG("Has NREC: %s",
 
265
                        YES_NO(u->hsp.pcm_capabilities.flags & BT_PCM_FLAG_NREC));
 
266
 
 
267
        } else if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
 
268
 
 
269
                while (bytes_left > 0) {
 
270
                        if (codec.type == BT_A2DP_SBC_SINK &&
 
271
                                        !(codec.lock & BT_WRITE_LOCK))
 
272
                                break;
 
273
 
 
274
                        bytes_left -= codec.length;
 
275
                        ptr += codec.length;
 
276
                        memcpy(&codec, ptr, sizeof(codec));
 
277
                }
 
278
 
 
279
                DBG("bytes_left = %d, codec.length = %d",
 
280
                                                bytes_left, codec.length);
 
281
 
 
282
                if (bytes_left <= 0 ||
 
283
                                codec.length != sizeof(u->a2dp.sbc_capabilities))
 
284
                        return -1;
 
285
 
 
286
                assert(codec.type == BT_A2DP_SBC_SINK);
 
287
 
 
288
                memcpy(&u->a2dp.sbc_capabilities, &codec,
 
289
                                        sizeof(u->a2dp.sbc_capabilities));
 
290
        } else {
 
291
                assert(0);
 
292
        }
 
293
 
 
294
        return 0;
 
295
}
 
296
 
 
297
static int get_caps(struct userdata *u)
 
298
{
 
299
        union {
 
300
                struct bt_get_capabilities_req getcaps_req;
 
301
                struct bt_get_capabilities_rsp getcaps_rsp;
 
302
                bt_audio_error_t error;
 
303
                uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 
304
        } msg;
 
305
 
 
306
        assert(u);
 
307
 
 
308
        memset(&msg, 0, sizeof(msg));
 
309
        msg.getcaps_req.h.type = BT_REQUEST;
 
310
        msg.getcaps_req.h.name = BT_GET_CAPABILITIES;
 
311
        msg.getcaps_req.h.length = sizeof(msg.getcaps_req);
 
312
 
 
313
        strncpy(msg.getcaps_req.destination, u->address,
 
314
                        sizeof(msg.getcaps_req.destination));
 
315
        msg.getcaps_req.transport = u->transport;
 
316
        msg.getcaps_req.flags = BT_FLAG_AUTOCONNECT;
 
317
 
 
318
        if (service_send(u, &msg.getcaps_req.h) < 0)
 
319
                return -1;
 
320
 
 
321
        msg.getcaps_rsp.h.length = 0;
 
322
        if (service_expect(u, &msg.getcaps_rsp.h, BT_GET_CAPABILITIES) < 0)
 
323
                return -1;
 
324
 
 
325
        return parse_caps(u, &msg.getcaps_rsp);
 
326
}
 
327
 
 
328
static uint8_t a2dp_default_bitpool(uint8_t freq, uint8_t mode)
 
329
{
 
330
        switch (freq) {
 
331
        case BT_SBC_SAMPLING_FREQ_16000:
 
332
        case BT_SBC_SAMPLING_FREQ_32000:
 
333
                return 53;
 
334
 
 
335
        case BT_SBC_SAMPLING_FREQ_44100:
 
336
 
 
337
                switch (mode) {
 
338
                case BT_A2DP_CHANNEL_MODE_MONO:
 
339
                case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 
340
                        return 31;
 
341
 
 
342
                case BT_A2DP_CHANNEL_MODE_STEREO:
 
343
                case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 
344
                        return 53;
 
345
 
 
346
                default:
 
347
                        DBG("Invalid channel mode %u", mode);
 
348
                        return 53;
 
349
                }
 
350
 
 
351
        case BT_SBC_SAMPLING_FREQ_48000:
 
352
 
 
353
                switch (mode) {
 
354
                case BT_A2DP_CHANNEL_MODE_MONO:
 
355
                case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 
356
                        return 29;
 
357
 
 
358
                case BT_A2DP_CHANNEL_MODE_STEREO:
 
359
                case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 
360
                        return 51;
 
361
 
 
362
                default:
 
363
                        DBG("Invalid channel mode %u", mode);
 
364
                        return 51;
 
365
                }
 
366
 
 
367
        default:
 
368
                DBG("Invalid sampling freq %u", freq);
 
369
                return 53;
 
370
        }
 
371
}
 
372
 
 
373
static int setup_a2dp(struct userdata *u)
 
374
{
 
375
        sbc_capabilities_t *cap;
 
376
        int i;
 
377
 
 
378
        static const struct {
 
379
                uint32_t rate;
 
380
                uint8_t cap;
 
381
        } freq_table[] = {
 
382
                { 16000U, BT_SBC_SAMPLING_FREQ_16000 },
 
383
                { 32000U, BT_SBC_SAMPLING_FREQ_32000 },
 
384
                { 44100U, BT_SBC_SAMPLING_FREQ_44100 },
 
385
                { 48000U, BT_SBC_SAMPLING_FREQ_48000 }
 
386
        };
 
387
 
 
388
        assert(u);
 
389
        assert(u->transport == BT_CAPABILITIES_TRANSPORT_A2DP);
 
390
 
 
391
        cap = &u->a2dp.sbc_capabilities;
 
392
 
 
393
        /* Find the lowest freq that is at least as high as the requested
 
394
         * sampling rate */
 
395
        for (i = 0; (unsigned) i < ARRAY_SIZE(freq_table); i++)
 
396
                if (freq_table[i].rate >= u->rate &&
 
397
                        (cap->frequency & freq_table[i].cap)) {
 
398
                        u->rate = freq_table[i].rate;
 
399
                        cap->frequency = freq_table[i].cap;
 
400
                        break;
 
401
                }
 
402
 
 
403
        if ((unsigned) i >= ARRAY_SIZE(freq_table)) {
 
404
                for (; i >= 0; i--) {
 
405
                        if (cap->frequency & freq_table[i].cap) {
 
406
                                u->rate = freq_table[i].rate;
 
407
                                cap->frequency = freq_table[i].cap;
 
408
                                break;
 
409
                        }
 
410
                }
 
411
 
 
412
                if (i < 0) {
 
413
                        DBG("Not suitable sample rate");
 
414
                        return -1;
 
415
                }
 
416
        }
 
417
 
 
418
        if (u->channels <= 1) {
 
419
                if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
 
420
                        cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
 
421
                        u->channels = 1;
 
422
                } else
 
423
                        u->channels = 2;
 
424
        }
 
425
 
 
426
        if (u->channels >= 2) {
 
427
                u->channels = 2;
 
428
 
 
429
                if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 
430
                        cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
 
431
                else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 
432
                        cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
 
433
                else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 
434
                        cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
 
435
                else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
 
436
                        cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
 
437
                        u->channels = 1;
 
438
                } else {
 
439
                        DBG("No supported channel modes");
 
440
                        return -1;
 
441
                }
 
442
        }
 
443
 
 
444
        if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
 
445
                cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
 
446
        else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
 
447
                cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
 
448
        else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
 
449
                cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
 
450
        else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
 
451
                cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
 
452
        else {
 
453
                DBG("No supported block lengths");
 
454
                return -1;
 
455
        }
 
456
 
 
457
        if (cap->subbands & BT_A2DP_SUBBANDS_8)
 
458
                cap->subbands = BT_A2DP_SUBBANDS_8;
 
459
        else if (cap->subbands & BT_A2DP_SUBBANDS_4)
 
460
                cap->subbands = BT_A2DP_SUBBANDS_4;
 
461
        else {
 
462
                DBG("No supported subbands");
 
463
                return -1;
 
464
        }
 
465
 
 
466
        if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
 
467
                cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
 
468
        else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
 
469
                cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
 
470
 
 
471
        cap->min_bitpool = (uint8_t) MAX(MIN_BITPOOL, cap->min_bitpool);
 
472
        cap->max_bitpool = (uint8_t) MIN(
 
473
                a2dp_default_bitpool(cap->frequency, cap->channel_mode),
 
474
                cap->max_bitpool);
 
475
 
 
476
        return 0;
 
477
}
 
478
 
 
479
static void setup_sbc(struct a2dp_info *a2dp)
 
480
{
 
481
        sbc_capabilities_t *active_capabilities;
 
482
 
 
483
        assert(a2dp);
 
484
 
 
485
        active_capabilities = &a2dp->sbc_capabilities;
 
486
 
 
487
        if (a2dp->sbc_initialized)
 
488
                sbc_reinit(&a2dp->sbc, 0);
 
489
        else
 
490
                sbc_init(&a2dp->sbc, 0);
 
491
        a2dp->sbc_initialized = TRUE;
 
492
 
 
493
        switch (active_capabilities->frequency) {
 
494
        case BT_SBC_SAMPLING_FREQ_16000:
 
495
                a2dp->sbc.frequency = SBC_FREQ_16000;
 
496
                break;
 
497
        case BT_SBC_SAMPLING_FREQ_32000:
 
498
                a2dp->sbc.frequency = SBC_FREQ_32000;
 
499
                break;
 
500
        case BT_SBC_SAMPLING_FREQ_44100:
 
501
                a2dp->sbc.frequency = SBC_FREQ_44100;
 
502
                break;
 
503
        case BT_SBC_SAMPLING_FREQ_48000:
 
504
                a2dp->sbc.frequency = SBC_FREQ_48000;
 
505
                break;
 
506
        default:
 
507
                assert(0);
 
508
        }
 
509
 
 
510
        switch (active_capabilities->channel_mode) {
 
511
        case BT_A2DP_CHANNEL_MODE_MONO:
 
512
                a2dp->sbc.mode = SBC_MODE_MONO;
 
513
                break;
 
514
        case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 
515
                a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL;
 
516
                break;
 
517
        case BT_A2DP_CHANNEL_MODE_STEREO:
 
518
                a2dp->sbc.mode = SBC_MODE_STEREO;
 
519
                break;
 
520
        case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 
521
                a2dp->sbc.mode = SBC_MODE_JOINT_STEREO;
 
522
                break;
 
523
        default:
 
524
                assert(0);
 
525
        }
 
526
 
 
527
        switch (active_capabilities->allocation_method) {
 
528
        case BT_A2DP_ALLOCATION_SNR:
 
529
                a2dp->sbc.allocation = SBC_AM_SNR;
 
530
                break;
 
531
        case BT_A2DP_ALLOCATION_LOUDNESS:
 
532
                a2dp->sbc.allocation = SBC_AM_LOUDNESS;
 
533
                break;
 
534
        default:
 
535
                assert(0);
 
536
        }
 
537
 
 
538
        switch (active_capabilities->subbands) {
 
539
        case BT_A2DP_SUBBANDS_4:
 
540
                a2dp->sbc.subbands = SBC_SB_4;
 
541
                break;
 
542
        case BT_A2DP_SUBBANDS_8:
 
543
                a2dp->sbc.subbands = SBC_SB_8;
 
544
                break;
 
545
        default:
 
546
                assert(0);
 
547
        }
 
548
 
 
549
        switch (active_capabilities->block_length) {
 
550
        case BT_A2DP_BLOCK_LENGTH_4:
 
551
                a2dp->sbc.blocks = SBC_BLK_4;
 
552
                break;
 
553
        case BT_A2DP_BLOCK_LENGTH_8:
 
554
                a2dp->sbc.blocks = SBC_BLK_8;
 
555
                break;
 
556
        case BT_A2DP_BLOCK_LENGTH_12:
 
557
                a2dp->sbc.blocks = SBC_BLK_12;
 
558
                break;
 
559
        case BT_A2DP_BLOCK_LENGTH_16:
 
560
                a2dp->sbc.blocks = SBC_BLK_16;
 
561
                break;
 
562
        default:
 
563
                assert(0);
 
564
        }
 
565
 
 
566
        a2dp->sbc.bitpool = active_capabilities->max_bitpool;
 
567
        a2dp->codesize = (uint16_t) sbc_get_codesize(&a2dp->sbc);
 
568
}
 
569
 
 
570
static int bt_open(struct userdata *u)
 
571
{
 
572
        union {
 
573
                struct bt_open_req open_req;
 
574
                struct bt_open_rsp open_rsp;
 
575
                bt_audio_error_t error;
 
576
                uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 
577
        } msg;
 
578
 
 
579
        memset(&msg, 0, sizeof(msg));
 
580
        msg.open_req.h.type = BT_REQUEST;
 
581
        msg.open_req.h.name = BT_OPEN;
 
582
        msg.open_req.h.length = sizeof(msg.open_req);
 
583
 
 
584
        strncpy(msg.open_req.destination, u->address,
 
585
                        sizeof(msg.open_req.destination));
 
586
        msg.open_req.seid = u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ?
 
587
                                u->a2dp.sbc_capabilities.capability.seid :
 
588
                                BT_A2DP_SEID_RANGE + 1;
 
589
        msg.open_req.lock = u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ?
 
590
                                BT_WRITE_LOCK : BT_READ_LOCK | BT_WRITE_LOCK;
 
591
 
 
592
        if (service_send(u, &msg.open_req.h) < 0)
 
593
                return -1;
 
594
 
 
595
        msg.open_rsp.h.length = sizeof(msg.open_rsp);
 
596
        if (service_expect(u, &msg.open_rsp.h, BT_OPEN) < 0)
 
597
                return -1;
 
598
 
 
599
        return 0;
 
600
}
 
601
 
 
602
static int set_conf(struct userdata *u)
 
603
{
 
604
        union {
 
605
                struct bt_set_configuration_req setconf_req;
 
606
                struct bt_set_configuration_rsp setconf_rsp;
 
607
                bt_audio_error_t error;
 
608
                uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 
609
        } msg;
 
610
 
 
611
        if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
 
612
                if (setup_a2dp(u) < 0)
 
613
                        return -1;
 
614
        }
 
615
 
 
616
        memset(&msg, 0, sizeof(msg));
 
617
        msg.setconf_req.h.type = BT_REQUEST;
 
618
        msg.setconf_req.h.name = BT_SET_CONFIGURATION;
 
619
        msg.setconf_req.h.length = sizeof(msg.setconf_req);
 
620
 
 
621
        if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
 
622
                memcpy(&msg.setconf_req.codec, &u->a2dp.sbc_capabilities,
 
623
                        sizeof(u->a2dp.sbc_capabilities));
 
624
                msg.setconf_req.h.length += msg.setconf_req.codec.length -
 
625
                        sizeof(msg.setconf_req.codec);
 
626
        } else {
 
627
                msg.setconf_req.codec.transport = BT_CAPABILITIES_TRANSPORT_SCO;
 
628
                msg.setconf_req.codec.seid = BT_A2DP_SEID_RANGE + 1;
 
629
                msg.setconf_req.codec.length = sizeof(pcm_capabilities_t);
 
630
        }
 
631
 
 
632
        if (service_send(u, &msg.setconf_req.h) < 0)
 
633
                return -1;
 
634
 
 
635
        msg.setconf_rsp.h.length = sizeof(msg.setconf_rsp);
 
636
        if (service_expect(u, &msg.setconf_rsp.h, BT_SET_CONFIGURATION) < 0)
 
637
                return -1;
 
638
 
 
639
        u->link_mtu = msg.setconf_rsp.link_mtu;
 
640
 
 
641
        /* setup SBC encoder now we agree on parameters */
 
642
        if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
 
643
                setup_sbc(&u->a2dp);
 
644
                u->block_size = u->a2dp.codesize;
 
645
                DBG("SBC parameters:\n\tallocation=%u\n"
 
646
                        "\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
 
647
                        u->a2dp.sbc.allocation, u->a2dp.sbc.subbands,
 
648
                        u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool);
 
649
        } else
 
650
                u->block_size = u->link_mtu;
 
651
 
 
652
        return 0;
 
653
}
 
654
 
 
655
static int setup_bt(struct userdata *u)
 
656
{
 
657
        assert(u);
 
658
 
 
659
        if (get_caps(u) < 0)
 
660
                return -1;
 
661
 
 
662
        DBG("Got device caps");
 
663
 
 
664
        if (bt_open(u) < 0)
 
665
                return -1;
 
666
 
 
667
        if (set_conf(u) < 0)
 
668
                return -1;
 
669
 
 
670
        return 0;
 
671
}
 
672
 
 
673
static int init_profile(struct userdata *u)
 
674
{
 
675
        assert(u);
 
676
 
 
677
        return setup_bt(u);
 
678
}
 
679
 
 
680
static void shutdown_bt(struct userdata *u)
 
681
{
 
682
        assert(u);
 
683
 
 
684
        if (u->stream_fd != -1) {
 
685
                stop_stream(u);
 
686
                DBG("close(stream_fd)");
 
687
                close(u->stream_fd);
 
688
                u->stream_fd = -1;
 
689
        }
 
690
 
 
691
        if (u->service_fd != -1) {
 
692
                DBG("bt_audio_service_close");
 
693
                bt_audio_service_close(u->service_fd);
 
694
                u->service_fd = -1;
 
695
        }
 
696
}
 
697
 
 
698
static void make_fd_nonblock(int fd)
 
699
{
 
700
        int v;
 
701
 
 
702
        assert(fd >= 0);
 
703
        assert((v = fcntl(fd, F_GETFL)) >= 0);
 
704
 
 
705
        if (!(v & O_NONBLOCK))
 
706
                assert(fcntl(fd, F_SETFL, v|O_NONBLOCK) >= 0);
 
707
}
 
708
 
 
709
static void make_socket_low_delay(int fd)
 
710
{
 
711
/* FIXME: is this widely supported? */
 
712
#ifdef SO_PRIORITY
 
713
        int priority;
 
714
        assert(fd >= 0);
 
715
 
 
716
        priority = 6;
 
717
        if (setsockopt(fd, SOL_SOCKET, SO_PRIORITY, (void*)&priority,
 
718
                        sizeof(priority)) < 0)
 
719
                ERR("SO_PRIORITY failed: %s", strerror(errno));
 
720
#endif
 
721
}
 
722
 
 
723
static int read_stream(struct userdata *u)
 
724
{
 
725
        int ret = 0;
 
726
        ssize_t l;
 
727
        char *buf;
 
728
 
 
729
        assert(u);
 
730
        assert(u->stream_fd >= 0);
 
731
 
 
732
        buf = alloca(u->link_mtu);
 
733
 
 
734
        for (;;) {
 
735
                l = read(u->stream_fd, buf, u->link_mtu);
 
736
                if (u->debug_stream_read)
 
737
                        DBG("read from socket: %lli bytes", (long long) l);
 
738
                if (l <= 0) {
 
739
                        if (l < 0 && errno == EINTR)
 
740
                                continue;
 
741
                        else {
 
742
                                ERR("Failed to read date from stream_fd: %s",
 
743
                                        ret < 0 ? strerror(errno) : "EOF");
 
744
                                return -1;
 
745
                        }
 
746
                } else {
 
747
                        break;
 
748
                }
 
749
        }
 
750
 
 
751
        return ret;
 
752
}
 
753
 
 
754
/* It's what PulseAudio is doing, not sure it's necessary for this
 
755
 * test */
 
756
static ssize_t pa_write(int fd, const void *buf, size_t count)
 
757
{
 
758
        ssize_t r;
 
759
 
 
760
        if ((r = send(fd, buf, count, MSG_NOSIGNAL)) >= 0)
 
761
                return r;
 
762
 
 
763
        if (errno != ENOTSOCK)
 
764
                return r;
 
765
 
 
766
        return write(fd, buf, count);
 
767
}
 
768
 
 
769
static int write_stream(struct userdata *u)
 
770
{
 
771
        int ret = 0;
 
772
        ssize_t l;
 
773
        char *buf;
 
774
 
 
775
        assert(u);
 
776
        assert(u->stream_fd >= 0);
 
777
        buf = alloca(u->link_mtu);
 
778
 
 
779
        for (;;) {
 
780
                l = pa_write(u->stream_fd, buf, u->link_mtu);
 
781
                if (u->debug_stream_write)
 
782
                        DBG("written to socket: %lli bytes", (long long) l);
 
783
                assert(l != 0);
 
784
                if (l < 0) {
 
785
                        if (errno == EINTR)
 
786
                                continue;
 
787
                        else {
 
788
                                ERR("Failed to write data: %s", strerror(errno));
 
789
                                ret = -1;
 
790
                                break;
 
791
                        }
 
792
                } else {
 
793
                        assert((size_t)l <= u->link_mtu);
 
794
                        break;
 
795
                }
 
796
        }
 
797
 
 
798
        return ret;
 
799
}
 
800
 
 
801
static gboolean stream_cb(GIOChannel *gin, GIOCondition condition, gpointer data)
 
802
{
 
803
        struct userdata *u;
 
804
 
 
805
        assert(u = data);
 
806
 
 
807
        if (condition & G_IO_IN) {
 
808
                if (read_stream(u) < 0)
 
809
                        goto fail;
 
810
        } else if (condition & G_IO_OUT) {
 
811
                if (write_stream(u) < 0)
 
812
                        goto fail;
 
813
        } else {
 
814
                DBG("Got %d", condition);
 
815
                g_main_loop_quit(main_loop);
 
816
                return FALSE;
 
817
        }
 
818
 
 
819
        return TRUE;
 
820
 
 
821
fail:
 
822
        stop_stream(u);
 
823
        return FALSE;
 
824
}
 
825
 
 
826
static int start_stream(struct userdata *u)
 
827
{
 
828
        union {
 
829
                bt_audio_msg_header_t rsp;
 
830
                struct bt_start_stream_req start_req;
 
831
                struct bt_start_stream_rsp start_rsp;
 
832
                struct bt_new_stream_ind streamfd_ind;
 
833
                bt_audio_error_t error;
 
834
                uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 
835
        } msg;
 
836
 
 
837
        assert(u);
 
838
 
 
839
        if (u->stream_fd >= 0)
 
840
                return 0;
 
841
        if (u->stream_watch != 0) {
 
842
                g_source_remove(u->stream_watch);
 
843
                u->stream_watch = 0;
 
844
        }
 
845
        if (u->stream_channel != 0) {
 
846
                g_io_channel_unref(u->stream_channel);
 
847
                u->stream_channel = NULL;
 
848
        }
 
849
 
 
850
        memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
 
851
        msg.start_req.h.type = BT_REQUEST;
 
852
        msg.start_req.h.name = BT_START_STREAM;
 
853
        msg.start_req.h.length = sizeof(msg.start_req);
 
854
 
 
855
        if (service_send(u, &msg.start_req.h) < 0)
 
856
                return -1;
 
857
 
 
858
        msg.rsp.length = sizeof(msg.start_rsp);
 
859
        if (service_expect(u, &msg.rsp, BT_START_STREAM) < 0)
 
860
                return -1;
 
861
 
 
862
        msg.rsp.length = sizeof(msg.streamfd_ind);
 
863
        if (service_expect(u, &msg.rsp, BT_NEW_STREAM) < 0)
 
864
                return -1;
 
865
 
 
866
        if ((u->stream_fd = bt_audio_service_get_data_fd(u->service_fd)) < 0) {
 
867
                DBG("Failed to get stream fd from audio service.");
 
868
                return -1;
 
869
        }
 
870
 
 
871
        make_fd_nonblock(u->stream_fd);
 
872
        make_socket_low_delay(u->stream_fd);
 
873
 
 
874
        assert(u->stream_channel = g_io_channel_unix_new(u->stream_fd));
 
875
 
 
876
        u->stream_watch = g_io_add_watch(u->stream_channel,
 
877
                                        G_IO_IN|G_IO_OUT|G_IO_ERR|G_IO_HUP|G_IO_NVAL,
 
878
                                        stream_cb, u);
 
879
 
 
880
        return 0;
 
881
}
 
882
 
 
883
static int stop_stream(struct userdata *u)
 
884
{
 
885
        union {
 
886
                bt_audio_msg_header_t rsp;
 
887
                struct bt_stop_stream_req stop_req;
 
888
                struct bt_stop_stream_rsp stop_rsp;
 
889
                bt_audio_error_t error;
 
890
                uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 
891
        } msg;
 
892
        int r = 0;
 
893
 
 
894
        if (u->stream_fd < 0)
 
895
                return 0;
 
896
 
 
897
        assert(u);
 
898
        assert(u->stream_channel);
 
899
 
 
900
        g_source_remove(u->stream_watch);
 
901
        u->stream_watch = 0;
 
902
        g_io_channel_unref(u->stream_channel);
 
903
        u->stream_channel = NULL;
 
904
 
 
905
        memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
 
906
        msg.stop_req.h.type = BT_REQUEST;
 
907
        msg.stop_req.h.name = BT_STOP_STREAM;
 
908
        msg.stop_req.h.length = sizeof(msg.stop_req);
 
909
 
 
910
        if (service_send(u, &msg.stop_req.h) < 0) {
 
911
                r = -1;
 
912
                goto done;
 
913
        }
 
914
 
 
915
        msg.rsp.length = sizeof(msg.stop_rsp);
 
916
        if (service_expect(u, &msg.rsp, BT_STOP_STREAM) < 0)
 
917
                r = -1;
 
918
 
 
919
done:
 
920
        close(u->stream_fd);
 
921
        u->stream_fd = -1;
 
922
 
 
923
        return r;
 
924
}
 
925
 
 
926
static gboolean sleep_cb(gpointer data)
 
927
{
 
928
        struct userdata *u;
 
929
 
 
930
        assert(u = data);
 
931
 
 
932
        u->gin_watch = g_io_add_watch(u->gin,
 
933
                G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL, input_cb, data);
 
934
 
 
935
        printf(">>> ");
 
936
        fflush(stdout);
 
937
 
 
938
        return FALSE;
 
939
}
 
940
 
 
941
static gboolean input_cb(GIOChannel *gin, GIOCondition condition, gpointer data)
 
942
{
 
943
        char *line, *tmp;
 
944
        gsize term_pos;
 
945
        GError *error = NULL;
 
946
        struct userdata *u;
 
947
        int success;
 
948
 
 
949
        assert(u = data);
 
950
        if (!(condition & G_IO_IN)) {
 
951
                DBG("Got %d", condition);
 
952
                g_main_loop_quit(main_loop);
 
953
                return FALSE;
 
954
        }
 
955
 
 
956
        if (g_io_channel_read_line(gin, &line, NULL, &term_pos, &error) !=
 
957
                G_IO_STATUS_NORMAL)
 
958
                return FALSE;
 
959
 
 
960
        line[term_pos] = '\0';
 
961
        g_strstrip(line);
 
962
        if ((tmp = strchr(line, '#')))
 
963
                *tmp = '\0';
 
964
        success = FALSE;
 
965
 
 
966
#define IF_CMD(cmd) \
 
967
        if (!success && (success = (strncmp(line, #cmd, strlen(#cmd)) == 0)))
 
968
 
 
969
        IF_CMD(quit) {
 
970
                g_main_loop_quit(main_loop);
 
971
                return FALSE;
 
972
        }
 
973
 
 
974
        IF_CMD(sleep) {
 
975
                unsigned int seconds;
 
976
                if (sscanf(line, "%*s %d", &seconds) != 1)
 
977
                        DBG("sleep SECONDS");
 
978
                else {
 
979
                        g_source_remove(u->gin_watch);
 
980
                        g_timeout_add_seconds(seconds, sleep_cb, u);
 
981
                        return FALSE;
 
982
                }
 
983
        }
 
984
 
 
985
        IF_CMD(debug) {
 
986
                char *what = NULL;
 
987
                int enable;
 
988
 
 
989
                if (sscanf(line, "%*s %as %d", &what, &enable) != 1)
 
990
                        DBG("debug [stream_read|stream_write] [0|1]");
 
991
                if (strncmp(what, "stream_read", 12) == 0) {
 
992
                        u->debug_stream_read = enable;
 
993
                } else if (strncmp(what, "stream_write", 13) == 0) {
 
994
                        u->debug_stream_write = enable;
 
995
                } else {
 
996
                        DBG("debug [stream_read|stream_write] [0|1]");
 
997
                }
 
998
        }
 
999
 
 
1000
        IF_CMD(init_bt) {
 
1001
                DBG("%d", init_bt(u));
 
1002
        }
 
1003
 
 
1004
        IF_CMD(init_profile) {
 
1005
                DBG("%d", init_profile(u));
 
1006
        }
 
1007
 
 
1008
        IF_CMD(start_stream) {
 
1009
                DBG("%d", start_stream(u));
 
1010
        }
 
1011
 
 
1012
        IF_CMD(stop_stream) {
 
1013
                DBG("%d", stop_stream(u));
 
1014
        }
 
1015
 
 
1016
        IF_CMD(shutdown_bt) {
 
1017
                shutdown_bt(u);
 
1018
        }
 
1019
 
 
1020
        IF_CMD(rate) {
 
1021
                if (sscanf(line, "%*s %d", &u->rate) != 1)
 
1022
                        DBG("set with rate RATE");
 
1023
                DBG("rate %d", u->rate);
 
1024
        }
 
1025
 
 
1026
        IF_CMD(bdaddr) {
 
1027
                char *address;
 
1028
 
 
1029
                if (sscanf(line, "%*s %as", &address) != 1)
 
1030
                        DBG("set with bdaddr BDADDR");
 
1031
 
 
1032
                free(u->address);
 
1033
 
 
1034
                u->address = address;
 
1035
                DBG("bdaddr %s", u->address);
 
1036
        }
 
1037
 
 
1038
        IF_CMD(profile) {
 
1039
                char *profile = NULL;
 
1040
 
 
1041
                if (sscanf(line, "%*s %as", &profile) != 1)
 
1042
                        DBG("set with profile [hsp|a2dp]");
 
1043
                if (strncmp(profile, "hsp", 4) == 0) {
 
1044
                        u->transport = BT_CAPABILITIES_TRANSPORT_SCO;
 
1045
                } else if (strncmp(profile, "a2dp", 5) == 0) {
 
1046
                        u->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
 
1047
                } else {
 
1048
                        DBG("set with profile [hsp|a2dp]");
 
1049
                }
 
1050
 
 
1051
                free(profile);
 
1052
                DBG("profile %s", u->transport == BT_CAPABILITIES_TRANSPORT_SCO ?
 
1053
                        "hsp" : "a2dp");
 
1054
        }
 
1055
 
 
1056
        if (!success && strlen(line) != 0) {
 
1057
                DBG("%s, unknown command", line);
 
1058
        }
 
1059
 
 
1060
        printf(">>> ");
 
1061
        fflush(stdout);
 
1062
        return TRUE;
 
1063
}
 
1064
 
 
1065
 
 
1066
static void show_usage(char* prgname)
 
1067
{
 
1068
        printf("%s: ipctest [--interactive] BDADDR\n", basename(prgname));
 
1069
}
 
1070
 
 
1071
static void sig_term(int sig)
 
1072
{
 
1073
        g_main_loop_quit(main_loop);
 
1074
}
 
1075
 
 
1076
int main(int argc, char *argv[])
 
1077
{
 
1078
        if (argc < 2) {
 
1079
                show_usage(argv[0]);
 
1080
                exit(EXIT_FAILURE);
 
1081
        }
 
1082
 
 
1083
        assert(main_loop = g_main_loop_new(NULL, FALSE));
 
1084
 
 
1085
        if (strncmp("--interactive", argv[1], 14) == 0) {
 
1086
                if (argc < 3) {
 
1087
                        show_usage(argv[0]);
 
1088
                        exit(EXIT_FAILURE);
 
1089
                }
 
1090
 
 
1091
                data.address = strdup(argv[2]);
 
1092
 
 
1093
                signal(SIGTERM, sig_term);
 
1094
                signal(SIGINT, sig_term);
 
1095
 
 
1096
                assert(data.gin = g_io_channel_unix_new(fileno(stdin)));
 
1097
 
 
1098
                data.gin_watch = g_io_add_watch(data.gin,
 
1099
                        G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL, input_cb, &data);
 
1100
 
 
1101
                printf(">>> ");
 
1102
                fflush(stdout);
 
1103
 
 
1104
                g_main_loop_run(main_loop);
 
1105
 
 
1106
        } else {
 
1107
                data.address = strdup(argv[1]);
 
1108
 
 
1109
                assert(init_bt(&data) == 0);
 
1110
 
 
1111
                assert(init_profile(&data) == 0);
 
1112
 
 
1113
                assert(start_stream(&data) == 0);
 
1114
 
 
1115
                g_main_loop_run(main_loop);
 
1116
 
 
1117
                assert(stop_stream(&data) == 0);
 
1118
 
 
1119
                shutdown_bt(&data);
 
1120
        }
 
1121
 
 
1122
        g_main_loop_unref(main_loop);
 
1123
 
 
1124
        printf("\nExiting\n");
 
1125
 
 
1126
        exit(EXIT_SUCCESS);
 
1127
 
 
1128
        return 0;
 
1129
}