Fix merge errors

boards/nrf5340_audio_dk_nrf5340_cpuapp.overlay

@@ -6,7 +6,11 @@
 		    gpios = <&arduino_header 10 GPIO_ACTIVE_HIGH>;
             label = "Controller to host timesync pin";
         };
-    };
+        alternate_toggle: pin_1 {
+		    gpios = <&arduino_header 11 GPIO_ACTIVE_HIGH>;
+            label = "alternate toggle pin";
+        };
+   };
 };
 
 &uart0 {

prj.conf

@@ -41,3 +41,4 @@ CONFIG_BT_BUF_CMD_TX_COUNT=10
 # for the timesync command
 CONFIG_BT_HCI_RAW_CMD_EXT=y
 
+CONFIG_NRFX_TIMER2=y

src/main.c

@@ -31,13 +31,15 @@
 #include <zephyr/bluetooth/buf.h>
 #include <zephyr/bluetooth/hci_raw.h>
 
+#include <nrfx_timer.h>
 #include "audio_sync_timer.h"
 
 #define LOG_MODULE_NAME hci_uart
 LOG_MODULE_REGISTER(LOG_MODULE_NAME);
 
-static const struct device *const hci_uart_dev =
-	DEVICE_DT_GET(DT_CHOSEN(zephyr_bt_c2h_uart));
+static const struct device *const hci_uart_dev  = DEVICE_DT_GET(DT_CHOSEN(zephyr_bt_c2h_uart));
+static const struct device *const gmap_uart_dev = DEVICE_DT_GET(DT_NODELABEL(uart1));
+
 static K_THREAD_STACK_DEFINE(tx_thread_stack, CONFIG_BT_HCI_TX_STACK_SIZE);
 static struct k_thread tx_thread_data;
 static K_FIFO_DEFINE(tx_queue);
@@ -66,6 +68,30 @@ static K_FIFO_DEFINE(uart_tx_queue);
  */
 #define H4_DISCARD_LEN 33
 
+#ifdef CONFIG_AUDIO_SYNC_TIMER_USES_RTC
+#define TIMESYNC_GPIO  DT_NODELABEL(timesync)
+
+#if DT_NODE_HAS_STATUS(TIMESYNC_GPIO, okay)
+static const struct gpio_dt_spec timesync_pin = GPIO_DT_SPEC_GET(TIMESYNC_GPIO, gpios);
+#else
+#error "No timesync gpio available!"
+#endif
+
+#define ALTERNATE_TOGGLE_GPIO DT_NODELABEL(alternate_toggle)
+static const struct gpio_dt_spec alternate_toggle_pin = GPIO_DT_SPEC_GET(ALTERNATE_TOGGLE_GPIO, gpios);
+
+#endif
+
+// enable to have alternate toggle indicate time from audio in to sdu sync ref in UGT to UGG direction
+#define PERIPHERAL_TO_CENTRAL_AUDIO_IN_TO_SDU_SYNC_REF
+
+// Presentation time from Audio to SDU Sync Ref (UGT->UGG) resp. SDU Sync Ref to Audio Out (UGG->UGT)
+// HS timer on nRF5340 seems to be off: 60000 ticks = 59372 us
+#define PRESENTATION_TIME_US 60634
+
+static uint32_t last_sdu_sync_ref_us;
+static uint32_t sdu_interval_us;
+
 static int h4_read(const struct device *uart, uint8_t *buf, size_t len)
 {
 	int rx = uart_fifo_read(uart, buf, len);
@@ -258,6 +284,15 @@ static void tx_thread(void *p1, void *p2, void *p3)
 
 		/* Wait until a buffer is available */
 		buf = k_fifo_get(&tx_queue, K_FOREVER);
+
+#if DT_NODE_HAS_STATUS(TIMESYNC_GPIO, okay)
+		// toggle GPIO and send 'B' for each TX packet to be able to capture by Logic Analzyer
+		if (bt_buf_get_type(buf) == BT_BUF_ISO_OUT) {
+			gpio_pin_toggle_dt( &timesync_pin );
+			uart_poll_out(gmap_uart_dev, 'B');
+		}
+#endif
+
 		/* Pass buffer to the stack */
 		err = bt_send(buf);
         if (err!=BT_HCI_ERR_SUCCESS) {
@@ -361,13 +396,6 @@ static int hci_uart_init(void)
 SYS_INIT(hci_uart_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
 
 #ifdef CONFIG_AUDIO_SYNC_TIMER_USES_RTC
-#define TIMESYNC_GPIO  DT_NODELABEL(timesync)
-
-#if DT_NODE_HAS_STATUS(TIMESYNC_GPIO, okay)
-static const struct gpio_dt_spec timesync_pin = GPIO_DT_SPEC_GET(TIMESYNC_GPIO, gpios);
-#else
-#error "No timesync gpio available!"
-#endif
 
 #define  HCI_CMD_ISO_TIMESYNC	(0x200)
 
@@ -426,14 +454,135 @@ uint8_t hci_cmd_iso_timesync_cb(struct net_buf *buf)
 }
 #endif
 
+uint16_t little_endian_read_16(const uint8_t * buffer, int position){
+	return (uint16_t)(((uint16_t) buffer[position]) | (((uint16_t)buffer[position+1]) << 8));
+}
+
+static uint32_t little_endian_read_32(const uint8_t * buffer, int position){
+    return ((uint32_t) buffer[position]) | (((uint32_t)buffer[position+1]) << 8) | (((uint32_t)buffer[position+2]) << 16) | (((uint32_t) buffer[position+3]) << 24);
+}
+
+#if DT_NODE_HAS_STATUS(TIMESYNC_GPIO, okay)
+
+// make sure there's no IRQ between getting the time and the toggle
+// verify timestamp as it's 100% coorect
+#define LOCK_IRQS_FOR_SYNC_TOGGLE
+static uint32_t toggle_and_get_bluetooth_time_us(void) {
+	uint32_t timestamp_toggle_us;
+
+#ifdef LOCK_IRQS_FOR_SYNC_TOGGLE
+	// Lock interrupts
+	uint32_t key = arch_irq_lock();
+#endif
+
+	while (1){
+		// Get current time once
+		timestamp_toggle_us = audio_sync_timer_capture();
+
+		// Get current time again
+		uint32_t timestamp_toggle_us_verify = audio_sync_timer_capture();
+
+		// check if time didn't jump
+		int32_t timestamp_delta = (int32_t) (timestamp_toggle_us_verify - timestamp_toggle_us);
+		if ((timestamp_delta >= 0) && (timestamp_delta < 10)){
+			break;
+		}
+	}
+
+	// Toggle
+	gpio_pin_toggle_dt( &timesync_pin );
+
+#ifdef LOCK_IRQS_FOR_SYNC_TOGGLE
+	// Unlock interrupts
+	arch_irq_unlock(key);
+#endif
+
+	return timestamp_toggle_us;
+}
+
+#endif
+
+
+#define SYNC_TOGGLE_TIMER_INSTANCE_NUMBER 2
+static const nrfx_timer_t sync_toggle_timer_instance =
+	NRFX_TIMER_INSTANCE(SYNC_TOGGLE_TIMER_INSTANCE_NUMBER);
+
+
+static nrfx_timer_config_t cfg = {.frequency = NRFX_MHZ_TO_HZ(1UL),
+				  .mode = NRF_TIMER_MODE_TIMER,
+				  .bit_width = NRF_TIMER_BIT_WIDTH_32,
+				  .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY,
+				  .p_context = NULL};
+
+static volatile enum {
+	ALTERNATE_TOGGLE_STATE_IDLE,
+	ALTERNATE_TOGGLE_STATE_W2_RISE,
+	ALTERNATE_TOGGLE_STATE_W2_FALL
+} alternate_toggle_state = ALTERNATE_TOGGLE_STATE_IDLE;
+
+static volatile uint32_t alternate_toggle_fall_us;
+
+static uint32_t get_local_time_us(void) {
+	nrf_timer_cc_set(NRF_TIMER2, NRF_TIMER_CC_CHANNEL2, 0);
+	nrf_timer_task_trigger(NRF_TIMER2, nrf_timer_capture_task_get(NRF_TIMER_CC_CHANNEL2));
+	uint32_t current_time_us = nrf_timer_cc_get(NRF_TIMER2, NRF_TIMER_CC_CHANNEL2);
+	while (current_time_us == 0) {
+		current_time_us = nrf_timer_cc_get(NRF_TIMER2, NRF_TIMER_CC_CHANNEL2);
+	}
+	return current_time_us;
+}
+
+static void alternate_toggle_timer_isr_handler(nrf_timer_event_t event_type, void *p_context){
+	ARG_UNUSED(p_context);
+	if(event_type == NRF_TIMER_EVENT_COMPARE1){
+		switch (alternate_toggle_state) {
+			case ALTERNATE_TOGGLE_STATE_W2_RISE:
+				alternate_toggle_state = ALTERNATE_TOGGLE_STATE_W2_FALL;
+				gpio_pin_set_dt(&alternate_toggle_pin, 1);
+				nrfx_timer_compare(&sync_toggle_timer_instance, NRF_TIMER_CC_CHANNEL1, alternate_toggle_fall_us, true);
+				break;
+			case ALTERNATE_TOGGLE_STATE_W2_FALL:
+				alternate_toggle_state = ALTERNATE_TOGGLE_STATE_IDLE;
+				gpio_pin_set_dt(&alternate_toggle_pin, 0);
+				break;
+			default:
+				__ASSERT(0, "Unknown state");
+				break;
+		}
+	}
+}
+
+static void alternate_toggle_setup(uint32_t rise_us, uint32_t fall_us) {
+	alternate_toggle_fall_us = fall_us;
+	alternate_toggle_state = ALTERNATE_TOGGLE_STATE_W2_RISE;
+	nrfx_timer_compare(&sync_toggle_timer_instance, NRF_TIMER_CC_CHANNEL1, rise_us, true);
+}
+
 int main(void)
 {
+	// toggle timer setup
+	nrfx_err_t ret;
+	ret = nrfx_timer_init(&sync_toggle_timer_instance, &cfg, alternate_toggle_timer_isr_handler);
+	if (ret - NRFX_ERROR_BASE_NUM) {
+		LOG_ERR("nrfx timer init error: %d", ret);
+		return -ENODEV;
+	}
+	IRQ_CONNECT(NRFX_IRQ_NUMBER_GET(NRF_TIMER_INST_GET(SYNC_TOGGLE_TIMER_INSTANCE_NUMBER)), IRQ_PRIO_LOWEST,
+			NRFX_TIMER_INST_HANDLER_GET(SYNC_TOGGLE_TIMER_INSTANCE_NUMBER), 0, 0);
+	nrfx_timer_enable(&sync_toggle_timer_instance);
+	alternate_toggle_state = ALTERNATE_TOGGLE_STATE_IDLE;
+
 	/* incoming events and data from the controller */
 	static K_FIFO_DEFINE(rx_queue);
 	int err;
 
 	LOG_DBG("Start");
-	__ASSERT(hci_uart_dev, "UART device is NULL");
+	__ASSERT(hci_uart_dev, "HCI UART device is NULL");
+
+	if (!device_is_ready(gmap_uart_dev)) {
+		printk("GMAP UART device is not ready\n");
+		return -1;
+	}
 
 	/* Enable the raw interface, this will in turn open the HCI driver */
 	bt_enable_raw(&rx_queue);
@@ -476,6 +625,10 @@ int main(void)
 	gpio_pin_configure_dt(&timesync_pin, GPIO_OUTPUT_INACTIVE);
 #endif
 
+#if DT_NODE_HAS_STATUS(TIMESYNC_GPIO, okay)
+	gpio_pin_configure_dt(&alternate_toggle_pin, GPIO_OUTPUT_INACTIVE);
+#endif
+
 	bt_hci_raw_cmd_ext_register(&cmd_list, 1);
 #endif
 
@@ -487,20 +640,125 @@ int main(void)
 			NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);
 	k_thread_name_set(&tx_thread_data, "HCI uart TX");
 
-#if 0
-    while (1) {
-		uint8_t c = 'A';
-		uart_fifo_fill(hci_uart_dev, &c, 1);
-		uart_irq_tx_enable(hci_uart_dev);
-		k_sleep( K_MSEC(100) );
-	}
-    return 0;
-#endif
-
-    while (1) {
+	while (1) {
 		struct net_buf *buf;
 
-		buf = k_fifo_get(&rx_queue, K_FOREVER);
+		buf = net_buf_get(&rx_queue, K_FOREVER);
+
+#if DT_NODE_HAS_STATUS(TIMESYNC_GPIO, okay)
+		// ISO RX Measurement Code
+		const uint8_t * packet = buf->data;
+		if (packet[0] == H4_ISO){
+
+			// TODO: check if packets contains timestamp. all Controller -> Host packets have it so far
+
+			// get rx timestamp = sdu sync reference: Packet Type (1) | ISO Header (4) | Timestamp (if TS flag is set)
+			const uint32_t timestamp_sdu_sync_reference_us = little_endian_read_32(packet, 5);
+
+			// ignore empty ISO packets
+			const uint16_t data_total_length = little_endian_read_16(packet, 3);
+			if (data_total_length > 8) {
+
+				// get current Bluetooth time + toggle GPIO
+				const uint32_t bluetooth_time_us = toggle_and_get_bluetooth_time_us();
+
+				// calculate time of toggle relative to sdu sync reference usually
+				// - negative for UGT->UGG and
+				// - positive in UGT->UGG direction)
+				const int32_t delta_us = (int32_t)(bluetooth_time_us - timestamp_sdu_sync_reference_us);
+
+				// convert to string and send over UART and RTT
+				char delta_string[15];
+				const uint8_t first_payload_byte = packet[13];
+				snprintf(delta_string, sizeof(delta_string), "R%+06d@%02X!", delta_us,first_payload_byte);
+				for (size_t i = 0; delta_string[i] != '\0'; i++) {
+					uart_poll_out(gmap_uart_dev, delta_string[i]);
+				}
+
+				LOG_INF("Toggle %8u - SDU Sync Reference %8u -> delta %s", bluetooth_time_us, timestamp_sdu_sync_reference_us, delta_string);
+			}
+		}
+
+    	if (packet[0] == H4_EVT) {
+    		if (packet[1] == 0x0e) {
+    			uint16_t opcode = little_endian_read_16(packet, 4);
+    			const uint16_t hci_opcode_reset = 0x0c03;;
+    			if (opcode == hci_opcode_reset) {
+    				LOG_INF("HCI Reset");
+    				last_sdu_sync_ref_us = 0;
+    				sdu_interval_us = 0;
+    			}
+    			const uint16_t hci_opcode_le_read_tx_iso_sync = 0x2061;
+    			if (opcode == hci_opcode_le_read_tx_iso_sync) {
+    				const uint8_t * return_params = &packet[6];
+
+    				// get current Bluetooth time + toggle GPIO
+    				const uint32_t bluetooth_time_us = toggle_and_get_bluetooth_time_us();
+
+    				// get current local time
+    				const uint32_t local_time_us = get_local_time_us();
+
+    				// status: 0
+    				uint16_t handle = little_endian_read_16(return_params, 1);
+    				ARG_UNUSED(handle);
+
+    				// get packet sequence number (assuming counter == packet_sequence_number & 0xff)
+    				const uint16_t packet_sequence_number = little_endian_read_16(return_params, 3);
+
+    				// get tx timestamp = sdu sync reference: Packet Type (1) | ISO Header (4) | Timestamp (if TS flag is set)
+    				const uint32_t iso_tx_us = little_endian_read_32(return_params, 5);
+
+    				// get SDU interval based on sdu sync ref interval (round to the next 100 us)
+    				if (last_sdu_sync_ref_us > 0) {
+    					const uint32_t sdu_interval_reported_us = iso_tx_us - last_sdu_sync_ref_us;
+    					sdu_interval_us = ((sdu_interval_reported_us + 50) / 100 ) * 100;
+    				}
+    				last_sdu_sync_ref_us = iso_tx_us;
+
+    				// calculate time of toggle relative to sdu sync reference (usually negative as the packet is received before it should be played)
+    				const int32_t delta_us = (int32_t)(bluetooth_time_us - iso_tx_us);
+
+    				// convert to string and send over UART and RTT
+    				char delta_string[15];
+    				snprintf(delta_string, sizeof(delta_string), "T%+06d@%02X!", delta_us,packet_sequence_number & 0xff);
+    				for (size_t i = 0; delta_string[i] != '\0'; i++) {
+    					uart_poll_out(gmap_uart_dev, delta_string[i]);
+    				}
+    				// LOG_INF("Toggle %8u - TX  %8u - %02X-> delta %s", bluetooth_time_us, iso_tx_us, packet_sequence_number, delta_string);
+
+#ifdef PERIPHERAL_TO_CENTRAL_AUDIO_IN_TO_SDU_SYNC_REF
+    				// schedule Audio In to Sync Ref for Peripheral to Central direction
+    				if (sdu_interval_us > 0) {
+    					if (alternate_toggle_state == ALTERNATE_TOGGLE_STATE_IDLE) {
+
+    						// note: for Peripheral to Central, the SDU Sync Ref matches the ISO TX Timestamp
+
+    						// calculate next audio in time such that:
+    						// - it's in the future
+    						// - audio_in_us + presentatioon time = sdu_sync_ref
+    						uint32_t audio_in_us = iso_tx_us - PRESENTATION_TIME_US;
+    						while (audio_in_us < (bluetooth_time_us + 10000)) {
+    							audio_in_us += sdu_interval_us;
+    						}
+
+    						// convert into local time reference
+    						const uint32_t rise_us = local_time_us + (audio_in_us - bluetooth_time_us);
+    						const uint32_t fall_us = rise_us + PRESENTATION_TIME_US;
+
+    						//	LOG_INF("bt %u, tx %u, audio in %u // sdu %u // now %u, rise %u",
+							//		bluetooth_time_us, iso_tx_us, audio_in_us,
+							//		sdu_interval_us,
+							//		local_time_us, rise_us);
+
+    						alternate_toggle_setup(rise_us, fall_us);
+    					}
+    				}
+#endif
+    			}
+    		}
+     	}
+#endif
+
 		err = h4_send(buf);
 		if (err) {
 			LOG_ERR("Failed to send");