Loop unrolling and speed-up techniques

Today I started some investigation on DTMF detection. After some research I decided to try the Goertzel algorithm, wrote some code from scratch and tried it on a real equipment based on an ARM7 core. It did fine actually, but after some benchmarking I thought it was somewhat slow.

I wrote the code thinking on future optimization but keeping simplicity in mind, specially because I didn't know how it was going to behave. However, as said above, calculations took much longer than what I expected, even though they were quite fast, exceeding the requirements for real-time processing (telephone channel, 8khz, mono). I still wanted to keep the code in C for portability so I tried some tricks.

I wrote the code with fixed-point math in mind, since floating point arithmetic would kill performance.

First, here are some basic definitions:

/* Struct for a single Goertzel calculator */
typedef struct
{
 dsp_t sp1,sp2; //past values (IIR)
 dsp_t coeff;   //calculated only once

} GOERTZ_s;

/* Calculate Goertzel 'step' */
#define GOERTZ_calc( _s, _val ) \
 { \
     dsp_t _gs = (_val) + FP_MULT2( _s.coeff, _s.sp1 ) - _s.sp2; \
     _s.sp2 = _s.sp1; \
     _s.sp1 = _gs; \
 }

The first approach was the simplest one, I needed sixteen Goertzel calculations (8 frequencies + 8 second harmonic for the first frequencies). I had a buffer where the incoming PCM data was copied to so I had to process that array for each of the 16 Goertzel 'calculators'.

static inline void GOERTZ_processAll( dsp_t sample )
{
 int i;
 for (i=0; i < 16; i++)
     GOERTZ_calc( goertzs[i], sample );
}

/* this is inside another function */
int i;
for (i=0; i < BUFFER_SIZE; i++)
 GOERTZ_processAll( bufferData[i] );

Where BUFFER_SIZE is the size of bufferData[] and goertzs[] is a 16-element array of GOERTZ_s structs, already initialized. Notice the inline modifier in GOERTZ_processAll(). It's really important since function inlining will help in execution time, specially when the function is called so frequently.

That code worked alright, but was too slow, at least for my intuition. The progress into a more efficient scheme had many steps, but basically there were three important changes:

• dsp_t was defined as int16_t, but the processor's 'native' word length is 32-bit, so changing dsp_t to be int32_t speeded up the calculations by removing cast and special assignment instructions.
• Loop unrolling was done in GOERTZ_processAll(). That means more flash is used but it's a good price to pay for a good speed up. Besides that, here we are talking about repeating the same thing 16 times so it's not a big problem if we have, to say, a 512k flash microcontroller or dsp.
• Instead of passing a single value to GOERTZ_processAll() a 8-byte array is used. This improves time considerably but also increases code size. As said in the previous point that wasn't an issue this case. Note that a 16-byte array won't necessarily increase performance. It depends on the core architecture, and in this case it made things worse (but better than single value parameter passing).

With these modifications I got a 100% performance increase, which means that there can be twice as much DTMF decoders compared to the non-optimized version.

Here is the code with the modifications:

static inline void GOERTZ_processAll( dsp_t samples[8] )
{
 #define DO(j) \
     GOERTZ_calc( goertzs[j], samples[0] ); \
     GOERTZ_calc( goertzs[j], samples[1] ); \
     GOERTZ_calc( goertzs[j], samples[2] ); \
     GOERTZ_calc( goertzs[j], samples[3] ); \
     GOERTZ_calc( goertzs[j], samples[4] ); \
     GOERTZ_calc( goertzs[j], samples[5] ); \
     GOERTZ_calc( goertzs[j], samples[6] ); \
     GOERTZ_calc( goertzs[j], samples[7] );

 DO(0);
 DO(1);
 DO(2);
 DO(3);
 DO(4);
 DO(5);
 DO(6);
 DO(7);
 DO(8);
 DO(9);
 DO(10);
 DO(11);
 DO(12);
 DO(13);
 DO(14);
 DO(15);

#undef DO
}

/* somewhere in another function */
int i;
for (i=0; i < BUFFER_SIZE; i +=8 )
 GOERTZ_processAll( bufferData + i );

But it can do better...

After observing how good performance went after this modifications I tried to do more loop unrolling to see if I could improve it. If the data buffer is large enough then the for loop that process 8 samples at a time wastes useful CPU instructions, so I included it inside a function called GOERTZ_processBuffer() which takes a buffer pointer and its length. There was another 100% performance increase from the previous optimization. This means a 4x speed up from the original code! Note that, once again, more flash is being used for loop unrolling.

