[PATCH 2/3] ptp: add a software clock based on clock_monotonic_raw

Torben Hohn
2011-03-03T17:27:25Z

First version of a software clock. Not very useful yet,
as it doesnt generate events, but at least it allows
for testing the ptp framework without special hardware.

Signed-off-by: Torben Hohn <torbenh@gmx.de>
Cc: Richard Cochran <richard.cochran@omicron.at>
Cc: John Stultz <johnstul@us.ibm.com>
Cc: Thomas Gleixner <tglx@linutronix.de>
---
drivers/ptp/Kconfig | 6 +
drivers/ptp/Makefile | 1 +
drivers/ptp/ptp_software.c | 221 ++++++++++++++++++++++++++++++++++++++++++++
3 files changed, 228 insertions(+), 0 deletions(-)
create mode 100644 drivers/ptp/ptp_software.c

diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index d99e6f9..0427361 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -40,4 +40,10 @@ config PTP_1588_CLOCK_IXP46X
To compile this driver as a module, choose M here: the module
will be called ptp_ixp46x.

+config PTP_1588_CLOCK_SOFTWARE
+ tristate "software PTP clock"
+ depends on PTP_1588_CLOCK
+ help
+ This driver adds a software PTP clock.
+
endmenu
diff --git a/drivers/ptp/Makefile b/drivers/ptp/Makefile
index f6933e8..8f18a55 100644
--- a/drivers/ptp/Makefile
+++ b/drivers/ptp/Makefile
@@ -5,3 +5,4 @@
ptp-y := ptp_clock.o ptp_chardev.o ptp_sysfs.o
obj-$(CONFIG_PTP_1588_CLOCK) += ptp.o
obj-$(CONFIG_PTP_1588_CLOCK_IXP46X) += ptp_ixp46x.o
+obj-$(CONFIG_PTP_1588_CLOCK_SOFTWARE) += ptp_software.o
diff --git a/drivers/ptp/ptp_software.c b/drivers/ptp/ptp_software.c
new file mode 100644
index 0000000..1182426
--- /dev/null
+++ b/drivers/ptp/ptp_software.c
@@ -0,0 +1,221 @@
+/*
+ * Software PTP Clock
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <linux/ptp_clock_kernel.h>
+
+#define DRIVER "ptp_sofware"
+#define N_EXT_TS 2
+
+struct ixp_clock {
+ struct ptp_clock *ptp_clock;
+ struct ptp_clock_info caps;
+ s64 offset_ns;
+ struct timespec last_timestamp;
+ s32 freq;
+};
+
+DEFINE_SEQLOCK(clock_lock);
+
+
+/*
+ * Register access functions
+ */
+
+static struct timespec timespec_mul_freq(struct timespec val, u32 ppb)
+{
+ struct timespec retval;
+ u32 remain_sec;
+ u64 muled_sec = (u64) val.tv_sec * ppb;
+ u64 muled_nsec = (u64) val.tv_nsec * ppb;
+
+ muled_sec = div_u64_rem(muled_sec, NSEC_PER_SEC, &remain_sec);
+ muled_nsec = div_u64(muled_nsec, NSEC_PER_SEC);
+
+ set_normalized_timespec(&retval, muled_sec, muled_nsec+remain_sec);
+ return retval;
+}
+
+static struct timespec __transformed_time(struct timespec val, struct ixp_clock *clock)
+{
+ struct timespec diff, muled, retval;
+ int neg_adj = 0;
+ s32 ppb;
+
+ if (clock->freq < 0) {
+ neg_adj = 1;
+ ppb = -clock->freq;
+ } else {
+ ppb = clock->freq;
+ }
+
+ diff = timespec_sub(val, clock->last_timestamp);
+ muled = timespec_mul_freq(diff, (u32) ppb);
+
+ if (neg_adj) {
+ retval = timespec_sub(val, muled);
+ } else {
+ retval = timespec_add(val, muled);
+ }
+
+ if (clock->offset_ns < 0)
+ retval = timespec_sub(retval, ns_to_timespec(-clock->offset_ns));
+ else
+ retval = timespec_add(retval, ns_to_timespec(clock->offset_ns));
+
+ return retval;
+}
+
+/*
+ * PTP clock operations
+ */
+
+static int ptp_ixp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+ struct timespec now_kernel, now_this;
+ struct ixp_clock *ixp_clock = container_of(ptp, struct ixp_clock, caps);
+
+ getrawmonotonic(&now_kernel);
+
+ write_seqlock(&clock_lock);
+
+ now_this = __transformed_time(now_kernel, ixp_clock);
+
+ ixp_clock->offset_ns = timespec_to_ns(&now_this) - timespec_to_ns(&now_kernel);
+ ixp_clock->last_timestamp = now_kernel;
+ ixp_clock->freq = ppb;
+
+ write_sequnlock(&clock_lock);
+
+ return 0;
+}
+
+static int ptp_ixp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+ struct ixp_clock *ixp_clock = container_of(ptp, struct ixp_clock, caps);
+
+ write_seqlock(&clock_lock);
+ ixp_clock->offset_ns += delta;
+ write_sequnlock(&clock_lock);
+
+ return 0;
+}
+
+static int ptp_ixp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+ unsigned long seq;
+ struct timespec now_kernel;
+ struct ixp_clock *ixp_clock = container_of(ptp, struct ixp_clock, caps);
+
+ getrawmonotonic(&now_kernel);
+
+ do {
+ seq = read_seqbegin(&clock_lock);
+
+ *ts = __transformed_time(now_kernel, ixp_clock);
+ } while (read_seqretry(&clock_lock, seq));
+
+ return 0;
+}
+
+static int ptp_ixp_settime(struct ptp_clock_info *ptp,
+ const struct timespec *ts)
+{
+ struct ixp_clock *ixp_clock = container_of(ptp, struct ixp_clock, caps);
+
+ struct timespec now_kernel;
+
+ getrawmonotonic(&now_kernel);
+
+ write_seqlock(&clock_lock);
+ ixp_clock->last_timestamp = now_kernel;
+ ixp_clock->offset_ns = timespec_to_ns(ts) - timespec_to_ns(&now_kernel);
+ write_sequnlock(&clock_lock);
+
+ return 0;
+}
+
+static int ptp_ixp_enable(struct ptp_clock_info *ptp,
+ struct ptp_clock_request *rq, int on)
+{
+ //struct ixp_clock *ixp_clock = container_of(ptp, struct ixp_clock, caps);
+
+ switch (rq->type) {
+ case PTP_CLK_REQ_EXTTS:
+ switch (rq->extts.index) {
+ default:
+ return -EINVAL;
+ }
+ return 0;
+ default:
+ break;
+ }
+
+ return -EOPNOTSUPP;
+}
+
+static struct ptp_clock_info ptp_ixp_caps = {
+ .owner = THIS_MODULE,
+ .name = "software timer",
+ .max_adj = 66666655,
+ .n_ext_ts = 0,
+ .pps = 0,
+ .adjfreq = ptp_ixp_adjfreq,
+ .adjtime = ptp_ixp_adjtime,
+ .gettime = ptp_ixp_gettime,
+ .settime = ptp_ixp_settime,
+ .enable = ptp_ixp_enable,
+};
+
+/* module operations */
+
+static struct ixp_clock ixp_clock;
+
+static void __exit ptp_ixp_exit(void)
+{
+ ptp_clock_unregister(ixp_clock.ptp_clock);
+}
+
+static int __init ptp_ixp_init(void)
+{
+ ixp_clock.caps = ptp_ixp_caps;
+
+ ixp_clock.ptp_clock = ptp_clock_register(&ixp_clock.caps);
+
+ if (IS_ERR(ixp_clock.ptp_clock))
+ return PTR_ERR(ixp_clock.ptp_clock);
+
+ return 0;
+}
+
+module_init(ptp_ixp_init);
+module_exit(ptp_ixp_exit);
+
+MODULE_AUTHOR("Richard Cochran <richard.cochran@omicron.at>");
+MODULE_DESCRIPTION("PTP clock using the IXP46X timer");
+MODULE_LICENSE("GPL");
--
1.7.2.3