DSPRelated.com
Code
   Code Snippets    Submitted by jerhee

Fixed-point Math Library

Jordan ● November 30, 2010 ● Coded in C 
/* **********************************************************************
 *
 * Fixed Point Math Library
 *
 * **********************************************************************
 *
 * qmath.h
 *
 * Alex Forencich
 * alex@alexelectronics.com
 *
 * Jordan Rhee
 * rhee.jordan@gmail.com
 *
 * IEEE UCSD
 * http://ieee.ucsd.edu
 *
 * **********************************************************************/

#ifndef _QMATH_H_
#define _QMATH_H_

#ifdef __cplusplus
extern "C" {
#endif

#ifndef INLINE
#ifdef _MSC_VER
#define INLINE __inline
#else
#define INLINE inline
#endif /* _MSC_VER */
#endif /* INLINE */

/*
 * Default fractional bits. This precision is used in the routines
 * and macros without a leading underscore. 
 * For example, if you are mostly working with values that come from
 * a 10-bit A/D converter, you may want to choose 21. This leaves 11
 * bits for the whole part, which will help avoid overflow in addition.
 * On ARM, bit shifts require a single cycle, so all fracbits 
 * require the same amount of time to compute and there is no advantage
 * to selecting fracbits that are a multiple of 8.
 */
#define	FIXED_FRACBITS 24

#define FIXED_RESOLUTION (1 << FIXED_FRACBITS)
#define FIXED_INT_MASK (0xffffffffL << FIXED_FRACBITS)
#define FIXED_FRAC_MASK (~FIXED_INT_MASK)

// square roots
#define FIXED_SQRT_ERR 1 << (FIXED_FRACBITS - 10)

// fixedp2a
#define FIXED_DECIMALDIGITS 6

typedef long fixedp;

// conversions for arbitrary fracbits
#define _short2q(x, fb)			((fixedp)((x) << (fb)))
#define _int2q(x, fb)			((fixedp)((x) << (fb)))
#define _long2q(x, fb)			((fixedp)((x) << (fb)))
#define _float2q(x, fb)			((fixedp)((x) * (1 << (fb))))
#define _double2q(x, fb)		((fixedp)((x) * (1 << (fb))))

// conversions for default fracbits
#define short2q(x)			_short2q(x, FIXED_FRACBITS)
#define int2q(x)			_int2q(x, FIXED_FRACBITS)
#define long2q(x)			_long2q(x, FIXED_FRACBITS)
#define float2q(x)			_float2q(x, FIXED_FRACBITS)
#define double2q(x)			_double2q(x, FIXED_FRACBITS)

// conversions for arbitrary fracbits
#define _q2short(x, fb)		((short)((x) >> (fb)))
#define _q2int(x, fb)		((int)((x) >> (fb)))
#define _q2long(x, fb)		((long)((x) >> (fb)))
#define _q2float(x, fb)		((float)(x) / (1 << (fb)))
#define _q2double(x, fb)	((double)(x) / (1 << (fb)))

// conversions for default fracbits
#define q2short(x)			_q2short(x, FIXED_FRACBITS)
#define q2int(x)			_q2int(x, FIXED_FRACBITS)
#define q2long(x)			_q2long(x, FIXED_FRACBITS)
#define q2float(x)			_q2float(x, FIXED_FRACBITS)
#define q2double(x)			_q2double(x, FIXED_FRACBITS)

// evaluates to the whole (integer) part of x
#define qipart(x)			q2long(x)

// evaluates to the fractional part of x
#define qfpart(x)			((x) & FIXED_FRAC_MASK)

/*
 * Constants
 */
#define _QPI      3.1415926535897932384626433832795
#define QPI      double2q(_QPI)
#define _Q2PI     6.283185307179586476925286766559
#define Q2PI     double2q(_Q2PI)
#define _QPIO2    1.5707963267948966192313216916398
#define QPIO2    double2q(_QPIO2)
#define _QPIO4    0.78539816339744830961566084581988
#define QPIO4    double2q(_QPIO4)
#define _QLN_E    2.71828182845904523536
#define QLN_E    double2q(_QLN_E)
#define _QLN_10   2.30258509299404568402
#define QLN_10   double2q(_QLN_10)
#define _Q1OLN_10 0.43429448190325182765
#define Q1OLN_10 double2q(_Q1OLN_10)

// Both operands in addition and subtraction must have the same fracbits.
// If you need to add or subtract fixed point numbers with different
// fracbits, then use q2q to convert each operand beforehand.
#define qadd(a, b) ((a) + (b))
#define qsub(a, b) ((a) - (b))

/**
 * q2q - convert one fixed point type to another
 * x - the fixed point number to convert
 * xFb - source fracbits
 * yFb - destination fracbits
 */
static INLINE fixedp q2q(fixedp x, int xFb, int yFb)
{
	if(xFb == yFb) {
		return x;
	} else if(xFb < yFb) {
		return x << (yFb - xFb);
	} else {
		return x >> (xFb - yFb);
	}
}

/**
 * Multiply two fixed point numbers with arbitrary fracbits
 * x - left operand
 * y - right operand
 * xFb - number of fracbits for X
 * yFb - number of fracbits for Y
 * resFb - number of fracbits for the result
 * 
 */
#define _qmul(x, y, xFb, yFb, resFb) ((fixedp)(((long long)(x) * (long long)(y)) >> ((xFb) + (yFb) - (resFb))))

/**
 * Fixed point multiply for default fracbits.
 */
#define qmul(x, y) _qmul(x, y, FIXED_FRACBITS, FIXED_FRACBITS, FIXED_FRACBITS)

/**
 * divide
 * shift into 64 bits and divide, then truncate
 */
#define _qdiv(x, y, xFb, yFb, resFb) ((fixedp)((((long long)x) << ((xFb) + (yFb) - (resFb))) / y))

/**
 * Fixed point divide for default fracbbits.
 */
#define qdiv(x, y) _qdiv(x, y, FIXED_FRACBITS, FIXED_FRACBITS, FIXED_FRACBITS)

/**
 * Invert a number (x^-1) for arbitrary fracbits
 */
#define _qinv(x, xFb, resFb) ((fixedp)((((long long)1) << (xFb + resFb)) / x))

/**
 * Invert a number with default fracbits.
 */
#define qinv(x) _qinv(x, FIXED_FRACBITS, FIXED_FRACBITS);

/**
 * Modulus. Modulus is only defined for two numbers of the same fracbits
 */
#define qmod(x, y) ((x) % (y))

/**
 * Absolute value. Works for any fracbits.
 */
#define qabs(x) (((x) < 0) ? (-x) : (x))

/**
 * Floor for arbitrary fracbits
 */
#define _qfloor(x, fb) ((x) & (0xffffffff << (fb)))

/**
 * Floor for default fracbits
 */
#define qfloor(x) _qfloor(x, FIXED_FRACBITS)

/**
 * ceil for arbitrary fracbits.
 */
static INLINE fixedp _qceil(fixedp x, int fb)
{
	// masks off fraction bits and adds one if there were some fractional bits
	fixedp f = _qfloor(x, fb);
	if (f != x) return qadd(f, _int2q(1, fb));
	return x;
}

/**
 * ceil for default fracbits
 */
#define qceil(x) _qceil(x, FIXED_FRACBITS)

/**
 * square root for default fracbits
 */
fixedp qsqrt(fixedp p_Square);

/**
 * log (base e) for default fracbits
 */
fixedp qlog( fixedp p_Base );

/**
 * log base 10 for default fracbits
 */
fixedp qlog10( fixedp p_Base );

/**
 * exp (e to the x) for default fracbits
 */
fixedp qexp(fixedp p_Base);

/**
 * pow for default fracbits
 */
fixedp qpow( fixedp p_Base, fixedp p_Power );

/**
 * sine for default fracbits
 */
fixedp qsin(fixedp theta);

/**
 * cosine for default fracbits
 */
fixedp qcos(fixedp theta);

/**
 * tangent for default fracbits
 */
fixedp qtan(fixedp theta);

/**
 * fixedp2a - converts a fixed point number with default fracbits to a string
 */
char *q2a(char *buf, fixedp n);

#ifdef __cplusplus
}	// extern C
#endif

