Firmwares/riscv-bldc-forced-commutation/src/delay.c

124 lines
3.2 KiB
C
Raw Normal View History

/*
* delay.c
*
* Created on: 30.07.2018
* Author: eyck
*/
#include "delay.h"
#define rdmcycle(x) { \
uint32_t lo, hi, hi2; \
__asm__ __volatile__ ("1:\n\t" \
"csrr %0, mcycleh\n\t" \
"csrr %1, mcycle\n\t" \
"csrr %2, mcycleh\n\t" \
"bne %0, %2, 1b\n\t" \
: "=r" (hi), "=r" (lo), "=r" (hi2)) ; \
*(x) = lo | ((uint64_t) hi << 32); \
}
typedef struct {
uint32_t n;
uint32_t mult;
uint32_t shift;
} int_inverse ;
int_inverse f_cpu_1000_inv;
int_inverse f_cpu_1000000_inv;
uint32_t F_CPU=1000000;
void calc_inv(uint32_t n, int_inverse * res){
uint32_t one = ~0;
uint32_t d = one/n;
uint32_t r = one%n + 1;
if (r >= n) ++d;
if (d == 0) --d;
uint32_t shift = 0;
while ((d & 0x80000000) == 0){
d <<= 1;
++shift;
}
res->n = n;
res->mult = d;
res->shift = shift;
}
uint32_t divide32_using_inverse(uint32_t n, int_inverse *inv){
uint32_t d = (uint32_t)(((uint64_t)n * inv->mult) >> 32);
d >>= inv->shift;
if (n - d*inv->n >= inv->n) ++d;
return d;
}
// Almost full-range 64/32 divide.
// If divisor-1 has i bits, then the answer is exact for n of up to 64-i bits
// e.g. for divisors up to a million, n can have up to 45 bits
// On RV32IM with divide32_using_inverse inlines this uses 5 multiplies,
// 33 instructions, zero branches, 3 loads, 0 stores.
uint64_t divide64_using_inverse(uint64_t n, int_inverse *inv){
uint32_t preshift = (31 - inv->shift) & 31;
uint64_t d = (uint64_t)divide32_using_inverse(n >> preshift, inv) << preshift;
uint32_t r = n - d * inv->n;
d += divide32_using_inverse(r, inv);
return d;
}
uint32_t millis(){
uint64_t x;
rdmcycle(&x);
x = divide64_using_inverse(x, &f_cpu_1000_inv);
return((uint32_t) (x & 0xFFFFFFFF));
}
uint32_t micros(void){
uint64_t x;
rdmcycle(&x);
// For Power-of-two MHz F_CPU,
// this compiles into a simple shift,
// and is faster than the general solution.
#if F_CPU==16000000
x = x / (F_CPU / 1000000);
#else
#if F_CPU==256000000
x = x / (F_CPU / 1000000);
#else
x = divide64_using_inverse(x, &f_cpu_1000000_inv);
#endif
#endif
return((uint32_t) (x & 0xFFFFFFFF));
}
void delayMS(uint32_t dwMs){
uint64_t current, later;
rdmcycle(&current);
later = current + dwMs * (F_CPU/1000);
if (later > current){ // usual case
while (later > current)
rdmcycle(&current);
} else { // wrap. Though this is unlikely to be hit w/ 64-bit mcycle
while (later < current)
rdmcycle(&current);
while (current < later)
rdmcycle(&current);
}
}
void delayUS(uint32_t dwUs){
uint64_t current, later;
rdmcycle(&current);
later = current + dwUs * (F_CPU/1000000);
if (later > current){ // usual case
while (later > current)
rdmcycle(&current);
} else {// wrap. Though this is unlikely to be hit w/ 64-bit mcycle
while (later < current)
rdmcycle(&current);
while (current < later)
rdmcycle(&current);
}
}