En roles

ports/psoc6/modbluetooth.c

@@ -1,3 +1,4 @@
+
 // All includes may or may not be needed
 #include "py/binary.h"
 #include "py/gc.h"
@@ -24,6 +25,7 @@
 #include "wiced_memory.h"
 #include "cyhal.h"
 #include "stdio.h"
+#include "stdlib.h"
 #include "cycfg_gatt_db.h"
 #include "cycfg_gap.h"
 #include "wiced_bt_dev.h"
@@ -35,16 +37,28 @@
 #include "wiced_bt_dev.h"
 #include "wiced_bt_gatt.h"
 
+#include "cycfg_bt_settings.h"
+#include "cybsp_bt_config.h"
+#include "wiced_bt_stack.h"
+
+
+
 // ******************************************************************************
 #if MICROPY_PY_BLUETOOTH
 #define NUM_ADV_PACKETS                 (3u)
 #define ERRNO_BLUETOOTH_NOT_ACTIVE      MP_ENODEV
 #define CASE_RETURN_STR(const)          case const: \
         return #const;
 
+
+// Convert microseconds to BLE slots (1 slot = 0.625 ms)
+#define US_TO_SLOTS(us) ((uint16_t)((us) / 625))
+
 // Bring externs from MTB GeneratedSource/
 extern uint8_t app_gap_device_name[];
 
+wiced_bt_ble_advert_elem_t cy_bt_adv_pkt[8];    // Max 8 elements possible
+
 // MPY defines and variables
 volatile int mp_bluetooth_ble_stack_state = MP_BLUETOOTH_BLE_STATE_OFF;
 static uint8_t address_mode = BLE_ADDR_PUBLIC;
@@ -109,10 +123,140 @@ const char *get_bt_gatt_status_name(wiced_bt_gatt_status_t status) {
     return "UNKNOWN_STATUS";
 }
 
