arch/mips/vdso/gettimeofday.c

   1 /*
   2  * Copyright (C) 2015 Imagination Technologies
   3  * Author: Alex Smith <alex.smith@imgtec.com>
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of the GNU General Public License as published by the
   7  * Free Software Foundation;  either version 2 of the  License, or (at your
   8  * option) any later version.
   9  */
  10
  11 #include "vdso.h"
  12
  13 #include <linux/compiler.h>
  14 #include <linux/irqchip/mips-gic.h>
  15 #include <linux/time.h>
  16
  17 #include <asm/clocksource.h>
  18 #include <asm/io.h>
  19 #include <asm/mips-cm.h>
  20 #include <asm/unistd.h>
  21 #include <asm/vdso.h>
  22
  23 static __always_inline int do_realtime_coarse(struct timespec *ts,
  24                                               const union mips_vdso_data *data)
  25 {
  26         u32 start_seq;
  27
  28         do {
  29                 start_seq = vdso_data_read_begin(data);
  30
  31                 ts->tv_sec = data->xtime_sec;
  32                 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
  33         } while (vdso_data_read_retry(data, start_seq));
  34
  35         return 0;
  36 }
  37
  38 static __always_inline int do_monotonic_coarse(struct timespec *ts,
  39                                                const union mips_vdso_data *data)
  40 {
  41         u32 start_seq;
  42         u32 to_mono_sec;
  43         u32 to_mono_nsec;
  44
  45         do {
  46                 start_seq = vdso_data_read_begin(data);
  47
  48                 ts->tv_sec = data->xtime_sec;
  49                 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
  50
  51                 to_mono_sec = data->wall_to_mono_sec;
  52                 to_mono_nsec = data->wall_to_mono_nsec;
  53         } while (vdso_data_read_retry(data, start_seq));
  54
  55         ts->tv_sec += to_mono_sec;
  56         timespec_add_ns(ts, to_mono_nsec);
  57
  58         return 0;
  59 }
  60
  61 #ifdef CONFIG_CSRC_R4K
  62
  63 static __always_inline u64 read_r4k_count(void)
  64 {
  65         unsigned int count;
  66
  67         __asm__ __volatile__(
  68         "       .set push\n"
  69         "       .set mips32r2\n"
  70         "       rdhwr   %0, $2\n"
  71         "       .set pop\n"
  72         : "=r" (count));
  73
  74         return count;
  75 }
  76
  77 #endif
  78
  79 #ifdef CONFIG_CLKSRC_MIPS_GIC
  80
  81 static __always_inline u64 read_gic_count(const union mips_vdso_data *data)
  82 {
  83         void __iomem *gic = get_gic(data);
  84         u32 hi, hi2, lo;
  85
  86         do {
  87                 hi = __raw_readl(gic + GIC_UMV_SH_COUNTER_63_32_OFS);
  88                 lo = __raw_readl(gic + GIC_UMV_SH_COUNTER_31_00_OFS);
  89                 hi2 = __raw_readl(gic + GIC_UMV_SH_COUNTER_63_32_OFS);
  90         } while (hi2 != hi);
  91
  92         return (((u64)hi) << 32) + lo;
  93 }
  94
  95 #endif
  96
  97 static __always_inline u64 get_ns(const union mips_vdso_data *data)
  98 {
  99         u64 cycle_now, delta, nsec;
 100
 101         switch (data->clock_mode) {
 102 #ifdef CONFIG_CSRC_R4K
 103         case VDSO_CLOCK_R4K:
 104                 cycle_now = read_r4k_count();
 105                 break;
 106 #endif
 107 #ifdef CONFIG_CLKSRC_MIPS_GIC
 108         case VDSO_CLOCK_GIC:
 109                 cycle_now = read_gic_count(data);
 110                 break;
 111 #endif
 112         default:
 113                 return 0;
 114         }
 115
 116         delta = (cycle_now - data->cs_cycle_last) & data->cs_mask;
 117
 118         nsec = (delta * data->cs_mult) + data->xtime_nsec;
 119         nsec >>= data->cs_shift;
 120
 121         return nsec;
 122 }
 123
 124 static __always_inline int do_realtime(struct timespec *ts,
 125                                        const union mips_vdso_data *data)
 126 {
 127         u32 start_seq;
 128         u64 ns;
 129
 130         do {
 131                 start_seq = vdso_data_read_begin(data);
 132
 133                 if (data->clock_mode == VDSO_CLOCK_NONE)
 134                         return -ENOSYS;
 135
 136                 ts->tv_sec = data->xtime_sec;
 137                 ns = get_ns(data);
 138         } while (vdso_data_read_retry(data, start_seq));
 139
 140         ts->tv_nsec = 0;
 141         timespec_add_ns(ts, ns);
 142
 143         return 0;
 144 }
 145
 146 static __always_inline int do_monotonic(struct timespec *ts,
 147                                         const union mips_vdso_data *data)
 148 {
 149         u32 start_seq;
 150         u64 ns;
 151         u32 to_mono_sec;
 152         u32 to_mono_nsec;
 153
 154         do {
 155                 start_seq = vdso_data_read_begin(data);
 156
 157                 if (data->clock_mode == VDSO_CLOCK_NONE)
 158                         return -ENOSYS;
 159
 160                 ts->tv_sec = data->xtime_sec;
 161                 ns = get_ns(data);
 162
 163                 to_mono_sec = data->wall_to_mono_sec;
 164                 to_mono_nsec = data->wall_to_mono_nsec;
 165         } while (vdso_data_read_retry(data, start_seq));
 166
 167         ts->tv_sec += to_mono_sec;
 168         ts->tv_nsec = 0;
 169         timespec_add_ns(ts, ns + to_mono_nsec);
 170
 171         return 0;
 172 }
 173
 174 #ifdef CONFIG_MIPS_CLOCK_VSYSCALL
 175
 176 /*
 177  * This is behind the ifdef so that we don't provide the symbol when there's no
 178  * possibility of there being a usable clocksource, because there's nothing we
 179  * can do without it. When libc fails the symbol lookup it should fall back on
 180  * the standard syscall path.
 181  */
 182 int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz)
 183 {
 184         const union mips_vdso_data *data = get_vdso_data();
 185         struct timespec ts;
 186         int ret;
 187
 188         ret = do_realtime(&ts, data);
 189         if (ret)
 190                 return ret;
 191
 192         if (tv) {
 193                 tv->tv_sec = ts.tv_sec;
 194                 tv->tv_usec = ts.tv_nsec / 1000;
 195         }
 196
 197         if (tz) {
 198                 tz->tz_minuteswest = data->tz_minuteswest;
 199                 tz->tz_dsttime = data->tz_dsttime;
 200         }
 201
 202         return 0;
 203 }
 204
 205 #endif /* CONFIG_CLKSRC_MIPS_GIC */
 206
 207 int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts)
 208 {
 209         const union mips_vdso_data *data = get_vdso_data();
 210         int ret;
 211
 212         switch (clkid) {
 213         case CLOCK_REALTIME_COARSE:
 214                 ret = do_realtime_coarse(ts, data);
 215                 break;
 216         case CLOCK_MONOTONIC_COARSE:
 217                 ret = do_monotonic_coarse(ts, data);
 218                 break;
 219         case CLOCK_REALTIME:
 220                 ret = do_realtime(ts, data);
 221                 break;
 222         case CLOCK_MONOTONIC:
 223                 ret = do_monotonic(ts, data);
 224                 break;
 225         default:
 226                 ret = -ENOSYS;
 227                 break;
 228         }
 229
 230         /* If we return -ENOSYS libc should fall back to a syscall. */
 231         return ret;
 232 }