[net-next, v4, 03/11] ptp: track available ptp vclocks information

From: Yangbo Lu
Date: Fri Jun 25 2021 - 05:24:43 EST


Track available ptp vclocks information. Record index values
of available ptp vclocks during registering and unregistering.

This is preparation for supporting ptp vclocks info query
through ethtool.

Signed-off-by: Yangbo Lu <yangbo.lu@xxxxxxx>
---
Changes for v3:
- Added this patch.
Changes for v4:
- Dynamically allocated memory for vclock index storage.
---
drivers/ptp/ptp_clock.c | 15 ++++++++++++++-
drivers/ptp/ptp_private.h | 1 +
drivers/ptp/ptp_sysfs.c | 28 +++++++++++++++++++++++++---
3 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c
index 9fa8fea13616..3ded22b4a062 100644
--- a/drivers/ptp/ptp_clock.c
+++ b/drivers/ptp/ptp_clock.c
@@ -196,6 +196,7 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
{
struct ptp_clock *ptp;
int err = 0, index, major = MAJOR(ptp_devt);
+ size_t size;

if (info->n_alarm > PTP_MAX_ALARMS)
return ERR_PTR(-EINVAL);
@@ -236,9 +237,17 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
if (parent->class && strcmp(parent->class->name, "ptp") == 0)
ptp->is_virtual_clock = true;

- if (!ptp->is_virtual_clock)
+ if (!ptp->is_virtual_clock) {
ptp->max_vclocks = PTP_DEFAULT_MAX_VCLOCKS;

+ size = sizeof(int) * ptp->max_vclocks;
+ ptp->vclock_index = kzalloc(size, GFP_KERNEL);
+ if (!ptp->vclock_index) {
+ err = -ENOMEM;
+ goto no_mem_for_vclocks;
+ }
+ }
+
err = ptp_populate_pin_groups(ptp);
if (err)
goto no_pin_groups;
@@ -283,6 +292,8 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
no_pps:
ptp_cleanup_pin_groups(ptp);
no_pin_groups:
+ kfree(ptp->vclock_index);
+no_mem_for_vclocks:
if (ptp->kworker)
kthread_destroy_worker(ptp->kworker);
kworker_err:
@@ -307,6 +318,8 @@ int ptp_clock_unregister(struct ptp_clock *ptp)
ptp->defunct = 1;
wake_up_interruptible(&ptp->tsev_wq);

+ kfree(ptp->vclock_index);
+
if (ptp->kworker) {
kthread_cancel_delayed_work_sync(&ptp->aux_work);
kthread_destroy_worker(ptp->kworker);
diff --git a/drivers/ptp/ptp_private.h b/drivers/ptp/ptp_private.h
index 87cb55953b69..f75fadd9b244 100644
--- a/drivers/ptp/ptp_private.h
+++ b/drivers/ptp/ptp_private.h
@@ -49,6 +49,7 @@ struct ptp_clock {
struct kthread_delayed_work aux_work;
unsigned int max_vclocks;
unsigned int n_vclocks;
+ int *vclock_index;
struct mutex n_vclocks_mux; /* protect concurrent n_vclocks access */
bool is_virtual_clock;
};
diff --git a/drivers/ptp/ptp_sysfs.c b/drivers/ptp/ptp_sysfs.c
index 0c6411409a18..53353f015662 100644
--- a/drivers/ptp/ptp_sysfs.c
+++ b/drivers/ptp/ptp_sysfs.c
@@ -213,6 +213,9 @@ static ssize_t n_vclocks_store(struct device *dev,
if (!vclock)
goto out;

+ *(ptp->vclock_index + ptp->n_vclocks + i) =
+ vclock->clock->index;
+
dev_info(dev, "new virtual clock ptp%d\n",
vclock->clock->index);
}
@@ -223,6 +226,9 @@ static ssize_t n_vclocks_store(struct device *dev,
i = ptp->n_vclocks - num;
device_for_each_child_reverse(dev, &i,
unregister_vclock);
+
+ for (i = 1; i <= ptp->n_vclocks - num; i++)
+ *(ptp->vclock_index + ptp->n_vclocks - i) = -1;
}

if (num == 0)
@@ -256,6 +262,9 @@ static ssize_t max_vclocks_store(struct device *dev,
const char *buf, size_t count)
{
struct ptp_clock *ptp = dev_get_drvdata(dev);
+ unsigned int *vclock_index;
+ int err = -EINVAL;
+ size_t size;
u32 max;

if (kstrtou32(buf, 0, &max) || max == 0)
@@ -267,16 +276,29 @@ static ssize_t max_vclocks_store(struct device *dev,
if (mutex_lock_interruptible(&ptp->n_vclocks_mux))
return -ERESTARTSYS;

- if (max < ptp->n_vclocks) {
- mutex_unlock(&ptp->n_vclocks_mux);
- return -EINVAL;
+ if (max < ptp->n_vclocks)
+ goto out;
+
+ size = sizeof(int) * max;
+ vclock_index = kzalloc(size, GFP_KERNEL);
+ if (!vclock_index) {
+ err = -ENOMEM;
+ goto out;
}

+ size = sizeof(int) * ptp->n_vclocks;
+ memcpy(vclock_index, ptp->vclock_index, size);
+
+ kfree(ptp->vclock_index);
+ ptp->vclock_index = vclock_index;
ptp->max_vclocks = max;

mutex_unlock(&ptp->n_vclocks_mux);

return count;
+out:
+ mutex_unlock(&ptp->n_vclocks_mux);
+ return err;
}
static DEVICE_ATTR_RW(max_vclocks);

--
2.25.1