swarfir.c

/* -----------------------------------------------------------------------------
 *  Copyright (C) 2021 daiteq s.r.o.                     http://www.daiteq.com
 *
 *  This program is distributed WITHOUT ANY WARRANTY; without even
 *  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 *  PURPOSE.
 *
 * -----------------------------------------------------------------------------
 *  Filename    : swarfir.c
 *  Authors     : Roman Bartosinski
 *  Description : FIR filter with SWAR operations
 *  Release     :
 *  Version     : 1.0
 *  Date        : 27.4.2021
 * -----------------------------------------------------------------------------
 */

#include <stdint.h>
#include <leon-myuart.h>
#include <my_printf.h>
#include <sysregs.h>
#include <lconf.h>
#include <stub.h>
#include <swarconf.h>


//#define PRINT_RESULTS   1
//#define PRINT_FAILED    1

#define TEST_OFFLINE 1
#define TEST_ONLINE  1

// example definitions - swar with 8bit elements
#define BITSZ             8

#ifndef CONFIG_DATA_LENGTH
  #define DATA_LENGTH       50
#else
  #define DATA_LENGTH       CONFIG_DATA_LENGTH
#endif /* CONFIG_DATA_LENGTH */

#ifndef CONFIG_FIR_LENGTH
  #define FIR_LENGTH        5    /* more accurately it is a number of FIR coefficients [c0,c1,c2,c3,c4,...]  */
#else
  #define FIR_LENGTH        CONFIG_FIR_LENGTH
#endif /* CONFIG_FIR_LENGTH */

//#ifdef CONFIG_ONLINE_MULTI
  //#define ONLINE_MULTIPLE   CONFIG_ONLINE_MULTI
//#else
  //#define ONLINE_MULTIPLE   1
//#endif /* CONFIG_ONLINE_MULTI */

#ifdef CONFIG_ONLINE_FIRLEN
  #define ONLINE_FIRLEN   CONFIG_ONLINE_FIRLEN
#else
  #define ONLINE_FIRLEN   (TEST_FIRCOEF_SIZE)
#endif /* CONFIG_ONLINE_MULTI */


#define MAXRAND           20

typedef int swpack __attribute__((subword(BITSZ)));
typedef int swelm __attribute__((subword(BITSZ,1)));

typedef char dataelm;
typedef int resultelm;

typedef union {
  swpack    s;
  int       i;
  unsigned  u;
} multi_t;

#define MAXNUM            ((1<<BITSZ))
#define MINVAL            0
#define MAXVAL            ((1<<BITSZ)-1)

#define PACKING           (32/BITSZ)
#define DATA_ARRAY_SIZE   ((DATA_LENGTH+PACKING-1)/PACKING)   /* array for packed data */
#define COEF_ARRAY_SIZE   ((FIR_LENGTH+PACKING-1)/PACKING)    /* array for packed coefficients */

#define MAXFIRLENGTH      128
/* -------------------------------------------------------------------------- */
void print_dir_data(const char *prefix, dataelm *pd, int len);
void print_swar_data(const char *prefix, swpack *ps, int len);
void print_counters(const char *prefix, unsigned *tm, unsigned *ins);

/* -------------------------------------------------------------------------- */
// experimental data
// signal (sinus 1kHz + sinus 15kHz, sampl.f=48kHz
const dataelm test_input[] = {
  0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,
  -55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,
  -92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,
  92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,
  -115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,
  80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,
  -121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,
  46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,
  -109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,
  80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,
  -80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,
  92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,
  -92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,
  9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,
  -9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,
  -33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,
  33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,
  -42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,
  121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,
  -46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,
  121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,
  -42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,
  33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,
  -33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,
  -9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,
  -18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,
  9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,
  55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,
  -38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,
  38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,
  -80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,
  42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,
  -109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,
  109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,
  -121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,
  115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,
  -115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,-55,0,55,-9,
  18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,0,-55,9,-18,
  -92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,-92,-18,9,
  -55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,18,-9,55,
  0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,-115,-33,-38,
  -92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,80,115,33,38,92,
  18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42,-80,
  -115,-33,-38,-92,-18,9,-55,0,55,-9,18,92,38,33,115,80,42,121,109,46,109,121,42,
  80,115,33,38,92,18,-9,55,0,-55,9,-18,-92,-38,-33,-115,-80,-42,-121,-109,-46,-109,-121,-42
};
#define TEST_INDATA_SIZE  (sizeof(test_input)/sizeof(dataelm))
// coefficients of FIR filter
const dataelm test_fircoef[] = {
  0,0,0,0,1,1,0,-2,-3,-3,0,6,14,20,23,20,14,6,0,-3,-3,-2,0,1,1
};
#define TEST_FIRCOEF_SIZE  (sizeof(test_fircoef)/sizeof(dataelm))
// extra longer vector for testing only
const dataelm test_fircoef_extra[] = {
  0,0,0,0,1,1,0,-2,-3,-3,0,6,14,20,23,20,14,6,0,-3,-3,-2,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};

