ptp: ocp: Add R4006 I2C peripheral support

541 lines not on this page · raw message

index 681a201c2..1e587ea4e 100644
@@ -25,6 +25,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/property.h>
 #include <linux/slab.h>
 #include <linux/sysfs.h>
 

@@ -264,6 +265,49 @@ static const struct i2c_lock_operations i2c_parent_lock_ops = {
 	.unlock_bus =  i2c_parent_unlock_bus,
 };
 
+static struct fwnode_handle *
+i2c_mux_get_channel_swnode(struct i2c_mux_core *muxc, u32 chan_id)
+{
+	struct fwnode_handle *dev_node = dev_fwnode(muxc->dev);
+	struct fwnode_handle *mux_node, *child = NULL;
+	u32 reg;
+
+	if (!is_software_node(dev_node))
+		return NULL;
+
+	if (muxc->arbitrator)
+		mux_node = fwnode_get_named_child_node(dev_node, "i2c-arb");
+	else if (muxc->gate)
+		mux_node = fwnode_get_named_child_node(dev_node, "i2c-gate");
+	else
+		mux_node = fwnode_get_named_child_node(dev_node, "i2c-mux");
+
+	if (mux_node) {
+		/* A "reg" property indicates an old-style firmware entry. */
+		if (!fwnode_property_read_u32(mux_node, "reg", &reg)) {
+			fwnode_handle_put(mux_node);
+			mux_node = NULL;
+		}
+	}
+
+	if (!mux_node)
+		mux_node = fwnode_handle_get(dev_node);
+	else if (muxc->arbitrator || muxc->gate)
+		child = fwnode_handle_get(mux_node);
+
+	if (!child) {
+		fwnode_for_each_child_node(mux_node, child) {
+			if (fwnode_property_read_u32(child, "reg", &reg))
+				continue;
+			if (chan_id == reg)
+				break;
+		}
+	}
+
+	fwnode_handle_put(mux_node);
+	return child;
+}
+
 int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 			u32 force_nr, u32 chan_id)
 {

@@ -364,6 +408,12 @@ int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 
 		priv->adap.dev.of_node = child;
 		of_node_put(mux_node);
+	} else {
+		struct fwnode_handle *fwnode;
+
+		fwnode = i2c_mux_get_channel_swnode(muxc, chan_id);
+		if (fwnode)
+			device_set_node(&priv->adap.dev, fwnode);
 	}
 
 	/*

@@ -408,6 +458,8 @@ int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 	return 0;
 
 err_free_priv:
+	if (is_software_node(dev_fwnode(&priv->adap.dev)))
+		fwnode_handle_put(dev_fwnode(&priv->adap.dev));
 	kfree(priv);
 	return ret;
 }

@@ -420,6 +472,7 @@ void i2c_mux_del_adapters(struct i2c_mux_core *muxc)
 	while (muxc->num_adapters) {
 		struct i2c_adapter *adap = muxc->adapter[--muxc->num_adapters];
 		struct i2c_mux_priv *priv = adap->algo_data;
+		struct fwnode_handle *fwnode = dev_fwnode(&adap->dev);
 		struct device_node *np = adap->dev.of_node;
 
 		muxc->adapter[muxc->num_adapters] = NULL;

@@ -430,6 +483,8 @@ void i2c_mux_del_adapters(struct i2c_mux_core *muxc)
 
 		sysfs_remove_link(&priv->adap.dev.kobj, "mux_device");
 		i2c_del_adapter(adap);
+		if (is_software_node(fwnode))
+			fwnode_handle_put(fwnode);
 		of_node_put(np);
 		kfree(priv);
 	}

index 35e911f1a..b362a7c4d 100644
@@ -21,10 +21,14 @@
 #include <net/devlink.h>
 #include <linux/i2c.h>
 #include <linux/mtd/mtd.h>
+#include <linux/mutex.h>
 #include <linux/nvmem-consumer.h>
+#include <linux/property.h>
 #include <linux/crc16.h>
 #include <linux/dpll.h>
 
+#include <dt-bindings/leds/common.h>
+
 #define PCI_DEVICE_ID_META_TIMECARD		0x0400
 
 #define PCI_VENDOR_ID_CELESTICA			0x18d4

@@ -347,6 +351,32 @@ struct ptp_ocp_serial_port {
 #define OCP_SIGNAL_NUM			4
 #define OCP_FREQ_NUM			4
 
+#define OCP_R4006_MUX_CHANNELS		4
+#define OCP_R4006_SENSOR_COUNT		5
+#define OCP_R4006_LED_COUNT		5
+#define OCP_R4006_LED_COMPONENT_COUNT	(3 * OCP_R4006_LED_COUNT)
+#define OCP_R4006_NODE_COUNT		(1 + OCP_R4006_MUX_CHANNELS + \
+					 OCP_R4006_SENSOR_COUNT + 1 + \
+					 OCP_R4006_LED_COUNT + \
+					 OCP_R4006_LED_COMPONENT_COUNT)
+
+struct ptp_ocp_r4006 {
+	struct software_node mux_node;
+	struct property_entry mux_props[6];
+	struct software_node channel_nodes[OCP_R4006_MUX_CHANNELS];
+	struct property_entry channel_props[OCP_R4006_MUX_CHANNELS][4];
+	struct software_node sensor_nodes[OCP_R4006_SENSOR_COUNT];
+	struct property_entry sensor_props[OCP_R4006_SENSOR_COUNT][3];
+	struct software_node led_node;
+	struct property_entry led_props[6];
+	struct software_node led_nodes[OCP_R4006_LED_COUNT];
+	struct property_entry led_group_props[OCP_R4006_LED_COUNT][7];
+	struct software_node component_nodes[OCP_R4006_LED_COMPONENT_COUNT];
+	struct property_entry component_props[OCP_R4006_LED_COMPONENT_COUNT][4];
+	const struct software_node *node_group[OCP_R4006_NODE_COUNT + 1];
+	bool registered;
+};
+
 enum {
 	PORT_GNSS,
 	PORT_GNSS2,

@@ -403,6 +433,7 @@ struct ptp_ocp {
 	u16			fw_version;
 	u8			board_id[OCP_BOARD_ID_LEN];
 	u8			serial[OCP_SERIAL_LEN];
+	struct mutex		eeprom_lock; /* Serializes EEPROM discovery. */
 	bool			has_eeprom_data;
 	u32			pps_req_map;
 	int			flash_start;

