[PATCH net-next v15 08/13] dpll: sit9531x: add support to get and set priority on input pins

[Ali Rouhi <arouhi@sitime.com>]

From: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@devoxsoftware.com>

Keep a priority per input and PLL, seeded from the table the part
loaded, and rebuild the table from it on a set. Priority and state are
separate: a priority set on a disconnected input applies when it is
connected. The range is 0 to 255 over eleven slots; inputs of equal
priority keep their table order.

A table rewritten outside the driver, by a profile reload or an I2C
tool, becomes the new configuration, and the poll notifies the pins
whose priority moved.

Signed-off-by: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@devoxsoftware.com>
Signed-off-by: Ali Rouhi <arouhi@sitime.com>
---
 drivers/dpll/sit9531x/core.c |  52 ++++++++++-
 drivers/dpll/sit9531x/core.h |   2 +
 drivers/dpll/sit9531x/dpll.c | 163 +++++++++++++++++++++++++++++++----
 drivers/dpll/sit9531x/dpll.h |   4 +-
 drivers/dpll/sit9531x/prop.c |   2 +
 5 files changed, 201 insertions(+), 22 deletions(-)

diff --git a/drivers/dpll/sit9531x/core.c b/drivers/dpll/sit9531x/core.c
index af6d7918958d..b4e44f0d9820 100644
--- a/drivers/dpll/sit9531x/core.c
+++ b/drivers/dpll/sit9531x/core.c
@@ -605,6 +605,33 @@ bool sit9531x_input_prio_present(struct sit9531x_dev *sitdev, u8 pll_idx,
 	return !!(sitdev->chan[pll_idx].prio_mask & BIT(input_idx));
 }
 
+/*
+ * @input_idx is in hardware encoding (see sit9531x_input_hw_src()). A source
+ * the PLL has never listed reports the last slot index, 10. 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;
+
+	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];
+	if (chan->cfg_known & BIT(input_idx))
+		*prio = chan->cfg_prio[input_idx];
+	else
+		*prio = SIT9531X_PRIO_MAX_SLOTS - 1;
+
+	return 0;
+}
+
 /*
  * Take the configured priorities from the hardware table: a listed source gets
  * the first slot it occupies.
@@ -735,7 +762,8 @@ static u8 sit9531x_prio_top_live(const struct sit9531x_dev *sitdev,
  * until it loses signal.
  */
 static u8 sit9531x_prio_activesel_pick(struct sit9531x_dev *sitdev,
-				       const u8 *old, const u8 *srcs, u8 cur)
+				       const u8 *old, const u8 *srcs, u8 cur,
+				       u8 routed)
 {
 	u8 top, i;
 
@@ -744,6 +772,12 @@ static u8 sit9531x_prio_activesel_pick(struct sit9531x_dev *sitdev,
 	    top != sit9531x_prio_top_live(sitdev, old))
 		return top;
 
+	if (sit9531x_prio_src_usable(routed) &&
+	    sit9531x_prio_src_live(sitdev, routed))
+		for (i = 0; i < SIT9531X_PRIO_MAX_SLOTS; i++)
+			if (srcs[i] == routed)
+				return routed;
+
 	cur = sit9531x_prio_src_canon(sitdev, cur & SIT9531X_PRIO_NIBBLE_MASK);
 
 	if (sit9531x_prio_src_usable(cur) &&
@@ -826,12 +860,18 @@ static int sit9531x_prio_table_commit(struct sit9531x_dev *sitdev, u8 pll_idx,
 
 	if (!empty) {
 		u8 sel = sit9531x_prio_slot_get(val, slot + 1);
+		u8 routed = SIT9531X_PRIO_SRC_NONE;
 
+		/*
+		 * The pick needs the signal state now, not as of the last poll.
+		 */
 		rc = sit9531x_input_mon_fetch(sitdev);
 		if (rc)
 			goto rollback;
+		if (!chan->stale && chan->routed_ref != SIT9531X_REF_INVALID)
+			routed = sit9531x_input_hw_src(chan->routed_ref);
 		sel = sit9531x_prio_activesel_pick(sitdev, chan->prio_srcs,
-						   srcs, sel);
+						   srcs, sel, routed);
 		val = sit9531x_prio_slot_set(val, slot + 1, sel);
 	}
 
@@ -2277,6 +2317,14 @@ static int sit9531x_dpll_pins_register(struct sit9531x_dpll *sitdpll)
 			goto error;
 		}
 
+		/*
+		 * Taken before the pin is published; see
+		 * sit9531x_dpll_pin_baseline().
+		 */
+		mutex_lock(&sitdpll->dev->multiop_lock);
+		sit9531x_dpll_pin_baseline(pin);
+		mutex_unlock(&sitdpll->dev->multiop_lock);
+
 		rc = sit9531x_dpll_pin_register(pin, index);
 		if (rc) {
 			kfree(pin);
diff --git a/drivers/dpll/sit9531x/core.h b/drivers/dpll/sit9531x/core.h
index c2fa6a17be43..fa779acf7de5 100644
--- a/drivers/dpll/sit9531x/core.h
+++ b/drivers/dpll/sit9531x/core.h
@@ -265,6 +265,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 56bdec4bcf67..f03145f4d1ee 100644
--- a/drivers/dpll/sit9531x/dpll.c
+++ b/drivers/dpll/sit9531x/dpll.c
@@ -34,6 +34,14 @@ sit9531x_dpll_is_xo_pin(const struct sit9531x_dpll_pin *pin)
 	       pin->id == SIT9531X_MAX_INPUTS;
 }
 
+/* Check if the pin's state and priority can change without a request */
+static bool
+sit9531x_dpll_is_selection_pin(const struct sit9531x_dpll_pin *pin)
+{
+	return sit9531x_dpll_is_input_pin(pin) &&
+	       !sit9531x_dpll_is_xo_pin(pin);
+}
+
 /*
  * A frozen PLL with an empty table is in forced holdover; report holdover only
  * while its holdover memory is valid.
@@ -396,11 +404,78 @@ sit9531x_dpll_input_pin_state_on_dpll_set(const struct dpll_pin *pin,
 	return rc;
 }
 
+/* Report the configured priority of the source on this PLL, listed or not */
+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;
+
+	*prio = slot;
+	return 0;
+}
+
+/*
+ * Rewrite the Page 1 priority table for a listed pin; an unlisted pin keeps the
+ * priority until it is connected.
+ */
+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 > U8_MAX) {
+		NL_SET_ERR_MSG(extack, "Priority out of range (0-255)");
+		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)
+		dpin->prio = prio;
+	mutex_unlock(&sitdev->multiop_lock);
+
+	if (rc) {
+		NL_SET_ERR_MSG(extack, "Failed to set input priority");
+		return rc;
+	}
+
+	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,
 	.operstate_on_dpll_get	= sit9531x_dpll_input_pin_operstate_on_dpll_get,
