From 17a663eb0dfc203e4497f05cf0aa0bdc7fdffab5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C5=81ukasz=20Rymanowski?= <lukasz.rymanowski@codecoup.pl>
Date: Tue, 16 Jan 2018 19:46:34 +0100
Subject: [PATCH] nimble/att: Fix response for unknown opcode

Fixes MYNEWT-886

With this patch if unknown ATT request come to ATT channel, Nimble
replies with ATT Error Response with error code Request Not Supported.

That should fix GATT/SR/UNS/BI-01-C

X-Original-Commit: 768633db326ebb8596181cb563028bca2f38d2ef
---
 nimble/host/src/ble_att.c               | 22 +++++++++++++++++++-
 nimble/host/src/ble_att_priv.h          |  3 +++
 nimble/host/src/ble_att_svr.c           |  2 +-
 nimble/host/test/src/ble_att_svr_test.c | 27 +++++++++++++++++++++++++
 4 files changed, 52 insertions(+), 2 deletions(-)

diff --git a/nimble/host/src/ble_att.c b/nimble/host/src/ble_att.c
index 774444ab0..94f6f41ed 100644
--- a/nimble/host/src/ble_att.c
+++ b/nimble/host/src/ble_att.c
@@ -461,6 +461,22 @@ ble_att_chan_mtu(const struct ble_l2cap_chan *chan)
     return mtu;
 }
 
+static void
+ble_att_rx_handle_unknown_request(uint8_t op, uint16_t conn_handle,
+                                  struct os_mbuf **om)
+{
+    /* If this is command (bit6 is set to 1), do nothing */
+    if (op & 0x40) {
+        return;
+    }
+
+    os_mbuf_adj(*om, OS_MBUF_PKTLEN(*om));
+    ble_att_svr_tx_error_rsp(conn_handle, *om, op, 0,
+                             BLE_ATT_ERR_REQ_NOT_SUPPORTED);
+
+    *om = NULL;
+}
+
 static int
 ble_att_rx(struct ble_l2cap_chan *chan)
 {
@@ -485,7 +501,8 @@ ble_att_rx(struct ble_l2cap_chan *chan)
 
     entry = ble_att_rx_dispatch_entry_find(op);
     if (entry == NULL) {
-        return BLE_HS_EINVAL;
+        ble_att_rx_handle_unknown_request(op, conn_handle, om);
+        return BLE_HS_ENOTSUP;
     }
 
     ble_att_inc_rx_stat(op);
@@ -495,6 +512,9 @@ ble_att_rx(struct ble_l2cap_chan *chan)
 
     rc = entry->bde_fn(conn_handle, om);
     if (rc != 0) {
+        if (rc == BLE_HS_ENOTSUP) {
+            ble_att_rx_handle_unknown_request(op, conn_handle, om);
+        }
         return rc;
     }
 
diff --git a/nimble/host/src/ble_att_priv.h b/nimble/host/src/ble_att_priv.h
index e85a1c695..8ab15b101 100644
--- a/nimble/host/src/ble_att_priv.h
+++ b/nimble/host/src/ble_att_priv.h
@@ -222,6 +222,9 @@ int ble_att_svr_init(void);
 void ble_att_svr_hide_range(uint16_t start_handle, uint16_t end_handle);
 void ble_att_svr_restore_range(uint16_t start_handle, uint16_t end_handle);
 
+int ble_att_svr_tx_error_rsp(uint16_t conn_handle, struct os_mbuf *txom,
+                             uint8_t req_op, uint16_t handle,
+                             uint8_t error_code);
 /*** $clt */
 
 /** An information-data entry in a find information response. */
diff --git a/nimble/host/src/ble_att_svr.c b/nimble/host/src/ble_att_svr.c
index f0911663e..e2d2c9a80 100644
--- a/nimble/host/src/ble_att_svr.c
+++ b/nimble/host/src/ble_att_svr.c
@@ -573,7 +573,7 @@ ble_att_svr_write_handle(uint16_t conn_handle, uint16_t attr_handle,
     return 0;
 }
 
-static int
+int
 ble_att_svr_tx_error_rsp(uint16_t conn_handle, struct os_mbuf *txom,
                          uint8_t req_op, uint16_t handle, uint8_t error_code)
 {
diff --git a/nimble/host/test/src/ble_att_svr_test.c b/nimble/host/test/src/ble_att_svr_test.c
index 3e71b603c..5dd9987cc 100644
--- a/nimble/host/test/src/ble_att_svr_test.c
+++ b/nimble/host/test/src/ble_att_svr_test.c
@@ -2052,6 +2052,32 @@ TEST_CASE(ble_att_svr_test_oom)
     TEST_ASSERT_FATAL(rc == 0);
 }
 
+TEST_CASE(ble_att_svr_test_unsupported_req)
+{
+    uint16_t conn_handle;
+    int rc;
+    uint8_t buf[] = {0x3f, 0x00, 0x00, 0x01, 0x02, 0x03};
+
+    conn_handle = ble_att_svr_test_misc_init(0);
+
+    /* Put handle into buf */
+    (*(uint16_t *)&buf[1]) = htole16(conn_handle);
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc != 0);
+    ble_hs_test_util_verify_tx_err_rsp(0x3f, 0,
+                                       BLE_ATT_ERR_REQ_NOT_SUPPORTED);
+
+    /* Check for no response when unknown command is sent */
+    buf[0] = 0x4f;
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                    buf, sizeof buf);
+    TEST_ASSERT(rc != 0);
+
+    /* Ensure no response sent. */
+    TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
+}
+
 TEST_SUITE(ble_att_svr_suite)
 {
     /* When checking for mbuf leaks, ensure no stale prep entries. */
@@ -2077,6 +2103,7 @@ TEST_SUITE(ble_att_svr_suite)
     ble_att_svr_test_notify();
     ble_att_svr_test_indicate();
     ble_att_svr_test_oom();
+    ble_att_svr_test_unsupported_req();
 }
 
 int