static inline void GOERTZ_processBuffer( dsp_t *samples, int num )
{
 #define DO(j) \
   for ( i=0; i < num; i+=32 ) { \
       GOERTZ_calc( gss[j], samples[0+i] ); \
       GOERTZ_calc( gss[j], samples[1+i] ); \
       GOERTZ_calc( gss[j], samples[2+i] ); \
       GOERTZ_calc( gss[j], samples[3+i] ); \
       ... up to 32 .. ; }


 DO(0);
 DO(1);
 DO(2);
 DO(3);
 DO(4);
 DO(5);
 DO(6);
 DO(7);
 DO(8);
 DO(9);
 DO(10);
 DO(11);
 DO(12);
 DO(13);
 DO(14);
 DO(15);

#undef DO
}

Dirty little bugs - a pin/macro approach

Program testing can be used to show the presence of bugs, but never to show their absence!

Edsger W. Dijkstra

/-

Measuring and observing usually (if not always) change what one is measuring. I've never seen Schrödinger's cat, at least not when leading with embedded systems nor in my garden, but the former implication causes many problems when trying to debug time-critical functions/operations in embedded software and hardware.

Usually a printf-like UART output works but that would slow down or even hang code which can't wait for the UART to stream its data out. Even though with a substantial buffer it will come a time where it will get stuck.

There is a nice, well known techinque, that uses output pins to reflect some state, such as which function or interrupt handler the microcontroller is doing at the time. Since setting the output value requires only a few instructions, this approach suits better the time-critical scenarios. This technique is helpful in many ways: one may measure from interrupt latency and cpu usage to interrupt or thread deadlock.

That's how the following code came up to mind, here is a simple header file which is based on the macros I posted some time ago here.

#ifndef _dbgpin_h_
#define _dbgpin_h_

#include "portutil.h"

/* Enables or disables debugging */
#if    DBGPIN_ENABLED 1

/* Debug pins */
#define DBGPIN_01    0,2
#define DBGPIN_02    0,3
//---- etc

/* Initialization */
#define DBGPIN_INIT( pin )    do { FPIN_AS_OUTPUT(pin); FPIN_CLR(pin);  } while(0);

/**
* Beware of returns, exclude them!
*/
#if    DBGPIN_ENABLED
#define DBGPIN_BLOCK( pin, block ) \
           FPIN_SET_( pin ); \
           do { \
               block \
           } while(0); \
           FPIN_CLR_( pin );
#else
#define    DBGPIN_BLOCK( pin, block ) block
#endif

#endif /* _dbgpin_h_ */

Here is an example:

DBGPIN_BLOCK( DBGPIN_01,
a = b;
callSomething();
);

There is a small drawback: the do-while structure won't let any variable declared inside 'block' to be visible outside. That can be solved by removing the do-while, if you need it that way just go ahead and delete those two lines.

Also notice that a return in 'block' won't allow the macro to clear the corresponding pin, so applying the macro to a function with many return points is messy. The alternative is to change the original function's name and call it through a sort of wrapper which does it inside a DBGPIN_BLOCK macro. It's not pure beauty but it works nice, and once you've done the debugging it can be disabled by changing DBGPIN_ENABLED to 0.

Embedded C++ constructs - Part 2

As mentioned in part 1, I'll now get into an UART example, providing the basic functionality through a class. This example works with the LPC ARM7 family from NXP Semiconductors (Philips).

Register abstraction

First here is how I abstract a hardware register. It's nothing special but a simple template class:

namespace HW
{
  /**
   * Individual HW register
   */
  template < unsigned long int BASE, unsigned long int OFF >
  class Reg
  {
  public:
      void operator = (const unsigned long int & val) {
          *((volatile unsigned long int *)(BASE + 4*OFF)) = val;
      }

      unsigned long int & operator = ( const Reg & reg ) {
          return *((volatile unsigned long int *)(BASE + 4*OFF));
      }

      operator unsigned long int () {
          return *((volatile unsigned long int *)(BASE + 4*OFF));
      }
  };

};

(I used C-like casts here but it would be better to use reinterpret_cast<> instead)

Three operators are implemented. It's essential to be able to cast the class to an unsigned int so we can use it transparently.

The template parameters let us provide a base and offset address which will be useful when a peripheral is abstracted through a template class.

UART peripheral abstraction

The UART peripheral in the LPC2xxx family consists of a collection of registers, starting from a base address. There are some interesting bits lying around on the microcontroller too that control peripheral power, the peripheral's clock and associated interrupts. Here all of them are encapsulated in a single class, except for the interrupts (to do).

