a PTP hardware clock supporting timers (alarms) can now be used via the POSIX
timer (timer_create, timer_settime etc) interface
Signed-off-by: Kieran Tyrrell <kie...@sienda.com>
---
drivers/ptp/ptp_clock.c | 233 +++++++++++++++++++++++++++++++++++++++
drivers/ptp/ptp_private.h | 4 +
include/linux/ptp_clock_kernel.h | 2 +
kernel/signal.c | 1 +
4 files changed, 240 insertions(+)
diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c
index 2e481b9..067e41c 100644
--- a/drivers/ptp/ptp_clock.c
+++ b/drivers/ptp/ptp_clock.c
@@ -36,6 +36,8 @@
#define PTP_PPS_EVENT PPS_CAPTUREASSERT
#define PTP_PPS_MODE (PTP_PPS_DEFAULTS | PPS_CANWAIT | PPS_TSFMT_TSPEC)
+#define PTP_TIMER_MINIMUM_INTERVAL_NS 100000
+
/* private globals */
static dev_t ptp_devt;
@@ -43,6 +45,108 @@ static struct class *ptp_class;
static DEFINE_IDA(ptp_clocks_map);
+static int ptp_clock_gettime(struct posix_clock *pc, struct timespec *tp);
+
+static int set_device_timer_earliest(struct ptp_clock *ptp)
+{
+ struct timerqueue_node *next;
+ int err;
+ unsigned long tq_lock_flags;
+ struct timespec64 ts;
+
+ spin_lock_irqsave(&ptp->tq_lock, tq_lock_flags);
+
+ next = timerqueue_getnext(&ptp->timerqueue);
+
+ /* Skip over expired or not set timers */
+ while (next) {
+ if (next->expires.tv64 != 0)
+ break;
+ next = timerqueue_iterate_next(next);
+ }
+
+ spin_unlock_irqrestore(&ptp->tq_lock, tq_lock_flags);
+
+ if (next) {
+ ts = ktime_to_timespec64(next->expires);
+ err = ptp->info->timersettime(ptp->info, &ts);
+ if(err)
+ return err;
+ }
+
+ return 0;
+}
+
+static void ptp_alarm_work(struct work_struct *work)
+{
+ struct ptp_clock *ptp = container_of(work, struct ptp_clock,
alarm_work);
+ struct task_struct *task;
+ struct siginfo info;
+ struct timerqueue_node *next;
+ struct k_itimer *kit;
+ struct timespec time_now;
+ s64 ns_now;
+ int shared;
+ bool signal_failed_to_send;
+ unsigned long tq_lock_flags;
+
+ if(0 != ptp_clock_gettime(&ptp->clock, &time_now))
+ return;
+
+ ns_now = timespec_to_ns(&time_now);
+
+ spin_lock_irqsave(&ptp->tq_lock, tq_lock_flags);
+
+ next = timerqueue_getnext(&ptp->timerqueue);
+
+ /* Skip over expired or not set timers */
+ while (next) {
+ if (next->expires.tv64 != 0)
+ break;
+ next = timerqueue_iterate_next(next);
+ }
+
+ while (next) {
+ if (next->expires.tv64 > ns_now)
+ break;
+
+ rcu_read_lock();
+
+ kit = container_of(next, struct k_itimer, it.real.timer.node);
+
+ task = pid_task(kit->it_pid, PIDTYPE_PID);
+ if (task)
+ {
+ memset(&info, 0, sizeof(info));
+ info.si_signo = SIGALRM;
+ info.si_code = SI_TIMER;
+ info._sifields._timer._tid = kit->it_id;
+ kit->sigq->info.si_sys_private = 0;
+ shared = !(kit->it_sigev_notify & SIGEV_THREAD_ID);
+ signal_failed_to_send = send_sigqueue(kit->sigq, task,
shared) > 0;
+ }
+ rcu_read_unlock();
+
+ next = timerqueue_iterate_next(next);
+
+ /* update and reinsert the last one that has fired */
+ timerqueue_del(&ptp->timerqueue, &kit->it.real.timer.node);
+ if ( (0 == ktime_to_ns(kit->it.real.interval)) ||
signal_failed_to_send) {
+ /* this is not a periodic timer (or the signal failed
to send), so stop it */
+ kit->it.real.timer.node.expires = ns_to_ktime(0);
+ }
+ else {
+ /* this IS a periodic timer, so set the next fire time
*/
+ kit->it.real.timer.node.expires =
ktime_add(kit->it.real.timer.node.expires, kit->it.real.interval);
+ }
+ timerqueue_add(&ptp->timerqueue, &kit->it.real.timer.node);
+ }
+
+ spin_unlock_irqrestore(&ptp->tq_lock, tq_lock_flags);
+
+ set_device_timer_earliest(ptp);
+}
+
/* time stamp event queue operations */
static inline int queue_free(struct timestamp_event_queue *q)
@@ -163,12 +267,135 @@ static int ptp_clock_adjtime(struct posix_clock *pc,
struct timex *tx)
return err;
}
+static int ptp_timer_create(struct posix_clock *pc, struct k_itimer *kit)
+{
+ struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
+ int err = 0;
+ unsigned long tq_lock_flags;
+
+ if(ptp->info->timerenable == 0)
+ return -EOPNOTSUPP;
+
+ spin_lock_irqsave(&ptp->tq_lock, tq_lock_flags);
+
+ if(NULL == timerqueue_getnext(&ptp->timerqueue))
+ {
+ /* list is empty, so hardware timer is disabled, enable it */
+ err = ptp->info->timerenable(ptp->info, true);
+ }
+
+ if(0 == err)
+ {
+ timerqueue_init(&kit->it.real.timer.node);
+ /* ensure expiry time is 0 (timer disabled) */
+ kit->it.real.timer.node.expires = ns_to_ktime(0);
+ timerqueue_add(&ptp->timerqueue, &kit->it.real.timer.node);
+ }
+
+ spin_unlock_irqrestore(&ptp->tq_lock, tq_lock_flags);
+
+ return err;
+}
+
+static int ptp_timer_delete(struct posix_clock *pc, struct k_itimer *kit)
+{
+ struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
+ int err=0;
+ unsigned long tq_lock_flags;
+
+ if(ptp->info->timerenable == 0)
+ return -EOPNOTSUPP;
+
+ spin_lock_irqsave(&ptp->tq_lock, tq_lock_flags);
+
+ timerqueue_del(&ptp->timerqueue, &kit->it.real.timer.node);
+
+ if(NULL == timerqueue_getnext(&ptp->timerqueue))
+ {
+ /* there are no more timers set on this device, so we can
disable the hardware timer */
+ err = ptp->info->timerenable(ptp->info, false);
+ }
+
+ spin_unlock_irqrestore(&ptp->tq_lock, tq_lock_flags);
+
+ return err;
+}
+
+static void ptp_timer_gettime(struct posix_clock *pc,
+ struct k_itimer *kit, struct itimerspec
*tsp)
+{
+ struct timespec time_now;
+
+ if(NULL == tsp)
+ return;
+
+ if(0 != ptp_clock_gettime(pc, &time_now))
+ return;
+
+ tsp->it_interval = ktime_to_timespec(kit->it.real.interval);
+ tsp->it_value =
timespec_sub(ktime_to_timespec(kit->it.real.timer.node.expires), time_now);
+}
+
+
+static int ptp_timer_settime(struct posix_clock *pc,
+ struct k_itimer *kit, int flags,
+ struct itimerspec *tsp, struct
itimerspec *old)
+{
+ struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
+ int err;
+ unsigned long tq_lock_flags;
+ struct timespec time_now;
+ ktime_t fire_time;
+
+ if(ptp->info->timersettime == 0)
+ return -EOPNOTSUPP;
+
+ if (old) {
+ ptp_timer_gettime(pc, kit, old);
+ }
+
+ fire_time = timespec_to_ktime(tsp->it_value);
+
+ if( (fire_time.tv64 != 0) && !(flags & TIMER_ABSTIME))
+ {
+ err = ptp_clock_gettime(pc, &time_now);
+ if(err)
+ return err;
+ /* convert relative to absolute time */
+ fire_time = ktime_add(fire_time, timespec_to_ktime(time_now));
+ }
+
+ /* remove, update and reinsert the node */
+ spin_lock_irqsave(&ptp->tq_lock, tq_lock_flags);
+
+ timerqueue_del(&ptp->timerqueue, &kit->it.real.timer.node);
+
+ kit->it.real.timer.node.expires = fire_time;
+ kit->it.real.interval = timespec_to_ktime(tsp->it_interval);
+
+#ifdef PTP_TIMER_MINIMUM_INTERVAL_NS
+ if ( (ktime_to_ns(kit->it.real.interval) != 0 )
+ &&
(ktime_to_ns(kit->it.real.interval)<PTP_TIMER_MINIMUM_INTERVAL_NS) )
+ kit->it.real.interval =
ns_to_ktime(PTP_TIMER_MINIMUM_INTERVAL_NS);
+#endif
+
+ timerqueue_add(&ptp->timerqueue, &kit->it.real.timer.node);
+
+ spin_unlock_irqrestore(&ptp->tq_lock, tq_lock_flags);
+
+ return set_device_timer_earliest(ptp);
+}
+
static struct posix_clock_operations ptp_clock_ops = {
.owner = THIS_MODULE,
.clock_adjtime = ptp_clock_adjtime,
.clock_gettime = ptp_clock_gettime,
.clock_getres = ptp_clock_getres,
.clock_settime = ptp_clock_settime,
+ .timer_create = ptp_timer_create,
+ .timer_delete = ptp_timer_delete,
+ .timer_gettime = ptp_timer_gettime,
+ .timer_settime = ptp_timer_settime,
.ioctl = ptp_ioctl,
.open = ptp_open,
.poll = ptp_poll,
@@ -217,6 +444,9 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info
*info,
mutex_init(&ptp->tsevq_mux);
mutex_init(&ptp->pincfg_mux);
init_waitqueue_head(&ptp->tsev_wq);
+ spin_lock_init(&ptp->tq_lock);
+ timerqueue_init_head(&ptp->timerqueue);
+ INIT_WORK(&ptp->alarm_work, ptp_alarm_work);
/* Create a new device in our class. */
ptp->dev = device_create(ptp_class, parent, ptp->devid, ptp,
@@ -286,6 +516,8 @@ int ptp_clock_unregister(struct ptp_clock *ptp)
}
EXPORT_SYMBOL(ptp_clock_unregister);
+
+
void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event)
{
struct pps_event_time evt;
@@ -293,6 +525,7 @@ void ptp_clock_event(struct ptp_clock *ptp, struct
ptp_clock_event *event)
switch (event->type) {
case PTP_CLOCK_ALARM:
+ schedule_work(&ptp->alarm_work);
break;
case PTP_CLOCK_EXTTS:
diff --git a/drivers/ptp/ptp_private.h b/drivers/ptp/ptp_private.h
index 9c5d414..d491299 100644
--- a/drivers/ptp/ptp_private.h
+++ b/drivers/ptp/ptp_private.h
@@ -54,6 +54,10 @@ struct ptp_clock {
struct device_attribute *pin_dev_attr;
struct attribute **pin_attr;
struct attribute_group pin_attr_group;
+
+ struct timerqueue_head timerqueue;
+ spinlock_t tq_lock;
+ struct work_struct alarm_work;
};
/*
diff --git a/include/linux/ptp_clock_kernel.h b/include/linux/ptp_clock_kernel.h
index 6b15e16..8d953f3 100644
--- a/include/linux/ptp_clock_kernel.h
+++ b/include/linux/ptp_clock_kernel.h
@@ -118,6 +118,8 @@ struct ptp_clock_info {
struct ptp_clock_request *request, int on);
int (*verify)(struct ptp_clock_info *ptp, unsigned int pin,
enum ptp_pin_function func, unsigned int chan);
+ int (*timerenable)(struct ptp_clock_info *ptp, bool enable);
+ int (*timersettime)(struct ptp_clock_info *ptp, struct timespec64 *ts);
};
struct ptp_clock;
diff --git a/kernel/signal.c b/kernel/signal.c
index af21afc..e7331b3 100644
--- a/kernel/signal.c
+++ b/kernel/signal.c
@@ -1561,6 +1561,7 @@ out:
ret:
return ret;
}
+EXPORT_SYMBOL(send_sigqueue);
/*
* Let a parent know about the death of a child.
--
2.1.4
------------------------------------------------------------------------------
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
