blob: 36c8c3e2fa6ef5f93df5dc7c3fe7d91b244cde91 [file] [log] [blame] [edit]
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdarg.h>
#include <ctype.h>
#include "src/shared/util.h"
#include "src/shared/ringbuf.h"
#include "src/shared/queue.h"
#include "src/shared/io.h"
#include "src/shared/hfp.h"
struct hfp_gw {
int ref_count;
int fd;
bool close_on_unref;
struct io *io;
struct ringbuf *read_buf;
struct ringbuf *write_buf;
struct queue *cmd_handlers;
bool writer_active;
bool permissive_syntax;
bool result_pending;
hfp_command_func_t command_callback;
hfp_destroy_func_t command_destroy;
void *command_data;
hfp_debug_func_t debug_callback;
hfp_destroy_func_t debug_destroy;
void *debug_data;
hfp_disconnect_func_t disconnect_callback;
hfp_destroy_func_t disconnect_destroy;
void *disconnect_data;
bool in_disconnect;
bool destroyed;
};
struct cmd_handler {
char *prefix;
void *user_data;
hfp_destroy_func_t destroy;
hfp_result_func_t callback;
};
struct hfp_gw_result {
const char *data;
unsigned int offset;
};
static void destroy_cmd_handler(void *data)
{
struct cmd_handler *handler = data;
if (handler->destroy)
handler->destroy(handler->user_data);
free(handler->prefix);
free(handler);
}
static bool match_handler_prefix(const void *a, const void *b)
{
const struct cmd_handler *handler = a;
const char *prefix = b;
if (strlen(handler->prefix) != strlen(prefix))
return false;
if (memcmp(handler->prefix, prefix, strlen(prefix)))
return false;
return true;
}
static void write_watch_destroy(void *user_data)
{
struct hfp_gw *hfp = user_data;
hfp->writer_active = false;
}
static bool can_write_data(struct io *io, void *user_data)
{
struct hfp_gw *hfp = user_data;
ssize_t bytes_written;
bytes_written = ringbuf_write(hfp->write_buf, hfp->fd);
if (bytes_written < 0)
return false;
if (ringbuf_len(hfp->write_buf) > 0)
return true;
return false;
}
static void wakeup_writer(struct hfp_gw *hfp)
{
if (hfp->writer_active)
return;
if (!ringbuf_len(hfp->write_buf))
return;
if (!io_set_write_handler(hfp->io, can_write_data,
hfp, write_watch_destroy))
return;
hfp->writer_active = true;
}
static void skip_whitespace(struct hfp_gw_result *result)
{
while (result->data[result->offset] == ' ')
result->offset++;
}
static bool call_prefix_handler(struct hfp_gw *hfp, const char *data)
{
struct cmd_handler *handler;
const char *separators = ";?=\0";
struct hfp_gw_result result;
enum hfp_gw_cmd_type type;
char lookup_prefix[18];
uint8_t pref_len = 0;
const char *prefix;
int i;
result.offset = 0;
result.data = data;
skip_whitespace(&result);
if (strlen(data + result.offset) < 3)
return false;
if (strncmp(data + result.offset, "AT", 2))
if (strncmp(data + result.offset, "at", 2))
return false;
result.offset += 2;
prefix = data + result.offset;
if (isalpha(prefix[0])) {
lookup_prefix[pref_len++] = toupper(prefix[0]);
} else {
pref_len = strcspn(prefix, separators);
if (pref_len > 17 || pref_len < 2)
return false;
for (i = 0; i < pref_len; i++)
lookup_prefix[i] = toupper(prefix[i]);
}
lookup_prefix[pref_len] = '\0';
result.offset += pref_len;
if (lookup_prefix[0] == 'D') {
type = HFP_GW_CMD_TYPE_SET;
goto done;
}
if (data[result.offset] == '=') {
result.offset++;
if (data[result.offset] == '?') {
result.offset++;
type = HFP_GW_CMD_TYPE_TEST;
} else {
type = HFP_GW_CMD_TYPE_SET;
}
goto done;
}
if (data[result.offset] == '?') {
result.offset++;
type = HFP_GW_CMD_TYPE_READ;
goto done;
}
type = HFP_GW_CMD_TYPE_COMMAND;
done:
handler = queue_find(hfp->cmd_handlers, match_handler_prefix,
lookup_prefix);
if (!handler)
return false;
handler->callback(&result, type, handler->user_data);
return true;
}
static void next_field(struct hfp_gw_result *result)
{
if (result->data[result->offset] == ',')
result->offset++;
}
bool hfp_gw_result_get_number_default(struct hfp_gw_result *result,
unsigned int *val,
unsigned int default_val)
{
skip_whitespace(result);
if (result->data[result->offset] == ',') {
if (val)
*val = default_val;
result->offset++;
return true;
}
return hfp_gw_result_get_number(result, val);
}
bool hfp_gw_result_get_number(struct hfp_gw_result *result, unsigned int *val)
{
unsigned int i;
int tmp = 0;
skip_whitespace(result);
i = result->offset;
while (result->data[i] >= '0' && result->data[i] <= '9')
tmp = tmp * 10 + result->data[i++] - '0';
if (i == result->offset)
return false;
if (val)
*val = tmp;
result->offset = i;
skip_whitespace(result);
next_field(result);
return true;
}
bool hfp_gw_result_open_container(struct hfp_gw_result *result)
{
skip_whitespace(result);
/* The list shall be preceded by a left parenthesis "(") */
if (result->data[result->offset] != '(')
return false;
result->offset++;
return true;
}
bool hfp_gw_result_close_container(struct hfp_gw_result *result)
{
skip_whitespace(result);
/* The list shall be followed by a right parenthesis (")" V250 5.7.3.1*/
if (result->data[result->offset] != ')')
return false;
result->offset++;
return true;
}
bool hfp_gw_result_get_string(struct hfp_gw_result *result, char *buf,
uint8_t len)
{
int i = 0;
const char *data = result->data;
unsigned int offset;
skip_whitespace(result);
if (data[result->offset] != '"')
return false;
offset = result->offset;
offset++;
while (data[offset] != '\0' && data[offset] != '"') {
if (i == len)
return false;
buf[i++] = data[offset];
offset++;
}
if (i == len)
return false;
buf[i] = '\0';
if (data[offset] == '"')
offset++;
else
return false;
result->offset = offset;
skip_whitespace(result);
next_field(result);
return true;
}
bool hfp_gw_result_get_unquoted_string(struct hfp_gw_result *result, char *buf,
uint8_t len)
{
const char *data = result->data;
unsigned int offset;
int i = 0;
char c;
skip_whitespace(result);
c = data[result->offset];
if (c == '"' || c == ')' || c == '(')
return false;
offset = result->offset;
while (data[offset] != '\0' && data[offset] != ',' &&
data[offset] != ')') {
if (i == len)
return false;
buf[i++] = data[offset];
offset++;
}
if (i == len)
return false;
buf[i] = '\0';
result->offset = offset;
next_field(result);
return true;
}
bool hfp_gw_result_has_next(struct hfp_gw_result *result)
{
return result->data[result->offset] != '\0';
}
static void process_input(struct hfp_gw *hfp)
{
char *str, *ptr;
size_t len, count;
str = ringbuf_peek(hfp->read_buf, 0, &len);
if (!str)
return;
ptr = memchr(str, '\r', len);
if (!ptr) {
char *str2;
size_t len2;
/* If there is no more data in ringbuffer,
* it's just an incomplete command.
*/
if (len == ringbuf_len(hfp->read_buf))
return;
str2 = ringbuf_peek(hfp->read_buf, len, &len2);
if (!str2)
return;
ptr = memchr(str2, '\r', len2);
if (!ptr)
return;
*ptr = '\0';
count = asprintf(&ptr, "%s%s", str, str2);
str = ptr;
} else {
count = ptr - str;
*ptr = '\0';
}
hfp->result_pending = true;
if (!call_prefix_handler(hfp, str)) {
if (hfp->command_callback)
hfp->command_callback(str, hfp->command_data);
else
hfp_gw_send_result(hfp, HFP_RESULT_ERROR);
}
len = ringbuf_drain(hfp->read_buf, count + 1);
if (str == ptr)
free(ptr);
}
static void read_watch_destroy(void *user_data)
{
}
static bool can_read_data(struct io *io, void *user_data)
{
struct hfp_gw *hfp = user_data;
ssize_t bytes_read;
bytes_read = ringbuf_read(hfp->read_buf, hfp->fd);
if (bytes_read < 0)
return false;
if (hfp->result_pending)
return true;
process_input(hfp);
return true;
}
struct hfp_gw *hfp_gw_new(int fd)
{
struct hfp_gw *hfp;
if (fd < 0)
return NULL;
hfp = new0(struct hfp_gw, 1);
if (!hfp)
return NULL;
hfp->fd = fd;
hfp->close_on_unref = false;
hfp->read_buf = ringbuf_new(4096);
if (!hfp->read_buf) {
free(hfp);
return NULL;
}
hfp->write_buf = ringbuf_new(4096);
if (!hfp->write_buf) {
ringbuf_free(hfp->read_buf);
free(hfp);
return NULL;
}
hfp->io = io_new(fd);
if (!hfp->io) {
ringbuf_free(hfp->write_buf);
ringbuf_free(hfp->read_buf);
free(hfp);
return NULL;
}
hfp->cmd_handlers = queue_new();
if (!hfp->cmd_handlers) {
io_destroy(hfp->io);
ringbuf_free(hfp->write_buf);
ringbuf_free(hfp->read_buf);
free(hfp);
return NULL;
}
if (!io_set_read_handler(hfp->io, can_read_data,
hfp, read_watch_destroy)) {
queue_destroy(hfp->cmd_handlers,
destroy_cmd_handler);
io_destroy(hfp->io);
ringbuf_free(hfp->write_buf);
ringbuf_free(hfp->read_buf);
free(hfp);
return NULL;
}
hfp->writer_active = false;
hfp->permissive_syntax = false;
hfp->result_pending = false;
return hfp_gw_ref(hfp);
}
struct hfp_gw *hfp_gw_ref(struct hfp_gw *hfp)
{
if (!hfp)
return NULL;
__sync_fetch_and_add(&hfp->ref_count, 1);
return hfp;
}
void hfp_gw_unref(struct hfp_gw *hfp)
{
if (!hfp)
return;
if (__sync_sub_and_fetch(&hfp->ref_count, 1))
return;
hfp_gw_set_command_handler(hfp, NULL, NULL, NULL);
io_set_write_handler(hfp->io, NULL, NULL, NULL);
io_set_read_handler(hfp->io, NULL, NULL, NULL);
io_set_disconnect_handler(hfp->io, NULL, NULL, NULL);
io_destroy(hfp->io);
hfp->io = NULL;
if (hfp->close_on_unref)
close(hfp->fd);
hfp_gw_set_debug(hfp, NULL, NULL, NULL);
ringbuf_free(hfp->read_buf);
hfp->read_buf = NULL;
ringbuf_free(hfp->write_buf);
hfp->write_buf = NULL;
queue_destroy(hfp->cmd_handlers, destroy_cmd_handler);
hfp->cmd_handlers = NULL;
if (!hfp->in_disconnect) {
free(hfp);
return;
}
hfp->destroyed = true;
}
static void read_tracing(const void *buf, size_t count, void *user_data)
{
struct hfp_gw *hfp = user_data;
util_hexdump('>', buf, count, hfp->debug_callback, hfp->debug_data);
}
static void write_tracing(const void *buf, size_t count, void *user_data)
{
struct hfp_gw *hfp = user_data;
util_hexdump('<', buf, count, hfp->debug_callback, hfp->debug_data);
}
bool hfp_gw_set_debug(struct hfp_gw *hfp, hfp_debug_func_t callback,
void *user_data, hfp_destroy_func_t destroy)
{
if (!hfp)
return false;
if (hfp->debug_destroy)
hfp->debug_destroy(hfp->debug_data);
hfp->debug_callback = callback;
hfp->debug_destroy = destroy;
hfp->debug_data = user_data;
if (hfp->debug_callback) {
ringbuf_set_input_tracing(hfp->read_buf, read_tracing, hfp);
ringbuf_set_input_tracing(hfp->write_buf, write_tracing, hfp);
} else {
ringbuf_set_input_tracing(hfp->read_buf, NULL, NULL);
ringbuf_set_input_tracing(hfp->write_buf, NULL, NULL);
}
return true;
}
bool hfp_gw_set_close_on_unref(struct hfp_gw *hfp, bool do_close)
{
if (!hfp)
return false;
hfp->close_on_unref = do_close;
return true;
}
bool hfp_gw_set_permissive_syntax(struct hfp_gw *hfp, bool permissive)
{
if (!hfp)
return false;
hfp->permissive_syntax = permissive;
return true;
}
bool hfp_gw_send_result(struct hfp_gw *hfp, enum hfp_result result)
{
const char *str;
if (!hfp)
return false;
switch (result) {
case HFP_RESULT_OK:
str = "OK";
break;
case HFP_RESULT_ERROR:
str = "ERROR";
break;
default:
return false;
}
if (ringbuf_printf(hfp->write_buf, "\r\n%s\r\n", str) < 0)
return false;
wakeup_writer(hfp);
hfp->result_pending = false;
return true;
}
bool hfp_gw_send_error(struct hfp_gw *hfp, enum hfp_error error)
{
if (!hfp)
return false;
if (ringbuf_printf(hfp->write_buf, "\r\n+CME ERROR: %u\r\n", error) < 0)
return false;
wakeup_writer(hfp);
hfp->result_pending = false;
return true;
}
bool hfp_gw_send_info(struct hfp_gw *hfp, const char *format, ...)
{
va_list ap;
char *fmt;
int len;
if (!hfp || !format)
return false;
if (asprintf(&fmt, "\r\n%s\r\n", format) < 0)
return false;
va_start(ap, format);
len = ringbuf_vprintf(hfp->write_buf, fmt, ap);
va_end(ap);
free(fmt);
if (len < 0)
return false;
if (hfp->result_pending)
return true;
wakeup_writer(hfp);
return true;
}
bool hfp_gw_set_command_handler(struct hfp_gw *hfp,
hfp_command_func_t callback,
void *user_data, hfp_destroy_func_t destroy)
{
if (!hfp)
return false;
if (hfp->command_destroy)
hfp->command_destroy(hfp->command_data);
hfp->command_callback = callback;
hfp->command_destroy = destroy;
hfp->command_data = user_data;
return true;
}
bool hfp_gw_register(struct hfp_gw *hfp, hfp_result_func_t callback,
const char *prefix,
void *user_data,
hfp_destroy_func_t destroy)
{
struct cmd_handler *handler;
handler = new0(struct cmd_handler, 1);
if (!handler)
return false;
handler->callback = callback;
handler->user_data = user_data;
handler->prefix = strdup(prefix);
if (!handler->prefix) {
free(handler);
return false;
}
if (queue_find(hfp->cmd_handlers, match_handler_prefix,
handler->prefix)) {
destroy_cmd_handler(handler);
return false;
}
handler->destroy = destroy;
return queue_push_tail(hfp->cmd_handlers, handler);
}
bool hfp_gw_unregister(struct hfp_gw *hfp, const char *prefix)
{
struct cmd_handler *handler;
char *lookup_prefix;
lookup_prefix = strdup(prefix);
if (!lookup_prefix)
return false;
handler = queue_remove_if(hfp->cmd_handlers, match_handler_prefix,
lookup_prefix);
free(lookup_prefix);
if (!handler)
return false;
destroy_cmd_handler(handler);
return true;
}
static void disconnect_watch_destroy(void *user_data)
{
struct hfp_gw *hfp = user_data;
if (hfp->disconnect_destroy)
hfp->disconnect_destroy(hfp->disconnect_data);
if (hfp->destroyed)
free(hfp);
}
static bool io_disconnected(struct io *io, void *user_data)
{
struct hfp_gw *hfp = user_data;
hfp->in_disconnect = true;
if (hfp->disconnect_callback)
hfp->disconnect_callback(hfp->disconnect_data);
hfp->in_disconnect = false;
return false;
}
bool hfp_gw_set_disconnect_handler(struct hfp_gw *hfp,
hfp_disconnect_func_t callback,
void *user_data,
hfp_destroy_func_t destroy)
{
if (!hfp)
return false;
if (hfp->disconnect_destroy)
hfp->disconnect_destroy(hfp->disconnect_data);
if (!io_set_disconnect_handler(hfp->io, io_disconnected, hfp,
disconnect_watch_destroy)) {
hfp->disconnect_callback = NULL;
hfp->disconnect_destroy = NULL;
hfp->disconnect_data = NULL;
return false;
}
hfp->disconnect_callback = callback;
hfp->disconnect_destroy = destroy;
hfp->disconnect_data = user_data;
return true;
}
bool hfp_gw_disconnect(struct hfp_gw *hfp)
{
if (!hfp)
return false;
return io_shutdown(hfp->io);
}