From 86ded2c40c7bf00cde603d210f2af2d5b1e9f470 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 May 2018 18:59:41 +1000 Subject: [PATCH] HAL_ChibiOS: cleanup sdcard API usage --- libraries/AP_HAL_ChibiOS/SPIDevice.cpp | 74 ++++++++-------------- libraries/AP_HAL_ChibiOS/SPIDevice.h | 15 ++--- libraries/AP_HAL_ChibiOS/sdcard.cpp | 88 ++++++++++++++++++++------ libraries/AP_HAL_ChibiOS/sdcard.h | 1 + 4 files changed, 101 insertions(+), 77 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index 864a1026c2..decafc71e4 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -146,7 +146,13 @@ void SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) bus.bouncebuffer_setup(send_buf, len, recv_buf, len); - spiExchange(spi_devices[device_desc.bus].driver, len, send_buf, recv_buf); + if (send == nullptr) { + spiReceive(spi_devices[device_desc.bus].driver, len, recv_buf); + } else if (recv == nullptr) { + spiSend(spi_devices[device_desc.bus].driver, len, send_buf); + } else { + spiExchange(spi_devices[device_desc.bus].driver, len, send_buf, recv_buf); + } if (recv_buf != recv) { memcpy(recv, recv_buf, len); @@ -155,6 +161,22 @@ void SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) set_chip_select(old_cs_forced); } +bool SPIDevice::clock_pulse(uint32_t n) +{ + if (!cs_forced) { + //special mode to init sdcard without cs asserted + bus.semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER); + acquire_bus(true, true); + spiIgnore(spi_devices[device_desc.bus].driver, n); + acquire_bus(false, true); + bus.semaphore.give(); + } else { + bus.semaphore.assert_owner(); + spiIgnore(spi_devices[device_desc.bus].driver, n); + } + return true; +} + uint16_t SPIDevice::derive_freq_flag(uint32_t _frequency) { uint32_t spi_clock_freq = SPI1_CLOCK; @@ -183,9 +205,9 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, hal.console->printf("SPI: not owner of 0x%x\n", unsigned(get_bus_id())); return false; } - if (send_len == recv_len && send == recv) { + if ((send_len == recv_len && send == recv) || !send || !recv) { // simplest cases, needed for DMA - do_transfer(send, recv, recv_len); + do_transfer(send, recv, recv_len?recv_len:send_len); return true; } uint8_t buf[send_len+recv_len]; @@ -318,52 +340,6 @@ SPIDeviceManager::get_device(const char *name) return AP_HAL::OwnPtr(new SPIDevice(*busp, desc)); } -#if HAL_USE_MMC_SPI == TRUE - -void SPIDevice::spi_select() { - bus.semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER); - acquire_bus(true, false); -} - -void SPIDevice::spi_unselect() { - acquire_bus(false, false); - bus.semaphore.give(); -} - -void SPIDevice::spi_ignore(size_t n) { - if (!cs_forced) { - //special mode to init sdcard without cs asserted - bus.semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER); - acquire_bus(true, true); - spiIgnore(spi_devices[device_desc.bus].driver, n); - acquire_bus(false, true); - bus.semaphore.give(); - } else { - bus.semaphore.assert_owner(); - spiIgnore(spi_devices[device_desc.bus].driver, n); - } -} - -void SPIDevice::spi_send(size_t tx_len, const void *txbuf) { - bus.semaphore.assert_owner(); - const uint8_t *tbuf = (uint8_t *) txbuf; - bus.bouncebuffer_setup_tx(tbuf, tx_len); - spiSend(spi_devices[device_desc.bus].driver, tx_len, tbuf); -} - -void SPIDevice::spi_receive(size_t rx_len, void *rxbuf) { - bus.semaphore.assert_owner(); - uint8_t *rbuf = (uint8_t *) rxbuf; - bus.bouncebuffer_setup_rx(rbuf, rx_len); - spiReceive(spi_devices[device_desc.bus].driver, rx_len, rbuf); - if (rxbuf != rbuf) { - memcpy(rxbuf, rbuf, rx_len); - } -} - -#endif - - #ifdef HAL_SPI_CHECK_CLOCK_FREQ /* test clock frequencies. This measures the actual SPI clock diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h index 20bcc5bdab..9a9849e858 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.h +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h @@ -76,6 +76,12 @@ public: bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) override; + /* + * send N bytes of clock pulses without taking CS. This is used + * when initialising microSD interfaces over SPI + */ + bool clock_pulse(uint32_t len) override; + /* See AP_HAL::Device::get_semaphore() */ AP_HAL::Semaphore *get_semaphore() override; @@ -97,15 +103,6 @@ public: static void test_clock_freq(void); #endif -#if HAL_USE_MMC_SPI == TRUE - //functions to be used by hal_mmc_spi.c sdcard driver - void spi_select(); - void spi_unselect(); - void spi_ignore(size_t n); - void spi_send(size_t n, const void *txbuf); - void spi_receive(size_t n, void *rxbuf); -#endif - private: SPIBus &bus; SPIDesc &device_desc; diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index a283948ff1..39acff0026 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -18,6 +18,8 @@ #include "sdcard.h" #include "hwdef/common/spi_hook.h" +extern const AP_HAL::HAL& hal; + #ifdef USE_POSIX static FATFS SDC_FS; // FATFS object #endif @@ -28,10 +30,14 @@ static AP_HAL::OwnPtr device; static MMCConfig mmcconfig; static SPIConfig lowspeed; static SPIConfig highspeed; +static bool sdcard_running; #endif -void sdcard_init() { - +/* + initialise microSD card if avaialble + */ +void sdcard_init() +{ #ifdef USE_POSIX #if HAL_USE_SDC sdcStart(&SDCD1, NULL); @@ -48,9 +54,14 @@ void sdcard_init() { //Create APM Directory mkdir("/APM", 0777); } -#endif -#if HAL_USE_MMC_SPI +#elif HAL_USE_MMC_SPI device = AP_HAL::get_HAL().spi->get_device("sdcard"); + if (!device) { + printf("No sdcard SPI device found\n"); + return; + } + + sdcard_running = true; mmcObjectInit(&MMCD1); @@ -63,6 +74,7 @@ void sdcard_init() { if (mmcConnect(&MMCD1) == HAL_FAILED) { printf("Err: Failed to initialize SDCARD_SPI!\n"); + sdcard_running = false; } else { if (f_mount(&SDC_FS, "/", 1) != FR_OK) { printf("Err: Failed to mount SD Card!\n"); @@ -77,35 +89,73 @@ void sdcard_init() { #endif } +/* + stop sdcard interface (for reboot) + */ +void sdcard_stop(void) +{ +#if HAL_USE_MMC_SPI + if (sdcard_running) { + mmcDisconnect(&MMCD1); + mmcStop(&MMCD1); + sdcard_running = false; + } +#endif +} + #if HAL_USE_MMC_SPI -void spiStartHook(SPIDriver *spip, const SPIConfig *config) { - device->set_speed( - config == &lowspeed ? - AP_HAL::Device::SPEED_LOW : AP_HAL::Device::SPEED_HIGH); +/* + hooks to allow hal_mmc_spi.c to work with HAL_ChibiOS SPI + layer. This provides bounce buffers for DMA, DMA channel sharing and + bus locking + */ + +void spiStartHook(SPIDriver *spip, const SPIConfig *config) +{ + device->set_speed(config == &lowspeed ? + AP_HAL::Device::SPEED_LOW : AP_HAL::Device::SPEED_HIGH); } -void spiStopHook(SPIDriver *spip) { +void spiStopHook(SPIDriver *spip) +{ } -void spiSelectHook(SPIDriver *spip) { - static_cast(device.get())->spi_select(); +void spiSelectHook(SPIDriver *spip) +{ + if (sdcard_running) { + device->get_semaphore()->take_blocking(); + device->set_chip_select(true); + } } -void spiUnselectHook(SPIDriver *spip) { - static_cast(device.get())->spi_unselect(); +void spiUnselectHook(SPIDriver *spip) +{ + if (sdcard_running) { + device->set_chip_select(false); + device->get_semaphore()->give(); + } } -void spiIgnoreHook(SPIDriver *spip, size_t n) { - static_cast(device.get())->spi_ignore(n); +void spiIgnoreHook(SPIDriver *spip, size_t n) +{ + if (sdcard_running) { + device->clock_pulse(n); + } } -void spiSendHook(SPIDriver *spip, size_t n, const void *txbuf) { - static_cast(device.get())->spi_send(n, txbuf); +void spiSendHook(SPIDriver *spip, size_t n, const void *txbuf) +{ + if (sdcard_running) { + device->transfer((const uint8_t *)txbuf, n, nullptr, 0); + } } -void spiReceiveHook(SPIDriver *spip, size_t n, void *rxbuf) { - static_cast(device.get())->spi_receive(n, rxbuf); +void spiReceiveHook(SPIDriver *spip, size_t n, void *rxbuf) +{ + if (sdcard_running) { + device->transfer(nullptr, 0, (uint8_t *)rxbuf, n); + } } #endif diff --git a/libraries/AP_HAL_ChibiOS/sdcard.h b/libraries/AP_HAL_ChibiOS/sdcard.h index 2171c5b8fd..acea83113d 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.h +++ b/libraries/AP_HAL_ChibiOS/sdcard.h @@ -16,3 +16,4 @@ #pragma once void sdcard_init(); +void sdcard_stop();