arch/arm/vdso/vgettimeofday.c

   1 /*
   2  * Copyright 2015 Mentor Graphics Corporation.
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; version 2 of the
   7  * License.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <linux/compiler.h>
  19 #include <linux/hrtimer.h>
  20 #include <linux/time.h>
  21 #include <asm/arch_timer.h>
  22 #include <asm/barrier.h>
  23 #include <asm/bug.h>
  24 #include <asm/page.h>
  25 #include <asm/unistd.h>
  26 #include <asm/vdso_datapage.h>
  27
  28 #ifndef CONFIG_AEABI
  29 #error This code depends on AEABI system call conventions
  30 #endif
  31
  32 extern struct vdso_data *__get_datapage(void);
  33
  34 static notrace u32 __vdso_read_begin(const struct vdso_data *vdata)
  35 {
  36         u32 seq;
  37 repeat:
  38         seq = ACCESS_ONCE(vdata->seq_count);
  39         if (seq & 1) {
  40                 cpu_relax();
  41                 goto repeat;
  42         }
  43         return seq;
  44 }
  45
  46 static notrace u32 vdso_read_begin(const struct vdso_data *vdata)
  47 {
  48         u32 seq;
  49
  50         seq = __vdso_read_begin(vdata);
  51
  52         smp_rmb(); /* Pairs with smp_wmb in vdso_write_end */
  53         return seq;
  54 }
  55
  56 static notrace int vdso_read_retry(const struct vdso_data *vdata, u32 start)
  57 {
  58         smp_rmb(); /* Pairs with smp_wmb in vdso_write_begin */
  59         return vdata->seq_count != start;
  60 }
  61
  62 static notrace long clock_gettime_fallback(clockid_t _clkid,
  63                                            struct timespec *_ts)
  64 {
  65         register struct timespec *ts asm("r1") = _ts;
  66         register clockid_t clkid asm("r0") = _clkid;
  67         register long ret asm ("r0");
  68         register long nr asm("r7") = __NR_clock_gettime;
  69
  70         asm volatile(
  71         "       swi #0\n"
  72         : "=r" (ret)
  73         : "r" (clkid), "r" (ts), "r" (nr)
  74         : "memory");
  75
  76         return ret;
  77 }
  78
  79 static notrace int do_realtime_coarse(struct timespec *ts,
  80                                       struct vdso_data *vdata)
  81 {
  82         u32 seq;
  83
  84         do {
  85                 seq = vdso_read_begin(vdata);
  86
  87                 ts->tv_sec = vdata->xtime_coarse_sec;
  88                 ts->tv_nsec = vdata->xtime_coarse_nsec;
  89
  90         } while (vdso_read_retry(vdata, seq));
  91
  92         return 0;
  93 }
  94
  95 static notrace int do_monotonic_coarse(struct timespec *ts,
  96                                        struct vdso_data *vdata)
  97 {
  98         struct timespec tomono;
  99         u32 seq;
 100
 101         do {
 102                 seq = vdso_read_begin(vdata);
 103
 104                 ts->tv_sec = vdata->xtime_coarse_sec;
 105                 ts->tv_nsec = vdata->xtime_coarse_nsec;
 106
 107                 tomono.tv_sec = vdata->wtm_clock_sec;
 108                 tomono.tv_nsec = vdata->wtm_clock_nsec;
 109
 110         } while (vdso_read_retry(vdata, seq));
 111
 112         ts->tv_sec += tomono.tv_sec;
 113         timespec_add_ns(ts, tomono.tv_nsec);
 114
 115         return 0;
 116 }
 117
 118 #ifdef CONFIG_ARM_ARCH_TIMER
 119
 120 static notrace u64 get_ns(struct vdso_data *vdata)
 121 {
 122         u64 cycle_delta;
 123         u64 cycle_now;
 124         u64 nsec;
 125
 126         cycle_now = arch_counter_get_cntvct();
 127
 128         cycle_delta = (cycle_now - vdata->cs_cycle_last) & vdata->cs_mask;
 129
 130         nsec = (cycle_delta * vdata->cs_mult) + vdata->xtime_clock_snsec;
 131         nsec >>= vdata->cs_shift;
 132
 133         return nsec;
 134 }
 135
 136 static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata)
 137 {
 138         u64 nsecs;
 139         u32 seq;
 140
 141         do {
 142                 seq = vdso_read_begin(vdata);
 143
 144                 if (!vdata->tk_is_cntvct)
 145                         return -1;
 146
 147                 ts->tv_sec = vdata->xtime_clock_sec;
 148                 nsecs = get_ns(vdata);
 149
 150         } while (vdso_read_retry(vdata, seq));
 151
 152         ts->tv_nsec = 0;
 153         timespec_add_ns(ts, nsecs);
 154
 155         return 0;
 156 }
 157
 158 static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata)
 159 {
 160         struct timespec tomono;
 161         u64 nsecs;
 162         u32 seq;
 163
 164         do {
 165                 seq = vdso_read_begin(vdata);
 166
 167                 if (!vdata->tk_is_cntvct)
 168                         return -1;
 169
 170                 ts->tv_sec = vdata->xtime_clock_sec;
 171                 nsecs = get_ns(vdata);
 172
 173                 tomono.tv_sec = vdata->wtm_clock_sec;
 174                 tomono.tv_nsec = vdata->wtm_clock_nsec;
 175
 176         } while (vdso_read_retry(vdata, seq));
 177
 178         ts->tv_sec += tomono.tv_sec;
 179         ts->tv_nsec = 0;
 180         timespec_add_ns(ts, nsecs + tomono.tv_nsec);
 181
 182         return 0;
 183 }
 184
 185 #else /* CONFIG_ARM_ARCH_TIMER */
 186
 187 static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata)
 188 {
 189         return -1;
 190 }
 191
 192 static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata)
 193 {
 194         return -1;
 195 }
 196
 197 #endif /* CONFIG_ARM_ARCH_TIMER */
 198
 199 notrace int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts)
 200 {
 201         struct vdso_data *vdata;
 202         int ret = -1;
 203
 204         vdata = __get_datapage();
 205
 206         switch (clkid) {
 207         case CLOCK_REALTIME_COARSE:
 208                 ret = do_realtime_coarse(ts, vdata);
 209                 break;
 210         case CLOCK_MONOTONIC_COARSE:
 211                 ret = do_monotonic_coarse(ts, vdata);
 212                 break;
 213         case CLOCK_REALTIME:
 214                 ret = do_realtime(ts, vdata);
 215                 break;
 216         case CLOCK_MONOTONIC:
 217                 ret = do_monotonic(ts, vdata);
 218                 break;
 219         default:
 220                 break;
 221         }
 222
 223         if (ret)
 224                 ret = clock_gettime_fallback(clkid, ts);
 225
 226         return ret;
 227 }
 228
 229 static notrace long gettimeofday_fallback(struct timeval *_tv,
 230                                           struct timezone *_tz)
 231 {
 232         register struct timezone *tz asm("r1") = _tz;
 233         register struct timeval *tv asm("r0") = _tv;
 234         register long ret asm ("r0");
 235         register long nr asm("r7") = __NR_gettimeofday;
 236
 237         asm volatile(
 238         "       swi #0\n"
 239         : "=r" (ret)
 240         : "r" (tv), "r" (tz), "r" (nr)
 241         : "memory");
 242
 243         return ret;
 244 }
 245
 246 notrace int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz)
 247 {
 248         struct timespec ts;
 249         struct vdso_data *vdata;
 250         int ret;
 251
 252         vdata = __get_datapage();
 253
 254         ret = do_realtime(&ts, vdata);
 255         if (ret)
 256                 return gettimeofday_fallback(tv, tz);
 257
 258         if (tv) {
 259                 tv->tv_sec = ts.tv_sec;
 260                 tv->tv_usec = ts.tv_nsec / 1000;
 261         }
 262         if (tz) {
 263                 tz->tz_minuteswest = vdata->tz_minuteswest;
 264                 tz->tz_dsttime = vdata->tz_dsttime;
 265         }
 266
 267         return ret;
 268 }
 269
 270 /* Avoid unresolved references emitted by GCC */
 271
 272 void __aeabi_unwind_cpp_pr0(void)
 273 {
 274 }
 275
 276 void __aeabi_unwind_cpp_pr1(void)
 277 {
 278 }
 279
 280 void __aeabi_unwind_cpp_pr2(void)
 281 {
 282 }