wip: implementing functionality

11 files changed, 209 insertions, 273 deletions

diff --git a/devices/ble_hci/common-hal/_bleio/Adapter.c b/devices/ble_hci/common-hal/_bleio/Adapter.c
index 6c94bb1a9..2e1baea04 100644
--- a/devices/ble_hci/common-hal/_bleio/Adapter.c
+++ b/devices/ble_hci/common-hal/_bleio/Adapter.c
@@ -240,7 +240,12 @@ void common_hal_bleio_adapter_set_enabled(bleio_adapter_obj_t *self, bool enable
     self->advertising_timeout_msecs = 0;
 
     // Reset list of known attributes.
+    // Indices into the list are handles. Handle 0x0000 designates an invalid handle,
+    // so store None there to skip it.
     self->attributes = mp_obj_new_list(0, NULL);
+    bleio_adapter_add_attribute(mp_const_none);
+    self->last_added_service_handle = BLE_GATT_HANDLE_INVALID;
+    self->last_added_characteristic_handle = BLE_GATT_HANDLE_INVALID;
 }
 
 bool common_hal_bleio_adapter_get_enabled(bleio_adapter_obj_t *self) {
@@ -683,6 +688,25 @@ void common_hal_bleio_adapter_erase_bonding(bleio_adapter_obj_t *self) {
     //FIX bonding_erase_storage();
 }
 
+uint16_t bleio_adapter_add_attribute(bleio_adapter_obj_t *adapter, mp_obj_t *attribute) {
+    // The handle is the index of this attribute in the attributes list.
+    uint16_t handle = (uint16_t) adapter->attributes->len;
+    mp_obj_list_append(adapter->attributes, attribute);
+    return handle;
+}
+
+mp_obj_t* bleio_adapter_get_attribute(bleio_adapter_obj_t *adapter, uint16_t handle) {
+    if (handle == 0 || handle >= adapter->attributes->len) {
+        return mp_const_none;
+    }
+    return adapter->attributes->items[handle];
+}
+
+uint16_t bleio_adapter_max_attribute_handle(bleio_adapter_obj_t *adapter) {
+    return adapter->attributes->len - 1;
+}
+
+
 void bleio_adapter_gc_collect(bleio_adapter_obj_t* adapter) {
     gc_collect_root((void**)adapter, sizeof(bleio_adapter_obj_t) / sizeof(size_t));
     gc_collect_root((void**)bleio_connections, sizeof(bleio_connections) / sizeof(size_t));
diff --git a/devices/ble_hci/common-hal/_bleio/Adapter.h b/devices/ble_hci/common-hal/_bleio/Adapter.h
index 9ff122d26..1fae5f682 100644
--- a/devices/ble_hci/common-hal/_bleio/Adapter.h
+++ b/devices/ble_hci/common-hal/_bleio/Adapter.h
@@ -68,8 +68,15 @@ typedef struct _bleio_adapter_obj_t {
     // All the local attributes for this device. The index into the list
     // corresponds to the handle.
     mp_obj_list_t *attributes;
+    // Handle for last added service. Characteristics can only be added immediately after
+    // the service they belong to. This vets that.
+    uint16_t last_added_service_handle;
+    uint16_t last_added_characteristic_handle;
 } bleio_adapter_obj_t;
 
+uint16_t bleio_adapter_add_attribute(bleio_adapter_obj_t *adapter, mp_obj_t *attribute);
+mp_obj_t* bleio_adapter_get_attribute(bleio_adapter_obj_t *adapter, uint16_t handle);
+uint16_t bleio_adapter_max_attribute_handle(bleio_adapter_obj_t *adapter);
 void bleio_adapter_background(bleio_adapter_obj_t* adapter);
 void bleio_adapter_gc_collect(bleio_adapter_obj_t* adapter);
 void bleio_adapter_reset(bleio_adapter_obj_t* adapter);
diff --git a/devices/ble_hci/common-hal/_bleio/Attribute.c b/devices/ble_hci/common-hal/_bleio/Attribute.c
index d9041da71..0c56c08a8 100644
--- a/devices/ble_hci/common-hal/_bleio/Attribute.c
+++ b/devices/ble_hci/common-hal/_bleio/Attribute.c
@@ -25,10 +25,24 @@
  */
 
 #include "shared-bindings/_bleio/Attribute.h"
+#include "shared-bindings/_bleio/Characteristic.h"
+#include "shared-bindings/_bleio/Descriptor.h"
+#include "shared-bindings/_bleio/Service.h"
 
-// Return the type of the attribute
-bleio_attribute_type_uuid(mp_obj_t *attribute) {
-    if mp_is_o
+// Return the type of the attribute.
+ble_attribute_type bleio_attribute_type_uuid(mp_obj_t *attribute) {
+    if (MP_OBJ_IS_TYPE(attribute, &bleio_characteristic_type)) {
+        return BLE_TYPE_CHARACTERISTIC;
+    }
+    if (MP_OBJ_IS_TYPE(attribute, &bleio_descriptor_type)) {
+        return BLE_TYPE_DESCRIPTOR;
+    }
+    if (MP_OBJ_IS_TYPE(attribute, &bleio_service_type)) {
+        bleio_service_obj_t *service = MP_OBJ_TO_PTR(attribute);
+        return service->is_secondary ? BLE_TYPE_SECONDARY_SERVICE : BLE_TYPE_PRIMARY_SERVICE;
+    }
+    return BLE_TYPE_UNKNOWN;
+}
 
 // Convert a _bleio security mode to a ble_gap_conn_sec_mode_t setting.
 // void bleio_attribute_gatts_set_security_mode(ble_gap_conn_sec_mode_t *perm, bleio_attribute_security_mode_t security_mode) {
diff --git a/devices/ble_hci/common-hal/_bleio/Attribute.h b/devices/ble_hci/common-hal/_bleio/Attribute.h
index f527bcf74..5ad2142d3 100644
--- a/devices/ble_hci/common-hal/_bleio/Attribute.h
+++ b/devices/ble_hci/common-hal/_bleio/Attribute.h
@@ -3,7 +3,7 @@
  *
  * The MIT License (MIT)
  *
- * Copyright (c) 2018 Dan Halbert for Adafruit Industries
+ * Copyright (c) 2020 Dan Halbert for Adafruit Industries
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -29,6 +29,15 @@
 
 #include "shared-module/_bleio/Attribute.h"
 
+// Types returned by attribute table lookups. These are UUIDs.
+enum ble_attribute_type {
+  BLE_TYPE_UNKNOWN           = 0x0000,
+  BLE_TYPE_PRIMARY_SERVICE   = 0x2800,
+  BLE_TYPE_SECONDARY_SERVICE = 0x2801,
+  BLE_TYPE_CHARACTERISTIC    = 0x2803,
+  BLE_TYPE_DESCRIPTOR        = 0x2900
+};
+
 // typedef struct
 // {
 //   uint8_t sm : 4;                     /**< Security Mode (1 or 2), 0 for no permissions at all. */
diff --git a/devices/ble_hci/common-hal/_bleio/Characteristic.c b/devices/ble_hci/common-hal/_bleio/Characteristic.c
index 74cbede0e..1c678b761 100644
--- a/devices/ble_hci/common-hal/_bleio/Characteristic.c
+++ b/devices/ble_hci/common-hal/_bleio/Characteristic.c
@@ -34,58 +34,10 @@
 
 #include "common-hal/_bleio/Adapter.h"
 
-// STATIC uint16_t characteristic_get_cccd(uint16_t cccd_handle, uint16_t conn_handle) {
-//     uint16_t cccd;
-//     // ble_gatts_value_t value = {
-//     //     .p_value = (uint8_t*) &cccd,
-//     //     .len = 2,
-//     // };
-
-//     // const uint32_t err_code = sd_ble_gatts_value_get(conn_handle, cccd_handle, &value);
-
-//     // if (err_code == BLE_ERROR_GATTS_SYS_ATTR_MISSING) {
-//     //     // CCCD is not set, so say that neither Notify nor Indicate is enabled.
-//     //     cccd = 0;
-//     // } else {
-//     //     check_nrf_error(err_code);
-//     // }
-
-//     return cccd;
-// }
-
-
-// STATIC void characteristic_gatts_notify_indicate(uint16_t handle, uint16_t conn_handle, mp_buffer_info_t *bufinfo, uint16_t hvx_type) {
-//     uint16_t hvx_len = bufinfo->len;
-
-//     ble_gatts_hvx_params_t hvx_params = {
-//         .handle = handle,
-//         .type = hvx_type,
-//         .offset = 0,
-//         .p_len = &hvx_len,
-//         .p_data = bufinfo->buf,
-//     };
-
-//     while (1) {
-//         const uint32_t err_code = sd_ble_gatts_hvx(conn_handle, &hvx_params);
-//         if (err_code == NRF_SUCCESS) {
-//             break;
-//         }
-//         // TX buffer is full
-//         // We could wait for an event indicating the write is complete, but just retrying is easier.
-//         if (err_code == NRF_ERROR_RESOURCES) {
-//             RUN_BACKGROUND_TASKS;
-//             continue;
-//         }
-
-//         // Some real error has occurred.
-//         check_nrf_error(err_code);
-//     }
-// }
-
 void common_hal_bleio_characteristic_construct(bleio_characteristic_obj_t *self, bleio_service_obj_t *service, uint16_t handle, bleio_uuid_obj_t *uuid, bleio_characteristic_properties_t props, bleio_attribute_security_mode_t read_perm, bleio_attribute_security_mode_t write_perm, mp_int_t max_length, bool fixed_length, mp_buffer_info_t *initial_value_bufinfo) {
     self->service = service;
     self->uuid = uuid;
-    //FIX self->handle = BLE_GATT_HANDLE_INVALID;
+    self->handle = BLE_GATT_HANDLE_INVALID;
     self->props = props;
     self->read_perm = read_perm;
     self->write_perm = write_perm;
@@ -153,30 +105,23 @@ void common_hal_bleio_characteristic_set_value(bleio_characteristic_obj_t *self,
             // Always write the value locally even if no connections are active.
             // conn_handle is ignored for non-system attributes, so we use BLE_CONN_HANDLE_INVALID.
             common_hal_bleio_gatts_write(self->handle, BLE_CONN_HANDLE_INVALID, bufinfo);
-            // Check to see if we need to notify or indicate any active connections.
-            for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
-                bleio_connection_internal_t *connection = &bleio_connections[i];
-                uint16_t conn_handle = connection->conn_handle;
-                if (conn_handle == BLE_CONN_HANDLE_INVALID) {
-                    continue;
-                }
-
-                //FIX
-                // uint16_t cccd = 0;
+            // Notify or indicate all active connections.
+            uint16_t cccd = 0;
+
+            const bool notify = self->props & CHAR_PROP_NOTIFY;
+            const bool indicate = self->props & CHAR_PROP_INDICATE;
+            // Read the CCCD value, if there is one.
+            if ((notify | indicate) && self->cccd_handle != BLE_GATT_HANDLE_INVALID) {
+                common_hal_bleio_gatts_read(self->cccd_handle, conn_handle, &cccd, sizeof(cccd));
+            }
 
-                // const bool notify = self->props & CHAR_PROP_NOTIFY;
-                // const bool indicate = self->props & CHAR_PROP_INDICATE;
-                // if (notify | indicate) {
-                //     cccd = characteristic_get_cccd(self->cccd_handle, conn_handle);
-                // }
+            // It's possible that both notify and indicate are set.
+            if (notify && (cccd & BLE_GATT_HVX_NOTIFICATION)) {
+                att_notify(self->handle, bufinfo->buf, MIN(bufinfo->len, self->max_length));
+            }
+            if (indicate && (cccd & BLE_GATT_HVX_INDICATION)) {
+                att_indicate(self->handle, bufinfo->buf, MIN(bufinfo->len, self->max_length));
 
-                // // It's possible that both notify and indicate are set.
-                // if (notify && (cccd & BLE_GATT_HVX_NOTIFICATION)) {
-                //     characteristic_gatts_notify_indicate(self->handle, conn_handle, bufinfo, BLE_GATT_HVX_NOTIFICATION);
-                // }
-                // if (indicate && (cccd & BLE_GATT_HVX_INDICATION)) {
-                //     characteristic_gatts_notify_indicate(self->handle, conn_handle, bufinfo, BLE_GATT_HVX_INDICATION);
-                // }
             }
         }
     }
@@ -191,35 +136,16 @@ bleio_characteristic_properties_t common_hal_bleio_characteristic_get_properties
 }
 
 void common_hal_bleio_characteristic_add_descriptor(bleio_characteristic_obj_t *self, bleio_descriptor_obj_t *descriptor) {
-    //FIX
-    // ble_uuid_t desc_uuid;
-    // bleio_uuid_convert_to_nrf_ble_uuid(descriptor->uuid, &desc_uuid);
-
-    // ble_gatts_attr_md_t desc_attr_md = {
-    //     // Data passed is not in a permanent location and should be copied.
-    //     .vloc = BLE_GATTS_VLOC_STACK,
-    //     .vlen = !descriptor->fixed_length,
-    // };
-
-    // bleio_attribute_gatts_set_security_mode(&desc_attr_md.read_perm, descriptor->read_perm);
-    // bleio_attribute_gatts_set_security_mode(&desc_attr_md.write_perm, descriptor->write_perm);
-
-    // mp_buffer_info_t desc_value_bufinfo;
-    // mp_get_buffer_raise(descriptor->value, &desc_value_bufinfo, MP_BUFFER_READ);
-
-    // ble_gatts_attr_t desc_attr = {
-    //     .p_uuid = &desc_uuid,
-    //     .p_attr_md = &desc_attr_md,
-    //     .init_len = desc_value_bufinfo.len,
-    //     .p_value = desc_value_bufinfo.buf,
-    //     .init_offs = 0,
-    //     .max_len = descriptor->max_length,
-    // };
+    if (self->handle != common_hal_bleio_adapter_obj->last_added_characteristic_handle) {
+        mp_raise_bleio_BluetoothError(
+            translate("Descriptor can only be added to most recently added characteristic"));
+    }
 
-    // check_nrf_error(sd_ble_gatts_descriptor_add(self->handle, &desc_attr, &descriptor->handle));
+    descriptor->handle = bleio_adapter_add_attribute(common_hal_bleio_adapter_obj, descriptor);
 
-    // descriptor->next = self->descriptor_list;
-    // self->descriptor_list = descriptor;
+    // Link together all the descriptors for this characteristic.
+    descriptor->next = self->descriptor_list;
+    self->descriptor_list = descriptor;
 }
 
 void common_hal_bleio_characteristic_set_cccd(bleio_characteristic_obj_t *self, bool notify, bool indicate) {
@@ -234,11 +160,12 @@ void common_hal_bleio_characteristic_set_cccd(bleio_characteristic_obj_t *self,
     const uint16_t conn_handle = bleio_connection_get_conn_handle(self->service->connection);
     common_hal_bleio_check_connected(conn_handle);
 
-    //FIX
-    // uint16_t cccd_value =
-    //     (notify ? BLE_GATT_HVX_NOTIFICATION : 0) |
-    //     (indicate ? BLE_GATT_HVX_INDICATION : 0);
+    uint16_t cccd_value =
+        (notify ? BLE_GATT_HVX_NOTIFICATION : 0) |
+        (indicate ? BLE_GATT_HVX_INDICATION : 0);
 
+    (void) cccd_value;
+    //FIX call att_something to set remote CCCD
     // ble_gattc_write_params_t write_params = {
     //     .write_op = BLE_GATT_OP_WRITE_REQ,
     //     .handle = self->cccd_handle,
diff --git a/devices/ble_hci/common-hal/_bleio/Characteristic.h b/devices/ble_hci/common-hal/_bleio/Characteristic.h
index 4d5fa02f0..7887afdfb 100644
--- a/devices/ble_hci/common-hal/_bleio/Characteristic.h
+++ b/devices/ble_hci/common-hal/_bleio/Characteristic.h
@@ -43,6 +43,7 @@ typedef struct _bleio_characteristic_obj {
     uint16_t max_length;
     bool fixed_length;
     uint16_t handle;
+    uint16_t value_handle; // Should be handle+1.
     bleio_characteristic_properties_t props;
     bleio_attribute_security_mode_t read_perm;
     bleio_attribute_security_mode_t write_perm;
diff --git a/devices/ble_hci/common-hal/_bleio/Service.c b/devices/ble_hci/common-hal/_bleio/Service.c
index ef2de6ee7..696b8b943 100644
--- a/devices/ble_hci/common-hal/_bleio/Service.c
+++ b/devices/ble_hci/common-hal/_bleio/Service.c
@@ -3,7 +3,7 @@
  *
  * The MIT License (MIT)
  *
- * Copyright (c) 2019 Dan Halbert for Adafruit Industries
+ * Copyright (c) 2020 Dan Halbert for Adafruit Industries
  * Copyright (c) 2018 Artur Pacholec
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -33,7 +33,6 @@
 #include "shared-bindings/_bleio/Adapter.h"
 
 uint32_t _common_hal_bleio_service_construct(bleio_service_obj_t *self, bleio_uuid_obj_t *uuid, bool is_secondary, mp_obj_list_t * characteristic_list) {
-    self->handle = 0xFFFF;
     self->uuid = uuid;
     self->characteristic_list = characteristic_list;
     self->is_remote = false;
@@ -42,16 +41,18 @@ uint32_t _common_hal_bleio_service_construct(bleio_service_obj_t *self, bleio_uu
 
     vm_used_ble = true;
 
-    self->handle = bleio_adapter_add_attribute(
-        common_hal_bleio_adapter_obj,
-        is_secondary ? BLE_TYPE_SECONDARY_SERVICE : BLE_TYPE_PRIMARY_SERVICE,
-        uuid, &status);
-    return status;
+    self->handle = bleio_adapter_add_attribute(common_hal_bleio_adapter_obj, self);
+    if (self->handle = BLE_GATT_HANDLE_INVALID) {
+        return 1;
+    }
+    return 0;
 }
 
 void common_hal_bleio_service_construct(bleio_service_obj_t *self, bleio_uuid_obj_t *uuid, bool is_secondary) {
-    check_hci_error(_common_hal_bleio_service_construct(self, uuid, is_secondary,
-                                                        mp_obj_new_list(0, NULL)));
+    if (_common_hal_bleio_service_construct(self, uuid, is_secondary,
+                                            mp_obj_new_list(0, NULL)) != 0) {
+        mp_raise_RuntimeError(translate("Failed to add service"));
+    }
 }
 
 void bleio_service_from_connection(bleio_service_obj_t *self, mp_obj_t connection) {
@@ -83,83 +84,36 @@ void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self,
                                                  bleio_characteristic_obj_t *characteristic,
                                                  mp_buffer_info_t *initial_value_bufinfo) {
 
-    //FIX how it's done by ArduinoBLE when a service is added.
-    // uint16_t startHandle = attributeCount();
-    uint16_t start_handle bleio_adapter_num_attributes(common_hal_bleio_adapter_obj);
-
-  // for (unsigned int i = 0; i < service->characteristicCount(); i++) {
-  //   BLELocalCharacteristic* characteristic = service->characteristic(i);
-
-  //   characteristic->retain();
-  //   _attributes.add(characteristic);
-  //   characteristic->setHandle(attributeCount());
-
-  //   // add the characteristic again to make space of the characteristic value handle
-  //   _attributes.add(characteristic);
-
-  //   for (unsigned int j = 0; j < characteristic->descriptorCount(); j++) {
-  //     BLELocalDescriptor* descriptor = characteristic->descriptor(j);
-
-  //     descriptor->retain();
-  //     _attributes.add(descriptor);
-  //     descriptor->setHandle(attributeCount());
-  //   }
-  // }
-
-  service->setHandles(startHandle, attributeCount());
-    // ble_gatts_char_md_t char_md = {
-    //     .char_props.broadcast      = (characteristic->props & CHAR_PROP_BROADCAST) ? 1 : 0,
-    //     .char_props.read           = (characteristic->props & CHAR_PROP_READ) ? 1 : 0,
-    //     .char_props.write_wo_resp  = (characteristic->props & CHAR_PROP_WRITE_NO_RESPONSE) ? 1 : 0,
-    //     .char_props.write          = (characteristic->props & CHAR_PROP_WRITE) ? 1 : 0,
-    //     .char_props.notify         = (characteristic->props & CHAR_PROP_NOTIFY) ? 1 : 0,
-    //     .char_props.indicate       = (characteristic->props & CHAR_PROP_INDICATE) ? 1 : 0,
-    // };
-
-    // ble_gatts_attr_md_t cccd_md = {
-    //     .vloc = BLE_GATTS_VLOC_STACK,
-    // };
-
-    // ble_gatts_attr_md_t char_attr_md = {
-    //     .vloc = BLE_GATTS_VLOC_STACK,
-    //     .vlen = !characteristic->fixed_length,
-    // };
-
-    // if (char_md.char_props.notify || char_md.char_props.indicate) {
-    //     BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
-    //     // Make CCCD write permission match characteristic read permission.
-    //     bleio_attribute_gatts_set_security_mode(&cccd_md.write_perm, characteristic->read_perm);
-
-    //     char_md.p_cccd_md = &cccd_md;
-    // }
-
-    // bleio_attribute_gatts_set_security_mode(&char_attr_md.read_perm, characteristic->read_perm);
-    // bleio_attribute_gatts_set_security_mode(&char_attr_md.write_perm, characteristic->write_perm);
+    if (self->handle != common_hal_bleio_adapter_obj->last_added_service_handle) {
+        mp_raise_bleio_BluetoothError(
+            translate("Characteristic can only be added to most recently added service"));
+    }
+    characteristic->decl_handle = bleio_adapter_add_attribute(common_hal_bleio_adapter_obj, characteristic);
+    // This is the value handle
+    characteristic->value_handle = bleio_adapter_add_attribute(common_hal_bleio_adapter_obj, characteristic);
+
+    if (characteristic->props & (CHAR_PROP_NOTIFY | CHAR_PROP_INDICATE)) {
+        // We need a CCCD.
+        bleio_descriptor_obj_t *cccd = m_new_obj(bleio_descriptor_obj_t);
+        cccd->base.type = &bleio_descriptor_type;
+        cccd->read_perm = SECURITY_MODE_OPEN;
+        // Make CCCD write permission match characteristic read permission.
+        cccd->write_perm = characteristic->read_perm;
+        characteristic->cccd_handle = common_hal_bleio_characteristic_add_descriptor(characteristic, cccd);
+    }
+
     // #if CIRCUITPY_VERBOSE_BLE
     // // Turn on read authorization so that we receive an event to print on every read.
     // char_attr_md.rd_auth = true;
     // #endif
 
-    // ble_gatts_attr_t char_attr = {
-    //     .p_uuid = &char_uuid,
-    //     .p_attr_md = &char_attr_md,
-    //     .init_len = 0,
-    //     .p_value = NULL,
-    //     .init_offs = 0,
-    //     .max_len = characteristic->max_length,
-    // };
-
-    // ble_gatts_char_handles_t char_handles;
-
-    // check_nrf_error(sd_ble_gatts_characteristic_add(self->handle, &char_md, &char_attr, &char_handles));
+    // These are not supplied or available.
+    characteristic->user_desc_handle = BLE_GATT_HANDLE_INVALID;
+    characteristic->sccd_handle = BLE_GATT_HANDLE_INVALID;
 
-    // characteristic->user_desc_handle = char_handles.user_desc_handle;
-    // characteristic->cccd_handle = char_handles.cccd_handle;
-    // characteristic->sccd_handle = char_handles.sccd_handle;
-    // characteristic->handle = char_handles.value_handle;
     // #if CIRCUITPY_VERBOSE_BLE
     // mp_printf(&mp_plat_print, "Char handle %x user %x cccd %x sccd %x\n", characteristic->handle, characteristic->user_desc_handle, characteristic->cccd_handle, characteristic->sccd_handle);
     // #endif
 
-    // mp_obj_list_append(self->characteristic_list, MP_OBJ_FROM_PTR(characteristic));
+    mp_obj_list_append(self->characteristic_list, MP_OBJ_FROM_PTR(characteristic));
 }
diff --git a/devices/ble_hci/common-hal/_bleio/__init__.c b/devices/ble_hci/common-hal/_bleio/__init__.c
index 07aaee747..c64719099 100644
--- a/devices/ble_hci/common-hal/_bleio/__init__.c
+++ b/devices/ble_hci/common-hal/_bleio/__init__.c
@@ -63,6 +63,10 @@ void check_hci_error(hci_result_t result) {
             mp_raise_bleio_BluetoothError(translate("Error writing to HCI adapter"));
             return;
 
+        case HCI_ATT_ERROR:
+            mp_raise_RuntimeError(translate("Error in ATT protocol code"));
+            return;
+
         default:
             // Should be an HCI status error, > 0.
             if (result > 0) {
diff --git a/devices/ble_hci/common-hal/_bleio/att.c b/devices/ble_hci/common-hal/_bleio/att.c
index ad1dddca2..013a51737 100644
--- a/devices/ble_hci/common-hal/_bleio/att.c
+++ b/devices/ble_hci/common-hal/_bleio/att.c
@@ -30,22 +30,15 @@
 
 #include "py/obj.h"
 #include "common-hal/_bleio/Adapter.h"
+#include "common-hal/_bleio/Attribute.h"
 #include "supervisor/shared/tick.h"
 
-enum ble_attribute_type {
-  BLE_TYPE_UNKNOWN           = 0x0000,
-  BLE_TYPE_PRIMARY_SERVICE   = 0x2800,
-  BLE_TYPE_SECONDARY_SERVICE = 0x2801,
-  BLE_TYPE_CHARACTERISTIC    = 0x2803,
-  BLE_TYPE_DESCRIPTOR        = 0x2900
-};
-
 STATIC uint16_t max_mtu = BT_ATT_DEFAULT_LE_MTU;  // 23
 STATIC unsigned long timeout = 5000;
 
 STATIC volatile bool confirm;
 
-STATIC uint16_t long_write_handle = 0x0000;
+STATIC uint16_t long_write_handle = BLE_GATT_HANDLE_INVALID;
 STATIC uint8_t* long_write_value = NULL;
 STATIC uint16_t long_write_value_length = 0;
 
@@ -123,7 +116,7 @@ STATIC void check_and_save_expected_rsp(uint16_t conn_handle, uint8_t opcode, ui
 void att_init(void) {
     max_mtu = BT_ATT_DEFAULT_LE_MTU;
     timeout = 5000;
-    long_write_handle = 0x0000;
+    long_write_handle = BLE_GATT_HANDLE_INVALID;
     long_write_value = NULL;
     long_write_value_length = 0;
 
@@ -225,12 +218,12 @@ bool att_disconnect_from_address(bt_addr_le_t *addr) {
 
 //                 reqStart_handle = rawService->end_handle + 1;
 
-//                 if (reqStart_handle == 0x0000) {
-//                     reqEnd_handle = 0x0000;
+//                 if (reqStart_handle == BLE_GATT_HANDLE_INVALID) {
+//                     reqEnd_handle = BLE_GATT_HANDLE_INVALID;
 //                 }
 //             }
 //         } else {
-//             reqEnd_handle = 0x0000;
+//             reqEnd_handle = BLE_GATT_HANDLE_INVALID;
 //         }
 //     }
 
@@ -498,7 +491,7 @@ void att_remove_connection(uint16_t handle, uint8_t reason) {
         //     }
         // }
 
-        long_write_handle = 0x0000;
+        long_write_handle = BLE_GATT_HANDLE_INVALID;
         long_write_value_length = 0;
     }
 
@@ -619,7 +612,7 @@ bool att_disconnect_all(void) {
 //     return BLEDevice();
 // }
 
-bool att_handle_notify(uint16_t handle, const uint8_t* value, int length) {
+bool att_notify(uint16_t handle, const uint8_t* value, int length) {
     int num_notifications = 0;
 
     for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
@@ -627,21 +620,20 @@ bool att_handle_notify(uint16_t handle, const uint8_t* value, int length) {
             continue;
         }
 
-        //FIX This seems fishy. Why just .mtu instead of .mtu + 1 for opcode
-        uint8_t notification[bleio_connections[i].mtu];
-        uint16_t notification_length = 0;
-
-        notification[0] = BT_ATT_OP_NOTIFY;
-        notification_length++;
-
-        memcpy(&notification[1], &handle, sizeof(handle));
-        notification_length += sizeof(handle);
+        typedef struct notify_t __packed {
+            struct bt_att_hdr hdr;
+            struct bt_att_notify ntf;
+        };
 
-        length = MIN((uint16_t)(bleio_connections[i].mtu - notification_length), (uint16_t)length);
-        memcpy(&notification[notification_length], value, length);
-        notification_length += length;
+        size_t allowed_length = MIN((uint16_t)(bleio_connections[i].mtu - sizeof(notify_t)), (uint16_t)length);
 
-        hci_send_acl_pkt(bleio_connections[i].conn_handle, BT_L2CAP_CID_ATT, notification_length, notification);
+        uint8_t notify_bytes[sizeof(cmd_s) + allowed_length];
+        notify_t *notify_p = (notify_t *) notify_bytes;
+        notify_p->hdr.code = BT_ATT_OP_NOTIFY;;
+        notify_p->ntf.handle = handle;
+        memcpy(notify_p->ntf.value, data, allowed_length);
+        hci_send_acl_pkt(bleio_connections[i].conn_handle, BT_L2CAP_CID_ATT,
+                         size_of(notify_bytes), notify_bytes);
 
         num_notifications++;
     }
@@ -649,7 +641,7 @@ bool att_handle_notify(uint16_t handle, const uint8_t* value, int length) {
     return (num_notifications > 0);
 }
 
-bool att_handle_indicate(uint16_t handle, const uint8_t* value, int length) {
+bool att_indicate(conn_handle, uint16_t handle, const uint8_t* value, int length) {
     int num_indications = 0;
 
     for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
@@ -657,22 +649,23 @@ bool att_handle_indicate(uint16_t handle, const uint8_t* value, int length) {
             continue;
         }
 
-        uint8_t indication[bleio_connections[i].mtu];
-        uint16_t indication_length = 0;
+        typedef struct indicate_t __packed {
+            struct bt_att_hdr hdr;
+            struct bt_att_indicate ind;
+        };
 
-        indication[0] = BT_ATT_OP_INDICATE;
-        indication_length++;
+        size_t allowed_length = MIN((uint16_t)(bleio_connections[i].mtu - sizeof(indicate_t)), (uint16_t)length);
 
-        memcpy(&indication[1], &handle, sizeof(handle));
-        indication_length += sizeof(handle);
-
-        length = MIN((uint16_t)(bleio_connections[i].mtu - indication_length), (uint16_t)length);
-        memcpy(&indication[indication_length], value, length);
-        indication_length += length;
+        uint8_t indicate_bytes[sizeof(cmd_s) + allowed_length];
+        struct indicate_s *indicate_p = (indicate_s *) indicate_bytes;
+        indicate_p->hdr.code = BT_ATT_OP_INDICATE;;
+        indicate_p->ind.handle = handle;
+        memcpy(indicate->ind.value, data, allowed_length);
 
         confirm = false;
 
-        hci_send_acl_pkt(bleio_connections[i].conn_handle, BT_L2CAP_CID_ATT, indication_length, indication);
+        hci_send_acl_pkt(bleio_connections[i].conn_handle, BT_L2CAP_CID_ATT,
+                         sizeof(indicate_bytes), indicate_bytes);
 
         while (!confirm) {
             hci_poll_for_incoming_pkt();
@@ -706,7 +699,7 @@ STATIC void process_error(uint16_t conn_handle, uint8_t dlen, uint8_t data[]) {
 STATIC void process_mtu_req(uint16_t conn_handle, uint8_t dlen, uint8_t data[]) {
     struct bt_att_exchange_mtu_req *req = (struct bt_att_exchange_mtu_req *) data;
     if (dlen != sizeof(req)) {
-        send_error(conn_handle, BT_ATT_OP_MTU_REQ, 0x0000, BT_ATT_ERR_INVALID_PDU);
+        send_error(conn_handle, BT_ATT_OP_MTU_REQ, BLE_GATT_HANDLE_INVALID, BT_ATT_ERR_INVALID_PDU);
         return;
     }
 
@@ -983,7 +976,7 @@ STATIC void process_read_or_read_blob_req(uint16_t conn_handle, uint16_t mtu, ui
 
     if (opcode == BT_ATT_OP_READ_REQ) {
         if (dlen != sizeof(struct bt_att_read_req)) {
-            send_error(conn_handle, BT_ATT_OP_READ_REQ, 0x0000, BT_ATT_ERR_INVALID_PDU);
+            send_error(conn_handle, BT_ATT_OP_READ_REQ, BLE_GATT_HANDLE_INVALID, BT_ATT_ERR_INVALID_PDU);
             return;
         }
 
@@ -993,7 +986,7 @@ STATIC void process_read_or_read_blob_req(uint16_t conn_handle, uint16_t mtu, ui
 
     } else {
         if (dlen != sizeof(struct bt_att_read_blob_req)) {
-            send_error(conn_handle, BT_ATT_OP_READ_BLOB_REQ, 0x0000, BT_ATT_ERR_INVALID_PDU);
+            send_error(conn_handle, BT_ATT_OP_READ_BLOB_REQ, BLE_GATT_HANDLE_INVALID, BT_ATT_ERR_INVALID_PDU);
             return;
         }
 
@@ -1234,7 +1227,7 @@ STATIC void process_write_req_or_cmd(uint16_t conn_handle, uint16_t mtu, uint8_t
 
     if (dlen < sizeof(struct bt_att_write_req)) {
         if (with_response) {
-            send_error(conn_handle, BT_ATT_OP_WRITE_REQ, 0x0000, BT_ATT_ERR_INVALID_PDU);
+            send_error(conn_handle, BT_ATT_OP_WRITE_REQ, BLE_GATT_HANDLE_INVALID, BT_ATT_ERR_INVALID_PDU);
         }
         return;
     }
@@ -1326,41 +1319,41 @@ STATIC void process_write_rsp(uint16_t conn_handle, uint8_t dlen, uint8_t data[]
 }
 
 STATIC void process_prepare_write_req(uint16_t conn_handle, uint16_t mtu, uint8_t dlen, uint8_t data[]) {
-    //FIX struct bt_att_prepare_write_req *req = (struct bt_att_prepare_write_req *) data;
+    FIX struct bt_att_prepare_write_req *req = (struct bt_att_prepare_write_req *) data;
 
     if (dlen < sizeof(struct bt_att_prepare_write_req)) {
-        send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, 0x0000, BT_ATT_ERR_INVALID_PDU);
+        send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, BLE_GATT_HANDLE_INVALID, BT_ATT_ERR_INVALID_PDU);
         return;
     }
 
-    // uint16_t handle = req->handle;
-    // uint16_t offset = req->offset;
+    uint16_t handle = req->handle;
+    uint16_t offset = req->offset;
 
-    // if ((uint16_t)(handle - 1) > GATT.attributeCount()) {
-    //     send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_ATTR_NOT_FOUND);
-    //     return;
-    // }
+    if (handle > bleio_adapter_max_attribute_handle(common_hal_bleio_adapter_obj)) {
+        send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_ATTR_NOT_FOUND);
+        return;
+    }
 
-    // BLELocalAttribute* attribute = GATT.attribute(handle - 1);
+    mp_obj_t *attribute = bleio_adapter_get_attribute(common_hal_bleio_adapter_obj, handle);
 
-    // if (attribute->type() != BLE_TYPE_CHARACTERISTIC) {
-    //     send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_ATTR_NOT_LONG);
-    //     return;
-    // }
+    if (!MP_OBJ_IS_TYPE(attribute, &bleio_characteristic_type)) {
+        send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_ATTR_NOT_LONG);
+        return;
+    }
 
-    // BLELocalCharacteristic* characteristic = (BLELocalCharacteristic*)attribute;
+    bleio_characteristic_obj_t* characteristic = MP_OBJ_TO_PTR(attribute);
 
-    // if (handle != characteristic->value_handle()) {
-    //     send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_ATTR_NOT_LONG);
-    //     return;
-    // }
+    if (handle != characteristic->value_handle) {
+        send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_ATTR_NOT_LONG);
+        return;
+    }
 
-    // if ((characteristic->properties() & BLEWrqite) == 0) {
-    //     send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_WRITE_NOT_PERM);
-    //     return;
-    // }
+    if (characteristic->props & CHAR_PROP_WRITE) {
+        send_error(conn_handle, BT_ATT_OP_PREPARE_WRITE_REQ, handle, BT_ATT_ERR_WRITE_NOT_PERM);
+        return;
+    }
 
-    // if (long_write_handle == 0) {
+    if (long_write_handle == BLE_GATT_HANDLE_INVALID)
     //     int valueSize = characteristic->valueSize();
 
     //     long_write_value = (uint8_t*)realloc(long_write_value, valueSize);
@@ -1398,7 +1391,7 @@ STATIC void process_exec_write_req(uint16_t conn_handle, uint16_t mtu, uint8_t d
     struct bt_att_exec_write_req *req = (struct bt_att_exec_write_req *) data;
 
     if (dlen != sizeof(struct bt_att_exec_write_req)) {
-        send_error(conn_handle, BT_ATT_OP_EXEC_WRITE_REQ, 0x0000, BT_ATT_ERR_INVALID_PDU);
+        send_error(conn_handle, BT_ATT_OP_EXEC_WRITE_REQ, BLE_GATT_HANDLE_INVALID, BT_ATT_ERR_INVALID_PDU);
         return;
     }
 
@@ -1413,7 +1406,7 @@ STATIC void process_exec_write_req(uint16_t conn_handle, uint16_t mtu, uint8_t d
         }
     }
 
-    long_write_handle = 0x0000;
+    long_write_handle = BLE_GATT_HANDLE_INVALID;
     long_write_value_length = 0;
 
     uint8_t response[mtu];
@@ -1515,33 +1508,35 @@ int att_read_req(uint16_t conn_handle, uint16_t handle, uint8_t response_buffer[
 }
 
 int att_write_req(uint16_t conn_handle, uint16_t handle, const uint8_t* data, uint8_t data_len, uint8_t response_buffer[]) {
-    struct __packed {
-        struct bt_att_hdr h;
-        struct bt_att_write_req r;
-    } req = { {
-            .code = BT_ATT_OP_WRITE_REQ,
-        }, {
-            .handle = handle,
-        }
+    typedef struct write_req_t __packed {
+        struct bt_att_hdr hdr;
+        struct bt_att_write_req req;
     };
-    memcpy(req.r.value, data, data_len);
 
-    return send_req_wait_for_rsp(conn_handle, sizeof(req) + data_len, (uint8_t *) &req, response_buffer);
+    uint8_t req_bytes[sizeof(write_req_t) + data_len];
+    struct write_req_t *write_req_P = (write_req_t *) req_bytes;
+    req_p->hdr.code = BT_ATT_OP_WRITE_REQ;
+    req_p->req.handle = handle;
+    memcpy(req_p->req.value, data, data_len);
+
+    memcpy(req.req.value, data, data_len);
+
+    return send_req_wait_for_rsp(conn_handle, sizeof(req_bytes), req, response_buffer);
 }
 
 void att_write_cmd(uint16_t conn_handle, uint16_t handle, const uint8_t* data, uint8_t data_len) {
-    struct __packed {
+    struct cmd_s __packed {
         struct bt_att_hdr h;
         struct bt_att_write_cmd r;
-    } req = { {
-            .code = BT_ATT_OP_WRITE_CMD,
-        }, {
-            .handle = handle,
-        }
     };
-    memcpy(req.r.value, data, data_len);
 
-    return send_req(conn_handle, sizeof(req) + data_len, (uint8_t *) &req);
+    uint8_t cmd_bytes[sizeof(cmd_s) + data_len];
+    struct cmd_s *cmd_p = (cmd_s *) cmd_bytes;
+    cmd_p->h.code = BT_ATT_OP_WRITE_CMD;
+    cmd_p->r.handle = handle;
+    memcpy(cmd_p->r.value, data, data_len);
+
+    return send_cmd(conn_handle, sizeof(cmd_bytes), cmd_bytes);
 }
 
 void att_process_data(uint16_t conn_handle, uint8_t dlen, uint8_t data[]) {
diff --git a/devices/ble_hci/common-hal/_bleio/att.h b/devices/ble_hci/common-hal/_bleio/att.h
index ee01e33f6..6cf9b305b 100644
--- a/devices/ble_hci/common-hal/_bleio/att.h
+++ b/devices/ble_hci/common-hal/_bleio/att.h
@@ -39,10 +39,10 @@ bool att_disconnect_all(void);
 bool att_disconnect_from_address(bt_addr_le_t *addr);
 bool att_discover_attributes(bt_addr_le_t *addr, const char* service_uuid_filter);
 bool att_exchange_mtu(uint16_t conn_handle);
-bool att_handle_indicate(uint16_t handle, const uint8_t* value, int length);
 bool att_handle_is_connected(uint16_t handle);
-bool att_handle_notify(uint16_t handle, const uint8_t* value, int length);
+bool att_indicate(uint16_t handle, const uint8_t* value, int length);
 bool att_is_connected(void);
+bool att_notify(uint16_t handle, const uint8_t* value, int length);
 int att_read_req(uint16_t conn_handle, uint16_t handle, uint8_t response_buffer[]);
 int att_write_req(uint16_t conn_handle, uint16_t handle, const uint8_t* data, uint8_t data_len, uint8_t response_buffer[]);
 uint16_t att_conn_handle(bt_addr_le_t *addr);
diff --git a/devices/ble_hci/common-hal/_bleio/hci.h b/devices/ble_hci/common-hal/_bleio/hci.h
index 736877ddc..89a2b3304 100644
--- a/devices/ble_hci/common-hal/_bleio/hci.h
+++ b/devices/ble_hci/common-hal/_bleio/hci.h
@@ -37,6 +37,7 @@ typedef int hci_result_t;
 #define HCI_WRITE_TIMEOUT (-3)
 #define HCI_READ_ERROR (-4)
 #define HCI_WRITE_ERROR (-5)
+#define HCI_ATT_ERROR (-6)
 
 void hci_init(void);