namespace HW
{
  /**
   * UART Module Abstraction
   */
  template<unsigned int BASE, unsigned int PCON_BIT, unsigned int PCLK_SEL>
  class RegUART
  {
  public:
      void powerOn() {
          PCONP |= (1<<PCON_BIT);
      }

      void setClkDiv( unsigned int divv ) {
          PCLOCK_SELECT( PCLK_SEL, divv );
      }

      HW::Reg<BASE, 0> RBR;
      HW::Reg<BASE, 0> THR;
      HW::Reg<BASE, 0> DLL;
      HW::Reg<BASE, 1>   DLM;
      HW::Reg<BASE, 1>   IER;
      HW::Reg<BASE, 2>   IIR;
      HW::Reg<BASE, 2>   FCR;
      HW::Reg<BASE, 3>   LCR;
      HW::Reg<BASE, 5>   LSR;
      HW::Reg<BASE, 7>   SCR;
      HW::Reg<BASE, 8>   ACR;
      HW::Reg<BASE, 9>  ICR;
      HW::Reg<BASE, 10>  FDR;
      HW::Reg<BASE, 12>  TER;
  };
};

#define HWUART0 HW::RegUART< UART0_BASE_ADDR, PCLK_UART0, PCLK_UART0 >
#define HWUART1 HW::RegUART< UART1_BASE_ADDR, PCLK_UART1, PCLK_UART1 >
#define HWUART2 HW::RegUART< UART2_BASE_ADDR, PCLK_UART2, PCLK_UART2 >
#define HWUART3 HW::RegUART< UART3_BASE_ADDR, PCLK_UART3, PCLK_UART3 >

PCONP and PCLOCK_SELECT() are macros defined in another file, nothing special.

HWUARTx are macros that help when specifying certain port. It will be used in the next piece of code.

#include <string.h>

namespace Drivers
{
   /**
   * LPC2xxx UART Peripheral
   * Polled.
   *
   * @param T Hw::RegUART
   *   Use HWUART0, HWUART1...
   */
   template< class T >
   class PolledUart
   {
   private:
   T UART;

   public:

       /**
        * Init UART
        *
        * @param brate  desired baudrate
        */
       void init(unsigned int brate) {

       UART.setClkDiv( PCLK_DIV_1 );
       UART.powerOn();

       UART.LCR = 0x80;  //DLAB = 1

       UART.DLL = (Fpclk/16)/brate & 0xFF;
       UART.DLM = (((Fpclk/16)/brate) >> 8) & 0xFF;

       UART.LCR = 3 | //8 bit char length
        (0<<2) | // 1 stop bit
        (0<<3) | //no parity
        (0<<4) | //partity type
        (0<<6) | //disable break transmission
        (0<<0) ; //enable access to divisor latches

       UART.FCR = (0x07); //FIFO ENABLE, Rx & Tx
   }

       /**
        * Transmit a single byte
        *
        * @param c
        */
       void tx( unsigned char c ) {
           while ( !( UART.LSR &  (1<<5) ) )
               ;
           UART.THR = c;
       }

       /**
        * Transmit several bytes
        *
        * @param data   data to transmit
        * @param length  length of 'data'
        */
       void tx( const unsigned char *data, unsigned int length ) {
           while( length-- > 0 )
               tx( *(data++) );
       }

       /**
        * Transmit a C string
        *
        * @param str
        */
       void tx( const char *str ) {
           tx( (const unsigned char *) str, strlen(str) );
       }

       /**
        * Byte receive
           * BLOCKING
        *
        * @return unsigned char  received char
        */
       unsigned char rx(void)
       {
           while ( !( UART.LSR & (1<<0) ) )
               ;

           return UART.RBR;
       }

       /**
        * Returns if there is data to be read in the FIFO
        *
        * @return bool  true if there is data available
        */
       bool isDataAvailable(void)
       {
           if ( UART.LSR & (1<<0) )
               return true;
           else
               return false;
       }
   };
};

PolledUart implements UART basic functionality. No constructor is used to avoid undesired code creation, so init() must be called before using any other function.

Even though this class contains a RegUART class as a private member it won't take any extra memory since the compiler (at least gcc-elf-arm here) will optimize the template. I did some tests and there is no difference in code size with a simple C function doing the same statements directly.

Finally, to see how it works, if we wanted to use UART0 through this class we could instantiate it like this:

Drivers::PolledUart< HWUART0 > Uart0;

void testUart0()
{
   Uart0.init(115200);  //init @ 115200 bps

   Uart0.tx("Hello there!\n");
}

