[PATCH v10 07/14] 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.

A priority change can shift the slots of the other inputs on the same
PLL.  The core notifies only the pin the request named, so this callback
notifies the rest itself, with __dpll_pin_change_ntf(): the core already
holds the device lock across a pin op, and that helper is the one that
expects to be called with it held.

The two directions treat an absent input differently, because the core
asks different things of them.  A set names a slot to move an input to,
so an input that is not in the table is refused with -EINVAL rather than
inserted behind the request's back.  A get has to return a number, so an
absent input reports the lowest slot -- the same value a real
lowest-priority input reports, since the interface has no way to say
"not in the table".

Signed-off-by: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@xxxxxxxxxxxxxxxxx>
Assisted-by: Claude:claude-4-opus [chat]
Signed-off-by: Ali Rouhi <arouhi@xxxxxxxxxx>
---

Notes:
    Changes in v10:
    Notified the sibling pins with the helper meant for a caller that
    already holds the device lock, left the pin the request named to the
    core, and skipped a pin that is being unregistered.

 drivers/dpll/sit9531x/core.c | 119 ++++++++++++++++++++++++++------
 drivers/dpll/sit9531x/core.h |  10 +++
 drivers/dpll/sit9531x/dpll.c | 129 +++++++++++++++++++++++++++++++++++
 drivers/dpll/sit9531x/prop.c |   2 +
 4 files changed, 240 insertions(+), 20 deletions(-)

diff --git a/drivers/dpll/sit9531x/core.c b/drivers/dpll/sit9531x/core.c
index 84c1f86e6d4c..ac184c93258c 100644
--- a/drivers/dpll/sit9531x/core.c
+++ b/drivers/dpll/sit9531x/core.c
@@ -543,34 +543,87 @@ bool sit9531x_input_prio_present(struct sit9531x_dev *sitdev, u8 pll_idx,
 }
 
 /*
- * 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
- * from exactly the values the table holds -- here after a write, and once
- * per poll from the read-back in sit9531x_chan_state_fetch().
+ * 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)
+ *
+ * Reports the last slot this source occupied on this PLL.  The value is
+ * cached from the hardware table read at startup and refreshed after every
+ * table write and poll read-back, so pin-get reflects hardware state without
+ * issuing synchronous register reads per pin.  A source with no known slot
+ * falls back to the lowest-priority valid slot.
+ *
+ * Caller must hold sitdev->multiop_lock.
+ */
+int sit9531x_input_prio_get(struct sit9531x_dev *sitdev, u8 pll_idx,
+			    u8 input_idx, u8 *prio)
+{
+	const struct sit9531x_chan *chan;
+	u8 slot;
+
+	lockdep_assert_held(&sitdev->multiop_lock);
+
+	if (pll_idx >= SIT9531X_NUM_PLLS)
+		return -EINVAL;
+	input_idx = sit9531x_prio_src_canon(sitdev, input_idx);
+	if (input_idx >= SIT9531X_PRIO_NUM_SRC)
+		return -EINVAL;
+
+	chan = &sitdev->chan[pll_idx];
+	slot = chan->prio_last[input_idx];
+	if (!slot)
+		slot = SIT9531X_PRIO_MAX_SLOTS;
+
+	*prio = slot - 1;
+
+	return 0;
+}
+
+/*
+ * Refresh a PLL's cached view of its priority table from the source codes
+ * the table holds -- here after a write, and once per poll from the
+ * read-back in sit9531x_chan_state_fetch().
+ *
+ * The membership mask is what the pin state getters test; the per-slot
+ * copy and the last-slot-seen array are what priority get answers from,
+ * so neither costs a register read per pin.
  */
 static void sit9531x_prio_mask_build(struct sit9531x_dev *sitdev, u8 pll_idx,
 				     const u8 *srcs, u8 written)
 {
+	struct sit9531x_chan *chan = &sitdev->chan[pll_idx];
+	u8 first[SIT9531X_PRIO_NUM_SRC] = { 0 };
 	u16 mask = 0;
-	u8 slot;
-
-	/*
-	 * A table written only in part is not described by the values the
-	 * request carried, and there is nothing here to describe it with
-	 * instead, so the mask it had stands until the next poll reads the
-	 * table back.
-	 */
-	if (written < SIT9531X_PRIO_MAX_SLOTS)
-		return;
+	u8 slot, src, src_canon;
 
 	for (slot = 0; slot < SIT9531X_PRIO_MAX_SLOTS; slot++) {
-		u8 src = srcs[slot] & SIT9531X_PRIO_NIBBLE_MASK;
+		/*
+		 * A slot the caller could not write still holds what it
+		 * held before, so take that rather than the value the
+		 * request wanted to put there.
+		 */
+		src = slot < written ? srcs[slot] : chan->prio_srcs[slot];
+		src &= SIT9531X_PRIO_NIBBLE_MASK;
+		chan->prio_srcs[slot] = src;
+		src_canon = sit9531x_prio_src_canon(sitdev, src);
+		if (!sit9531x_prio_src_usable(src))
+			continue;
 
-		if (sit9531x_prio_src_usable(src))
-			mask |= BIT(src);
+		mask |= BIT(src_canon);
+		if (!first[src_canon])
+			first[src_canon] = slot + 1;
 	}
 
-	sitdev->chan[pll_idx].prio_mask = mask;
+	/*
+	 * Assign unconditionally: a source that has left the table has no
+	 * slot, and leaving its old one behind would keep reporting it as
+	 * listed for as long as the device runs.
+	 */
+	for (src = 0; src < SIT9531X_PRIO_NUM_SRC; src++)
+		chan->prio_last[src] = first[src];
+
+	chan->prio_mask = mask;
 }
 
 /* Attempts to release a forced holdover before reporting it stuck. */
