zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_Linux: Add fake device to I2CDriver 

This allows us to re-use I2CDevice from I2CDriver while the drivers are
converted.  We create a fake device with addr = 0 for each I2CDriver but
we only use the register/unregister logic. The transfer logic still uses
the methods from I2CDriver in order to use the right address.

Now we can interoperate I2CDevice drivers with the ones base in
I2CDriver since they are going to use the same semaphore and bus.

The I2CDriver constructors were changed to re-use the logic in I2CDevice
(it uses a number rather than an string) and the semaphore doesn't live
outside anymore, its embedded in the fake I2CDevice, as well as the
bus's file descritor.
mission-4.1.18
Lucas De Marchi 9 years ago
parent
5194f7e5ce
commit
e40785b002
3 changed files with 65 additions and 138 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 27
      libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
  2. 155
      libraries/AP_HAL_Linux/I2CDriver.cpp
  3. 21
      libraries/AP_HAL_Linux/I2CDriver.h

27
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
Unescape View File

@ -45,35 +45,28 @@ static UARTDriver uartEDriver(false); @@ -45,35 +45,28 @@ static UARTDriver uartEDriver(false);
static I2CDeviceManager i2c_mgr_instance;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
static Semaphore i2cSemaphore0;
static I2CDriver i2cDriver0(&i2cSemaphore0, "/dev/i2c-0");
static Semaphore i2cSemaphore1;
static I2CDriver i2cDriver1(&i2cSemaphore1, "/dev/i2c-1");
static Semaphore i2cSemaphore2;
static I2CDriver i2cDriver2(&i2cSemaphore2, "/dev/i2c-2");
static I2CDriver i2cDriver0(0);
static I2CDriver i2cDriver1(1);
static I2CDriver i2cDriver2(2);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
static Semaphore i2cSemaphore0;
static I2CDriver i2cDriver0(&i2cSemaphore0, "/dev/i2c-2");
static I2CDriver i2cDriver0(2);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
static Semaphore i2cSemaphore0;
static const char * const i2c_devpaths[] = {
static const std::vector<const char *> i2c_devpaths({
/* UEFI with lpss set to ACPI */
"/devices/platform/80860F41:05",
/* UEFI with lpss set to PCI */
"/devices/pci0000:00/0000:00:18.6",
NULL
};
static I2CDriver i2cDriver0(&i2cSemaphore0, i2c_devpaths);
});
static I2CDriver i2cDriver0(i2c_devpaths);
/* One additional emulated bus */
static Semaphore i2cSemaphore1;
static I2CDriver i2cDriver1(&i2cSemaphore1, "/dev/i2c-10");
static I2CDriver i2cDriver1(10);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
static Semaphore i2cSemaphore0;
static Empty::I2CDriver i2cDriver0(&i2cSemaphore0);
#else
static Semaphore i2cSemaphore0;
static I2CDriver i2cDriver0(&i2cSemaphore0, "/dev/i2c-1");
static I2CDriver i2cDriver0(1);
#endif
static SPIDeviceManager spiDeviceManager;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \

155
libraries/AP_HAL_Linux/I2CDriver.cpp
Unescape View File