+void free_advert_elements(wiced_bt_ble_advert_elem_t *adv_elements, uint8_t num_elements) {
+    if (adv_elements) {
+        for (uint8_t i = 0; i < num_elements; i++) {
+            if (adv_elements[i].p_data) {
+                free(adv_elements[i].p_data);
+            }
+        }
+        free(adv_elements);
+    }
+}
+
+wiced_result_t parse_ltv_to_advert_elements(const uint8_t *adv_data, uint16_t adv_len,
+    wiced_bt_ble_advert_elem_t **adv_elements,
+    uint8_t *num_elements) {
+
+    if (!adv_data || adv_len == 0 || !adv_elements || !num_elements) {
+        return WICED_BT_ERROR;
+    }
+
+    // Count the number of elements first
+    uint8_t element_count = 0;
+    uint16_t offset = 0;
+    while (offset < adv_len) {
+        if (offset + 1 > adv_len) {
+            break;                       // Insufficient data
+
+        }
+        uint8_t field_len = adv_data[offset];
+        if (field_len == 0 || offset + field_len + 1 > adv_len) {
+            break;
+        }
+
+        element_count++;
+        offset += field_len + 1;
+    }
+
+    if (element_count == 0) {
+        return WICED_BT_ERROR;
+    }
+
+    // Allocate memory for elements
+    *adv_elements = (wiced_bt_ble_advert_elem_t *)malloc(element_count * sizeof(wiced_bt_ble_advert_elem_t));
+    if (!*adv_elements) {
+        return WICED_BT_NO_RESOURCES;
+    }
+
+    // Parse and populate elements
+    offset = 0;
+    *num_elements = 0;
+
+    while (offset < adv_len && *num_elements < element_count) {
+        uint8_t field_len = adv_data[offset];
+        uint8_t field_type = adv_data[offset + 1];
+
+        if (field_len == 0 || offset + field_len + 1 > adv_len) {
+            break;
+        }
+
+        // Allocate memory for this element's data
+        uint8_t *element_data = (uint8_t *)malloc(field_len);
+        if (!element_data) {
+            // Cleanup on failure
+            for (int i = 0; i < *num_elements; i++) {
+                free((*adv_elements)[i].p_data);
+            }
+            free(*adv_elements);
+            *adv_elements = NULL;
+            return WICED_BT_NO_RESOURCES;
+        }
+
+        // Copy the data (excluding the length byte)
+        memcpy(element_data, &adv_data[offset + 1], field_len);
+
+        // Map Bluetooth SIG AD types to WICED BLE advert types
+        wiced_bt_ble_advert_elem_t *element = &(*adv_elements)[*num_elements];
+        element->len = field_len;
+        element->p_data = element_data;
+
+        // Map common AD types
+        switch (field_type) {
+            case 0x01: // Flags
+                element->advert_type = BTM_BLE_ADVERT_TYPE_FLAG;
+                break;
+            case 0x02: // Incomplete List of 16-bit Service Class UUIDs
+                element->advert_type = BTM_BLE_ADVERT_TYPE_16SRV_PARTIAL;
+                break;
+            case 0x03: // Complete List of 16-bit Service Class UUIDs
+                element->advert_type = BTM_BLE_ADVERT_TYPE_16SRV_COMPLETE;
+                break;
+            case 0x06: // Incomplete List of 128-bit Service Class UUIDs
+                element->advert_type = BTM_BLE_ADVERT_TYPE_128SRV_PARTIAL;
+                break;
+            case 0x07: // Complete List of 128-bit Service Class UUIDs
+                element->advert_type = BTM_BLE_ADVERT_TYPE_128SRV_COMPLETE;
+                break;
+            case 0x08: // Shortened Local Name
+                element->advert_type = BTM_BLE_ADVERT_TYPE_NAME_SHORT;
+                break;
+            case 0x09: // Complete Local Name
+                element->advert_type = BTM_BLE_ADVERT_TYPE_NAME_COMPLETE;
+                break;
+            case 0x0A: // Tx Power Level
+                element->advert_type = BTM_BLE_ADVERT_TYPE_TX_POWER;
+                break;
+            case 0x16: // Service Data - 16-bit UUID
+                element->advert_type = BTM_BLE_ADVERT_TYPE_SERVICE_DATA;
+                break;
+            case 0xFF: // Manufacturer Specific Data
+                element->advert_type = BTM_BLE_ADVERT_TYPE_MANUFACTURER;
+                break;
+            default:
+                // For unknown types, use raw data type
+                element->advert_type = BTM_BLE_ADVERT_TYPE_MANUFACTURER;
+                break;
+        }
+
+        (*num_elements)++;
+        offset += field_len + 1;
+    }
+
+    return WICED_SUCCESS;
+}
 
 // *********************************** MTB wrapper API's and callbacks *******************************************
 
 // ToDo : Add better function names to clearly differentiate API's purely using MTB vs MPY vs MPY+MTB
