/*
 * 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);
    }
}