[444] | 1 | /* |
---|
| 2 | * Copyright (c) 2011 Aeroflex Gaisler |
---|
| 3 | * |
---|
| 4 | * BSD license: |
---|
| 5 | * |
---|
| 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
---|
| 7 | * of this software and associated documentation files (the "Software"), to deal |
---|
| 8 | * in the Software without restriction, including without limitation the rights |
---|
| 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
---|
| 10 | * copies of the Software, and to permit persons to whom the Software is |
---|
| 11 | * furnished to do so, subject to the following conditions: |
---|
| 12 | * |
---|
| 13 | * The above copyright notice and this permission notice shall be included in |
---|
| 14 | * all copies or substantial portions of the Software. |
---|
| 15 | * |
---|
| 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
---|
| 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
---|
| 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL |
---|
| 19 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
---|
| 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
---|
| 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
---|
| 22 | * THE SOFTWARE. |
---|
| 23 | */ |
---|
| 24 | |
---|
| 25 | |
---|
| 26 | #include <sys/types.h> |
---|
| 27 | #include <sys/time.h> |
---|
| 28 | #include <sys/errno.h> |
---|
| 29 | #include <asm-leon/elfmacro.h> |
---|
| 30 | #include <asm-leon/leon.h> |
---|
| 31 | #include <asm-leon/irq.h> |
---|
| 32 | #include <asm-leon/jiffies.h> |
---|
| 33 | #include <asm-leon/param.h> |
---|
| 34 | #include <asm-leon/leoncompat.h> |
---|
| 35 | |
---|
| 36 | static __inline__ unsigned long do_gettimeoffset (); |
---|
| 37 | |
---|
| 38 | |
---|
| 39 | extern int *rtc; |
---|
| 40 | |
---|
| 41 | unsigned long wall_jiffies = INITIAL_JIFFIES; |
---|
| 42 | unsigned long tick_nsec = TICK_NSEC; |
---|
| 43 | unsigned long tick_usec = TICK_NSEC / 1000; |
---|
| 44 | unsigned long seperateirq = 1; |
---|
| 45 | unsigned long noalarm = 1; |
---|
| 46 | unsigned long force_noalarm = 0; |
---|
| 47 | unsigned long nodotimer = 0; |
---|
| 48 | int leonbare_hz = HZ; |
---|
| 49 | |
---|
| 50 | |
---|
| 51 | void settimer (); |
---|
| 52 | inline void |
---|
| 53 | do_timer (struct leonbare_pt_regs *regs) |
---|
| 54 | { |
---|
| 55 | unsigned long ticks; |
---|
| 56 | jiffies_64++; |
---|
| 57 | ticks = jiffies - wall_jiffies; |
---|
| 58 | if (ticks) |
---|
| 59 | { |
---|
| 60 | wall_jiffies += ticks; |
---|
| 61 | do |
---|
| 62 | { |
---|
| 63 | ticks--; |
---|
| 64 | xtime.tv_nsec += tick_nsec; |
---|
| 65 | if (xtime.tv_nsec >= 1000000000) |
---|
| 66 | { |
---|
| 67 | xtime.tv_nsec -= 1000000000; |
---|
| 68 | xtime.tv_sec++; |
---|
| 69 | } |
---|
| 70 | } |
---|
| 71 | while (ticks); |
---|
| 72 | } |
---|
| 73 | settimer (); |
---|
| 74 | } |
---|
| 75 | |
---|
| 76 | int |
---|
| 77 | leonbare_alarm (int irq, void *arg, struct leonbare_pt_regs *regs) |
---|
| 78 | { |
---|
| 79 | settimer (); |
---|
| 80 | } |
---|
| 81 | |
---|
| 82 | extern clock_t (*clock_custom) (void); |
---|
| 83 | clock_t |
---|
| 84 | leonbare_clock_custom () |
---|
| 85 | { |
---|
| 86 | int hz = leonbare_hz ? leonbare_hz : HZ; |
---|
| 87 | return (clock_t) ((jiffies * (CLOCK_TICK_RATE / hz)) + do_gettimeoffset ()); |
---|
| 88 | } |
---|
| 89 | |
---|
| 90 | |
---|
| 91 | tickerhandler ticker_callback = 0; |
---|
| 92 | int |
---|
| 93 | leonbare_tick (int irq, void *arg, struct leonbare_pt_regs *regs) |
---|
| 94 | { |
---|
| 95 | unsigned int ctrl; |
---|
| 96 | if (!seperateirq) |
---|
| 97 | { |
---|
| 98 | /* only leon3 comes here */ |
---|
| 99 | if (!noalarm) |
---|
| 100 | { |
---|
| 101 | ctrl = LEON3_GpTimer_Regs->e[1].ctrl; |
---|
| 102 | if (ctrl & LEON3_GPTIMER_IP) |
---|
| 103 | { |
---|
| 104 | leonbare_alarm (irq, arg, regs); |
---|
| 105 | LEON3_GpTimer_Regs->e[1].ctrl = ctrl & ~LEON3_GPTIMER_IP; |
---|
| 106 | } |
---|
| 107 | } |
---|
| 108 | ctrl = LEON3_GpTimer_Regs->e[0].ctrl; |
---|
| 109 | if (!(ctrl & LEON3_GPTIMER_IP)) |
---|
| 110 | { |
---|
| 111 | return 0; |
---|
| 112 | } |
---|
| 113 | LEON3_GpTimer_Regs->e[0].ctrl = ctrl & ~LEON3_GPTIMER_IP; |
---|
| 114 | } |
---|
| 115 | |
---|
| 116 | if (!nodotimer) |
---|
| 117 | { |
---|
| 118 | do_timer (regs); |
---|
| 119 | } |
---|
| 120 | if (ticker_callback) |
---|
| 121 | { |
---|
| 122 | ticker_callback (regs); |
---|
| 123 | } |
---|
| 124 | return 0; |
---|
| 125 | } |
---|
| 126 | |
---|
| 127 | static struct irqaction irqact1 = { (irqhandler) leonbare_tick, 0, 0, 0 }; |
---|
| 128 | static struct irqaction irqact2 = { (irqhandler) leonbare_alarm, 0, 0, 0 }; |
---|
| 129 | |
---|
| 130 | void |
---|
| 131 | leonbare_init_ticks () |
---|
| 132 | { |
---|
| 133 | int i, irq1 = 0, irq2 = 0; |
---|
| 134 | int hz = leonbare_hz ? leonbare_hz : HZ; |
---|
| 135 | amba_apb_device dev[1]; |
---|
| 136 | |
---|
| 137 | //--------------------- |
---|
| 138 | switch (LEONCOMPAT_VERSION) |
---|
| 139 | { |
---|
| 140 | case 3: |
---|
| 141 | default: |
---|
| 142 | amba_init (); |
---|
| 143 | if (LEON3_GpTimer_Regs && LEON3_IrqCtrl_Regs) |
---|
| 144 | { |
---|
| 145 | if ((LEON3_GpTimer_Regs->config & LEON3_GPTIMER_CONFIG_TIMERMASK) >= |
---|
| 146 | 2 && force_noalarm == 0) |
---|
| 147 | noalarm = 0; |
---|
| 148 | if (!(LEON3_GpTimer_Regs->config & LEON3_GPTIMER_CONFIG_SEPERATE)) |
---|
| 149 | seperateirq = 0; |
---|
| 150 | LEON3_GpTimer_Regs->e[0].val = 0; |
---|
| 151 | LEON3_GpTimer_Regs->e[0].rld = (((CLOCK_TICK_RATE / hz) - 1)); |
---|
| 152 | LEON3_GpTimer_Regs->e[0].ctrl = 0; |
---|
| 153 | LEON3_GpTimer_Regs->e[0].ctrl = |
---|
| 154 | LEON3_GPTIMER_EN | |
---|
| 155 | LEON3_GPTIMER_RL | LEON3_GPTIMER_LD | LEON3_GPTIMER_IRQEN; |
---|
| 156 | irq1 = LEON3_GpTimer_Irq; |
---|
| 157 | irq2 = LEON3_GpTimer_Irq + 1; |
---|
| 158 | } |
---|
| 159 | break; |
---|
| 160 | } |
---|
| 161 | //--------------------- |
---|
| 162 | |
---|
| 163 | if (irq1) |
---|
| 164 | { |
---|
| 165 | clock_custom = leonbare_clock_custom; |
---|
| 166 | chained_catch_interrupt (irq1, &irqact1); |
---|
| 167 | leonbare_enable_irq (irq1); |
---|
| 168 | } |
---|
| 169 | if (irq2 && (!noalarm) && seperateirq) |
---|
| 170 | { |
---|
| 171 | chained_catch_interrupt (irq2, &irqact2); |
---|
| 172 | leonbare_enable_irq (irq2); |
---|
| 173 | } |
---|
| 174 | } |
---|
| 175 | |
---|
| 176 | |
---|
| 177 | //''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' |
---|
| 178 | |
---|
| 179 | static __inline__ unsigned long |
---|
| 180 | do_gettimeoffset () |
---|
| 181 | { |
---|
| 182 | unsigned long usec = 0; |
---|
| 183 | //--------------------- |
---|
| 184 | switch (LEONCOMPAT_VERSION) |
---|
| 185 | { |
---|
| 186 | case 3: |
---|
| 187 | default: |
---|
| 188 | usec = ((LEON3_GpTimer_Regs->e[0].rld & 0x7fffff) - |
---|
| 189 | (LEON3_GpTimer_Regs->e[0].val & 0x7fffff)); |
---|
| 190 | break; |
---|
| 191 | } |
---|
| 192 | //--------------------- |
---|
| 193 | return usec; |
---|
| 194 | } |
---|
| 195 | |
---|
| 196 | /* get usec (timeval) resolution, |
---|
| 197 | * could use nsec (timespec) because pthread use it (todo) */ |
---|
| 198 | void |
---|
| 199 | do_gettimeofday (struct timeval *tv) |
---|
| 200 | { |
---|
| 201 | |
---|
| 202 | unsigned long flags; |
---|
| 203 | unsigned long seq; |
---|
| 204 | unsigned long usec, sec; |
---|
| 205 | |
---|
| 206 | do |
---|
| 207 | { |
---|
| 208 | unsigned long lost; |
---|
| 209 | seq = jiffies; |
---|
| 210 | |
---|
| 211 | usec = do_gettimeoffset (); |
---|
| 212 | lost = jiffies - wall_jiffies; |
---|
| 213 | |
---|
| 214 | if (unlikely (lost)) |
---|
| 215 | { |
---|
| 216 | usec += lost * tick_usec; |
---|
| 217 | } |
---|
| 218 | |
---|
| 219 | sec = xtime.tv_sec; |
---|
| 220 | usec += (xtime.tv_nsec / 1000); |
---|
| 221 | } |
---|
| 222 | while (seq != jiffies); |
---|
| 223 | |
---|
| 224 | while (usec >= 1000000) |
---|
| 225 | { |
---|
| 226 | usec -= 1000000; |
---|
| 227 | sec++; |
---|
| 228 | } |
---|
| 229 | |
---|
| 230 | tv->tv_sec = sec; |
---|
| 231 | tv->tv_usec = usec; |
---|
| 232 | } |
---|
| 233 | |
---|
| 234 | int |
---|
| 235 | gettimeofday (struct timeval *__p, void *__tz) |
---|
| 236 | { |
---|
| 237 | do_gettimeofday (__p); |
---|
| 238 | return 0; |
---|
| 239 | } |
---|
| 240 | |
---|
| 241 | //''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' |
---|
| 242 | |
---|
| 243 | static int |
---|
| 244 | do_settimeofday (struct timespec *tv) |
---|
| 245 | { |
---|
| 246 | time_t sec = tv->tv_sec; |
---|
| 247 | long nsec = tv->tv_nsec; |
---|
| 248 | |
---|
| 249 | if ((unsigned long) nsec >= NSEC_PER_SEC) |
---|
| 250 | return EINVAL; |
---|
| 251 | |
---|
| 252 | /* |
---|
| 253 | * This is revolting. We need to set "xtime" correctly. However, the |
---|
| 254 | * value in this location is the value at the most recent update of |
---|
| 255 | * wall time. Discover what correction gettimeofday() would have |
---|
| 256 | * made, and then undo it! |
---|
| 257 | */ |
---|
| 258 | nsec -= 1000 * (do_gettimeoffset () + |
---|
| 259 | (jiffies - wall_jiffies) * (USEC_PER_SEC / HZ)); |
---|
| 260 | |
---|
| 261 | set_normalized_timespec (&xtime, sec, nsec); |
---|
| 262 | return 0; |
---|
| 263 | } |
---|
| 264 | |
---|
| 265 | int |
---|
| 266 | settimeofday (const struct timeval *tv, const struct timezone *tz) |
---|
| 267 | { |
---|
| 268 | struct timespec ts; |
---|
| 269 | ts.tv_sec = tv->tv_sec; |
---|
| 270 | ts.tv_nsec = tv->tv_usec * NSEC_PER_USEC; |
---|
| 271 | return do_settimeofday (&ts); |
---|
| 272 | } |
---|