Line data Source code
1 : /*
2 : * Copyright (C) 2015-2016 Intel Corporation. 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 <algorithm>
20 : #include <assert.h>
21 : #include <dirent.h>
22 : #include <errno.h>
23 : #include <fcntl.h>
24 : #include <limits.h>
25 : #include <linux/i2c-dev.h>
26 : /*
27 : * linux/i2c-dev.h is a kernel header, but some distros rename it to
28 : * linux/i2c-dev.h.kernel when i2c-tools is installed. The header provided by
29 : * i2c-tools is old/broken and contains some symbols defined in
30 : * linux/i2c.h. The i2c.h will be only included if a well-known symbol is not
31 : * defined. This is a workaround while distros propagate the real fix like
32 : * http://lists.opensuse.org/archive/opensuse-commit/2015-10/msg00918.html (or
33 : * do like Archlinux that installs only the kernel header).
34 : */
35 : #ifndef I2C_SMBUS_BLOCK_MAX
36 : #include <linux/i2c.h>
37 : #endif
38 : #include <stdio.h>
39 : #include <stdlib.h>
40 : #include <sys/ioctl.h>
41 : #include <sys/stat.h>
42 : #include <unistd.h>
43 : #include <vector>
44 :
45 : #include <AP_HAL/AP_HAL.h>
46 : #include <AP_Math/AP_Math.h>
47 :
48 : #include "PollerThread.h"
49 : #include "Scheduler.h"
50 : #include "Semaphores.h"
51 : #include "Thread.h"
52 : #include "Util.h"
53 :
54 : /* Workaround broken header from i2c-tools */
55 : #ifndef I2C_RDRW_IOCTL_MAX_MSGS
56 : #define I2C_RDRW_IOCTL_MAX_MSGS 42
57 : #endif
58 :
59 : extern const AP_HAL::HAL& hal;
60 :
61 : namespace Linux {
62 :
63 : /*
64 : * TODO: move to Util or other upper class to be used by others
65 : *
66 : * Return pointer to the next char if @s starts with @prefix, otherwise
67 : * returns nullptr.
68 : */
69 : static inline char *startswith(const char *s, const char *prefix)
70 : {
71 : size_t len = strlen(prefix);
72 : if (strncmp(s, prefix, len) == 0) {
73 : return (char *) s + len;
74 : }
75 : return nullptr;
76 : }
77 :
78 : /* Private struct to maintain for each bus */
79 : class I2CBus : public TimerPollable::WrapperCb {
80 : public:
81 : ~I2CBus();
82 :
83 : /*
84 : * TimerPollable::WrapperCb methods to take
85 : * and release semaphore while calling the callback
86 : */
87 : void start_cb() override;
88 : void end_cb() override;
89 :
90 : int open(uint8_t n);
91 :
92 : PollerThread thread;
93 : Semaphore sem;
94 : int fd = -1;
95 : uint8_t bus;
96 : uint8_t ref;
97 : };
98 :
99 0 : I2CBus::~I2CBus()
100 : {
101 0 : if (fd >= 0) {
102 0 : ::close(fd);
103 : }
104 0 : }
105 :
106 0 : void I2CBus::start_cb()
107 : {
108 0 : sem.take_blocking();
109 0 : }
110 :
111 0 : void I2CBus::end_cb()
112 : {
113 0 : sem.give();
114 0 : }
115 :
116 0 : int I2CBus::open(uint8_t n)
117 : {
118 : char path[sizeof("/dev/i2c-XXX")];
119 : int r;
120 :
121 0 : if (fd >= 0) {
122 0 : return -EBUSY;
123 : }
124 :
125 0 : r = snprintf(path, sizeof(path), "/dev/i2c-%u", n);
126 0 : if (r < 0 || r >= (int)sizeof(path)) {
127 0 : return -EINVAL;
128 : }
129 :
130 0 : fd = ::open(path, O_RDWR | O_CLOEXEC);
131 0 : if (fd < 0) {
132 0 : return -errno;
133 : }
134 :
135 0 : bus = n;
136 :
137 0 : return fd;
138 : }
139 :
140 0 : I2CDevice::I2CDevice(I2CBus &bus, uint8_t address)
141 0 : : _bus(bus)
142 0 : , _address(address)
143 : {
144 0 : set_device_bus(bus.bus);
145 0 : set_device_address(address);
146 0 : }
147 :
148 0 : I2CDevice::~I2CDevice()
149 : {
150 : // Unregister itself from the I2CDeviceManager
151 0 : I2CDeviceManager::from(hal.i2c_mgr)->_unregister(_bus);
152 0 : }
153 :
154 0 : bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
155 : uint8_t *recv, uint32_t recv_len)
156 : {
157 0 : if (_split_transfers && send_len > 0 && recv_len > 0) {
158 0 : return transfer(send, send_len, nullptr, 0) &&
159 0 : transfer(nullptr, 0, recv, recv_len);
160 : }
161 :
162 0 : struct i2c_msg msgs[2] = { };
163 0 : unsigned nmsgs = 0;
164 :
165 0 : if (send && send_len != 0) {
166 0 : msgs[nmsgs].addr = _address;
167 0 : msgs[nmsgs].flags = 0;
168 0 : msgs[nmsgs].buf = const_cast<uint8_t*>(send);
169 0 : msgs[nmsgs].len = send_len;
170 0 : nmsgs++;
171 : }
172 :
173 0 : if (recv && recv_len != 0) {
174 0 : msgs[nmsgs].addr = _address;
175 0 : msgs[nmsgs].flags = I2C_M_RD;
176 0 : msgs[nmsgs].buf = recv;
177 0 : msgs[nmsgs].len = recv_len;
178 0 : nmsgs++;
179 : }
180 :
181 : /* interpret it as an input error if nothing has to be done */
182 0 : if (!nmsgs) {
183 0 : return false;
184 : }
185 :
186 0 : struct i2c_rdwr_ioctl_data i2c_data = { };
187 :
188 0 : i2c_data.msgs = msgs;
189 0 : i2c_data.nmsgs = nmsgs;
190 :
191 : int r;
192 0 : unsigned retries = _retries;
193 : do {
194 0 : r = ::ioctl(_bus.fd, I2C_RDWR, &i2c_data);
195 0 : } while (r == -1 && retries-- > 0);
196 :
197 0 : return r != -1;
198 : }
199 :
200 0 : bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
201 : uint32_t recv_len, uint8_t times)
202 : {
203 0 : const uint8_t max_times = I2C_RDRW_IOCTL_MAX_MSGS / 2;
204 :
205 0 : first_reg |= _read_flag;
206 :
207 0 : while (times > 0) {
208 0 : uint8_t n = MIN(times, max_times);
209 0 : struct i2c_msg msgs[2 * n];
210 0 : struct i2c_rdwr_ioctl_data i2c_data = { };
211 :
212 0 : memset(msgs, 0, 2 * n * sizeof(*msgs));
213 :
214 0 : i2c_data.msgs = msgs;
215 0 : i2c_data.nmsgs = 2 * n;
216 :
217 0 : for (uint8_t i = 0; i < i2c_data.nmsgs; i += 2) {
218 0 : msgs[i].addr = _address;
219 0 : msgs[i].flags = 0;
220 0 : msgs[i].buf = &first_reg;
221 0 : msgs[i].len = 1;
222 0 : msgs[i + 1].addr = _address;
223 0 : msgs[i + 1].flags = I2C_M_RD;
224 0 : msgs[i + 1].buf = recv;
225 0 : msgs[i + 1].len = recv_len;
226 :
227 0 : recv += recv_len;
228 : };
229 :
230 : int r;
231 0 : unsigned retries = _retries;
232 : do {
233 0 : r = ::ioctl(_bus.fd, I2C_RDWR, &i2c_data);
234 0 : } while (r == -1 && retries-- > 0);
235 :
236 0 : if (r == -1) {
237 0 : return false;
238 : }
239 :
240 0 : times -= n;
241 0 : }
242 :
243 0 : return true;
244 : }
245 :
246 0 : AP_HAL::Semaphore *I2CDevice::get_semaphore()
247 : {
248 0 : return &_bus.sem;
249 : }
250 :
251 0 : AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(
252 : uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
253 : {
254 0 : TimerPollable *p = _bus.thread.add_timer(cb, &_bus, period_usec);
255 0 : if (!p) {
256 0 : AP_HAL::panic("Could not create periodic callback");
257 : }
258 :
259 0 : if (!_bus.thread.is_started()) {
260 : char name[16];
261 0 : snprintf(name, sizeof(name), "ap-i2c-%u", _bus.bus);
262 :
263 0 : _bus.thread.set_stack_size(AP_LINUX_SENSORS_STACK_SIZE);
264 0 : _bus.thread.start(name, AP_LINUX_SENSORS_SCHED_POLICY,
265 : AP_LINUX_SENSORS_SCHED_PRIO);
266 : }
267 :
268 0 : return static_cast<AP_HAL::Device::PeriodicHandle>(p);
269 : }
270 :
271 0 : bool I2CDevice::adjust_periodic_callback(
272 : AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
273 : {
274 0 : return _bus.thread.adjust_timer(static_cast<TimerPollable*>(h), period_usec);
275 : }
276 :
277 48 : I2CDeviceManager::I2CDeviceManager()
278 : {
279 : /* Reserve space up-front for 4 buses */
280 48 : _buses.reserve(4);
281 48 : }
282 :
283 : AP_HAL::I2CDevice *
284 0 : I2CDeviceManager::get_device_ptr(uint8_t bus, uint8_t address,
285 : uint32_t bus_clock,
286 : bool use_smbus,
287 : uint32_t timeout_ms)
288 : {
289 0 : for (uint8_t i = 0, n = _buses.size(); i < n; i++) {
290 0 : if (_buses[i]->bus == bus) {
291 0 : return _create_device(*_buses[i], address);
292 : }
293 : }
294 :
295 : /* Bus not found for this device, create a new one */
296 0 : AP_HAL::OwnPtr<I2CBus> b{new I2CBus()};
297 0 : if (!b) {
298 0 : return nullptr;
299 : }
300 :
301 0 : if (b->open(bus) < 0) {
302 0 : return nullptr;
303 : }
304 :
305 0 : auto dev = _create_device(*b, address);
306 0 : if (!dev) {
307 0 : return nullptr;
308 : }
309 :
310 0 : _buses.push_back(b.leak());
311 :
312 0 : return dev;
313 0 : }
314 :
315 : /* Create a new device increasing the bus reference */
316 : AP_HAL::I2CDevice *
317 0 : I2CDeviceManager::_create_device(I2CBus &b, uint8_t address) const
318 : {
319 0 : auto *dev = NEW_NOTHROW I2CDevice(b, address);
320 0 : if (!dev) {
321 0 : return nullptr;
322 : }
323 0 : b.ref++;
324 0 : return dev;
325 : }
326 :
327 0 : void I2CDeviceManager::_unregister(I2CBus &b)
328 : {
329 0 : if (--b.ref > 0) {
330 0 : return;
331 : }
332 :
333 0 : for (auto it = _buses.begin(); it != _buses.end(); it++) {
334 0 : if ((*it)->bus == b.bus) {
335 0 : _buses.erase(it);
336 0 : delete &b;
337 0 : break;
338 : }
339 : }
340 : }
341 :
342 0 : void I2CDeviceManager::teardown()
343 : {
344 0 : for (auto it = _buses.begin(); it != _buses.end(); it++) {
345 : /* Try to stop thread - it may not even be started yet */
346 0 : (*it)->thread.stop();
347 : }
348 :
349 0 : for (auto it = _buses.begin(); it != _buses.end(); it++) {
350 : /* Try to join thread - failing is normal if thread was not started */
351 0 : (*it)->thread.join();
352 : }
353 0 : }
354 :
355 : /*
356 : get mask of bus numbers for all configured I2C buses
357 : */
358 0 : uint32_t I2CDeviceManager::get_bus_mask(void) const
359 : {
360 0 : return HAL_LINUX_I2C_BUS_MASK;
361 : }
362 :
363 : /*
364 : get mask of bus numbers for all configured internal I2C buses
365 : */
366 0 : uint32_t I2CDeviceManager::get_bus_mask_internal(void) const
367 : {
368 0 : return HAL_LINUX_I2C_INTERNAL_BUS_MASK;
369 : }
370 :
371 : /*
372 : get mask of bus numbers for all configured external I2C buses
373 : */
374 0 : uint32_t I2CDeviceManager::get_bus_mask_external(void) const
375 : {
376 0 : return HAL_LINUX_I2C_EXTERNAL_BUS_MASK;
377 : }
378 :
379 : }
|
