uint16_t dec_find_by_type_req(const uint8_t *pdu, size_t len, uint16_t *start,
uint16_t *end, bt_uuid_t *uuid,
uint8_t *value, size_t *vlen)
{
if (pdu == NULL)
return 0;
if (len < 7)
return 0;
/* Attribute Opcode (1 octet) */
if (pdu[0] != ATT_OP_FIND_BY_TYPE_REQ)
return 0;
/* First requested handle number (2 octets) */
*start = att_get_u16(&pdu[1]);
/* Last requested handle number (2 octets) */
*end = att_get_u16(&pdu[3]);
/* 16-bit UUID to find (2 octets) */
*uuid = att_get_uuid16(&pdu[5]);
/* Attribute value to find */
*vlen = len - 7;
if (*vlen > 0)
memcpy(value, pdu + 7, *vlen);
return len;
}
uint16_t dec_read_by_type_req(const uint8_t *pdu, size_t len, uint16_t *start,
uint16_t *end, bt_uuid_t *uuid)
{
const size_t min_len = sizeof(pdu[0]) + sizeof(*start) + sizeof(*end);
if (pdu == NULL)
return 0;
if (start == NULL || end == NULL || uuid == NULL)
return 0;
if (len < min_len + 2)
return 0;
if (pdu[0] != ATT_OP_READ_BY_TYPE_REQ)
return 0;
*start = att_get_u16(&pdu[1]);
*end = att_get_u16(&pdu[3]);
if (len == min_len + 2)
*uuid = att_get_uuid16(&pdu[5]);
else
*uuid = att_get_uuid128(&pdu[5]);
return len;
}
static struct gatt_included *included_from_buf(const uint8_t *buf, gsize len)
{
struct gatt_included *incl = g_new0(struct gatt_included, 1);
incl->handle = att_get_u16(&buf[0]);
incl->range.start = att_get_u16(&buf[2]);
incl->range.end = att_get_u16(&buf[4]);
if (len == 8) {
bt_uuid_t uuid128;
bt_uuid_t uuid16 = att_get_uuid16(&buf[6]);
bt_uuid_to_uuid128(&uuid16, &uuid128);
bt_uuid_to_string(&uuid128, incl->uuid, sizeof(incl->uuid));
}
return incl;
}
static void char_desc_cb(guint8 status, const guint8 *pdu, guint16 plen,
gpointer user_data)
{
struct att_data_list *list;
guint8 format;
uint16_t handle = 0xffff;
int i;
if (status != 0) {
resp_error(err_COMM_ERR); // Todo: status
return;
}
list = dec_find_info_resp(pdu, plen, &format);
if (list == NULL) {
resp_error(err_NOT_FOUND); // Todo: what does this mean?
return;
}
resp_begin(rsp_DESCRIPTORS);
for (i = 0; i < list->num; i++) {
char uuidstr[MAX_LEN_UUID_STR];
uint8_t *value;
bt_uuid_t uuid;
value = list->data[i];
handle = att_get_u16(value);
if (format == 0x01)
uuid = att_get_uuid16(&value[2]);
else
uuid = att_get_uuid128(&value[2]);
bt_uuid_to_string(&uuid, uuidstr, MAX_LEN_UUID_STR);
send_uint(tag_HANDLE, handle);
send_str (tag_UUID, uuidstr);
}
resp_end();
att_data_list_free(list);
if (handle != 0xffff && handle < end)
gatt_discover_char_desc(attrib, handle + 1, end, char_desc_cb, NULL);
}
static void char_desc_cb(guint8 status, const guint8 *pdu, guint16 plen,
gpointer user_data)
{
struct att_data_list *list;
guint8 format;
int i;
if (status != 0) {
printf("Discover all characteristic descriptors failed: "
"%s\n", att_ecode2str(status));
goto done;
}
list = dec_find_info_resp(pdu, plen, &format);
if (list == NULL)
goto done;
printf("\n");
for (i = 0; i < list->num; i++) {
char uuidstr[MAX_LEN_UUID_STR];
uint16_t handle;
uint8_t *value;
bt_uuid_t uuid;
value = list->data[i];
handle = att_get_u16(value);
if (format == 0x01)
uuid = att_get_uuid16(&value[2]);
else
uuid = att_get_uuid128(&value[2]);
bt_uuid_to_string(&uuid, uuidstr, MAX_LEN_UUID_STR);
printf("handle: 0x%04x, uuid: %s\n", handle, uuidstr);
}
att_data_list_free(list);
done:
printf("\n%s", get_prompt());
fflush(stdout);
}
static void char_desc_cb(guint8 status, const guint8 *pdu, guint16 plen,
gpointer user_data)
{
struct att_data_list *list;
guint8 format;
uint16_t handle = 0xffff;
int i;
if (status != 0) {
rl_printf("Discover descriptors finished: %s\n",
att_ecode2str(status));
return;
}
list = dec_find_info_resp(pdu, plen, &format);
if (list == NULL)
return;
for (i = 0; i < list->num; i++) {
char uuidstr[MAX_LEN_UUID_STR];
uint8_t *value;
bt_uuid_t uuid;
value = list->data[i];
handle = att_get_u16(value);
if (format == 0x01)
uuid = att_get_uuid16(&value[2]);
else
uuid = att_get_uuid128(&value[2]);
bt_uuid_to_string(&uuid, uuidstr, MAX_LEN_UUID_STR);
rl_printf("handle: 0x%04x, uuid: %s\n", handle, uuidstr);
}
att_data_list_free(list);
if (handle != 0xffff && handle < end)
gatt_discover_char_desc(attrib, handle + 1, end, char_desc_cb,
NULL);
}
uint16_t dec_find_by_type_req(const uint8_t *pdu, size_t len, uint16_t *start,
uint16_t *end, bt_uuid_t *uuid,
uint8_t *value, size_t *vlen)
{
size_t valuelen;
uint16_t min_len = sizeof(pdu[0]) + sizeof(*start) +
sizeof(*end) + sizeof(uint16_t);
if (pdu == NULL)
return 0;
if (len < min_len)
return 0;
if (pdu[0] != ATT_OP_FIND_BY_TYPE_REQ)
return 0;
/* First requested handle number */
if (start)
*start = att_get_u16(&pdu[1]);
/* Last requested handle number */
if (end)
*end = att_get_u16(&pdu[3]);
/* Always UUID16 */
if (uuid)
*uuid = att_get_uuid16(&pdu[5]);
valuelen = len - min_len;
/* Attribute value to find */
if (valuelen > 0 && value)
memcpy(value, pdu + min_len, valuelen);
if (vlen)
*vlen = valuelen;
return len;
}
static void char_discovered_cb(guint8 status, const guint8 *ipdu, guint16 iplen,
gpointer user_data)
{
struct discover_char *dc = user_data;
struct att_data_list *list;
unsigned int i, err = ATT_ECODE_ATTR_NOT_FOUND;
uint16_t last = 0;
if (status) {
err = status;
goto done;
}
list = dec_read_by_type_resp(ipdu, iplen);
if (list == NULL) {
err = ATT_ECODE_IO;
goto done;
}
for (i = 0; i < list->num; i++) {
uint8_t *value = list->data[i];
struct gatt_char *chars;
bt_uuid_t uuid;
last = att_get_u16(value);
if (list->len == 7) {
bt_uuid_t uuid16 = att_get_uuid16(&value[5]);
bt_uuid_to_uuid128(&uuid16, &uuid);
} else
uuid = att_get_uuid128(&value[5]);
if (dc->uuid && bt_uuid_cmp(dc->uuid, &uuid))
continue;
chars = g_try_new0(struct gatt_char, 1);
if (!chars) {
err = ATT_ECODE_INSUFF_RESOURCES;
goto done;
}
chars->handle = last;
chars->properties = value[2];
chars->value_handle = att_get_u16(&value[3]);
bt_uuid_to_string(&uuid, chars->uuid, sizeof(chars->uuid));
dc->characteristics = g_slist_append(dc->characteristics,
chars);
}
att_data_list_free(list);
if (last != 0 && (last + 1 < dc->end)) {
bt_uuid_t uuid;
guint16 oplen;
size_t buflen;
uint8_t *buf;
buf = g_attrib_get_buffer(dc->attrib, &buflen);
bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
oplen = enc_read_by_type_req(last + 1, dc->end, &uuid, buf,
buflen);
if (oplen == 0)
return;
g_attrib_send(dc->attrib, 0, buf, oplen, char_discovered_cb,
dc, NULL);
return;
}
done:
err = (dc->characteristics ? 0 : err);
dc->cb(dc->characteristics, err, dc->user_data);
discover_char_free(dc);
}
static void primary_all_cb(guint8 status, const guint8 *ipdu, guint16 iplen,
gpointer user_data)
{
struct discover_primary *dp = user_data;
struct att_data_list *list;
unsigned int i, err;
uint16_t start, end;
if (status) {
err = status == ATT_ECODE_ATTR_NOT_FOUND ? 0 : status;
goto done;
}
list = dec_read_by_grp_resp(ipdu, iplen);
if (list == NULL) {
err = ATT_ECODE_IO;
goto done;
}
for (i = 0, end = 0; i < list->num; i++) {
const uint8_t *data = list->data[i];
struct gatt_primary *primary;
bt_uuid_t uuid;
start = att_get_u16(&data[0]);
end = att_get_u16(&data[2]);
if (list->len == 6) {
bt_uuid_t uuid16 = att_get_uuid16(&data[4]);
bt_uuid_to_uuid128(&uuid16, &uuid);
} else if (list->len == 20) {
uuid = att_get_uuid128(&data[4]);
} else {
/* Skipping invalid data */
continue;
}
primary = g_try_new0(struct gatt_primary, 1);
if (!primary) {
att_data_list_free(list);
err = ATT_ECODE_INSUFF_RESOURCES;
goto done;
}
primary->range.start = start;
primary->range.end = end;
bt_uuid_to_string(&uuid, primary->uuid, sizeof(primary->uuid));
dp->primaries = g_slist_append(dp->primaries, primary);
}
att_data_list_free(list);
err = 0;
if (end != 0xffff) {
size_t buflen;
uint8_t *buf = g_attrib_get_buffer(dp->attrib, &buflen);
guint16 oplen = encode_discover_primary(end + 1, 0xffff, NULL,
buf, buflen);
g_attrib_send(dp->attrib, 0, buf, oplen, primary_all_cb,
dp, NULL);
return;
}
done:
dp->cb(dp->primaries, err, dp->user_data);
discover_primary_free(dp);
}
static void descriptor_cb(guint8 status, const guint8 *pdu, guint16 plen,
gpointer user_data)
{
struct query_data *current = user_data;
struct gatt_service *gatt = current->gatt;
struct characteristic *chr = current->chr;
struct att_data_list *list;
guint8 format;
int i;
INFO("%s status=%d",chr->path,status);
if (status != 0) {
chr->descriptor_count = 0;
if (att_device_connected(gatt))
goto update;
else
goto done;
}
list = dec_find_info_resp(pdu, plen, &format);
if (list == NULL) {
INFO("%s no descriptors",chr->path);
chr->descriptor_count = 0;
goto update;
}
INFO("%s %d descriptors",chr->path,list->num);
chr->descriptor_count = list->num;
for (i = 0; i < list->num; i++) {
guint16 handle;
bt_uuid_t uuid;
uint8_t *dinfo = list->data[i];
struct query_data *qfmt;
guint ret = 0;
handle = att_get_u16(dinfo);
if (format == 0x01) {
uuid = att_get_uuid16(&dinfo[2]);
} else {
/* Currently, only "user description" and "presentation
* format" descriptors are used, and both have 16-bit
* UUIDs. Therefore there is no need to support format
* 0x02 yet. */
INFO("unkown format");
add_characteristic_descriptor(current,0x0000);
continue;
}
qfmt = g_new0(struct query_data, 1);
qfmt->gatt = current->gatt;
qfmt->chr = current->chr;
qfmt->handle = handle;
if (uuid_desc16_cmp(&uuid, GATT_CHARAC_USER_DESC_UUID) == 0) {
query_list_append(gatt, qfmt);
ret = gatt_read_char(gatt->attrib, handle, 0, update_char_desc,
qfmt);
} else if (uuid_desc16_cmp(&uuid, GATT_CHARAC_FMT_UUID) == 0) {
query_list_append(gatt, qfmt);
ret = gatt_read_char(gatt->attrib, handle, 0,
update_char_format, qfmt);
} else if (uuid_desc16_cmp(&uuid, GATT_CLIENT_CHARAC_CFG_UUID) == 0) {
query_list_append(gatt, qfmt);
ret = gatt_read_char(gatt->attrib, handle, 0,
update_char_client_conf, qfmt);
} else if (uuid_desc16_cmp(&uuid, GATT_SERVER_CHARAC_CFG_UUID) == 0) {
query_list_append(gatt, qfmt);
ret = gatt_read_char(gatt->attrib, handle, 0,
update_char_server_conf, qfmt);
} else if (uuid_desc16_cmp(&uuid, GATT_CHARAC_EXT_PROPER_UUID) == 0) {
query_list_append(gatt, qfmt);
ret = gatt_read_char(gatt->attrib, handle, 0,
update_char_ext_props, qfmt);
} else
g_free(qfmt);
if (ret == 0)
add_characteristic_descriptor(current,0x0000);
}
att_data_list_free(list);
update:
check_updated_characteristics(gatt);
done:
query_list_remove(gatt, current);
g_free(current);
}
void char_desc_cb(guint8 status, const guint8 *pdu, guint16 plen,
gpointer user_data) {
struct att_data_list *list = NULL;
guint8 format;
uint16_t handle = 0xffff;
int i;
char uuid_str[MAX_LEN_UUID_STR];
uint8_t *value;
printf_dbg("[CB] IN char_desc_cb\n");
if (status) {
cb_ret_val = BL_REQUEST_FAIL_ERROR;
sprintf(cb_ret_msg, "Characteristic descriptor "
"callback: Failure: %s\n", att_ecode2str(status));
goto exit;
}
list = dec_find_info_resp(pdu, plen, &format);
if (list == NULL) {
cb_ret_val = BL_NO_ERROR;
strcpy(cb_ret_msg, "Characteristic descriptor callback: Nothing found\n");
goto exit;
}
for (i = 0; i < list->num; i++) {
bt_uuid_t uuid;
value = list->data[i];
handle = att_get_u16(value);
if (format == 0x01)
uuid = att_get_uuid16(&value[2]);
else
uuid = att_get_uuid128(&value[2]);
bt_uuid_to_string(&uuid, uuid_str, MAX_LEN_UUID_STR);
if (strcmp(uuid_str, GATT_PRIM_SVC_UUID_STR) &&
strcmp(uuid_str, GATT_SND_SVC_UUID_STR) &&
strcmp(uuid_str, GATT_INCLUDE_UUID_STR) &&
strcmp(uuid_str, GATT_CHARAC_UUID_STR)) {
bl_desc_t *bl_desc = bl_desc_new(uuid_str, handle);
if (bl_desc == NULL) {
cb_ret_val = BL_MALLOC_ERROR;
strcpy(cb_ret_msg, "Characteristic descriptor callback: Malloc "
"error\n");
goto exit;
}
if (bl_desc_list == NULL) {
bl_desc_list = g_slist_alloc();
if (bl_desc_list == NULL) {
cb_ret_val = BL_MALLOC_ERROR;
strcpy(cb_ret_msg, "Characteristic descriptor callback: Malloc "
"error\n");
goto exit;
}
bl_desc_list->data = bl_desc;
} else {
bl_desc_list = g_slist_append(bl_desc_list, bl_desc);
}
} else {
printf_dbg("Reach end of descriptor list\n");
goto exit;
}
}
if ((handle != 0xffff) && (handle < end_handle_cb)) {
printf_dbg("[CB] OUT with asking for a new request\n");
if (gatt_discover_char_desc(attrib, handle + 1, end_handle_cb,
char_desc_cb, NULL)) {
goto next;
}
cb_ret_val = BL_SEND_REQUEST_ERROR;
strcpy(cb_ret_msg, "Unable to send request\n");
}
exit:
if (bl_desc_list) {
// Return what we got if we add something
cb_ret_val = BL_NO_ERROR;
cb_ret_pointer = bl_desc_list;
}
bl_desc_list = NULL;
g_mutex_unlock(pending_callback);
next:
if (list)
att_data_list_free(list);
printf_dbg("[CB] OUT char_desc_cb\n");
}
