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