diff --git a/arch/arm/src/imxrt/imxrt_edma.c b/arch/arm/src/imxrt/imxrt_edma.c index f80debfcdda..77367280825 100644 --- a/arch/arm/src/imxrt/imxrt_edma.c +++ b/arch/arm/src/imxrt/imxrt_edma.c @@ -470,6 +470,8 @@ static void imxrt_dmaterminate(struct imxrt_dmach_s *dmach, int result) uintptr_t regaddr; uint8_t regval8; uint8_t chan; + edma_callback_t callback; + void *arg; /* Disable channel ERROR interrupts */ @@ -511,14 +513,17 @@ static void imxrt_dmaterminate(struct imxrt_dmach_s *dmach, int result) /* Perform the DMA complete callback */ - if (dmach->callback) - { - dmach->callback((DMACH_HANDLE)dmach, dmach->arg, true, result); - } + callback = dmach->callback; + arg = dmach->arg; dmach->callback = NULL; dmach->arg = NULL; dmach->state = IMXRT_DMA_IDLE; + + if (callback) + { + callback((DMACH_HANDLE)dmach, arg, true, result); + } } /**************************************************************************** @@ -1312,6 +1317,24 @@ unsigned int imxrt_dmach_getcount(DMACH_HANDLE handle) return remaining; } +/**************************************************************************** + * Name: imxrt_dmach_idle + * + * Description: + * This function checks if the dma is idle + * + * Returned Value: + * 0 - if idle + * !0 - not + * + ****************************************************************************/ + +unsigned int imxrt_dmach_idle(DMACH_HANDLE handle) +{ + struct imxrt_dmach_s *dmach = (struct imxrt_dmach_s *)handle; + return dmach->state == IMXRT_DMA_IDLE ? 0 : -1; +} + /**************************************************************************** * Name: imxrt_dmasample * diff --git a/arch/arm/src/imxrt/imxrt_edma.h b/arch/arm/src/imxrt/imxrt_edma.h index 39d89895a5e..f39e5538146 100644 --- a/arch/arm/src/imxrt/imxrt_edma.h +++ b/arch/arm/src/imxrt/imxrt_edma.h @@ -411,6 +411,20 @@ void imxrt_dmach_stop(DMACH_HANDLE handle); unsigned int imxrt_dmach_getcount(DMACH_HANDLE handle); +/**************************************************************************** + * Name: imxrt_dmach_idle + * + * Description: + * This function checks if the dma is idle + * + * Returned Value: + * 0 - if idle + * !0 - not + * + ****************************************************************************/ + +unsigned int imxrt_dmach_idle(DMACH_HANDLE handle); + /**************************************************************************** * Name: imxrt_dmasample * diff --git a/arch/arm/src/imxrt/imxrt_serial.c b/arch/arm/src/imxrt/imxrt_serial.c index 6a6def0e17d..b77f220b425 100644 --- a/arch/arm/src/imxrt/imxrt_serial.c +++ b/arch/arm/src/imxrt/imxrt_serial.c @@ -760,7 +760,6 @@ struct imxrt_uart_s #ifdef SERIAL_HAVE_TXDMA const unsigned int dma_txreqsrc; /* DMAMUX source of TX DMA request */ DMACH_HANDLE txdma; /* currently-open trasnmit DMA stream */ - sem_t txdmasem; /* Indicate TX DMA completion */ #endif /* RX DMA state */ @@ -770,6 +769,10 @@ struct imxrt_uart_s DMACH_HANDLE rxdma; /* currently-open receive DMA stream */ bool rxenable; /* DMA-based reception en/disable */ uint32_t rxdmanext; /* Next byte in the DMA buffer to be read */ +#ifdef CONFIG_ARMV7M_DCACHE + uint32_t rxdmaavail; /* Number of bytes available without need to + * to invalidate the data cache */ +#endif char *const rxfifo; /* Receive DMA buffer */ #endif }; @@ -1056,7 +1059,7 @@ static char g_lpuart8rxbuffer[CONFIG_LPUART8_RXBUFSIZE]; static char g_lpuart8txbuffer[LPUART8_TXBUFSIZE_ADJUSTED] \ LPUART8_TXBUFSIZE_ALGN; #endif - + #ifdef CONFIG_IMXRT_LPUART9 static char g_lpuart9rxbuffer[CONFIG_LPUART9_RXBUFSIZE]; static char g_lpuart9txbuffer[LPUART9_TXBUFSIZE_ADJUSTED] @@ -2018,9 +2021,6 @@ static int imxrt_dma_setup(struct uart_dev_s *dev) { return -EBUSY; } - - nxsem_init(&priv->txdmasem, 0, 1); - nxsem_set_protocol(&priv->txdmasem, SEM_PRIO_NONE); } /* Enable Tx DMA for the UART */ @@ -2075,6 +2075,9 @@ static int imxrt_dma_setup(struct uart_dev_s *dev) */ priv->rxdmanext = 0; +#ifdef CONFIG_ARMV7M_DCACHE + priv->rxdmaavail = 0; +#endif /* Enable receive Rx DMA for the UART */ @@ -2221,7 +2224,6 @@ static void imxrt_dma_shutdown(struct uart_dev_s *dev) imxrt_dmach_free(priv->txdma); priv->txdma = NULL; - nxsem_destroy(&priv->txdmasem); } #endif } @@ -2895,7 +2897,6 @@ static bool imxrt_rxflowcontrol(struct uart_dev_s *dev, static int imxrt_dma_receive(struct uart_dev_s *dev, unsigned int *status) { struct imxrt_uart_s *priv = (struct imxrt_uart_s *)dev; - static uint32_t last_nextrx = -1; uint32_t nextrx = imxrt_dma_nextrx(priv); int c = 0; @@ -2903,17 +2904,49 @@ static int imxrt_dma_receive(struct uart_dev_s *dev, unsigned int *status) if (nextrx != priv->rxdmanext) { - /* Now we must ensure the cache is updated if the DMA has - * updated again. +#ifdef CONFIG_ARMV7M_DCACHE + /* If the data cache is enabled, then we will also need to manage + * cache coherency. Are any bytes available in the currently coherent + * region of the data cache? */ - if (last_nextrx != nextrx) + if (priv->rxdmaavail == 0) { - up_invalidate_dcache((uintptr_t)priv->rxfifo, - (uintptr_t)priv->rxfifo + RXDMA_BUFFER_SIZE); - last_nextrx = nextrx; + uint32_t rxdmaavail; + uintptr_t addr; + + /* No.. then we will have to invalidate additional space in the Rx + * DMA buffer. + */ + + if (nextrx > priv->rxdmanext) + { + /* Number of available bytes */ + + rxdmaavail = nextrx - priv->rxdmanext; + } + else + { + /* Number of available bytes up to the end of RXDMA buffer */ + + rxdmaavail = RXDMA_BUFFER_SIZE - priv->rxdmanext; + } + + /* Invalidate the DMA buffer range */ + + addr = (uintptr_t)&priv->rxfifo[priv->rxdmanext]; + up_invalidate_dcache(addr, addr + rxdmaavail); + + /* We don't need to invalidate the data cache for the next + * rxdmaavail number of next bytes. + */ + + priv->rxdmaavail = rxdmaavail; } + priv->rxdmaavail--; +#endif + /* Now read from the DMA buffer */ c = priv->rxfifo[priv->rxdmanext]; @@ -2978,6 +3011,9 @@ static void imxrt_dma_reenable(struct imxrt_uart_s *priv) */ priv->rxdmanext = 0; +#ifdef CONFIG_ARMV7M_DCACHE + priv->rxdmaavail = 0; +#endif /* Start the DMA channel, and arrange for callbacks at the half and * full points in the FIFO. This ensures that we have half a FIFO @@ -3042,8 +3078,9 @@ static bool imxrt_dma_rxavailable(struct uart_dev_s *dev) * Name: imxrt_dma_txcallback * * Description: - * This function clears dma buffer at complete of DMA transfer and wakes up - * threads waiting for space in buffer. + * This function clears dma buffer at completion of DMA transfer. It wakes + * up threads waiting for space in buffer and restarts the DMA if there is + * more data to send. * ****************************************************************************/ @@ -3052,19 +3089,20 @@ static void imxrt_dma_txcallback(DMACH_HANDLE handle, void *arg, bool done, int result) { struct imxrt_uart_s *priv = (struct imxrt_uart_s *)arg; + /* Update 'nbytes' indicating number of bytes actually transferred by DMA. * This is important to free TX buffer space by 'uart_xmitchars_done'. */ priv->dev.dmatx.nbytes = priv->dev.dmatx.length + priv->dev.dmatx.nlength; - /* Adjust the pointers */ + /* Adjust the pointers and unblock writers */ uart_xmitchars_done(&priv->dev); - /* Release waiter */ + /* Send more data if available */ - nxsem_post(&priv->txdmasem); + imxrt_dma_txavailable(&priv->dev); } #endif @@ -3083,9 +3121,7 @@ static void imxrt_dma_txavailable(struct uart_dev_s *dev) /* Only send when the DMA is idle */ - int rv = nxsem_trywait(&priv->txdmasem); - - if (rv == 0) + if (imxrt_dmach_idle(priv->txdma) == 0) { uart_xmitchars_dma(dev); } diff --git a/arch/arm/src/s32k3xx/s32k3xx_edma.c b/arch/arm/src/s32k3xx/s32k3xx_edma.c index 01211dcc703..7a829de1e62 100644 --- a/arch/arm/src/s32k3xx/s32k3xx_edma.c +++ b/arch/arm/src/s32k3xx/s32k3xx_edma.c @@ -1,7 +1,7 @@ /**************************************************************************** * arch/arm/src/s32k3xx/s32k3xx_edma.c * - * Copyright (C) 2019, 2021 Gregory Nutt. All rights reserved. + * Copyright (C) 2019, 2021, 2023 Gregory Nutt. All rights reserved. * Copyright 2022 NXP * Authors: Gregory Nutt * David Sidrane @@ -733,6 +733,8 @@ static void s32k3xx_dmaterminate(struct s32k3xx_dmach_s *dmach, int result) struct s32k3xx_edmatcd_s *next; #endif uint8_t chan; + edma_callback_t callback; + void *arg; chan = dmach->chan; @@ -774,14 +776,17 @@ static void s32k3xx_dmaterminate(struct s32k3xx_dmach_s *dmach, int result) /* Perform the DMA complete callback */ - if (dmach->callback) - { - dmach->callback((DMACH_HANDLE)dmach, dmach->arg, true, result); - } + callback = dmach->callback; + arg = dmach->arg; dmach->callback = NULL; dmach->arg = NULL; dmach->state = S32K3XX_DMA_IDLE; + + if (callback) + { + callback((DMACH_HANDLE)dmach, arg, true, result); + } } /**************************************************************************** @@ -1532,6 +1537,24 @@ unsigned int s32k3xx_dmach_getcount(DMACH_HANDLE *handle) return remaining; } +/**************************************************************************** + * Name: s32k3xx_dmach_idle + * + * Description: + * This function checks if the dma is idle + * + * Returned Value: + * 0 - if idle + * !0 - not + * + ****************************************************************************/ + +unsigned int s32k3xx_dmach_idle(DMACH_HANDLE handle) +{ + struct s32k3xx_dmach_s *dmach = (struct s32k3xx_dmach_s *)handle; + return dmach->state == S32K3XX_DMA_IDLE ? 0 : -1; +} + /**************************************************************************** * Name: s32k3xx_dmasample * diff --git a/arch/arm/src/s32k3xx/s32k3xx_edma.h b/arch/arm/src/s32k3xx/s32k3xx_edma.h index 5c0441ac77e..00fff86f9cd 100644 --- a/arch/arm/src/s32k3xx/s32k3xx_edma.h +++ b/arch/arm/src/s32k3xx/s32k3xx_edma.h @@ -1,7 +1,7 @@ /**************************************************************************** * arch/arm/src/s32k3xx/s32k3xx_edma.h * - * Copyright (C) 2019, 2021 Gregory Nutt. All rights reserved. + * Copyright (C) 2019, 2021, 2023 Gregory Nutt. All rights reserved. * Copyright 2022 NXP * Authors: Gregory Nutt * David Sidrane @@ -431,6 +431,20 @@ void s32k3xx_dmach_stop(DMACH_HANDLE handle); unsigned int s32k3xx_dmach_getcount(DMACH_HANDLE *handle); +/**************************************************************************** + * Name: s32k3xx_dmach_idle + * + * Description: + * This function checks if the dma is idle + * + * Returned Value: + * 0 - if idle + * !0 - not + * + ****************************************************************************/ + +unsigned int s32k3xx_dmach_idle(DMACH_HANDLE handle); + /**************************************************************************** * Name: s32k3xx_dmasample * diff --git a/arch/arm/src/s32k3xx/s32k3xx_serial.c b/arch/arm/src/s32k3xx/s32k3xx_serial.c index 9155950ee1c..058562c7fe7 100644 --- a/arch/arm/src/s32k3xx/s32k3xx_serial.c +++ b/arch/arm/src/s32k3xx/s32k3xx_serial.c @@ -1332,7 +1332,6 @@ struct s32k3xx_uart_s #ifdef SERIAL_HAVE_TXDMA const unsigned int dma_txreqsrc; /* DMAMUX source of TX DMA request */ DMACH_HANDLE txdma; /* currently-open transmit DMA stream */ - sem_t txdmasem; /* Indicate TX DMA completion */ #endif /* RX DMA state */ @@ -1342,6 +1341,10 @@ struct s32k3xx_uart_s DMACH_HANDLE rxdma; /* currently-open receive DMA stream */ bool rxenable; /* DMA-based reception en/disable */ uint32_t rxdmanext; /* Next byte in the DMA buffer to be read */ +#ifdef CONFIG_ARMV7M_DCACHE + uint32_t rxdmaavail; /* Number of bytes available without need to + * to invalidate the data cache */ +#endif char *const rxfifo; /* Receive DMA buffer */ #endif }; @@ -2909,9 +2912,6 @@ static int s32k3xx_dma_setup(struct uart_dev_s *dev) { return -EBUSY; } - - nxsem_init(&priv->txdmasem, 0, 1); - nxsem_set_protocol(&priv->txdmasem, SEM_PRIO_NONE); } /* Enable Tx DMA for the UART */ @@ -2966,6 +2966,9 @@ static int s32k3xx_dma_setup(struct uart_dev_s *dev) */ priv->rxdmanext = 0; +#ifdef CONFIG_ARMV7M_DCACHE + priv->rxdmaavail = 0; +#endif /* Enable receive Rx DMA for the UART */ @@ -3113,7 +3116,6 @@ static void s32k3xx_dma_shutdown(struct uart_dev_s *dev) s32k3xx_dmach_free(priv->txdma); priv->txdma = NULL; - nxsem_destroy(&priv->txdmasem); } #endif } @@ -3780,7 +3782,6 @@ static bool s32k3xx_rxflowcontrol(struct uart_dev_s *dev, static int s32k3xx_dma_receive(struct uart_dev_s *dev, unsigned int *status) { struct s32k3xx_uart_s *priv = (struct s32k3xx_uart_s *)dev; - static uint32_t last_nextrx = -1; uint32_t nextrx = s32k3xx_dma_nextrx(priv); int c = 0; @@ -3788,17 +3789,49 @@ static int s32k3xx_dma_receive(struct uart_dev_s *dev, unsigned int *status) if (nextrx != priv->rxdmanext) { - /* Now we must ensure the cache is updated if the DMA has - * updated again. +#ifdef CONFIG_ARMV7M_DCACHE + /* If the data cache is enabled, then we will also need to manage + * cache coherency. Are any bytes available in the currently coherent + * region of the data cache? */ - if (last_nextrx != nextrx) + if (priv->rxdmaavail == 0) { - up_invalidate_dcache((uintptr_t)priv->rxfifo, - (uintptr_t)priv->rxfifo + RXDMA_BUFFER_SIZE); - last_nextrx = nextrx; + uint32_t rxdmaavail; + uintptr_t addr; + + /* No.. then we will have to invalidate additional space in the Rx + * DMA buffer. + */ + + if (nextrx > priv->rxdmanext) + { + /* Number of available bytes */ + + rxdmaavail = nextrx - priv->rxdmanext; + } + else + { + /* Number of available bytes up to the end of RXDMA buffer */ + + rxdmaavail = RXDMA_BUFFER_SIZE - priv->rxdmanext; + } + + /* Invalidate the DMA buffer range */ + + addr = (uintptr_t)&priv->rxfifo[priv->rxdmanext]; + up_invalidate_dcache(addr, addr + rxdmaavail); + + /* We don't need to invalidate the data cache for the next + * rxdmaavail number of next bytes. + */ + + priv->rxdmaavail = rxdmaavail; } + priv->rxdmaavail--; +#endif + /* Now read from the DMA buffer */ c = priv->rxfifo[priv->rxdmanext]; @@ -3863,6 +3896,9 @@ static void s32k3xx_dma_reenable(struct s32k3xx_uart_s *priv) */ priv->rxdmanext = 0; +#ifdef CONFIG_ARMV7M_DCACHE + priv->rxdmaavail = 0; +#endif /* Start the DMA channel, and arrange for callbacks at the half and * full points in the FIFO. This ensures that we have half a FIFO @@ -3947,9 +3983,9 @@ static void s32k3xx_dma_txcallback(DMACH_HANDLE handle, void *arg, bool done, uart_xmitchars_done(&priv->dev); - /* Release waiter */ + /* Send more data if available */ - nxsem_post(&priv->txdmasem); + s32k3xx_dma_txavailable(&priv->dev); } #endif @@ -3968,9 +4004,7 @@ static void s32k3xx_dma_txavailable(struct uart_dev_s *dev) /* Only send when the DMA is idle */ - int rv = nxsem_trywait(&priv->txdmasem); - - if (rv == OK) + if (s32k3xx_dmach_idle(priv->txdma) == 0) { uart_xmitchars_dma(dev); } diff --git a/arch/arm/src/stm32h7/stm32_serial.c b/arch/arm/src/stm32h7/stm32_serial.c index 537abe3f6a6..44159818a01 100644 --- a/arch/arm/src/stm32h7/stm32_serial.c +++ b/arch/arm/src/stm32h7/stm32_serial.c @@ -628,7 +628,6 @@ struct up_dev_s #ifdef SERIAL_HAVE_TXDMA const unsigned int txdma_channel; /* DMA channel assigned */ DMA_HANDLE txdma; /* currently-open trasnmit DMA stream */ - sem_t txdmasem; /* Indicate TX DMA completion */ #endif /* RX DMA state */ @@ -2224,9 +2223,6 @@ static int up_dma_setup(struct uart_dev_s *dev) { priv->txdma = stm32_dmachannel(priv->txdma_channel); - nxsem_init(&priv->txdmasem, 0, 1); - nxsem_set_protocol(&priv->txdmasem, SEM_PRIO_NONE); - /* Enable receive Tx DMA for the UART */ modifyreg32(priv->usartbase + STM32_USART_CR3_OFFSET, @@ -3321,11 +3317,7 @@ static void up_dma_txcallback(DMA_HANDLE handle, uint8_t status, void *arg) * This is important to free TX buffer space by 'uart_xmitchars_done'. */ - if (status & DMA_SCR_HTIE) - { - priv->dev.dmatx.nbytes += priv->dev.dmatx.length / 2; - } - else if (status & DMA_SCR_TCIE) + if (status & DMA_SCR_TCIE) { priv->dev.dmatx.nbytes += priv->dev.dmatx.length; if (priv->dev.dmatx.nlength) @@ -3353,11 +3345,13 @@ static void up_dma_txcallback(DMA_HANDLE handle, uint8_t status, void *arg) } } - nxsem_post(&priv->txdmasem); - /* Adjust the pointers */ uart_xmitchars_done(&priv->dev); + + /* Send more if availaible */ + + up_dma_txavailable(&priv->dev); } #endif @@ -3376,8 +3370,7 @@ static void up_dma_txavailable(struct uart_dev_s *dev) /* Only send when the DMA is idle */ - int rv = nxsem_trywait(&priv->txdmasem); - if (rv == OK) + if (stm32_dmaresidual(priv->txdma) == 0) { uart_xmitchars_dma(dev); }