// filtered data for checking for FIR with 25 coefficients
const resultelm test_output[] = {
  0,0,0,0,0,0,-55,-156,-156,-119,-38,293,754,1365,2142,3087,3994,4926,5848,6579,
  7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,
  0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,
  -8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,
  5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,
  3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,
  -8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,
  2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,
  5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,
  -7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,
  -2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,
  8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,
  -5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,
  -5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,
  8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,
  -3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,
  -7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,
  6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,
  1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,
  -8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,
  3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,
  5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,
  -8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,
  -1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,
  7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,
  -6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,
  -3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,
  8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,
  -5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,
  -5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,
  8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,
  -3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,
  -7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,
  6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,
  1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,
  -8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,
  3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,
  5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,
  -8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,
  -1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,
  7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,
  -6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,
  -4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,
  8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,
  -4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,
  -6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,
  7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,
  0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,
  -8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,
  5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,
  3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,
  -8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,
  2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,
  5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,
  -7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,
  -2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,
  8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,-3206,-4150,-5082,
  -5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,-7204,-6579,-5903,
  -5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,6579,7204,7706,8022,
  8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,1072,0,-1072,-2180,
  -3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,-8244,-8022,-7706,
  -7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,3206,4150,5082,5903,
  6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,5082,4150,3206,2180,
  1072,0,-1072,-2180,-3206,-4150,-5082,-5903,-6579,-7204,-7706,-8022,-8244,-8346,
  -8244,-8022,-7706,-7204,-6579,-5903,-5082,-4150,-3206,-2180,-1072,0,1072,2180,
  3206,4150,5082,5903,6579,7204,7706,8022,8244,8346,8244,8022,7706,7204,6579,5903,
  5082,4150,3206,2180,1072,0,-1072,-2180,-3206
};
#define TEST_OUTPUT_SIZE  (sizeof(test_output)/sizeof(resultelm))

resultelm test_dir_output[TEST_OUTPUT_SIZE];
resultelm test_swar_output[TEST_OUTPUT_SIZE];

/* -------------------------------------------------------------------------- */
// data for classical FIR computation
dataelm dir_in_x[DATA_LENGTH+FIR_LENGTH];  /* INPUT DATA + zeros */
dataelm dir_in_c[FIR_LENGTH];             /* COEFFICIENTS */
resultelm dir_out_y[DATA_LENGTH];            /* OUTPUT DATA */


// data for FIR computation with SWAR operations
swpack swar_in_x[(DATA_LENGTH+FIR_LENGTH+(PACKING-1))/PACKING]; /* space for data ramp up */
swpack swar_in_c[COEF_ARRAY_SIZE];

resultelm swar_out_y_red[DATA_LENGTH];
resultelm swar_out_y_accum[DATA_LENGTH];
resultelm swar_out_y_add[DATA_LENGTH];

/* array of extended vectors of coefficients */
#define EXTCOEF_LEN   (FIR_LENGTH+PACKING-1+(PACKING-1))/PACKING  /* array of packed coefficients */
//multi_t extcoefs[PACKING][EXTCOEF_LEN];
swpack swar_ecoefs[PACKING][EXTCOEF_LEN];


/* ========================================================================== */
// data preparation
void prepare_data(void)
{
  for (unsigned i=0;i<DATA_LENGTH;++i) {
    int d = rand2();
    d = ((d&0x1000)?-1:1) * (d % (MAXRAND));
    dir_in_x[i] = d;
    swar_in_x[i/PACKING][i%PACKING] = d;
  }

  for (unsigned i=DATA_LENGTH;i<(DATA_LENGTH+FIR_LENGTH);++i)
    dir_in_x[i] = 0;

  for (unsigned i=0;i<FIR_LENGTH;++i) {
    int c = rand2();
    c = ((c&0x1000)?-1:1) * (c % (MAXRAND));
    dir_in_c[i] = c;
    swar_in_c[i/PACKING][i%PACKING] = c;
  }

  // prepare coefficients with different aligning
  for (unsigned p=0; p<PACKING; ++p) {
    for (unsigned w=0; w<EXTCOEF_LEN; ++w) {
      swar_ecoefs[p][w] = 0;
    }
    for (unsigned c=0; c<FIR_LENGTH;++c) {
      unsigned iw = (c+p) / PACKING;
      unsigned ie = (c+p) % PACKING;
      swar_ecoefs[p][iw][ie] = dir_in_c[c];
    }
  }
  
#if 0
  // debug print array of coefficients
  for(unsigned p=0;p<PACKING;++p) {
    m_print(" #"); i_print(p); m_print(": ");
    print_swar_data("DIRecoef ", (swpack*)&swar_ecoefs[p][0], EXTCOEF_LEN*PACKING);
  }
#endif

}


