1 files changed, 76 insertions, 6 deletions

diff --git a/arch/arm/mach-ux500/board-mop500-sensors.c b/arch/arm/mach-ux500/board-mop500-sensors.c
index 51cdb5e50e8..d546eb1be93 100644
--- a/arch/arm/mach-ux500/board-mop500-sensors.c
+++ b/arch/arm/mach-ux500/board-mop500-sensors.c
@@ -10,6 +10,7 @@
 #include <linux/i2c.h>
 #include <linux/input/lps001wp.h>
 #include <asm/mach-types.h>
+#include <mach/id.h>
 
 #include "board-mop500.h"
 
@@ -44,11 +45,6 @@ static struct lps001wp_prs_platform_data __initdata lps001wp_pdata = {
 
 static struct i2c_board_info __initdata mop500_i2c2_devices[] = {
 	{
-		/* LSM303DLH Accelerometer */
-		I2C_BOARD_INFO("lsm303dlh_a", 0x18),
-		.platform_data = &lsm303dlh_pdata,
-	},
-	{
 		/* LSM303DLH Magnetometer */
 		I2C_BOARD_INFO("lsm303dlh_m", 0x1E),
 		.platform_data = &lsm303dlh_pdata,
@@ -66,6 +62,31 @@ static struct i2c_board_info __initdata mop500_i2c2_devices[] = {
 };
 
 /*
+ * Break this out due to the fact that this have changed address on snowball
+ */
+static struct i2c_board_info __initdata mop500_2_i2c2_devices[] = {
+	{
+		/* LSM303DLH Accelerometer */
+		I2C_BOARD_INFO("lsm303dlh_a", 0x18),
+		.platform_data = &lsm303dlh_pdata,
+	},
+};
+
+/*
+ * This is needed due to the fact that the i2c address changed in V7 =<
+ * and there is no way of knowing if the HW is V7 or higher so we just
+ * have to try and fail.
+ */
+static struct i2c_board_info __initdata snowball_i2c2_devices[] = {
+	{
+		/* LSM303DLH Accelerometer */
+		I2C_BOARD_INFO("lsm303dlh_a", 0x19),
+		.platform_data = &lsm303dlh_pdata,
+	},
+};
+
+
+/*
  * Register/Add i2c sensors
  */
 void mop500_sensors_i2c_add(int busnum, struct i2c_board_info const *info,
@@ -94,9 +115,43 @@ void mop500_sensors_i2c_add(int busnum, struct i2c_board_info const *info,
 	i2c_put_adapter(adap);
 }
 
+/*
+ * Register/Add i2c sensors
+ */
+void mop500_sensors_probe_add_lsm303dlh_a(void)
+{
+	static const int busnum = 2;
+	struct i2c_adapter *adap;
+	struct i2c_client *client;
+	static const unsigned short i2c_addr_list[] = {
+		0x18, 0x19, I2C_CLIENT_END };
+	struct i2c_board_info i2c_info = {
+			/* LSM303DLH Accelerometer */
+			I2C_BOARD_INFO("lsm303dlh_a", 0),
+			.platform_data = &lsm303dlh_pdata,
+	};
+
+	adap = i2c_get_adapter(busnum);
+	if (!adap) {
+		/* We have no i2c adapter yet lets create it. */
+		pr_err(__FILE__ ": Could not get adapter %d\n", busnum);
+		return;
+	}
+	client = i2c_new_probed_device(adap, &i2c_info,
+			i2c_addr_list, NULL);
+	if (!client)
+		pr_err(__FILE__ ": failed to register %s to i2c%d\n",
+				i2c_info.type,
+				busnum);
+	i2c_put_adapter(adap);
+}
 
-void __init mop500_sensors_init(void)
+static int mop500_sensors_init(void)
 {
+
+	if (!machine_is_snowball() && !uib_is_stuib())
+		return 0;
+
 	if (machine_is_hrefv60()) {
 		lsm303dlh_pdata.irq_a1 = HREFV60_ACCEL_INT1_GPIO;
 		lsm303dlh_pdata.irq_a2 = HREFV60_ACCEL_INT2_GPIO;
@@ -113,4 +168,19 @@ void __init mop500_sensors_init(void)
 
 	mop500_sensors_i2c_add(2, mop500_i2c2_devices,
 			ARRAY_SIZE(mop500_i2c2_devices));
+
+	if (machine_is_snowball()) {
+		if (cpu_is_u8500v21())
+			/* This is ugly but we cant know what address
+			 * to use */
+			mop500_sensors_probe_add_lsm303dlh_a();
+		else /* Add the accelerometer with new addr */
+			mop500_sensors_i2c_add(2, snowball_i2c2_devices,
+					ARRAY_SIZE(snowball_i2c2_devices));
+	} else /* none snowball have the old addr */
+		mop500_sensors_i2c_add(2, mop500_2_i2c2_devices,
+				ARRAY_SIZE(mop500_2_i2c2_devices));
+	return 0;
 }
+
+module_init(mop500_sensors_init);