291 lines
9.9 KiB
Rust
291 lines
9.9 KiB
Rust
/// A compiled CORDIC instruction operating on indexed slots.
|
|
///
|
|
/// Slot i holds the result of instruction i. All operand references
|
|
/// point to earlier slots (j < i), matching the TrigGraph's topological order.
|
|
///
|
|
/// CORDIC mapping:
|
|
/// Sin, Cos → rotation mode (angle → sin/cos via shift-and-add)
|
|
/// Hypot → vectoring mode (magnitude via shift-and-add)
|
|
/// Atan2 → vectoring mode (angle via shift-and-add)
|
|
/// Mul → fixed-point multiply (shift-and-add)
|
|
/// Add, Sub, Neg, Abs, Min, Max, Clamp → direct binary ops
|
|
#[derive(Debug, Clone)]
|
|
pub enum CORDICInstr {
|
|
InputX,
|
|
InputY,
|
|
InputZ,
|
|
LoadImm(i64),
|
|
|
|
Add(u32, u32),
|
|
Sub(u32, u32),
|
|
Mul(u32, u32),
|
|
Div(u32, u32),
|
|
Neg(u32),
|
|
Abs(u32),
|
|
|
|
Sin(u32),
|
|
Cos(u32),
|
|
Tan(u32),
|
|
|
|
Asin(u32),
|
|
Acos(u32),
|
|
Atan(u32),
|
|
|
|
Sinh(u32),
|
|
Cosh(u32),
|
|
Tanh(u32),
|
|
|
|
Asinh(u32),
|
|
Acosh(u32),
|
|
Atanh(u32),
|
|
|
|
Sqrt(u32),
|
|
Exp(u32),
|
|
Ln(u32),
|
|
|
|
Hypot(u32, u32),
|
|
Atan2(u32, u32),
|
|
|
|
Min(u32, u32),
|
|
Max(u32, u32),
|
|
Clamp { val: u32, lo: u32, hi: u32 },
|
|
}
|
|
|
|
/// Precomputed arctan table for CORDIC iterations.
|
|
/// atan(2^-i) in fixed-point with the given number of fractional bits.
|
|
pub fn atan_table(word_bits: u8) -> Vec<i64> {
|
|
let frac_bits = word_bits - 1;
|
|
(0..word_bits)
|
|
.map(|i| {
|
|
let angle = (2.0f64).powi(-(i as i32)).atan();
|
|
(angle * (1i64 << frac_bits) as f64).round() as i64
|
|
})
|
|
.collect()
|
|
}
|
|
|
|
/// CORDIC gain constant K_n = prod(1/sqrt(1 + 2^{-2i})) for n iterations.
|
|
pub fn cordic_gain(iterations: u8, frac_bits: u8) -> i64 {
|
|
let mut k = 1.0f64;
|
|
for i in 0..iterations {
|
|
k *= 1.0 / (1.0 + (2.0f64).powi(-2 * i as i32)).sqrt();
|
|
}
|
|
(k * (1i64 << frac_bits) as f64).round() as i64
|
|
}
|
|
|
|
// Binary encoding
|
|
|
|
const OP_INPUT_X: u8 = 0x00;
|
|
const OP_INPUT_Y: u8 = 0x01;
|
|
const OP_INPUT_Z: u8 = 0x02;
|
|
const OP_LOAD_IMM: u8 = 0x03;
|
|
const OP_ADD: u8 = 0x04;
|
|
const OP_SUB: u8 = 0x05;
|
|
const OP_MUL: u8 = 0x06;
|
|
const OP_DIV: u8 = 0x10;
|
|
const OP_NEG: u8 = 0x07;
|
|
const OP_ABS: u8 = 0x08;
|
|
const OP_SIN: u8 = 0x09;
|
|
const OP_COS: u8 = 0x0A;
|
|
const OP_HYPOT: u8 = 0x0B;
|
|
const OP_ATAN2: u8 = 0x0C;
|
|
const OP_MIN: u8 = 0x0D;
|
|
const OP_MAX: u8 = 0x0E;
|
|
const OP_CLAMP: u8 = 0x0F;
|
|
const OP_TAN: u8 = 0x11;
|
|
const OP_ASIN: u8 = 0x12;
|
|
const OP_ACOS: u8 = 0x13;
|
|
const OP_ATAN: u8 = 0x14;
|
|
const OP_SINH: u8 = 0x15;
|
|
const OP_COSH: u8 = 0x16;
|
|
const OP_TANH: u8 = 0x17;
|
|
const OP_ASINH: u8 = 0x18;
|
|
const OP_ACOSH: u8 = 0x19;
|
|
const OP_ATANH: u8 = 0x1A;
|
|
const OP_SQRT: u8 = 0x1B;
|
|
const OP_EXP: u8 = 0x1C;
|
|
const OP_LN: u8 = 0x1D;
|
|
|
|
pub fn encode_instruction(buf: &mut Vec<u8>, instr: &CORDICInstr) {
|
|
match instr {
|
|
CORDICInstr::InputX => buf.push(OP_INPUT_X),
|
|
CORDICInstr::InputY => buf.push(OP_INPUT_Y),
|
|
CORDICInstr::InputZ => buf.push(OP_INPUT_Z),
|
|
CORDICInstr::LoadImm(v) => {
|
|
buf.push(OP_LOAD_IMM);
|
|
buf.extend_from_slice(&v.to_le_bytes());
|
|
}
|
|
CORDICInstr::Add(a, b) => {
|
|
buf.push(OP_ADD);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Sub(a, b) => {
|
|
buf.push(OP_SUB);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Mul(a, b) => {
|
|
buf.push(OP_MUL);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Div(a, b) => {
|
|
buf.push(OP_DIV);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Neg(a) => {
|
|
buf.push(OP_NEG);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
}
|
|
CORDICInstr::Abs(a) => {
|
|
buf.push(OP_ABS);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
}
|
|
CORDICInstr::Sin(a) => {
|
|
buf.push(OP_SIN);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
}
|
|
CORDICInstr::Cos(a) => {
|
|
buf.push(OP_COS);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
}
|
|
CORDICInstr::Tan(a) => { buf.push(OP_TAN); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Asin(a) => { buf.push(OP_ASIN); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Acos(a) => { buf.push(OP_ACOS); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Atan(a) => { buf.push(OP_ATAN); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Sinh(a) => { buf.push(OP_SINH); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Cosh(a) => { buf.push(OP_COSH); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Tanh(a) => { buf.push(OP_TANH); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Asinh(a) => { buf.push(OP_ASINH); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Acosh(a) => { buf.push(OP_ACOSH); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Atanh(a) => { buf.push(OP_ATANH); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Sqrt(a) => { buf.push(OP_SQRT); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Exp(a) => { buf.push(OP_EXP); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Ln(a) => { buf.push(OP_LN); buf.extend_from_slice(&a.to_le_bytes()); }
|
|
CORDICInstr::Hypot(a, b) => {
|
|
buf.push(OP_HYPOT);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Atan2(a, b) => {
|
|
buf.push(OP_ATAN2);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Min(a, b) => {
|
|
buf.push(OP_MIN);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Max(a, b) => {
|
|
buf.push(OP_MAX);
|
|
buf.extend_from_slice(&a.to_le_bytes());
|
|
buf.extend_from_slice(&b.to_le_bytes());
|
|
}
|
|
CORDICInstr::Clamp { val, lo, hi } => {
|
|
buf.push(OP_CLAMP);
|
|
buf.extend_from_slice(&val.to_le_bytes());
|
|
buf.extend_from_slice(&lo.to_le_bytes());
|
|
buf.extend_from_slice(&hi.to_le_bytes());
|
|
}
|
|
}
|
|
}
|
|
|
|
fn read_u32(data: &[u8], pos: usize) -> Option<u32> {
|
|
Some(u32::from_le_bytes(data.get(pos..pos + 4)?.try_into().ok()?))
|
|
}
|
|
|
|
fn read_i64(data: &[u8], pos: usize) -> Option<i64> {
|
|
Some(i64::from_le_bytes(data.get(pos..pos + 8)?.try_into().ok()?))
|
|
}
|
|
|
|
pub fn decode_instruction(data: &[u8]) -> Option<(CORDICInstr, usize)> {
|
|
let op = *data.first()?;
|
|
match op {
|
|
OP_INPUT_X => Some((CORDICInstr::InputX, 1)),
|
|
OP_INPUT_Y => Some((CORDICInstr::InputY, 1)),
|
|
OP_INPUT_Z => Some((CORDICInstr::InputZ, 1)),
|
|
OP_LOAD_IMM => {
|
|
let v = read_i64(data, 1)?;
|
|
Some((CORDICInstr::LoadImm(v), 9))
|
|
}
|
|
OP_ADD => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Add(a, b), 9))
|
|
}
|
|
OP_SUB => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Sub(a, b), 9))
|
|
}
|
|
OP_MUL => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Mul(a, b), 9))
|
|
}
|
|
OP_DIV => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Div(a, b), 9))
|
|
}
|
|
OP_NEG => {
|
|
let a = read_u32(data, 1)?;
|
|
Some((CORDICInstr::Neg(a), 5))
|
|
}
|
|
OP_ABS => {
|
|
let a = read_u32(data, 1)?;
|
|
Some((CORDICInstr::Abs(a), 5))
|
|
}
|
|
OP_SIN => {
|
|
let a = read_u32(data, 1)?;
|
|
Some((CORDICInstr::Sin(a), 5))
|
|
}
|
|
OP_COS => {
|
|
let a = read_u32(data, 1)?;
|
|
Some((CORDICInstr::Cos(a), 5))
|
|
}
|
|
OP_TAN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Tan(a), 5)) }
|
|
OP_ASIN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Asin(a), 5)) }
|
|
OP_ACOS => { let a = read_u32(data, 1)?; Some((CORDICInstr::Acos(a), 5)) }
|
|
OP_ATAN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Atan(a), 5)) }
|
|
OP_SINH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Sinh(a), 5)) }
|
|
OP_COSH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Cosh(a), 5)) }
|
|
OP_TANH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Tanh(a), 5)) }
|
|
OP_ASINH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Asinh(a), 5)) }
|
|
OP_ACOSH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Acosh(a), 5)) }
|
|
OP_ATANH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Atanh(a), 5)) }
|
|
OP_SQRT => { let a = read_u32(data, 1)?; Some((CORDICInstr::Sqrt(a), 5)) }
|
|
OP_EXP => { let a = read_u32(data, 1)?; Some((CORDICInstr::Exp(a), 5)) }
|
|
OP_LN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Ln(a), 5)) }
|
|
OP_HYPOT => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Hypot(a, b), 9))
|
|
}
|
|
OP_ATAN2 => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Atan2(a, b), 9))
|
|
}
|
|
OP_MIN => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Min(a, b), 9))
|
|
}
|
|
OP_MAX => {
|
|
let a = read_u32(data, 1)?;
|
|
let b = read_u32(data, 5)?;
|
|
Some((CORDICInstr::Max(a, b), 9))
|
|
}
|
|
OP_CLAMP => {
|
|
let val = read_u32(data, 1)?;
|
|
let lo = read_u32(data, 5)?;
|
|
let hi = read_u32(data, 9)?;
|
|
Some((CORDICInstr::Clamp { val, lo, hi }, 13))
|
|
}
|
|
_ => None,
|
|
}
|
|
}
|