/* ========================================================================== */
// classical computation element by element
// data must be correctly long (with zeroes after real data)
// datalen = number of input data (or length of required output vector)
// firlen = number of coefficients (b0,b1,b2,b3,...)
void fir_offline(resultelm *out_y, dataelm *in_x, int len_data, dataelm *in_c)
{
  for (unsigned j=0; j<len_data; ++j) {
    resultelm s = 0;
    for (unsigned i=0; i<FIR_LENGTH; ++i) {
      s += in_c[i] * in_x[i+j];
    }
    out_y[j] = s;
  }
}


dataelm dir_x_mem[MAXFIRLENGTH];  // [x(n),x(n-1),x(n-2),x(n-3),...]
dataelm dir_c_mem[MAXFIRLENGTH];  // [c0,c1,c2,c3,c4,...]
unsigned mem_firlen;
unsigned rbuf_next;

void fir_online_init(unsigned firlen, dataelm *in_c)
{
  if (firlen>MAXFIRLENGTH) {
    m_print("!!! Too long FIR filter\n");
    firlen = MAXFIRLENGTH;
  }
  for (unsigned n=0;n<firlen;++n) {
    dir_x_mem[n] = 0;
    dir_c_mem[n] = in_c[n]; //firlen-n-1];
  }
  mem_firlen = firlen;
}

resultelm fir_online(dataelm in_x)
{
  resultelm y = 0;
  for (unsigned i=mem_firlen-1;i>0;--i) {
    dir_x_mem[i] = dir_x_mem[i-1];  // x(n-1) = x'(n)
    y += dir_x_mem[i]*dir_c_mem[i];
  }
  dir_x_mem[0] = in_x;
  y+= dir_x_mem[0]*dir_c_mem[0];
  return y;
}

/* ---------------------------------- */

void fir_online_rbuf_init(unsigned firlen, dataelm *in_c)
{
  if (firlen>MAXFIRLENGTH) {
    m_print("!!! Too long FIR filter\n");
    firlen = MAXFIRLENGTH;
  }
  rbuf_next = firlen-1; // fill from back to front
  for (unsigned n=0;n<firlen;++n) {
    dir_x_mem[n] = 0;
    dir_c_mem[n] = in_c[n];
  }
  mem_firlen = firlen;
}

resultelm fir_online_rbuf(dataelm in_x)
{
  resultelm y = 0;
  unsigned xi = rbuf_next;
  dir_x_mem[rbuf_next] = in_x;
  
  for (unsigned i=0;i<mem_firlen;++i) {
    y += dir_c_mem[i] * dir_x_mem[xi];
    if (++xi>=mem_firlen) xi=0;
  }
  if (rbuf_next) rbuf_next--;
  else rbuf_next = mem_firlen-1;
  return y;
}

/* ---------------------------------- */
unsigned bufsize;
unsigned bufmask;

void fir_online_align_rbuf_init(unsigned firlen, dataelm *in_c)
{
  if (firlen>MAXFIRLENGTH) {
    m_print("!!! Too long FIR filter\n");
    firlen = MAXFIRLENGTH;
  }
  unsigned p = 32 - __builtin_clz(firlen);
  mem_firlen = firlen;
  bufsize = 1<<p;
  bufmask = bufsize-1;

  rbuf_next = bufsize-1; // fill from back to front
  for (unsigned n=0;n<bufsize;++n) {
    dir_x_mem[n] = 0;
  }
  for (unsigned n=0;n<firlen;++n) {
    dir_c_mem[n] = in_c[n];
  }
}

resultelm fir_online_align_rbuf(dataelm in_x)
{
  resultelm y = 0;
  unsigned xi = rbuf_next;
  dir_x_mem[rbuf_next] = in_x;
  
  for (unsigned i=0;i<mem_firlen;++i) {
    y += dir_c_mem[i] * dir_x_mem[xi];
    xi = (xi+1)&bufmask;
  }
  rbuf_next = (rbuf_next-1)&bufmask;
  return y;
}


