diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index 5d8360d8ec..d878d5478a 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -6,7 +6,6 @@ namespace Linux { class RPIOUARTDriver; class I2CDevice; class I2CDeviceManager; - class I2CDriver; class SPIDeviceManager; class SPIDeviceDriver; class Storage; diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index 6cde32cdaf..52b0e142da 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -8,7 +8,6 @@ #include "SPIUARTDriver.h" #include "RPIOUARTDriver.h" #include "I2CDevice.h" -#include "I2CDriver.h" #include "SPIDriver.h" #include "AnalogIn_ADS1115.h" #include "AnalogIn_IIO.h" diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 26c03c52a4..cda750987d 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -45,29 +45,6 @@ static UARTDriver uartFDriver(false); static I2CDeviceManager i2c_mgr_instance; -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO -static I2CDriver i2cDriver0(0); -static I2CDriver i2cDriver1(1); -static I2CDriver i2cDriver2(2); -#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI -static I2CDriver i2cDriver0(2); -#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE -static const std::vector i2c_devpaths({ - /* UEFI with lpss set to ACPI */ - "platform/80860F41:05", - /* UEFI with lpss set to PCI */ - "pci0000:00/0000:00:18.6", -}); -static I2CDriver i2cDriver0(i2c_devpaths); -/* One additional emulated bus */ -static I2CDriver i2cDriver1(10); -#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT -static Semaphore i2cSemaphore0; -static Empty::I2CDriver i2cDriver0(&i2cSemaphore0); -#else -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 || \ @@ -174,7 +151,18 @@ static RCOutput_ZYNQ rcoutDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP static RCOutput_Bebop rcoutDriver(i2c_mgr_instance.get_device(HAL_RCOUT_BEBOP_BLDC_I2C_BUS, HAL_RCOUT_BEBOP_BLDC_I2C_ADDR)); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE -static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(i2c_devpaths, PCA9685_PRIMARY_ADDRESS), false, 0, MINNOW_GPIO_S5_1); +static const std::vector i2c_devpaths({ + /* UEFI with lpss set to ACPI */ + "platform/80860F41:05", + /* UEFI with lpss set to PCI */ + "pci0000:00/0000:00:18.6", +}); +static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device( + { /* UEFI with lpss set to ACPI */ + "platform/80860F41:05", + /* UEFI with lpss set to PCI */ + "pci0000:00/0000:00:18.6" }, + PCA9685_PRIMARY_ADDRESS), false, 0, MINNOW_GPIO_S5_1); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT static RCOutput_QFLIGHT rcoutDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO @@ -204,19 +192,6 @@ HAL_Linux::HAL_Linux() : &uartEDriver, &uartFDriver, &i2c_mgr_instance, -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO - &i2cDriver0, - &i2cDriver1, - &i2cDriver2, -#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE - &i2cDriver0, - &i2cDriver1, - NULL, -#else - &i2cDriver0, - NULL, - NULL, -#endif &spiDeviceManager, &analogIn, &storageDriver, @@ -333,13 +308,6 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const scheduler->init(); gpio->init(); - i2c->begin(); -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO - i2c1->begin(); - i2c2->begin(); -#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE - i2c1->begin(); -#endif spi->init(); rcout->init(); rcin->init(); diff --git a/libraries/AP_HAL_Linux/I2CDriver.cpp b/libraries/AP_HAL_Linux/I2CDriver.cpp deleted file mode 100644 index 62c6ce7cd3..0000000000 --- a/libraries/AP_HAL_Linux/I2CDriver.cpp +++ /dev/null @@ -1,281 +0,0 @@ -#include -#include -#include -#include -#include -#ifndef I2C_SMBUS_BLOCK_MAX -#include -#endif -#include -#include -#include -#include -#include -#include - -#include - -#include "I2CDriver.h" -#include "Util.h" - -extern const AP_HAL::HAL &hal; - -using namespace Linux; - -I2CDriver::I2CDriver(uint8_t bus) - : _fake_dev(I2CDeviceManager::from(hal.i2c_mgr)->get_device(bus, 0)) -{ -} - -I2CDriver::I2CDriver(std::vector devpaths) - : _fake_dev(I2CDeviceManager::from(hal.i2c_mgr)->get_device(devpaths, 0)) -{ -} - -/* - tell the I2C library what device we want to talk to - */ -bool I2CDriver::set_address(uint8_t addr) -{ - if (!_fake_dev) { - return false; - } - - if (_addr == addr) { - /* 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; - - return true; -} - -void I2CDriver::setTimeout(uint16_t ms) -{ - // unimplemented -} - -void I2CDriver::setHighSpeed(bool active) -{ - // unimplemented -} - -uint8_t I2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data) -{ - if (!set_address(addr)) { - return 1; - } - if (::write(_fake_dev->get_fd(), data, len) != len) { - return 1; - } - return 0; // success -} - - -uint8_t I2CDriver::writeRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data) -{ - uint8_t buf[len+1]; - buf[0] = reg; - if (len != 0) { - memcpy(&buf[1], data, len); - } - return write(addr, len+1, buf); -} - -/* - this is a copy of i2c_smbus_access() from i2c-dev.h. We need it for - platforms with older headers - */ -static inline __s32 _i2c_smbus_access(int file, char read_write, __u8 command, - int size, union i2c_smbus_data *data) -{ - struct i2c_smbus_ioctl_data args; - args.read_write = read_write; - args.command = command; - args.size = size; - args.data = data; - return ioctl(file,I2C_SMBUS,&args); -} - -uint8_t I2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val) -{ - if (!set_address(addr)) { - return 1; - } - union i2c_smbus_data data; - data.byte = val; - if (_i2c_smbus_access(_fake_dev->get_fd(),I2C_SMBUS_WRITE, reg, - I2C_SMBUS_BYTE_DATA, &data) == -1) { - return 1; - } - return 0; -} - -uint8_t I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data) -{ - if (!set_address(addr)) { - return 1; - } - if (::read(_fake_dev->get_fd(), data, len) != len) { - return 1; - } - return 0; -} - -uint8_t I2CDriver::readRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data) -{ - if (!_fake_dev) { - return 1; - } - struct i2c_msg msgs[] = { - { - addr : addr, - flags : 0, - len : 1, - buf : (typeof(msgs->buf))® - }, - { - addr : addr, - flags : I2C_M_RD, - len : len, - buf : (typeof(msgs->buf))data, - } - }; - struct i2c_rdwr_ioctl_data i2c_data = { - msgs : msgs, - nmsgs : 2 - }; - - // prevent valgrind error - memset(data, 0, len); - - if (ioctl(_fake_dev->get_fd(), I2C_RDWR, &i2c_data) == -1) { - return 1; - } - - return 0; -} - - -uint8_t I2CDriver::readRegistersMultiple(uint8_t addr, uint8_t reg, - uint8_t len, - uint8_t count, uint8_t* data) -{ -#ifdef I2C_RDRW_IOCTL_MAX_MSGS - const uint8_t max_count = I2C_RDRW_IOCTL_MAX_MSGS / 2; -#else - const uint8_t max_count = 8; -#endif - - if (!_fake_dev) { - return 1; - } - while (count > 0) { - uint8_t n = count > max_count ? max_count : count; - struct i2c_msg msgs[2*n]; - struct i2c_rdwr_ioctl_data i2c_data = { - msgs : msgs, - nmsgs : (typeof(i2c_data.nmsgs))(2*n) - }; - for (uint8_t i=0; ibuf))® - msgs[i*2+1].addr = addr; - msgs[i*2+1].flags = I2C_M_RD; - msgs[i*2+1].len = len; - msgs[i*2+1].buf = (typeof(msgs->buf))data; - data += len; - }; - if (ioctl(_fake_dev->get_fd(), I2C_RDWR, &i2c_data) == -1) { - return 1; - } - count -= n; - } - return 0; -} - - -uint8_t I2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data) -{ - if (!set_address(addr)) { - return 1; - } - union i2c_smbus_data v; - memset(&v, 0, sizeof(v)); - if (_i2c_smbus_access(_fake_dev->get_fd(),I2C_SMBUS_READ, reg, - I2C_SMBUS_BYTE_DATA, &v)) { - return 1; - } - *data = v.byte; - return 0; -} - -/* - main transfer function - */ -bool I2CDriver::do_transfer(uint8_t addr, const uint8_t *send, - uint32_t send_len, uint8_t *recv, - uint32_t recv_len) -{ - struct i2c_msg i2cmsg[2] = { - { - addr : addr, - flags : 0, - len : (typeof(i2cmsg->len))send_len, - buf : (typeof(i2cmsg->buf))send - }, - { - addr : addr, - flags : I2C_M_RD, - len : (typeof(i2cmsg->len))recv_len, - buf : (typeof(i2cmsg->buf))recv, - } - }; - struct i2c_rdwr_ioctl_data msg_rdwr; - - if (send_len == 0 && recv_len) { - msg_rdwr.msgs = &i2cmsg[1]; - msg_rdwr.nmsgs = 1; - } else if (send_len && recv_len == 0) { - msg_rdwr.msgs = &i2cmsg[0]; - msg_rdwr.nmsgs = 1; - }else if (send_len && recv_len) { - msg_rdwr.msgs = &i2cmsg[0]; - msg_rdwr.nmsgs = 2; - } else { - return false; - } - 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(); -} diff --git a/libraries/AP_HAL_Linux/I2CDriver.h b/libraries/AP_HAL_Linux/I2CDriver.h deleted file mode 100644 index fd3da8441b..0000000000 --- a/libraries/AP_HAL_Linux/I2CDriver.h +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once - -#include - -#include - -#include "AP_HAL_Linux.h" -#include "I2CDevice.h" - -class Linux::I2CDriver : public AP_HAL::I2CDriver { -public: - I2CDriver(uint8_t bus); - I2CDriver(std::vector devpaths); - - void begin() { } - void end() { } - void setTimeout(uint16_t ms); - void setHighSpeed(bool active); - - /* write: for i2c devices which do not obey register conventions */ - uint8_t write(uint8_t addr, uint8_t len, uint8_t* data); - /* writeRegister: write a single 8-bit value to a register */ - uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val); - /* writeRegisters: write bytes to contiguous registers */ - uint8_t writeRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data); - - /* read: for i2c devices which do not obey register conventions */ - uint8_t read(uint8_t addr, uint8_t len, uint8_t* data); - /* readRegister: read from a device register - writes the register, - * then reads back an 8-bit value. */ - uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data); - - /* readRegister: read contiguous device registers - writes the first - * register, then reads back multiple bytes */ - uint8_t readRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data); - - uint8_t readRegistersMultiple(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t count, - uint8_t* data); - - uint8_t lockup_count(); - - 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::OwnPtr _fake_dev; - char *_device = NULL; - uint8_t _addr; - bool _print_ioctl_error = true; -};