Linux: added I2C class

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

4 files changed, 76 insertions, 114 deletions

diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk
index c90673317..712f26631 100644
--- a/src/drivers/blinkm/module.mk
+++ b/src/drivers/blinkm/module.mk
@@ -37,6 +37,10 @@
 
 MODULE_COMMAND	= blinkm
 
-SRCS		= blinkm.cpp
+ifeq ($(PX4_TARGET_OS),nuttx)
+SRCS		= blinkm_nuttx.cpp
+else
+SRCS		= blinkm_linux.cpp
+endif
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp
index 52f336b3a..1a3492c8f 100644
--- a/src/drivers/device/i2c_linux.cpp
+++ b/src/drivers/device/i2c_linux.cpp
@@ -41,17 +41,19 @@
  */
 
 #include "i2c.h"
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
 
 namespace device
 {
 
-unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 };
-
 I2C::I2C(const char *name,
 	 const char *devname,
 	 int bus,
-	 uint16_t address,
-	 uint32_t frequency) :
+	 uint16_t address) :
 	// base class
 	CDev(name, devname),
 	// public
@@ -60,8 +62,8 @@ I2C::I2C(const char *name,
 	// private
 	_bus(bus),
 	_address(address),
-	_frequency(frequency),
-	_dev(nullptr)
+	_fd(-1),
+	_dname(devname)
 {
 	// fill in _device_id fields for a I2C device
 	_device_id.devid_s.bus_type = DeviceBusType_I2C;
@@ -73,162 +75,98 @@ I2C::I2C(const char *name,
 
 I2C::~I2C()
 {
-	if (_dev) {
-		px4_i2cuninitialize(_dev);
-		_dev = nullptr;
-	}
-}
-
-int
-I2C::set_bus_clock(unsigned bus, unsigned clock_hz)
-{
-	int index = bus - 1;
-
-	if (index < 0 || index >= static_cast<int>(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) {
-		return -EINVAL;
-	}
-
-	if (_bus_clocks[index] > 0) {
-		// debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz);
+	if (_fd >= 0) {
+		::close(_fd);
+		_fd = -1;
 	}
-	_bus_clocks[index] = clock_hz;
-
-	return PX4_OK;
 }
 
 int
 I2C::init()
 {
 	int ret = PX4_OK;
-	unsigned bus_index;
-
-	// attach to the i2c bus
-	_dev = px4_i2cinitialize(_bus);
-
-	if (_dev == nullptr) {
-		debug("failed to init I2C");
-		ret = -ENOENT;
-		goto out;
-	}
-
-	// the above call fails for a non-existing bus index,
-	// so the index math here is safe.
-	bus_index = _bus - 1;
-
-	// abort if the max frequency we allow (the frequency we ask)
-	// is smaller than the bus frequency
-	if (_bus_clocks[bus_index] > _frequency) {
-		(void)px4_i2cuninitialize(_dev);
-		_dev = nullptr;
-		log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)",
-			_bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);
-		ret = -EINVAL;
-		goto out;
-	}
 
-	// set the bus frequency on the first access if it has
-	// not been set yet
-	if (_bus_clocks[bus_index] == 0) {
-		_bus_clocks[bus_index] = _frequency;
-	}
-
-	// set frequency for this instance once to the bus speed
-	// the bus speed is the maximum supported by all devices on the bus,
-	// as we have to prioritize performance over compatibility.
-	// If a new device requires a lower clock speed, this has to be
-	// manually set via "fmu i2c <bus> <clock>" before starting any
-	// drivers.
-	// This is necessary as automatically lowering the bus speed
-	// for maximum compatibility could induce timing issues on
-	// critical sensors the adopter might be unaware of.
-	I2C_SETFREQUENCY(_dev, _bus_clocks[bus_index]);
-
-	// call the probe function to check whether the device is present
-	ret = probe();
-
-	if (ret != PX4_OK) {
-		debug("probe failed");
-		goto out;
-	}
+	// Assume the driver set the desired bus frequency. There is no standard
+	// way to set it from user space.
 
 	// do base class init, which will create device node, etc
 	ret = CDev::init();
 
 	if (ret != PX4_OK) {
 		debug("cdev init failed");
-		goto out;
+		return ret;
 	}
 
-	// tell the world where we are
-	log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)",
-		_bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);
+	_fd = ::open(_dname.c_str(), O_RDWR);
+        if (_fd < 0) {
+                warnx("could not open %s", _dname.c_str());
+                return -errno;
+        }
 
-out:
-	if ((ret != PX4_OK) && (_dev != nullptr)) {
-		px4_i2cuninitialize(_dev);
-		_dev = nullptr;
-	}
 	return ret;
 }
 
 int
-I2C::probe()
-{
-	// Assume the device is too stupid to be discoverable.
-	return PX4_OK;
-}
-
-int
 I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
 {
-	px4_i2c_msg_t msgv[2];
+	struct i2c_msg msgv[2];
 	unsigned msgs;
+	struct i2c_rdwr_ioctl_data packets;
 	int ret;
 	unsigned retry_count = 0;
 
 	do {
 		//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
-
 		msgs = 0;
 
 		if (send_len > 0) {
 			msgv[msgs].addr = _address;
 			msgv[msgs].flags = 0;
-			msgv[msgs].buffer = const_cast<uint8_t *>(send);
-			msgv[msgs].length = send_len;
+			msgv[msgs].buf = const_cast<uint8_t *>(send);
+			msgv[msgs].len = send_len;
 			msgs++;
 		}
 
 		if (recv_len > 0) {
 			msgv[msgs].addr = _address;
 			msgv[msgs].flags = I2C_M_READ;
-			msgv[msgs].buffer = recv;
-			msgv[msgs].length = recv_len;
+			msgv[msgs].buf = recv;
+			msgv[msgs].len = recv_len;
 			msgs++;
 		}
 
 		if (msgs == 0)
 			return -EINVAL;
 
-		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
+		packets.msgs  = msgv;
+		packets.nmsgs = msgs;
+
+		ret = ::ioctl(_fd, I2C_RDWR, &packets);
+		if (ret < 0) {
+        		warnx("I2C transfer failed");
+        		return 1;
+    		}
 
 		/* success */
 		if (ret == PX4_OK)
 			break;
 
+// No way to reset device from userspace
+#if 0
 		/* if we have already retried once, or we are going to give up, then reset the bus */
 		if ((retry_count >= 1) || (retry_count >= _retries))
 			px4_i2creset(_dev);
+#endif
 
 	} while (retry_count++ < _retries);
 
 	return ret;
-
 }
 
 int
-I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs)
+I2C::transfer(struct i2c_msg *msgv, unsigned msgs)
 {
+	struct i2c_rdwr_ioctl_data packets;
 	int ret;
 	unsigned retry_count = 0;
 
@@ -236,17 +174,26 @@ I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs)
 	for (unsigned i = 0; i < msgs; i++)
 		msgv[i].addr = _address;
 