/* ========================================================================== */
// swar computation
// use swar multiplication with reduction
void fir_swar_mul_red(resultelm *out_y, swpack *in_x, int len_data)
{
  #pragma swar reduce
  // loop over all len_data data
  for (unsigned j=0; j<len_data; ++j) {
    //multi_t iny;
    swpack swz[1]; /* for automatic generation of swarctrl */
    unsigned pi = j%PACKING;
    unsigned pw = j/PACKING;
    // reset accumulator
    int acc = 0;
    // loop 1:((len_fir/PACKING)+1)
    for (unsigned w=0;w<EXTCOEF_LEN;++w) {
      // swar mul xi,bi => accum  [bi=select coefs with correct alignment]
      swz[0] = in_x[pw+w] * swar_ecoefs[pi][w];
      acc += *((int *)&swz);
    }
    // save accum to yi
    out_y[j] = acc;
  }
}

/* -------------------------------------------------------------------------- */
// use swar multiplication with lane accumulators
void fir_swar_mul_accum(resultelm *out_y, swpack *in_x, int len_data)
{
  for (unsigned j=0; j<len_data; ++j) {
    swpack swz[1];
    unsigned pi = j%PACKING;
    unsigned pw = j/PACKING;
    // internal accumulators are reset automatically with settings swar control word
    for (unsigned w=0;w<EXTCOEF_LEN;++w) {
      swz[0] = in_x[pw+w] * swar_ecoefs[pi][w];
    }
    unsigned sum = 0;
    for (unsigned a=0;a<PACKING;++a) {
      sum += __builtin_swaraccum(a<<1);
    }
    out_y[j] = sum;
  }
}

/* -------------------------------------------------------------------------- */
// use multiplication without lane accumulators or reduction
swpack swar_tmp_mem[EXTCOEF_LEN]; /* space for internal subtotals */

/* this version needs elements with sufficient bitwidth - saturation is more possible */
void fir_swar_mul_add(resultelm *out_y, swpack *in_x, int len_data)
{
  #pragma swar saturate
  for (unsigned j=0; j<len_data; ++j) {
    unsigned pi = j%PACKING;
    unsigned pw = j/PACKING;
    // tmp[i] = x[n-i]*c[i]
    for (unsigned w=0;w<EXTCOEF_LEN;++w) {
      swar_tmp_mem[w] = in_x[pw+w] * swar_ecoefs[pi][w];
    }
    // sum in packed data
    swpack sum[1];
    sum[0] = swar_tmp_mem[0];
    for (unsigned w=1;w<EXTCOEF_LEN;++w) {
      sum[0] = sum[0] + swar_tmp_mem[w];
    }
    // sum of packed data
    resultelm out = 0;
    for (unsigned p=0;p<PACKING;++p)
      out += (*((unsigned *)&sum[0]) >> (p*BITSZ)) & MAXVAL;
    out_y[j] = out;
  }
}

/* -------------------------------------------------------------------------- */
swpack swar_x_mem[MAXFIRLENGTH/PACKING+1]; /* space for the latest data vector */
swpack swar_c_mem[MAXFIRLENGTH/PACKING+1]; /* space for the coefficients (for testing another alignment) */
unsigned swar_mem_coefwrds = 0;

void fir_swar_online_init(unsigned firlen, dataelm *in_c)
{
  if (firlen>MAXFIRLENGTH) {
    m_print("!!! Too long FIR filter.");
    firlen = MAXFIRLENGTH;
  }
  unsigned cw = ((firlen+PACKING-1)/PACKING);
  for (unsigned w=0;w<cw;++w) {
    swar_c_mem[w] = 0; // swar_in_c[w];
    swar_x_mem[w] = 0;
  }
  for (unsigned c=0;c<firlen;++c) {
    unsigned w = c / PACKING;
    unsigned e = c % PACKING;
    ((unsigned *)&swar_c_mem)[w] |= (in_c[c] & MAXVAL)<<(BITSZ*e);
  }

#if 0
  // debug print coefficients
  print_swar_data("coeffs ", swar_c_mem, firlen);
#endif

  swar_mem_coefwrds = cw;
}

