mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-15 08:30:02 +00:00
add various C libraries for BIO use.
Note that there is no LTO/trimming in the BIO flow, so wantonly including libraries you don't use just grows your code size for no use.
This commit is contained in:
parent
19d8294369
commit
c80877d064
5 changed files with 492 additions and 7 deletions
|
|
@ -1,5 +1,21 @@
|
|||
#ifndef BIO_H
|
||||
#define BIO_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
// ----------------------------------------------------------------
|
||||
// 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
|
||||
);
|
||||
}
|
||||
// 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
|
||||
276
libs/bio-lib/src/c/include/fp_q16.h
Normal file
276
libs/bio-lib/src/c/include/fp_q16.h
Normal file
|
|
@ -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 <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
/* --- 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 */
|
||||
73
libs/bio-lib/src/c/include/softdiv.h
Normal file
73
libs/bio-lib/src/c/include/softdiv.h
Normal file
|
|
@ -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 <stdint.h>
|
||||
|
||||
// ----------------------------------------------------------------
|
||||
// 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;
|
||||
}
|
||||
56
libs/bio-lib/src/c/include/ws2812.h
Normal file
56
libs/bio-lib/src/c/include/ws2812.h
Normal file
|
|
@ -0,0 +1,56 @@
|
|||
#include <stdint.h>
|
||||
#include <bio.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue