diff --git a/libs/bio-lib/src/c/include/bio.h b/libs/bio-lib/src/c/include/bio.h index 3708fac43..8282de08b 100644 --- a/libs/bio-lib/src/c/include/bio.h +++ b/libs/bio-lib/src/c/include/bio.h @@ -1,5 +1,21 @@ +#ifndef BIO_H +#define BIO_H + #include +// ---------------------------------------------------------------- +// entry point - this MUST be at the top of this file so that +// _start ends up at 0x0. +// ---------------------------------------------------------------- + +__attribute__((naked, section(".text._start"))) +void _start(void) { + __asm__ volatile ( + "li sp, 0x1000\n" // Load stack pointer to 0x1000 - top of RAM + "j main\n" // Jump to main + ); +} + // pop_fifoN: read from reserved register xN. // volatile prevents the compiler from caching or dropping the read. static inline __attribute__((always_inline)) uint32_t pop_fifo0() { @@ -46,10 +62,74 @@ static inline __attribute__((always_inline)) void push_fifo3(uint32_t tx) { __asm__ volatile ("mv x19, %0" : : "r"(tx) : "memory"); } -__attribute__((naked, section(".text._start"))) -void _start(void) { - __asm__ volatile ( - "li sp, 0x1000\n" // Load stack pointer to 0xE00 - "j main\n" // Jump to main - ); -} \ No newline at end of file +// GPIO accessors +static inline __attribute__((always_inline)) void set_gpio_mask(uint32_t mask) { + __asm__ volatile ("mv x26, %0" : : "r"(mask) : "memory"); +} +static inline __attribute__((always_inline)) uint32_t get_gpio_mask() { + uint32_t mask; + __asm__ volatile ("mv %0, x26" : "=r"(mask)); + return mask; +} + +static inline __attribute__((always_inline)) void write_gpio_pins(uint32_t val) { + __asm__ volatile ("mv x21, %0" : : "r"(val) : "memory"); +} +static inline __attribute__((always_inline)) uint32_t read_gpio_pins() { + uint32_t a; + __asm__ volatile ("mv %0, x21" : "=r"(a)); + return a; +} + +static inline __attribute__((always_inline)) void set_gpio_pins(uint32_t val) { + __asm__ volatile ("mv x22, %0" : : "r"(val) : "memory"); +} +// the `_n` name reminds us that *0* values clear the pin, and 1 does nothing +static inline __attribute__((always_inline)) void clear_gpio_pins_n(uint32_t val_n) { + __asm__ volatile ("mv x23, %0" : : "r"(val_n) : "memory"); +} + +static inline __attribute__((always_inline)) void set_output_pins(uint32_t val) { + __asm__ volatile ("mv x24, %0" : : "r"(val) : "memory"); +} +static inline __attribute__((always_inline)) void set_input_pins(uint32_t val) { + __asm__ volatile ("mv x25, %0" : : "r"(val) : "memory"); +} + +// Events +static inline __attribute__((always_inline)) void wait_quantum() { + __asm__ volatile ("mv x20, zero" : : : "memory"); +} +static inline __attribute__((always_inline)) uint32_t event_status() { + uint32_t a; + __asm__ volatile ("mv %0, x30" : "=r"(a)); + return a; +} +static inline __attribute__((always_inline)) void set_event_mask(uint32_t mask) { + __asm__ volatile ("mv x27, %0" : : "r"(mask) : "memory"); +} +static inline __attribute__((always_inline)) void set_event_bits(uint32_t mask) { + __asm__ volatile ("mv x28, %0" : : "r"(mask) : "memory"); +} +static inline __attribute__((always_inline)) void clear_event_bits(uint32_t mask) { + __asm__ volatile ("mv x29, %0" : : "r"(mask) : "memory"); +} + +// Debug +static inline __attribute__((always_inline)) uint32_t core_id() { + uint32_t a; + __asm__ volatile ("mv %0, x31" : "=r"(a)); + return a >> 30; +} +static inline __attribute__((always_inline)) uint32_t aclk_counter() { + uint32_t a; + __asm__ volatile ("mv %0, x31" : "=r"(a)); + return a & 0x3FFFFFFF; +} +static inline __attribute__((always_inline)) uint32_t raw_x31() { + uint32_t a; + __asm__ volatile ("mv %0, x31" : "=r"(a)); + return a; +} + +#endif \ No newline at end of file diff --git a/libs/bio-lib/src/c/include/fp.h b/libs/bio-lib/src/c/include/fp_q12.h similarity index 100% rename from libs/bio-lib/src/c/include/fp.h rename to libs/bio-lib/src/c/include/fp_q12.h diff --git a/libs/bio-lib/src/c/include/fp_q16.h b/libs/bio-lib/src/c/include/fp_q16.h new file mode 100644 index 000000000..ffb81596e --- /dev/null +++ b/libs/bio-lib/src/c/include/fp_q16.h @@ -0,0 +1,276 @@ +/* + * fp_q16.h - Fixed-point math library + * + * Format: configurable Q format via FP_FRAC_BITS + * Default: Q16.16 (16 integer bits, 16 fractional bits) + * + * Representation: signed 32-bit integer + * Range (Q16.16): -32768.0 to +32767.99999 + * Resolution: 1/65536 ~= 0.0000153 + * + * Cosine is computed via CORDIC (rotation mode, 16 iterations) rather than + * a lookup table. Effective precision is ~15 ulp, substantially better than + * the 64-entry table used in the Q12 version. + */ + +#ifndef FP_H +#define FP_H + +#include +#include + +/* --- Configuration --- */ + +#ifndef FP_FRAC_BITS +#define FP_FRAC_BITS 16 +#endif + +#define FP_ONE (1 << FP_FRAC_BITS) +#define FP_MASK (FP_ONE - 1) + +typedef int32_t fp_t; + +/* --- Constants --- + * + * All computed at compile time from integer literals. + * + * At Q16 (FP_ONE = 65536): + * PI = round(3.14159265358979 * 65536) = 205887 + * TWO_PI = 411775 + * HALF_PI = 102944 + */ +#define FP_PI ((fp_t)(3.14159265358979f * FP_ONE + 0.5f)) +#define FP_TWO_PI ((fp_t)(6.28318530717959f * FP_ONE + 0.5f)) +#define FP_HALF_PI ((fp_t)(1.57079632679490f * FP_ONE + 0.5f)) + +/* --- Conversion --- */ + +#define FP_FROM_INT(x) ((fp_t)((x) << FP_FRAC_BITS)) +#define FP_TO_INT(x) ((int32_t)((x) >> FP_FRAC_BITS)) +#define FP_FROM_FLOAT(x) ((fp_t)((x) * FP_ONE + 0.5f)) +#define FP_TO_FLOAT(x) ((float)(x) / FP_ONE) + +/* --- Arithmetic --- */ + +static inline fp_t fp_add(fp_t a, fp_t b) +{ + return a + b; +} + +static inline fp_t fp_sub(fp_t a, fp_t b) +{ + return a - b; +} + +/* + * fp_mul - fixed-point multiply using RISC-V mulh/mul pair. + * + * Reconstructs the full 64-bit product from the high (mulh) and low (mul) + * 32-bit words, then shifts right by FP_FRAC_BITS to re-align the binary + * point. The early-clobber on 'hi' (&) prevents the compiler from aliasing + * it with an input register. + */ + +static inline fp_t fp_mul(fp_t a, fp_t b) +{ + int64_t result = (int64_t)a * (int64_t)b; + return (fp_t)(result >> FP_FRAC_BITS); +} + +/* + * u32_divrem - unsigned 32-bit shift-and-subtract divider. + * + * 32 iterations of: shift one bit of the dividend into the partial + * remainder, subtract the divisor if it fits, record the quotient bit. + * No hardware divide instruction required. + */ +static inline uint32_t u32_divrem(uint32_t dividend, uint32_t divisor, + uint32_t *remainder) +{ + uint32_t quotient = 0; + uint32_t rem = 0; + int i; + + for (i = 31; i >= 0; i--) { + rem = (rem << 1) | ((dividend >> i) & 1); + if (rem >= divisor) { + rem -= divisor; + quotient |= (1u << i); + } + } + + if (remainder) *remainder = rem; + return quotient; +} + +/* + * fp_div - signed fixed-point division, no hardware divide. + * + * Works in two passes through u32_divrem: + * pass 1: integer quotient and remainder of |a| / |b| + * pass 2: fractional bits from (remainder << FP_FRAC_BITS) / |b| + * Sign is restored at the end. + */ +static inline fp_t fp_div(fp_t a, fp_t b) +{ + int32_t sign = ((a ^ b) < 0) ? -1 : 1; + uint32_t ua = (uint32_t)(a < 0 ? -a : a); + uint32_t ub = (uint32_t)(b < 0 ? -b : b); + uint32_t q, r; + fp_t res; + + q = u32_divrem(ua, ub, &r); + res = (fp_t)((q << FP_FRAC_BITS) | u32_divrem(r << FP_FRAC_BITS, ub, NULL)); + + return sign < 0 ? -res : res; +} + +/* --- CORDIC cosine --- */ + +/* + * CORDIC rotation-mode overview + * ============================== + * At each step i (0 .. N-1) we rotate a 2-D vector (x, y) by ±atan(2^-i): + * + * x' = x - d * (y >> i) + * y' = y + d * (x >> i) d = +1 if remaining angle > 0, else -1 + * z' = z - d * atan_table[i] + * + * Starting with (x,y,z) = (K, 0, target_angle), after N iterations: + * x ≈ cos(target_angle) (in Q16, scaled by FP_ONE) + * y ≈ sin(target_angle) + * + * where K = 1 / prod_{i=0}^{N-1} sqrt(1 + 2^{-2i}) is the CORDIC gain + * pre-applied to the initial x so the output is already unit-scaled. + * + * Number of iterations + * -------------------- + * 16 iterations give ~16 bits of precision, matching FP_FRAC_BITS = 16. + * Each iteration adds roughly one bit of accuracy. + * + * atan table (Q16) + * ---------------- + * atan_table[i] = round( atan(2^-i) * 65536 ) + * + * Generated by: + * import math + * Q = 65536 + * [round(math.atan(2**-i) * Q) for i in range(16)] + * + * i=0: atan(1) = 0.7853982 -> 51472 + * i=1: atan(0.5) = 0.4636476 -> 30386 + * i=2: atan(0.25) = 0.2449787 -> 16055 + * i=3: atan(0.125) = 0.1243550 -> 8150 + * ... + * + * CORDIC gain K (Q16) + * -------------------- + * K = prod_{i=0}^{15} 1/sqrt(1 + 2^{-2i}) ~= 0.60725293500888 + * K_Q16 = round(0.60725293500888 * 65536) = 39797 + */ + +#define CORDIC_ITERATIONS 16 + +static const int32_t cordic_atan_table[CORDIC_ITERATIONS] = { + 51472, 30386, 16055, 8150, 4091, 2047, 1024, 512, + 256, 128, 64, 32, 16, 8, 4, 2 +}; + +/* + * CORDIC gain K in Q16. + * + * K = 1 / prod_{i=0}^{15} sqrt(1 + 2^{-2i}) + * = 0.6072529350088812561... + * round(K * 65536) = 39797 + * + * We start the CORDIC vector at (K_Q16, 0) so that the output x is already + * in Q16 with magnitude cos(angle), no post-multiply needed. + */ +#define CORDIC_K_Q16 ((int32_t)39797) + +/* + * fp_cos_cordic - internal CORDIC rotation for angle in [0, pi/2]. + * + * Input: angle in Q16, must be in [0, FP_HALF_PI]. + * Output: cos(angle) in Q16. + * + * The initial vector is (K, 0) with K pre-scaled by FP_ONE so all + * arithmetic stays in integer Q16 throughout. + */ +static inline fp_t fp_cos_cordic(fp_t angle) +{ + int32_t x = CORDIC_K_Q16; + int32_t y = 0; + int32_t z = angle; + int i; + + for (i = 0; i < CORDIC_ITERATIONS; i++) { + int32_t dx = y >> i; + int32_t dy = x >> i; + if (z >= 0) { + /* Rotate counter-clockwise: subtract angle step */ + x -= dx; + y += dy; + z -= cordic_atan_table[i]; + } else { + /* Rotate clockwise: add angle step */ + x += dx; + y -= dy; + z += cordic_atan_table[i]; + } + } + + return (fp_t)x; +} + +/* + * fp_cos - cosine of x (angle in Q16 radians). + * + * Symmetry used to cover the full circle from [0, pi/2]: + * [0, pi/2]: cos(x) -> CORDIC direct + * [pi/2, pi]: cos(pi - x) -> CORDIC, result negated + * [pi, 3pi/2]: cos(x - pi) -> same as [0, pi/2] by symmetry, negated + * [3pi/2, 2pi]: cos(2pi - x) -> CORDIC direct + * + * Range reduction for large magnitudes uses integer period counting + * (one division by 2*pi) to avoid slow repeated subtraction loops. + */ +static inline fp_t fp_cos(fp_t x) +{ + int32_t negate = 0; + + /* --- Range-reduce to [0, 2*pi) --- */ + if (x < 0) { + /* cos is even: cos(-x) = cos(x) */ + x = -x; + } + if (x >= FP_TWO_PI) { + /* Integer number of full periods to subtract. + * fp_div is used here; for performance-critical paths the caller + * should pre-reduce the angle. */ + int32_t periods = FP_TO_INT(fp_div(x, FP_TWO_PI)); + x -= (fp_t)(periods * FP_TWO_PI); + /* Guard against rounding pushing x back over 2pi */ + if (x >= FP_TWO_PI) x -= FP_TWO_PI; + } + + /* --- Quadrant reduction to [0, pi/2] --- */ + + /* [pi, 2pi] -> [0, pi]: cos(2pi - x) = cos(x) */ + if (x > FP_PI) { + x = FP_TWO_PI - x; + } + + /* [pi/2, pi] -> [0, pi/2]: cos(pi - x) = -cos(x) */ + if (x > FP_HALF_PI) { + x = FP_PI - x; + negate = 1; + } + + /* --- CORDIC in [0, pi/2] --- */ + fp_t result = fp_cos_cordic(x); + + return negate ? -result : result; +} + +#endif /* FP_H */ \ No newline at end of file diff --git a/libs/bio-lib/src/c/include/softdiv.h b/libs/bio-lib/src/c/include/softdiv.h new file mode 100644 index 000000000..bedf4b8f2 --- /dev/null +++ b/libs/bio-lib/src/c/include/softdiv.h @@ -0,0 +1,73 @@ +// soft_div.c +// Software implementations of integer division/remainder routines +// for freestanding targets lacking hardware divide. +// +// Naming follows the GCC/LLVM compiler-rt ABI so the compiler's +// implicit calls resolve automatically. +// +// Note that there is no link-time optimization in this flow, so +// this file should only be included if a divider is required. + +#include + +// ---------------------------------------------------------------- +// Unsigned 32-bit divide/remainder (the foundation for everything) +// ---------------------------------------------------------------- + +// Returns quotient; optionally writes remainder to *rem_out. +static uint32_t udiv32_rem(uint32_t n, uint32_t d, uint32_t *rem_out) { + // Division by zero: match hardware "undefined" behaviour by returning + // all-ones quotient and the original numerator as remainder, + // consistent with what real RISC-V hardware would do. + if (d == 0) { + if (rem_out) *rem_out = n; + return 0xFFFFFFFFu; + } + + uint32_t q = 0; + uint32_t r = 0; + + for (int i = 31; i >= 0; i--) { + r = (r << 1) | ((n >> i) & 1u); + if (r >= d) { + r -= d; + q |= (1u << i); + } + } + + if (rem_out) *rem_out = r; + return q; +} + +// ---------------------------------------------------------------- +// Compiler-rt ABI entry points +// ---------------------------------------------------------------- + +uint32_t __udivsi3(uint32_t n, uint32_t d) { + return udiv32_rem(n, d, 0); +} + +uint32_t __umodsi3(uint32_t n, uint32_t d) { + uint32_t r; + udiv32_rem(n, d, &r); + return r; +} + +int32_t __divsi3(int32_t n, int32_t d) { + // Compute sign, then delegate to unsigned core. + int negative = (n < 0) ^ (d < 0); + uint32_t un = (n < 0) ? (uint32_t)(-(uint32_t)n) : (uint32_t)n; // careful: avoids UB on INT_MIN + uint32_t ud = (d < 0) ? (uint32_t)(-(uint32_t)d) : (uint32_t)d; + uint32_t uq = udiv32_rem(un, ud, 0); + return negative ? -(int32_t)uq : (int32_t)uq; +} + +int32_t __modsi3(int32_t n, int32_t d) { + // Remainder has the sign of the dividend (C11 §6.5.5). + int negative = (n < 0); + uint32_t un = (n < 0) ? (uint32_t)(-(uint32_t)n) : (uint32_t)n; + uint32_t ud = (d < 0) ? (uint32_t)(-(uint32_t)d) : (uint32_t)d; + uint32_t r; + udiv32_rem(un, ud, &r); + return negative ? -(int32_t)r : (int32_t)r; +} \ No newline at end of file diff --git a/libs/bio-lib/src/c/include/ws2812.h b/libs/bio-lib/src/c/include/ws2812.h new file mode 100644 index 000000000..81d57b1ee --- /dev/null +++ b/libs/bio-lib/src/c/include/ws2812.h @@ -0,0 +1,56 @@ +#include +#include + +// pin is provided as the GPIO number to drive +// strip is an array of u32's that contain GRB data +// len is the length of the strip +void ws2812c(uint32_t pin, uint32_t *strip, uint32_t len) { + uint32_t led; + // sanity check the pin value + if (pin > 31) { + return; + } + uint32_t mask = 1 << pin; + uint32_t antimask = ~mask; + set_gpio_mask(mask); + set_output_pins(mask); + + // ensure timing with a nil quantum here + clear_gpio_pins_n(antimask); + wait_quantum(); + // main loop + for (uint32_t i = 0; i < len; i++) { + led = strip[i]; + for (uint32_t bit = 0; bit < 24; bit++) { + if ((led & 0x800000) == 0) { + // 2 hi + set_gpio_pins(mask); + wait_quantum(); + wait_quantum(); + // 5 lo + clear_gpio_pins_n(antimask); + wait_quantum(); + wait_quantum(); + wait_quantum(); + wait_quantum(); + wait_quantum(); + } else { + // 5 hi + set_gpio_pins(mask); + wait_quantum(); + wait_quantum(); + wait_quantum(); + wait_quantum(); + wait_quantum(); + // 5 lo + clear_gpio_pins_n(antimask); + wait_quantum(); + wait_quantum(); + wait_quantum(); + wait_quantum(); + wait_quantum(); + } + led <<= 1; + } + } +} \ No newline at end of file