resultelm fir_swar_online(dataelm in_x)
{
  resultelm y = 0;
  // update data vector
  unsigned rem = in_x & MAXVAL;
  for(unsigned w = 0;w<swar_mem_coefwrds;++w) {
    unsigned new = (*((unsigned *)&swar_x_mem[w]) << BITSZ) | rem;
    rem = *((unsigned *)&swar_x_mem[w]) >> (32-BITSZ);
    swar_x_mem[w] = new;
  }
  // compute result for the latest data vector
  #pragma swar manual
  //#pragma swar reduce
  __builtin_swarctrl(SW_VIDEO | SW_SGND | SW_RED | SW_OP_MUL);
  for(unsigned w=0;w<swar_mem_coefwrds;++w) {
    unsigned yc = __builtin_swar(swar_x_mem[w], swar_c_mem[w]);
    y += yc;
  }
  return y;
}

/* ---------------------------------- */
// 
swpack swar_c_arr[PACKING][MAXFIRLENGTH/PACKING+1]; /* space for aligned vectors of coefficients */
unsigned swar_ca_len;
unsigned swar_pidx, swar_widx;

void fir_swar_online_rbuf_init(unsigned firlen, dataelm *in_c)
{
  swar_ca_len = (firlen + PACKING-1 + PACKING-1) / PACKING; // pocet slov
  swar_pidx = 0;
  swar_widx = 0;
  // prepare swar_c_arr
  // reset swar_x_mem
  for (unsigned w=0; w<swar_ca_len; ++w) {
    swar_x_mem[w] = 0;
    for (unsigned p; p<PACKING; ++p)
      swar_c_arr[p][w] = 0;
  }
  for (unsigned p=0; p<PACKING; ++p) {
    for (unsigned c=0; c<firlen; ++c) {
      unsigned w = (c+PACKING-p-1) / PACKING;
      unsigned e = (c+PACKING-p-1) % PACKING;
      swar_c_arr[p][w][e] = in_c[c];
    }
  }
}

resultelm fir_swar_online_rbuf(dataelm in_x)
{
  #pragma swar manual
  resultelm y = 0;
  // update X rbuf with a new value
  if (swar_pidx==0) *((unsigned *)&swar_x_mem[swar_widx]) = 0;
  swar_x_mem[swar_widx][PACKING-swar_pidx-1] = in_x;
  unsigned wi = swar_widx;
  __builtin_swarctrl(SW_VIDEO | SW_SGND | SW_RED | SW_OP_MUL);
  for (unsigned w=0;w<swar_ca_len;++w) {
    y += __builtin_swar(swar_x_mem[wi],swar_c_arr[swar_pidx][w]);
    if (++wi>=swar_ca_len) wi=0;
  }

  if (++swar_pidx>=PACKING) {
    swar_pidx=0;
    if (swar_widx==0) 
      swar_widx=swar_ca_len-1;
    else
      swar_widx--;
  }
  return y;
}

/* ---------------------------------- */
// 
unsigned swar_ca_len;
unsigned swar_pidx, swar_widx;
unsigned swar_xbufsize;
unsigned swar_xbufmask;

void fir_swar_online_align_rbuf_init(unsigned firlen, dataelm *in_c)
{
  swar_ca_len = (firlen + PACKING-1 + PACKING-1) / PACKING; // minimal number of swar words
  unsigned mbspt = 32-__builtin_clz(swar_ca_len);
  swar_xbufsize = 1<<mbspt;
  swar_xbufmask = swar_xbufsize-1;
  swar_pidx = 0;
  swar_widx = 0;
  // prepare swar_c_arr
  // reset swar_x_mem
  for (unsigned w=0; w<swar_ca_len; ++w) {
    for (unsigned p; p<PACKING; ++p)
      swar_c_arr[p][w] = 0;
  }
  for (unsigned w=0;w<swar_xbufsize;++w)
    swar_x_mem[w] = 0;

  for (unsigned p=0; p<PACKING; ++p) {
    for (unsigned c=0; c<firlen; ++c) {
      unsigned w = (c+PACKING-p-1) / PACKING;
      unsigned e = (c+PACKING-p-1) % PACKING;
      swar_c_arr[p][w][e] = in_c[c];
    }
  }
}


resultelm fir_swar_online_align_rbuf(dataelm in_x)
{
  resultelm y = 0;
 
  // update X rbuf with a new value
  if (swar_pidx==0) *((unsigned *)&swar_x_mem[swar_widx]) = 0;
  swar_x_mem[swar_widx][PACKING-swar_pidx-1] = in_x;
  unsigned wi = swar_widx;

  #pragma swar manual

  __builtin_swarctrl(SW_VIDEO | SW_SGND | SW_RED | SW_OP_MUL);
  for (unsigned w=0;w<swar_ca_len;++w) {
    y += __builtin_swar(swar_x_mem[wi],swar_c_arr[swar_pidx][w]);
    wi = (wi + 1) & swar_xbufmask;
  }

  if (++swar_pidx>=PACKING) {
    swar_pidx=0;
    swar_widx = (swar_widx - 1) & swar_xbufmask;
  }
  return y;
}