+
+static void stop_advertisement(void) {
+    wiced_result_t status = WICED_BT_SUCCESS;
+    status = wiced_bt_start_advertisements(BTM_BLE_ADVERT_OFF, 0, NULL);
+    bluetooth_assert_raise_val("Stopping Bluetooth LE advertisements failed with error: ", status, WICED_SUCCESS);
+}
+
+
 static void start_advertisement(void) {
     wiced_result_t wiced_status;
 
@@ -174,9 +318,6 @@ static void ble_init() {
     if (WICED_BT_GATT_SUCCESS != gatt_status) {
         mp_printf(&mp_plat_print, "\n GATT DB Initialization failed with err 0x%x\n", gatt_status);
     }
-
-    /* Start Undirected LE Advertisements on device startup. */
-    start_advertisement();
 }
 
 // Receive management events from the LE stack
@@ -250,14 +391,12 @@ int mp_bluetooth_init(void) {
 
     /* Register call back and configuration with stack */
     wiced_result = wiced_bt_stack_init(bt_management_callback, &wiced_bt_cfg_settings);
-    if (WICED_BT_SUCCESS == wiced_result) {
-        mp_printf(&mp_plat_print, "Bluetooth Stack Initialization Successful \n");
-        mp_bluetooth_ble_stack_state = MP_BLUETOOTH_BLE_STATE_ACTIVE;
-    } else {
-        mp_printf(&mp_plat_print, "Bluetooth Stack Initialization failed \n");
+    bluetooth_assert_raise_val("Bluetooth Stack Initialization failed with error: ", wiced_result, WICED_BT_SUCCESS);
+    if (WICED_BT_SUCCESS != wiced_result) {
         mp_bluetooth_ble_stack_state = MP_BLUETOOTH_BLE_STATE_STOPPING;
+        return 0;
     }
-
+    mp_bluetooth_ble_stack_state = MP_BLUETOOTH_BLE_STATE_ACTIVE;
     return 0;
 }
 
@@ -276,10 +415,6 @@ bool mp_bluetooth_is_active(void) {
     return mp_bluetooth_ble_stack_state == MP_BLUETOOTH_BLE_STATE_ACTIVE;
 }
 
-void print_bd_address(wiced_bt_device_address_t bdadr) {
-    printf("%02X:%02X:%02X:%02X:%02X:%02X \n", bdadr[0], bdadr[1], bdadr[2], bdadr[3], bdadr[4], bdadr[5]);
-}
-
 // Gets the current address of this device in big-endian format.
 void mp_bluetooth_get_current_address(uint8_t *addr_type, uint8_t *addr) {
     // wiced_bt_device_address_t bda = { 0 };
@@ -357,17 +492,128 @@ int mp_bluetooth_gap_set_device_name(const uint8_t *buf, size_t len) {
     return MP_EOPNOTSUPP;
 }
 
-// Start advertisement. Will re-start advertisement when already enabled.
-// Returns errno on failure.
-int mp_bluetooth_gap_advertise_start(bool connectable, int32_t interval_us, const uint8_t *adv_data, size_t adv_data_len, const uint8_t *sr_data, size_t sr_data_len) {
-    return 0;
+int mp_bluetooth_gap_advertise_start(bool connectable, int32_t interval_us,
+    const uint8_t *adv_data, size_t adv_data_len,
+    const uint8_t *sr_data, size_t sr_data_len) {
+
+    if (!mp_bluetooth_is_active()) {
+        return ERRNO_BLUETOOTH_NOT_ACTIVE;
+    }
+
+    wiced_result_t result = WICED_BT_SUCCESS;
+    // Static storage for last used adv/scanned data
+    static wiced_bt_ble_advert_elem_t *last_adv_elements = NULL;
+    static uint8_t last_num_adv_elements = 0;
+    static wiced_bt_ble_advert_elem_t *last_sr_elements = NULL;
+    static uint8_t last_num_sr_elements = 0;
+
+    // Stop advertising if interval_us is 0 or None
+    if (interval_us == 0) {
+        stop_advertisement();
+        return 0;
+    } else {
+        mp_printf(&mp_plat_print, "Advertising interval is ignored and set to default interval internally - 0x0020 to 0x4000\r\n");
+    }
+
+    // Handle adv_data
+    if (adv_data == NULL) {
+        // Reuse previous
+        adv_data = NULL;
+        adv_data_len = 0;
+    }
+    if (adv_data_len == 0 && last_adv_elements) {
+        // Clear previous
+        free_advert_elements(last_adv_elements, last_num_adv_elements);
+        last_adv_elements = NULL;
+        last_num_adv_elements = 0;
+    }
+    if (adv_data && adv_data_len > 0) {
+        if (last_adv_elements) {
+            free_advert_elements(last_adv_elements, last_num_adv_elements);
+        }
+        result = parse_ltv_to_advert_elements(adv_data, adv_data_len, &last_adv_elements, &last_num_adv_elements);
+        if (result != WICED_SUCCESS) {
+            return result;
+        }
+    }
+
+    // Handle scan response data
+    if (sr_data == NULL) {
+        sr_data = NULL;
+        sr_data_len = 0;
+    }
+    if (sr_data_len == 0 && last_sr_elements) {
+        free_advert_elements(last_sr_elements, last_num_sr_elements);
+        last_sr_elements = NULL;
+        last_num_sr_elements = 0;
+    }
+    if (sr_data && sr_data_len > 0) {
+        if (last_sr_elements) {
+            free_advert_elements(last_sr_elements, last_num_sr_elements);
+        }
+        result = parse_ltv_to_advert_elements(sr_data, sr_data_len, &last_sr_elements, &last_num_sr_elements);
+        if (result != WICED_SUCCESS) {
+            return result;
+        }
+    }
+
+    // Set advertisement data
+    result = wiced_bt_ble_set_raw_advertisement_data(last_num_adv_elements, last_adv_elements);
+    if (result != WICED_SUCCESS) {
+        return result;
+    }
+
+    if (sr_data && sr_data_len > 0) {
+        result = wiced_bt_ble_set_raw_scan_response_data(last_num_sr_elements, last_sr_elements);
+        if (result != WICED_SUCCESS) {
+            return result;
+        }
+    }
+
+    // Start advertising
+    result = wiced_bt_start_advertisements(BTM_BLE_ADVERT_UNDIRECTED_HIGH, BLE_ADDR_PUBLIC, NULL);
+
+    return result;
 }
 
+
 // Stop advertisement. No-op when already stopped.
 void mp_bluetooth_gap_advertise_stop(void) {
+    wiced_result_t wiced_status;
+    if (!mp_bluetooth_is_active()) {
+        return;
+    }
+    wiced_status = wiced_bt_start_advertisements(BTM_BLE_ADVERT_OFF, 0, NULL);
+    bluetooth_assert_raise_val("Stopping Bluetooth LE advertisements failed with error: ", wiced_status, WICED_SUCCESS);
     return;
 }
 
+#if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
+
+int mp_bluetooth_gap_scan_start(int32_t duration_ms, int32_t interval_us, int32_t window_us, bool active_scan) {
+    return 0;
+}
+
+// Stop discovery
+int mp_bluetooth_gap_scan_stop(void) {
+    return 0;
+}
+
+// Connect to a found peripheral.
+int mp_bluetooth_gap_peripheral_connect(uint8_t addr_type, const uint8_t *addr, int32_t duration_ms, int32_t min_conn_interval_us, int32_t max_conn_interval_us) {
+    // ret_status = wiced_bt_gatt_le_connect( p_scan_result->remote_bd_addr, p_scan_result->ble_addr_type, BLE_CONN_MODE_LOW_DUTY, TRUE );
+    // printf( "wiced_bt_gatt_connect status %d\r\n", ret_status );
+    return 0;
+}
+
+// Cancel in-progress connection to a peripheral.
+int mp_bluetooth_gap_peripheral_connect_cancel(void) {
+    return 0;
+}
+
+#endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
+
+
 // Start adding services. Must be called before mp_bluetooth_register_service.
 int mp_bluetooth_gatts_register_service_begin(bool append) {
     return 0;
@@ -401,6 +647,35 @@ int mp_bluetooth_gatts_set_buffer(uint16_t value_handle, size_t len, bool append
     return 0;
 }
 
+#if MICROPY_PY_BLUETOOTH_ENABLE_GATT_CLIENT
+
+// Find all primary services on the connected peripheral.
+int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle, const mp_obj_bluetooth_uuid_t *uuid) {
+    return 0;
+}
+// Find all characteristics on the specified service on a connected peripheral.
+int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle, const mp_obj_bluetooth_uuid_t *uuid) {
+    return 0;
+}
+// Find all descriptors on the specified characteristic on a connected peripheral.
+int mp_bluetooth_gattc_discover_descriptors(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) {
+    return 0;
+}
+// Initiate read of a value from the remote peripheral.
+int mp_bluetooth_gattc_read(uint16_t conn_handle, uint16_t value_handle) {
+    return 0;
+}
+
+// Write the value to the remote peripheral.
+int mp_bluetooth_gattc_write(uint16_t conn_handle, uint16_t value_handle, const uint8_t *value, size_t value_len, unsigned int mode) {
+    return 0;
+}
+// Initiate MTU exchange for a specific connection using the preferred MTU.
+int mp_bluetooth_gattc_exchange_mtu(uint16_t conn_handle) {
+    return 0;
+}
+#endif // MICROPY_PY_BLUETOOTH_ENABLE_GATT_CLIENT
+
 // Disconnect from a central or peripheral.
 int mp_bluetooth_gap_disconnect(uint16_t conn_handle) {
     return 0;