ise.c

/* -----------------------------------------------------------------------------
 *  Copyright (C) 2019 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    : ise.c
 *  Authors     : Martin Danek
 *  Description : models of the ISE for LEON2
 *  Release     :
 *  Version     : 1.0
 *  Date        : 20.04.2019
 * -----------------------------------------------------------------------------
 */

#include <stdint.h>

#include "my_printf.h"
#include "ise.h"
#define BALANCE

int swar_dem(unsigned print, int *exp, int *signal, unsigned packingFactor, unsigned bits, unsigned interleaved, unsigned cplx) {
  unsigned i, mask;
#ifndef BALANCE
  unsigned ar, ai, br, bi;
  unsigned re, im, res;
#else
  int ar, ai, br, bi;
  int re, im, res;
#endif
  unsigned split;

  mask = (1 << bits) - 1;
  split = (packingFactor / 2) * bits;
  re=0;
  im=0;
  res=0;
  if (print) printf("exp %08X signal %08X\n", *exp, *signal);
  for (i=0;i<packingFactor;i+=2) { // 2 values per one complex number
    ai = ((*exp) >> bits*i) & mask;
    ar = ((*exp) >> (bits*(i+1))) & mask;
    bi = ((*signal) >> bits*i) & mask;
    br = ((*signal) >> (bits*(i+1))) & mask;
#ifdef BALANCE
    if (bits==1) {
      ai=(ai?ai:-1);
      ar=(ar?ar:-1);
      bi=(bi?bi:-1);
      br=(br?br:-1);
    }
    else {
      ai = ai - (1<<(bits-1));
      ar = ar - (1<<(bits-1));
      bi = bi - (1<<(bits-1));
      br = br - (1<<(bits-1));
    }
#endif
    if (cplx) {
      // complex multiplication
      re = mask & (((ar * br) >> bits) + ((ai * bi) >> bits)) >> 1;
#ifndef BALANCE
      im = mask & ((((ar * bi) >> bits) - ((ai * br) >> bits)) >> 1) + ((1<<bits)-1);
#else
      im = mask & ((((ar * bi) >> bits) - ((ai * br) >> bits)) >> 1);
#endif
    }
    else {
      // real multiplication
      re = mask & ((ar * br) >> bits);
      im = mask & ((ai * bi) >> bits);
    }
    if (interleaved) {
      // interleaved store
      res |= (re << (bits*(i+1)));
      res |= (im << (bits*i));
    }
    else {
      // consecutive store
      res |= (re << (split + (bits*(i>>1))));
      res |= (im << (bits*(i>>1)));
    }
    if (print) printf("%5d e_r %3d e_i %3d s_r %3d s_i %3d re %3d im %3d res %08X\n",i, ar, ai, br, bi, re, im, res);
  }
  return res;
}


int swar_corr(unsigned print, int *code, int *signal, unsigned packingFactor, unsigned bits, unsigned sgnd, unsigned reduce, int *sop) {
  unsigned i, mask;
  int a, c;
  int b;
  int res;

  mask = (1 << bits) - 1;
  res=0;
  for (i=0;i<packingFactor;i++) {
    a = ((*code) >> i) & 1;
    if (sgnd) a = (a==0)?-1:a;
    b = ((*signal) >> (bits * i)) & mask;
    c = a * b;
    res += c;
    if (print) printf("%06d cd % 2d sg %3d c % 3d res %10d\n", i, a, b, c, res);
  }
  if (reduce)
    *sop += res;
  else
    *sop = res;
  return *sop;
}


#define NPI 0x80000000
#define NPI_HALF 0x40000000

#define SINE_THR_N_1 85669753 // 0.125327831168065
#define SINE_THR_N_2 172723448 // 0.252680255142079
#define SINE_THR_N_3 262760287 // 0.384396774495639
#define SINE_THR_N_4 357913941 // 0.523598775598299
#define SINE_THR_N_5 461496472 // 0.675131532937032
#define SINE_THR_N_6 579705789 // 0.848062078981481
#define SINE_THR_N_7 728294928 // 1.06543581651074
#define SINE_THR_N_8 NPI_HALF  // 1.5707963267949

#define COSINE_THR_N_1 345446896 // 0.505360510284157
#define COSINE_THR_N_2 494036035 // 0.722734247813416
#define COSINE_THR_N_3 612245352 // 0.895664793857865
#define COSINE_THR_N_4 715827883 // 1.0471975511966
#define COSINE_THR_N_5 810981537 // 1.18639955229926
#define COSINE_THR_N_6 901018376 // 1.31811607165282
#define COSINE_THR_N_7 988072071 // 1.44546849562683
#define COSINE_THR_N_8 NPI_HALF  // 1.5707963267949


