kernel: usb: Fix command ring TRB handling

Signed-off-by: Ian Moffett <ian@osmora.org>

2 files changed, 20 insertions, 9 deletions

diff --git a/sys/dev/usb/xhci.c b/sys/dev/usb/xhci.c
index a433fb9..a6996bf 100644
--- a/sys/dev/usb/xhci.c
+++ b/sys/dev/usb/xhci.c
@@ -110,16 +110,19 @@ xhci_submit_cmd(struct xhci_hc *hc, struct xhci_trb trb)
     struct xhci_caps *caps = XHCI_CAPS(hc->base);
 
     /* Push the TRB to the command ring */
-    cmd_db = XHCI_CMD_DB(hc->base, caps->dboff);
-    hc->cmd_ring[hc->cmd_ptr++] = trb;
-    hc->cycle = ~hc->cycle;
-
-    /* Wrap if needed */
-    if (hc->cmd_ptr >= XHCI_CMDRING_LEN) {
-        hc->cmd_ptr = 0;
+    hc->cmd_ring[hc->cmd_ptr++] = trb.dword0;
+    hc->cmd_ring[hc->cmd_ptr++] = trb.dword1;
+    hc->cmd_ring[hc->cmd_ptr++] = trb.dword2;
+    hc->cmd_ring[hc->cmd_ptr++] = trb.dword3 | hc->cycle;
+    hc->cmd_count++;
+
+    if (hc->cmd_count >= XHCI_CMDRING_LEN) {
+        /* TODO: Add link TRB */
+        __assert(0 && "TODO");
     }
 
     /* Ring the command doorbell */
+    cmd_db = XHCI_CMD_DB(hc->base, caps->dboff);
     *cmd_db = 0;
     return 0;
 }
@@ -371,6 +374,7 @@ xhci_init_hc(struct xhci_hc *hc)
     /* Set cmdring state */
     hc->cycle = 1;
     hc->cmd_ptr = 0;
+    hc->cmd_count = 0;
 
     /* Allocate resources and tell the HC about them */
     opregs->dcbaa_ptr = xhci_alloc_dcbaa(hc);
diff --git a/sys/include/dev/usb/xhcivar.h b/sys/include/dev/usb/xhcivar.h
index 2667e2c..06ed3ee 100644
--- a/sys/include/dev/usb/xhcivar.h
+++ b/sys/include/dev/usb/xhcivar.h
@@ -57,6 +57,12 @@ struct xhci_nop_trb {
 struct xhci_trb {
     union {
         struct xhci_nop_trb nop;
+        struct {
+            uint32_t dword0;
+            uint32_t dword1;
+            uint32_t dword2;
+            uint32_t dword3;
+        };
     };
 };
 
@@ -92,10 +98,11 @@ struct xhci_hc {
     size_t protocnt;
     uintptr_t *dcbaap;
     uint8_t cycle : 1;
-    uint16_t cmd_ptr;   /* Command ring enqueue ptr */
+    uint16_t cmd_ptr;   /* Command ring index  */
+    uint16_t cmd_count; /* Command ring entry count */
+    uint32_t *cmd_ring;
     struct xhci_opregs *opregs;
     struct xhci_proto protos[XHCI_MAX_PROTOS];
-    struct xhci_trb *cmd_ring;
     struct xhci_evring_segment *evring_seg;
 };