/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* comparing */
int check_results(unsigned len, resultelm *in, resultelm *sw)
{
  int res = 0;
  m_print("Check results: ");
  for (unsigned p=0;p<len;++p) {
    if (in[p]!=sw[p]) {
#if PRINT_FAILED
      m_print("\n failed pack #"); i_print(p); m_print(" ("); 
      i_print(in[p]); m_print(" @"); i_hexprint(&in[p]); m_print(" , "); 
      i_print(sw[p]);  m_print(" @"); i_hexprint(&sw[p]); m_print(")");
#endif /* PRINT_FAILED */
      res++;
    }
  }
  if (!res) {
    m_print("  OK\n");
  } else { 
    m_print(" FAILED "); i_print(res); NL; 
  }
  NL;
  return res;
}


/* print data */
void print_dir_data(const char *prefix, dataelm *pd, int len)
{
  m_print(prefix); m_print(" data:\n");
  for(int i=0;i<len;++i) {
    m_print("  "); i_print(*pd++);
  }
  NL; NL;
}

void print_swar_data(const char *prefix, swpack *ps, int packlen)
{
  swpack *ptmp = ps;
  m_print(prefix); m_print(" swar data:\n");
  // print packed swar data
  m_print("  PACK:");
  for(unsigned i=0;i<((packlen+PACKING-1)/PACKING);++i) {
    m_print("  "); i_hexprint(*((unsigned int *)ps) );
    ps++;
  }
  NL; m_print("  ELM: ");
  // print swar element data
  ps = ptmp;
  for(unsigned i=0;i<packlen;++i) {
    multi_t v; v.s = *(ps+i/PACKING);
    int ii = i % PACKING;
    unsigned e = (v.u>>(BITSZ*ii))&((1<<BITSZ)-1);
    m_print("  "); i_hexprint(e);
  }
  NL;
}

/* print results */
void print_results(const char *prefix, resultelm *pd, int len)
{
  m_print(prefix); m_print(" data @"); i_hexprint(pd); m_print(":\n");
  for(int i=0;i<len;++i) {
    m_print("  "); i_print(*pd++);
  }
  NL; NL;
}

/* print counters */
void print_counters(const char *prefix, unsigned *tm, unsigned *ins)
{
  unsigned ticks_lo, ticks_hi, insns_lo, insns_hi;
  sysregs_stop();
  sysregs_read(&ticks_lo, &ticks_hi, &insns_lo, &insns_hi);
  m_print(prefix);
  if (ticks_hi) {
    m_print(" EXECUTED IN ("); i_print(ticks_hi); m_print(" * 2^32 + "); i_print(ticks_lo); m_print(") TICKS AND ("); i_print(insns_hi); m_print(" * 2^32 + "); i_print(insns_lo); m_print(" INSTRUCTIONS");NL;NL;
  } else {
    m_print(" EXECUTED IN "); i_print(ticks_lo); m_print(" TICKS AND "); i_print(insns_lo); m_print(" INSTRUCTIONS");NL;
  }
  if (tm) *tm = ticks_lo;
  if (ins) *ins = insns_lo;
}

/* for clearing BSS section */
extern unsigned long __bss_start;  /* start address for the .bss section. defined in linker script */
extern unsigned long _end;         /* end address for the .bss section. defined in linker script */

