Alexandru Gagniuc | 065b7da | 2014-04-15 15:41:38 -0500 | [diff] [blame^] | 1 | /* |
| 2 | * udelay() impementation for SMI handlers |
| 3 | * This is neat in that it never writes to hardware registers, and thus does not |
| 4 | * modify the state of the hardware while servicing SMIs. |
| 5 | * |
| 6 | * Copyright (C) 2014 Alexandru Gagniuc <mr.nuke.me@gmail.com> |
| 7 | * Subject to the GNU GPL v2, or (at your option) any later version. |
| 8 | */ |
| 9 | |
| 10 | #include <cpu/x86/msr.h> |
| 11 | #include <cpu/x86/tsc.h> |
| 12 | #include <delay.h> |
| 13 | #include <stdint.h> |
| 14 | |
| 15 | void udelay(uint32_t us) |
| 16 | { |
| 17 | uint8_t fid, did, pstate_idx; |
| 18 | uint64_t tsc_clock, tsc_start, tsc_now, tsc_wait_ticks; |
| 19 | msr_t msr; |
| 20 | const uint64_t tsc_base = 100000000; |
| 21 | |
| 22 | /* Get initial timestamp before we do the math */ |
| 23 | tsc_start = rdtscll(); |
| 24 | |
| 25 | /* Get the P-state. This determines which MSR to read */ |
| 26 | msr = rdmsr(0xc0010063); |
| 27 | pstate_idx = msr.lo & 0x07; |
| 28 | |
| 29 | /* Get FID and VID for current P-State */ |
| 30 | msr = rdmsr(0xc0010064 + pstate_idx); |
| 31 | |
| 32 | /* Extract the FID and VID values */ |
| 33 | fid = msr.lo & 0x3f; |
| 34 | did = (msr.lo >> 6) & 0x7; |
| 35 | |
| 36 | /* Calculate the CPU clock (from base freq of 100MHz) */ |
| 37 | tsc_clock = tsc_base * (fid + 0x10) / (1 << did); |
| 38 | |
| 39 | /* Now go on and wait */ |
| 40 | tsc_wait_ticks = (tsc_clock / 1000000) * us; |
| 41 | |
| 42 | do { |
| 43 | tsc_now = rdtscll(); |
| 44 | } while (tsc_now - tsc_wait_ticks < tsc_start); |
| 45 | } |