1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2006-2009 Nokia Corporation
6 * Copyright (C) 2004-2009 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
service_send(struct userdata * u,const bt_audio_msg_header_t * msg)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
service_recv(struct userdata * u,bt_audio_msg_header_t * rsp)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
service_expect(struct userdata * u,bt_audio_msg_header_t * rsp,uint8_t expected_name)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
init_bt(struct userdata * u)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
parse_caps(struct userdata * u,const struct bt_get_capabilities_rsp * rsp)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
get_caps(struct userdata * u)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
a2dp_default_bitpool(uint8_t freq,uint8_t mode)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
setup_a2dp(struct userdata * u)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
setup_sbc(struct a2dp_info * a2dp)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
bt_open(struct userdata * u)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
set_conf(struct userdata * u)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
setup_bt(struct userdata * u)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
init_profile(struct userdata * u)673 static int init_profile(struct userdata *u)
674 {
675 assert(u);
676
677 return setup_bt(u);
678 }
679
shutdown_bt(struct userdata * u)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
make_fd_nonblock(int fd)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
make_socket_low_delay(int fd)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
read_stream(struct userdata * u)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 */
pa_write(int fd,const void * buf,size_t count)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
write_stream(struct userdata * u)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
stream_cb(GIOChannel * gin,GIOCondition condition,gpointer data)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
start_stream(struct userdata * u)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
stop_stream(struct userdata * u)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
sleep_cb(gpointer data)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
input_cb(GIOChannel * gin,GIOCondition condition,gpointer data)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 if (u->address)
1033 free(u->address);
1034
1035 u->address = address;
1036 DBG("bdaddr %s", u->address);
1037 }
1038
1039 IF_CMD(profile) {
1040 char *profile = NULL;
1041
1042 if (sscanf(line, "%*s %as", &profile) != 1)
1043 DBG("set with profile [hsp|a2dp]");
1044 if (strncmp(profile, "hsp", 4) == 0) {
1045 u->transport = BT_CAPABILITIES_TRANSPORT_SCO;
1046 } else if (strncmp(profile, "a2dp", 5) == 0) {
1047 u->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
1048 } else {
1049 DBG("set with profile [hsp|a2dp]");
1050 }
1051
1052 if (profile)
1053 free(profile);
1054 DBG("profile %s", u->transport == BT_CAPABILITIES_TRANSPORT_SCO ?
1055 "hsp" : "a2dp");
1056 }
1057
1058 if (!success && strlen(line) != 0) {
1059 DBG("%s, unknown command", line);
1060 }
1061
1062 printf(">>> ");
1063 fflush(stdout);
1064 return TRUE;
1065 }
1066
1067
show_usage(char * prgname)1068 static void show_usage(char* prgname)
1069 {
1070 printf("%s: ipctest [--interactive] BDADDR\n", basename(prgname));
1071 }
1072
sig_term(int sig)1073 static void sig_term(int sig)
1074 {
1075 g_main_loop_quit(main_loop);
1076 }
1077
main(int argc,char * argv[])1078 int main(int argc, char *argv[])
1079 {
1080 if (argc < 2) {
1081 show_usage(argv[0]);
1082 exit(EXIT_FAILURE);
1083 }
1084
1085 assert(main_loop = g_main_loop_new(NULL, FALSE));
1086
1087 if (strncmp("--interactive", argv[1], 14) == 0) {
1088 if (argc < 3) {
1089 show_usage(argv[0]);
1090 exit(EXIT_FAILURE);
1091 }
1092
1093 data.address = strdup(argv[2]);
1094
1095 signal(SIGTERM, sig_term);
1096 signal(SIGINT, sig_term);
1097
1098 assert(data.gin = g_io_channel_unix_new(fileno(stdin)));
1099
1100 data.gin_watch = g_io_add_watch(data.gin,
1101 G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL, input_cb, &data);
1102
1103 printf(">>> ");
1104 fflush(stdout);
1105
1106 g_main_loop_run(main_loop);
1107
1108 } else {
1109 data.address = strdup(argv[1]);
1110
1111 assert(init_bt(&data) == 0);
1112
1113 assert(init_profile(&data) == 0);
1114
1115 assert(start_stream(&data) == 0);
1116
1117 g_main_loop_run(main_loop);
1118
1119 assert(stop_stream(&data) == 0);
1120
1121 shutdown_bt(&data);
1122 }
1123
1124 g_main_loop_unref(main_loop);
1125
1126 printf("\nExiting\n");
1127
1128 exit(EXIT_SUCCESS);
1129
1130 return 0;
1131 }
1132