From d2164713b390a5e4a1ed9183372cc7556a27fccd Mon Sep 17 00:00:00 2001
From: xj <kameblue@foxmail.com>
Date: Thu, 4 Jul 2024 05:25:53 -0700
Subject: [PATCH] Add fls function

---
 .../drivers/usb/components/port/xhci/xhci.c   |  9 ++--
 .../drivers/usb/components/port/xhci/xhci.h   | 45 +++++++++++++++++++
 2 files changed, 49 insertions(+), 5 deletions(-)

diff --git a/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.c b/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.c
index 75354f2db..da275556a 100644
--- a/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.c
+++ b/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.c
@@ -128,9 +128,8 @@ static inline size_t xhci_align ( size_t len ) {
 	size_t align;
 
 	/* Align to own length (rounded up to a power of two) */
-//	align = ( 1 << fls ( len - 1 ) );
-//	align = ( 1 << fls ( len - 1 ) );
-	align = XHCI_MIN_ALIGN;
+	align = ( 1 << fls ( len - 1 ) );
+//	align = XHCI_MIN_ALIGN;
 
 	/* Round up to XHCI_MIN_ALIGN if needed */
 	if ( align < XHCI_MIN_ALIGN )
@@ -2228,8 +2227,8 @@ int xhci_work_endpoint_open ( struct xhci_host *xhci, struct xhci_slot *slot, st
 	
 	/* Calculate interval */
 	if ( ctx_type & XHCI_EP_TYPE_PERIODIC ) {
-//		ep->interval = ( fls ( ep->interval ) - 1 );
-		ep->interval = 256;
+		ep->interval = ( fls ( ep->interval ) - 1 );
+//		ep->interval = 256;
 	}
 
 	ep->ctx_type = ctx_type;
diff --git a/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.h b/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.h
index d653a404d..1d8403a4e 100644
--- a/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.h
+++ b/Ubiquitous/XiZi_AIoT/services/drivers/usb/components/port/xhci/xhci.h
@@ -992,4 +992,49 @@ void xhci_dump_trbs(const union xhci_trb *trbs, unsigned int count);
 /* Dump slot context */
 void xhci_dump_slot_ctx(const struct xhci_slot_context *const sc);
 
+
+/* 
+ * Find the last (most-significant) bit set
+ * @x: the 64 bit length variate to search.
+ */
+static __always_inline int fls(uint64_t x)
+{
+    int r = 64;
+
+    if(!x)
+        return 0;
+
+    if (!(x & 0xffffffff00000000u)) {
+        x <<= 32;
+        r -= 32;
+    }
+
+    if (!(x & 0xffff000000000000u)) {
+        x <<= 16;
+        r -= 16;
+    }
+
+	if (!(x & 0xff00000000000000u)) {
+        x <<= 8;
+        r -= 8;
+    }
+
+	if (!(x & 0xf000000000000000u)) {
+        x <<= 4;
+        r -= 4;
+    }
+
+	if (!(x & 0xc000000000000000u)) {
+        x <<= 2;
+        r -= 2;
+    }
+
+	if (!(x & 0x8000000000000000u)) {
+        x <<= 1;
+        r -= 1;
+    }
+
+    return r;
+}
+
 #endif