drivers/nble: Add initial implementation of bt_gatt_register

Change-Id: I2f72c203da9b99841cd83b26b25fbf435c54baf5
Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

drivers/nble/gatt.c

@@ -15,15 +15,184 @@
  */
 
 #include <errno.h>
+#include <misc/byteorder.h>
 
 #include <bluetooth/gatt.h>
 #include <bluetooth/log.h>
 
 #include "gatt_internal.h"
 
+#define NBLE_BUF_SIZE	384
+
+struct ble_gatt_service {
+	const struct bt_gatt_attr *attrs;
+	uint16_t attr_count;
+};
+
+static struct ble_gatt_service svc_db[BLE_GATTS_MAX_SERVICES];
+static uint8_t svc_count;
+
+/**
+ * Copy a UUID in a buffer using the smallest memory length
+ * @param p_buf Pointer to the memory where the UUID shall be copied
+ * @param p_uuid Pointer to the UUID to copy
+ * @return The length required to store the UUID in the memory
+ */
+static uint8_t bt_gatt_uuid_memcpy(uint8_t *buf, const struct bt_uuid *uuid)
+{
+	uint8_t *ptr = buf;
+
+	/* Store the type of the UUID */
+	*ptr = uuid->type;
+	ptr++;
+
+	/* Store the UUID data */
+	if (uuid->type == BT_UUID_TYPE_16) {
+		uint16_t le16;
+
+		le16 = sys_cpu_to_le16(BT_UUID_16(uuid)->val);
+		memcpy(ptr, &le16, sizeof(le16));
+		ptr += sizeof(le16);
+	} else {
+		memcpy(ptr, BT_UUID_128(uuid)->val, 16);
+		ptr += 16;
+	}
+
+	return ptr - buf;
+}
+
+/* These attributes need the value to be read */
+static struct bt_uuid *whitelist[] = {
+	BT_UUID_GATT_PRIMARY,
+	BT_UUID_GATT_SECONDARY,
+	BT_UUID_GATT_INCLUDE,
+	BT_UUID_GATT_CHRC,
+	BT_UUID_GATT_CEP,
+	BT_UUID_GATT_CUD,
+	BT_UUID_GATT_CPF,
+	BT_UUID_GAP_DEVICE_NAME,
+	BT_UUID_GAP_APPEARANCE,
+	BT_UUID_GAP_PPCP
+};
+
+static int attr_read(struct bt_gatt_attr *attr, uint8_t *data, size_t len)
+{
+	uint8_t i;
+	int data_size;
+
+	if (!data || len < 0) {
+		return -ENOMEM;
+	}
+
+	data_size = bt_gatt_uuid_memcpy(data, attr->uuid);
+
+	for (i = 0; i < ARRAY_SIZE(whitelist); i++) {
+		if (!bt_uuid_cmp(attr->uuid, whitelist[i])) {
+			int read;
+
+			read = attr->read(NULL, attr, data + data_size, len, 0);
+			if (read < 0) {
+				return read;
+			}
+
+			data_size += read;
+			break;
+		}
+	}
+
+	return data_size;
+}
+
 int bt_gatt_register(struct bt_gatt_attr *attrs, size_t count)
 {
-	return -ENOSYS;
+	struct ble_gatt_register_req param;
+	size_t i;
+	/* TODO: Replace the following with net_buf */
+	uint8_t attr_table[NBLE_BUF_SIZE];
+	uint8_t attr_table_size;
+
+	if (!attrs || !count) {
+		return -EINVAL;
+	}
+	BT_ASSERT(svc_count < BLE_GATTS_MAX_SERVICES);
+
+	svc_db[svc_count].attrs = attrs;
+	svc_db[svc_count].attr_count = count;
+	svc_count++;
+	param.attr_base = attrs;
+	param.attr_count = count;
+
+	attr_table_size = 0;
+
+	for (i = 0; i < count; i++) {
+		struct bt_gatt_attr *attr = &attrs[i];
+		struct ble_gatt_attr *att;
+
+		if (attr_table_size + sizeof(*att) > sizeof(attr_table)) {
+			return -ENOMEM;
+		}
+
+		att = (void *)&attr_table[attr_table_size];
+		att->perm = attr->perm;
+
+		attr_table_size += sizeof(*att);
+
+		/* Read attribute data */
+		att->data_size = attr_read(attr, att->data,
+					   sizeof(attr_table) -
+					   attr_table_size);
+		if (att->data_size < 0) {
+			BT_ERR("Failed to read attr: %d", att->data_size);
+			return att->data_size;
+		}
+
+		/* Compute the new element size and align it on upper 4 bytes
+		 * boundary.
+		 */
+		attr_table_size += (att->data_size + 3) & ~3;
+
+		BT_DBG("table size = %u attr data_size = %u", attr_table_size,
+		       att->data_size);
+	}
+
+	ble_gatt_register_req(&param, attr_table, attr_table_size);
+	return 0;
+}
+
+void on_ble_gatt_register_rsp(const struct ble_gatt_register_rsp *rsp,
+			      const struct ble_gatt_attr_handles *handles,
+			      uint8_t len)
+{
+	BT_DBG("status %u", rsp->status);
+
+	if (rsp->status != 0) {
+		return;
+	}
+#if defined(CONFIG_BLUETOOTH_DEBUG_GATT)
+	{
+		int idx;
+
+		for (idx = 0; idx < rsp->attr_count; idx++) {
+			/* The following order of declaration is assumed for
+			 * this to work (otherwise idx-2 will fail!):
+			 * BT_GATT_CHARACTERISTIC -> ble core returns invalid
+			 * handle.
+			 * BT_GATT_DESCRIPTOR -> value handle of characteristic
+			 * BT_GATT_CCC -> cccd handle is ignored as no storage
+			 * but reference value is updated in CCC with value
+			 * handle from descriptor.
+			 */
+			if (handles[idx].handle != 0) {
+				char uuid[37];
+
+				bt_uuid_to_str(rsp->attr_base[idx].uuid,
+					       uuid, sizeof(uuid));
+				BT_DBG("handle %u uuid %s", handles[idx].handle,
+				       uuid);
+			}
+		}
+	}
+#endif
 }
 
 void bt_gatt_foreach_attr(uint16_t start_handle, uint16_t end_handle,
@@ -40,14 +209,41 @@ int bt_gatt_attr_read(struct bt_conn *conn, const struct bt_gatt_attr *attr,
 		      void *buf, uint16_t buf_len, uint16_t offset,
 		      const void *value, uint16_t value_len)
 {
-	return -ENOSYS;
+	uint16_t len;
+
+	BT_DBG("handle 0x%04x offset %u", attr->handle, offset);
+
+	/* simply return the value length. used as max_value. */
+	if (buf == NULL) {
+		return value_len;
+	}
+
+	if (offset > value_len) {
+		return -EINVAL;
+	}
+
+	len = min(buf_len, value_len - offset);
+
+	memcpy(buf, value + offset, len);
+
+	return len;
 }
 
 int bt_gatt_attr_read_service(struct bt_conn *conn,
 			      const struct bt_gatt_attr *attr,
 			      void *buf, uint16_t len, uint16_t offset)
 {
-	return -ENOSYS;
+	struct bt_uuid *uuid = attr->user_data;
+
+	if (uuid->type == BT_UUID_TYPE_16) {
+		uint16_t uuid16 = sys_cpu_to_le16(BT_UUID_16(uuid)->val);
+
+		return bt_gatt_attr_read(conn, attr, buf, len, offset,
+					 &uuid16, 2);
+	}
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset,
+				 BT_UUID_128(uuid)->val, 16);
 }
 
 int bt_gatt_attr_read_included(struct bt_conn *conn,
@@ -57,11 +253,39 @@ int bt_gatt_attr_read_included(struct bt_conn *conn,
 	return -ENOSYS;
 }
 
+struct gatt_chrc {
+	uint8_t properties;
+	uint16_t value_handle;
+	union {
+		uint16_t uuid16;
+		uint8_t  uuid[16];
+	};
+} __packed;
+
 int bt_gatt_attr_read_chrc(struct bt_conn *conn,
 			   const struct bt_gatt_attr *attr, void *buf,
 			   uint16_t len, uint16_t offset)
 {
-	return -ENOSYS;
+	struct bt_gatt_chrc *chrc = attr->user_data;
+	struct gatt_chrc pdu;
+	uint8_t value_len;
+
+	pdu.properties = chrc->properties;
+
+	/* Handle cannot be read at this point */
+	pdu.value_handle = 0x0000;
+
+	value_len = sizeof(pdu.properties) + sizeof(pdu.value_handle);
+
+	if (chrc->uuid->type == BT_UUID_TYPE_16) {
+		pdu.uuid16 = sys_cpu_to_le16(BT_UUID_16(chrc->uuid)->val);
+		value_len += 2;
+	} else {
+		memcpy(pdu.uuid, BT_UUID_128(chrc->uuid)->val, 16);
+		value_len += 16;
+	}
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset, &pdu, value_len);
 }
 
 int bt_gatt_attr_read_ccc(struct bt_conn *conn,
@@ -186,13 +410,6 @@ void on_ble_gatts_get_attribute_value_rsp(const struct ble_gatts_attribute_respo
 	BT_DBG("");
 }
 
-void on_ble_gatt_register_rsp(const struct ble_gatt_register_rsp *rsp,
-			      const struct ble_gatt_attr_handles *handles,
-			      uint8_t len)
-{
-	BT_DBG("");
-}
-
 void on_ble_gattc_discover_rsp(const struct ble_gattc_disc_rsp *rsp,
 			       const uint8_t *data, uint8_t len)
 {

drivers/nble/gatt_internal.h

@@ -28,7 +28,7 @@ struct ble_core_response;
 /* Max number of service supported, if changed update BLE core needs to be
  * updated too!
  */
-#define BLE_GATTS_MAX_SERVICES (CONFIG_BT_GATT_BLE_MAX_SERVICES)
+#define BLE_GATTS_MAX_SERVICES 10
 
 /*
  * Internal APIs used between host and BLE controller
@@ -427,22 +427,20 @@ void ble_gattc_discover_req(const struct ble_core_discover_params *req,
 void on_ble_gattc_discover_rsp(const struct ble_gattc_disc_rsp *rsp,
 			       const uint8_t *data, uint8_t len);
 
+
 /** GATT Attribute stream structure.
  *
  * This structure is a "compressed" copy of @ref bt_gatt_attr.
  * UUID pointer and user_data pointer are used as offset into buffer itself.
  * The offset is from the beginning of the buffer. therefore a value of 0
- * means that UUID or user_data is not present.
+ * means that UUID or user_data is not present. */
- */
 struct ble_gatt_attr {
-	/** Attribute UUID offset */
-	uint16_t uuid_offset;
-	/** Attribute user data offset */
-	uint16_t user_data_offset;
-	/**< User data max length */
-	uint16_t max_len;
 	/** Attribute permissions */
 	uint16_t perm;
+	/** Attribute variable data size */
+	uint16_t data_size;
+	/** Attribute variable data: always starts with the UUID and data follows */
+	uint8_t data[0];
 };
 
 struct ble_gattc_read_params {