@@ -579,7 +632,8 @@ static void sit9531x_prio_mask_build(struct sit9531x_dev *sitdev, u8 pll_idx,
 static int sit9531x_prio_table_commit(struct sit9531x_dev *sitdev, u8 pll_idx,
 				      const u8 *srcs)
 {
-	u8 val, slot, attempt, written = 0;
+	struct sit9531x_chan *chan = &sitdev->chan[pll_idx];
+	u8 val, slot, attempt, written = 0, restored = 0;
 	int rc = 0, prg_rc, ho_rc = 0;
 	u16 reg;
 
@@ -629,9 +683,34 @@ static int sit9531x_prio_table_commit(struct sit9531x_dev *sitdev, u8 pll_idx,
 
 	written = SIT9531X_PRIO_MAX_SLOTS;
 
+	if (rc && written) {
+		/*
+		 * Put the slots that did reach the device back the way they
+		 * were.  Latching a table that is neither the previous order
+		 * nor the requested one hands the reference selection loop
+		 * a priority list nobody asked for.  The cache is the table
+		 * as last read, which is what those slots held.
+		 */
+		for (slot = 0; slot < written; slot += 2) {
+			u8 old;
+
+			old = sit9531x_prio_slot_set(0, slot,
+						     chan->prio_srcs[slot]);
+			old = sit9531x_prio_slot_set(old, slot + 1,
+						     chan->prio_srcs[slot + 1]);
+			if (sit9531x_write_u8(sitdev,
+					      sit9531x_prio_reg(pll_idx, slot),
+					      old))
+				break;
+
+			restored = slot + 2;
+		}
+		written = restored;
+	}
+
 commit:
 	/*
-	 * Latch unconditionally: slots written before a failed write are in
+	 * Latch unconditionally: the slots that reached the device are in
 	 * the table regardless, so the latch keeps hardware and the cache
 	 * refresh below consistent with what was actually written.
 	 */
diff --git a/drivers/dpll/sit9531x/core.h b/drivers/dpll/sit9531x/core.h
index 033e1059bde9..2c5d0100b450 100644
--- a/drivers/dpll/sit9531x/core.h
+++ b/drivers/dpll/sit9531x/core.h
@@ -126,6 +126,12 @@ struct sit9531x_out {
  * @ho_freeze:		holdover freeze active
  * @ho_valid:		holdover memory acquired, i.e. the holdover window
  *			holds a valid estimate to fall back on
+ * @prio_srcs:		cached copy of the priority table, one source code
+ *			per slot; refreshed together with @prio_mask, so
+ *			priority reads generate no register traffic
+ * @prio_last:		slot each source occupies, plus one (0 = the source
+ *			is not in the table); refreshed from the same scan
+ *			as @prio_mask, so the two never disagree
  * @prio_mask:		bit per hardware source code present in this PLL's
  *			priority table, i.e. the sources it may select.  Read
  *			back from the table by the periodic worker and
@@ -140,6 +146,8 @@ struct sit9531x_chan {
 	bool		inner_lol;
 	bool		ho_freeze;
 	bool		ho_valid;
+	u8		prio_srcs[SIT9531X_PRIO_MAX_SLOTS];
+	u8		prio_last[SIT9531X_PRIO_NUM_SRC];
 	u16		prio_mask;
 };
 
@@ -237,6 +245,8 @@ bool sit9531x_input_prio_present(struct sit9531x_dev *sitdev,
 				 u8 pll_idx, u8 input_idx);
 int sit9531x_input_prio_set(struct sit9531x_dev *sitdev, u8 pll_idx,
 			    u8 input_idx, u8 prio);
+int sit9531x_input_prio_get(struct sit9531x_dev *sitdev, u8 pll_idx,
+			    u8 input_idx, u8 *prio);
 int sit9531x_input_prio_remove(struct sit9531x_dev *sitdev, u8 pll_idx,
 			       u8 input_idx);
 int sit9531x_input_prio_add(struct sit9531x_dev *sitdev, u8 pll_idx,
diff --git a/drivers/dpll/sit9531x/dpll.c b/drivers/dpll/sit9531x/dpll.c
index 4437c95f578f..56a8213dee07 100644
--- a/drivers/dpll/sit9531x/dpll.c
+++ b/drivers/dpll/sit9531x/dpll.c
@@ -489,10 +489,139 @@ 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
+ *
+ * Reports the cached slot from sit9531x_input_prio_get().  The cache is
+ * refreshed from hardware at startup and by periodic read-back, so pin-get
+ * reports hardware priority without synchronous per-pin I2C reads.
+ */
+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 dpll_pin *changed[SIT9531X_MAX_INPUTS + 1];
+	struct sit9531x_dpll_pin *sibling;
+	struct sit9531x_dpll_pin *dpin = pin_priv;
+	struct sit9531x_dpll *sitdpll = dpll_priv;
+	struct sit9531x_dev *sitdev = sitdpll->dev;
+	u8 changed_cnt = 0, hw_src, slot;
+	int get_rc, 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);
+	if (!rc) {
+		list_for_each_entry(sibling, &sitdpll->pins, list) {
+			if (!sit9531x_dpll_is_input_pin(sibling) ||
+			    sit9531x_dpll_is_xo_pin(sibling))
+				continue;
+
+			hw_src = sit9531x_input_hw_src(sibling->id);
+			get_rc = sit9531x_input_prio_get(sitdev, sitdpll->id,
+							 hw_src, &slot);
+			if (get_rc)
+				continue;
+
+			if (sibling->prio == slot)
+				continue;
+
+			sibling->prio = slot;
+
+			/*
+			 * The core notifies the pin the request named, so
+			 * only the others are collected here.  A pin whose
+			 * dpll_pin is already NULL is mid-unregister: that
+			 * runs with the device lock dropped between the
+			 * unregister and the free, so it can be seen from
+			 * here, and notifying through it would follow a
+			 * pointer that is on its way out.
+			 */
+			if (sibling == dpin || !sibling->dpll_pin)
+				continue;
+
+			if (changed_cnt < ARRAY_SIZE(changed))
+				changed[changed_cnt++] = sibling->dpll_pin;
+		}
+	}
+	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 == -ERANGE) {
+		NL_SET_ERR_MSG(extack,
+			       "Priority is past the last reference this DPLL lists");
+		return rc;
+	}
+	if (rc) {
+		NL_SET_ERR_MSG(extack, "Failed to set input priority");
+		return rc;
+	}
+
+	/*
+	 * The core notifies only the pin the request named, so the ones whose
+	 * slots moved are notified here.  This runs inside a pin op, where
+	 * the core already holds the lock the notification needs, so it is
+	 * the underscore helper rather than the wrapper that takes it.
+	 */
+	while (changed_cnt--)
+		__dpll_pin_change_ntf(changed[changed_cnt]);
+
+	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,
 };
 
 /*
diff --git a/drivers/dpll/sit9531x/prop.c b/drivers/dpll/sit9531x/prop.c
index 1a09bd168163..4c6a2249300f 100644
--- a/drivers/dpll/sit9531x/prop.c
+++ b/drivers/dpll/sit9531x/prop.c
@@ -191,6 +191,7 @@ sit9531x_pin_props_get(struct sit9531x_dev *sitdev,
 		 */
 		props->dpll_props.type = DPLL_PIN_TYPE_INT_OSCILLATOR;
 		props->dpll_props.capabilities =
+			DPLL_PIN_CAPABILITIES_PRIORITY_CAN_CHANGE |
 			DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE;
 		curr_freq = 0;
 	} else if (dir == DPLL_PIN_DIRECTION_OUTPUT &&
@@ -214,6 +215,7 @@ sit9531x_pin_props_get(struct sit9531x_dev *sitdev,
 	} else if (dir == DPLL_PIN_DIRECTION_INPUT) {
 		props->dpll_props.type = DPLL_PIN_TYPE_EXT;
 		props->dpll_props.capabilities =
+			DPLL_PIN_CAPABILITIES_PRIORITY_CAN_CHANGE |
 			DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE;
 		curr_freq = sitdev->ref[index].freq;
 	} else {
-- 
2.43.0