From 6b15b2f44f98d21e7128831100ad2c1a213db1e4 Mon Sep 17 00:00:00 2001 From: Alexander Malishev Date: Sun, 29 Apr 2018 04:09:09 +0400 Subject: [PATCH] HAL_ChibiOS: Enable sdcard on spi bus --- libraries/AP_HAL_ChibiOS/Device.cpp | 9 ++ libraries/AP_HAL_ChibiOS/Device.h | 5 + .../AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 34 ++---- libraries/AP_HAL_ChibiOS/SPIDevice.cpp | 72 ++++++++++- libraries/AP_HAL_ChibiOS/SPIDevice.h | 13 ++ .../AP_HAL_ChibiOS/hwdef/common/spi_hook.h | 21 ++++ .../hwdef/omnibusf4pro/hwdef.dat | 6 +- .../hwdef/scripts/chibios_hwdef.py | 14 +++ libraries/AP_HAL_ChibiOS/sdcard.cpp | 112 ++++++++++++++++++ libraries/AP_HAL_ChibiOS/sdcard.h | 18 +++ 10 files changed, 270 insertions(+), 34 deletions(-) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/spi_hook.h create mode 100644 libraries/AP_HAL_ChibiOS/sdcard.cpp create mode 100644 libraries/AP_HAL_ChibiOS/sdcard.h diff --git a/libraries/AP_HAL_ChibiOS/Device.cpp b/libraries/AP_HAL_ChibiOS/Device.cpp index 4e7d8a419f..b716ef3a6e 100644 --- a/libraries/AP_HAL_ChibiOS/Device.cpp +++ b/libraries/AP_HAL_ChibiOS/Device.cpp @@ -149,6 +149,12 @@ bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_u */ void DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, uint8_t *&buf_rx, uint16_t rx_len) +{ + bouncebuffer_setup_tx(buf_tx, tx_len); + bouncebuffer_setup_rx(buf_rx, rx_len); +} + +void DeviceBus::bouncebuffer_setup_tx(const uint8_t *&buf_tx, uint16_t tx_len) { if (buf_tx && !IS_DMA_SAFE(buf_tx)) { if (tx_len > bounce_buffer_tx_size) { @@ -165,7 +171,10 @@ void DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, memcpy(bounce_buffer_tx, buf_tx, tx_len); buf_tx = bounce_buffer_tx; } +} +void DeviceBus::bouncebuffer_setup_rx(uint8_t *&buf_rx, uint16_t rx_len) +{ if (buf_rx && !IS_DMA_SAFE(buf_rx)) { if (rx_len > bounce_buffer_rx_size) { if (bounce_buffer_rx_size) { diff --git a/libraries/AP_HAL_ChibiOS/Device.h b/libraries/AP_HAL_ChibiOS/Device.h index 53e6c2893b..ddbfd0bbec 100644 --- a/libraries/AP_HAL_ChibiOS/Device.h +++ b/libraries/AP_HAL_ChibiOS/Device.h @@ -37,6 +37,11 @@ public: void bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, uint8_t *&buf_rx, uint16_t rx_len); + + void bouncebuffer_setup_tx(const uint8_t *&buf_tx, uint16_t tx_len); + + void bouncebuffer_setup_rx(uint8_t *&buf_rx, uint16_t rx_len); + void bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len); private: diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 108d0b5e0f..ee02df701e 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -23,6 +23,7 @@ #include #include #include "shared_dma.h" +#include "sdcard.h" #include "hwdef/common/usbcfg.h" #include @@ -69,9 +70,7 @@ static Empty::RCOutput rcoutDriver; static ChibiOS::Scheduler schedulerInstance; static ChibiOS::Util utilInstance; static Empty::OpticalFlow opticalFlowDriver; -#ifdef USE_POSIX -static FATFS SDC_FS; // FATFS object -#endif + #if HAL_WITH_IO_MCU HAL_UART_IO_DRIVER; @@ -147,8 +146,11 @@ static THD_FUNCTION(main_loop,arg) #ifdef HAL_SPI_CHECK_CLOCK_FREQ // optional test of SPI clock frequencies ChibiOS::SPIDevice::test_clock_freq(); -#endif - +#endif + + //Setup SD Card and Initialise FATFS bindings + sdcard_init(); + hal.uartB->begin(38400); hal.uartC->begin(57600); hal.analogin->init(); @@ -217,28 +219,6 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg); #endif - //Setup SD Card and Initialise FATFS bindings - /* - * Start SD Driver - */ -#ifdef USE_POSIX - FRESULT err; - sdcStart(&SDCD1, NULL); - - if(sdcConnect(&SDCD1) == HAL_FAILED) { - printf("Err: Failed to initialize SDIO!\n"); - } else { - err = f_mount(&SDC_FS, "/", 1); - if (err != FR_OK) { - printf("Err: Failed to mount SD Card!\n"); - sdcDisconnect(&SDCD1); - } else { - printf("Successfully mounted SDCard..\n"); - } - //Create APM Directory - mkdir("/APM", 0777); - } -#endif assert(callbacks); g_callbacks = callbacks; diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index fbdc6838f9..cae2e37afa 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -114,6 +114,10 @@ SPIDevice::~SPIDevice() free(pname); } +SPIDriver * SPIDevice::get_driver() { + return spi_devices[device_desc.bus].driver; +} + bool SPIDevice::set_speed(AP_HAL::Device::Speed speed) { switch (speed) { @@ -225,19 +229,21 @@ bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint3 } /* - allow for control of SPI chip select pin - */ -bool SPIDevice::set_chip_select(bool set) + used to acquire bus and (optionally) assert cs +*/ +bool SPIDevice::acquire_bus(bool set, bool skip_cs) { bus.semaphore.assert_owner(); if (set && cs_forced) { return true; } if (!set && !cs_forced) { - return false; + return true; } if (!set && cs_forced) { - spiUnselectI(spi_devices[device_desc.bus].driver); /* Slave Select de-assertion. */ + if(!skip_cs) { + spiUnselectI(spi_devices[device_desc.bus].driver); /* Slave Select de-assertion. */ + } spiReleaseBus(spi_devices[device_desc.bus].driver); /* Ownership release. */ cs_forced = false; bus.dma_handle->unlock(); @@ -255,12 +261,21 @@ bool SPIDevice::set_chip_select(bool set) } spiStart(spi_devices[device_desc.bus].driver, &bus.spicfg); /* Setup transfer parameters. */ bus.spi_started = true; - spiSelectI(spi_devices[device_desc.bus].driver); /* Slave Select assertion. */ + if(!skip_cs) { + spiSelectI(spi_devices[device_desc.bus].driver); /* Slave Select assertion. */ + } cs_forced = true; } return true; } +/* + allow for control of SPI chip select pin + */ +bool SPIDevice::set_chip_select(bool set) { + return acquire_bus(set, false); +} + /* return a SPIDevice given a string device name */ @@ -303,6 +318,51 @@ 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 /* diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h index 3ca3c9123d..20bcc5bdab 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.h +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h @@ -88,11 +88,24 @@ public: bool set_chip_select(bool set) override; + bool acquire_bus(bool acquire, bool skip_cs); + + SPIDriver * get_driver(); + #ifdef HAL_SPI_CHECK_CLOCK_FREQ // used to measure clock frequencies 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/hwdef/common/spi_hook.h b/libraries/AP_HAL_ChibiOS/hwdef/common/spi_hook.h new file mode 100644 index 0000000000..a1971ba5f6 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/spi_hook.h @@ -0,0 +1,21 @@ +#pragma once + +#include "hal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if HAL_USE_MMC_SPI == TRUE +void spiStartHook(SPIDriver *spip, const SPIConfig *config); +void spiStopHook(SPIDriver *spip); +void spiSelectHook(SPIDriver *spip); +void spiUnselectHook(SPIDriver *spip); +void spiIgnoreHook(SPIDriver *spip,size_t n); +void spiSendHook(SPIDriver *spip,size_t n, const void *txbuf); +void spiReceiveHook(SPIDriver *spip, size_t n, void *rxbuf); +#endif + +#ifdef __cplusplus +} +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat index b4b7aac140..51cc790485 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat @@ -90,7 +90,7 @@ PC9 SBUS_INVERT_TX OUTPUT LOW # SPI Device table SPIDEV mpu6000 SPI1 DEVID1 MPU6000_CS MODE3 1*MHZ 8*MHZ -SPIDEV sdcard SPI2 DEVID2 SDCARD_CS MODE3 1*MHZ 16*MHZ +SPIDEV sdcard SPI2 DEVID2 SDCARD_CS MODE0 400*KHZ 25*MHZ SPIDEV bmp280 SPI3 DEVID3 BMP280_CS MODE3 1*MHZ 8*MHZ SPIDEV osd SPI3 DEVID4 OSD_CS MODE0 1*MHZ 4*MHZ @@ -109,6 +109,10 @@ define HAL_COMPASS_HMC5843_I2C_ADDR 0x1E define STORAGE_FLASH_PAGE 1 define HAL_STORAGE_SIZE 8192 +define HAL_OS_FATFS_IO 1 +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + # define default battery setup define HAL_BATT_VOLT_PIN 12 define HAL_BATT_CURR_PIN 11 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 9dbcd9378c..2c8ac18691 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -311,6 +311,13 @@ def enable_can(f): f.write('#define HAL_WITH_UAVCAN 1\n') env_vars['HAL_WITH_UAVCAN'] = '1' +def has_sdcard_spi(): + '''check for sdcard connected to spi bus''' + for dev in spidev: + if(dev[0] == 'sdcard'): + return True + return False + def write_mcu_config(f): '''write MCU config defines''' f.write('// MCU type (ChibiOS define)\n') @@ -328,6 +335,13 @@ def write_mcu_config(f): f.write('#define USE_POSIX\n\n') f.write('#define HAL_USE_SDC TRUE\n') build_flags.append('USE_FATFS=yes') + elif has_sdcard_spi(): + f.write('// MMC via SPI available, enable POSIX filesystem support\n') + f.write('#define USE_POSIX\n\n') + f.write('#define HAL_USE_MMC_SPI TRUE\n') + f.write('#define HAL_USE_SDC FALSE\n') + f.write('#define HAL_SDCARD_SPI_HOOK TRUE\n') + build_flags.append('USE_FATFS=yes') else: f.write('#define HAL_USE_SDC FALSE\n') build_flags.append('USE_FATFS=no') diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp new file mode 100644 index 0000000000..a283948ff1 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -0,0 +1,112 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ + +#include "SPIDevice.h" +#include "sdcard.h" +#include "hwdef/common/spi_hook.h" + +#ifdef USE_POSIX +static FATFS SDC_FS; // FATFS object +#endif + +#if HAL_USE_MMC_SPI +MMCDriver MMCD1; +static AP_HAL::OwnPtr device; +static MMCConfig mmcconfig; +static SPIConfig lowspeed; +static SPIConfig highspeed; +#endif + +void sdcard_init() { + +#ifdef USE_POSIX +#if HAL_USE_SDC + sdcStart(&SDCD1, NULL); + + if(sdcConnect(&SDCD1) == HAL_FAILED) { + printf("Err: Failed to initialize SDIO!\n"); + } else { + if (f_mount(&SDC_FS, "/", 1) != FR_OK) { + printf("Err: Failed to mount SD Card!\n"); + sdcDisconnect(&SDCD1); + } else { + printf("Successfully mounted SDCard..\n"); + } + //Create APM Directory + mkdir("/APM", 0777); + } +#endif +#if HAL_USE_MMC_SPI + device = AP_HAL::get_HAL().spi->get_device("sdcard"); + + mmcObjectInit(&MMCD1); + + mmcconfig.spip = + static_cast(device.get())->get_driver(); + mmcconfig.hscfg = &highspeed; + mmcconfig.lscfg = &lowspeed; + + mmcStart(&MMCD1, &mmcconfig); + + if (mmcConnect(&MMCD1) == HAL_FAILED) { + printf("Err: Failed to initialize SDCARD_SPI!\n"); + } else { + if (f_mount(&SDC_FS, "/", 1) != FR_OK) { + printf("Err: Failed to mount SD Card!\n"); + mmcDisconnect(&MMCD1); + } else { + printf("Successfully mounted SDCard..\n"); + } + //Create APM Directory + mkdir("/APM", 0777); + } +#endif +#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); +} + +void spiStopHook(SPIDriver *spip) { +} + +void spiSelectHook(SPIDriver *spip) { + static_cast(device.get())->spi_select(); +} + +void spiUnselectHook(SPIDriver *spip) { + static_cast(device.get())->spi_unselect(); +} + +void spiIgnoreHook(SPIDriver *spip, size_t n) { + static_cast(device.get())->spi_ignore(n); +} + +void spiSendHook(SPIDriver *spip, size_t n, const void *txbuf) { + static_cast(device.get())->spi_send(n, txbuf); +} + +void spiReceiveHook(SPIDriver *spip, size_t n, void *rxbuf) { + static_cast(device.get())->spi_receive(n, rxbuf); +} + +#endif + diff --git a/libraries/AP_HAL_ChibiOS/sdcard.h b/libraries/AP_HAL_ChibiOS/sdcard.h new file mode 100644 index 0000000000..2171c5b8fd --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/sdcard.h @@ -0,0 +1,18 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#pragma once + +void sdcard_init();