static void handle_rsp(struct bt_att *att, uint8_t opcode, uint8_t *pdu,
ssize_t pdu_len)
{
struct att_send_op *op = att->pending_req;
uint8_t req_opcode;
uint8_t rsp_opcode;
uint8_t *rsp_pdu = NULL;
uint16_t rsp_pdu_len = 0;
/*
* If no request is pending, then the response is unexpected. Disconnect
* the bearer.
*/
if (!op) {
util_debug(att->debug_callback, att->debug_data,
"Received unexpected ATT response");
io_shutdown(att->io);
return;
}
/*
* If the received response doesn't match the pending request, or if
* the request is malformed, end the current request with failure.
*/
if (opcode == BT_ATT_OP_ERROR_RSP) {
/* Return if error response cause a retry */
if (handle_error_rsp(att, pdu, pdu_len, &req_opcode)) {
wakeup_writer(att);
return;
}
} else if (!(req_opcode = get_req_opcode(opcode)))
goto fail;
if (req_opcode != op->opcode)
goto fail;
rsp_opcode = opcode;
if (pdu_len > 0) {
rsp_pdu = pdu;
rsp_pdu_len = pdu_len;
}
goto done;
fail:
util_debug(att->debug_callback, att->debug_data,
"Failed to handle response PDU; opcode: 0x%02x", opcode);
rsp_opcode = BT_ATT_OP_ERROR_RSP;
done:
if (op->callback)
op->callback(rsp_opcode, rsp_pdu, rsp_pdu_len, op->user_data);
destroy_att_send_op(op);
att->pending_req = NULL;
wakeup_writer(att);
}
bool mgmt_cancel(struct mgmt *mgmt, unsigned int id)
{
struct mgmt_request *request;
if (!mgmt || !id)
return false;
request = queue_remove_if(mgmt->request_queue, match_request_id,
UINT_TO_PTR(id));
if (request)
goto done;
request = queue_remove_if(mgmt->reply_queue, match_request_id,
UINT_TO_PTR(id));
if (request)
goto done;
request = queue_remove_if(mgmt->pending_list, match_request_id,
UINT_TO_PTR(id));
if (!request)
return false;
done:
destroy_request(request);
wakeup_writer(mgmt);
return true;
}
static GSource *channel_create_watch(GIOChannel *channel,
GIOCondition condition)
{
GSource *source;
GAtMuxWatch *watch;
GAtMuxChannel *dlc = (GAtMuxChannel *) channel;
GAtMux *mux = dlc->mux;
source = g_source_new(&watch_funcs, sizeof(GAtMuxWatch));
watch = (GAtMuxWatch *) source;
watch->channel = channel;
g_io_channel_ref(channel);
watch->condition = condition;
if ((watch->condition & G_IO_OUT) && dlc->throttled == FALSE)
wakeup_writer(mux);
debug(mux, "creating source: %p, channel: %p, writer: %d, reader: %d",
watch, channel,
condition & G_IO_OUT,
condition & G_IO_IN);
dlc->sources = g_slist_prepend(dlc->sources, watch);
return source;
}
void g_at_mux_set_dlc_status(GAtMux *mux, guint8 dlc, int status)
{
GAtMuxChannel *channel;
debug(mux, "got status %d, for channel %hu", status, dlc);
if (dlc < 1 || dlc > MAX_CHANNELS)
return;
channel = mux->dlcs[dlc-1];
if (channel == NULL)
return;
if (status & G_AT_MUX_DLC_STATUS_RTR) {
GSList *l;
mux->dlcs[dlc-1]->throttled = FALSE;
debug(mux, "setting throttled to FALSE");
for (l = mux->dlcs[dlc-1]->sources; l; l = l->next) {
GAtMuxWatch *source = l->data;
if (source->condition & G_IO_OUT) {
wakeup_writer(mux);
break;
}
}
} else
mux->dlcs[dlc-1]->throttled = 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;
}
unsigned int mgmt_reply(struct mgmt *mgmt, uint16_t opcode, uint16_t index,
uint16_t length, const void *param,
mgmt_request_func_t callback,
void *user_data, mgmt_destroy_func_t destroy)
{
struct mgmt_request *request;
if (!mgmt)
return 0;
request = create_request(opcode, index, length, param,
callback, user_data, destroy);
if (!request)
return 0;
if (mgmt->next_request_id < 1)
mgmt->next_request_id = 1;
request->id = mgmt->next_request_id++;
if (!queue_push_tail(mgmt->reply_queue, request)) {
free(request->buf);
free(request);
return 0;
}
wakeup_writer(mgmt);
return request->id;
}
static void request_complete(struct mgmt *mgmt, uint8_t status,
uint16_t opcode, uint16_t index,
uint16_t length, const void *param)
{
struct mgmt_request *request;
GList *list;
list = lookup_pending(mgmt, opcode, index);
if (!list)
return;
request = list->data;
mgmt->pending_list = g_list_delete_link(mgmt->pending_list, list);
if (request->callback)
request->callback(status, length, param, request->user_data);
destroy_request(request, NULL);
if (mgmt->destroyed)
return;
wakeup_writer(mgmt);
}
unsigned int bt_att_send(struct bt_att *att, uint8_t opcode,
const void *pdu, uint16_t length,
bt_att_response_func_t callback, void *user_data,
bt_att_destroy_func_t destroy)
{
struct att_send_op *op;
bool result;
if (!att || !att->io)
return 0;
op = create_att_send_op(att, opcode, pdu, length, callback, user_data,
destroy);
if (!op)
return 0;
if (att->next_send_id < 1)
att->next_send_id = 1;
op->id = att->next_send_id++;
/* Add the op to the correct queue based on its type */
switch (op->type) {
case ATT_OP_TYPE_REQ:
result = queue_push_tail(att->req_queue, op);
break;
case ATT_OP_TYPE_IND:
result = queue_push_tail(att->ind_queue, op);
break;
case ATT_OP_TYPE_CMD:
case ATT_OP_TYPE_NOT:
case ATT_OP_TYPE_UNKNOWN:
case ATT_OP_TYPE_RSP:
case ATT_OP_TYPE_CONF:
default:
result = queue_push_tail(att->write_queue, op);
break;
}
if (!result) {
free(op->pdu);
free(op);
return 0;
}
wakeup_writer(att);
return op->id;
}
unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode,
const void *data, uint8_t size,
bt_hci_callback_func_t callback,
void *user_data, bt_hci_destroy_func_t destroy)
{
struct cmd *cmd;
if (!hci)
return 0;
cmd = new0(struct cmd, 1);
if (!cmd)
return 0;
cmd->opcode = opcode;
cmd->size = size;
if (cmd->size > 0) {
cmd->data = malloc(cmd->size);
if (!cmd->data) {
free(cmd);
return 0;
}
memcpy(cmd->data, data, cmd->size);
}
if (hci->next_cmd_id < 1)
hci->next_cmd_id = 1;
cmd->id = hci->next_cmd_id++;
cmd->callback = callback;
cmd->destroy = destroy;
cmd->user_data = user_data;
if (!queue_push_tail(hci->cmd_queue, cmd)) {
free(cmd->data);
free(cmd);
return 0;
}
wakeup_writer(hci);
return cmd->id;
}
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;
case HFP_RESULT_RING:
case HFP_RESULT_NO_CARRIER:
case HFP_RESULT_BUSY:
case HFP_RESULT_NO_ANSWER:
case HFP_RESULT_DELAYED:
case HFP_RESULT_BLACKLISTED:
case HFP_RESULT_CME_ERROR:
case HFP_RESULT_NO_DIALTONE:
case HFP_RESULT_CONNECT:
default:
return false;
}
if (ringbuf_printf(hfp->write_buf, "\r\n%s\r\n", str) < 0)
return false;
wakeup_writer(hfp);
/*
* There might be already something to read in the ring buffer.
* If so, let's read it.
*/
if (hfp->result_pending) {
hfp->result_pending = false;
process_input(hfp);
}
return true;
}
bool bt_att_cancel(struct bt_att *att, unsigned int id)
{
struct att_send_op *op;
if (!att || !id)
return false;
if (att->pending_req && att->pending_req->id == id) {
/* Don't cancel the pending request; remove it's handlers */
cancel_att_send_op(att->pending_req);
return true;
}
if (att->pending_ind && att->pending_ind->id == id) {
/* Don't cancel the pending indication; remove it's handlers */
cancel_att_send_op(att->pending_ind);
return true;
}
op = queue_remove_if(att->req_queue, match_op_id, UINT_TO_PTR(id));
if (op)
goto done;
op = queue_remove_if(att->ind_queue, match_op_id, UINT_TO_PTR(id));
if (op)
goto done;
op = queue_remove_if(att->write_queue, match_op_id, UINT_TO_PTR(id));
if (op)
goto done;
if (!op)
return false;
done:
destroy_att_send_op(op);
wakeup_writer(att);
return true;
}
static void process_response(struct bt_hci *hci, uint16_t opcode,
const void *data, size_t size)
{
struct cmd *cmd;
if (opcode == BT_HCI_CMD_NOP)
goto done;
cmd = queue_remove_if(hci->rsp_queue, match_cmd_opcode,
UINT_TO_PTR(opcode));
if (!cmd)
return;
if (cmd->callback)
cmd->callback(data, size, cmd->user_data);
cmd_free(cmd);
done:
wakeup_writer(hci);
}
bool bt_hci_cancel(struct bt_hci *hci, unsigned int id)
{
struct cmd *cmd;
if (!hci || !id)
return false;
cmd = queue_remove_if(hci->cmd_queue, match_cmd_id, UINT_TO_PTR(id));
if (!cmd) {
cmd = queue_remove_if(hci->rsp_queue, match_cmd_id,
UINT_TO_PTR(id));
if (!cmd)
return false;
}
cmd_free(cmd);
wakeup_writer(hci);
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);
/*
* There might be already something to read in the ring buffer.
* If so, let's read it.
*/
if (hfp->result_pending) {
hfp->result_pending = false;
process_input(hfp);
}
return true;
}
bool mgmt_cancel(struct mgmt *mgmt, unsigned int id)
{
struct mgmt_request *request;
GList *list;
if (!mgmt || !id)
return false;
list = g_queue_find_custom(mgmt->request_queue, GUINT_TO_POINTER(id),
compare_request_id);
if (list) {
request = list->data;
g_queue_delete_link(mgmt->request_queue, list);
goto done;
}
list = g_queue_find_custom(mgmt->reply_queue, GUINT_TO_POINTER(id),
compare_request_id);
if (list) {
request = list->data;
g_queue_delete_link(mgmt->reply_queue, list);
goto done;
}
list = g_list_find_custom(mgmt->pending_list, GUINT_TO_POINTER(id),
compare_request_id);
if (!list)
return false;
request = list->data;
mgmt->pending_list = g_list_delete_link(mgmt->pending_list, list);
done:
destroy_request(request, NULL);
wakeup_writer(mgmt);
return true;
}
bool bt_att_cancel(struct bt_att *att, unsigned int id)
{
struct att_send_op *op;
if (!att || !id)
return false;
if (att->pending_req && att->pending_req->id == id) {
op = att->pending_req;
goto done;
}
if (att->pending_ind && att->pending_ind->id == id) {
op = att->pending_ind;
goto done;
}
op = queue_remove_if(att->req_queue, match_op_id, UINT_TO_PTR(id));
if (op)
goto done;
op = queue_remove_if(att->ind_queue, match_op_id, UINT_TO_PTR(id));
if (op)
goto done;
op = queue_remove_if(att->write_queue, match_op_id, UINT_TO_PTR(id));
if (op)
goto done;
if (!op)
return false;
done:
destroy_att_send_op(op);
wakeup_writer(att);
return true;
}
static void request_complete(struct mgmt *mgmt, uint8_t status,
uint16_t opcode, uint16_t index,
uint16_t length, const void *param)
{
struct opcode_index match = { .opcode = opcode, .index = index };
struct mgmt_request *request;
request = queue_remove_if(mgmt->pending_list,
match_request_opcode_index, &match);
if (request) {
if (request->callback)
request->callback(status, length, param,
request->user_data);
destroy_request(request);
}
if (mgmt->destroyed)
return;
wakeup_writer(mgmt);
}
static void handle_conf(struct bt_att *att, uint8_t *pdu, ssize_t pdu_len)
{
struct att_send_op *op = att->pending_ind;
/*
* Disconnect the bearer if the confirmation is unexpected or the PDU is
* invalid.
*/
if (!op || pdu_len) {
util_debug(att->debug_callback, att->debug_data,
"Received unexpected/invalid ATT confirmation");
io_shutdown(att->io);
return;
}
if (op->callback)
op->callback(BT_ATT_OP_HANDLE_VAL_CONF, NULL, 0, op->user_data);
destroy_att_send_op(op);
att->pending_ind = NULL;
wakeup_writer(att);
}