// main
int main(void)
{
  /* reset BSS section to zero */
  for(unsigned long *pMem = &__bss_start; pMem < &_end;) {
    *(pMem++) = 0;
  }

  init_uart();

  m_print("TEST: SWARFIR\n");
#if 1
  m_print(" - info:\n");
  m_print("  - data length  : "); i_print(DATA_LENGTH); NL;
  m_print("  - filter length: "); i_print(FIR_LENGTH-1); NL;
  m_print("  - memory for direct FIR [Bytes]:\n");
  m_print("   - input data  : "); i_print(sizeof(dir_in_x)); NL;
  m_print("   - coefficients: "); i_print(sizeof(dir_in_c)); NL;
  m_print("   - output data : "); i_print(sizeof(dir_out_y)); NL;
  m_print("  - memory for SWAR FIR [Bytes]:\n");
  m_print("   - input data  : "); i_print(sizeof(swar_in_x)); NL;
  m_print("   - coefficients: "); i_print(sizeof(swar_in_c)); NL;
  m_print("   - output data : "); i_print(sizeof(swar_out_y_add)); NL;
  m_print("  - position in memory:\n");
  m_print("   - dir_out_Y       : "); i_hexprint(&dir_out_y[0]); NL;
  m_print("   - swar_out_Y(add) : "); i_hexprint(&swar_out_y_add[0]); NL;
#endif

#if TEST_OFFLINE
  m_print(" - preparing data\n");
  sysregs_init();
  prepare_data();
  print_counters("  ", NULL, NULL);

#if 0
  print_dir_data("IN-X", dir_in_x, DATA_LENGTH);
  print_dir_data("IN-C", dir_in_c, FIR_LENGTH);
  print_swar_data("SWAR-IN-X", swar_in_x, DATA_LENGTH);
  print_swar_data("SWAR-IN-C", swar_in_c, FIR_LENGTH);
#endif

  m_print("\n - Compute offline\n");

#if 1
  m_print("  * FIR\n");
  sysregs_init();
  fir_offline(dir_out_y, dir_in_x, DATA_LENGTH, dir_in_c);
  print_counters("  ", NULL, NULL);
  #if PRINT_RESULTS
    print_results("OUT-Y", dir_out_y, DATA_LENGTH);
  #endif /* PRINT_RESULTS */
#endif

#if 1
  m_print("  * SWAR FIR (mul,add)\n");
  sysregs_init();
  fir_swar_mul_add(swar_out_y_add, swar_in_x, DATA_LENGTH);
  print_counters("  ", NULL, NULL);
  #if PRINT_RESULTS
    print_results("SWAR(add) Y", swar_out_y_add, DATA_LENGTH);
  #endif /* PRINT_RESULTS */

  check_results(DATA_LENGTH, dir_out_y, swar_out_y_add);
#endif

#if 1
  m_print("  * SWAR FIR (reduction)\n");
  sysregs_init();
  fir_swar_mul_red(swar_out_y_red, swar_in_x, DATA_LENGTH);
  print_counters("  ", NULL, NULL);
  #if PRINT_RESULTS
    print_results("SWAR(red) Y", swar_out_y_red, DATA_LENGTH);
  #endif /* PRINT_RESULTS */

  check_results(DATA_LENGTH, dir_out_y, swar_out_y_red);
#endif

#if 1
  m_print("  * SWAR FIR (accumulators)\n");
  sysregs_init();
  fir_swar_mul_accum(swar_out_y_accum, swar_in_x, DATA_LENGTH);
  print_counters("  ", NULL, NULL);
  #if PRINT_RESULTS
    print_results("SWAR(acc) Y", swar_out_y_accum, DATA_LENGTH);
  #endif /* PRINT_RESULTS */

  check_results(DATA_LENGTH, dir_out_y, swar_out_y_accum);
#endif

#endif /* TEST_OFFLINE */


/* ---------------------------------- */
/* ---------------------------------- */
/* ---------------------------------- */


#if TEST_ONLINE
  m_print("\n - Compute online\n");

#if 1
  m_print("  * FIR online\n");
  fir_online_init(FIR_LENGTH, dir_in_c);
  sysregs_init();
  for (unsigned i=0;i<DATA_LENGTH;++i) {
    dir_out_y[i] = fir_online(dir_in_x[i]);
  }
  print_counters("  ", NULL, NULL);
  #if PRINT_RESULTS
    print_results("OUT-Y", dir_out_y, DATA_LENGTH);
  #endif /* PRINT_RESULTS */
#endif

#if 1
  m_print("  * FIR swar online\n");
  fir_swar_online_init(FIR_LENGTH, dir_in_c);
  sysregs_init();
  for (unsigned i=0;i<DATA_LENGTH;++i) {
    swar_out_y_red[i] = fir_swar_online(dir_in_x[i]);
  }
  print_counters("  ", NULL, NULL);
  #if PRINT_RESULTS
    print_results("SWAR(online)", swar_out_y_red, DATA_LENGTH);
  #endif /* PRINT_RESULTS */

  check_results(DATA_LENGTH, dir_out_y, swar_out_y_red);
#endif


/* ---------------------------------- */
/* low pass FIR filter */
  m_print("\nExample: on-line low pass FIR filtering\n");
  unsigned times[10], insts[10];
  unsigned nres = 0;

  unsigned id = 0;
  unsigned len = (TEST_INDATA_SIZE<TEST_OUTPUT_SIZE) ? TEST_INDATA_SIZE : TEST_OUTPUT_SIZE;
  if (DATA_LENGTH<len) len = DATA_LENGTH;

  m_print("\n   - FIR length (# coeffs) = "); i_print(ONLINE_FIRLEN); NL; /* sizeof(test_fircoef)/sizeof(dataelm) */
  m_print("   - Data length (samples) = "); i_print(len); NL;

#if 1
  m_print("  * FIR online\n");
  fir_online_init(ONLINE_FIRLEN, test_fircoef_extra);
  id = 0;
  sysregs_init();
  for (unsigned i=0;i<len;++i) {
    if (id>=len) id -= len;
    test_dir_output[id] = fir_online(test_input[id]);
    id++;
  }
  print_counters("  ", &times[0], &insts[0]);
  #if PRINT_RESULTS
    print_results("OUTPUT", test_dir_output, len);
  #endif /* PRINT_RESULTS */
//  check_results(len, test_dir_output, test_output);
#endif

#if 1
  m_print("  * FIR swar online\n");
  fir_swar_online_init(ONLINE_FIRLEN, test_fircoef_extra);
  id = 0;
  sysregs_init();
  for (unsigned i=0;i<len;++i) {
    if (id>=len) id -= len;
    test_swar_output[id] = fir_swar_online(test_input[id]);
    id++;
  }
  print_counters("  ", &times[1], &insts[1]);
  #if PRINT_RESULTS
    print_results("OUTPUT", test_swar_output, len);
  #endif /* PRINT_RESULTS */
  check_results(len, test_swar_output, test_dir_output);
#endif

/* ---------------------------------- */
/* round buffer */

#if 1
  m_print("  * FIR online rbuf\n");
  fir_online_rbuf_init(ONLINE_FIRLEN, test_fircoef_extra);
  id = 0;
  sysregs_init();
  for (unsigned i=0;i<len;++i) {
    if (id>=len) id -= len;
    //test_dir_output[id] 
    test_swar_output[id] = fir_online_rbuf(test_input[id]);
    id++;
  }
  print_counters("  ", &times[2], &insts[2]);
  #if PRINT_RESULTS
    print_results("OUTPUT", test_swar_output, len);
  #endif /* PRINT_RESULTS */
  check_results(len, test_dir_output, test_swar_output);
#endif

#if 1
  m_print("  * FIR swar online rbuf\n");
  fir_swar_online_rbuf_init(ONLINE_FIRLEN, test_fircoef_extra);
  id = 0;
  sysregs_init();
  for (unsigned i=0;i<len;++i) {
    if (id>=len) id -= len;
    test_swar_output[id] = fir_swar_online_rbuf(test_input[id]);
    id++;
  }
  print_counters("  ", &times[3], &insts[3]);
  #if PRINT_RESULTS
    print_results("OUTPUT", test_swar_output, len);
  #endif /* PRINT_RESULTS */
  check_results(len, test_dir_output, test_swar_output);
#endif

/* ---------------------------------- */
/* aligned round buffer */

#if 1
  m_print("  * FIR online aligned rbuf\n");
  fir_online_align_rbuf_init(ONLINE_FIRLEN, test_fircoef_extra);
  id = 0;
  sysregs_init();
  for (unsigned i=0;i<len;++i) {
    if (id>=len) id -= len;
    test_swar_output[id] = fir_online_align_rbuf(test_input[id]);
    id++;
  }
  print_counters("  ", &times[4], &insts[4]);
  #if PRINT_RESULTS
    print_results("OUTPUT", test_swar_output, len);
  #endif /* PRINT_RESULTS */
  check_results(len, test_dir_output, test_swar_output);
#endif

#if 1
  m_print("  * FIR swar online aligned rbuf\n");
  fir_swar_online_align_rbuf_init(ONLINE_FIRLEN, test_fircoef_extra);
  id = 0;
  sysregs_init();
  for (unsigned i=0;i<len;++i) {
    if (id>=len) id -= len;
    test_swar_output[id] = fir_swar_online_align_rbuf(test_input[id]);
    id++;
  }
  print_counters("  ", &times[5], &insts[5]);
  #if PRINT_RESULTS
    print_results("OUTPUT", test_swar_output, len);
  #endif /* PRINT_RESULTS */
  check_results(len, test_dir_output, test_swar_output);
#endif

/* ---------------------------------- */

#endif /* TEST_ONLINE */

  m_print("\n*** END OF TEST ***\n");
  __asm__ volatile ("\t ta 1\n\t nop\n\t nop\n");
  return 0;
}