APM:Libraries
SPIDevice.h
Go to the documentation of this file.
1 /*
3 
4 (c) 2017 night_ghost@ykoctpa.ru
5 
6 based on:
7 
8  * Copyright (C) 2015 Intel Corporation. All rights reserved.
9  *
10  * This file is free software: you can redistribute it and/or modify it
11  * under the terms of the GNU General Public License as published by the
12  * Free Software Foundation, either version 3 of the License, or
13  * (at your option) any later version.
14  *
15  * This file is distributed in the hope that it will be useful, but
16  * WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
18  * See the GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License along
21  * with this program. If not, see <http://www.gnu.org/licenses/>.
22  */
23 #pragma once
24 
25 
26 #include <AP_HAL/HAL.h>
27 #include "Semaphores.h"
28 #include "Scheduler.h"
29 #include <dma.h>
30 #include <spi.h>
31 
32 namespace F4Light {
33 
34 #define MAX_BUS_NUM 3
35 
36 typedef enum SPIFrequency {
37  SPI_18MHZ = 0,
38  SPI_9MHZ = 1,
39  SPI_4_5MHZ = 2,
45  SPI_36MHZ = 8,
46 } SPIFrequency;
47 
48 typedef uint8_t (*spi_WaitFunc)(uint8_t b);
49 
50 struct spi_pins {
51  uint8_t sck;
52  uint8_t miso;
53  uint8_t mosi;
54 };
55 
56 typedef enum SPI_TRANSFER_MODE {
62 
63 typedef struct SPIDESC {
64  const char * const name;
65  const spi_dev * const dev;
66  uint8_t bus;
68  uint16_t cs_pin;
71  SPI_transferMode mode; // mode of operations: 0 - polling, 1&2 DMA, 3 interrupts, 4 software
72  uint32_t prio; // DMA priority
73  uint8_t assert_dly; // delay after CS goes low
74  uint8_t release_dly; // delay before CS goes high
75 } SPIDesc;
76 
80  SPI_ISR_SEND_DMA, // want DMA but can't
81  SPI_ISR_SKIP_RX, // wait for 1 dummy byte
83  SPI_ISR_WAIT_RX, // wait for receiving of last fake byte from TX
84  SPI_ISR_WAIT_RX_DMA, // wait for receiving of last fake byte from TX
86  SPI_ISR_RXTX, // full duplex
90 };
91 
92 //#define DEBUG_SPI
93 
94 #ifdef DEBUG_SPI
95 // for debug
96 typedef struct SPI_TRANS {
97  const spi_dev * dev;
98  uint32_t time;
99  enum SPI_ISR_MODE mode;
100  uint8_t act;
101  uint8_t data;
102  uint32_t cr2;
103  uint32_t sr1;
104  uint16_t send_len;
105  uint16_t recv_len;
106  uint16_t dummy_len;
107  spi_WaitFunc cb;
108 } spi_trans;
109 
110 #define SPI_LOG_SIZE 200
111 #endif
112 
113 class SPIDevice : public AP_HAL::SPIDevice {
114 public:
115  SPIDevice(const SPIDesc &device_desc);
116 
118 
119  /* AP_HAL::SPIDevice implementation */
120 
121  bool set_speed(AP_HAL::Device::Speed speed) override;
122 
123  bool transfer(const uint8_t *send, uint32_t send_len,
124  uint8_t *recv, uint32_t recv_len) override;
125 
126  // one byte
127  uint8_t transfer(uint8_t out);
128  void send(uint8_t out); // without wait for answer
129 
130  /* See AP_HAL::SPIDevice::transfer_fullduplex() */
131  bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) override;
132 
133  /* single-byte transfers don't do it */
134  inline void apply_speed() {
135  spi_set_speed(_desc.dev, determine_baud_rate(_speed));
136  }
137 
138  uint16_t send_strobe(const uint8_t *buffer, uint16_t len); // send in ISR and strobe each byte by CS
139  void wait_busy() { spi_wait_busy(_desc.dev); }
140  uint8_t wait_for(uint8_t out, spi_WaitFunc cb, uint32_t dly); // wait for needed byte in ISR
141 
142  /* See AP_HAL::Device::get_semaphore() */
143  inline F4Light::Semaphore *get_semaphore() { uint8_t n = _desc.bus - 1; if(n<MAX_BUS_NUM) { return &_semaphores[n];} else return NULL; } // numbers from 1
144 
145  /* See AP_HAL::Device::register_periodic_callback() */
146  inline AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb proc) override
147  {
148  return Scheduler::register_timer_task(period_usec, proc, get_semaphore() );
149  }
150 
151  inline bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override
152  {
153  return Scheduler::adjust_timer_task(h, period_usec);
154  }
155 
157 
158  void register_completion_callback(Handler h);
159 
160  inline void register_completion_callback(AP_HAL::MemberProc proc){
161  Revo_handler r = { .mp=proc };
162  register_completion_callback(r.h);
163  }
164 
166  Revo_handler r = { .hp=proc };
167  register_completion_callback(r.h);
168  }
169 
170  void dma_isr();
171  void spi_isr();
172 
173 #define SPI_BUFFER_SIZE 512 // one SD-card sector
174 
175 protected:
176  const SPIDesc &_desc;
177 
180 
181  static F4Light::Semaphore _semaphores[MAX_BUS_NUM]; // per bus
182  static void * owner[MAX_BUS_NUM];
183  static uint8_t *buffer[MAX_BUS_NUM]; // array of pointers, allocate on 1st use
184 
186  uint8_t byte_time; // in 0.25uS
187 
188  void init(void);
189 
190  inline void _cs_assert(){ if(_cs){_cs->write(0); delay_ns100(_desc.assert_dly); } } // Select device and wait a little
191  inline void _cs_release(){ spi_wait_busy(_desc.dev); if(_cs){ delay_ns100(_desc.release_dly); _cs->write(1); } } // Deselect device after some delay
192 
193  const spi_pins* dev_to_spi_pins(const spi_dev *dev);
194 
195  spi_baud_rate determine_baud_rate(SPIFrequency freq);
196 
197  uint8_t _transfer(uint8_t data);
198 
199  void get_dma_ready();
200 
201  void setup_dma_transfer(const uint8_t *send, const uint8_t *recv, uint32_t btr );
202  void setup_isr_transfer();
203 
204  void start_dma_transfer();
205  uint8_t do_transfer(bool is_DMA, uint32_t nbytes);
206 
208  void *_task;
209 
210 
211  // vars for send_strobe() and wait_for()
212  const uint8_t *_send_address;
213  uint16_t _send_len;
214  uint16_t _dummy_len;
215  uint8_t *_recv_address;
216  uint16_t _recv_len;
217 
220  uint8_t _recv_data;
221 
222  void isr_transfer_finish();
223  void disable_dma();
224 
225 #ifdef BOARD_SOFTWARE_SPI
226  volatile GPIO_TypeDef *sck_port;
227  uint16_t sck_pin;
228 
229  volatile GPIO_TypeDef *mosi_port;
230  uint16_t mosi_pin;
231 
232  volatile GPIO_TypeDef *miso_port;
233  uint16_t miso_pin;
234 
235  uint16_t dly_time;
236  void spi_soft_set_speed();
237  uint8_t _transfer_s(uint8_t bt);
238 
239 #endif
240 
241 #ifdef DEBUG_SPI
242  static spi_trans spi_trans_array[SPI_LOG_SIZE];
243  static uint8_t spi_trans_ptr;
244 #endif
245 
246 };
247 
249 public:
250  friend class SPIDevice;
251 
253 
254  AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name) { return _get_device(name); }
255  static AP_HAL::OwnPtr<F4Light::SPIDevice> _get_device(const char *name);
256 
257 protected:
258 
259 };
260 
261 } // namespace
const spi_dev *const dev
Definition: SPIDevice.h:65
AP_HAL::MemberProc mp
Definition: handler.h:18
F4Light::Semaphore * get_semaphore()
Definition: SPIDevice.h:143
struct F4Light::SPIDESC SPIDesc
Handler h
Definition: handler.h:20
void register_completion_callback(AP_HAL::Proc proc)
Definition: SPIDevice.h:165
void write(uint8_t value)
Definition: GPIO.h:113
uint8_t * _recv_address
Definition: SPIDevice.h:215
uint16_t _dummy_len
Definition: SPIDevice.h:214
uint16_t _recv_len
Definition: SPIDevice.h:216
DigitalSource * _cs
Definition: SPIDevice.h:178
static uint8_t buffer[SRXL_FRAMELEN_MAX]
Definition: srxl.cpp:56
enum F4Light::SPI_TRANSFER_MODE SPI_transferMode
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override
Definition: SPIDevice.h:151
uint16_t _send_len
Definition: SPIDevice.h:213
void spi_set_speed(const spi_dev *dev, uint16_t baudPrescaler)
Definition: spi.c:205
SPIFrequency lowspeed
Definition: SPIDevice.h:69
SPI_ISR_MODE _isr_mode
Definition: SPIDevice.h:218
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb proc) override
Definition: SPIDevice.h:146
#define MAX_BUS_NUM
Definition: SPIDevice.h:34
static void spi_wait_busy(const spi_dev *dev)
Definition: spi.h:275
void register_completion_callback(AP_HAL::MemberProc proc)
Definition: SPIDevice.h:160
void(* Proc)(void)
SPI_ISR_MODE
Definition: SPIDevice.h:77
spi_WaitFunc _compare_cb
Definition: SPIDevice.h:219
SPI_TRANSFER_MODE
Definition: SPIDevice.h:56
const char * name
Definition: BusTest.cpp:11
uint8_t(* spi_WaitFunc)(uint8_t b)
Definition: SPIDevice.h:48
const SPIDesc & _desc
Definition: SPIDevice.h:176
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
const uint8_t * _send_address
Definition: SPIDevice.h:212
SPIFrequency
Definition: SPIDevice.h:36
bool unregister_callback(PeriodicHandle h)
Definition: SPIDevice.h:156
uint8_t release_dly
Definition: SPIDevice.h:74
SPI_transferMode mode
Definition: SPIDevice.h:71
spi_mode
SPI mode configuration.
Definition: spi.h:54
uint16_t cs_pin
Definition: SPIDevice.h:68
static AP_HAL::OwnPtr< AP_HAL::Device > dev
Definition: ICM20789.cpp:16
uint32_t prio
Definition: SPIDevice.h:72
static AP_HAL::Device::PeriodicHandle register_timer_task(uint32_t period_us, AP_HAL::Device::PeriodicCb proc, F4Light::Semaphore *sem)
Definition: Scheduler.h:218
const char *const name
Definition: SPIDevice.h:64
void * PeriodicHandle
Definition: Device.h:42
uint8_t assert_dly
Definition: SPIDevice.h:73
AP_HAL::OwnPtr< AP_HAL::SPIDevice > get_device(const char *name)
Definition: SPIDevice.h:254
#define NULL
Definition: hal_types.h:59
static bool adjust_timer_task(AP_HAL::Device::PeriodicHandle h, uint32_t period_us)
Definition: Scheduler.cpp:707
static bool unregister_timer_task(AP_HAL::Device::PeriodicHandle h)
Definition: Scheduler.cpp:718
void init()
Generic board initialization function.
Definition: system.cpp:136
spi_baud_rate
SPI baud rate configuration, as a divisor of f_PCLK, the PCLK clock frequency.
Definition: spi.h:70
spi_mode sm
Definition: SPIDevice.h:67
SPIFrequency _speed
Definition: SPIDevice.h:179
uint64_t Handler
Definition: hal_types.h:19
static void delay_ns100(uint32_t us)
Definition: delay.h:36
Handler _completion_cb
Definition: SPIDevice.h:207
AP_HAL::Proc hp
Definition: handler.h:16
Definition: spi.h:29
SPIFrequency highspeed
Definition: SPIDevice.h:70
uint8_t _recv_data
Definition: SPIDevice.h:220