1 files changed, 53 insertions, 29 deletions

diff --git a/drivers/ptp/ptp_vclock.c b/drivers/ptp/ptp_vclock.c
index cb179a3ea5081..1c0ed4805c0aa 100644
--- a/drivers/ptp/ptp_vclock.c
+++ b/drivers/ptp/ptp_vclock.c
@@ -5,6 +5,7 @@
  * Copyright 2021 NXP
  */
 #include <linux/slab.h>
+#include <linux/hashtable.h>
 #include "ptp_private.h"
 
 #define PTP_VCLOCK_CC_SHIFT		31
@@ -13,6 +14,32 @@
 #define PTP_VCLOCK_FADJ_DENOMINATOR	15625ULL
 #define PTP_VCLOCK_REFRESH_INTERVAL	(HZ * 2)
 
+/* protects vclock_hash addition/deletion */
+static DEFINE_SPINLOCK(vclock_hash_lock);
+
+static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8);
+
+static void ptp_vclock_hash_add(struct ptp_vclock *vclock)
+{
+	spin_lock(&vclock_hash_lock);
+
+	hlist_add_head_rcu(&vclock->vclock_hash_node,
+			   &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]);
+
+	spin_unlock(&vclock_hash_lock);
+}
+
+static void ptp_vclock_hash_del(struct ptp_vclock *vclock)
+{
+	spin_lock(&vclock_hash_lock);
+
+	hlist_del_init_rcu(&vclock->vclock_hash_node);
+
+	spin_unlock(&vclock_hash_lock);
+
+	synchronize_rcu();
+}
+
 static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 {
 	struct ptp_vclock *vclock = info_to_vclock(ptp);
@@ -68,7 +95,7 @@ static int ptp_vclock_gettimex(struct ptp_clock_info *ptp,
 	int err;
 	u64 ns;
 
-	err = pptp->info->gettimex64(pptp->info, &pts, sts);
+	err = pptp->info->getcyclesx64(pptp->info, &pts, sts);
 	if (err)
 		return err;
 
@@ -104,7 +131,7 @@ static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp,
 	int err;
 	u64 ns;
 
-	err = pptp->info->getcrosststamp(pptp->info, xtstamp);
+	err = pptp->info->getcrosscycles(pptp->info, xtstamp);
 	if (err)
 		return err;
 
@@ -143,10 +170,7 @@ static u64 ptp_vclock_read(const struct cyclecounter *cc)
 	struct ptp_clock *ptp = vclock->pclock;
 	struct timespec64 ts = {};
 
-	if (ptp->info->gettimex64)
-		ptp->info->gettimex64(ptp->info, &ts, NULL);
-	else
-		ptp->info->gettime64(ptp->info, &ts);
+	ptp->info->getcycles64(ptp->info, &ts);
 
 	return timespec64_to_ns(&ts);
 }
@@ -168,17 +192,19 @@ struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
 
 	vclock->pclock = pclock;
 	vclock->info = ptp_vclock_info;
-	if (pclock->info->gettimex64)
+	if (pclock->info->getcyclesx64)
 		vclock->info.gettimex64 = ptp_vclock_gettimex;
 	else
 		vclock->info.gettime64 = ptp_vclock_gettime;
-	if (pclock->info->getcrosststamp)
+	if (pclock->info->getcrosscycles)
 		vclock->info.getcrosststamp = ptp_vclock_getcrosststamp;
 	vclock->cc = ptp_vclock_cc;
 
 	snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",
 		 pclock->index);
 
+	INIT_HLIST_NODE(&vclock->vclock_hash_node);
+
 	spin_lock_init(&vclock->lock);
 
 	vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);
@@ -190,11 +216,15 @@ struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
 	timecounter_init(&vclock->tc, &vclock->cc, 0);
 	ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);
 
+	ptp_vclock_hash_add(vclock);
+
 	return vclock;
 }
 
 void ptp_vclock_unregister(struct ptp_vclock *vclock)
 {
+	ptp_vclock_hash_del(vclock);
+
 	ptp_clock_unregister(vclock->clock);
 	kfree(vclock);
 }
@@ -235,37 +265,31 @@ out:
 }
 EXPORT_SYMBOL(ptp_get_vclocks_index);
 
-ktime_t ptp_convert_timestamp(const struct skb_shared_hwtstamps *hwtstamps,
-			      int vclock_index)
+ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index)
 {
-	char name[PTP_CLOCK_NAME_LEN] = "";
+	unsigned int hash = vclock_index % HASH_SIZE(vclock_hash);
 	struct ptp_vclock *vclock;
-	struct ptp_clock *ptp;
 	unsigned long flags;
-	struct device *dev;
 	u64 ns;
+	u64 vclock_ns = 0;
 
-	snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", vclock_index);
-	dev = class_find_device_by_name(ptp_class, name);
-	if (!dev)
-		return 0;
+	ns = ktime_to_ns(*hwtstamp);
 
-	ptp = dev_get_drvdata(dev);
-	if (!ptp->is_virtual_clock) {
-		put_device(dev);
-		return 0;
-	}
+	rcu_read_lock();
 
-	vclock = info_to_vclock(ptp->info);
+	hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) {
+		if (vclock->clock->index != vclock_index)
+			continue;
 
-	ns = ktime_to_ns(hwtstamps->hwtstamp);
+		spin_lock_irqsave(&vclock->lock, flags);
+		vclock_ns = timecounter_cyc2time(&vclock->tc, ns);
+		spin_unlock_irqrestore(&vclock->lock, flags);
+		break;
+	}
 
-	spin_lock_irqsave(&vclock->lock, flags);
-	ns = timecounter_cyc2time(&vclock->tc, ns);
-	spin_unlock_irqrestore(&vclock->lock, flags);
+	rcu_read_unlock();
 
-	put_device(dev);
-	return ns_to_ktime(ns);
+	return ns_to_ktime(vclock_ns);
 }
 EXPORT_SYMBOL(ptp_convert_timestamp);
 #endif