mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-08-24 09:50:04 +00:00
![Grygorii Strashko](/assets/img/avatar_default.png)
Many PTP drivers required to perform some asynchronous or periodic work, like periodically handling PHC counter overflow or handle delayed timestamp for RX/TX network packets. In most of the cases, such work is implemented using workqueues. Unfortunately, Kernel workqueues might introduce significant delay in work scheduling under high system load and on -RT, which could cause misbehavior of PTP drivers due to internal counter overflow, for example, and there is no way to tune its execution policy and priority manuallly. Hence, The kthread_worker can be used insted of workqueues, as it create separte named kthread for each worker and its its execution policy and priority can be configured using chrt tool. This prblem was reported for two drivers TI CPSW CPTS and dp83640, so instead of modifying each of these driver it was proposed to add PTP auxiliary worker to the PHC subsystem. The patch adds PTP auxiliary worker in PHC subsystem using kthread_worker and kthread_delayed_work and introduces two new PHC subsystem APIs: - long (*do_aux_work)(struct ptp_clock_info *ptp) callback in ptp_clock_info structure, which driver should assign if it require to perform asynchronous or periodic work. Driver should return the delay of the PTP next auxiliary work scheduling time (>=0) or negative value in case further scheduling is not required. - int ptp_schedule_worker(struct ptp_clock *ptp, unsigned long delay) which allows schedule PTP auxiliary work. The name of kthread_worker thread corresponds PTP PHC device name "ptp%d". Signed-off-by: Grygorii Strashko <grygorii.strashko@ti.com> Signed-off-by: David S. Miller <davem@davemloft.net>
423 lines
9.9 KiB
C
423 lines
9.9 KiB
C
/*
|
|
* PTP 1588 clock support
|
|
*
|
|
* 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/idr.h>
|
|
#include <linux/device.h>
|
|
#include <linux/err.h>
|
|
#include <linux/init.h>
|
|
#include <linux/kernel.h>
|
|
#include <linux/module.h>
|
|
#include <linux/posix-clock.h>
|
|
#include <linux/pps_kernel.h>
|
|
#include <linux/slab.h>
|
|
#include <linux/syscalls.h>
|
|
#include <linux/uaccess.h>
|
|
#include <uapi/linux/sched/types.h>
|
|
|
|
#include "ptp_private.h"
|
|
|
|
#define PTP_MAX_ALARMS 4
|
|
#define PTP_PPS_DEFAULTS (PPS_CAPTUREASSERT | PPS_OFFSETASSERT)
|
|
#define PTP_PPS_EVENT PPS_CAPTUREASSERT
|
|
#define PTP_PPS_MODE (PTP_PPS_DEFAULTS | PPS_CANWAIT | PPS_TSFMT_TSPEC)
|
|
|
|
/* private globals */
|
|
|
|
static dev_t ptp_devt;
|
|
static struct class *ptp_class;
|
|
|
|
static DEFINE_IDA(ptp_clocks_map);
|
|
|
|
/* time stamp event queue operations */
|
|
|
|
static inline int queue_free(struct timestamp_event_queue *q)
|
|
{
|
|
return PTP_MAX_TIMESTAMPS - queue_cnt(q) - 1;
|
|
}
|
|
|
|
static void enqueue_external_timestamp(struct timestamp_event_queue *queue,
|
|
struct ptp_clock_event *src)
|
|
{
|
|
struct ptp_extts_event *dst;
|
|
unsigned long flags;
|
|
s64 seconds;
|
|
u32 remainder;
|
|
|
|
seconds = div_u64_rem(src->timestamp, 1000000000, &remainder);
|
|
|
|
spin_lock_irqsave(&queue->lock, flags);
|
|
|
|
dst = &queue->buf[queue->tail];
|
|
dst->index = src->index;
|
|
dst->t.sec = seconds;
|
|
dst->t.nsec = remainder;
|
|
|
|
if (!queue_free(queue))
|
|
queue->head = (queue->head + 1) % PTP_MAX_TIMESTAMPS;
|
|
|
|
queue->tail = (queue->tail + 1) % PTP_MAX_TIMESTAMPS;
|
|
|
|
spin_unlock_irqrestore(&queue->lock, flags);
|
|
}
|
|
|
|
static s32 scaled_ppm_to_ppb(long ppm)
|
|
{
|
|
/*
|
|
* The 'freq' field in the 'struct timex' is in parts per
|
|
* million, but with a 16 bit binary fractional field.
|
|
*
|
|
* We want to calculate
|
|
*
|
|
* ppb = scaled_ppm * 1000 / 2^16
|
|
*
|
|
* which simplifies to
|
|
*
|
|
* ppb = scaled_ppm * 125 / 2^13
|
|
*/
|
|
s64 ppb = 1 + ppm;
|
|
ppb *= 125;
|
|
ppb >>= 13;
|
|
return (s32) ppb;
|
|
}
|
|
|
|
/* posix clock implementation */
|
|
|
|
static int ptp_clock_getres(struct posix_clock *pc, struct timespec64 *tp)
|
|
{
|
|
tp->tv_sec = 0;
|
|
tp->tv_nsec = 1;
|
|
return 0;
|
|
}
|
|
|
|
static int ptp_clock_settime(struct posix_clock *pc, const struct timespec64 *tp)
|
|
{
|
|
struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
|
|
|
|
return ptp->info->settime64(ptp->info, tp);
|
|
}
|
|
|
|
static int ptp_clock_gettime(struct posix_clock *pc, struct timespec64 *tp)
|
|
{
|
|
struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
|
|
int err;
|
|
|
|
err = ptp->info->gettime64(ptp->info, tp);
|
|
return err;
|
|
}
|
|
|
|
static int ptp_clock_adjtime(struct posix_clock *pc, struct timex *tx)
|
|
{
|
|
struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
|
|
struct ptp_clock_info *ops;
|
|
int err = -EOPNOTSUPP;
|
|
|
|
ops = ptp->info;
|
|
|
|
if (tx->modes & ADJ_SETOFFSET) {
|
|
struct timespec64 ts;
|
|
ktime_t kt;
|
|
s64 delta;
|
|
|
|
ts.tv_sec = tx->time.tv_sec;
|
|
ts.tv_nsec = tx->time.tv_usec;
|
|
|
|
if (!(tx->modes & ADJ_NANO))
|
|
ts.tv_nsec *= 1000;
|
|
|
|
if ((unsigned long) ts.tv_nsec >= NSEC_PER_SEC)
|
|
return -EINVAL;
|
|
|
|
kt = timespec64_to_ktime(ts);
|
|
delta = ktime_to_ns(kt);
|
|
err = ops->adjtime(ops, delta);
|
|
} else if (tx->modes & ADJ_FREQUENCY) {
|
|
s32 ppb = scaled_ppm_to_ppb(tx->freq);
|
|
if (ppb > ops->max_adj || ppb < -ops->max_adj)
|
|
return -ERANGE;
|
|
if (ops->adjfine)
|
|
err = ops->adjfine(ops, tx->freq);
|
|
else
|
|
err = ops->adjfreq(ops, ppb);
|
|
ptp->dialed_frequency = tx->freq;
|
|
} else if (tx->modes == 0) {
|
|
tx->freq = ptp->dialed_frequency;
|
|
err = 0;
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
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,
|
|
.ioctl = ptp_ioctl,
|
|
.open = ptp_open,
|
|
.poll = ptp_poll,
|
|
.read = ptp_read,
|
|
};
|
|
|
|
static void delete_ptp_clock(struct posix_clock *pc)
|
|
{
|
|
struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
|
|
|
|
mutex_destroy(&ptp->tsevq_mux);
|
|
mutex_destroy(&ptp->pincfg_mux);
|
|
ida_simple_remove(&ptp_clocks_map, ptp->index);
|
|
kfree(ptp);
|
|
}
|
|
|
|
static void ptp_aux_kworker(struct kthread_work *work)
|
|
{
|
|
struct ptp_clock *ptp = container_of(work, struct ptp_clock,
|
|
aux_work.work);
|
|
struct ptp_clock_info *info = ptp->info;
|
|
long delay;
|
|
|
|
delay = info->do_aux_work(info);
|
|
|
|
if (delay >= 0)
|
|
kthread_queue_delayed_work(ptp->kworker, &ptp->aux_work, delay);
|
|
}
|
|
|
|
/* public interface */
|
|
|
|
struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
|
|
struct device *parent)
|
|
{
|
|
struct ptp_clock *ptp;
|
|
int err = 0, index, major = MAJOR(ptp_devt);
|
|
|
|
if (info->n_alarm > PTP_MAX_ALARMS)
|
|
return ERR_PTR(-EINVAL);
|
|
|
|
/* Initialize a clock structure. */
|
|
err = -ENOMEM;
|
|
ptp = kzalloc(sizeof(struct ptp_clock), GFP_KERNEL);
|
|
if (ptp == NULL)
|
|
goto no_memory;
|
|
|
|
index = ida_simple_get(&ptp_clocks_map, 0, MINORMASK + 1, GFP_KERNEL);
|
|
if (index < 0) {
|
|
err = index;
|
|
goto no_slot;
|
|
}
|
|
|
|
ptp->clock.ops = ptp_clock_ops;
|
|
ptp->clock.release = delete_ptp_clock;
|
|
ptp->info = info;
|
|
ptp->devid = MKDEV(major, index);
|
|
ptp->index = index;
|
|
spin_lock_init(&ptp->tsevq.lock);
|
|
mutex_init(&ptp->tsevq_mux);
|
|
mutex_init(&ptp->pincfg_mux);
|
|
init_waitqueue_head(&ptp->tsev_wq);
|
|
|
|
if (ptp->info->do_aux_work) {
|
|
char *worker_name = kasprintf(GFP_KERNEL, "ptp%d", ptp->index);
|
|
|
|
kthread_init_delayed_work(&ptp->aux_work, ptp_aux_kworker);
|
|
ptp->kworker = kthread_create_worker(0, worker_name ?
|
|
worker_name : info->name);
|
|
kfree(worker_name);
|
|
if (IS_ERR(ptp->kworker)) {
|
|
err = PTR_ERR(ptp->kworker);
|
|
pr_err("failed to create ptp aux_worker %d\n", err);
|
|
goto kworker_err;
|
|
}
|
|
}
|
|
|
|
err = ptp_populate_pin_groups(ptp);
|
|
if (err)
|
|
goto no_pin_groups;
|
|
|
|
/* Create a new device in our class. */
|
|
ptp->dev = device_create_with_groups(ptp_class, parent, ptp->devid,
|
|
ptp, ptp->pin_attr_groups,
|
|
"ptp%d", ptp->index);
|
|
if (IS_ERR(ptp->dev))
|
|
goto no_device;
|
|
|
|
/* Register a new PPS source. */
|
|
if (info->pps) {
|
|
struct pps_source_info pps;
|
|
memset(&pps, 0, sizeof(pps));
|
|
snprintf(pps.name, PPS_MAX_NAME_LEN, "ptp%d", index);
|
|
pps.mode = PTP_PPS_MODE;
|
|
pps.owner = info->owner;
|
|
ptp->pps_source = pps_register_source(&pps, PTP_PPS_DEFAULTS);
|
|
if (!ptp->pps_source) {
|
|
pr_err("failed to register pps source\n");
|
|
goto no_pps;
|
|
}
|
|
}
|
|
|
|
/* Create a posix clock. */
|
|
err = posix_clock_register(&ptp->clock, ptp->devid);
|
|
if (err) {
|
|
pr_err("failed to create posix clock\n");
|
|
goto no_clock;
|
|
}
|
|
|
|
return ptp;
|
|
|
|
no_clock:
|
|
if (ptp->pps_source)
|
|
pps_unregister_source(ptp->pps_source);
|
|
no_pps:
|
|
device_destroy(ptp_class, ptp->devid);
|
|
no_device:
|
|
ptp_cleanup_pin_groups(ptp);
|
|
no_pin_groups:
|
|
if (ptp->kworker)
|
|
kthread_destroy_worker(ptp->kworker);
|
|
kworker_err:
|
|
mutex_destroy(&ptp->tsevq_mux);
|
|
mutex_destroy(&ptp->pincfg_mux);
|
|
ida_simple_remove(&ptp_clocks_map, index);
|
|
no_slot:
|
|
kfree(ptp);
|
|
no_memory:
|
|
return ERR_PTR(err);
|
|
}
|
|
EXPORT_SYMBOL(ptp_clock_register);
|
|
|
|
int ptp_clock_unregister(struct ptp_clock *ptp)
|
|
{
|
|
ptp->defunct = 1;
|
|
wake_up_interruptible(&ptp->tsev_wq);
|
|
|
|
if (ptp->kworker) {
|
|
kthread_cancel_delayed_work_sync(&ptp->aux_work);
|
|
kthread_destroy_worker(ptp->kworker);
|
|
}
|
|
|
|
/* Release the clock's resources. */
|
|
if (ptp->pps_source)
|
|
pps_unregister_source(ptp->pps_source);
|
|
|
|
device_destroy(ptp_class, ptp->devid);
|
|
ptp_cleanup_pin_groups(ptp);
|
|
|
|
posix_clock_unregister(&ptp->clock);
|
|
return 0;
|
|
}
|
|
EXPORT_SYMBOL(ptp_clock_unregister);
|
|
|
|
void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event)
|
|
{
|
|
struct pps_event_time evt;
|
|
|
|
switch (event->type) {
|
|
|
|
case PTP_CLOCK_ALARM:
|
|
break;
|
|
|
|
case PTP_CLOCK_EXTTS:
|
|
enqueue_external_timestamp(&ptp->tsevq, event);
|
|
wake_up_interruptible(&ptp->tsev_wq);
|
|
break;
|
|
|
|
case PTP_CLOCK_PPS:
|
|
pps_get_ts(&evt);
|
|
pps_event(ptp->pps_source, &evt, PTP_PPS_EVENT, NULL);
|
|
break;
|
|
|
|
case PTP_CLOCK_PPSUSR:
|
|
pps_event(ptp->pps_source, &event->pps_times,
|
|
PTP_PPS_EVENT, NULL);
|
|
break;
|
|
}
|
|
}
|
|
EXPORT_SYMBOL(ptp_clock_event);
|
|
|
|
int ptp_clock_index(struct ptp_clock *ptp)
|
|
{
|
|
return ptp->index;
|
|
}
|
|
EXPORT_SYMBOL(ptp_clock_index);
|
|
|
|
int ptp_find_pin(struct ptp_clock *ptp,
|
|
enum ptp_pin_function func, unsigned int chan)
|
|
{
|
|
struct ptp_pin_desc *pin = NULL;
|
|
int i;
|
|
|
|
mutex_lock(&ptp->pincfg_mux);
|
|
for (i = 0; i < ptp->info->n_pins; i++) {
|
|
if (ptp->info->pin_config[i].func == func &&
|
|
ptp->info->pin_config[i].chan == chan) {
|
|
pin = &ptp->info->pin_config[i];
|
|
break;
|
|
}
|
|
}
|
|
mutex_unlock(&ptp->pincfg_mux);
|
|
|
|
return pin ? i : -1;
|
|
}
|
|
EXPORT_SYMBOL(ptp_find_pin);
|
|
|
|
int ptp_schedule_worker(struct ptp_clock *ptp, unsigned long delay)
|
|
{
|
|
return kthread_mod_delayed_work(ptp->kworker, &ptp->aux_work, delay);
|
|
}
|
|
EXPORT_SYMBOL(ptp_schedule_worker);
|
|
|
|
/* module operations */
|
|
|
|
static void __exit ptp_exit(void)
|
|
{
|
|
class_destroy(ptp_class);
|
|
unregister_chrdev_region(ptp_devt, MINORMASK + 1);
|
|
ida_destroy(&ptp_clocks_map);
|
|
}
|
|
|
|
static int __init ptp_init(void)
|
|
{
|
|
int err;
|
|
|
|
ptp_class = class_create(THIS_MODULE, "ptp");
|
|
if (IS_ERR(ptp_class)) {
|
|
pr_err("ptp: failed to allocate class\n");
|
|
return PTR_ERR(ptp_class);
|
|
}
|
|
|
|
err = alloc_chrdev_region(&ptp_devt, 0, MINORMASK + 1, "ptp");
|
|
if (err < 0) {
|
|
pr_err("ptp: failed to allocate device region\n");
|
|
goto no_region;
|
|
}
|
|
|
|
ptp_class->dev_groups = ptp_groups;
|
|
pr_info("PTP clock support registered\n");
|
|
return 0;
|
|
|
|
no_region:
|
|
class_destroy(ptp_class);
|
|
return err;
|
|
}
|
|
|
|
subsys_initcall(ptp_init);
|
|
module_exit(ptp_exit);
|
|
|
|
MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
|
|
MODULE_DESCRIPTION("PTP clocks support");
|
|
MODULE_LICENSE("GPL");
|