CDEV::I2C: Enforce one speed per bus

src/drivers/device/i2c.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,6 +45,8 @@
 namespace device
 {
 
+unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 };
+
 I2C::I2C(const char *name,
 	 const char *devname,
 	 int bus,
@@ -76,12 +78,30 @@ I2C::~I2C()
 		up_i2cuninitialize(_dev);
 }
 
+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);
+	}
+	_bus_clocks[index] = clock_hz;
+
+	return OK;
+}
+
 int
 I2C::init()
 {
 	int ret = OK;
+	unsigned bus_index;
 
-	/* attach to the i2c bus */
+	// attach to the i2c bus
 	_dev = up_i2cinitialize(_bus);
 
 	if (_dev == nullptr) {
@@ -90,6 +110,37 @@ I2C::init()
 		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)up_i2cuninitialize(_dev);
+		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();
 
@@ -107,9 +158,13 @@ I2C::init()
 	}
 
 	// tell the world where we are
-	log("on I2C bus %d at 0x%02x", _bus, _address);
+	log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)",
+		_bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);
 
 out:
+	if ((ret != OK) && (_dev != nullptr)) {
+		up_i2cuninitialize(_dev);
+	}
 	return ret;
 }
 
@@ -152,12 +207,6 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
 		if (msgs == 0)
 			return -EINVAL;
 
-		/*
-		 * I2C architecture means there is an unavoidable race here
-		 * if there are any devices on the bus with a different frequency
-		 * preference.  Really, this is pointless.
-		 */
-		I2C_SETFREQUENCY(_dev, _frequency);
 		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
 
 		/* success */
@@ -186,12 +235,6 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
 
 
 	do {
-		/*
-		 * I2C architecture means there is an unavoidable race here
-		 * if there are any devices on the bus with a different frequency
-		 * preference.  Really, this is pointless.
-		 */
-		I2C_SETFREQUENCY(_dev, _frequency);
 		ret = I2C_TRANSFER(_dev, msgv, msgs);
 
 		/* success */

src/drivers/device/i2c.h

@@ -59,6 +59,10 @@ public:
 	 * Get the address
 	 */
 	int16_t		get_address() const { return _address; }
+
+	static int	set_bus_clock(unsigned bus, unsigned clock_hz);
+
+	static unsigned	int	_bus_clocks[3];
 	
 protected:
 	/**

src/drivers/px4fmu/fmu.cpp

@@ -55,6 +55,7 @@
 #include <nuttx/arch.h>
 
 #include <drivers/device/device.h>
+#include <drivers/device/i2c.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
@@ -111,6 +112,8 @@ public:
 	int		set_pwm_alt_rate(unsigned rate);
 	int		set_pwm_alt_channels(uint32_t channels);
 
+	int		set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
+
 private:
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	static const unsigned _max_actuators = 4;
@@ -505,6 +508,12 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
 	return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
 }
 
+int
+PX4FMU::set_i2c_bus_clock(unsigned bus, unsigned clock_hz)
+{
+	return device::I2C::set_bus_clock(bus, clock_hz);
+}
+
 void
 PX4FMU::subscribe()
 {
@@ -1593,6 +1602,11 @@ fmu_new_mode(PortMode new_mode)
 	return OK;
 }
 
+int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz)
+{
+	return g_fmu->set_i2c_bus_clock(bus, clock_hz);
+}
+
 int
 fmu_start(void)
 {
@@ -1867,12 +1881,27 @@ fmu_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(verb, "i2c")) {
+		if (argc > 3) {
+			int bus = strtol(argv[2], 0, 0);
+			int clock_hz = strtol(argv[3], 0, 0);
+			int ret = fmu_new_i2c_speed(bus, clock_hz);
+
+			if (ret) {
+				errx(ret, "setting I2C clock failed");
+			}
+
+			exit(0);
+		} else {
+			warnx("i2c cmd args: <bus id> <clock Hz>");
+		}
+	}
 
 	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
-	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
+	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds], i2c <bus> <hz>\n");
 #endif
 	exit(1);
 }