+	.prio_get		= sit9531x_dpll_input_pin_prio_get,
+	.prio_set		= sit9531x_dpll_input_pin_prio_set,
 };
 
 /*
@@ -477,6 +552,43 @@ sit9531x_dpll_pin_ops_get(const struct sit9531x_dpll_pin *pin)
 	return &sit9531x_dpll_input_pin_ops;
 }
 
+/* Caller must hold sitdev->multiop_lock. */
+static void sit9531x_dpll_pin_snapshot(struct sit9531x_dpll *sitdpll,
+				       const struct sit9531x_dpll_pin *pin,
+				       enum dpll_pin_state *state,
+				       enum dpll_pin_operstate *operstate,
+				       u8 *prio)
+{
+	struct sit9531x_dev *sitdev = sitdpll->dev;
+	u8 id = pin->id;
+
+	if (id == SIT9531X_INTSYNC_PIN_ID && sitdev->intsync_src == sitdpll->id)
+		*state = DPLL_PIN_STATE_DISCONNECTED;
+	else
+		sit9531x_dpll_selection_state_get(sitdev, sitdpll, id, state);
+	sit9531x_dpll_selection_operstate_get(sitdev, sitdpll, id, operstate);
+	if (sit9531x_input_prio_get(sitdev, sitdpll->id,
+				    sit9531x_input_hw_src(id), prio))
+		*prio = pin->prio;
+}
+
+/*
+ * Take the baseline before the pin is registered, so the first poll notifies a
+ * change a request made. Caller must hold sitdev->multiop_lock.
+ */
+void sit9531x_dpll_pin_baseline(struct sit9531x_dpll_pin *pin)
+{
+	struct sit9531x_dpll *sitdpll = pin->dpll;
+
+	lockdep_assert_held(&sitdpll->dev->multiop_lock);
+
+	if (!sit9531x_dpll_is_selection_pin(pin))
+		return;
+
+	sit9531x_dpll_pin_snapshot(sitdpll, pin, &pin->pin_state,
+				   &pin->operstate, &pin->prio);
+}
+
 /*
  * Send notifications for changed lock status and pin state; called from
  * sit9531x_dev_periodic_work().
@@ -511,32 +623,45 @@ void sit9531x_dpll_changes_check(struct sit9531x_dpll *sitdpll)
 	if (sitdpll->ntf_pending && !dpll_device_change_ntf(sitdpll->dpll_dev))
 		sitdpll->ntf_pending = false;
 
+	mutex_lock(&sitdev->multiop_lock);
 	list_for_each_entry(pin, &sitdpll->pins, list) {
-		const struct dpll_pin_ops *ops;
+		enum dpll_pin_operstate operstate;
 		enum dpll_pin_state state;
 		bool changed;
+		u8 prio;
 
-		/*
-		 * Only references and the INTSYNC destination change state
-		 * without a request.
-		 */
-		if (!sit9531x_dpll_is_input_pin(pin) ||
-		    sit9531x_dpll_is_xo_pin(pin))
+		if (!sit9531x_dpll_is_selection_pin(pin))
 			continue;
 
