src/shared/hfp.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29 #include <unistd.h>
  30 #include <string.h>
  31 #include <stdarg.h>
  32 #include <ctype.h>
  33
  34 #include "src/shared/util.h"
  35 #include "src/shared/ringbuf.h"
  36 #include "src/shared/queue.h"
  37 #include "src/shared/io.h"
  38 #include "src/shared/hfp.h"
  39
  40 struct hfp_gw {
  41         int ref_count;
  42         int fd;
  43         bool close_on_unref;
  44         struct io *io;
  45         struct ringbuf *read_buf;
  46         struct ringbuf *write_buf;
  47         struct queue *cmd_handlers;
  48         bool writer_active;
  49         bool result_pending;
  50         hfp_command_func_t command_callback;
  51         hfp_destroy_func_t command_destroy;
  52         void *command_data;
  53         hfp_debug_func_t debug_callback;
  54         hfp_destroy_func_t debug_destroy;
  55         void *debug_data;
  56
  57         hfp_disconnect_func_t disconnect_callback;
  58         hfp_destroy_func_t disconnect_destroy;
  59         void *disconnect_data;
  60
  61         bool in_disconnect;
  62         bool destroyed;
  63 };
  64
  65 struct hfp_hf {
  66         int ref_count;
  67         int fd;
  68         bool close_on_unref;
  69         struct io *io;
  70         struct ringbuf *read_buf;
  71         struct ringbuf *write_buf;
  72
  73         bool writer_active;
  74         struct queue *cmd_queue;
  75
  76         struct queue *event_handlers;
  77
  78         hfp_debug_func_t debug_callback;
  79         hfp_destroy_func_t debug_destroy;
  80         void *debug_data;
  81
  82         hfp_disconnect_func_t disconnect_callback;
  83         hfp_destroy_func_t disconnect_destroy;
  84         void *disconnect_data;
  85
  86         bool in_disconnect;
  87         bool destroyed;
  88 };
  89
  90 struct cmd_handler {
  91         char *prefix;
  92         void *user_data;
  93         hfp_destroy_func_t destroy;
  94         hfp_result_func_t callback;
  95 };
  96
  97 struct hfp_context {
  98         const char *data;
  99         unsigned int offset;
 100 };
 101
 102 struct cmd_response {
 103         hfp_response_func_t resp_cb;
 104         struct hfp_context *response;
 105         char *resp_data;
 106         void *user_data;
 107 };
 108
 109 struct event_handler {
 110         char *prefix;
 111         void *user_data;
 112         hfp_destroy_func_t destroy;
 113         hfp_hf_result_func_t callback;
 114 };
 115
 116 static void destroy_cmd_handler(void *data)
 117 {
 118         struct cmd_handler *handler = data;
 119
 120         if (handler->destroy)
 121                 handler->destroy(handler->user_data);
 122
 123         free(handler->prefix);
 124
 125         free(handler);
 126 }
 127
 128 static bool match_handler_prefix(const void *a, const void *b)
 129 {
 130         const struct cmd_handler *handler = a;
 131         const char *prefix = b;
 132
 133         if (strcmp(handler->prefix, prefix) != 0)
 134                 return false;
 135
 136         return true;
 137 }
 138
 139 static void write_watch_destroy(void *user_data)
 140 {
 141         struct hfp_gw *hfp = user_data;
 142
 143         hfp->writer_active = false;
 144 }
 145
 146 static bool can_write_data(struct io *io, void *user_data)
 147 {
 148         struct hfp_gw *hfp = user_data;
 149         ssize_t bytes_written;
 150
 151         bytes_written = ringbuf_write(hfp->write_buf, hfp->fd);
 152         if (bytes_written < 0)
 153                 return false;
 154
 155         if (ringbuf_len(hfp->write_buf) > 0)
 156                 return true;
 157
 158         return false;
 159 }
 160
 161 static void wakeup_writer(struct hfp_gw *hfp)
 162 {
 163         if (hfp->writer_active)
 164                 return;
 165
 166         if (!ringbuf_len(hfp->write_buf))
 167                 return;
 168
 169         if (!io_set_write_handler(hfp->io, can_write_data,
 170                                         hfp, write_watch_destroy))
 171                 return;
 172
 173         hfp->writer_active = true;
 174 }
 175
 176 static void skip_whitespace(struct hfp_context *context)
 177 {
 178         while (context->data[context->offset] == ' ')
 179                 context->offset++;
 180 }
 181
 182 static void handle_unknown_at_command(struct hfp_gw *hfp,
 183                                                         const char *data)
 184 {
 185         if (hfp->command_callback) {
 186                 hfp->result_pending = true;
 187                 hfp->command_callback(data, hfp->command_data);
 188         } else {
 189                 hfp_gw_send_result(hfp, HFP_RESULT_ERROR);
 190         }
 191 }
 192
 193 static bool handle_at_command(struct hfp_gw *hfp, const char *data)
 194 {
 195         struct cmd_handler *handler;
 196         const char *separators = ";?=\0";
 197         struct hfp_context context;
 198         enum hfp_gw_cmd_type type;
 199         char lookup_prefix[18];
 200         uint8_t pref_len = 0;
 201         const char *prefix;
 202         int i;
 203
 204         context.offset = 0;
 205         context.data = data;
 206
 207         skip_whitespace(&context);
 208
 209         if (strlen(data + context.offset) < 3)
 210                 return false;
 211
 212         if (strncmp(data + context.offset, "AT", 2))
 213                 if (strncmp(data + context.offset, "at", 2))
 214                         return false;
 215
 216         context.offset += 2;
 217         prefix = data + context.offset;
 218
 219         if (isalpha(prefix[0])) {
 220                 lookup_prefix[pref_len++] = toupper(prefix[0]);
 221         } else {
 222                 pref_len = strcspn(prefix, separators);
 223                 if (pref_len > 17 || pref_len < 2)
 224                         return false;
 225
 226                 for (i = 0; i < pref_len; i++)
 227                         lookup_prefix[i] = toupper(prefix[i]);
 228         }
 229
 230         lookup_prefix[pref_len] = '\0';
 231         context.offset += pref_len;
 232
 233         if (lookup_prefix[0] == 'D') {
 234                 type = HFP_GW_CMD_TYPE_SET;
 235                 goto done;
 236         }
 237
 238         if (data[context.offset] == '=') {
 239                 context.offset++;
 240                 if (data[context.offset] == '?') {
 241                         context.offset++;
 242                         type = HFP_GW_CMD_TYPE_TEST;
 243                 } else {
 244                         type = HFP_GW_CMD_TYPE_SET;
 245                 }
 246                 goto done;
 247         }
 248
 249         if (data[context.offset] == '?') {
 250                 context.offset++;
 251                 type = HFP_GW_CMD_TYPE_READ;
 252                 goto done;
 253         }
 254
 255         type = HFP_GW_CMD_TYPE_COMMAND;
 256
 257 done:
 258
 259         handler = queue_find(hfp->cmd_handlers, match_handler_prefix,
 260                                                                 lookup_prefix);
 261         if (!handler) {
 262                 handle_unknown_at_command(hfp, data);
 263                 return true;
 264         }
 265
 266         hfp->result_pending = true;
 267         handler->callback(&context, type, handler->user_data);
 268
 269         return true;
 270 }
 271
 272 static void next_field(struct hfp_context *context)
 273 {
 274         if (context->data[context->offset] == ',')
 275                 context->offset++;
 276 }
 277
 278 bool hfp_context_get_number_default(struct hfp_context *context,
 279                                                 unsigned int *val,
 280                                                 unsigned int default_val)
 281 {
 282         skip_whitespace(context);
 283
 284         if (context->data[context->offset] == ',') {
 285                 if (val)
 286                         *val = default_val;
 287
 288                 context->offset++;
 289                 return true;
 290         }
 291
 292         return hfp_context_get_number(context, val);
 293 }
 294
 295 bool hfp_context_get_number(struct hfp_context *context,
 296                                                         unsigned int *val)
 297 {
 298         unsigned int i;
 299         int tmp = 0;
 300
 301         skip_whitespace(context);
 302
 303         i = context->offset;
 304
 305         while (context->data[i] >= '0' && context->data[i] <= '9')
 306                 tmp = tmp * 10 + context->data[i++] - '0';
 307
 308         if (i == context->offset)
 309                 return false;
 310
 311         if (val)
 312                 *val = tmp;
 313         context->offset = i;
 314
 315         skip_whitespace(context);
 316         next_field(context);
 317
 318         return true;
 319 }
 320
 321 bool hfp_context_open_container(struct hfp_context *context)
 322 {
 323         skip_whitespace(context);
 324
 325         /* The list shall be preceded by a left parenthesis "(") */
 326         if (context->data[context->offset] != '(')
 327                 return false;
 328
 329         context->offset++;
 330
 331         return true;
 332 }
 333
 334 bool hfp_context_close_container(struct hfp_context *context)
 335 {
 336         skip_whitespace(context);
 337
 338         /* The list shall be followed by a right parenthesis (")" V250 5.7.3.1*/
 339         if (context->data[context->offset] != ')')
 340                 return false;
 341
 342         context->offset++;
 343
 344         next_field(context);
 345
 346         return true;
 347 }
 348
 349 bool hfp_context_get_string(struct hfp_context *context, char *buf,
 350                                                                 uint8_t len)
 351 {
 352         int i = 0;
 353         const char *data = context->data;
 354         unsigned int offset;
 355
 356         skip_whitespace(context);
 357
 358         if (data[context->offset] != '"')
 359                 return false;
 360
 361         offset = context->offset;
 362         offset++;
 363
 364         while (data[offset] != '\0' && data[offset] != '"') {
 365                 if (i == len)
 366                         return false;
 367
 368                 buf[i++] = data[offset];
 369                 offset++;
 370         }
 371
 372         if (i == len)
 373                 return false;
 374
 375         buf[i] = '\0';
 376
 377         if (data[offset] == '"')
 378                 offset++;
 379         else
 380                 return false;
 381
 382         context->offset = offset;
 383
 384         skip_whitespace(context);
 385         next_field(context);
 386
 387         return true;
 388 }
 389
 390 bool hfp_context_get_unquoted_string(struct hfp_context *context,
 391                                                         char *buf, uint8_t len)
 392 {
 393         const char *data = context->data;
 394         unsigned int offset;
 395         int i = 0;
 396         char c;
 397
 398         skip_whitespace(context);
 399
 400         c = data[context->offset];
 401         if (c == '"' || c == ')' || c == '(')
 402                 return false;
 403
 404         offset = context->offset;
 405
 406         while (data[offset] != '\0' && data[offset] != ',' &&
 407                                                         data[offset] != ')') {
 408                 if (i == len)
 409                         return false;
 410
 411                 buf[i++] = data[offset];
 412                 offset++;
 413         }
 414
 415         if (i == len)
 416                 return false;
 417
 418         buf[i] = '\0';
 419
 420         context->offset = offset;
 421
 422         next_field(context);
 423
 424         return true;
 425 }
 426
 427 bool hfp_context_has_next(struct hfp_context *context)
 428 {
 429         return context->data[context->offset] != '\0';
 430 }
 431
 432 void hfp_context_skip_field(struct hfp_context *context)
 433 {
 434         const char *data = context->data;
 435         unsigned int offset = context->offset;
 436
 437         while (data[offset] != '\0' && data[offset] != ',')
 438                 offset++;
 439
 440         context->offset = offset;
 441         next_field(context);
 442 }
 443
 444 bool hfp_context_get_range(struct hfp_context *context, uint32_t *min,
 445                                                                 uint32_t *max)
 446 {
 447         uint32_t l, h;
 448         uint32_t start;
 449
 450         start = context->offset;
 451
 452         if (!hfp_context_get_number(context, &l))
 453                 goto failed;
 454
 455         if (context->data[context->offset] != '-')
 456                 goto failed;
 457
 458         context->offset++;
 459
 460         if (!hfp_context_get_number(context, &h))
 461                 goto failed;
 462
 463         *min = l;
 464         *max = h;
 465
 466         next_field(context);
 467
 468         return true;
 469
 470 failed:
 471         context->offset = start;
 472         return false;
 473 }
 474
 475 static void process_input(struct hfp_gw *hfp)
 476 {
 477         char *str, *ptr;
 478         size_t len, count;
 479         bool free_ptr = false;
 480         bool read_again;
 481
 482         do {
 483                 str = ringbuf_peek(hfp->read_buf, 0, &len);
 484                 if (!str)
 485                         return;
 486
 487                 ptr = memchr(str, '\r', len);
 488                 if (!ptr) {
 489                         char *str2;
 490                         size_t len2;
 491
 492                         /*
 493                          * If there is no more data in ringbuffer,
 494                          * it's just an incomplete command.
 495                          */
 496                         if (len == ringbuf_len(hfp->read_buf))
 497                                 return;
 498
 499                         str2 = ringbuf_peek(hfp->read_buf, len, &len2);
 500                         if (!str2)
 501                                 return;
 502
 503                         ptr = memchr(str2, '\r', len2);
 504                         if (!ptr)
 505                                 return;
 506
 507                         *ptr = '\0';
 508
 509                         count = len2 + len;
 510                         ptr = malloc(count);
 511                         if (!ptr)
 512                                 return;
 513
 514                         memcpy(ptr, str, len);
 515                         memcpy(ptr + len, str2, len2);
 516
 517                         free_ptr = true;
 518                         str = ptr;
 519                 } else {
 520                         count = ptr - str;
 521                         *ptr = '\0';
 522                 }
 523
 524                 if (!handle_at_command(hfp, str))
 525                         /*
 526                          * Command is not handled that means that was some
 527                          * trash. Let's skip that and keep reading from ring
 528                          * buffer.
 529                          */
 530                         read_again = true;
 531                 else
 532                         /*
 533                          * Command has been handled. If we are waiting for a
 534                          * result from upper layer, we can stop reading. If we
 535                          * already reply i.e. ERROR on unknown command, then we
 536                          * can keep reading ring buffer. Actually ring buffer
 537                          * should be empty but lets just look there.
 538                          */
 539                         read_again = !hfp->result_pending;
 540
 541                 ringbuf_drain(hfp->read_buf, count + 1);
 542
 543                 if (free_ptr)
 544                         free(ptr);
 545
 546         } while (read_again);
 547 }
 548
 549 static void read_watch_destroy(void *user_data)
 550 {
 551 }
 552
 553 static bool can_read_data(struct io *io, void *user_data)
 554 {
 555         struct hfp_gw *hfp = user_data;
 556         ssize_t bytes_read;
 557
 558         bytes_read = ringbuf_read(hfp->read_buf, hfp->fd);
 559         if (bytes_read < 0)
 560                 return false;
 561
 562         if (hfp->result_pending)
 563                 return true;
 564
 565         process_input(hfp);
 566
 567         return true;
 568 }
 569
 570 struct hfp_gw *hfp_gw_new(int fd)
 571 {
 572         struct hfp_gw *hfp;
 573
 574         if (fd < 0)
 575                 return NULL;
 576
 577         hfp = new0(struct hfp_gw, 1);
 578         hfp->fd = fd;
 579         hfp->close_on_unref = false;
 580
 581         hfp->read_buf = ringbuf_new(4096);
 582         if (!hfp->read_buf) {
 583                 free(hfp);
 584                 return NULL;
 585         }
 586
 587         hfp->write_buf = ringbuf_new(4096);
 588         if (!hfp->write_buf) {
 589                 ringbuf_free(hfp->read_buf);
 590                 free(hfp);
 591                 return NULL;
 592         }
 593
 594         hfp->io = io_new(fd);
 595         if (!hfp->io) {
 596                 ringbuf_free(hfp->write_buf);
 597                 ringbuf_free(hfp->read_buf);
 598                 free(hfp);
 599                 return NULL;
 600         }
 601
 602         hfp->cmd_handlers = queue_new();
 603
 604         if (!io_set_read_handler(hfp->io, can_read_data, hfp,
 605                                                         read_watch_destroy)) {
 606                 queue_destroy(hfp->cmd_handlers, destroy_cmd_handler);
 607                 io_destroy(hfp->io);
 608                 ringbuf_free(hfp->write_buf);
 609                 ringbuf_free(hfp->read_buf);
 610                 free(hfp);
 611                 return NULL;
 612         }
 613
 614         hfp->writer_active = false;
 615         hfp->result_pending = false;
 616
 617         return hfp_gw_ref(hfp);
 618 }
 619
 620 struct hfp_gw *hfp_gw_ref(struct hfp_gw *hfp)
 621 {
 622         if (!hfp)
 623                 return NULL;
 624
 625         __sync_fetch_and_add(&hfp->ref_count, 1);
 626
 627         return hfp;
 628 }
 629
 630 void hfp_gw_unref(struct hfp_gw *hfp)
 631 {
 632         if (!hfp)
 633                 return;
 634
 635         if (__sync_sub_and_fetch(&hfp->ref_count, 1))
 636                 return;
 637
 638         hfp_gw_set_command_handler(hfp, NULL, NULL, NULL);
 639
 640         io_set_write_handler(hfp->io, NULL, NULL, NULL);
 641         io_set_read_handler(hfp->io, NULL, NULL, NULL);
 642         io_set_disconnect_handler(hfp->io, NULL, NULL, NULL);
 643
 644         io_destroy(hfp->io);
 645         hfp->io = NULL;
 646
 647         if (hfp->close_on_unref)
 648                 close(hfp->fd);
 649
 650         hfp_gw_set_debug(hfp, NULL, NULL, NULL);
 651
 652         ringbuf_free(hfp->read_buf);
 653         hfp->read_buf = NULL;
 654
 655         ringbuf_free(hfp->write_buf);
 656         hfp->write_buf = NULL;
 657
 658         queue_destroy(hfp->cmd_handlers, destroy_cmd_handler);
 659         hfp->cmd_handlers = NULL;
 660
 661         if (!hfp->in_disconnect) {
 662                 free(hfp);
 663                 return;
 664         }
 665
 666         hfp->destroyed = true;
 667 }
 668
 669 static void read_tracing(const void *buf, size_t count, void *user_data)
 670 {
 671         struct hfp_gw *hfp = user_data;
 672
 673         util_hexdump('>', buf, count, hfp->debug_callback, hfp->debug_data);
 674 }
 675
 676 static void write_tracing(const void *buf, size_t count, void *user_data)
 677 {
 678         struct hfp_gw *hfp = user_data;
 679
 680         util_hexdump('<', buf, count, hfp->debug_callback, hfp->debug_data);
 681 }
 682
 683 bool hfp_gw_set_debug(struct hfp_gw *hfp, hfp_debug_func_t callback,
 684                                 void *user_data, hfp_destroy_func_t destroy)
 685 {
 686         if (!hfp)
 687                 return false;
 688
 689         if (hfp->debug_destroy)
 690                 hfp->debug_destroy(hfp->debug_data);
 691
 692         hfp->debug_callback = callback;
 693         hfp->debug_destroy = destroy;
 694         hfp->debug_data = user_data;
 695
 696         if (hfp->debug_callback) {
 697                 ringbuf_set_input_tracing(hfp->read_buf, read_tracing, hfp);
 698                 ringbuf_set_input_tracing(hfp->write_buf, write_tracing, hfp);
 699         } else {
 700                 ringbuf_set_input_tracing(hfp->read_buf, NULL, NULL);
 701                 ringbuf_set_input_tracing(hfp->write_buf, NULL, NULL);
 702         }
 703
 704         return true;
 705 }
 706
 707 bool hfp_gw_set_close_on_unref(struct hfp_gw *hfp, bool do_close)
 708 {
 709         if (!hfp)
 710                 return false;
 711
 712         hfp->close_on_unref = do_close;
 713
 714         return true;
 715 }
 716
 717 bool hfp_gw_send_result(struct hfp_gw *hfp, enum hfp_result result)
 718 {
 719         const char *str;
 720
 721         if (!hfp)
 722                 return false;
 723
 724         switch (result) {
 725         case HFP_RESULT_OK:
 726                 str = "OK";
 727                 break;
 728         case HFP_RESULT_ERROR:
 729                 str = "ERROR";
 730                 break;
 731         case HFP_RESULT_RING:
 732         case HFP_RESULT_NO_CARRIER:
 733         case HFP_RESULT_BUSY:
 734         case HFP_RESULT_NO_ANSWER:
 735         case HFP_RESULT_DELAYED:
 736         case HFP_RESULT_BLACKLISTED:
 737         case HFP_RESULT_CME_ERROR:
 738         case HFP_RESULT_NO_DIALTONE:
 739         case HFP_RESULT_CONNECT:
 740         default:
 741                 return false;
 742         }
 743
 744         if (ringbuf_printf(hfp->write_buf, "\r\n%s\r\n", str) < 0)
 745                 return false;
 746
 747         wakeup_writer(hfp);
 748
 749         /*
 750          * There might be already something to read in the ring buffer.
 751          * If so, let's read it.
 752          */
 753         if (hfp->result_pending) {
 754                 hfp->result_pending = false;
 755                 process_input(hfp);
 756         }
 757
 758         return true;
 759 }
 760
 761 bool hfp_gw_send_error(struct hfp_gw *hfp, enum hfp_error error)
 762 {
 763         if (!hfp)
 764                 return false;
 765
 766         if (ringbuf_printf(hfp->write_buf, "\r\n+CME ERROR: %u\r\n", error) < 0)
 767                 return false;
 768
 769         wakeup_writer(hfp);
 770
 771         /*
 772          * There might be already something to read in the ring buffer.
 773          * If so, let's read it.
 774          */
 775         if (hfp->result_pending) {
 776                 hfp->result_pending = false;
 777                 process_input(hfp);
 778         }
 779
 780         return true;
 781 }
 782
 783 bool hfp_gw_send_info(struct hfp_gw *hfp, const char *format, ...)
 784 {
 785         va_list ap;
 786         char *fmt;
 787         int len;
 788
 789         if (!hfp || !format)
 790                 return false;
 791
 792         if (asprintf(&fmt, "\r\n%s\r\n", format) < 0)
 793                 return false;
 794
 795         va_start(ap, format);
 796         len = ringbuf_vprintf(hfp->write_buf, fmt, ap);
 797         va_end(ap);
 798
 799         free(fmt);
 800
 801         if (len < 0)
 802                 return false;
 803
 804         if (hfp->result_pending)
 805                 return true;
 806
 807         wakeup_writer(hfp);
 808
 809         return true;
 810 }
 811
 812 bool hfp_gw_set_command_handler(struct hfp_gw *hfp,
 813                                 hfp_command_func_t callback,
 814                                 void *user_data, hfp_destroy_func_t destroy)
 815 {
 816         if (!hfp)
 817                 return false;
 818
 819         if (hfp->command_destroy)
 820                 hfp->command_destroy(hfp->command_data);
 821
 822         hfp->command_callback = callback;
 823         hfp->command_destroy = destroy;
 824         hfp->command_data = user_data;
 825
 826         return true;
 827 }
 828
 829 bool hfp_gw_register(struct hfp_gw *hfp, hfp_result_func_t callback,
 830                                                 const char *prefix,
 831                                                 void *user_data,
 832                                                 hfp_destroy_func_t destroy)
 833 {
 834         struct cmd_handler *handler;
 835
 836         handler = new0(struct cmd_handler, 1);
 837         handler->callback = callback;
 838         handler->user_data = user_data;
 839
 840         handler->prefix = strdup(prefix);
 841         if (!handler->prefix) {
 842                 free(handler);
 843                 return false;
 844         }
 845
 846         if (queue_find(hfp->cmd_handlers, match_handler_prefix,
 847                                                         handler->prefix)) {
 848                 destroy_cmd_handler(handler);
 849                 return false;
 850         }
 851
 852         handler->destroy = destroy;
 853
 854         return queue_push_tail(hfp->cmd_handlers, handler);
 855 }
 856
 857 bool hfp_gw_unregister(struct hfp_gw *hfp, const char *prefix)
 858 {
 859         struct cmd_handler *handler;
 860         char *lookup_prefix;
 861
 862         lookup_prefix = strdup(prefix);
 863         if (!lookup_prefix)
 864                 return false;
 865
 866         handler = queue_remove_if(hfp->cmd_handlers, match_handler_prefix,
 867                                                                 lookup_prefix);
 868         free(lookup_prefix);
 869
 870         if (!handler)
 871                 return false;
 872
 873         destroy_cmd_handler(handler);
 874
 875         return true;
 876 }
 877
 878 static void disconnect_watch_destroy(void *user_data)
 879 {
 880         struct hfp_gw *hfp = user_data;
 881
 882         if (hfp->disconnect_destroy)
 883                 hfp->disconnect_destroy(hfp->disconnect_data);
 884
 885         if (hfp->destroyed)
 886                 free(hfp);
 887 }
 888
 889 static bool io_disconnected(struct io *io, void *user_data)
 890 {
 891         struct hfp_gw *hfp = user_data;
 892
 893         hfp->in_disconnect = true;
 894
 895         if (hfp->disconnect_callback)
 896                 hfp->disconnect_callback(hfp->disconnect_data);
 897
 898         hfp->in_disconnect = false;
 899
 900         return false;
 901 }
 902
 903 bool hfp_gw_set_disconnect_handler(struct hfp_gw *hfp,
 904                                         hfp_disconnect_func_t callback,
 905                                         void *user_data,
 906                                         hfp_destroy_func_t destroy)
 907 {
 908         if (!hfp)
 909                 return false;
 910
 911         if (hfp->disconnect_destroy)
 912                 hfp->disconnect_destroy(hfp->disconnect_data);
 913
 914         if (!io_set_disconnect_handler(hfp->io, io_disconnected, hfp,
 915                                                 disconnect_watch_destroy)) {
 916                 hfp->disconnect_callback = NULL;
 917                 hfp->disconnect_destroy = NULL;
 918                 hfp->disconnect_data = NULL;
 919                 return false;
 920         }
 921
 922         hfp->disconnect_callback = callback;
 923         hfp->disconnect_destroy = destroy;
 924         hfp->disconnect_data = user_data;
 925
 926         return true;
 927 }
 928
 929 bool hfp_gw_disconnect(struct hfp_gw *hfp)
 930 {
 931         if (!hfp)
 932                 return false;
 933
 934         return io_shutdown(hfp->io);
 935 }
 936
 937 static bool match_handler_event_prefix(const void *a, const void *b)
 938 {
 939         const struct event_handler *handler = a;
 940         const char *prefix = b;
 941
 942         if (strcmp(handler->prefix, prefix) != 0)
 943                 return false;
 944
 945         return true;
 946 }
 947
 948 static void destroy_event_handler(void *data)
 949 {
 950         struct event_handler *handler = data;
 951
 952         if (handler->destroy)
 953                 handler->destroy(handler->user_data);
 954
 955         free(handler->prefix);
 956
 957         free(handler);
 958 }
 959
 960 static bool hf_can_write_data(struct io *io, void *user_data)
 961 {
 962         struct hfp_hf *hfp = user_data;
 963         ssize_t bytes_written;
 964
 965         bytes_written = ringbuf_write(hfp->write_buf, hfp->fd);
 966         if (bytes_written < 0)
 967                 return false;
 968
 969         if (ringbuf_len(hfp->write_buf) > 0)
 970                 return true;
 971
 972         return false;
 973 }
 974
 975 static void hf_write_watch_destroy(void *user_data)
 976 {
 977         struct hfp_hf *hfp = user_data;
 978
 979         hfp->writer_active = false;
 980 }
 981
 982 static void hf_skip_whitespace(struct hfp_context *context)
 983 {
 984         while (context->data[context->offset] == ' ')
 985                 context->offset++;
 986 }
 987
 988 static bool is_response(const char *prefix, enum hfp_result *result,
 989                                                 enum hfp_error *cme_err,
 990                                                 struct hfp_context *context)
 991 {
 992         if (strcmp(prefix, "OK") == 0) {
 993                 *result = HFP_RESULT_OK;
 994                 /*
 995                  * Set cme_err to 0 as this is not valid when result is not
 996                  * CME ERROR
 997                  */
 998                 *cme_err = 0;
 999                 return true;
1000         }
1001
1002         if (strcmp(prefix, "ERROR") == 0) {
1003                 *result = HFP_RESULT_ERROR;
1004                 *cme_err = 0;
1005                 return true;
1006         }
1007
1008         if (strcmp(prefix, "NO CARRIER") == 0) {
1009                 *result = HFP_RESULT_NO_CARRIER;
1010                 *cme_err = 0;
1011                 return true;
1012         }
1013
1014         if (strcmp(prefix, "NO ANSWER") == 0) {
1015                 *result = HFP_RESULT_NO_ANSWER;
1016                 *cme_err = 0;
1017                 return true;
1018         }
1019
1020         if (strcmp(prefix, "BUSY") == 0) {
1021                 *result = HFP_RESULT_BUSY;
1022                 *cme_err = 0;
1023                 return true;
1024         }
1025
1026         if (strcmp(prefix, "DELAYED") == 0) {
1027                 *result = HFP_RESULT_DELAYED;
1028                 *cme_err = 0;
1029                 return true;
1030         }
1031
1032         if (strcmp(prefix, "BLACKLISTED") == 0) {
1033                 *result = HFP_RESULT_BLACKLISTED;
1034                 *cme_err = 0;
1035                 return true;
1036         }
1037
1038         if (strcmp(prefix, "+CME ERROR") == 0) {
1039                 uint32_t val;
1040
1041                 *result = HFP_RESULT_CME_ERROR;
1042
1043                 if (hfp_context_get_number(context, &val) &&
1044                                         val <= HFP_ERROR_NETWORK_NOT_ALLOWED)
1045                         *cme_err = val;
1046                 else
1047                         *cme_err = HFP_ERROR_AG_FAILURE;
1048
1049                 return true;
1050         }
1051
1052         return false;
1053 }
1054
1055 static void hf_wakeup_writer(struct hfp_hf *hfp)
1056 {
1057         if (hfp->writer_active)
1058                 return;
1059
1060         if (!ringbuf_len(hfp->write_buf))
1061                 return;
1062
1063         if (!io_set_write_handler(hfp->io, hf_can_write_data,
1064                                         hfp, hf_write_watch_destroy))
1065                 return;
1066
1067         hfp->writer_active = true;
1068 }
1069
1070 static void hf_call_prefix_handler(struct hfp_hf *hfp, const char *data)
1071 {
1072         struct event_handler *handler;
1073         const char *separators = ";:\0";
1074         struct hfp_context context;
1075         enum hfp_result result;
1076         enum hfp_error cme_err;
1077         char lookup_prefix[18];
1078         uint8_t pref_len = 0;
1079         const char *prefix;
1080         int i;
1081
1082         context.offset = 0;
1083         context.data = data;
1084
1085         hf_skip_whitespace(&context);
1086
1087         if (strlen(data + context.offset) < 2)
1088                 return;
1089
1090         prefix = data + context.offset;
1091
1092         pref_len = strcspn(prefix, separators);
1093         if (pref_len > 17 || pref_len < 2)
1094                 return;
1095
1096         for (i = 0; i < pref_len; i++)
1097                 lookup_prefix[i] = toupper(prefix[i]);
1098
1099         lookup_prefix[pref_len] = '\0';
1100         context.offset += pref_len + 1;
1101
1102         if (is_response(lookup_prefix, &result, &cme_err, &context)) {
1103                 struct cmd_response *cmd;
1104
1105                 cmd = queue_peek_head(hfp->cmd_queue);
1106                 if (!cmd)
1107                         return;
1108
1109                 cmd->resp_cb(result, cme_err, cmd->user_data);
1110
1111                 queue_remove(hfp->cmd_queue, cmd);
1112                 free(cmd);
1113
1114                 hf_wakeup_writer(hfp);
1115                 return;
1116         }
1117
1118         handler = queue_find(hfp->event_handlers, match_handler_event_prefix,
1119                                                                 lookup_prefix);
1120         if (!handler)
1121                 return;
1122
1123         handler->callback(&context, handler->user_data);
1124 }
1125
1126 static char *find_cr_lf(char *str, size_t len)
1127 {
1128         char *ptr;
1129         size_t count, offset;
1130
1131         offset = 0;
1132
1133         ptr = memchr(str, '\r', len);
1134         while (ptr) {
1135                 /*
1136                  * Check if there is more data after '\r'. If so check for
1137                  * '\n'
1138                  */
1139                 count = ptr - str;
1140                 if ((count < (len - 1)) && *(ptr + 1) == '\n')
1141                         return ptr;
1142
1143                 /* There is only '\r'? Let's try to find next one */
1144                 offset += count + 1;
1145
1146                 if (offset >= len)
1147                         return NULL;
1148
1149                 ptr = memchr(str + offset, '\r', len - offset);
1150         }
1151
1152         return NULL;
1153 }
1154
1155 static void hf_process_input(struct hfp_hf *hfp)
1156 {
1157         char *str, *ptr, *str2, *tmp;
1158         size_t len, count, offset, len2;
1159         bool free_tmp = false;
1160
1161         str = ringbuf_peek(hfp->read_buf, 0, &len);
1162         if (!str)
1163                 return;
1164
1165         offset = 0;
1166
1167         ptr = find_cr_lf(str, len);
1168         while (ptr) {
1169                 count = ptr - (str + offset);
1170                 if (count == 0) {
1171                         /* 2 is for <cr><lf> */
1172                         offset += 2;
1173                 } else {
1174                         *ptr = '\0';
1175                         hf_call_prefix_handler(hfp, str + offset);
1176                         offset += count + 2;
1177                 }
1178
1179                 ptr = find_cr_lf(str + offset, len - offset);
1180         }
1181
1182         /*
1183          * Just check if there is no wrapped data in ring buffer.
1184          * Should not happen too often
1185          */
1186         if (len == ringbuf_len(hfp->read_buf))
1187                 goto done;
1188
1189         str2 = ringbuf_peek(hfp->read_buf, len, &len2);
1190         if (!str2)
1191                 goto done;
1192
1193         ptr = find_cr_lf(str2, len2);
1194         if (!ptr) {
1195                 /* Might happen that we wrap between \r and \n */
1196                 ptr = memchr(str2, '\n', len2);
1197                 if (!ptr)
1198                         goto done;
1199         }
1200
1201         count = ptr - str2;
1202
1203         if (count) {
1204                 *ptr = '\0';
1205
1206                 tmp = malloc(len + count);
1207                 if (!tmp)
1208                         goto done;
1209
1210                 /* "str" here is not a string so we need to use memcpy */
1211                 memcpy(tmp, str, len);
1212                 memcpy(tmp + len, str2, count);
1213
1214                 free_tmp = true;
1215         } else {
1216                 str[len-1] = '\0';
1217                 tmp = str;
1218         }
1219
1220         hf_call_prefix_handler(hfp, tmp);
1221         offset += count;
1222
1223 done:
1224         ringbuf_drain(hfp->read_buf, offset);
1225
1226         if (free_tmp)
1227                 free(tmp);
1228 }
1229
1230 static bool hf_can_read_data(struct io *io, void *user_data)
1231 {
1232         struct hfp_hf *hfp = user_data;
1233         ssize_t bytes_read;
1234
1235         bytes_read = ringbuf_read(hfp->read_buf, hfp->fd);
1236         if (bytes_read < 0)
1237                 return false;
1238
1239         hf_process_input(hfp);
1240
1241         return true;
1242 }
1243
1244 struct hfp_hf *hfp_hf_new(int fd)
1245 {
1246         struct hfp_hf *hfp;
1247
1248         if (fd < 0)
1249                 return NULL;
1250
1251         hfp = new0(struct hfp_hf, 1);
1252         hfp->fd = fd;
1253         hfp->close_on_unref = false;
1254
1255         hfp->read_buf = ringbuf_new(4096);
1256         if (!hfp->read_buf) {
1257                 free(hfp);
1258                 return NULL;
1259         }
1260
1261         hfp->write_buf = ringbuf_new(4096);
1262         if (!hfp->write_buf) {
1263                 ringbuf_free(hfp->read_buf);
1264                 free(hfp);
1265                 return NULL;
1266         }
1267
1268         hfp->io = io_new(fd);
1269         if (!hfp->io) {
1270                 ringbuf_free(hfp->write_buf);
1271                 ringbuf_free(hfp->read_buf);
1272                 free(hfp);
1273                 return NULL;
1274         }
1275
1276         hfp->event_handlers = queue_new();
1277         hfp->cmd_queue = queue_new();
1278         hfp->writer_active = false;
1279
1280         if (!io_set_read_handler(hfp->io, hf_can_read_data, hfp,
1281                                                         read_watch_destroy)) {
1282                 queue_destroy(hfp->event_handlers,
1283                                                 destroy_event_handler);
1284                 io_destroy(hfp->io);
1285                 ringbuf_free(hfp->write_buf);
1286                 ringbuf_free(hfp->read_buf);
1287                 free(hfp);
1288                 return NULL;
1289         }
1290
1291         return hfp_hf_ref(hfp);
1292 }
1293
1294 struct hfp_hf *hfp_hf_ref(struct hfp_hf *hfp)
1295 {
1296         if (!hfp)
1297                 return NULL;
1298
1299         __sync_fetch_and_add(&hfp->ref_count, 1);
1300
1301         return hfp;
1302 }
1303
1304 void hfp_hf_unref(struct hfp_hf *hfp)
1305 {
1306         if (!hfp)
1307                 return;
1308
1309         if (__sync_sub_and_fetch(&hfp->ref_count, 1))
1310                 return;
1311
1312         io_set_write_handler(hfp->io, NULL, NULL, NULL);
1313         io_set_read_handler(hfp->io, NULL, NULL, NULL);
1314         io_set_disconnect_handler(hfp->io, NULL, NULL, NULL);
1315
1316         io_destroy(hfp->io);
1317         hfp->io = NULL;
1318
1319         if (hfp->close_on_unref)
1320                 close(hfp->fd);
1321
1322         hfp_hf_set_debug(hfp, NULL, NULL, NULL);
1323
1324         ringbuf_free(hfp->read_buf);
1325         hfp->read_buf = NULL;
1326
1327         ringbuf_free(hfp->write_buf);
1328         hfp->write_buf = NULL;
1329
1330         queue_destroy(hfp->event_handlers, destroy_event_handler);
1331         hfp->event_handlers = NULL;
1332
1333         queue_destroy(hfp->cmd_queue, free);
1334         hfp->cmd_queue = NULL;
1335
1336         if (!hfp->in_disconnect) {
1337                 free(hfp);
1338                 return;
1339         }
1340
1341         hfp->destroyed = true;
1342 }
1343
1344 static void hf_read_tracing(const void *buf, size_t count,
1345                                                         void *user_data)
1346 {
1347         struct hfp_hf *hfp = user_data;
1348
1349         util_hexdump('>', buf, count, hfp->debug_callback, hfp->debug_data);
1350 }
1351
1352 static void hf_write_tracing(const void *buf, size_t count,
1353                                                         void *user_data)
1354 {
1355         struct hfp_hf *hfp = user_data;
1356
1357         util_hexdump('<', buf, count, hfp->debug_callback, hfp->debug_data);
1358 }
1359
1360 bool hfp_hf_set_debug(struct hfp_hf *hfp, hfp_debug_func_t callback,
1361                                 void *user_data, hfp_destroy_func_t destroy)
1362 {
1363         if (!hfp)
1364                 return false;
1365
1366         if (hfp->debug_destroy)
1367                 hfp->debug_destroy(hfp->debug_data);
1368
1369         hfp->debug_callback = callback;
1370         hfp->debug_destroy = destroy;
1371         hfp->debug_data = user_data;
1372
1373         if (hfp->debug_callback) {
1374                 ringbuf_set_input_tracing(hfp->read_buf, hf_read_tracing, hfp);
1375                 ringbuf_set_input_tracing(hfp->write_buf, hf_write_tracing,
1376                                                                         hfp);
1377         } else {
1378                 ringbuf_set_input_tracing(hfp->read_buf, NULL, NULL);
1379                 ringbuf_set_input_tracing(hfp->write_buf, NULL, NULL);
1380         }
1381
1382         return true;
1383 }
1384
1385 bool hfp_hf_set_close_on_unref(struct hfp_hf *hfp, bool do_close)
1386 {
1387         if (!hfp)
1388                 return false;
1389
1390         hfp->close_on_unref = do_close;
1391
1392         return true;
1393 }
1394
1395 bool hfp_hf_send_command(struct hfp_hf *hfp, hfp_response_func_t resp_cb,
1396                                 void *user_data, const char *format, ...)
1397 {
1398         va_list ap;
1399         char *fmt;
1400         int len;
1401         struct cmd_response *cmd;
1402
1403         if (!hfp || !format || !resp_cb)
1404                 return false;
1405
1406         if (asprintf(&fmt, "%s\r", format) < 0)
1407                 return false;
1408
1409         cmd = new0(struct cmd_response, 1);
1410
1411         va_start(ap, format);
1412         len = ringbuf_vprintf(hfp->write_buf, fmt, ap);
1413         va_end(ap);
1414
1415         free(fmt);
1416
1417         if (len < 0) {
1418                 free(cmd);
1419                 return false;
1420         }
1421
1422         cmd->resp_cb = resp_cb;
1423         cmd->user_data = user_data;
1424
1425         if (!queue_push_tail(hfp->cmd_queue, cmd)) {
1426                 ringbuf_drain(hfp->write_buf, len);
1427                 free(cmd);
1428                 return false;
1429         }
1430
1431         hf_wakeup_writer(hfp);
1432
1433         return true;
1434 }
1435
1436 bool hfp_hf_register(struct hfp_hf *hfp, hfp_hf_result_func_t callback,
1437                                                 const char *prefix,
1438                                                 void *user_data,
1439                                                 hfp_destroy_func_t destroy)
1440 {
1441         struct event_handler *handler;
1442
1443         if (!callback)
1444                 return false;
1445
1446         handler = new0(struct event_handler, 1);
1447         handler->callback = callback;
1448         handler->user_data = user_data;
1449
1450         handler->prefix = strdup(prefix);
1451         if (!handler->prefix) {
1452                 free(handler);
1453                 return false;
1454         }
1455
1456         if (queue_find(hfp->event_handlers, match_handler_event_prefix,
1457                                                         handler->prefix)) {
1458                 destroy_event_handler(handler);
1459                 return false;
1460         }
1461
1462         handler->destroy = destroy;
1463
1464         return queue_push_tail(hfp->event_handlers, handler);
1465 }
1466
1467 bool hfp_hf_unregister(struct hfp_hf *hfp, const char *prefix)
1468 {
1469         struct cmd_handler *handler;
1470
1471         /* Cast to void as queue_remove needs that */
1472         handler = queue_remove_if(hfp->event_handlers,
1473                                                 match_handler_event_prefix,
1474                                                 (void *) prefix);
1475
1476         if (!handler)
1477                 return false;
1478
1479         destroy_event_handler(handler);
1480
1481         return true;
1482 }
1483
1484 static void hf_disconnect_watch_destroy(void *user_data)
1485 {
1486         struct hfp_hf *hfp = user_data;
1487
1488         if (hfp->disconnect_destroy)
1489                 hfp->disconnect_destroy(hfp->disconnect_data);
1490
1491         if (hfp->destroyed)
1492                 free(hfp);
1493 }
1494
1495 static bool hf_io_disconnected(struct io *io, void *user_data)
1496 {
1497         struct hfp_hf *hfp = user_data;
1498
1499         hfp->in_disconnect = true;
1500
1501         if (hfp->disconnect_callback)
1502                 hfp->disconnect_callback(hfp->disconnect_data);
1503
1504         hfp->in_disconnect = false;
1505
1506         return false;
1507 }
1508
1509 bool hfp_hf_set_disconnect_handler(struct hfp_hf *hfp,
1510                                                 hfp_disconnect_func_t callback,
1511                                                 void *user_data,
1512                                                 hfp_destroy_func_t destroy)
1513 {
1514         if (!hfp)
1515                 return false;
1516
1517         if (hfp->disconnect_destroy)
1518                 hfp->disconnect_destroy(hfp->disconnect_data);
1519
1520         if (!io_set_disconnect_handler(hfp->io, hf_io_disconnected, hfp,
1521                                                 hf_disconnect_watch_destroy)) {
1522                 hfp->disconnect_callback = NULL;
1523                 hfp->disconnect_destroy = NULL;
1524                 hfp->disconnect_data = NULL;
1525                 return false;
1526         }
1527
1528         hfp->disconnect_callback = callback;
1529         hfp->disconnect_destroy = destroy;
1530         hfp->disconnect_data = user_data;
1531
1532         return true;
1533 }
1534
1535 bool hfp_hf_disconnect(struct hfp_hf *hfp)
1536 {
1537         if (!hfp)
1538                 return false;
1539
1540         return io_shutdown(hfp->io);
1541 }