@ -23,93 +23,14 @@ extern const AP_HAL::HAL& hal; @@ -23,93 +23,14 @@ extern const AP_HAL::HAL& hal;
using namespace Linux;
/*
constructor
*/
I2CDriver::I2CDriver(AP_HAL::Semaphore* semaphore, const char *device) :
_semaphore(semaphore)
I2CDriver::I2CDriver(uint8_t bus)
: _fake_dev(I2CDeviceManager::from(hal.i2c_mgr)->get_device(bus, 0))
{
_device = strdup(device);
#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
if (!((Util*)hal.util)->is_chardev_node(_device))
AP_HAL::panic("I2C device is not a chardev node");
#endif
}
/* Match a given device by the prefix its devpath, i.e. the path returned by
* udevadm info -q path /dev/<i2c-device>'. This constructor can be used when
* the number of the I2C bus is not stable across reboots. It matches the
* first device with a prefix in @devpaths */
I2CDriver::I2CDriver(AP_HAL::Semaphore* semaphore,
const char * const devpaths[]) :
_semaphore(semaphore)
I2CDriver::I2CDriver(std::vector<const char *> devpaths)
: _fake_dev(I2CDeviceManager::from(hal.i2c_mgr)->get_device(devpaths, 0))
{
const char *dirname = "/sys/class/i2c-dev";
struct dirent *de;
DIR *d;
d = opendir(dirname);
if (!d)
AP_HAL::panic("Could not get list of I2C buses");
for (de = readdir(d); de; de = readdir(d)) {
const char *p, * const *t;
char buf[PATH_MAX], buf2[PATH_MAX];
if (strcmp(de->d_name, ".") == 0 || strcmp(de->d_name, "..") == 0)
continue;
if (snprintf(buf, sizeof(buf), "%s/%s", dirname, de->d_name) >= PATH_MAX)
continue;
p = realpath(buf, buf2);
if (!p || strncmp(p, "/sys", sizeof("/sys") - 1))
continue;
p += sizeof("/sys") - 1;
for (t = devpaths; t && *t; t++) {
if (strncmp(p, *t, strlen(*t)) == 0)
break;
}
if (!*t)
continue;
/* Found the device name, use the new path */
asprintf(&_device, "/dev/%s", de->d_name);
break;
}
closedir(d);
if (!((Util*)hal.util)->is_chardev_node(_device))
AP_HAL::panic("I2C device is not a chardev node");
}
I2CDriver::~I2CDriver()
{
free(_device);
}
/*
called from HAL class init()
*/
void I2CDriver::begin()
{
if (_fd != -1) {
close(_fd);
}
_fd = open(_device, O_RDWR);
}
void I2CDriver::end()
{
if (_fd != -1) {
::close(_fd);
_fd = -1;
}
}
/*
@ -117,30 +38,34 @@ void I2CDriver::end() @@ -117,30 +38,34 @@ void I2CDriver::end()
*/
bool I2CDriver::set_address(uint8_t addr)
{
if (_fd == -1) {
if (!_fake_dev) {
return false;
}
if (_addr == addr) {
goto end;
} else {
if (ioctl(_fd, I2C_SLAVE, addr) < 0) {
if (errno != EBUSY) {
return false;
}
/* Only print this message once per i2c bus */
if (_print_ioctl_error) {
hal.console->printf("couldn't set i2c slave address: %s\n",
strerror(errno));
hal.console->printf("trying I2C_SLAVE_FORCE\n");
_print_ioctl_error = false;
}
if (ioctl(_fd, I2C_SLAVE_FORCE, addr) < 0) {
return false;
}
/* nothing to do */
return true;
}
int fd = _fake_dev->get_fd();
if (ioctl(fd, I2C_SLAVE, addr) < 0) {
if (errno != EBUSY) {
return false;
}
/* Only print this message once per i2c bus */
if (_print_ioctl_error) {
hal.console->printf("couldn't set i2c slave address: %s\n",
strerror(errno));
hal.console->printf("trying I2C_SLAVE_FORCE\n");
_print_ioctl_error = false;
}
if (ioctl(fd, I2C_SLAVE_FORCE, addr) < 0) {
return false;
}
_addr = addr;
}
end:
_addr = addr;
return true;
}
@ -159,7 +84,7 @@ uint8_t I2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data) @@ -159,7 +84,7 @@ uint8_t I2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data)
if (!set_address(addr)) {
return 1;
}
if (::write(_fd, data, len) != len) {
if (::write(_fake_dev->get_fd(), data, len) != len) {
return 1;
}
return 0; // success
@ -199,8 +124,8 @@ uint8_t I2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val) @@ -199,8 +124,8 @@ uint8_t I2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
}
union i2c_smbus_data data;
data.byte = val;
if (_i2c_smbus_access(_fd,I2C_SMBUS_WRITE, reg,
I2C_SMBUS_BYTE_DATA, &data) == -1) {
if (_i2c_smbus_access(_fake_dev->get_fd(),I2C_SMBUS_WRITE, reg,
I2C_SMBUS_BYTE_DATA, &data) == -1) {
return 1;
}
return 0;
@ -211,7 +136,7 @@ uint8_t I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data) @@ -211,7 +136,7 @@ uint8_t I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
if (!set_address(addr)) {
return 1;
}
if (::read(_fd, data, len) != len) {
if (::read(_fake_dev->get_fd(), data, len) != len) {
return 1;
}
return 0;
@ -220,7 +145,7 @@ uint8_t I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data) @@ -220,7 +145,7 @@ uint8_t I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
uint8_t I2CDriver::readRegisters(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t* data)
{
if (_fd == -1) {
if (!_fake_dev) {
return 1;
}
struct i2c_msg msgs[] = {
@ -245,7 +170,7 @@ uint8_t I2CDriver::readRegisters(uint8_t addr, uint8_t reg, @@ -245,7 +170,7 @@ uint8_t I2CDriver::readRegisters(uint8_t addr, uint8_t reg,
// prevent valgrind error
memset(data, 0, len);
if (ioctl(_fd, I2C_RDWR, &i2c_data) == -1) {
if (ioctl(_fake_dev->get_fd(), I2C_RDWR, &i2c_data) == -1) {
return 1;
}
@ -263,7 +188,7 @@ uint8_t I2CDriver::readRegistersMultiple(uint8_t addr, uint8_t reg, @@ -263,7 +188,7 @@ uint8_t I2CDriver::readRegistersMultiple(uint8_t addr, uint8_t reg,
const uint8_t max_count = 8;
#endif
if (_fd == -1) {
if (!_fake_dev) {
return 1;
}
while (count > 0) {
@ -284,7 +209,7 @@ uint8_t I2CDriver::readRegistersMultiple(uint8_t addr, uint8_t reg, @@ -284,7 +209,7 @@ uint8_t I2CDriver::readRegistersMultiple(uint8_t addr, uint8_t reg,
msgs[i*2+1].buf = (typeof(msgs->buf))data;
data += len;
};
if (ioctl(_fd, I2C_RDWR, &i2c_data) == -1) {
if (ioctl(_fake_dev->get_fd(), I2C_RDWR, &i2c_data) == -1) {
return 1;
}
count -= n;
@ -300,7 +225,7 @@ uint8_t I2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data) @@ -300,7 +225,7 @@ uint8_t I2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data)
}
union i2c_smbus_data v;
memset(&v, 0, sizeof(v));
if (_i2c_smbus_access(_fd,I2C_SMBUS_READ, reg,
if (_i2c_smbus_access(_fake_dev->get_fd(),I2C_SMBUS_READ, reg,
I2C_SMBUS_BYTE_DATA, &v)) {
return 1;
}
@ -346,11 +271,17 @@ bool I2CDriver::do_transfer(uint8_t addr, const uint8_t *send, @@ -346,11 +271,17 @@ bool I2CDriver::do_transfer(uint8_t addr, const uint8_t *send,
else {
return false;
}
return ioctl(_fd, I2C_RDWR, &msg_rdwr) == (int)msg_rdwr.nmsgs;
return ioctl(_fake_dev->get_fd(), I2C_RDWR, &msg_rdwr) == (int)msg_rdwr.nmsgs;
}
uint8_t I2CDriver::lockup_count()
{
return 0;
}
AP_HAL::Semaphore *I2CDriver::get_semaphore()
{
return _fake_dev->get_semaphore();
}
#endif // CONFIG_HAL_BOARD

21
libraries/AP_HAL_Linux/I2CDriver.h
Unescape View File

@ -2,16 +2,20 @@ @@ -2,16 +2,20 @@
#ifndef __AP_HAL_LINUX_I2CDRIVER_H__
#define __AP_HAL_LINUX_I2CDRIVER_H__
#include <vector>
#include <AP_HAL/utility/OwnPtr.h>
#include "AP_HAL_Linux.h"
#include "I2CDevice.h"
class Linux::I2CDriver : public AP_HAL::I2CDriver {
public:
I2CDriver(AP_HAL::Semaphore* semaphore, const char *device);
I2CDriver(AP_HAL::Semaphore* semaphore, const char * const devpaths[]);
~I2CDriver();
I2CDriver(uint8_t bus);
I2CDriver(std::vector<const char *> devpaths);
void begin();
void end();
void begin() { }
void end() { }
void setTimeout(uint16_t ms);
void setHighSpeed(bool active);
@ -40,16 +44,15 @@ public: @@ -40,16 +44,15 @@ public:
uint8_t lockup_count();
AP_HAL::Semaphore* get_semaphore() { return _semaphore; }
AP_HAL::Semaphore *get_semaphore();
bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)override;
private:
bool set_address(uint8_t addr);
AP_HAL::Semaphore* _semaphore;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _fake_dev;
char *_device = NULL;
int _fd = -1;
uint8_t _addr;
bool _print_ioctl_error = true;
};

Write Preview
Loading…
Cancel
Save