mk-rootfs.sh

hc

2023-12-11 072de836f53be56a70cecf70b43ae43b7ce17376

 kernel/drivers/i2c/busses/i2c-fsi.c
..
..
@@ -658,13 +658,29 @@
658
658
 	.functionality = fsi_i2c_functionality,
659
659
 };
660
660
 
661
+static struct device_node *fsi_i2c_find_port_of_node(struct device_node *fsi,
662
+						     int port)
663
+{
664
+	struct device_node *np;
665
+	u32 port_no;
666
+	int rc;
667
+
668
+	for_each_child_of_node(fsi, np) {
669
+		rc = of_property_read_u32(np, "reg", &port_no);
670
+		if (!rc && port_no == port)
671
+			return np;
672
+	}
673
+
674
+	return NULL;
675
+}
676
+
661
677
 static int fsi_i2c_probe(struct device *dev)
662
678
 {
663
679
 	struct fsi_i2c_master *i2c;
664
680
 	struct fsi_i2c_port *port;
665
681
 	struct device_node *np;
682
+	u32 port_no, ports, stat;
666
683
 	int rc;
667
-	u32 port_no;
668
684
 
669
685
 	i2c = devm_kzalloc(dev, sizeof(*i2c), GFP_KERNEL);
670
686
 	if (!i2c)
..
..
@@ -678,15 +694,23 @@
678
694
 	if (rc)
679
695
 		return rc;
680
696
 
681
-	/* Add adapter for each i2c port of the master. */
682
-	for_each_available_child_of_node(dev->of_node, np) {
683
-		rc = of_property_read_u32(np, "reg", &port_no);
684
-		if (rc || port_no > USHRT_MAX)
697
+	rc = fsi_i2c_read_reg(i2c->fsi, I2C_FSI_STAT, &stat);
698
+	if (rc)
699
+		return rc;
700
+
701
+	ports = FIELD_GET(I2C_STAT_MAX_PORT, stat) + 1;
702
+	dev_dbg(dev, "I2C master has %d ports\n", ports);
703
+
704
+	for (port_no = 0; port_no < ports; port_no++) {
705
+		np = fsi_i2c_find_port_of_node(dev->of_node, port_no);
706
+		if (!of_device_is_available(np))
685
707
 			continue;
686
708
 
687
709
 		port = kzalloc(sizeof(*port), GFP_KERNEL);
688
-		if (!port)
710
+		if (!port) {
711
+			of_node_put(np);
689
712
 			break;
713
+		}
690
714
 
691
715
 		port->master = i2c;
692
716
 		port->port = port_no;