#endif /* _QMATH_H_ */

/* **********************************************************************
 *
 * Fixed Point Math Library
 *
 * **********************************************************************
 *
 * qmath.c
 *
 * Alex Forencich
 * alex@alexelectronics.com
 *
 * Jordan Rhee
 * rhee.jordan@gmail.com
 *
 * IEEE UCSD
 * http://ieee.ucsd.edu
 *
 * **********************************************************************/

#include "qmath.h"

/**
 * square root
 */
fixedp qsqrt(fixedp p_Square)
{
	fixedp   res;
	fixedp   delta;
	fixedp   maxError;

	if ( p_Square <= 0 )
		return 0;

	/* start at half */
	res = (p_Square >> 1);

	/* determine allowable error */
	maxError =  qmul(p_Square, FIXED_SQRT_ERR);

	do
	{
		delta =  (qmul( res, res ) - p_Square);
		res -=  qdiv(delta, ( res << 1 ));
	}
	while ( delta > maxError || delta < -maxError );

	return res;
}

/**
 * log (base e)
 */
fixedp qlog( fixedp p_Base )
{
    fixedp w = 0;
	fixedp y = 0;
	fixedp z = 0;
	fixedp num = int2q(1);
	fixedp dec = 0;

	if ( p_Base == int2q(1) )
		return 0;

	if ( p_Base == 0 )
		return 0xffffffff;

	for ( dec=0 ; qabs( p_Base ) >= int2q(2) ; dec += int2q(1) )
		p_Base = qdiv(p_Base, QLN_E);

	p_Base -= int2q(1);
	z = p_Base;
	y = p_Base;
	w = int2q(1);

	while ( y != y + w )
	{
		z = 0 - qmul( z , p_Base );
		num += int2q(1);
		w = qdiv( z , num );
		y += w;
	}

	return y + dec;
}

