From b93644221e61209550af7d50b8065ed4465c24b8 Mon Sep 17 00:00:00 2001
From: Glenn Ruben Bakke <glennbakke@gmail.com>
Date: Thu, 9 Feb 2017 23:56:32 +0100
Subject: [PATCH] nrf5/sdk: Adding new function to the softdevice handler
 driver to add vendor specific uuids and return an index to the entry back by
 reference.

---
 nrf5/sdk/softdevice.c | 18 ++++++++++++++++++
 nrf5/sdk/softdevice.h |  2 ++
 2 files changed, 20 insertions(+)

diff --git a/nrf5/sdk/softdevice.c b/nrf5/sdk/softdevice.c
index e59ddd66d1..439a56a16d 100644
--- a/nrf5/sdk/softdevice.c
+++ b/nrf5/sdk/softdevice.c
@@ -26,12 +26,19 @@
 
 #include <stdio.h>
 #include <string.h>
+
+#include "py/runtime.h"
 #include "softdevice.h"
 #include "mpconfigport.h"
 #include "nrf_sdm.h"
 #include "ble_gap.h"
 #include "ble.h" // sd_ble_uuid_encode
 
+#define SD_TEST_OR_ENABLE() \
+if (sd_enabled() == 0) { \
+    (void)sd_enable(); \
+}
+
 #if (BLUETOOTH_SD != 100) && (BLUETOOTH_SD != 110)
 #include "nrf_nvic.h"
 
@@ -199,3 +206,14 @@ void sd_advertise(void) {
 
     printf("Advertisment start status: " UINT_FMT "\n", (uint16_t)err_code);
 }
+
+bool sd_uuid_add_vs(uint8_t * p_uuid, uint8_t * idx) {
+    SD_TEST_OR_ENABLE();
+
+    if (sd_ble_uuid_vs_add((ble_uuid128_t const *)p_uuid, idx) != 0) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Can not add Vendor Specific 128-bit UUID."));
+    }
+
+    return true;
+}
diff --git a/nrf5/sdk/softdevice.h b/nrf5/sdk/softdevice.h
index 78cb60a127..341f93072d 100644
--- a/nrf5/sdk/softdevice.h
+++ b/nrf5/sdk/softdevice.h
@@ -25,6 +25,7 @@
  */
 
 #include <stdint.h>
+#include <stdbool.h>
 
 uint32_t sd_enable(void);
 
@@ -36,3 +37,4 @@ void sd_address_get(void);
 
 void sd_advertise(void);
 
+bool sd_uuid_add_vs(uint8_t * p_uuid, uint8_t * idx);