+static int of_i3c_master_add_i3c_dev(struct i3c_master_controller *master,
+ struct device_node *node, u32 *reg)
+{
+ struct i3c_device_info info = { };
+ enum i3c_addr_slot_status addrstatus;
+ struct i3c_device *i3cdev;
+ u32 init_dyn_addr = 0;
+
+ if (reg[0]) {
+ if (reg[0] > I3C_MAX_ADDR)
+ return -EINVAL;
+
+ addrstatus = i3c_bus_get_addr_slot_status(master->bus, reg[0]);
+ if (addrstatus != I3C_ADDR_SLOT_FREE)
+ return -EINVAL;
+ }
+
+ info.static_addr = reg[0];
+
+ if (!of_property_read_u32(node, "assigned-address", &init_dyn_addr)) {
+ if (init_dyn_addr > I3C_MAX_ADDR)
+ return -EINVAL;
+
+ addrstatus = i3c_bus_get_addr_slot_status(master->bus,
+ init_dyn_addr);
+ if (addrstatus != I3C_ADDR_SLOT_FREE)
+ return -EINVAL;
+ }
+
+ info.pid = ((u64)reg[1] << 32) | reg[2];
+
+ if ((info.pid & GENMASK_ULL(63, 48)) ||
+ I3C_PID_RND_LOWER_32BITS(info.pid))
+ return -EINVAL;
+
+ i3cdev = i3c_master_alloc_i3c_dev(master, &info, &i3c_device_type);
+ if (IS_ERR(i3cdev))
+ return PTR_ERR(i3cdev);
+
+ i3cdev->init_dyn_addr = init_dyn_addr;
+ i3cdev->dev.of_node = node;
+ list_add_tail(&i3cdev->common.node, &master->bus->devs.i3c);
+
+ return 0;
+}
+