/**
 * log base 10
 */
fixedp qlog10( fixedp p_Base )
{
    // ln(p_Base)/ln(10)
    // more accurately, ln(p_Base) * (1/ln(10))
    return qmul(qlog(p_Base), Q1OLN_10);
}

/**
 * exp (e to the x)
 */
fixedp qexp(fixedp p_Base)
{
	fixedp w;
	fixedp y;
	fixedp num;

	for ( w=int2q(1), y=int2q(1), num=int2q(1) ; y != y+w ; num += int2q(1) )
	{
		w = qmul(w, qdiv(p_Base, num));
		y += w;
	}

	return y;
}

/**
 * pow
 */
fixedp qpow( fixedp p_Base, fixedp p_Power )
{
	if ( p_Base < 0 && qmod(p_Power, int2q(2)) != 0 )
		return - qexp( qmul(p_Power, qlog( -p_Base )) );
	else
		return qexp( qmul(p_Power, qlog(qabs( p_Base ))) );
}

/**
 * sinx, internal sine function
 */
static fixedp sinx(fixedp x)
{
	fixedp xpwr;
	long xftl;
	fixedp xresult;
	short positive;
	int i;

	xresult = 0;
	xpwr = x;
	xftl = 1;
	positive = -1;

	// Note: 12! largest for long
	for (i = 1; i < 7; i+=2)
	{
		if ( positive )
			xresult += qdiv(xpwr, long2q(xftl));
		else
			xresult -= qdiv(xpwr, long2q(xftl));

		xpwr = qmul(x, xpwr);
		xpwr = qmul(x, xpwr);
		xftl *= i+1;
		xftl *= i+2;
		positive = !positive;
	}

	return xresult;
}

/**
 * sine
 */
fixedp qsin(fixedp theta)
{
	fixedp xresult;
	short bBottom = 0;
	//static fixed xPI = XPI;
	//static fixed x2PI = X2PI;
	//static fixed xPIO2 = XPIO2;

	fixedp x = qmod(theta, Q2PI);
	if ( x < 0 )
		x += Q2PI;

	if ( x > QPI )
	{
		bBottom = -1;
		x -= QPI;
	}

	if ( x <= QPIO2 )
		xresult = sinx(x);
	else
		xresult = sinx(QPIO2-(x-QPIO2));

	if ( bBottom )
		return -xresult;

	return xresult;
}

/**
 * cosine
 */
fixedp qcos(fixedp theta)
{
	return qsin(theta + QPIO2);
}

/**
 * tangent
 */
fixedp qtan(fixedp theta)
{
	return qdiv(qsin(theta), qcos(theta));
}

/**
 * q2a - converts a fixed point number to a string
 */
char *q2a(char *buf, fixedp n)
{
	long ipart = qipart(n);
	long fpart = qfpart(n);
	long intdigits = 0;

	int i = 0;
	int j = 0;
	int d = 0;

	int offset = 0;

	long v;
	long t;
	long st = 0;

	if (n != 0)
	{
		intdigits = qipart(qceil(qlog10(qabs(n))));
	}

	if (intdigits < 1) intdigits = 1;

	offset = intdigits - 1;

	if (n < 0)
	{
		buf[0] = '-';
		offset++;
		n = -n;
		ipart = -ipart;
		fpart = -fpart;
	}

	// integer portion

	for (i = 0; i < intdigits; i++)
	{
		j = offset - i;
		d = ipart % 10;
		ipart = ipart / 10;
		buf[j] = '0' + d;
	}

	// decimal point

	buf[offset + 1] = '.';

	// fractional portion

	v = 5;
	t = FIXED_RESOLUTION >> 1;

	for (i = 0; i < FIXED_DECIMALDIGITS - 1; i++)
	{
		v = (v << 1) + (v << 3);
	}

	for (i = 0; i < FIXED_FRACBITS; i++)
	{
		if (fpart & t)
		{
			st += v;
		}
		v = v >> 1;
		t = t >> 1;
	}

	offset += FIXED_DECIMALDIGITS + 1;

	for (i = 0; i < FIXED_DECIMALDIGITS; i++)
	{
		j = offset - i;
		d = st % 10;
		st = st / 10;
		buf[j] = '0' + d;
	}

	// ending zero
	buf[offset + 1] = '\0';

	return buf;
}

