diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index f9e36db9bc..d00f473f5a 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -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 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 || \ diff --git a/libraries/AP_HAL_Linux/I2CDriver.cpp b/libraries/AP_HAL_Linux/I2CDriver.cpp index 7670203175..8d676cc310 100644 --- a/libraries/AP_HAL_Linux/I2CDriver.cpp +++ b/libraries/AP_HAL_Linux/I2CDriver.cpp @@ -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/'. 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 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() */ 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; - } - } - _addr = addr; + /* nothing to do */ + return true; } -end: + + 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; + return true; } @@ -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) } 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) 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) 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, // 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, 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, 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) } 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, 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 diff --git a/libraries/AP_HAL_Linux/I2CDriver.h b/libraries/AP_HAL_Linux/I2CDriver.h index 727ee418b3..b40ee53e5b 100644 --- a/libraries/AP_HAL_Linux/I2CDriver.h +++ b/libraries/AP_HAL_Linux/I2CDriver.h @@ -2,16 +2,20 @@ #ifndef __AP_HAL_LINUX_I2CDRIVER_H__ #define __AP_HAL_LINUX_I2CDRIVER_H__ +#include + +#include + #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 devpaths); - void begin(); - void end(); + void begin() { } + void end() { } void setTimeout(uint16_t ms); void setHighSpeed(bool active); @@ -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 _fake_dev; char *_device = NULL; - int _fd = -1; uint8_t _addr; bool _print_ioctl_error = true; };