// Compute sine(arg) for <0;2*pi> mapped to <0;0xffffffff>
int sineQuantNorm(unsigned bits, unsigned arg) {
  unsigned pos, val;
  pos=1;

//   printf("arg1 %f 2*PI %f\n",arg, 2*PI);


//   printf("arg2 %f\n",arg);

  if (arg>NPI) {
//     arg=arg-NPI;
    arg&=(~NPI);
    pos=0;
  }

  if (arg>(NPI_HALF)) {
//     arg=(NPI)-arg;
    arg=((~arg)+1)&(~NPI);
  }

//   printf("arg3 %f\n",arg);

  if (bits==1) {
    val=pos;
  }

  if (bits==2) {
    if      (arg<SINE_THR_N_4) val=2;
    else val=3;
    if (pos==0) val=3-val;
  }

  if (bits==3) {
    if      (arg<SINE_THR_N_2) val=4;
    else if (arg<SINE_THR_N_4) val=5;
    else if (arg<SINE_THR_N_6) val=6;
    else val=7;
    if (pos==0) val=7-val;
  }

  if (bits==4) {
    if      (arg<SINE_THR_N_1) val=8;
    else if (arg<SINE_THR_N_2) val=9;
    else if (arg<SINE_THR_N_3) val=10;
    else if (arg<SINE_THR_N_4) val=11;
    else if (arg<SINE_THR_N_5) val=12;
    else if (arg<SINE_THR_N_6) val=13;
    else if (arg<SINE_THR_N_7) val=14;
    else val=15;
    if (pos==0) val=15-val;
  }

  return val;

}

// Compute cosine(arg) for <0;2*pi> mapped to <0;0xffffffff>
int cosineQuantNorm(unsigned bits, unsigned arg) {
  unsigned pos, val;
  pos=1;

//   printf("arg1 %f 2*PI %f\n",arg, 2*PI);


//   printf("arg2 %f\n",arg);

  if (arg>NPI) {
//     arg=2*NPI-arg;
    arg=(~arg)+1;
  }

  if (arg>(NPI_HALF)) {
//     arg=(NPI)-arg;
    arg=((~arg)+1)&(~NPI);
    pos=0;
  }

//   printf("arg3 %f\n",arg);

  if (bits==1) {
    val=pos;
  }

  if (bits==2) {
    if      (arg<COSINE_THR_N_4) val=3;
    else val=2;
    if (pos==0) val=3-val;
  }

  if (bits==3) {
    if      (arg<COSINE_THR_N_2) val=7;
    else if (arg<COSINE_THR_N_4) val=6;
    else if (arg<COSINE_THR_N_6) val=5;
    else val=4;
    if (pos==0) val=7-val;
  }

  if (bits==4) {
    if      (arg<COSINE_THR_N_1) val=15;
    else if (arg<COSINE_THR_N_2) val=14;
    else if (arg<COSINE_THR_N_3) val=13;
    else if (arg<COSINE_THR_N_4) val=12;
    else if (arg<COSINE_THR_N_5) val=11;
    else if (arg<COSINE_THR_N_7) val=10;
    else if (arg<COSINE_THR_N_6) val=9;
    else val=8;
    if (pos==0) val=15-val;
  }

  return val;

}


int swar_sincos(unsigned print, int coef, unsigned bits) {
  unsigned i;
  unsigned sin1, cos1;
  int res;

  cos1 = (cosineQuantNorm(bits, coef));
  sin1 = (sineQuantNorm(bits, coef));
  if (print) printf("%08X c1 %1X s1 %1X\n", coef, cos1, sin1);
  switch (bits) {
    case 1:
      res = (cos1 << 1) | sin1;
      break;
    case 2:
      res = (cos1 << 2) | sin1;
      break;
    case 3:
      res = (cos1 << 3) | sin1;
      break;
    case 4:
      res = (cos1 << 4) | sin1;
      break;
    default:
      printf("SWAR_SINCOS: INVALID BIT COUNT\n");
      break;
  }
  if (print) printf("sincos: %08X\n", res);
  return res;
}


int swar_alu(unsigned print, int *veca, int *vecb, unsigned packingFactor, unsigned bits, unsigned oper, unsigned sgnd, unsigned sat, unsigned reduce, int *acc) {
  unsigned i, mask;
  int a, b;
  int c, res;
  unsigned neg;

  mask = (1 << bits) - 1;

  c=0;
  res=0;
  neg=0;

  if (print) printf("veca %08X vecb %08X\n", *veca, *vecb);
  for (i=0;i<packingFactor;i++) {

    a = ((*veca) >> bits*i) & mask;
    b = ((*vecb) >> bits*i) & mask;

    if (sgnd) {
      if (a & (1<<(bits-1))) a |= ~(mask);
      if (b & (1<<(bits-1))) b |= ~(mask);
    }
    switch (oper) {
      case ADD: c = a + b; break;
      case SUB: c = a - b; neg=(a<b)?1:0; break;
      case MUL: c = a * b; break;
      default: printf("SWAR_ALU: INVALID OPERATION\n"); break;
    }

    if ((sat==SATUR)&&(!sgnd)) {
      if (c>((1<<bits)-1)) c = (1<<bits)-1;
      else if (c<0) {
        if (neg) c=0;
        else c = (1<<bits)-1;
      }
    }

    if ((sat==SATUR)&&(sgnd)&&(c>((1<<(bits-1))-1))) c = (1<<(bits-1))-1;
    else if ((sat==SATUR)&&(sgnd)&&(c<(-(1<<(bits-1))))) c = (-(1<<(bits-1)));

    if (reduce) res += c;
    else res |= (c & mask) << (bits*i);

    if ((!sgnd) && (bits<16)) c &= (1 << (bits<<1)) - 1;
//    if (print) printf("acc[%d] = %08X\n", i, acc[i]);
    *(acc+i) += c;

    if (print) printf("%5d a %3d b %3d c %3d res %08X\n",i, a, b, c, res); //  acc(%08X) , acc[i]
  }
  return res;
}