Fixed-point IIR Filter Code Generator for ARM

Jordan ● November 30, 2010 ● Coded in ASM for the ARM
/* 
 * fixedp lowpass_iir(fixedp *w, fixedp x);
 *
 * Fixed point IIR filtering routine for ARM. Computes output y for
 * input x. The output will have the same fracbits as the input.
 *  w: caller-allocated array for state storage. Should be length 2*L.
 *  x: sample to filter
 * 
 * Required data:
 *   LENGTH: number of sections
 *   .sos: sos matrix
 *   SOS_FRACBITS: sos fracbits
 *   .gain: scale values array
 *   G_FRACBITS: scale values fracbits
 *   
 * Register usage:
 *   r0: address of internal state array (w)
 *   r1: x
 *   r2: address of SOS array
 *   r3: address of gain array
 *   r4: w0
 *   r5: w1
 *   r6: y
 *   r7: long multiply lo word
 *   r8: long multiply hi word
 *   r9:  B1
 *   r10: B2
 *   r11: A1
 *   r12: A2
 *   r14: loop counter
 */

.set LENGTH,  3
.set SOS_FRACBITS,  30
.set G_FRACBITS,  31

.section .rodata
.align 4

.sos:
    .word 0x49be7eaf, 0x40000000, 0xc4c9d93a, 0x251f228c
    .word 0x1d81c8a5, 0x40000000, 0xdaa0b600, 0x37cef3c1
    .word 0x40000000, 0x00000000, 0xd87b730c, 0x00000000

.gain:

    .word 0x06b1dbb5, 0x42fe27a0, 0x613d5d5a

.text
.arm

.global	lowpass_iir
.func   lowpass_iir
lowpass_iir:
	push {r4-r11, lr}

	mov  r14, #0								/* i = 0 */
	ldr  r2, =.sos								/* load address of SOS matrix */
	ldr  r3, =.gain								/* load address of gain coefficient array */

	cmp  r14, #LENGTH
	bge  .endloop

.loop:
	/* load all the SOS coefficients we need into r8-r12 and increment the SOS pointer */
	ldmia r2!, {r9-r12}							/* B1, B2, A1, A2 */

	/* x = gain[i]*x */
	ldr    r6, [r3], #4							/* load current element of gain array into r6 and increment by 4 */
	smull  r7, r8, r1, r6						/* 64-bit multiply: r5:r4 = x*gain[i]; */
	mov    r1, r7, lsr#G_FRACBITS				/* shift lo word to the right by G_FRACBITS */
	orr    r1, r1, r8, lsl#(32 - G_FRACBITS)		/* shift hi word to the right by G_FRACBITS  and OR with lo word*/

	/* load w0 and w1 into r4, r5, but do NOT increment */
	ldm    r0, {r4-r5}

	/* y(r6) = x(r1) + w[W0](r4)*/
	add r6, r1, r4

	/* w0(r4) = .sos[B1](r9)*x(r1) - .sos[A1](r11)*y(r6) + w[W1](r5); */
	rsb    r11, r11, #0							/* .sos[A1] = -sos[A1] */
	smull  r7, r8, r9, r1						/* r8:r7 = sos[B1]*x */
	smlal  r7, r8, r11, r6						/* r8:r7 += -sos[A1]*y */
	mov    r4, r7, lsr#SOS_FRACBITS				/* shift lo word to the right by SOS_FRACBITS */
	orr    r4, r4, r8, lsl#(32 - SOS_FRACBITS)	/* shift hi word to the right by SOS_FRACBITS and OR with lo word*/
	add    r4, r4, r5							/* add w1 */

	/* w2 = sos[B2]*x(r1) - .sos[A2](r12)*y(r6); */
	rsb    r12, r12, #0							/* .sos[A2] = -sos[A2] */
	smull  r7, r8, r10, r1                      /* r8:r7 = sos[B2](r10)*x(r1) */
	smlal  r7, r8, r12, r6						/* r8:r7 += -sos[A2](r12)*y(r6) */
	mov    r5, r7, lsr#SOS_FRACBITS				/* shift lo word to the right by SOS_FRACBITS */
	orr    r5, r5, r8, lsl#(32 - SOS_FRACBITS)	/* shift hi word to the right by SOS_FRACBITS and OR with lo word*/

	/* need to store w0, w1 back to memory and increment */
	stmia r0!, {r4-r5}

	/* x = y */
	mov    r1, r6

	/* increment pointer and branch to top of loop */
	add r14, r14, #1
	cmp r14, #LENGTH
	blt   .loop

