mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 17:53:59 -04:00
HAL_ChibiOS: wrap cacheBuffer functions
needed to avoid problem with end() method in UARTDriver shadowing with F7 implementation of cache macros
This commit is contained in:
parent
b867ef35c0
commit
d1565a96c5
@ -188,7 +188,7 @@ void AnalogIn::adccallback(ADCDriver *adcp)
|
|||||||
{
|
{
|
||||||
const adcsample_t *buffer = samples;
|
const adcsample_t *buffer = samples;
|
||||||
|
|
||||||
cacheBufferInvalidate(buffer, sizeof(adcsample_t)*ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS);
|
stm32_cacheBufferInvalidate(buffer, sizeof(adcsample_t)*ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS);
|
||||||
for (uint8_t i = 0; i < ADC_DMA_BUF_DEPTH; i++) {
|
for (uint8_t i = 0; i < ADC_DMA_BUF_DEPTH; i++) {
|
||||||
for (uint8_t j = 0; j < ADC_GRP1_NUM_CHANNELS; j++) {
|
for (uint8_t j = 0; j < ADC_GRP1_NUM_CHANNELS; j++) {
|
||||||
sample_sum[j] += *buffer++;
|
sample_sum[j] += *buffer++;
|
||||||
|
@ -990,7 +990,7 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
|
|||||||
up with this great method.
|
up with this great method.
|
||||||
*/
|
*/
|
||||||
dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR));
|
dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR));
|
||||||
cacheBufferFlush(group.dma_buffer, buffer_length);
|
stm32_cacheBufferFlush(group.dma_buffer, buffer_length);
|
||||||
dmaStreamSetMemory0(group.dma, group.dma_buffer);
|
dmaStreamSetMemory0(group.dma, group.dma_buffer);
|
||||||
dmaStreamSetTransactionSize(group.dma, buffer_length/sizeof(uint32_t));
|
dmaStreamSetTransactionSize(group.dma, buffer_length/sizeof(uint32_t));
|
||||||
dmaStreamSetFIFO(group.dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL);
|
dmaStreamSetFIFO(group.dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL);
|
||||||
|
@ -97,7 +97,7 @@ void SoftSigReader::_irq_handler(void* self, uint32_t flags)
|
|||||||
// we need to restart the DMA as quickly as possible to prevent losing pulses, so we
|
// we need to restart the DMA as quickly as possible to prevent losing pulses, so we
|
||||||
// make a fixed length copy to a 2nd buffer. On the F100 this reduces the time with DMA
|
// make a fixed length copy to a 2nd buffer. On the F100 this reduces the time with DMA
|
||||||
// disabled from 20us to under 1us
|
// disabled from 20us to under 1us
|
||||||
cacheBufferInvalidate(sig_reader->signal, SOFTSIG_BOUNCE_BUF_SIZE*4);
|
stm32_cacheBufferInvalidate(sig_reader->signal, SOFTSIG_BOUNCE_BUF_SIZE*4);
|
||||||
memcpy(sig_reader->signal2, sig_reader->signal, SOFTSIG_BOUNCE_BUF_SIZE*4);
|
memcpy(sig_reader->signal2, sig_reader->signal, SOFTSIG_BOUNCE_BUF_SIZE*4);
|
||||||
//restart the DMA transfers
|
//restart the DMA transfers
|
||||||
dmaStreamDisable(sig_reader->dma);
|
dmaStreamDisable(sig_reader->dma);
|
||||||
|
@ -426,7 +426,7 @@ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags)
|
|||||||
if (!uart_drv->sdef.dma_rx) {
|
if (!uart_drv->sdef.dma_rx) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
cacheBufferInvalidate(uart_drv->rx_bounce_buf, RX_BOUNCE_BUFSIZE);
|
stm32_cacheBufferInvalidate(uart_drv->rx_bounce_buf, RX_BOUNCE_BUFSIZE);
|
||||||
uint8_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(uart_drv->rxdma);
|
uint8_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(uart_drv->rxdma);
|
||||||
if (len > 0) {
|
if (len > 0) {
|
||||||
if (uart_drv->half_duplex) {
|
if (uart_drv->half_duplex) {
|
||||||
@ -436,7 +436,7 @@ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
cacheBufferInvalidate(uart_drv->rx_bounce_buf, len);
|
stm32_cacheBufferInvalidate(uart_drv->rx_bounce_buf, len);
|
||||||
uart_drv->_readbuf.write(uart_drv->rx_bounce_buf, len);
|
uart_drv->_readbuf.write(uart_drv->rx_bounce_buf, len);
|
||||||
|
|
||||||
uart_drv->receive_timestamp_update();
|
uart_drv->receive_timestamp_update();
|
||||||
@ -765,7 +765,7 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n)
|
|||||||
|
|
||||||
tx_bounce_buf_ready = false;
|
tx_bounce_buf_ready = false;
|
||||||
osalDbgAssert(txdma != nullptr, "UART TX DMA allocation failed");
|
osalDbgAssert(txdma != nullptr, "UART TX DMA allocation failed");
|
||||||
cacheBufferFlush(tx_bounce_buf, tx_len);
|
stm32_cacheBufferFlush(tx_bounce_buf, tx_len);
|
||||||
dmaStreamSetMemory0(txdma, tx_bounce_buf);
|
dmaStreamSetMemory0(txdma, tx_bounce_buf);
|
||||||
dmaStreamSetTransactionSize(txdma, tx_len);
|
dmaStreamSetTransactionSize(txdma, tx_len);
|
||||||
uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
|
uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
|
||||||
@ -917,7 +917,7 @@ void UARTDriver::_timer_tick(void)
|
|||||||
if (!(rxdma->stream->CR & STM32_DMA_CR_EN)) {
|
if (!(rxdma->stream->CR & STM32_DMA_CR_EN)) {
|
||||||
uint8_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(rxdma);
|
uint8_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(rxdma);
|
||||||
if (len != 0) {
|
if (len != 0) {
|
||||||
cacheBufferInvalidate(rx_bounce_buf, len);
|
stm32_cacheBufferInvalidate(rx_bounce_buf, len);
|
||||||
_readbuf.write(rx_bounce_buf, len);
|
_readbuf.write(rx_bounce_buf, len);
|
||||||
|
|
||||||
receive_timestamp_update();
|
receive_timestamp_update();
|
||||||
|
@ -74,7 +74,7 @@ void bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf,
|
|||||||
*buf = bouncebuffer->dma_buf;
|
*buf = bouncebuffer->dma_buf;
|
||||||
#if defined(STM32H7)
|
#if defined(STM32H7)
|
||||||
osalDbgAssert((((uint32_t)*buf)&31) == 0, "bouncebuffer read align");
|
osalDbgAssert((((uint32_t)*buf)&31) == 0, "bouncebuffer read align");
|
||||||
cacheBufferInvalidate(*buf, (size+31)&~31);
|
stm32_cacheBufferInvalidate(*buf, (size+31)&~31);
|
||||||
#endif
|
#endif
|
||||||
bouncebuffer->busy = true;
|
bouncebuffer->busy = true;
|
||||||
}
|
}
|
||||||
@ -118,7 +118,7 @@ void bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t
|
|||||||
*buf = bouncebuffer->dma_buf;
|
*buf = bouncebuffer->dma_buf;
|
||||||
#if defined(STM32H7)
|
#if defined(STM32H7)
|
||||||
osalDbgAssert((((uint32_t)*buf)&31) == 0, "bouncebuffer write align");
|
osalDbgAssert((((uint32_t)*buf)&31) == 0, "bouncebuffer write align");
|
||||||
cacheBufferFlush(*buf, (size+31)&~31);
|
stm32_cacheBufferFlush(*buf, (size+31)&~31);
|
||||||
#endif
|
#endif
|
||||||
bouncebuffer->busy = true;
|
bouncebuffer->busy = true;
|
||||||
}
|
}
|
||||||
|
@ -379,7 +379,7 @@ bool stm32_flash_erasepage(uint32_t page)
|
|||||||
|
|
||||||
stm32_flash_wait_idle();
|
stm32_flash_wait_idle();
|
||||||
|
|
||||||
cacheBufferInvalidate(stm32_flash_getpageaddr(page), stm32_flash_getpagesize(page));
|
stm32_cacheBufferInvalidate(stm32_flash_getpageaddr(page), stm32_flash_getpagesize(page));
|
||||||
|
|
||||||
stm32_flash_lock();
|
stm32_flash_lock();
|
||||||
#if STM32_FLASH_DISABLE_ISR
|
#if STM32_FLASH_DISABLE_ISR
|
||||||
|
@ -278,6 +278,6 @@ void memory_flush_all(void)
|
|||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
for (i=0; i<NUM_MEMORY_REGIONS; i++) {
|
for (i=0; i<NUM_MEMORY_REGIONS; i++) {
|
||||||
cacheBufferFlush(memory_regions[i].address, memory_regions[i].size);
|
stm32_cacheBufferFlush(memory_regions[i].address, memory_regions[i].size);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -341,3 +341,13 @@ iomode_t palReadLineMode(ioline_t line)
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
void stm32_cacheBufferInvalidate(const void *p, size_t size)
|
||||||
|
{
|
||||||
|
cacheBufferInvalidate(p, size);
|
||||||
|
}
|
||||||
|
|
||||||
|
void stm32_cacheBufferFlush(const void *p, size_t size)
|
||||||
|
{
|
||||||
|
cacheBufferFlush(p, size);
|
||||||
|
}
|
||||||
|
@ -87,6 +87,9 @@ void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n);
|
|||||||
// get RTC backup registers starting at given idx
|
// get RTC backup registers starting at given idx
|
||||||
void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n);
|
void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n);
|
||||||
|
|
||||||
|
void stm32_cacheBufferInvalidate(const void *p, size_t size);
|
||||||
|
void stm32_cacheBufferFlush(const void *p, size_t size);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
Loading…
Reference in New Issue
Block a user