[PATCH] i2c: Devices which have some i2c addr can work in same i2c bus

[David Wu <david.wu@xxxxxxxxxxxxxx>]

From: Zhang aihui <zah@xxxxxxxxxxxxxx>

If i2c slave devices don't work at the same time, which have
the same i2c addr, it would register two devices, can make them
working.

Change-Id: I1bfb7783924b08bdc6e12bf47c2de01bdac7c2e2
Signed-off-by: Zhang aihui <zah@xxxxxxxxxxxxxx>
Signed-off-by: David Wu <david.wu@xxxxxxxxxxxxxx>
---
 drivers/i2c/i2c-core-base.c | 51 +++++++++++++++++++++++++++++--------
 1 file changed, 41 insertions(+), 10 deletions(-)

diff --git a/drivers/i2c/i2c-core-base.c b/drivers/i2c/i2c-core-base.c
index ae3af738b03f..53a8141e6238 100644
--- a/drivers/i2c/i2c-core-base.c
+++ b/drivers/i2c/i2c-core-base.c
@@ -62,6 +62,7 @@
 static DEFINE_MUTEX(core_lock);
 static DEFINE_IDR(i2c_adapter_idr);
 
+static int i2c_check_addr_ex(struct i2c_adapter *adapter, int addr);
 static int i2c_detect(struct i2c_adapter *adapter, struct i2c_driver *driver);
 
 static DEFINE_STATIC_KEY_FALSE(i2c_trace_msg_key);
@@ -849,7 +850,8 @@ static void i2c_adapter_unlock_bus(struct i2c_adapter *adapter,
 
 static void i2c_dev_set_name(struct i2c_adapter *adap,
 			     struct i2c_client *client,
-			     struct i2c_board_info const *info)
+			     struct i2c_board_info const *info,
+			     int status)
 {
 	struct acpi_device *adev = ACPI_COMPANION(&client->dev);
 
@@ -863,8 +865,12 @@ static void i2c_dev_set_name(struct i2c_adapter *adap,
 		return;
 	}
 
-	dev_set_name(&client->dev, "%d-%04x", i2c_adapter_id(adap),
-		     i2c_encode_flags_to_addr(client));
+	if (status == 0)
+		dev_set_name(&client->dev, "%d-%04x", i2c_adapter_id(adap),
+			i2c_encode_flags_to_addr(client));
+	else
+		dev_set_name(&client->dev, "%d-%04x-%01x", i2c_adapter_id(adap),
+			i2c_encode_flags_to_addr(client), status);
 }
 
 int i2c_dev_irq_from_resources(const struct resource *resources,
@@ -940,9 +946,11 @@ i2c_new_client_device(struct i2c_adapter *adap, struct i2c_board_info const *inf
 	}
 
 	/* Check for address business */
-	status = i2c_check_addr_busy(adap, i2c_encode_flags_to_addr(client));
+	status = i2c_check_addr_ex(adap, i2c_encode_flags_to_addr(client));
 	if (status)
-		goto out_err;
+		dev_err(&adap->dev,
+			"%d i2c clients have been registered at 0x%02x",
+			status, client->addr);
 
 	client->dev.parent = &client->adapter->dev;
 	client->dev.bus = &i2c_bus_type;
@@ -951,7 +959,7 @@ i2c_new_client_device(struct i2c_adapter *adap, struct i2c_board_info const *inf
 	client->dev.fwnode = info->fwnode;
 
 	device_enable_async_suspend(&client->dev);
-	i2c_dev_set_name(adap, client, info);
+	i2c_dev_set_name(adap, client, info, status);
 
 	if (info->swnode) {
 		status = device_add_software_node(&client->dev, info->swnode);
@@ -976,10 +984,6 @@ i2c_new_client_device(struct i2c_adapter *adap, struct i2c_board_info const *inf
 	device_remove_software_node(&client->dev);
 out_err_put_of_node:
 	of_node_put(info->of_node);
-out_err:
-	dev_err(&adap->dev,
-		"Failed to register i2c client %s at 0x%02x (%d)\n",
-		client->name, client->addr, status);
 out_err_silent:
 	kfree(client);
 	return ERR_PTR(status);
@@ -1988,6 +1992,33 @@ EXPORT_SYMBOL(i2c_del_driver);
 
 /* ------------------------------------------------------------------------- */
 
+struct i2c_addr_cnt {
+	int addr;
+	int cnt;
+};
+
+static int __i2c_check_addr_ex(struct device *dev, void *addrp)
+{
+	struct i2c_client *client = i2c_verify_client(dev);
+	struct i2c_addr_cnt *addrinfo = (struct i2c_addr_cnt *)addrp;
+	int addr = addrinfo->addr;
+
+	if (client && client->addr == addr)
+		addrinfo->cnt++;
+
+	return 0;
+}
+
+static int i2c_check_addr_ex(struct i2c_adapter *adapter, int addr)
+{
+	struct i2c_addr_cnt addrinfo;
+
+	addrinfo.addr = addr;
+	addrinfo.cnt = 0;
+	device_for_each_child(&adapter->dev, &addrinfo, __i2c_check_addr_ex);
+	return addrinfo.cnt;
+}
+
 struct i2c_cmd_arg {
 	unsigned	cmd;
 	void		*arg;
-- 
2.25.1