.endloop:
	/* set return val, restore stack, and return */
	mov r0, r6
	pop {r4-r11, lr}
	bx  lr

.endfunc
.end

Fixed-point FIR Filter Code Generator for ARM Assembly

Jordan ● November 30, 2010 ● Coded in ASM for the ARM
/* **********************************************************************
 *
 * Fixed Point Filtering Library
 *
 * **********************************************************************
 *
 * lowpass_fir.S
 *
 * Jordan Rhee
 * rhee.jordan@gmail.com
 *
 * IEEE UCSD
 * http://ieee.ucsd.edu
 *
 * Generated with IEEE UCSD Fixed Pointer Filter Code Generator
 * http://ieee.ucsd.edu/projects/qfilt.php
 *
 * **********************************************************************/

/* 
 * fixedp lowpass_fir(fixedp *w, fixedp x);
 * 
 * Fixed point FIR filtering routine for ARM. Computes output y for
 * input x. The output will have the same fracbits as the input.
 *  w: caller-allocated array for state storage. Should be length LENGTH+1.
 *  x: sample to filter
 *
 * Required data:
 *   LENGTH: number of coefficients
 *   .h: coefficient array
 *   H_FRACBITS: fracbits of coefficients
 *   
 *   r0: address of internal state array. w[LENGTH] contains
 *       index of head of circular buffer.
 *   r1: x
 *   r2: address of coefficient array (h)
 *   r3: j: index of current state value
 *   r4: i: index of current coefficient
 *   r5: h[i]: current filter coefficient
 *   r6: w[j]: current state value
 *   r7: long multiply lo word
 *   r8: long multiply hi word
 */

.set LENGTH,  20
.set H_FRACBITS,  30

.section .rodata
.align 4

.h:

    .word 0xffc5ef57, 0xfeb3416c, 0xfdf673b8, 0xffc7fb45
    .word 0x02b1826b, 0x0123c987, 0xfb542f40, 0xfc248828
    .word 0x0ab1bf40, 0x1b3f7457, 0x1b3f7457, 0x0ab1bf40
    .word 0xfc248828, 0xfb542f40, 0x0123c987, 0x02b1826b
    .word 0xffc7fb45, 0xfdf673b8, 0xfeb3416c, 0xffc5ef57

.text
.arm

.global	lowpass_fir
.func   lowpass_fir
lowpass_fir:
	push {r4-r8}
	
	/* w(r0)[j(w[N])] = x */
	ldr  r3, [r0, #(4*LENGTH)]		/* load value of j */
	str  r1, [r0, r3, lsl #2]			/* store x into w[j] */

	/* y = 0; */
	mov r7, #0
	mov r8, #0

	/* load base address of coefficient array */
	ldr r2, =.h

	/* i = 0 */
	mov r4, #0
	cmp r4, #LENGTH
	bge .endloop

.loop:
	/* y += h[i] * w[j] */
	ldr    r5, [r2, r4, lsl #2]					/* r5 = h[i] */
	ldr    r6, [r0, r3, lsl #2]					/* r6 = w[j] */
	smlal  r7, r8, r5, r6						/* r8:r7 += h[i] * w[j] */
	
	subs r3, r3, #1								/* j-- */
	movmi r3, #(LENGTH - 1)						/* if j == -1, then j = N-1 */

	add   r4, r4, #1							/* i++ */
	cmp r4, #LENGTH								/* is i less than N */
	blt .loop

.endloop:
	add r3, r3, #1								/* increment j and store back to memory */
	cmp r3, #LENGTH
	moveq r3, #0
	str r3, [r0, #(4*LENGTH)]					/* save new value of j */
	
	mov    r0, r7, lsr #H_FRACBITS				/* shift lo word to the right by H_FRACBITS */
	orr    r0, r0, r8, lsl #(32 - H_FRACBITS)	/* shift hi word to the right by H_FRACBITS and OR with lo word*/

	pop {r4-r8}
	bx  lr

.endfunc
.end