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 } 283