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