From c4c88f1bc968eda685a7c18648bead78985f2393 Mon Sep 17 00:00:00 2001
From: Jakob Krantz <jakob.krantz@u-blox.com>
Date: Mon, 20 Nov 2017 11:39:51 +0100
Subject: [PATCH] Adds functionality for setting the master-slave role policy.
 Fixes bug when handling HCI_EVENT_ROLE_CHANGE.

---
 src/hci.c     | 22 +++++++++++++++++-----
 src/hci.h     |  9 +++++++++
 src/hci_cmd.c |  7 +++++++
 src/hci_cmd.h |  1 +
 4 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/src/hci.c b/src/hci.c
index fef3cfde9..ec00b45a1 100644
--- a/src/hci.c
+++ b/src/hci.c
@@ -2133,9 +2133,10 @@ static void event_handler(uint8_t *packet, int size){
 #ifdef ENABLE_CLASSIC
         case HCI_EVENT_ROLE_CHANGE:
             if (packet[2]) break;   // status != 0
-            handle = little_endian_read_16(packet, 3);
-            conn = hci_connection_for_handle(handle);
-            if (!conn) break;       // no conn
+            reverse_bd_addr(&packet[3], addr);
+            addr_type = BD_ADDR_TYPE_CLASSIC;
+            conn = hci_connection_for_bd_addr_and_type(addr, addr_type);
+            if (!conn) break;
             conn->role = packet[9];
             break;
 #endif
@@ -2429,6 +2430,9 @@ void hci_init(const hci_transport_t *transport, const void *config){
 #ifdef ENABLE_CLASSIC
     // classic name
     hci_stack->local_name = default_classic_name;
+
+    // Master slave policy
+    hci_stack->master_slave_policy = 1;
 #endif
 
     // Secure Simple Pairing default: enable, no I/O capabilities, general bonding, mitm not required, auto accept 
@@ -3144,7 +3148,7 @@ static void hci_run(void){
                 connection->state = ACCEPTED_CONNECTION_REQUEST;
                 connection->role  = HCI_ROLE_SLAVE;
                 if (connection->address_type == BD_ADDR_TYPE_CLASSIC){
-                    hci_send_cmd(&hci_accept_connection_request, connection->address, 1);
+                    hci_send_cmd(&hci_accept_connection_request, connection->address, hci_stack->master_slave_policy);
                 } 
                 return;
 #endif
@@ -4496,8 +4500,16 @@ int hci_get_sco_packet_length(void){
     if (hci_stack->sco_voice_setting & 0x0020) return 51;
     return 27;
 }
-#endif
 
+/**
+* @brief Sets the master/slave policy
+* @param policy (0: attempt to become master, 1: let connecting device decide)
+*/
+void hci_set_master_slave_policy(uint8_t policy){
+    hci_stack->master_slave_policy = policy;
+}
+
+#endif
 
 HCI_STATE hci_get_state(void){
     return hci_stack->state;
diff --git a/src/hci.h b/src/hci.h
index 9e5f81e70..129a02cb4 100644
--- a/src/hci.h
+++ b/src/hci.h
@@ -848,6 +848,10 @@ typedef struct {
     bd_addr_t custom_bd_addr; 
     uint8_t   custom_bd_addr_set;
 
+#ifdef ENABLE_CLASSIC
+    uint8_t master_slave_policy;
+#endif
+
 } hci_stack_t;
 
 
@@ -998,6 +1002,11 @@ uint8_t* hci_get_outgoing_packet_buffer(void);
  */
 void hci_release_packet_buffer(void);
 
+/**
+* @brief Sets the master/slave policy
+* @param policy (0: attempt to become master, 1: let connecting device decide)
+*/
+void hci_set_master_slave_policy(uint8_t policy);
 
 /* API_END */
 
diff --git a/src/hci_cmd.c b/src/hci_cmd.c
index cd736cc1e..01319408c 100644
--- a/src/hci_cmd.c
+++ b/src/hci_cmd.c
@@ -547,6 +547,13 @@ const hci_cmd_t hci_write_link_policy_settings = {
 OPCODE(OGF_LINK_POLICY, 0x0d), "H2"
 };
 
+/**
+ * @param policy
+ */
+const hci_cmd_t hci_write_default_link_policy_setup = {
+    OPCODE(OGF_LINK_POLICY, 0x0F), "2"
+};
+
 
 /**
  *  Controller & Baseband Commands 
diff --git a/src/hci_cmd.h b/src/hci_cmd.h
index d9f84fb6d..aeeb91a08 100644
--- a/src/hci_cmd.h
+++ b/src/hci_cmd.h
@@ -148,6 +148,7 @@ extern const hci_cmd_t hci_write_extended_inquiry_response;
 extern const hci_cmd_t hci_write_inquiry_mode;
 extern const hci_cmd_t hci_write_le_host_supported;
 extern const hci_cmd_t hci_write_link_policy_settings;
+extern const hci_cmd_t hci_write_default_link_policy_setup;
 extern const hci_cmd_t hci_write_link_supervision_timeout;
 extern const hci_cmd_t hci_write_local_name;
 extern const hci_cmd_t hci_write_loopback_mode;