-
 	do {
-		ret = I2C_TRANSFER(_dev, msgv, msgs);
+		packets.msgs  = msgv;
+		packets.nmsgs = msgs;
+
+		ret = ::ioctl(_fd, I2C_RDWR, &packets);
+		if (ret < 0) {
+        		warnx("I2C transfer failed");
+        		return 1;
+    		}
 
 		/* success */
 		if (ret == PX4_OK)
 			break;
 
+// No way to reset device from userspace
+#if 0
 		/* if we have already retried once, or we are going to give up, then reset the bus */
 		if ((retry_count >= 1) || (retry_count >= _retries))
 			px4_i2creset(_dev);
+#endif
 
 	} while (retry_count++ < _retries);
 
diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h
index 3de6c9658..56eaea4b9 100644
--- a/src/drivers/device/i2c_linux.h
+++ b/src/drivers/device/i2c_linux.h
@@ -43,6 +43,9 @@
 #include "device.h"
 
 #include <px4_i2c.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <string>
 
 namespace device __EXPORT
 {
@@ -60,10 +63,6 @@ public:
 	 */
 	int16_t		get_address() const { return _address; }
 
-	static int	set_bus_clock(unsigned bus, unsigned clock_hz);
-
-	static unsigned	int	_bus_clocks[3];
-	
 protected:
 	/**
 	 * The number of times a read or write operation will be retried on
@@ -89,16 +88,16 @@ protected:
 	I2C(const char *name,
 	    const char *devname,
 	    int bus,
-	    uint16_t address,
-	    uint32_t frequency);
+	    uint16_t address);
 	virtual ~I2C();
 
 	virtual int	init();
-
+#if 0
 	/**
 	 * Check for the presence of the device on the bus.
 	 */
 	virtual int	probe();
+#endif
 
 	/**
 	 * Perform an I2C transaction to the device.
@@ -123,8 +122,9 @@ protected:
 	 * @return		OK if the transfer was successful, -errno
 	 *			otherwise.
 	 */
-	int		transfer(px4_i2c_msg_t *msgv, unsigned msgs);
+	int		transfer(struct i2c_msg *msgv, unsigned msgs);
 
+#if 0
 	/**
 	 * Change the bus address.
 	 *
@@ -137,11 +137,12 @@ protected:
 		_address = address;
 		_device_id.devid_s.address = _address;
 	}
+#endif
 
 private:
 	uint16_t		_address;
-	uint32_t		_frequency;
-	px4_i2c_dev_t		*_dev;
+	int 			_fd;
+	std::string			_dname;
 
 	I2C(const device::I2C&);
 	I2C operator=(const device::I2C&);
diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp
index 733fafbd8..dd67bc5d1 100644
--- a/src/drivers/ms5611/ms5611_i2c.cpp
+++ b/src/drivers/ms5611/ms5611_i2c.cpp
@@ -74,13 +74,17 @@ public:
 	virtual int	dev_read(unsigned offset, void *data, unsigned count);
 	virtual int	dev_ioctl(unsigned operation, unsigned &arg);
 
+#ifdef __PX4_NUTTX
 protected:
 	virtual int	probe();
+#endif
 
 private:
 	ms5611::prom_u	&_prom;
 
+#ifdef __PX4_NUTTX
 	int		_probe_address(uint8_t address);
+#endif
 
 	/**
 	 * Send a reset command to the MS5611.
@@ -112,7 +116,11 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
 }
 
 MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) :
-	I2C("MS5611_I2C", nullptr, bus, 0, 400000),
+	I2C("MS5611_I2C", nullptr, bus, 0
+#ifdef __PX4_NUTTX
+, 400000
+#endif
+),
 	_prom(prom)
 {
 }
@@ -172,6 +180,7 @@ MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg)
 	return ret;
 }
 
+#ifdef __PX4_NUTTX
 int
 MS5611_I2C::probe()
 {
@@ -206,6 +215,7 @@ MS5611_I2C::_probe_address(uint8_t address)
 
 	return PX4_OK;
 }
+#endif
 
 
 int