@@ -414,6 +445,8 @@ struct ptp_ocp {
 	const struct ocp_sma_op *sma_op;
 	struct dpll_device *dpll;
 	dpll_tracker tracker;
+	struct ptp_ocp_r4006 *r4006;
+	struct delayed_work i2c_work;
 	int signals_nr;
 	int freq_in_nr;
 };

@@ -488,6 +521,71 @@ static struct ptp_ocp_eeprom_map art_eeprom_map[] = {
 	{ }
 };
 
+struct ptp_ocp_r4006_i2c_device {
+	const char *node_name;
+	const char *compatible;
+	const char *type;
+	u8 channel;
+	u8 address;
+};
+
+/* Channel 3's BNO08x at 0x4a has no upstream Linux driver, so omit it. */
+static const struct ptp_ocp_r4006_i2c_device ptp_ocp_r4006_sensors[] = {
+	{ "temperature@48", "national,lm75b", "lm75b", 0, 0x48 },
+	{ "temperature@49", "national,lm75b", "lm75b", 0, 0x49 },
+	{ "temperature@4a", "national,lm75b", "lm75b", 0, 0x4a },
+	{ "humidity@44", "sensirion,sht3x", "sht3x", 1, 0x44 },
+	{ "pressure@63", "invensense,icp10100", "icp10100", 2, 0x63 },
+};
+
+static_assert(ARRAY_SIZE(ptp_ocp_r4006_sensors) == OCP_R4006_SENSOR_COUNT);
+
+struct ptp_ocp_r4006_led {
+	const char *node_name;
+	const char *function;
+	u8 function_enumerator;
+	bool has_function_enumerator;
+	u8 channel[3];
+};
+
+static const struct ptp_ocp_r4006_led ptp_ocp_r4006_leds[] = {
+	{
+		.node_name = "multi-led@c",
+		.function = LED_FUNCTION_STATUS,
+		.channel = { 13, 12, 14 },
+	},
+	{
+		.node_name = "multi-led@6",
+		.function = LED_FUNCTION_INDICATOR,
+		.function_enumerator = 1,
+		.has_function_enumerator = true,
+		.channel = { 7, 6, 8 },
+	},
+	{
+		.node_name = "multi-led@9",
+		.function = LED_FUNCTION_INDICATOR,
+		.function_enumerator = 2,
+		.has_function_enumerator = true,
+		.channel = { 10, 9, 11 },
+	},
+	{
+		.node_name = "multi-led@0",
+		.function = LED_FUNCTION_INDICATOR,
+		.function_enumerator = 3,
+		.has_function_enumerator = true,
+		.channel = { 1, 0, 2 },
+	},
+	{
+		.node_name = "multi-led@3",
+		.function = LED_FUNCTION_INDICATOR,
+		.function_enumerator = 4,
+		.has_function_enumerator = true,
+		.channel = { 4, 3, 5 },
+	},
+};
+
+static_assert(ARRAY_SIZE(ptp_ocp_r4006_leds) == OCP_R4006_LED_COUNT);
+
 #define bp_assign_entry(bp, res, val) ({				\
 	uintptr_t addr = (uintptr_t)(bp) + (res)->bp_offset;		\
 	*(typeof(val) *)addr = val;					\

