128 lines
3.2 KiB
C
128 lines
3.2 KiB
C
#pragma once
|
|
|
|
//#include <math/fix8.h>
|
|
//#include <math/fix16.h>
|
|
|
|
#define FPDECL(width,fract) \
|
|
typedef struct { int ## width ## _t value; } fp ## width ## fract ## _t; \
|
|
static inline fp ## width ## fract ## _t FP ## width ## fract (float v) { return (fp ## width ## fract ## _t){ (int ## width ## _t)(v * 65536) }; }; \
|
|
//
|
|
|
|
|
|
FPDECL(8,3);
|
|
FPDECL(16,3);
|
|
FPDECL(32,3);
|
|
FPDECL(64,3);
|
|
|
|
FPDECL(8,7);
|
|
FPDECL(16,7);
|
|
FPDECL(32,7);
|
|
FPDECL(64,7);
|
|
|
|
FPDECL(16,11);
|
|
FPDECL(32,11);
|
|
FPDECL(64,11);
|
|
|
|
FPDECL(16,15);
|
|
FPDECL(32,15);
|
|
FPDECL(64,15);
|
|
|
|
#define FPDOT3(dst,n) { dst.value = (n << 3); }
|
|
#define FPDOT7(dst,n) { dst.value = (n << 7); }
|
|
#define FPDOT11(dst,n) { dst.value = (n << 11); }
|
|
#define FPDOT15(dst,n) { dst.value = (n << 15); }
|
|
|
|
/**
|
|
* Dynamic Fixpoint Arithemtics
|
|
*/
|
|
|
|
typedef struct dfp16 {
|
|
int16_t value;
|
|
uint8_t fragsize;
|
|
} dfp16_t;
|
|
|
|
typedef struct dfp32 {
|
|
int32_t value;
|
|
uint8_t fragsize;
|
|
} dfp32_t;
|
|
|
|
static inline dfp16_t dfp16from32(dfp32_t v){ return (dfp16_t){ fragsize: v.fragsize, value: v.value }; };
|
|
static inline dfp32_t dfp32from16(dfp16_t v){ return (dfp32_t){ fragsize: v.fragsize, value: v.value }; };
|
|
|
|
static inline dfp16_t dfp16_from_float(uint8_t prec,float v){ return (dfp16_t){ fragsize: prec, value: (int16_t)(v * (1<<prec)) }; };
|
|
static inline dfp32_t dfp32_from_float(uint8_t prec,float v){ return (dfp32_t){ fragsize: prec, value: (int32_t)(v * (1<<prec)) }; };
|
|
|
|
static inline float float_from_dfp16(dfp16_t v){ return ((float)v.value) / (1 << v.fragsize); };
|
|
static inline float float_from_dfp32(dfp32_t v){ return ((float)v.value) / (1 << v.fragsize); };
|
|
|
|
#define SHL(a,n) (n < 0 ? a >> -n : a << n)
|
|
#define SHR(a,n) (n < 0 ? a << -n : a >> n)
|
|
|
|
static inline
|
|
dfp16_t dfp16_add(uint8_t prec,dfp16_t a,dfp16_t b){
|
|
dfp16_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
|
|
v.value += SHL(b.value,(prec-b.fragsize));
|
|
return v;
|
|
};
|
|
|
|
static inline
|
|
dfp16_t dfp16_sub(uint8_t prec,dfp16_t a,dfp16_t b){
|
|
dfp16_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
|
|
v.value -= SHL(b.value,(prec-b.fragsize));
|
|
return v;
|
|
};
|
|
|
|
static inline
|
|
dfp16_t dfp16_mul(int8_t prec,dfp16_t a,dfp16_t b){
|
|
return (dfp16_t){
|
|
fragsize: prec,
|
|
value: SHL((int32_t)a.value * (int32_t)b.value, (prec - (a.fragsize + b.fragsize)))
|
|
};
|
|
};
|
|
|
|
static inline
|
|
dfp16_t dfp16_div(int8_t prec,dfp16_t a,dfp16_t b){
|
|
return (dfp16_t){
|
|
fragsize: prec,
|
|
value:
|
|
SHL(
|
|
((int32_t)a.value / (int32_t)b.value),
|
|
(prec + b.fragsize - a.fragsize)
|
|
)
|
|
};
|
|
};
|
|
|
|
static inline
|
|
dfp32_t dfp32_add(uint8_t prec,dfp32_t a,dfp32_t b){
|
|
dfp32_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
|
|
v.value += SHL(b.value,(prec-b.fragsize));
|
|
return v;
|
|
};
|
|
|
|
static inline
|
|
dfp32_t dfp32_sub(uint8_t prec,dfp32_t a,dfp32_t b){
|
|
dfp32_t v = { fragsize: prec, value: SHL(a.value,(prec-a.fragsize)) };
|
|
v.value -= SHL(b.value,(prec-b.fragsize));
|
|
return v;
|
|
};
|
|
|
|
static inline
|
|
dfp32_t dfp32_mul(uint8_t prec,dfp32_t a,dfp32_t b){
|
|
return (dfp32_t){
|
|
fragsize: prec,
|
|
value: SHL((int64_t)a.value * (int64_t)b.value, (prec - (a.fragsize + b.fragsize)))
|
|
};
|
|
};
|
|
|
|
static inline
|
|
dfp32_t dfp32_div(uint8_t prec,dfp32_t a,dfp32_t b){
|
|
return (dfp32_t){
|
|
fragsize: prec,
|
|
value:
|
|
SHL(
|
|
((int64_t)a.value),
|
|
(prec + b.fragsize - a.fragsize)
|
|
) / b.value
|
|
};
|
|
};
|