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/time.h>
  15
  16 #include <asm/clocksource.h>
  17 #include <asm/io.h>
  18 #include <asm/unistd.h>
  19 #include <asm/vdso.h>
  20
  21 #if MIPS_ISA_REV < 6
  22 #define VDSO_SYSCALL_CLOBBERS "hi", "lo",
  23 #else
  24 #define VDSO_SYSCALL_CLOBBERS
  25 #endif
  26
  27 #ifdef CONFIG_MIPS_CLOCK_VSYSCALL
  28
  29 static __always_inline long gettimeofday_fallback(struct timeval *_tv,
  30                                           struct timezone *_tz)
  31 {
  32         register struct timezone *tz asm("a1") = _tz;
  33         register struct timeval *tv asm("a0") = _tv;
  34         register long ret asm("v0");
  35         register long nr asm("v0") = __NR_gettimeofday;
  36         register long error asm("a3");
  37
  38         asm volatile(
  39         "       syscall\n"
  40         : "=r" (ret), "=r" (error)
  41         : "r" (tv), "r" (tz), "r" (nr)
  42         : "$1", "$3", "$8", "$9", "$10", "$11", "$12", "$13",
  43           "$14", "$15", "$24", "$25",
  44           VDSO_SYSCALL_CLOBBERS
  45           "memory");
  46
  47         return error ? -ret : ret;
  48 }
  49
  50 #endif
  51
  52 static __always_inline long clock_gettime_fallback(clockid_t _clkid,
  53                                            struct timespec *_ts)
  54 {
  55         register struct timespec *ts asm("a1") = _ts;
  56         register clockid_t clkid asm("a0") = _clkid;
  57         register long ret asm("v0");
  58         register long nr asm("v0") = __NR_clock_gettime;
  59         register long error asm("a3");
  60
  61         asm volatile(
  62         "       syscall\n"
  63         : "=r" (ret), "=r" (error)
  64         : "r" (clkid), "r" (ts), "r" (nr)
  65         : "$1", "$3", "$8", "$9", "$10", "$11", "$12", "$13",
  66           "$14", "$15", "$24", "$25",
  67           VDSO_SYSCALL_CLOBBERS
  68           "memory");
  69
  70         return error ? -ret : ret;
  71 }
  72
  73 static __always_inline int do_realtime_coarse(struct timespec *ts,
  74                                               const union mips_vdso_data *data)
  75 {
  76         u32 start_seq;
  77
  78         do {
  79                 start_seq = vdso_data_read_begin(data);
  80
  81                 ts->tv_sec = data->xtime_sec;
  82                 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
  83         } while (vdso_data_read_retry(data, start_seq));
  84
  85         return 0;
  86 }
  87
  88 static __always_inline int do_monotonic_coarse(struct timespec *ts,
  89                                                const union mips_vdso_data *data)
  90 {
  91         u32 start_seq;
  92         u64 to_mono_sec;
  93         u64 to_mono_nsec;
  94
  95         do {
  96                 start_seq = vdso_data_read_begin(data);
  97
  98                 ts->tv_sec = data->xtime_sec;
  99                 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
 100
 101                 to_mono_sec = data->wall_to_mono_sec;
 102                 to_mono_nsec = data->wall_to_mono_nsec;
 103         } while (vdso_data_read_retry(data, start_seq));
 104
 105         ts->tv_sec += to_mono_sec;
 106         timespec_add_ns(ts, to_mono_nsec);
 107
 108         return 0;
 109 }
 110
 111 #ifdef CONFIG_CSRC_R4K
 112
 113 static __always_inline u64 read_r4k_count(void)
 114 {
 115         unsigned int count;
 116
 117         __asm__ __volatile__(
 118         "       .set push\n"
 119         "       .set mips32r2\n"
 120         "       rdhwr   %0, $2\n"
 121         "       .set pop\n"
 122         : "=r" (count));
 123
 124         return count;
 125 }
 126
 127 #endif
 128
 129 #ifdef CONFIG_CLKSRC_MIPS_GIC
 130
 131 static __always_inline u64 read_gic_count(const union mips_vdso_data *data)
 132 {
 133         void __iomem *gic = get_gic(data);
 134         u32 hi, hi2, lo;
 135
 136         do {
 137                 hi = __raw_readl(gic + sizeof(lo));
 138                 lo = __raw_readl(gic);
 139                 hi2 = __raw_readl(gic + sizeof(lo));
 140         } while (hi2 != hi);
 141
 142         return (((u64)hi) << 32) + lo;
 143 }
 144
 145 #endif
 146
 147 static __always_inline u64 get_ns(const union mips_vdso_data *data)
 148 {
 149         u64 cycle_now, delta, nsec;
 150
 151         switch (data->clock_mode) {
 152 #ifdef CONFIG_CSRC_R4K
 153         case VDSO_CLOCK_R4K:
 154                 cycle_now = read_r4k_count();
 155                 break;
 156 #endif
 157 #ifdef CONFIG_CLKSRC_MIPS_GIC
 158         case VDSO_CLOCK_GIC:
 159                 cycle_now = read_gic_count(data);
 160                 break;
 161 #endif
 162         default:
 163                 return 0;
 164         }
 165
 166         delta = (cycle_now - data->cs_cycle_last) & data->cs_mask;
 167
 168         nsec = (delta * data->cs_mult) + data->xtime_nsec;
 169         nsec >>= data->cs_shift;
 170
 171         return nsec;
 172 }
 173
 174 static __always_inline int do_realtime(struct timespec *ts,
 175                                        const union mips_vdso_data *data)
 176 {
 177         u32 start_seq;
 178         u64 ns;
 179
 180         do {
 181                 start_seq = vdso_data_read_begin(data);
 182
 183                 if (data->clock_mode == VDSO_CLOCK_NONE)
 184                         return -ENOSYS;
 185
 186                 ts->tv_sec = data->xtime_sec;
 187                 ns = get_ns(data);
 188         } while (vdso_data_read_retry(data, start_seq));
 189
 190         ts->tv_nsec = 0;
 191         timespec_add_ns(ts, ns);
 192
 193         return 0;
 194 }
 195
 196 static __always_inline int do_monotonic(struct timespec *ts,
 197                                         const union mips_vdso_data *data)
 198 {
 199         u32 start_seq;
 200         u64 ns;
 201         u64 to_mono_sec;
 202         u64 to_mono_nsec;
 203
 204         do {
 205                 start_seq = vdso_data_read_begin(data);
 206
 207                 if (data->clock_mode == VDSO_CLOCK_NONE)
 208                         return -ENOSYS;
 209
 210                 ts->tv_sec = data->xtime_sec;
 211                 ns = get_ns(data);
 212
 213                 to_mono_sec = data->wall_to_mono_sec;
 214                 to_mono_nsec = data->wall_to_mono_nsec;
 215         } while (vdso_data_read_retry(data, start_seq));
 216
 217         ts->tv_sec += to_mono_sec;
 218         ts->tv_nsec = 0;
 219         timespec_add_ns(ts, ns + to_mono_nsec);
 220
 221         return 0;
 222 }
 223
 224 #ifdef CONFIG_MIPS_CLOCK_VSYSCALL
 225
 226 /*
 227  * This is behind the ifdef so that we don't provide the symbol when there's no
 228  * possibility of there being a usable clocksource, because there's nothing we
 229  * can do without it. When libc fails the symbol lookup it should fall back on
 230  * the standard syscall path.
 231  */
 232 int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz)
 233 {
 234         const union mips_vdso_data *data = get_vdso_data();
 235         struct timespec ts;
 236         int ret;
 237
 238         ret = do_realtime(&ts, data);
 239         if (ret)
 240                 return gettimeofday_fallback(tv, tz);
 241
 242         if (tv) {
 243                 tv->tv_sec = ts.tv_sec;
 244                 tv->tv_usec = ts.tv_nsec / 1000;
 245         }
 246
 247         if (tz) {
 248                 tz->tz_minuteswest = data->tz_minuteswest;
 249                 tz->tz_dsttime = data->tz_dsttime;
 250         }
 251
 252         return 0;
 253 }
 254
 255 #endif /* CONFIG_MIPS_CLOCK_VSYSCALL */
 256
 257 int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts)
 258 {
 259         const union mips_vdso_data *data = get_vdso_data();
 260         int ret = -1;
 261
 262         switch (clkid) {
 263         case CLOCK_REALTIME_COARSE:
 264                 ret = do_realtime_coarse(ts, data);
 265                 break;
 266         case CLOCK_MONOTONIC_COARSE:
 267                 ret = do_monotonic_coarse(ts, data);
 268                 break;
 269         case CLOCK_REALTIME:
 270                 ret = do_realtime(ts, data);
 271                 break;
 272         case CLOCK_MONOTONIC:
 273                 ret = do_monotonic(ts, data);
 274                 break;
 275         default:
 276                 break;
 277         }
 278
 279         if (ret)
 280                 ret = clock_gettime_fallback(clkid, ts);
 281
 282         return ret;
 283 }