-		ops = sit9531x_dpll_pin_ops_get(pin);
-		rc = ops->state_on_dpll_get(pin->dpll_pin, pin,
-					    sitdpll->dpll_dev, sitdpll,
-					    &state, NULL);
-		if (rc)
-			continue;
+		sit9531x_dpll_pin_snapshot(sitdpll, pin, &state, &operstate,
+					   &prio);
+
+		changed = state != pin->pin_state ||
+			  operstate != pin->operstate || prio != pin->prio;
+		if (changed)
+			dev_dbg(sitdev->dev,
+				"%s: state %u->%u operstate %u->%u prio %u->%u\n",
+				pin->label, pin->pin_state, state,
+				pin->operstate, operstate, pin->prio, prio);
 
-		changed = pin->seen && state != pin->pin_state;
 		pin->pin_state = state;
-		pin->seen = true;
-		if (changed) {
-			dev_dbg(sitdev->dev, "%s state changed\n", pin->label);
-			dpll_pin_change_ntf(pin->dpll_pin);
-		}
+		pin->operstate = operstate;
+		pin->prio = prio;
+
+		/*
+		 * The notification takes DPLL-subsystem locks the callbacks run
+		 * under; send it after dropping multiop_lock.
+		 */
+		if (changed)
+			pin->ntf_pending = true;
+	}
+	mutex_unlock(&sitdev->multiop_lock);
+
+	list_for_each_entry(pin, &sitdpll->pins, list) {
+		if (!pin->ntf_pending)
+			continue;
+		if (dpll_pin_change_ntf(pin->dpll_pin))
+			continue;
+		pin->ntf_pending = false;
 	}
 }
diff --git a/drivers/dpll/sit9531x/dpll.h b/drivers/dpll/sit9531x/dpll.h
index 6c02aa131328..3ded177b871b 100644
--- a/drivers/dpll/sit9531x/dpll.h
+++ b/drivers/dpll/sit9531x/dpll.h
@@ -30,7 +30,8 @@ struct sit9531x_dpll_pin {
 	u8				id;		/* hardware index */
 	u8				prio;
 	enum dpll_pin_state		pin_state;
-	bool				seen;	/* baseline taken by the poll */
+	enum dpll_pin_operstate		operstate;
+	bool				ntf_pending;
 };
 
 struct sit9531x_dpll {
@@ -56,6 +57,7 @@ int  sit9531x_dpll_register(struct sit9531x_dpll *sitdpll);
 void sit9531x_dpll_unregister(struct sit9531x_dpll *sitdpll);
 
 void sit9531x_dpll_lock_status_baseline(struct sit9531x_dpll *sitdpll);
+void sit9531x_dpll_pin_baseline(struct sit9531x_dpll_pin *pin);
 void sit9531x_dpll_changes_check(struct sit9531x_dpll *sitdpll);
 
 #endif /* _SIT9531X_DPLL_H */
diff --git a/drivers/dpll/sit9531x/prop.c b/drivers/dpll/sit9531x/prop.c
index b9405eba49a3..69877fc94028 100644
--- a/drivers/dpll/sit9531x/prop.c
+++ b/drivers/dpll/sit9531x/prop.c
@@ -146,6 +146,7 @@ sit9531x_pin_props_get(struct sit9531x_dev *sitdev,
 		/* INTSYNC destination: a reference the PLL can lock to. */
 		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 (sync_out) {
@@ -161,6 +162,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.39.2 (Apple Git-143)