static bool disconnect_cb(struct io *io, void *user_data)
{
struct bt_att *att = user_data;
int err;
socklen_t len;
len = sizeof(err);
if (getsockopt(att->fd, SOL_SOCKET, SO_ERROR, &err, &len) < 0) {
util_debug(att->debug_callback, att->debug_data,
"Failed to obtain disconnect error: %s",
strerror(errno));
err = 0;
}
util_debug(att->debug_callback, att->debug_data,
"Physical link disconnected: %s",
strerror(err));
io_destroy(att->io);
att->io = NULL;
bt_att_cancel_all(att);
bt_att_ref(att);
queue_foreach(att->disconn_list, disconn_handler, INT_TO_PTR(err));
bt_att_unregister_all(att);
bt_att_unref(att);
return false;
}
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);
}
static bool can_write_data(struct io *io, void *user_data)
{
struct bt_att *att = user_data;
struct att_send_op *op;
struct timeout_data *timeout;
ssize_t bytes_written;
op = pick_next_send_op(att);
if (!op)
return false;
bytes_written = write(att->fd, op->pdu, op->len);
if (bytes_written < 0) {
util_debug(att->debug_callback, att->debug_data,
"write failed: %s", strerror(errno));
if (op->callback)
op->callback(BT_ATT_OP_ERROR_RSP, NULL, 0,
op->user_data);
destroy_att_send_op(op);
return true;
}
util_debug(att->debug_callback, att->debug_data,
"ATT op 0x%02x", op->opcode);
util_hexdump('<', op->pdu, bytes_written,
att->debug_callback, att->debug_data);
/* Based on the operation type, set either the pending request or the
* pending indication. If it came from the write queue, then there is
* no need to keep it around.
*/
switch (op->type) {
case ATT_OP_TYPE_REQ:
att->pending_req = op;
break;
case ATT_OP_TYPE_IND:
att->pending_ind = op;
break;
default:
destroy_att_send_op(op);
return true;
}
timeout = new0(struct timeout_data, 1);
if (!timeout)
return true;
timeout->att = att;
timeout->id = op->id;
op->timeout_id = timeout_add(ATT_TIMEOUT_INTERVAL, timeout_cb,
timeout, free);
/* Return true as there may be more operations ready to write. */
return true;
}
void util_meminfo() {
struct memblock_t *info;
if (!opts.memchk)
return;
for (info = mem_start; info; info = info->next) {
util_debug("MEM", "lost: % 8u (bytes) at %s:%u\n",
info->byte,
info->file,
info->line);
}
util_debug("MEM", "Memory information:\n\
Total allocations: %llu\n\
Total deallocations: %llu\n\
Total allocated: %llu (bytes)\n\
Total deallocated: %llu (bytes)\n\
Leaks found: lost %llu (bytes) in %d allocations\n",
mem_at, mem_dt,
mem_ab, mem_db,
(mem_ab - mem_db),
(mem_at - mem_dt)
);
}
static bool timeout_cb(void *user_data)
{
struct timeout_data *timeout = user_data;
struct bt_att *att = timeout->att;
struct att_send_op *op = NULL;
if (att->pending_req && att->pending_req->id == timeout->id) {
op = att->pending_req;
att->pending_req = NULL;
} else if (att->pending_ind && att->pending_ind->id == timeout->id) {
op = att->pending_ind;
att->pending_ind = NULL;
}
if (!op)
return false;
util_debug(att->debug_callback, att->debug_data,
"Operation timed out: 0x%02x", op->opcode);
if (att->timeout_callback)
att->timeout_callback(op->id, op->opcode, att->timeout_data);
op->timeout_id = 0;
destroy_att_send_op(op);
/*
* Directly terminate the connection as required by the ATT protocol.
* This should trigger an io disconnect event which will clean up the
* io and notify the upper layer.
*/
io_shutdown(att->io);
return false;
}
static bool timeout_cb(void *user_data)
{
struct timeout_data *timeout = user_data;
struct bt_att *att = timeout->att;
struct att_send_op *op = NULL;
if (att->pending_req && att->pending_req->id == timeout->id) {
op = att->pending_req;
att->pending_req = NULL;
} else if (att->pending_ind && att->pending_ind->id == timeout->id) {
op = att->pending_ind;
att->pending_ind = NULL;
}
if (!op)
return false;
att->invalid = true;
util_debug(att->debug_callback, att->debug_data,
"Operation timed out: 0x%02x", op->opcode);
if (att->timeout_callback)
att->timeout_callback(op->id, op->opcode, att->timeout_data);
op->timeout_id = 0;
destroy_att_send_op(op);
return false;
}
static bool handle_error_rsp(struct bt_att *att, uint8_t *pdu,
ssize_t pdu_len, uint8_t *opcode)
{
const struct bt_att_pdu_error_rsp *rsp;
struct att_send_op *op = att->pending_req;
if (pdu_len != sizeof(*rsp)) {
*opcode = 0;
return false;
}
rsp = (void *) pdu;
*opcode = rsp->opcode;
/* Attempt to change security */
if (!change_security(att, rsp->ecode))
return false;
util_debug(att->debug_callback, att->debug_data,
"Retrying operation %p", op);
att->pending_req = NULL;
/* Push operation back to request queue */
return queue_push_head(att->req_queue, op);
}
static void exchange_mtu_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t client_rx_mtu;
uint16_t final_mtu;
uint8_t rsp_pdu[2];
if (length != 2) {
bt_att_send_error_rsp(server->att, opcode, 0,
BT_ATT_ERROR_INVALID_PDU);
return;
}
client_rx_mtu = get_le16(pdu);
final_mtu = MAX(MIN(client_rx_mtu, server->mtu), BT_ATT_DEFAULT_LE_MTU);
/* Respond with the server MTU */
put_le16(server->mtu, rsp_pdu);
bt_att_send(server->att, BT_ATT_OP_MTU_RSP, rsp_pdu, 2, NULL, NULL,
NULL);
/* Set MTU to be the minimum */
server->mtu = final_mtu;
bt_att_set_mtu(server->att, final_mtu);
util_debug(server->debug_callback, server->debug_data,
"MTU exchange complete, with MTU: %u", final_mtu);
}
static void write_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
struct gatt_db_attribute *attr;
uint16_t handle = 0;
struct async_write_op *op = NULL;
uint8_t ecode;
if (length < 2) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
handle = get_le16(pdu);
attr = gatt_db_get_attribute(server->db, handle);
if (!attr) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
util_debug(server->debug_callback, server->debug_data,
"Write %s - handle: 0x%04x",
(opcode == BT_ATT_OP_WRITE_REQ) ? "Req" : "Cmd",
handle);
ecode = check_permissions(server, attr, BT_ATT_PERM_WRITE |
BT_ATT_PERM_WRITE_AUTHEN |
BT_ATT_PERM_WRITE_ENCRYPT);
if (ecode)
goto error;
if (server->pending_write_op) {
ecode = BT_ATT_ERROR_UNLIKELY;
goto error;
}
op = new0(struct async_write_op, 1);
op->server = server;
op->opcode = opcode;
server->pending_write_op = op;
if (gatt_db_attribute_write(attr, 0, pdu + 2, length - 2, opcode,
server->att,
write_complete_cb, op))
return;
if (op)
async_write_op_destroy(op);
ecode = BT_ATT_ERROR_UNLIKELY;
error:
if (opcode == BT_ATT_OP_WRITE_CMD)
return;
bt_att_send_error_rsp(server->att, opcode, handle, ecode);
}
static void exec_write_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint8_t flags;
uint8_t ecode;
bool write;
uint16_t ehandle = 0;
if (length != 1) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
flags = ((uint8_t *) pdu)[0];
util_debug(server->debug_callback, server->debug_data,
"Exec Write Req - flags: 0x%02x", flags);
if (flags == 0x00)
write = false;
else if (flags == 0x01)
write = true;
else {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
if (!write) {
queue_remove_all(server->prep_queue, NULL, NULL,
prep_write_data_destroy);
bt_att_send(server->att, BT_ATT_OP_EXEC_WRITE_RSP, NULL, 0,
NULL, NULL, NULL);
return;
}
/* If there is more than one prep request, we are in reliable session */
if (queue_length(server->prep_queue) > 1) {
struct prep_write_data *prep_data;
prep_data = queue_find(server->prep_queue,
find_no_reliable_characteristic, NULL);
if (prep_data) {
ecode = BT_ATT_ERROR_REQUEST_NOT_SUPPORTED;
ehandle = prep_data->handle;
goto error;
}
}
exec_next_prep_write(server, 0, 0);
return;
error:
queue_remove_all(server->prep_queue, NULL, NULL,
prep_write_data_destroy);
bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
}
static void prep_write_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t handle = 0;
uint16_t offset;
struct gatt_db_attribute *attr;
struct prep_write_complete_data *pwcd;
uint8_t ecode, status;
if (length < 4) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
if (queue_length(server->prep_queue) >= server->max_prep_queue_len) {
ecode = BT_ATT_ERROR_PREPARE_QUEUE_FULL;
goto error;
}
handle = get_le16(pdu);
offset = get_le16(pdu + 2);
attr = gatt_db_get_attribute(server->db, handle);
if (!attr) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
util_debug(server->debug_callback, server->debug_data,
"Prep Write Req - handle: 0x%04x", handle);
ecode = check_permissions(server, attr, BT_ATT_PERM_WRITE |
BT_ATT_PERM_WRITE_AUTHEN |
BT_ATT_PERM_WRITE_ENCRYPT);
if (ecode)
goto error;
pwcd = new0(struct prep_write_complete_data, 1);
pwcd->pdu = malloc(length);
memcpy(pwcd->pdu, pdu, length);
pwcd->length = length;
pwcd->server = server;
status = gatt_db_attribute_write(attr, offset, NULL, 0,
BT_ATT_OP_PREP_WRITE_REQ,
server->att,
prep_write_complete_cb, pwcd);
if (status)
return;
ecode = BT_ATT_ERROR_UNLIKELY;
error:
bt_att_send_error_rsp(server->att, opcode, handle, ecode);
}
static gboolean can_write_data(GIOChannel *channel, GIOCondition cond,
gpointer user_data)
{
struct mgmt *mgmt = user_data;
struct mgmt_request *request;
ssize_t bytes_written;
if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL))
return FALSE;
request = g_queue_pop_head(mgmt->reply_queue);
if (!request) {
/* only reply commands can jump the queue */
if (mgmt->pending_list)
return FALSE;
request = g_queue_pop_head(mgmt->request_queue);
if (!request)
return FALSE;
}
bytes_written = write(mgmt->fd, request->buf, request->len);
if (bytes_written < 0) {
util_debug(mgmt->debug_callback, mgmt->debug_data,
"write failed: %s", strerror(errno));
if (request->callback)
request->callback(MGMT_STATUS_FAILED, 0, NULL,
request->user_data);
destroy_request(request, NULL);
return TRUE;
}
util_debug(mgmt->debug_callback, mgmt->debug_data,
"[0x%04x] command 0x%04x",
request->index, request->opcode);
util_hexdump('<', request->buf, bytes_written,
mgmt->debug_callback, mgmt->debug_data);
mgmt->pending_list = g_list_append(mgmt->pending_list, request);
return FALSE;
}
static bool can_read_data(struct io *io, void *user_data)
{
struct bt_att *att = user_data;
uint8_t opcode;
uint8_t *pdu;
ssize_t bytes_read;
bytes_read = read(att->fd, att->buf, att->mtu);
if (bytes_read < 0)
return false;
util_hexdump('>', att->buf, bytes_read,
att->debug_callback, att->debug_data);
if (bytes_read < ATT_MIN_PDU_LEN)
return true;
pdu = att->buf;
opcode = pdu[0];
/* Act on the received PDU based on the opcode type */
switch (get_op_type(opcode)) {
case ATT_OP_TYPE_RSP:
util_debug(att->debug_callback, att->debug_data,
"ATT response received: 0x%02x", opcode);
handle_rsp(att, opcode, pdu + 1, bytes_read - 1);
break;
case ATT_OP_TYPE_CONF:
util_debug(att->debug_callback, att->debug_data,
"ATT opcode cannot be handled: 0x%02x", opcode);
break;
default:
/* For all other opcodes notify the upper layer of the PDU and
* let them act on it.
*/
util_debug(att->debug_callback, att->debug_data,
"ATT PDU received: 0x%02x", opcode);
handle_notify(att, opcode, pdu + 1, bytes_read - 1);
break;
}
return true;
}
static void read_multiple_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
struct gatt_db_attribute *attr;
struct read_multiple_resp_data data;
uint8_t ecode = BT_ATT_ERROR_UNLIKELY;
size_t i = 0;
data.handles = NULL;
data.rsp_data = NULL;
if (length < 4) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
data.server = server;
data.num_handles = length / 2;
data.cur_handle = 0;
data.mtu = bt_att_get_mtu(server->att);
data.length = 0;
data.rsp_data = malloc(data.mtu - 1);
if (!data.rsp_data)
goto error;
data.handles = new0(uint16_t, data.num_handles);
if (!data.handles)
goto error;
for (i = 0; i < data.num_handles; i++)
data.handles[i] = get_le16(pdu + i * 2);
util_debug(server->debug_callback, server->debug_data,
"Read Multiple Req - %zu handles, 1st: 0x%04x",
data.num_handles, data.handles[0]);
attr = gatt_db_get_attribute(server->db, data.handles[0]);
if (!attr) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
if (gatt_db_attribute_read(attr, 0, opcode, server->att,
read_multiple_complete_cb, &data))
return;
error:
read_multiple_resp_data_free(&data);
bt_att_send_error_rsp(server->att, opcode, 0, ecode);
}
static void find_by_type_val_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t start, end, uuid16;
struct find_by_type_val_data data;
uint16_t mtu = bt_att_get_mtu(server->att);
uint8_t rsp_pdu[mtu];
uint16_t ehandle = 0;
bt_uuid_t uuid;
if (length < 6) {
data.ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
data.pdu = rsp_pdu;
data.len = 0;
data.mtu = mtu;
data.ecode = 0;
start = get_le16(pdu);
end = get_le16(pdu + 2);
uuid16 = get_le16(pdu + 4);
util_debug(server->debug_callback, server->debug_data,
"Find By Type Value - start: 0x%04x end: 0x%04x uuid: 0x%04x",
start, end, uuid16);
ehandle = start;
if (start > end) {
data.ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
bt_uuid16_create(&uuid, uuid16);
gatt_db_find_by_type_value(server->db, start, end, &uuid, pdu + 6,
length - 6,
find_by_type_val_att_cb,
&data);
if (!data.len)
data.ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
if (data.ecode)
goto error;
bt_att_send(server->att, BT_ATT_OP_FIND_BY_TYPE_VAL_RSP, data.pdu,
data.len, NULL, NULL, NULL);
return;
error:
bt_att_send_error_rsp(server->att, opcode, ehandle, data.ecode);
}
static void handle_read_req(struct bt_gatt_server *server, uint8_t opcode,
uint16_t handle,
uint16_t offset)
{
struct gatt_db_attribute *attr;
uint8_t ecode;
uint32_t perm;
struct async_read_op *op = NULL;
attr = gatt_db_get_attribute(server->db, handle);
if (!attr) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
util_debug(server->debug_callback, server->debug_data,
"Read %sReq - handle: 0x%04x",
opcode == BT_ATT_OP_READ_BLOB_REQ ? "Blob " : "",
handle);
perm = gatt_db_attribute_get_permissions(attr);
if (perm && !(perm & BT_ATT_PERM_READ)) {
ecode = BT_ATT_ERROR_READ_NOT_PERMITTED;
goto error;
}
if (server->pending_read_op) {
ecode = BT_ATT_ERROR_UNLIKELY;
goto error;
}
op = new0(struct async_read_op, 1);
if (!op) {
ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
goto error;
}
op->opcode = opcode;
op->server = server;
server->pending_read_op = op;
if (gatt_db_attribute_read(attr, offset, opcode, server->att,
read_complete_cb, op))
return;
ecode = BT_ATT_ERROR_UNLIKELY;
error:
if (op)
async_read_op_destroy(op);
bt_att_send_error_rsp(server->att, opcode, handle, ecode);
}
static bool can_write_data(struct io *io, void *user_data)
{
struct mgmt *mgmt = user_data;
struct mgmt_request *request;
ssize_t bytes_written;
request = queue_pop_head(mgmt->reply_queue);
if (!request) {
/* only reply commands can jump the queue */
if (!queue_isempty(mgmt->pending_list))
return false;
request = queue_pop_head(mgmt->request_queue);
if (!request)
return false;
}
bytes_written = write(mgmt->fd, request->buf, request->len);
if (bytes_written < 0) {
util_debug(mgmt->debug_callback, mgmt->debug_data,
"write failed: %s", strerror(errno));
if (request->callback)
request->callback(MGMT_STATUS_FAILED, 0, NULL,
request->user_data);
destroy_request(request);
return true;
}
util_debug(mgmt->debug_callback, mgmt->debug_data,
"[0x%04x] command 0x%04x",
request->index, request->opcode);
util_hexdump('<', request->buf, bytes_written,
mgmt->debug_callback, mgmt->debug_data);
queue_push_tail(mgmt->pending_list, request);
return false;
}
static void handle_read_req(struct bt_gatt_server *server, uint8_t opcode,
uint16_t handle,
uint16_t offset)
{
struct gatt_db_attribute *attr;
uint8_t ecode;
struct async_read_op *op = NULL;
ecode = authorize_req(server, opcode, handle);
if (ecode)
goto error;
attr = gatt_db_get_attribute(server->db, handle);
if (!attr) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
util_debug(server->debug_callback, server->debug_data,
"Read %sReq - handle: 0x%04x",
opcode == BT_ATT_OP_READ_BLOB_REQ ? "Blob " : "",
handle);
ecode = check_permissions(server, attr, BT_ATT_PERM_READ |
BT_ATT_PERM_READ_AUTHEN |
BT_ATT_PERM_READ_ENCRYPT);
if (ecode)
goto error;
if (server->pending_read_op) {
ecode = BT_ATT_ERROR_UNLIKELY;
goto error;
}
op = new0(struct async_read_op, 1);
op->opcode = opcode;
op->server = server;
server->pending_read_op = op;
if (gatt_db_attribute_read(attr, offset, opcode, server->att,
read_complete_cb, op))
return;
ecode = BT_ATT_ERROR_UNLIKELY;
error:
if (op)
async_read_op_destroy(op);
bt_att_send_error_rsp(server->att, opcode, handle, ecode);
}
void *util_memory_r(void *ptrn, size_t byte, unsigned int line, const char *file) {
struct memblock_t *oldinfo = NULL;
struct memblock_t *newinfo;
if (!ptrn)
return util_memory_a(byte, line, file);
if (!byte) {
util_memory_d(ptrn, line, file);
return NULL;
}
oldinfo = ((struct memblock_t*)ptrn - 1);
newinfo = ((struct memblock_t*)malloc(sizeof(struct memblock_t) + byte));
util_debug("MEM", "reallocation: % 8u -> %u (bytes) address 0x%08X -> 0x%08X @ %s:%u\n", oldinfo->byte, byte, ptrn, (void*)(newinfo+1), file, line);
/* new data */
if (!newinfo) {
util_memory_d(oldinfo+1, line, file);
return NULL;
}
/* copy old */
memcpy(newinfo+1, oldinfo+1, oldinfo->byte);
/* free old */
if (oldinfo->prev)
oldinfo->prev->next = oldinfo->next;
if (oldinfo->next)
oldinfo->next->prev = oldinfo->prev;
if (oldinfo == mem_start)
mem_start = oldinfo->next;
/* fill info */
newinfo->line = line;
newinfo->byte = byte;
newinfo->file = file;
newinfo->prev = NULL;
newinfo->next = mem_start;
if (mem_start)
mem_start->prev = newinfo;
mem_start = newinfo;
mem_ab -= oldinfo->byte;
mem_ab += newinfo->byte;
free(oldinfo);
return newinfo+1;
}
static void prep_write_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t handle = 0;
uint16_t offset;
struct gatt_db_attribute *attr;
uint8_t ecode;
if (length < 4) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
if (queue_length(server->prep_queue) >= server->max_prep_queue_len) {
ecode = BT_ATT_ERROR_PREPARE_QUEUE_FULL;
goto error;
}
handle = get_le16(pdu);
offset = get_le16(pdu + 2);
attr = gatt_db_get_attribute(server->db, handle);
if (!attr) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
util_debug(server->debug_callback, server->debug_data,
"Prep Write Req - handle: 0x%04x", handle);
ecode = check_permissions(server, attr, BT_ATT_PERM_WRITE |
BT_ATT_PERM_WRITE_AUTHEN |
BT_ATT_PERM_WRITE_ENCRYPT);
if (ecode)
goto error;
if (!store_prep_data(server, handle, offset, length - 4,
&((uint8_t *) pdu)[4])) {
ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
goto error;
}
bt_att_send(server->att, BT_ATT_OP_PREP_WRITE_RSP, pdu, length, NULL,
NULL, NULL);
return;
error:
bt_att_send_error_rsp(server->att, opcode, handle, ecode);
}
void util_memory_d(void *ptrn, unsigned int line, const char *file) {
struct memblock_t *info = NULL;
if (!ptrn) return;
info = ((struct memblock_t*)ptrn - 1);
util_debug("MEM", "released: % 8u (bytes) address 0x%08X @ %s:%u\n", info->byte, ptrn, file, line);
mem_db += info->byte;
mem_dt++;
if (info->prev)
info->prev->next = info->next;
if (info->next)
info->next->prev = info->prev;
if (info == mem_start)
mem_start = info->next;
free(info);
}
void *util_memory_a(size_t byte, unsigned int line, const char *file) {
struct memblock_t *info = malloc(sizeof(struct memblock_t) + byte);
void *data = (void*)(info+1);
if (!info) return NULL;
info->line = line;
info->byte = byte;
info->file = file;
info->prev = NULL;
info->next = mem_start;
if (mem_start)
mem_start->prev = info;
mem_start = info;
util_debug("MEM", "allocation: % 8u (bytes) address 0x%08X @ %s:%u\n", byte, data, file, line);
mem_at++;
mem_ab += info->byte;
return data;
}
static bool encode_pdu(struct bt_att *att, struct att_send_op *op,
const void *pdu, uint16_t length)
{
uint16_t pdu_len = 1;
struct sign_info *sign = att->local_sign;
uint32_t sign_cnt;
if (sign && (op->opcode & ATT_OP_SIGNED_MASK))
pdu_len += BT_ATT_SIGNATURE_LEN;
if (length && pdu)
pdu_len += length;
if (pdu_len > att->mtu)
return false;
op->len = pdu_len;
op->pdu = malloc(op->len);
if (!op->pdu)
return false;
((uint8_t *) op->pdu)[0] = op->opcode;
if (pdu_len > 1)
memcpy(op->pdu + 1, pdu, length);
if (!sign || !(op->opcode & ATT_OP_SIGNED_MASK) || !att->crypto)
return true;
if (!sign->counter(&sign_cnt, sign->user_data))
goto fail;
if ((bt_crypto_sign_att(att->crypto, sign->key, op->pdu, 1 + length,
sign_cnt, &((uint8_t *) op->pdu)[1 + length])))
return true;
util_debug(att->debug_callback, att->debug_data,
"ATT unable to generate signature");
fail:
free(op->pdu);
return false;
}
static void exec_write_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint8_t flags;
uint8_t ecode;
bool write;
if (length != 1) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
flags = ((uint8_t *) pdu)[0];
util_debug(server->debug_callback, server->debug_data,
"Exec Write Req - flags: 0x%02x", flags);
if (flags == 0x00)
write = false;
else if (flags == 0x01)
write = true;
else {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
if (!write) {
queue_remove_all(server->prep_queue, NULL, NULL,
prep_write_data_destroy);
bt_att_send(server->att, BT_ATT_OP_EXEC_WRITE_RSP, NULL, 0,
NULL, NULL, NULL);
return;
}
exec_next_prep_write(server, 0, 0);
return;
error:
bt_att_send_error_rsp(server->att, opcode, 0, ecode);
}
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);
}
static bool handle_signed(struct bt_att *att, uint8_t opcode, uint8_t *pdu,
ssize_t pdu_len)
{
uint8_t *signature;
uint32_t sign_cnt;
struct sign_info *sign;
/* Check if there is enough data for a signature */
if (pdu_len < 2 + BT_ATT_SIGNATURE_LEN)
goto fail;
sign = att->remote_sign;
if (!sign)
goto fail;
signature = pdu + (pdu_len - BT_ATT_SIGNATURE_LEN);
sign_cnt = get_le32(signature);
/* Validate counter */
if (!sign->counter(&sign_cnt, sign->user_data))
goto fail;
/* Generate signature and verify it */
if (!bt_crypto_sign_att(att->crypto, sign->key, pdu,
pdu_len - BT_ATT_SIGNATURE_LEN, sign_cnt,
signature))
goto fail;
return true;
fail:
util_debug(att->debug_callback, att->debug_data,
"ATT failed to verify signature: 0x%02x", opcode);
return false;
}
static void read_multiple_complete_cb(struct gatt_db_attribute *attr, int err,
const uint8_t *value, size_t len,
void *user_data)
{
struct read_multiple_resp_data *data = user_data;
struct gatt_db_attribute *next_attr;
uint32_t perm;
uint16_t handle = gatt_db_attribute_get_handle(attr);
if (err != 0) {
bt_att_send_error_rsp(data->server->att,
BT_ATT_OP_READ_MULT_REQ, handle, err);
read_multiple_resp_data_free(data);
return;
}
perm = gatt_db_attribute_get_permissions(attr);
if (perm && !(perm & BT_ATT_PERM_READ)) {
bt_att_send_error_rsp(data->server->att,
BT_ATT_OP_READ_MULT_REQ, handle,
BT_ATT_ERROR_READ_NOT_PERMITTED);
read_multiple_resp_data_free(data);
return;
}
len = MIN(len, data->mtu - data->length - 1);
memcpy(data->rsp_data + data->length, value, len);
data->length += len;
data->cur_handle++;
if ((data->length >= data->mtu - 1) ||
(data->cur_handle == data->num_handles)) {
bt_att_send(data->server->att, BT_ATT_OP_READ_MULT_RSP,
data->rsp_data, data->length, NULL, NULL, NULL);
read_multiple_resp_data_free(data);
return;
}
util_debug(data->server->debug_callback, data->server->debug_data,
"Read Multiple Req - #%zu of %zu: 0x%04x",
data->cur_handle + 1, data->num_handles,
data->handles[data->cur_handle]);
next_attr = gatt_db_get_attribute(data->server->db,
data->handles[data->cur_handle]);
if (!next_attr) {
bt_att_send_error_rsp(data->server->att,
BT_ATT_OP_READ_MULT_REQ,
data->handles[data->cur_handle],
BT_ATT_ERROR_INVALID_HANDLE);
read_multiple_resp_data_free(data);
return;
}
if (!gatt_db_attribute_read(next_attr, 0, BT_ATT_OP_READ_MULT_REQ,
data->server->att,
read_multiple_complete_cb, data)) {
bt_att_send_error_rsp(data->server->att,
BT_ATT_OP_READ_MULT_REQ,
data->handles[data->cur_handle],
BT_ATT_ERROR_UNLIKELY);
read_multiple_resp_data_free(data);
}
}
static void find_info_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t start, end;
uint16_t mtu = bt_att_get_mtu(server->att);
uint8_t rsp_pdu[mtu];
uint16_t rsp_len;
uint8_t ecode = 0;
uint16_t ehandle = 0;
struct queue *q = NULL;
if (length != 4) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
q = queue_new();
if (!q) {
ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
goto error;
}
start = get_le16(pdu);
end = get_le16(pdu + 2);
util_debug(server->debug_callback, server->debug_data,
"Find Info - start: 0x%04x end: 0x%04x",
start, end);
if (!start || !end) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
ehandle = start;
if (start > end) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
gatt_db_find_information(server->db, start, end, q);
if (queue_isempty(q)) {
ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
goto error;
}
if (!encode_find_info_rsp(server->db, q, mtu, rsp_pdu, &rsp_len)) {
ecode = BT_ATT_ERROR_UNLIKELY;
goto error;
}
bt_att_send(server->att, BT_ATT_OP_FIND_INFO_RSP, rsp_pdu, rsp_len,
NULL, NULL, NULL);
queue_destroy(q, NULL);
return;
error:
bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
queue_destroy(q, NULL);
}
static void read_by_type_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t start, end;
bt_uuid_t type;
uint16_t ehandle = 0;
uint8_t ecode;
struct queue *q = NULL;
struct async_read_op *op;
if (length != 6 && length != 20) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
q = queue_new();
if (!q) {
ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
goto error;
}
start = get_le16(pdu);
end = get_le16(pdu + 2);
get_uuid_le(pdu + 4, length - 4, &type);
util_debug(server->debug_callback, server->debug_data,
"Read By Type - start: 0x%04x end: 0x%04x",
start, end);
if (!start || !end) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
ehandle = start;
if (start > end) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
gatt_db_read_by_type(server->db, start, end, type, q);
if (queue_isempty(q)) {
ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
goto error;
}
if (server->pending_read_op) {
ecode = BT_ATT_ERROR_UNLIKELY;
goto error;
}
op = new0(struct async_read_op, 1);
if (!op) {
ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
goto error;
}
op->pdu = malloc(bt_att_get_mtu(server->att));
if (!op->pdu) {
free(op);
ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
goto error;
}
op->opcode = opcode;
op->server = server;
op->db_data = q;
server->pending_read_op = op;
process_read_by_type(op);
return;
error:
bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
queue_destroy(q, NULL);
}
static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t start, end;
bt_uuid_t type;
bt_uuid_t prim, snd;
uint16_t mtu = bt_att_get_mtu(server->att);
uint8_t rsp_pdu[mtu];
uint16_t rsp_len;
uint8_t ecode = 0;
uint16_t ehandle = 0;
struct queue *q = NULL;
if (length != 6 && length != 20) {
ecode = BT_ATT_ERROR_INVALID_PDU;
goto error;
}
q = queue_new();
if (!q) {
ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
goto error;
}
start = get_le16(pdu);
end = get_le16(pdu + 2);
get_uuid_le(pdu + 4, length - 4, &type);
util_debug(server->debug_callback, server->debug_data,
"Read By Grp Type - start: 0x%04x end: 0x%04x",
start, end);
if (!start || !end) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
ehandle = start;
if (start > end) {
ecode = BT_ATT_ERROR_INVALID_HANDLE;
goto error;
}
/*
* GATT defines that only the <<Primary Service>> and
* <<Secondary Service>> group types can be used for the
* "Read By Group Type" request (Core v4.1, Vol 3, sec 2.5.3). Return an
* error if any other group type is given.
*/
bt_uuid16_create(&prim, GATT_PRIM_SVC_UUID);
bt_uuid16_create(&snd, GATT_SND_SVC_UUID);
if (bt_uuid_cmp(&type, &prim) && bt_uuid_cmp(&type, &snd)) {
ecode = BT_ATT_ERROR_UNSUPPORTED_GROUP_TYPE;
goto error;
}
gatt_db_read_by_group_type(server->db, start, end, type, q);
if (queue_isempty(q)) {
ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
goto error;
}
if (!encode_read_by_grp_type_rsp(server->db, q, server->att, mtu,
rsp_pdu, &rsp_len)) {
ecode = BT_ATT_ERROR_UNLIKELY;
goto error;
}
queue_destroy(q, NULL);
bt_att_send(server->att, BT_ATT_OP_READ_BY_GRP_TYPE_RSP,
rsp_pdu, rsp_len,
NULL, NULL, NULL);
return;
error:
queue_destroy(q, NULL);
bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
}
