123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148 |
- #pragma once
- #include <inttypes.h>
- #include <AP_HAL/HAL.h>
- #include <AP_HAL/SPIDevice.h>
- #include "AP_HAL_ChibiOS.h"
- #if HAL_USE_SPI == TRUE
- #include "Semaphores.h"
- #include "Scheduler.h"
- #include "Device.h"
- namespace ChibiOS {
- class SPIBus : public DeviceBus {
- public:
- SPIBus(uint8_t bus);
- struct spi_dev_s *dev;
- uint8_t bus;
- SPIConfig spicfg;
- void dma_allocate(Shared_DMA *ctx);
- void dma_deallocate(Shared_DMA *ctx);
- bool spi_started;
- uint8_t slowdown;
-
-
-
-
-
- mutex_t dma_lock;
- };
- struct SPIDesc {
- SPIDesc(const char *_name, uint8_t _bus,
- uint8_t _device, ioline_t _pal_line,
- uint32_t _mode, uint32_t _lowspeed, uint32_t _highspeed)
- : name(_name), bus(_bus), device(_device),
- pal_line(_pal_line), mode(_mode),
- lowspeed(_lowspeed), highspeed(_highspeed)
- {
- }
- const char *name;
- uint8_t bus;
- uint8_t device;
- ioline_t pal_line;
- uint32_t mode;
- uint32_t lowspeed;
- uint32_t highspeed;
- };
- class SPIDevice : public AP_HAL::SPIDevice {
- public:
- SPIDevice(SPIBus &_bus, SPIDesc &_device_desc);
- virtual ~SPIDevice();
-
- bool set_speed(AP_HAL::Device::Speed speed) override;
-
- bool transfer(const uint8_t *send, uint32_t send_len,
- uint8_t *recv, uint32_t recv_len) override;
-
- bool transfer_fullduplex(const uint8_t *send, uint8_t *recv,
- uint32_t len) override;
-
- bool clock_pulse(uint32_t len) override;
-
-
- AP_HAL::Semaphore *get_semaphore() override;
-
- AP_HAL::Device::PeriodicHandle register_periodic_callback(
- uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
-
- bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
- bool set_chip_select(bool set) override;
- bool acquire_bus(bool acquire, bool skip_cs);
- SPIDriver * get_driver();
- #ifdef HAL_SPI_CHECK_CLOCK_FREQ
-
- static void test_clock_freq(void);
- #endif
-
- void set_slowdown(uint8_t slowdown) override;
- private:
- SPIBus &bus;
- SPIDesc &device_desc;
- uint32_t frequency;
- uint32_t freq_flag;
- uint32_t freq_flag_low;
- uint32_t freq_flag_high;
- char *pname;
- bool cs_forced;
- static void *spi_thread(void *arg);
- static uint32_t derive_freq_flag_bus(uint8_t busid, uint32_t _frequency);
- uint32_t derive_freq_flag(uint32_t _frequency);
-
- bool do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) WARN_IF_UNUSED;
- };
- class SPIDeviceManager : public AP_HAL::SPIDeviceManager {
- public:
- friend class SPIDevice;
- static SPIDeviceManager *from(AP_HAL::SPIDeviceManager *spi_mgr)
- {
- return static_cast<SPIDeviceManager*>(spi_mgr);
- }
- AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name) override;
- private:
- static SPIDesc device_table[];
- SPIBus *buses;
- };
- }
- #endif
|