guint gatt_discover_desc(GAttrib *attrib, uint16_t start, uint16_t end,
bt_uuid_t *uuid, gatt_cb_t func,
gpointer user_data)
{
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(attrib, &buflen);
struct discover_desc *dd;
guint16 plen;
plen = enc_find_info_req(start, end, buf, buflen);
if (plen == 0)
return 0;
dd = g_try_new0(struct discover_desc, 1);
if (dd == NULL)
return 0;
dd->attrib = g_attrib_ref(attrib);
dd->cb = func;
dd->user_data = user_data;
dd->start = start;
dd->end = end;
dd->uuid = g_memdup(uuid, sizeof(bt_uuid_t));
dd->id = g_attrib_send(attrib, 0, buf, plen, desc_discovered_cb,
discover_desc_ref(dd), discover_desc_unref);
return dd->id;
}
static void indicate_cb(
const uint8_t *pdu,
uint16_t len,
gpointer user_data) {
if (pdu[0] != ATT_OP_HANDLE_IND) {
std::cout << "Invalid opecode for indicate. pdu[0]:" << std::hex << static_cast<unsigned int>(pdu[0]) << std::endl;
return;
}
uint16_t handle = (pdu[2] << 8) | pdu[1];
std::cout << (boost::format("Indicate from %04x length:%d") % handle % len) << std::endl;
for (std::size_t index = 3, address = 0; index < len; ++index, ++address) {
if (address % 16 == 0) {
std::cout << std::endl;
std::cout << (boost::format("%08x:") % address);
}
if (address % 8 == 0) {
std::cout << ' ';
}
std::cout << (boost::format("%02x ") % static_cast<unsigned int>(pdu[index]));
}
std::cout << std::endl;
size_t plen;
user_data_t* ud = static_cast<user_data_t*>(user_data);
uint8_t* opdu = g_attrib_get_buffer(ud->attrib, &plen);
uint16_t olen = enc_confirmation(opdu, plen);
if (olen > 0)
g_attrib_send(ud->attrib, 0, opdu, olen, NULL, NULL, NULL);
}
static void events_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
{
GAttrib *attrib = user_data;
uint8_t *opdu;
uint16_t handle, i, olen = 0;
size_t plen;
handle = att_get_u16(&pdu[1]);
switch (pdu[0]) {
case ATT_OP_HANDLE_NOTIFY:
g_print("Notification handle = 0x%04x value: ", handle);
break;
case ATT_OP_HANDLE_IND:
g_print("Indication handle = 0x%04x value: ", handle);
break;
default:
g_print("Invalid opcode\n");
return;
}
for (i = 3; i < len; i++)
g_print("%02x ", pdu[i]);
g_print("\n");
if (pdu[0] == ATT_OP_HANDLE_NOTIFY)
return;
opdu = g_attrib_get_buffer(attrib, &plen);
olen = enc_confirmation(opdu, plen);
if (olen > 0)
g_attrib_send(attrib, 0, opdu, olen, NULL, NULL, NULL);
}
static void cmd_gatts(int argcp, char **argvp)
{
if (argcp == 2) {
GAttribResultFunc cb_func = NULL;
uint8_t *opdu = NULL;
size_t olen = gatt_attr_data_from_string(argvp[1], &opdu);
DBG("GATTS %p %d %zd", opdu, opt_mtu, olen);
/* Check if it is a value gatt server request */
if (opdu && (opt_mtu == 0 || olen <= opt_mtu) &&
!(opdu[0] == BT_ATT_OP_MTU_REQ ||
opdu[0] == BT_ATT_OP_FIND_INFO_REQ ||
opdu[0] == BT_ATT_OP_FIND_BY_TYPE_VAL_REQ ||
opdu[0] == BT_ATT_OP_READ_BY_TYPE_REQ ||
opdu[0] == BT_ATT_OP_READ_REQ ||
opdu[0] == BT_ATT_OP_READ_BLOB_REQ ||
opdu[0] == BT_ATT_OP_READ_MULT_REQ ||
opdu[0] == BT_ATT_OP_READ_BY_GRP_TYPE_REQ ||
opdu[0] == BT_ATT_OP_WRITE_REQ ||
opdu[0] == BT_ATT_OP_PREP_WRITE_REQ ||
opdu[0] == BT_ATT_OP_EXEC_WRITE_REQ)) {
if (opdu[0] == BT_ATT_OP_HANDLE_VAL_IND) {
cb_func = on_indicate_complete;
}
g_attrib_send(attrib, 0, opdu, olen, cb_func, NULL, NULL);
} else {
resp_error(err_BAD_PARAM);;
}
g_free(opdu);
} else {
resp_error(err_BAD_PARAM);;
}
}
static void events_handler(const uint8_t *pdu, uint16_t len, gpointer user_data) {
gatt_connection_t *conn = user_data;
uint8_t opdu[ATT_MAX_MTU];
uint16_t handle, i, olen = 0;
handle = att_get_u16(&pdu[1]);
// printf("0x%02x\n", handle);
switch (pdu[0]) {
case ATT_OP_HANDLE_NOTIFY:
if (conn->notification_handler) {
conn->notification_handler(handle, &pdu[3], len, conn->notification_user_data);
}
break;
case ATT_OP_HANDLE_IND:
if (conn->indication_handler) {
conn->indication_handler(handle, &pdu[3], len, conn->indication_user_data);
}
break;
default:
g_print("Invalid opcode\n");
return;
}
if (pdu[0] == ATT_OP_HANDLE_NOTIFY)
return;
olen = enc_confirmation(opdu, sizeof(opdu));
if (olen > 0)
g_attrib_send(conn->attrib, 0, opdu[0], opdu, olen, NULL, NULL, NULL);
}
guint gatt_discover_primary(GAttrib *attrib, bt_uuid_t *uuid, gatt_cb_t func,
gpointer user_data)
{
struct discover_primary *dp;
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(attrib, &buflen);
GAttribResultFunc cb;
guint16 plen;
plen = encode_discover_primary(0x0001, 0xffff, uuid, buf, buflen);
if (plen == 0)
return 0;
dp = g_try_new0(struct discover_primary, 1);
if (dp == NULL)
return 0;
dp->attrib = g_attrib_ref(attrib);
dp->cb = func;
dp->user_data = user_data;
if (uuid) {
dp->uuid = *uuid;
cb = primary_by_uuid_cb;
} else
cb = primary_all_cb;
return g_attrib_send(attrib, 0, buf, plen, cb, dp, NULL);
}
static void events_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
{
uint8_t *opdu;
uint8_t evt;
uint16_t handle, i, olen;
size_t plen;
evt = pdu[0];
if ( evt != ATT_OP_HANDLE_NOTIFY && evt != ATT_OP_HANDLE_IND )
{
printf("#Invalid opcode %02X in event handler??\n", evt);
return;
}
assert( len >= 3 );
handle = att_get_u16(&pdu[1]);
resp_begin( evt==ATT_OP_HANDLE_NOTIFY ? rsp_NOTIFY : rsp_IND );
send_uint( tag_HANDLE, handle );
send_data( pdu+3, len-3 );
resp_end();
if (evt == ATT_OP_HANDLE_NOTIFY)
return;
opdu = g_attrib_get_buffer(attrib, &plen);
olen = enc_confirmation(opdu, plen);
if (olen > 0)
g_attrib_send(attrib, 0, opdu, olen, NULL, NULL, NULL);
}
static void read_char_helper(guint8 status, const guint8 *rpdu,
guint16 rlen, gpointer user_data)
{
struct read_long_data *long_read = user_data;
int buflen;
uint8_t *buf = g_attrib_get_buffer(long_read->attrib, &buflen);
guint16 plen;
guint id;
if (status != 0 || rlen < buflen)
goto done;
long_read->buffer = g_malloc(rlen);
if (long_read->buffer == NULL)
goto done;
memcpy(long_read->buffer, rpdu, rlen);
long_read->size = rlen;
plen = enc_read_blob_req(long_read->handle, rlen - 1, buf, buflen);
id = g_attrib_send(long_read->attrib, long_read->id,
ATT_OP_READ_BLOB_REQ, buf, plen, read_blob_helper,
long_read, read_long_destroy);
if (id != 0) {
g_atomic_int_inc(&long_read->ref);
return;
}
status = ATT_ECODE_IO;
done:
long_read->func(status, rpdu, rlen, long_read->user_data);
}
guint gatt_discover_char(GAttrib *attrib, uint16_t start, uint16_t end,
bt_uuid_t *uuid, gatt_cb_t func,
gpointer user_data)
{
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(attrib, &buflen);
struct discover_char *dc;
bt_uuid_t type_uuid;
guint16 plen;
bt_uuid16_create(&type_uuid, GATT_CHARAC_UUID);
plen = enc_read_by_type_req(start, end, &type_uuid, buf, buflen);
if (plen == 0)
return 0;
dc = g_try_new0(struct discover_char, 1);
if (dc == NULL)
return 0;
dc->attrib = g_attrib_ref(attrib);
dc->cb = func;
dc->user_data = user_data;
dc->end = end;
dc->uuid = g_memdup(uuid, sizeof(bt_uuid_t));
return g_attrib_send(attrib, 0, buf, plen, char_discovered_cb,
dc, NULL);
}
static void read_char_helper(guint8 status, const guint8 *rpdu,
guint16 rlen, gpointer user_data)
{
struct read_long_data *long_read = user_data;
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(long_read->attrib, &buflen);
guint16 plen;
guint id;
if (status != 0 || rlen < buflen)
goto done;
long_read->buffer = g_malloc(rlen);
if (long_read->buffer == NULL) {
status = ATT_ECODE_INSUFF_RESOURCES;
goto done;
}
memcpy(long_read->buffer, rpdu, rlen);
long_read->size = rlen;
plen = enc_read_blob_req(long_read->handle, rlen - 1, buf, buflen);
id = g_attrib_send(long_read->attrib, long_read->id, buf, plen,
read_blob_helper, long_read, read_long_destroy);
if (id != 0) {
__sync_fetch_and_add(&long_read->ref, 1);
return;
}
status = ATT_ECODE_IO;
done:
long_read->func(status, rpdu, rlen, long_read->user_data);
}
static void events_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
{
uint8_t opdu[ATT_MAX_MTU];
uint16_t handle, i, olen;
handle = att_get_u16(&pdu[1]);
printf("\r");
switch (pdu[0]) {
case ATT_OP_HANDLE_NOTIFY:
printf("Notification handle = 0x%04x value: ", handle);
break;
case ATT_OP_HANDLE_IND:
printf("Indication handle = 0x%04x value: ", handle);
break;
default:
printf("Invalid opcode\n");
goto done;
}
for (i = 3; i < len; i++)
printf("%02x ", pdu[i]);
if (pdu[0] == ATT_OP_HANDLE_NOTIFY)
goto done;
olen = enc_confirmation(opdu, sizeof(opdu));
if (olen > 0)
g_attrib_send(attrib, 0, opdu[0], opdu, olen, NULL, NULL, NULL);
done:
printf("\r%s", get_prompt());
fflush(stdout);
}
guint gatt_read_char(GAttrib *attrib, uint16_t handle, GAttribResultFunc func,
gpointer user_data)
{
uint8_t *buf;
size_t buflen;
guint16 plen;
guint id;
struct read_long_data *long_read;
long_read = g_try_new0(struct read_long_data, 1);
if (long_read == NULL)
return 0;
long_read->attrib = attrib;
long_read->func = func;
long_read->user_data = user_data;
long_read->handle = handle;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_read_req(handle, buf, buflen);
id = g_attrib_send(attrib, 0, buf, plen, read_char_helper,
long_read, read_long_destroy);
if (id == 0)
g_free(long_read);
else {
__sync_fetch_and_add(&long_read->ref, 1);
long_read->id = id;
}
return id;
}
static void events_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
{
uint8_t opdu[ATT_MAX_MTU];
uint16_t handle, i, olen;
handle = att_get_u16(&pdu[1]);
printf("\n");
switch (pdu[0]) {
case ATT_OP_HANDLE_NOTIFY:
printf("Notification handle = 0x%04x value: ", handle);
break;
case ATT_OP_HANDLE_IND:
printf("Indication handle = 0x%04x value: ", handle);
break;
default:
printf("Invalid opcode\n");
return;
}
for (i = 3; i < len; i++)
printf("%02x ", pdu[i]);
printf("\n");
rl_forced_update_display();
if (pdu[0] == ATT_OP_HANDLE_NOTIFY)
return;
olen = enc_confirmation(opdu, sizeof(opdu));
if (olen > 0)
g_attrib_send(attrib, 0, opdu[0], opdu, olen, NULL, NULL, NULL);
}
static void primary_by_uuid_cb(guint8 status, const guint8 *ipdu,
guint16 iplen, gpointer user_data)
{
struct discover_primary *dp = user_data;
GSList *ranges, *last;
struct att_range *range;
uint8_t *buf;
guint16 oplen;
int err = 0;
size_t buflen;
if (status) {
err = status == ATT_ECODE_ATTR_NOT_FOUND ? 0 : status;
goto done;
}
ranges = dec_find_by_type_resp(ipdu, iplen);
if (ranges == NULL)
goto done;
dp->primaries = g_slist_concat(dp->primaries, ranges);
last = g_slist_last(ranges);
range = last->data;
if (range->end == 0xffff)
goto done;
/*
* If last handle is lower from previous start handle then it is smth
* wrong. Let's stop search, otherwise we might enter infinite loop.
*/
if (range->end < dp->start) {
err = ATT_ECODE_UNLIKELY;
goto done;
}
dp->start = range->end + 1;
buf = g_attrib_get_buffer(dp->attrib, &buflen);
oplen = encode_discover_primary(dp->start, 0xffff, &dp->uuid,
buf, buflen);
if (oplen == 0)
goto done;
g_attrib_send(dp->attrib, dp->id, buf, oplen, primary_by_uuid_cb,
discover_primary_ref(dp), discover_primary_unref);
return;
done:
dp->cb(err, dp->primaries, dp->user_data);
}
guint gatt_write_cmd(GAttrib *attrib, uint16_t handle, uint8_t *value, int vlen,
GDestroyNotify notify, gpointer user_data)
{
uint8_t *buf;
size_t buflen;
guint16 plen;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_write_cmd(handle, value, vlen, buf, buflen);
return g_attrib_send(attrib, 0, buf, plen, NULL, user_data, notify);
}
guint gatt_exchange_mtu(GAttrib *attrib, uint16_t mtu, GAttribResultFunc func,
gpointer user_data)
{
uint8_t *buf;
size_t buflen;
guint16 plen;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_mtu_req(mtu, buf, buflen);
return g_attrib_send(attrib, 0, buf, plen, func, user_data, NULL);
}
guint gatt_read_char(GAttrib *attrib, uint16_t handle, uint16_t offset,
GAttribResultFunc func, gpointer user_data)
{
uint8_t *buf;
int buflen;
guint16 plen;
guint id;
struct read_long_data *long_read;
long_read = g_try_new0(struct read_long_data, 1);
if (long_read == NULL)
return 0;
long_read->attrib = attrib;
long_read->func = func;
long_read->user_data = user_data;
long_read->handle = handle;
buf = g_attrib_get_buffer(attrib, &buflen);
if (offset > 0) {
plen = enc_read_blob_req(long_read->handle, offset, buf,
buflen);
id = g_attrib_send(attrib, 0, ATT_OP_READ_BLOB_REQ, buf, plen,
read_blob_helper, long_read, read_long_destroy);
} else {
plen = enc_read_req(handle, buf, buflen);
id = g_attrib_send(attrib, 0, ATT_OP_READ_REQ, buf, plen,
read_char_helper, long_read, read_long_destroy);
}
if (id == 0)
g_free(long_read);
else {
g_atomic_int_inc(&long_read->ref);
long_read->id = id;
}
return id;
}
static guint find_included(struct included_discovery *isd, uint16_t start)
{
bt_uuid_t uuid;
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(isd->attrib, &buflen);
guint16 oplen;
bt_uuid16_create(&uuid, GATT_INCLUDE_UUID);
oplen = enc_read_by_type_req(start, isd->end_handle, &uuid,
buf, buflen);
/* If id != 0 it means we are in the middle of include search */
if (isd->id)
return g_attrib_send(isd->attrib, isd->id, buf, oplen,
find_included_cb, isd_ref(isd),
(GDestroyNotify) isd_unref);
/* This is first call from the gattrib user */
isd->id = g_attrib_send(isd->attrib, 0, buf, oplen, find_included_cb,
isd_ref(isd), (GDestroyNotify) isd_unref);
return isd->id;
}
guint gatt_discover_char_desc(GAttrib *attrib, uint16_t start, uint16_t end,
GAttribResultFunc func, gpointer user_data)
{
uint8_t *buf;
size_t buflen;
guint16 plen;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_find_info_req(start, end, buf, buflen);
if (plen == 0)
return 0;
return g_attrib_send(attrib, 0, buf, plen, func, user_data, NULL);
}
static guint execute_write(GAttrib *attrib, uint8_t flags,
GAttribResultFunc func, gpointer user_data)
{
uint8_t *buf;
size_t buflen;
guint16 plen;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_exec_write_req(flags, buf, buflen);
if (plen == 0)
return 0;
return g_attrib_send(attrib, 0, buf, plen, func, user_data, NULL);
}
guint gatt_read_char_by_uuid(GAttrib *attrib, uint16_t start, uint16_t end,
bt_uuid_t *uuid, GAttribResultFunc func,
gpointer user_data)
{
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(attrib, &buflen);
guint16 plen;
plen = enc_read_by_type_req(start, end, uuid, buf, buflen);
if (plen == 0)
return 0;
return g_attrib_send(attrib, 0, buf, plen, func, user_data, NULL);
}
static guint find_included(struct included_discovery *isd, uint16_t start)
{
bt_uuid_t uuid;
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(isd->attrib, &buflen);
guint16 oplen;
bt_uuid16_create(&uuid, GATT_INCLUDE_UUID);
oplen = enc_read_by_type_req(start, isd->end_handle, &uuid,
buf, buflen);
return g_attrib_send(isd->attrib, 0, buf, oplen, find_included_cb,
isd_ref(isd), (GDestroyNotify) isd_unref);
}
static guint resolve_included_uuid(struct included_discovery *isd,
struct gatt_included *incl)
{
struct included_uuid_query *query;
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(isd->attrib, &buflen);
guint16 oplen = enc_read_req(incl->range.start, buf, buflen);
query = g_new0(struct included_uuid_query, 1);
query->isd = isd_ref(isd);
query->included = incl;
return g_attrib_send(isd->attrib, 0, buf, oplen,
resolve_included_uuid_cb, query, NULL);
}
guint gatt_find_info(GAttrib *attrib, uint16_t start, uint16_t end,
GAttribResultFunc func, gpointer user_data)
{
uint8_t *buf;
int buflen;
guint16 plen;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_find_info_req(start, end, buf, buflen);
if (plen == 0)
return 0;
return g_attrib_send(attrib, 0, ATT_OP_FIND_INFO_REQ, buf, plen, func,
user_data, NULL);
}
guint gatt_write_char(GAttrib *attrib, uint16_t handle, uint8_t *value,
int vlen, GAttribResultFunc func, gpointer user_data)
{
uint8_t *buf;
int buflen;
guint16 plen;
buf = g_attrib_get_buffer(attrib, &buflen);
if (func)
plen = enc_write_req(handle, value, vlen, buf, buflen);
else
plen = enc_write_cmd(handle, value, vlen, buf, buflen);
return g_attrib_send(attrib, 0, buf[0], buf, plen, func,
user_data, NULL);
}
static void read_blob_helper(guint8 status, const guint8 *rpdu, guint16 rlen,
gpointer user_data)
{
struct read_long_data *long_read = user_data;
uint8_t *buf;
int buflen;
guint8 *tmp;
guint16 plen;
guint id;
if (status != 0 || rlen == 1) {
status = 0;
goto done;
}
tmp = g_try_realloc(long_read->buffer, long_read->size + rlen - 1);
if (tmp == NULL) {
status = ATT_ECODE_INSUFF_RESOURCES;
goto done;
}
memcpy(&tmp[long_read->size], &rpdu[1], rlen - 1);
long_read->buffer = tmp;
long_read->size += rlen - 1;
buf = g_attrib_get_buffer(long_read->attrib, &buflen);
if (rlen < buflen)
goto done;
plen = enc_read_blob_req(long_read->handle, long_read->size - 1,
buf, buflen);
id = g_attrib_send(long_read->attrib, long_read->id,
ATT_OP_READ_BLOB_REQ, buf, plen,
read_blob_helper, long_read, read_long_destroy);
if (id != 0) {
g_atomic_int_inc(&long_read->ref);
return;
}
status = ATT_ECODE_IO;
done:
long_read->func(status, long_read->buffer, long_read->size,
long_read->user_data);
}
guint gatt_reliable_write_char(GAttrib *attrib, uint16_t handle,
const uint8_t *value, size_t vlen,
GAttribResultFunc func,
gpointer user_data)
{
uint8_t *buf;
guint16 plen;
size_t buflen;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_prep_write_req(handle, 0, value, vlen, buf, buflen);
if (!plen)
return 0;
return g_attrib_send(attrib, 0, buf, plen, func, user_data, NULL);
}
static void events_handler(const uint8_t *pdu, uint16_t len,
gpointer user_data)
{
struct gatt_service *gatt = user_data;
struct characteristic *chr;
GSList *l;
uint8_t opdu[ATT_MAX_MTU];
guint handle;
uint16_t olen;
if (len < 3) {
DBG("Malformed notification/indication packet (opcode 0x%02x)",
pdu[0]);
return;
}
handle = att_get_u16(&pdu[1]);
l = g_slist_find_custom(gatt->chars, GUINT_TO_POINTER(handle),
characteristic_handle_cmp);
if (!l)
return;
chr = l->data;
if (chr == NULL) {
DBG("Attribute handle 0x%02x not found", handle);
return;
}
switch (pdu[0]) {
case ATT_OP_HANDLE_IND:
olen = enc_confirmation(opdu, sizeof(opdu));
g_attrib_send(gatt->attrib, 0, opdu[0], opdu, olen,
NULL, NULL, NULL);
case ATT_OP_HANDLE_NOTIFY:
if (characteristic_set_value(chr, &pdu[3], len - 3) < 0)
DBG("Can't change Characteristic 0x%02x", handle);
g_slist_foreach(gatt->watchers, update_watchers_valuechange, chr);
break;
}
}
static guint prepare_write(struct write_long_data *long_write)
{
GAttrib *attrib = long_write->attrib;
uint16_t handle = long_write->handle;
uint16_t offset = long_write->offset;
uint8_t *buf, *value = long_write->value + offset;
size_t buflen, vlen = long_write->vlen - offset;
guint16 plen;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_prep_write_req(handle, offset, value, vlen, buf, buflen);
if (plen == 0)
return 0;
return g_attrib_send(attrib, 0, buf, plen, prepare_write_cb, long_write,
NULL);
}
static guint prepare_write(GAttrib *attrib, uint16_t handle, uint16_t offset,
uint8_t *value, size_t vlen, GAttribResultFunc func,
gpointer user_data)
{
guint16 plen;
size_t buflen;
uint8_t *buf;
buf = g_attrib_get_buffer(attrib, &buflen);
plen = enc_prep_write_req(handle, offset, &value[offset], vlen - offset,
buf, buflen);
if (plen == 0)
return 0;
return g_attrib_send(attrib, 0, buf, plen, prepare_write_cb,
user_data, NULL);
}
