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 <stdio.h>

#include "ise.h"
#define BALANCE

// #define DBGPRINT

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

  mask = (1 << bits) - 1;
  split = (packingFactor / 2) * bits;
  re=0;
  im=0;
  res=0;
#ifdef DBGPRINT
  printf("exp %016lX signal %016lX\n", exp, signal);
#endif /* DBGPRINT */
  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)));
    }
#ifdef DBGPRINT
    printf("%5d e_r %3d e_i %3d s_r %3d s_i %3d re %3ld im %3ld res %016lX\n",i, ar, ai, br, bi, re, im, res);
#endif /* DBGPRINT */
  }
  return res;
}


SREGTYPE swar_corr(REGTYPE code, REGTYPE signal, unsigned packingFactor, unsigned bits, unsigned sgnd, unsigned reduce, SREGTYPE *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;
#ifdef DBGPRINT
    printf("%06d cd % 2d sg %3d c % 3d res %10d\n", i, a, b, c, res);
#endif /* DBGPRINT */
  }
  if (reduce)
    *sop += res;
  else
    *sop = res;
  return *sop;
}

#ifdef RV32

#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

#else

#define NPI 0x8000000000000000
#define NPI_HALF 0x4000000000000000

#define SINE_THR_N_1 0x051B37797325C5C0 // 0.125327831168065 0.079786175349536
#define SINE_THR_N_2 0x0A4B8CF83D29BB80 // 0.252680255142079 0.160861246510332
#define SINE_THR_N_3 0x0FA9675F16BBCA80 // 0.384396774495639 0.244714587078246
#define SINE_THR_N_4 0x1555555555555600 // 0.523598775598299 0.333333333333333
#define SINE_THR_N_5 0x1B81E0985CC8EB00 // 0.675131532937032 0.429802082816549
#define SINE_THR_N_6 0x228D9BBCB992E400 // 0.848062078981481 0.539893087674768
#define SINE_THR_N_7 0x2B68E60F85AC8A00 // 1.065435816510739 0.678277506979335
#define SINE_THR_N_8 NPI_HALF           // 1.570796326794897 1.000000000000000

#define COSINE_THR_N_1 0x149719F07A537700 // 0.505360510284157 0.321722493020665
#define COSINE_THR_N_2 0x1D726443466D1E00 // 0.722734247813416 0.460106912325232
#define COSINE_THR_N_3 0x247E1F67A3371600 // 0.895664793857865 0.570197917183451
#define COSINE_THR_N_4 0x2AAAAAAAAAAAAC00 // 1.047197551196598 0.666666666666667
#define COSINE_THR_N_5 0x305698A0E9443800 // 1.186399552299258 0.755285412921754
#define COSINE_THR_N_6 0x35B47307C2D64600 // 1.318116071652818 0.839138753489668
#define COSINE_THR_N_7 0x3AE4C8868CDA3C00 // 1.445468495626831 0.920213824650464
#define COSINE_THR_N_8 NPI_HALF           // 1.570796326794897 1.000000000000000

#endif


// Compute sine(arg) for <0;2*pi> mapped to <0;0xffffffff>
REGTYPE sineQuantNorm(unsigned bits, REGTYPE 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>
REGTYPE cosineQuantNorm(unsigned bits, REGTYPE 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;

}


REGTYPE swar_sincos(REGTYPE coef, unsigned bits) {
  REGTYPE sin1, cos1;
  REGTYPE res;

  cos1 = (cosineQuantNorm(bits, coef));
  sin1 = (sineQuantNorm(bits, coef));
#ifdef DBGPRINT
  printf("%08lX c1 %1lX s1 %1lX\n", coef, cos1, sin1);
#endif /* DBGPRINT */
  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;
  }
#ifdef DBGPRINT
  printf("sincos: %016lX\n", res);
#endif /* DBGPRINT */
  return res;
}




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

  mask = (1 << bits) - 1;

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

#ifdef DBGPRINT
  printf("veca %016lX vecb %016lX packing %d bits %d oper %d sgnd %d sat %d red %d\n", veca, vecb, packingFactor, bits, oper, sgnd, sat, reduce);
#endif /* DBGPRINT */
  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) {
      if (!sgnd) {
        if (neg) c=0;
        else {
          if (c>((1<<bits)-1)) c = (1<<bits)-1;
        }
      }
      else {
        if ((SREGTYPE)c>(SREGTYPE)((1<<(bits-1))-1)) c = (1<<(bits-1))-1;
        else if ((SREGTYPE)c<(SREGTYPE)(-(1<<(bits-1)))) c = (-(1<<(bits-1)));
      }
    }
//     else {
//       if ((!sgnd) && (neg)) c = c | (~mask);
//     }

#ifdef DBGPRINT
    printf("i %d sgnd %d sat %d a %016lX b %016lX c %016lX neg %d\n", i, sgnd, sat, a, b, c, neg);
#endif

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

//    if ((!sgnd) && (bits<16)) c &= (1 << (bits<<1)) - 1;
    *(acc+i) += c;
#ifdef DBGPRINT
    printf("c %016lX acc[%d] = %016lX\n", c, i, *(acc+i));
#endif

#ifdef DBGPRINT
    printf("%5d a %3ld b %3ld c %3ld res %016lX\n",i, a, b, c, res); //  acc(%08X) , acc[i]
#endif /* DBGPRINT */
  }
  return res;
}