If the class is to be used by many C++ files one should consider declaring it extern inside a header file and implementing it in a single cpp one. That would save code by avoiding function inlining each time it's used.

Improvements

This is just a basic version to demonstrate how easy and clean code can get. Here are some modifications that will make the class more useful:

• If there is an RTOS it would be useful to protect this class from concurrent access by using semaphores. It's not a difficult task, a mutex should be declared and initialized (maybe inside a constructor).

• An interrupt-based uart is nice too, even better if RTOS' messages queues are used. This shouldn't be a problem either.

Embedded C++ constructs - Part 1

There has been a big explosion about using C++ within embedded systems. Recently David sent me some interesting papers and info about Embedded C++ so here I present what I've been doing so far (or at least a small portion of it).

Templates

First I have to say that templates are not a code bloat if they're managed with care. In fact the C++ compiler is supposed to optimize them at compile time.

Here are some base templates I've coded to ease pin mapping on the LPC23xx ARM family.

#include "lpc24xx.h"        /* LPC23/24xx register definitions */
#include "static_assert.h"  /* static assert for non-C++0x compilers */

namespace HW
{
  /**
  * Output Pin Abstraction
  */
  template<unsigned int port, unsigned int pin>
  class OutPin
  {
      STATIC_ASSERT(pin <= 31, PinMustBeLessThan32);

  public:
      OutPin() {
          *(&FIO0DIR + 8*port) |= (1<<pin); //configure pin as output
      }
      void Set(void) { *(&FIO0SET + 8*port) = (1<<pin);}
      void Clr(void) { *(&FIO0CLR + 8*port) = (1<<pin);}
  };

  /**
  * Input Pin Abstraction
  */
  template<unsigned int port, unsigned int pin>
  class InPin
  {
      STATIC_ASSERT(pin <= 31, PinMustBeLessThan32);
 
   public:
      InPin() {
          *(&FIO0DIR + 8*port) &= ~(1<<pin); //configure pin as input
      }

      operator bool () {
          if ( (*(&FIO0PIN + 8*port)) & (1<<pin) )
              return true;
          else
              return false;
      }
  };
};

This may look a like waste of code at first glance. However these classes avoid many mistakes and provide a hardware independent interface to pin outputs and inputs, it's just a question of redefining this templates to fit the platform.

Also note that the compiler will throw an error if pin's value is not within the allowed range. This is also a protection and won't waste any processor instructions or any other memory. It would be useful to limit the port range too. The above templates use the static assert method described earlier in this post.

The constructors will manage to configure the port pin as output or input. If the pin is defined globally then the constructor will be called before entering the main function, configuring the pin as it should. If it is defined inside a function or by using the new operator (which I would try to avoid)  then the constructor will be called when it is instantiated.

Here is an example:

// Pin 0.25
HW::OutPin< 0,25 > myLed;
// Pin 1.20
HW::InPin< 1,20 >  mySwitch;

void invert(void)
{
   if ( mySwitch )
       myLed.Clr();
   else
       myLed.Set();
}

Beautiful, isn't it? The generated code (arm-elf-gcc 4.2.x) is exactly the same as if it is done manually by writing/reading the corresponding registers. There is no loss in performance compared to the equivalent C code.

In the next post I will discuss a UART implementation by using similar code constructions.

What I do not like about C++

C++ lacks many useful C99 characteristics and g++ doesn't implement them either. I could live without most of them but what really hurts me is to avoid using Designated Initializers, specially on structs. It won't be a big problem if struct's data is on RAM but it's disappointing when structs are on ROM (const).

If we want a struct to be in ROM while using C++ it has to be declared const, but in addition constructors can't be used or the const-declared struct will be placed in RAM (cRazY). It is an understandable limitation but that means that the only way to initialize a struct is by passing each element one by one and in the exact order as they're defined in the struct. That is error prone, particularly when a new element is added in the middle of the struct. So when we try to switch to C++ to avoid errors we become prone to issues that were solved with C99.

So, that's the only thing I don't like about C++, the solution? Use C for cont struct initializers and C++ for the rest? Don't know.

Frame pointers and Function Call Tracing

Function call tracing is really helpful when tracking down a problem, specially on an embedded system. On the ARM architecture data abort events and such provide useful data regarding where the problem was found like the instruction where that happened and the values each register had on that moment. However that usually isn't enough, specially when functions like memcpy() that are usually called from many places in our program. Even worse: imagine running a RTOS. If we knew where memcpy() was called it would be different and probably a lot easier to debug and trace down. Yesterday I was talking with David and he mentioned the function __builtin_return_address that comes with GCC. He uses it together with C++ (i386) to detect memory leaks, however it's not available for the Arm architecture.

