[PATCH net-next v8 07/15] dpll: sit9531x: add support to get and set priority on input pins

[Ali Rouhi]

From: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@xxxxxxxxxxxxxxxxx>

The priority table is ordered: the slot an input occupies is its
priority, and the device selects the lowest occupied slot whose signal is
qualified.  Getting the priority is therefore a search of the table for
the input, and setting it moves the input to the requested slot.

An input absent from the table has no priority to report, and the core is
told so rather than handed a made-up number.

Signed-off-by: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@xxxxxxxxxxxxxxxxx>
Assisted-by: Claude:claude-4-opus [chat]
Signed-off-by: Ali Rouhi <arouhi@xxxxxxxxxx>
---
 drivers/dpll/sit9531x/core.c | 43 +++++++++++++++++++
 drivers/dpll/sit9531x/dpll.c | 80 ++++++++++++++++++++++++++++++++++++
 2 files changed, 123 insertions(+)

diff --git a/drivers/dpll/sit9531x/core.c b/drivers/dpll/sit9531x/core.c
index f42c41d8a42e..e706f6942f65 100644
--- a/drivers/dpll/sit9531x/core.c
+++ b/drivers/dpll/sit9531x/core.c
@@ -414,6 +414,49 @@ static int sit9531x_prio_prg_commit(struct sit9531x_dev *sitdev)
 	return 0;
 }
 
+/*
+ * sit9531x_input_prio_get - read an input's priority slot for a PLL
+ * @input_idx:	input source in hardware encoding (see
+ *		sit9531x_input_hw_src())
+ * @prio:	output slot position (0 = highest); set to
+ *		SIT9531X_PRIO_MAX_SLOTS when the source is not in the table
+ *
+ * Scans the PLL's 12-slot priority table on Page 1 and returns the
+ * highest-priority (lowest-numbered) slot that references the source.
+ * This reads the value the chip actually holds rather than a cached
+ * default, so pin-get reflects the real hardware priority.
+ *
+ * Caller must hold sitdev->multiop_lock.
+ */
+int sit9531x_input_prio_get(struct sit9531x_dev *sitdev, u8 pll_idx,
+			    u8 input_idx, u8 *prio)
+{
+	u8 val, slot, src;
+	int rc;
+
+	lockdep_assert_held(&sitdev->multiop_lock);
+
+	if (pll_idx >= SIT9531X_NUM_PLLS)
+		return -EINVAL;
+
+	for (slot = 0; slot < SIT9531X_PRIO_MAX_SLOTS; slot++) {
+		rc = sit9531x_read_u8(sitdev,
+				      sit9531x_prio_reg(pll_idx, slot), &val);
+		if (rc)
+			return rc;
+
+		src = sit9531x_prio_slot_get(val, slot);
+
+		if (src == input_idx) {
+			*prio = slot;
+			return 0;
+		}
+	}
+
+	*prio = SIT9531X_PRIO_MAX_SLOTS;
+	return 0;
+}
+
 /*
  * Rebuild a PLL's membership mask from the source codes of its priority
  * table.  The mask is what the pin state getters test, so it is refreshed
diff --git a/drivers/dpll/sit9531x/dpll.c b/drivers/dpll/sit9531x/dpll.c
index 29088707a3e6..67b9fbba9f2e 100644
--- a/drivers/dpll/sit9531x/dpll.c
+++ b/drivers/dpll/sit9531x/dpll.c
@@ -417,10 +417,90 @@ sit9531x_dpll_input_pin_state_on_dpll_set(const struct dpll_pin *pin,
 	return rc;
 }
 
+/*
+ * sit9531x_dpll_input_pin_prio_get - read input pin priority
+ *
+ * reads the PLL's priority table on Page 1 (via
+ * sit9531x_input_prio_get()) and returns the slot the input
+ * occupies, so pin-get reports the real hardware priority rather
+ * than a software default.
+ */
+static int
+sit9531x_dpll_input_pin_prio_get(const struct dpll_pin *pin, void *pin_priv,
+				 const struct dpll_device *dpll, void *dpll_priv,
+				 u32 *prio, struct netlink_ext_ack *extack)
+{
+	struct sit9531x_dpll_pin *dpin = pin_priv;
+	struct sit9531x_dpll *sitdpll = dpll_priv;
+	struct sit9531x_dev *sitdev = sitdpll->dev;
+	u8 slot;
+	int rc;
+
+	mutex_lock(&sitdev->multiop_lock);
+	rc = sit9531x_input_prio_get(sitdev, sitdpll->id,
+				     sit9531x_input_hw_src(dpin->id), &slot);
+	mutex_unlock(&sitdev->multiop_lock);
+	if (rc)
+		return rc;
+
+	dpin->prio = slot;
+	*prio = slot;
+	return 0;
+}
+
+/*
+ * sit9531x_dpll_input_pin_prio_set - set input pin priority
+ *
+ * writes input priority table on Page 1 via
+ * core.c sit9531x_input_prio_set().  Forces holdover during update.
+ */
+static int
+sit9531x_dpll_input_pin_prio_set(const struct dpll_pin *pin, void *pin_priv,
+				 const struct dpll_device *dpll, void *dpll_priv,
+				 u32 prio, struct netlink_ext_ack *extack)
+{
+	struct sit9531x_dpll_pin *dpin = pin_priv;
+	struct sit9531x_dpll *sitdpll = dpll_priv;
+	struct sit9531x_dev *sitdev = sitdpll->dev;
+	int rc;
+
+	if (dpin->dir != DPLL_PIN_DIRECTION_INPUT) {
+		NL_SET_ERR_MSG(extack, "Priority applies only to input pins");
+		return -EINVAL;
+	}
+
+	if (prio >= SIT9531X_PRIO_MAX_SLOTS) {
+		NL_SET_ERR_MSG(extack, "Priority out of range (0-10)");
+		return -EINVAL;
+	}
+
+	mutex_lock(&sitdev->multiop_lock);
+	rc = sit9531x_input_prio_set(sitdev, sitdpll->id,
+				     sit9531x_input_hw_src(dpin->id),
+				     (u8)prio);
+	mutex_unlock(&sitdev->multiop_lock);
+
+	if (rc == -EINVAL) {
+		NL_SET_ERR_MSG(extack,
+			       "Pin is not a reference of this DPLL; connect it first");
+		return rc;
+	}
+	if (rc) {
+		NL_SET_ERR_MSG(extack, "Failed to set input priority");
+		return rc;
+	}
+
+	dpin->prio = (u8)prio;
+
+	return 0;
+}
+
 static const struct dpll_pin_ops sit9531x_dpll_input_pin_ops = {
 	.direction_get		= sit9531x_dpll_input_pin_direction_get,
 	.state_on_dpll_get	= sit9531x_dpll_input_pin_state_on_dpll_get,
 	.state_on_dpll_set	= sit9531x_dpll_input_pin_state_on_dpll_set,
+	.prio_get		= sit9531x_dpll_input_pin_prio_get,
+	.prio_set		= sit9531x_dpll_input_pin_prio_set,
 	/*
 	 * The measurement compares the PLL's running feedback divider with
 	 * its configured one, so it describes the device's own reference
-- 
2.43.0