Line data Source code
1 : /*
2 : * Copyright (C) 2020 Peter Barker. All rights reserved.
3 : *
4 : * This file is free software: you can redistribute it and/or modify it
5 : * under the terms of the GNU General Public License as published by the
6 : * Free Software Foundation, either version 3 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * This file is distributed in the hope that it will be useful, but
10 : * WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12 : * See the GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License along
15 : * with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 : #include "I2CDevice.h"
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
21 :
22 : #include <SITL/SITL.h>
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 : using namespace HALSITL;
27 :
28 : /*
29 : * I2CBus
30 : */
31 : class I2CBus {
32 : friend class I2CDeviceManager;
33 : public:
34 :
35 8580 : I2CBus() { bus = i2c_buscount++; }
36 :
37 : uint8_t bus;
38 : Semaphore sem;
39 3308705 : int ioctl(uint8_t ioctl_number, void *data) {
40 3308705 : return _ioctl(ioctl_number, data);
41 : }
42 : void _timer_tick(); // in lieu of a thread-per-bus
43 : AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb);
44 :
45 : private:
46 : int _ioctl(uint8_t ioctl_number, void *data);
47 :
48 : struct callback_info {
49 : struct callback_info *next;
50 : AP_HAL::Device::PeriodicCb cb;
51 : uint32_t period_usec;
52 : uint64_t next_usec;
53 : } *callbacks;
54 :
55 : static uint8_t i2c_buscount;
56 :
57 : };
58 :
59 : uint8_t I2CBus::i2c_buscount;
60 :
61 3308705 : int I2CBus::_ioctl(uint8_t ioctl_number, void *data)
62 : {
63 3308705 : SITL::SIM *sitl = AP::sitl();
64 3308705 : return sitl->i2c_ioctl(ioctl_number, data);
65 : }
66 :
67 6397 : AP_HAL::Device::PeriodicHandle I2CBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
68 : {
69 : // mostly swiped from ChibiOS:
70 6397 : I2CBus::callback_info *callback = NEW_NOTHROW I2CBus::callback_info;
71 6397 : if (callback == nullptr) {
72 0 : return nullptr;
73 : }
74 6397 : callback->cb = cb;
75 6397 : callback->period_usec = period_usec;
76 6397 : callback->next_usec = AP_HAL::micros64() + period_usec;
77 :
78 : // add to linked list of callbacks on thread
79 6397 : callback->next = callbacks;
80 6397 : callbacks = callback;
81 6397 : return callback;
82 : }
83 :
84 973431444 : void I2CBus::_timer_tick()
85 : {
86 973431444 : const uint64_t now = AP_HAL::micros64();
87 1717333989 : for (struct callback_info *ci = callbacks; ci != nullptr; ci = ci->next) {
88 743902545 : if (ci->next_usec < now) {
89 23666067 : WITH_SEMAPHORE(sem);
90 23666067 : ci->next_usec += ci->period_usec;
91 23666067 : ci->cb();
92 23666067 : }
93 : }
94 973431444 : }
95 :
96 : /*
97 : * I2CDeviceManager
98 : */
99 :
100 : I2CBus I2CDeviceManager::buses[NUM_SITL_I2C_BUSES] {};
101 :
102 2145 : I2CDeviceManager::I2CDeviceManager()
103 : {
104 10725 : for (uint8_t i=0; i<ARRAY_SIZE(buses); i++) {
105 8580 : buses[i].bus = i;
106 : }
107 2145 : }
108 :
109 : AP_HAL::I2CDevice *
110 48229 : I2CDeviceManager::get_device_ptr(uint8_t bus,
111 : uint8_t address,
112 : uint32_t bus_clock,
113 : bool use_smbus,
114 : uint32_t timeout_ms)
115 : {
116 48229 : if (bus >= ARRAY_SIZE(buses)) {
117 1949 : return nullptr;
118 : }
119 46280 : return NEW_NOTHROW I2CDevice(buses[bus], address);
120 : }
121 :
122 243357861 : void I2CDeviceManager::_timer_tick()
123 : {
124 1216789305 : for (auto &bus : buses) {
125 973431444 : bus._timer_tick();
126 : }
127 243357861 : }
128 :
129 : /*
130 : * I2CDevice
131 : */
132 :
133 46280 : I2CDevice::I2CDevice(I2CBus &bus, uint8_t address)
134 46280 : : _bus(bus)
135 46280 : , _address(address)
136 : {
137 : // ::fprintf(stderr, "bus.bus=%u address=0x%02x\n", bus.bus, address);
138 46280 : set_device_bus(bus.bus);
139 46280 : set_device_address(address);
140 46280 : }
141 :
142 : #include <stdio.h>
143 :
144 : #include <signal.h>
145 :
146 2909129 : bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
147 : uint8_t *recv, uint32_t recv_len)
148 : {
149 2909129 : _bus.sem.check_owner();
150 :
151 : // combined transfer
152 2909129 : return _transfer(send, send_len, recv, recv_len);
153 : }
154 :
155 2909129 : bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
156 : uint8_t *recv, uint32_t recv_len)
157 : {
158 2909129 : struct i2c_msg msgs[2] = { };
159 2909129 : unsigned nmsgs = 0;
160 :
161 2909129 : if (send && send_len != 0) {
162 2811330 : msgs[nmsgs].bus = _bus.bus;
163 2811330 : msgs[nmsgs].addr = _address;
164 2811330 : msgs[nmsgs].flags = 0;
165 2811330 : msgs[nmsgs].buf = const_cast<uint8_t*>(send);
166 2811330 : msgs[nmsgs].len = send_len;
167 2811330 : nmsgs++;
168 : }
169 :
170 2909129 : if (recv && recv_len != 0) {
171 320615 : msgs[nmsgs].bus = _bus.bus;
172 320615 : msgs[nmsgs].addr = _address;
173 320615 : msgs[nmsgs].flags = I2C_M_RD;
174 320615 : msgs[nmsgs].buf = recv;
175 320615 : msgs[nmsgs].len = recv_len;
176 320615 : nmsgs++;
177 : }
178 :
179 : /* interpret it as an input error if nothing has to be done */
180 2909129 : if (!nmsgs) {
181 0 : return false;
182 : }
183 :
184 2909129 : struct i2c_rdwr_ioctl_data i2c_data = { };
185 :
186 2909129 : i2c_data.msgs = msgs;
187 2909129 : i2c_data.nmsgs = nmsgs;
188 :
189 : int r;
190 2909129 : unsigned retries = _retries;
191 : do {
192 3308705 : r = _bus.ioctl(I2C_RDWR, &i2c_data);
193 3308705 : } while (r == -1 && retries-- > 0);
194 :
195 2909129 : return r != -1;
196 : }
197 :
198 0 : bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
199 : uint32_t recv_len, uint8_t times)
200 : {
201 0 : ::fprintf(stderr, "read_registers_multiple called\n");
202 0 : return false;
203 : }
204 :
205 67304 : AP_HAL::Semaphore *I2CDevice::get_semaphore() {
206 67304 : return &_bus.sem;
207 : }
208 :
209 6397 : AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
210 : {
211 6397 : return _bus.register_periodic_callback(period_usec, cb);
212 : }
213 :
214 0 : bool I2CDevice::adjust_periodic_callback(Device::PeriodicHandle h, uint32_t period_usec)
215 : {
216 0 : return false;
217 : }
218 :
219 : #endif //#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