GCC and frame pointers

GCC implements a nice concept called Frame Pointer. One of the CPU's registers is reserved and used as a frame pointer. Each time a function starts its execution the frame pointer is set to point exactly after the return address the caller has placed on the stack. Apart from being useful to the compiler to refer to function arguments in an easier way it can also be used to deduce who called us, and who called the one who called us, and so. A nice explanation can be found here.

However GCC turns on the -fomit-frame-pointer flag for some optimization levels so if we need the frame pointer we need to force it by adding -fno-omit-frame-pointer.
GCC provides a function called __builtin_return_address() with which you can trace up the function calls, but it's not available for ARM. However I found a nice piece of code in the linux kernel for the ARM architecture here. I just stripped this part (remember it's published under the GPL license by Russell King):

 .align 0
 .type arm_return_addr %function
 .global arm_return_addr

arm_return_addr:
 mov ip, r0
 mov r0, fp
3:
 cmp r0, #0
 beq 1f                    //@ frame list hit end, bail
 cmp ip, #0
 beq 2f                    //@ reached desired frame
 ldr r0, [r0, #-12]        // else continue, get next fp
 sub ip, ip, #1
 b 3b
2:
 ldr r0, [r0, #-4]         //@ get target return address
1:
 mov pc, lr                //get back to callee

The 'magic' values are -4 and -12 which indicate the relative position of the previous function's link register (return address) and frame pointer. This values come from analysing the push instruction which is called in every function entry that pushes, apart from other registers, {pc, lr, ip, fp} in that precise order in the stack.
The C prototype for arm_return_addr would be:

void * arm_return_addr( unsigned int num );

Where num is the number of frames to search back. Take care and remember that if you reach the last frame it's value will be 0 and you should stop there.
What I did is to show the call trace once I get data abort or prefetch abort exception so I know what caused it and it's easier to track back.

Give me something I can understand: decrypting addresses

With the function discussed above we can get the return addresses but where to go from there? You can generate a listing with arm-elf-objdump and find the address, but there is a nice tool called arm-elf-addr2line which will do it for you, thanks to David for pointing this out. Just do something like:

    arm-elf-addr2line --exe=yourelf.elf

And it will output something like:

    main.c:90

Which means you can find it in main.c, line 90. Quite nice, isn't it?

Disadvantages

There is a stack penalty when using frame pointers. In one product I'm developing I saw between 20 to 30 words stack penalty when using frame pointers compared to the same program compiled without frame pointers.  That means about 20*4 = 80 bytes of stack. Since that was a RTOSsed product and has more than 10 tasks running simultaneously that number multiplies and yields an important increase in total stack usage. That can be a problem if RAM is not enough, not mentioning that a tight-tuned program will probably crash for the first time it's compiled to work with frame pointers because of stack overflows.

To see what causes this behaviour let's look at a simple C function:

int sumNumbers( int a, int b ) {
return a + b;
}

When compiled with -fomit-frame-pointer we get:

00007f6c <sumNumbers>:
   7f6c: e0810000  add r0, r1, r0
   7f70: e12fff1e  bx lr

But if we force frame pointers with -fno-omit-frame-pointer we obtain:

0000818c <sumNumbers>:
   818c: e1a0c00d  mov ip, sp
   8190: e92dd800  push {fp, ip, lr, pc}
   8194: e0810000  add r0, r1, r0
   8198: e24cb004  sub fp, ip, #4 ; 0x4
   819c: e89da800  ldm sp, {fp, sp, pc}

By using frame pointers GCC is obliged to push the registers fp, ip, lr and pc and set up the frame pointer, that means bigger code and higher stack usage, at least for small or medium sized functions. Now you may notice why the GCC documentations says "-O also turns on -fomit-frame-pointer on machines where doing so does not interfere with debugging."
Using frame pointers or not depends on whether you give priority to debugging or small code footprint (and smaller ram/stack footprint too).

It's important to remember that if the program is compiled without frame pointers then the arm_return_addr function must not be called since the frame pointer register will contain other information, likely not related to anthing to do with frame pointers.

Alternative methods

There is another method we can use with GCC. It involves using the -finstrument-functions compiler flag. That will force GCC to call user-defined functions when entering and exiting functions, so an array can be kept on RAM with the call tree. However that could slow down the whole program excessively. On the other hand care must be taken with multithreaded designs. For more information here is the GCC documentation.