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/barrier.h> 22 #include <asm/bug.h> 23 #include <asm/cp15.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 = READ_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 isb(); 127 cycle_now = read_sysreg(CNTVCT); 128 129 cycle_delta = (cycle_now - vdata->cs_cycle_last) & vdata->cs_mask; 130 131 nsec = (cycle_delta * vdata->cs_mult) + vdata->xtime_clock_snsec; 132 nsec >>= vdata->cs_shift; 133 134 return nsec; 135 } 136 137 static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata) 138 { 139 u64 nsecs; 140 u32 seq; 141 142 do { 143 seq = vdso_read_begin(vdata); 144 145 if (!vdata->tk_is_cntvct) 146 return -1; 147 148 ts->tv_sec = vdata->xtime_clock_sec; 149 nsecs = get_ns(vdata); 150 151 } while (vdso_read_retry(vdata, seq)); 152 153 ts->tv_nsec = 0; 154 timespec_add_ns(ts, nsecs); 155 156 return 0; 157 } 158 159 static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata) 160 { 161 struct timespec tomono; 162 u64 nsecs; 163 u32 seq; 164 165 do { 166 seq = vdso_read_begin(vdata); 167 168 if (!vdata->tk_is_cntvct) 169 return -1; 170 171 ts->tv_sec = vdata->xtime_clock_sec; 172 nsecs = get_ns(vdata); 173 174 tomono.tv_sec = vdata->wtm_clock_sec; 175 tomono.tv_nsec = vdata->wtm_clock_nsec; 176 177 } while (vdso_read_retry(vdata, seq)); 178 179 ts->tv_sec += tomono.tv_sec; 180 ts->tv_nsec = 0; 181 timespec_add_ns(ts, nsecs + tomono.tv_nsec); 182 183 return 0; 184 } 185 186 #else /* CONFIG_ARM_ARCH_TIMER */ 187 188 static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata) 189 { 190 return -1; 191 } 192 193 static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata) 194 { 195 return -1; 196 } 197 198 #endif /* CONFIG_ARM_ARCH_TIMER */ 199 200 notrace int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts) 201 { 202 struct vdso_data *vdata; 203 int ret = -1; 204 205 vdata = __get_datapage(); 206 207 switch (clkid) { 208 case CLOCK_REALTIME_COARSE: 209 ret = do_realtime_coarse(ts, vdata); 210 break; 211 case CLOCK_MONOTONIC_COARSE: 212 ret = do_monotonic_coarse(ts, vdata); 213 break; 214 case CLOCK_REALTIME: 215 ret = do_realtime(ts, vdata); 216 break; 217 case CLOCK_MONOTONIC: 218 ret = do_monotonic(ts, vdata); 219 break; 220 default: 221 break; 222 } 223 224 if (ret) 225 ret = clock_gettime_fallback(clkid, ts); 226 227 return ret; 228 } 229 230 static notrace long gettimeofday_fallback(struct timeval *_tv, 231 struct timezone *_tz) 232 { 233 register struct timezone *tz asm("r1") = _tz; 234 register struct timeval *tv asm("r0") = _tv; 235 register long ret asm ("r0"); 236 register long nr asm("r7") = __NR_gettimeofday; 237 238 asm volatile( 239 " swi #0\n" 240 : "=r" (ret) 241 : "r" (tv), "r" (tz), "r" (nr) 242 : "memory"); 243 244 return ret; 245 } 246 247 notrace int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz) 248 { 249 struct timespec ts; 250 struct vdso_data *vdata; 251 int ret; 252 253 vdata = __get_datapage(); 254 255 ret = do_realtime(&ts, vdata); 256 if (ret) 257 return gettimeofday_fallback(tv, tz); 258 259 if (tv) { 260 tv->tv_sec = ts.tv_sec; 261 tv->tv_usec = ts.tv_nsec / 1000; 262 } 263 if (tz) { 264 tz->tz_minuteswest = vdata->tz_minuteswest; 265 tz->tz_dsttime = vdata->tz_dsttime; 266 } 267 268 return ret; 269 } 270 271 /* Avoid unresolved references emitted by GCC */ 272 273 void __aeabi_unwind_cpp_pr0(void) 274 { 275 } 276 277 void __aeabi_unwind_cpp_pr1(void) 278 { 279 } 280 281 void __aeabi_unwind_cpp_pr2(void) 282 { 283 } 284