Add server handling for BT_ATT_OP_READ_MULT_REQ.
---
src/shared/gatt-server.c | 154 +++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 154 insertions(+)
diff --git a/src/shared/gatt-server.c b/src/shared/gatt-server.c
index f12ac47..b406ed6 100644
--- a/src/shared/gatt-server.c
+++ b/src/shared/gatt-server.c
@@ -92,6 +92,7 @@ struct bt_gatt_server {
unsigned int write_cmd_id;
unsigned int read_id;
unsigned int read_blob_id;
+ unsigned int read_multiple_id;
unsigned int prep_write_id;
unsigned int exec_write_id;
@@ -120,6 +121,7 @@ static void bt_gatt_server_free(struct bt_gatt_server *server)
bt_att_unregister(server->att, server->write_cmd_id);
bt_att_unregister(server->att, server->read_id);
bt_att_unregister(server->att, server->read_blob_id);
+ bt_att_unregister(server->att, server->read_multiple_id);
bt_att_unregister(server->att, server->prep_write_id);
bt_att_unregister(server->att, server->exec_write_id);
@@ -971,6 +973,149 @@ static void read_blob_cb(uint8_t opcode, const void *pdu,
handle_read_req(server, opcode, handle, offset);
}
+struct read_multiple_resp_data {
+ struct bt_gatt_server *server;
+ uint16_t *handles;
+ size_t cur_handle;
+ size_t num_handles;
+ uint8_t *rsp_data;
+ size_t length;
+ size_t mtu;
+};
+
+static void read_multiple_resp_data_free(struct read_multiple_resp_data *data)
+{
+ free(data->handles);
+ data->handles = NULL;
+
+ free(data->rsp_data);
+ data->rsp_data = NULL;
+}
+
+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 - #%lu of %lu: 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, NULL,
+ 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 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 - %lu 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, NULL,
+ 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 prep_write_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
@@ -1248,6 +1393,15 @@ static bool gatt_server_register_att_handlers(struct bt_gatt_server *server)
if (!server->read_blob_id)
return false;
+ /* Read Multiple Request */
+ server->read_multiple_id = bt_att_register(server->att,
+ BT_ATT_OP_READ_MULT_REQ,
+ read_multiple_cb,
+ server, NULL);
+
+ if (!server->read_multiple_id)
+ return false;
+
/* Prepare Write Request */
server->prep_write_id = bt_att_register(server->att,
BT_ATT_OP_PREP_WRITE_REQ,
--
2.2.0.rc0.207.ga3a616c
--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
