Restore DCache clean/invalidate.

Signed-off-by: HiFiPhile <[email protected]>

2 files changed, 69 insertions, 12 deletions

diff --git a/src/portable/chipidea/ci_hs/ci_hs_imxrt.h b/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
index 201936e53..c53556f79 100644
--- a/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
+++ b/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
@@ -63,4 +63,56 @@ static const ci_hs_controller_t _ci_controller[] =
 #define CI_HCD_INT_ENABLE(_p)   NVIC_EnableIRQ ((IRQn_Type)_ci_controller[_p].irqnum)
 #define CI_HCD_INT_DISABLE(_p)  NVIC_DisableIRQ((IRQn_Type)_ci_controller[_p].irqnum)
 
+//------------- DCache -------------//
+#if CFG_TUD_MEM_DCACHE_ENABLE || CFG_TUH_MEM_DCACHE_ENABLE
+#if __CORTEX_M == 7
+TU_ATTR_ALWAYS_INLINE static inline uint32_t round_up_to_cache_line_size(uint32_t size) {
+  if (size & (CFG_TUD_MEM_DCACHE_LINE_SIZE-1)) {
+    size = (size & ~(CFG_TUD_MEM_DCACHE_LINE_SIZE-1)) + CFG_TUD_MEM_DCACHE_LINE_SIZE;
+  }
+  return size;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool imxrt_is_cache_mem(uintptr_t addr) {
+  return !(0x20000000 <= addr && addr < 0x20100000);
+}
+
+bool dcd_dcache_clean(void const* addr, uint32_t data_size) {
+  const uintptr_t addr32 = (uintptr_t) addr;
+  if (imxrt_is_cache_mem(addr32)) {
+    TU_ASSERT(tu_is_aligned32(addr32));
+    data_size = round_up_to_cache_line_size(data_size);
+    SCB_CleanDCache_by_Addr((uint32_t *) addr32, (int32_t) data_size);
+  }
+  return true;
+}
+
+bool dcd_dcache_invalidate(void const* addr, uint32_t data_size) {
+  const uintptr_t addr32 = (uintptr_t) addr;
+  if (imxrt_is_cache_mem(addr32)) {
+    // Invalidating does not push cached changes back to RAM so we need to be
+    // *very* careful when we do it. If we're not aligned, then we risk resetting
+    // values back to their RAM state.
+    TU_ASSERT(tu_is_aligned32(addr32));
+    data_size = round_up_to_cache_line_size(data_size);
+    SCB_InvalidateDCache_by_Addr((void*) addr32, (int32_t) data_size);
+  }
+  return true;
+}
+
+bool dcd_dcache_clean_invalidate(void const* addr, uint32_t data_size) {
+  const uintptr_t addr32 = (uintptr_t) addr;
+  if (imxrt_is_cache_mem(addr32)) {
+    TU_ASSERT(tu_is_aligned32(addr32));
+    data_size = round_up_to_cache_line_size(data_size);
+    SCB_CleanInvalidateDCache_by_Addr((uint32_t *) addr32, (int32_t) data_size);
+  }
+  return true;
+}
+
+#elif __CORTEX_M == 4
+#error "Secondary M4 core's cache controller is not supported yet."
+#endif
+#endif
+
 #endif
diff --git a/src/portable/chipidea/ci_hs/dcd_ci_hs.c b/src/portable/chipidea/ci_hs/dcd_ci_hs.c
index 6d6a74524..bacc1b99d 100644
--- a/src/portable/chipidea/ci_hs/dcd_ci_hs.c
+++ b/src/portable/chipidea/ci_hs/dcd_ci_hs.c
@@ -34,18 +34,6 @@
 #if CFG_TUSB_MCU == OPT_MCU_MIMXRT1XXX
   #include "ci_hs_imxrt.h"
 
-  bool dcd_dcache_clean(void const* addr, uint32_t data_size) {
-    return imxrt_dcache_clean(addr, data_size);
-  }
-
-  bool dcd_dcache_invalidate(void const* addr, uint32_t data_size) {
-    return imxrt_dcache_invalidate(addr, data_size);
-  }
-
-  bool dcd_dcache_clean_invalidate(void const* addr, uint32_t data_size) {
-    return imxrt_dcache_clean_invalidate(addr, data_size);
-  }
-
 #elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
   #include "ci_hs_lpc18_43.h"
 
@@ -216,6 +204,8 @@ static void bus_reset(uint8_t rhport)
   _dcd_data.qhd[0][0].qtd_overlay.next = _dcd_data.qhd[0][1].qtd_overlay.next = QTD_NEXT_INVALID;
 
   _dcd_data.qhd[0][0].int_on_setup = 1; // OUT only
+
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
 }
 
 bool dcd_init(uint8_t rhport, const tusb_rhport_init_t* rh_init) {
@@ -241,6 +231,8 @@ bool dcd_init(uint8_t rhport, const tusb_rhport_init_t* rh_init) {
   dcd_reg->PORTSC1 = PORTSC1_FORCE_FULL_SPEED;
 #endif
 
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
+
   dcd_reg->ENDPTLISTADDR = (uint32_t) _dcd_data.qhd; // Endpoint List Address has to be 2K alignment
   dcd_reg->USBSTS  = dcd_reg->USBSTS;
   dcd_reg->USBINTR = INTR_USB | INTR_ERROR | INTR_PORT_CHANGE | INTR_SUSPEND;
@@ -307,6 +299,8 @@ void dcd_sof_enable(uint8_t rhport, bool en)
 
 static void qtd_init(dcd_qtd_t* p_qtd, void * data_ptr, uint16_t total_bytes)
 {
+  dcd_dcache_clean_invalidate((uint32_t*) tu_align((uint32_t) data_ptr, 4), total_bytes);
+
   tu_memclr(p_qtd, sizeof(dcd_qtd_t));
 
   p_qtd->next            = QTD_NEXT_INVALID;
@@ -380,6 +374,8 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 
   p_qhd->qtd_overlay.next        = QTD_NEXT_INVALID;
 
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
+
   // Enable EP Control
   uint32_t const epctrl = (p_endpoint_desc->bmAttributes.xfer << ENDPTCTRL_TYPE_POS) | ENDPTCTRL_ENABLE | ENDPTCTRL_TOGGLE_RESET;
 
@@ -437,6 +433,9 @@ static void qhd_start_xfer(uint8_t rhport, uint8_t epnum, uint8_t dir)
   p_qhd->qtd_overlay.halted = false;            // clear any previous error
   p_qhd->qtd_overlay.next   = (uint32_t) p_qtd; // link qtd to qhd
 
+  // flush cache
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
+
   if ( epnum == 0 )
   {
     // follows UM 24.10.8.1.1 Setup packet handling using setup lockout mechanism
@@ -466,7 +465,9 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t t
   return true;
 }
 
+#if !CFG_TUD_MEM_DCACHE_ENABLE
 // fifo has to be aligned to 4k boundary
+// It's imcompatible with dcache enabled transfer, since neither address nor size is aligned to cache line
 bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes)
 {
   uint8_t const epnum = tu_edpt_number(ep_addr);
@@ -526,6 +527,7 @@ bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
 
   return true;
 }
+#endif
 
 //--------------------------------------------------------------------+
 // ISR
@@ -623,6 +625,9 @@ void dcd_int_handler(uint8_t rhport)
 
   if (int_status & INTR_USB)
   {
+    // Make sure we read the latest version of _dcd_data.
+    dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
+
     uint32_t const edpt_complete = dcd_reg->ENDPTCOMPLETE;
     dcd_reg->ENDPTCOMPLETE = edpt_complete; // acknowledge