[BT] Add DBG for latency tests

darthcloud · Dec 26, 2020 · 247dd58 · 247dd58
1 parent 88ebc79
commit 247dd58
Show file tree

Hide file tree

Showing 6 changed files with 217 additions and 0 deletions.
diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
@@ -1,4 +1,5 @@
 idf_component_register(SRCS "main.c"
+                            "stats.c"
                             "adapter/adapter.c"
                             "adapter/config.c"
                             "adapter/hid_parser.c"
@@ -21,6 +22,7 @@ idf_component_register(SRCS "main.c"
                             "adapter/parallel_2p.c"
                             "adapter/parallel_auto.c"
                             "bluetooth/host.c"
+                            "bluetooth/debug.c"
                             "bluetooth/hci.c"
                             "bluetooth/l2cap.c"
                             "bluetooth/sdp.c"

diff --git a/main/bluetooth/debug.c b/main/bluetooth/debug.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2019-2020, Jacques Gagnon
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <xtensa/hal.h>
+#include <driver/gpio.h>
+#include "sdkconfig.h"
+#include "../stats.h"
+
+static uint8_t type;
+static uint32_t counter;
+static uint32_t start, end;
+
+//#define REPORT_INTEVAL_STATS
+//#define MIN_LATENCY_TEST
+void bt_dbg_init(uint8_t dev_type) {
+    end = 0;
+    start = xthal_get_ccount();
+    counter = 0;
+    type = dev_type;
+}
+
+void bt_dbg(uint8_t *data, uint16_t len) {
+#ifdef REPORT_INTEVAL_STATS
+        float average, max, min, std_dev;
+        uint32_t interval;
+
+        end = xthal_get_ccount();
+        counter++;
+
+        if (end > start) {
+            interval = (end - start)/CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ;
+        }
+        else {
+            interval = ((0xFFFFFFFF - start) + end)/CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ;
+        }
+        start = end;
+
+        average = getAverage(interval)/1000;
+        max = getMax(interval)/1000;
+        min = getMin(interval)/1000;
+        std_dev = getStdDev(interval)/1000;
+        printf("\e[1;1H\e[2J");
+
+        printf("Samples: %d\n", counter);
+        printf("Average: %.6f ms\n", average);
+        printf("Max:     %.6f ms\n", max);
+        printf("Min:     %.6f ms\n", min);
+        printf("Std Dev: %.6f ms\n", std_dev);
+#endif
+#ifdef MIN_LATENCY_TEST
+//#define PS3
+//#define PS4
+//#define PS5
+//#define WIIU
+//#define XB1
+//#define SW
+#ifdef PS3
+        if ((*(uint32_t *)&data[11]) & 0x01FFFF00) {
+            GPIO.out = 0xFBFFFFFF;
+        }
+        else {
+            GPIO.out = 0xFFFFFFFF;
+        }
+#else
+#ifdef PS4
+        if ((*(uint32_t *)&data[6+11]) & 0x0003FFF0) {
+            GPIO.out = 0xFBFFFFFF;
+        }
+        else {
+            GPIO.out = 0xFFFFFFFF;
+        }
+#else
+#ifdef PS5
+        if ((*(uint32_t *)&data[8+11]) & 0x0003FFF0) {
+            GPIO.out = 0xFBFFFFFF;
+        }
+        else {
+            GPIO.out = 0xFFFFFFFF;
+        }
+#else
+#ifdef WIIU
+        if (~(*(uint32_t *)&data[13+11]) & 0x0003FFFE) {
+            GPIO.out = 0xFBFFFFFF;
+        }
+        else {
+            GPIO.out = 0xFFFFFFFF;
+        }
+#else
+#ifdef XB1
+        if ((*(uint32_t *)&data[13+11]) & 0x000003FF) {
+            GPIO.out = 0xFBFFFFFF;
+        }
+        else {
+            GPIO.out = 0xFFFFFFFF;
+        }
+#else
+#ifdef SW
+        if ((*(uint16_t *)&data[+11])) {
+            GPIO.out = 0xFBFFFFFF;
+        }
+        else {
+            GPIO.out = 0xFFFFFFFF;
+        }
+#endif
+#endif
+#endif
+#endif
+#endif
+#endif
+#endif
+}
+
diff --git a/main/bluetooth/debug.h b/main/bluetooth/debug.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2019-2020, Jacques Gagnon
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef _BT_DBG_H_
+#define _BT_DBG_H_
+
+void bt_dbg_init(uint8_t dev_type);
+void bt_dbg(uint8_t *data, uint16_t len);
+
+#endif /* _BT_DBG_H_ */
diff --git a/main/bluetooth/host.c b/main/bluetooth/host.c
@@ -18,8 +18,10 @@
 #include "sdp.h"
 #include "att.h"
 #include "../util.h"
+#include "debug.h"
 
 //#define H4_TRACE /* Display packet dump that can be parsed by wireshark/text2pcap */
+//#define BT_DBG /* Run bt_host_dbg function after HID channels are setup */
 
 #define BT_TX 0
 #define BT_RX 1
@@ -32,6 +34,7 @@ enum {
     /* BT CTRL flags */
     BT_CTRL_READY,
     BT_HOST_DISCONN_SW_INHIBIT,
+    BT_HOST_DBG_MODE,
 };
 
 struct bt_host_link_keys {
@@ -316,6 +319,12 @@ static int bt_host_rx_pkt(uint8_t *data, uint16_t len) {
     bt_h4_trace(data, len, BT_RX);
 #endif /* H4_TRACE */
 
+#ifdef BT_DBG
+    if (atomic_test_bit(&bt_flags, BT_HOST_DBG_MODE)) {
+        bt_dbg(data, len);
+    }
+    else {
+#endif
     switch(bt_hci_pkt->h4_hdr.type) {
         case BT_HCI_H4_TYPE_ACL:
             bt_host_acl_hdlr(bt_hci_pkt, len);
@@ -327,6 +336,9 @@ static int bt_host_rx_pkt(uint8_t *data, uint16_t len) {
             printf("# %s unsupported packet type: 0x%02X\n", __FUNCTION__, bt_hci_pkt->h4_hdr.type);
             break;
     }
+#ifdef BT_DBG
+    }
+#endif
 
     return 0;
 }
@@ -406,6 +418,16 @@ int32_t bt_host_init(void) {
     io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
     gpio_config(&io_conf);
 
+#ifdef BT_DBG
+    io_conf.intr_type = GPIO_PIN_INTR_DISABLE;
+    io_conf.mode = GPIO_MODE_OUTPUT;
+    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
+    io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
+    io_conf.pin_bit_mask = 1ULL << 26;
+    gpio_config(&io_conf);
+    gpio_set_level(26, 1);
+#endif
+
     bt_host_load_bdaddr_from_file();
 
     esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
@@ -478,6 +500,10 @@ int32_t bt_host_store_link_key(struct bt_hci_evt_link_key_notify *link_key_notif
 }
 
 void bt_host_bridge(struct bt_dev *device, uint8_t report_id, uint8_t *data, uint32_t len) {
+#ifdef BT_DBG
+    atomic_set_bit(&bt_flags, BT_HOST_DBG_MODE);
+    bt_dbg_init(device->type);
+#else
     if (device->type == HID_GENERIC) {
         uint32_t i = 0;
         for (; i < REPORT_MAX; i++) {
@@ -499,4 +525,5 @@ void bt_host_bridge(struct bt_dev *device, uint8_t report_id, uint8_t *data, uin
         adapter_bridge(&bt_adapter.data[device->id]);
     }
     bt_adapter.data[device->id].report_cnt++;
+#endif
 }
diff --git a/main/stats.c b/main/stats.c
@@ -0,0 +1,48 @@
+/* Functions to calculate Average, Min, Max, and Std Dev */
+/* Taken from Arduino lag test code by @jorge_: https://pastebin.com/ktqn7izx */
+
+#include <stdint.h>
+#include <math.h>
+
+float getAverage(uint32_t newNum){
+  static uint32_t numSamples = 1;
+  static float curAvg;
+
+  curAvg = curAvg + (newNum - curAvg) / numSamples;
+  numSamples++;
+
+  return curAvg;
+}
+
+float getMax(uint32_t newNum){
+  static uint32_t maxVal = 0;
+
+  if(newNum > maxVal){
+    maxVal = newNum;
+  }
+
+  return maxVal;
+}
+
+float getMin(uint32_t newNum){
+  static uint32_t minVal = 0xFFFFFFFF;
+
+  if(newNum < minVal){
+    minVal = newNum;
+  }
+
+  return minVal;
+}
+
+float getStdDev(uint32_t newNum){
+    static float M = 0.0;
+    static float S = 0.0;
+    static uint32_t i = 1;
+
+    float tmpM = M;
+    M += (newNum - tmpM) / i;
+    S += (newNum - tmpM) * (newNum - M);
+    i++;
+
+    return sqrt(S/(i-2));
+}
diff --git a/main/stats.h b/main/stats.h
@@ -0,0 +1,12 @@
+/* Functions to calculate Average, Min, Max, and Std Dev */
+/* Taken from Arduino lag test code by @jorge_: https://pastebin.com/ktqn7izx */
+
+#ifndef _STATS_H_
+#define _STATS_H_
+
+float getAverage(uint32_t newNum);
+float getMax(uint32_t newNum);
+float getMin(uint32_t newNum);
+float getStdDev(uint32_t newNum);
+
+#endif /* _STATS_H_ */