forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSPIDevice.h
141 lines (111 loc) · 3.94 KB
/
SPIDevice.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
/*
* Copyright (C) 2015 Intel Corporation. All rights reserved.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <inttypes.h>
#include <vector>
#include <AP_HAL/HAL.h>
#include <AP_HAL/SPIDevice.h>
namespace Linux {
class SPIBus;
class SPIDesc;
struct SPIDesc {
SPIDesc(const char *name_, uint16_t bus_, uint16_t subdev_, uint8_t mode_,
uint8_t bits_per_word_, int16_t cs_pin_, uint32_t lowspeed_,
uint32_t highspeed_)
: name(name_), bus(bus_), subdev(subdev_), mode(mode_)
, bits_per_word(bits_per_word_), cs_pin(cs_pin_), lowspeed(lowspeed_)
, highspeed(highspeed_)
{
}
const char *name;
uint16_t bus;
uint16_t subdev;
uint8_t mode;
uint8_t bits_per_word;
int16_t cs_pin;
uint32_t lowspeed;
uint32_t highspeed;
};
class SPIDevice : public AP_HAL::SPIDevice {
public:
SPIDevice(SPIBus &bus, SPIDesc &device_desc);
virtual ~SPIDevice();
/* AP_HAL::SPIDevice implementation */
/* See AP_HAL::Device::set_speed() */
bool set_speed(AP_HAL::Device::Speed speed) override;
/* See AP_HAL::Device::transfer() */
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
/* See AP_HAL::SPIDevice::transfer_fullduplex() */
bool transfer_fullduplex(const uint8_t *send, uint8_t *recv,
uint32_t len) override;
/* See AP_HAL::SPIDevice::transfer_fullduplex() */
bool transfer_fullduplex(uint8_t *send_recv, uint32_t len) override;
/* See AP_HAL::Device::get_semaphore() */
AP_HAL::Semaphore *get_semaphore() override;
/* See AP_HAL::Device::register_periodic_callback() */
AP_HAL::Device::PeriodicHandle register_periodic_callback(
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
/* See AP_HAL::Device::adjust_periodic_callback() */
bool adjust_periodic_callback(
AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
protected:
SPIBus &_bus;
SPIDesc &_desc;
AP_HAL::DigitalSource *_cs;
uint32_t _speed;
/*
* Select device if using userspace CS
*/
void _cs_assert();
/*
* Deselect device if using userspace CS
*/
void _cs_release();
};
class SPIDeviceManager : public AP_HAL::SPIDeviceManager {
public:
friend class SPIDevice;
static SPIDeviceManager *from(AP_HAL::SPIDeviceManager *spi_mgr)
{
return static_cast<SPIDeviceManager*>(spi_mgr);
}
SPIDeviceManager()
{
/* Reserve space up-front for 3 buses */
_buses.reserve(3);
}
/* AP_HAL::SPIDeviceManager implementation */
AP_HAL::SPIDevice *get_device_ptr(const char *name) override;
/*
* Stop all SPI threads and block until they are finalized. This doesn't
* free memory because they can still be used by devices, however device
* drivers won't receive any new event
*/
void teardown();
/* See AP_HAL::SPIDeviceManager::get_count() */
uint8_t get_count() override;
/* See AP_HAL::SPIDeviceManager::get_device_name() */
const char *get_device_name(uint8_t idx) override;
protected:
void _unregister(SPIBus &b);
AP_HAL::SPIDevice *_create_device(SPIBus &b, SPIDesc &device_desc) const;
std::vector<SPIBus*> _buses;
static const uint8_t _n_device_desc;
static SPIDesc _device[];
};
}