@@ -1969,6 +2067,12 @@ ptp_ocp_nvmem_device_put(struct nvmem_device **nvmemp)
 	*nvmemp = NULL;
 }
 
+static bool
+ptp_ocp_has_eeprom_data(struct ptp_ocp *bp)
+{
+	return smp_load_acquire(&bp->has_eeprom_data);
+}
+
 static void
 ptp_ocp_read_eeprom(struct ptp_ocp *bp)
 {

@@ -1977,11 +2081,11 @@ ptp_ocp_read_eeprom(struct ptp_ocp *bp)
 	const void *tag;
 	int ret;
 
-	if (!bp->i2c_ctrl)
-		return;
-
 	tag = NULL;
 	nvmem = NULL;
+	mutex_lock(&bp->eeprom_lock);
+	if (!bp->i2c_ctrl || ptp_ocp_has_eeprom_data(bp))
+		goto out;
 
 	for (map = bp->eeprom_map; map->len; map++) {
 		if (map->tag != tag) {

@@ -2001,10 +2105,12 @@ ptp_ocp_read_eeprom(struct ptp_ocp *bp)
 			goto fail;
 	}
 
-	bp->has_eeprom_data = true;
+	/* Publish the EEPROM fields before readers observe valid data. */
+	smp_store_release(&bp->has_eeprom_data, true);
 
 out:
 	ptp_ocp_nvmem_device_put(&nvmem);
+	mutex_unlock(&bp->eeprom_lock);
 	return;
 
 fail:

@@ -2012,6 +2118,291 @@ ptp_ocp_read_eeprom(struct ptp_ocp *bp)
 	goto out;
 }
 
+static int
+ptp_ocp_i2c_adapter_match(struct device *dev, const void *data)
+{
+	return !!i2c_verify_adapter(dev);
+}
+
+static struct i2c_adapter *
+ptp_ocp_i2c_root_adapter(struct ptp_ocp *bp)
+{
+	struct i2c_adapter *adapter;
+	struct device *dev;
+
+	if (!bp->i2c_ctrl)
+		return NULL;
+
+	dev = device_find_child(&bp->i2c_ctrl->dev, NULL,
+				ptp_ocp_i2c_adapter_match);
+	if (!dev)
+		return NULL;
+
+	adapter = i2c_verify_adapter(dev);
+	if (!adapter || !try_module_get(adapter->owner)) {
+		put_device(dev);
+		return NULL;
+	}
+
+	/* The device reference from device_find_child() is owned by the caller. */
+	return adapter;
+}
+
+static int
+ptp_ocp_r4006_init_nodes(struct ptp_ocp *bp)
+{
+	static const char * const channel_names[] = {
+		"i2c@0", "i2c@1", "i2c@2", "i2c@3",
+	};
+	static const char * const output_names[] = {
+		"led@0", "led@1", "led@2", "led@3", "led@4", "led@5",
+		"led@6", "led@7", "led@8", "led@9", "led@a", "led@b",
+		"led@c", "led@d", "led@e", "led@f", "led@10", "led@11",
+	};
+	static const u32 colors[] = {
+		LED_COLOR_ID_RED, LED_COLOR_ID_GREEN, LED_COLOR_ID_BLUE,
+	};
+	struct device *dev = &bp->pdev->dev;
+	struct ptp_ocp_r4006 *r4006;
+	unsigned int component = 0;
+	unsigned int node = 0;
+	unsigned int i, j, prop;
+	int ret;
+
+	if (bp->r4006)
+		return 0;
+
+	r4006 = devm_kzalloc(dev, sizeof(*r4006), GFP_KERNEL);
+	if (!r4006)
+		return -ENOMEM;
+
+	r4006->mux_node.name =
+		devm_kasprintf(dev, GFP_KERNEL, "ocp%d-r4006-mux", bp->id);
+	if (!r4006->mux_node.name)
+		return -ENOMEM;
+	r4006->mux_node.properties = r4006->mux_props;
+	r4006->mux_props[0] =
+		PROPERTY_ENTRY_STRING("compatible", "nxp,pca9546");
+	r4006->mux_props[1] = PROPERTY_ENTRY_U32("reg", 0x70);
+	r4006->mux_props[2] = PROPERTY_ENTRY_BOOL("i2c-mux-idle-disconnect");
+	r4006->mux_props[3] = PROPERTY_ENTRY_U32("#address-cells", 1);
+	r4006->mux_props[4] = PROPERTY_ENTRY_U32("#size-cells", 0);
+	r4006->node_group[node++] = &r4006->mux_node;
+
+	for (i = 0; i < OCP_R4006_MUX_CHANNELS; i++) {
+		r4006->channel_nodes[i].name = channel_names[i];
+		r4006->channel_nodes[i].parent = &r4006->mux_node;
+		r4006->channel_nodes[i].properties = r4006->channel_props[i];
+		r4006->channel_props[i][0] = PROPERTY_ENTRY_U32("reg", i);
+		r4006->channel_props[i][1] = PROPERTY_ENTRY_U32("#address-cells", 1);
+		r4006->channel_props[i][2] = PROPERTY_ENTRY_U32("#size-cells", 0);
+		r4006->node_group[node++] = &r4006->channel_nodes[i];
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ptp_ocp_r4006_sensors); i++) {
+		const struct ptp_ocp_r4006_i2c_device *sensor;
+
+		sensor = &ptp_ocp_r4006_sensors[i];
+		r4006->sensor_nodes[i].name = sensor->node_name;
+		r4006->sensor_nodes[i].parent =
+			&r4006->channel_nodes[sensor->channel];
+		r4006->sensor_nodes[i].properties = r4006->sensor_props[i];
+		r4006->sensor_props[i][0] =
+			PROPERTY_ENTRY_STRING("compatible", sensor->compatible);
+		r4006->sensor_props[i][1] =
+			PROPERTY_ENTRY_U32("reg", sensor->address);
+		r4006->node_group[node++] = &r4006->sensor_nodes[i];
+	}
+
+	r4006->led_node.name = "led-controller@34";
+	r4006->led_node.parent = &r4006->channel_nodes[1];
+	r4006->led_node.properties = r4006->led_props;
+	r4006->led_props[0] =
+		PROPERTY_ENTRY_STRING("compatible", "issi,is32fl3207");
+	r4006->led_props[1] = PROPERTY_ENTRY_U32("reg", 0x34);
+	r4006->led_props[2] = PROPERTY_ENTRY_U32("issi,riset-ohms", 4700);
+	r4006->led_props[3] = PROPERTY_ENTRY_U32("#address-cells", 1);
+	r4006->led_props[4] = PROPERTY_ENTRY_U32("#size-cells", 0);
+	r4006->node_group[node++] = &r4006->led_node;
+
+	for (i = 0; i < ARRAY_SIZE(ptp_ocp_r4006_leds); i++) {
+		const struct ptp_ocp_r4006_led *led = &ptp_ocp_r4006_leds[i];
+		u32 group_reg;
+
+		group_reg = min3(led->channel[0], led->channel[1],
+				 led->channel[2]);
+		r4006->led_nodes[i].name = led->node_name;
+		r4006->led_nodes[i].parent = &r4006->led_node;
+		r4006->led_nodes[i].properties = r4006->led_group_props[i];
+		prop = 0;
+		r4006->led_group_props[i][prop++] =
+			PROPERTY_ENTRY_U32("reg", group_reg);
+		r4006->led_group_props[i][prop++] =
+			PROPERTY_ENTRY_U32("color", LED_COLOR_ID_RGB);
+		r4006->led_group_props[i][prop++] =
+			PROPERTY_ENTRY_STRING("function", led->function);
+		if (led->has_function_enumerator)
+			r4006->led_group_props[i][prop++] =
+				PROPERTY_ENTRY_U32("function-enumerator",
+						   led->function_enumerator);
+		r4006->led_group_props[i][prop++] =
+			PROPERTY_ENTRY_U32("#address-cells", 1);
+		r4006->led_group_props[i][prop] =
+			PROPERTY_ENTRY_U32("#size-cells", 0);
+		r4006->node_group[node++] = &r4006->led_nodes[i];
+
+		for (j = 0; j < ARRAY_SIZE(led->channel); j++, component++) {
+			u8 channel = led->channel[j];
+
+			r4006->component_nodes[component].name =
+				output_names[channel];
+			r4006->component_nodes[component].parent =
+				&r4006->led_nodes[i];
+			r4006->component_nodes[component].properties =
+				r4006->component_props[component];
+			r4006->component_props[component][0] =
+				PROPERTY_ENTRY_U32("reg", channel);
+			r4006->component_props[component][1] =
+				PROPERTY_ENTRY_U32("color", colors[j]);
+			r4006->component_props[component][2] =
+				PROPERTY_ENTRY_U32("led-max-microamp", 8150);
+			r4006->node_group[node++] =
+				&r4006->component_nodes[component];
+		}
+	}
+
+	if (WARN_ON(node != OCP_R4006_NODE_COUNT))
+		return -EINVAL;
+
+	ret = software_node_register_node_group(r4006->node_group);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to register R4006 firmware nodes\n");
+
+	r4006->registered = true;
+	bp->r4006 = r4006;
+	return 0;
+}
+
+static int
+ptp_ocp_r4006_add_device(struct ptp_ocp *bp, struct i2c_adapter *adapter,
+			 const struct software_node *node, const char *type,
+			 u16 address)
+{
+	struct fwnode_handle *fwnode = software_node_fwnode(node);
+	struct i2c_board_info info = { };
+	struct i2c_client *client;
+
+	client = i2c_find_device_by_fwnode(fwnode);
+	if (client) {
+		put_device(&client->dev);
+		return 0;
+	}
+
+	strscpy(info.type, type, sizeof(info.type));
+	info.addr = address;
+	info.fwnode = fwnode;
+	client = i2c_new_client_device(adapter, &info);
+	if (IS_ERR(client))
+		return dev_err_probe(&bp->pdev->dev, PTR_ERR(client),
+				     "failed to add %s at %d-00%02x\n",
+				     type, i2c_adapter_id(adapter), address);
+
+	return 0;
+}
+
+static void
+ptp_ocp_r4006_populate_channel(struct ptp_ocp *bp, unsigned int channel)
+{
+	struct ptp_ocp_r4006 *r4006 = bp->r4006;
+	const struct software_node *node;
+	struct fwnode_handle *fwnode;
+	struct i2c_adapter *adapter;
+	unsigned int i;
+
+	fwnode = software_node_fwnode(&r4006->channel_nodes[channel]);
+	adapter = i2c_get_adapter_by_fwnode(fwnode);
+	if (!adapter)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(ptp_ocp_r4006_sensors); i++) {
+		const struct ptp_ocp_r4006_i2c_device *sensor;
+
+		sensor = &ptp_ocp_r4006_sensors[i];
+		if (sensor->channel != channel)
+			continue;
+
+		node = &r4006->sensor_nodes[i];
+		ptp_ocp_r4006_add_device(bp, adapter, node, sensor->type,
+					 sensor->address);
+	}
+
+	if (channel == 1)
+		ptp_ocp_r4006_add_device(bp, adapter, &r4006->led_node,
+					 "is32fl3207", 0x34);
+
+	i2c_put_adapter(adapter);
+}
+
+static void
+ptp_ocp_i2c_work(struct work_struct *work)
+{
+	struct ptp_ocp *bp = container_of(work, struct ptp_ocp, i2c_work.work);
+	const struct software_node *node;
+	struct i2c_adapter *adapter;
+	unsigned int channel;
+	int ret;
+
+	if (!bp->eeprom_map)
+		return;
+
+	adapter = ptp_ocp_i2c_root_adapter(bp);
+	if (!adapter)
+		return;
+
+	if (!ptp_ocp_has_eeprom_data(bp))
+		ptp_ocp_read_eeprom(bp);
+	if (!ptp_ocp_has_eeprom_data(bp) || memcmp(bp->board_id, "R4006", 5))
+		goto out_put_adapter;
+
+	ret = ptp_ocp_r4006_init_nodes(bp);
+	if (ret)
+		goto out_put_adapter;
+
+	node = &bp->r4006->mux_node;
+	ret = ptp_ocp_r4006_add_device(bp, adapter, node, "pca9546", 0x70);
+	if (ret)
+		goto out_put_adapter;
+
+	for (channel = 0; channel < OCP_R4006_MUX_CHANNELS; channel++)
+		ptp_ocp_r4006_populate_channel(bp, channel);
+
+out_put_adapter:
+	i2c_put_adapter(adapter);
+}
+
+static void
+ptp_ocp_r4006_unregister(struct ptp_ocp *bp)
+{
+	struct fwnode_handle *fwnode;
+	struct i2c_client *client;
+
+	disable_delayed_work_sync(&bp->i2c_work);
+	if (!bp->r4006)
+		return;
+
+	fwnode = software_node_fwnode(&bp->r4006->mux_node);
+	client = i2c_find_device_by_fwnode(fwnode);
+	if (client) {
+		i2c_unregister_device(client);
+		put_device(&client->dev);
+	}
+
+	if (bp->r4006->registered)
+		software_node_unregister_node_group(bp->r4006->node_group);
+	bp->r4006 = NULL;
+}
+
 static struct device *
 ptp_ocp_find_flash(struct ptp_ocp *bp)
 {

@@ -2166,9 +2557,9 @@ ptp_ocp_devlink_info_get(struct devlink *devlink, struct devlink_info_req *req,
 	if (err)
 		return err;
 
-	if (!bp->has_eeprom_data) {
+	if (!ptp_ocp_has_eeprom_data(bp)) {
 		ptp_ocp_read_eeprom(bp);
-		if (!bp->has_eeprom_data)
+		if (!ptp_ocp_has_eeprom_data(bp))
 			return 0;
 	}
 

@@ -3756,7 +4147,7 @@ serialnum_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	struct ptp_ocp *bp = dev_get_drvdata(dev);
 
-	if (!bp->has_eeprom_data)
+	if (!ptp_ocp_has_eeprom_data(bp))
 		ptp_ocp_read_eeprom(bp);
 
 	return sysfs_emit(buf, "%pM\n", bp->serial);

@@ -4727,6 +5118,7 @@ ptp_ocp_device_init(struct ptp_ocp *bp, struct pci_dev *pdev)
 
 	bp->ptp_info = ptp_ocp_clock_info;
 	spin_lock_init(&bp->lock);
+	mutex_init(&bp->eeprom_lock);
 
 	for (i = 0; i < __PORT_COUNT; i++)
 		bp->port[i].line = -1;

@@ -4865,6 +5257,7 @@ ptp_ocp_detach(struct ptp_ocp *bp)
 {
 	int i;
 
+	ptp_ocp_r4006_unregister(bp);
 	ptp_ocp_debugfs_remove_device(bp);
 	ptp_ocp_detach_sysfs(bp);
 	ptp_ocp_attr_group_del(bp);

@@ -5079,6 +5472,7 @@ ptp_ocp_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto out_disable;
 
 	INIT_DELAYED_WORK(&bp->sync_work, ptp_ocp_sync_work);
+	INIT_DELAYED_WORK(&bp->i2c_work, ptp_ocp_i2c_work);
 
 	/* compat mode.
 	 * Older FPGA firmware only returns 2 irq's.

@@ -5096,6 +5490,7 @@ ptp_ocp_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	err = ptp_ocp_register_resources(bp, id->driver_data);
 	if (err)
 		goto out;
+	mod_delayed_work(system_wq, &bp->i2c_work, 1);
 
 	bp->ptp = ptp_clock_register(&bp->ptp_info, &pdev->dev);
 	if (IS_ERR(bp->ptp)) {

@@ -5189,38 +5584,76 @@ static struct pci_driver ptp_ocp_driver = {
 	.shutdown	= ptp_ocp_remove,
 };
 
+static struct ptp_ocp *
+ptp_ocp_i2c_parent(struct device *child)
+{
+	struct device *dev = child;
+
+	while ((dev = dev->parent))
+		if (dev->driver && !strcmp(dev->driver->name, KBUILD_MODNAME))
+			return dev_get_drvdata(dev);
+
+	return NULL;
+}
+
+static bool
+ptp_ocp_i2c_is_root(struct ptp_ocp *bp, struct i2c_adapter *adapter)
+{
+	struct device *parent = adapter->dev.parent;
+
+	if (!parent)
+		return false;
+	if (bp->i2c_ctrl && parent == &bp->i2c_ctrl->dev)
+		return true;
+
+	/* The adapter can notify before ptp_ocp_register_i2c() stores it. */
+	return parent->parent == &bp->pdev->dev;
+}
+
 static int
 ptp_ocp_i2c_notifier_call(struct notifier_block *nb,
 			  unsigned long action, void *data)
 {
-	struct device *dev, *child = data;
+	struct i2c_adapter *adapter;
+	struct i2c_client *client;
+	struct device *child = data;
 	struct ptp_ocp *bp;
-	bool add;
 
 	switch (action) {
 	case BUS_NOTIFY_ADD_DEVICE:
-	case BUS_NOTIFY_DEL_DEVICE:
-		add = action == BUS_NOTIFY_ADD_DEVICE;
-		break;
-	default:
+		adapter = i2c_verify_adapter(child);
+		if (!adapter)
+			return 0;
+		bp = ptp_ocp_i2c_parent(child);
+		if (!bp)
+			return 0;
+		if (ptp_ocp_i2c_is_root(bp, adapter))
+			ptp_ocp_symlink(bp, child, "i2c");
+		mod_delayed_work(system_wq, &bp->i2c_work, 1);
 		return 0;
-	}
 
-	if (!i2c_verify_adapter(child))
+	case BUS_NOTIFY_DEL_DEVICE:
+		adapter = i2c_verify_adapter(child);
+		if (!adapter)
+			return 0;
+		bp = ptp_ocp_i2c_parent(child);
+		if (bp && ptp_ocp_i2c_is_root(bp, adapter))
+			sysfs_remove_link(&bp->dev.kobj, "i2c");
 		return 0;
 
-	dev = child;
-	while ((dev = dev->parent))
-		if (dev->driver && !strcmp(dev->driver->name, KBUILD_MODNAME))
-			goto found;
-	return 0;
+	case BUS_NOTIFY_BOUND_DRIVER:
+		client = i2c_verify_client(child);
+		if (!client || client->addr != 0x50)
+			return 0;
+		bp = ptp_ocp_i2c_parent(child);
+		if (!bp || !ptp_ocp_i2c_is_root(bp, client->adapter))
+			return 0;
+		mod_delayed_work(system_wq, &bp->i2c_work, 1);
+		return 0;
 
-found:
-	bp = dev_get_drvdata(dev);
-	if (add)
-		ptp_ocp_symlink(bp, child, "i2c");
-	else
-		sysfs_remove_link(&bp->dev.kobj, "i2c");
+	default:
+		return 0;
+	}
 
 	return 0;
 }