src/firmware/CordicXilinx.cc

0001 /**\class CordicXilinx CordicXilinx.cc L1Trigger/L1TCalorimeter/src/firmware/CordicXilinx.cc
0002
0003  Description: Emulates parts of the Xilinx DSP IP CORDIC routine, as described in
0004               http://www.xilinx.com/support/documentation/ip_documentation/cordic/v6_0/pg105-cordic.pdf
0005               This class only implements the vector translation, returning magnitude and phase, given signed
0006               x and y inputs.  The inputs and outputs are not packed, so that normal signed integer
0007               arithmetic works as expected.  They can easily be packed into a fixed width by abs() and placing
0008               a sign bit at the appropriate offset.
0009               The applicable configuration parameters that are being emulated are the following:
0010                 - Functional Selection: Translate
0011                 - Phase Format: Radians
0012                 - Round Mode: Truncate
0013                 - Advanced Configuration Parameters: Iterations=0, Precision=0, Coarse Rotation, Compensation Scaling = Embedded Multiplier
0014               In addition, an arbitrary input and output width can be specified, HOWEVER, only in=24, out=19 has been rigorously tested
0015               against the Xilinx proprietary emulator.
0016
0017  Tests: Full circle at various magnitudes, including maximum; a few billion random inputs
0018         Limited hardware comparisons have shown agreement as well.
0019         Test framework: https://github.com/nsmith-/cordic_test
0020
0021  Original Author:  Nick Smith ( nick.smith@cern.ch )
0022
0023 */
0024
0025 #include "L1Trigger/L1TCalorimeter/interface/CordicXilinx.h"
0026
0027 #include <vector>
0028 #include <iostream>
0029 #include <iomanip>
0030 #include <cassert>
0031 #include <cmath>
0032 #include <cmath>
0033
0034 CordicXilinx::CordicXilinx(int inputBits, int outputBits, bool debug)
0035     : inputBits_(inputBits), outputBits_(outputBits), debug_(debug) {
0036   // Coarse rotation lowers necessary iterations by 2
0037   iterations_ = outputBits - 2;
0038   // Internal precision is by default this value (when set to 0 in xilinx config)
0039   internalBits_ = outputBits + ceil(log((float)iterations_) / log(2.));
0040
0041   double scaleFactor = 1.;
0042   for (int i = 1; i <= iterations_; ++i) {
0043     int rotation = encodeAngle(atan(pow(2., -i)));
0044     rotations_.push_back(rotation);
0045     scaleFactor *= pow(1 + pow(2., -2 * i), -0.5);
0046   }
0047   scaleFactor_ = scaleFactor * pow(2., internalBits_ - 1) + 0.5;
0048
0049   // Precompute angles table for speed
0050   encodedAngles_[Pi] = encodeAngle(M_PI);
0051   encodedAngles_[HalfPi] = encodeAngle(M_PI / 2);
0052   encodedAngles_[NHalfPi] = encodeAngle(-M_PI / 2);
0053
0054   if (debug_)
0055     printf(
0056         "Cordic setup: %d iterations, %d internal bits, scale factor = %d\n", iterations_, internalBits_, scaleFactor_);
0057 }
0058
0059 int CordicXilinx::encodeAngle(const double angleFloat) const {
0060   assert(fabs(angleFloat) <= M_PI);
0061   // Xilinx seems to store rounded rotation table
0062   return angleFloat * pow(2., internalBits_ - 3) + 0.5;
0063 }
0064
0065 void CordicXilinx::operator()(int32_t xInput, int32_t yInput, int32_t& aPhi, uint32_t& aMagnitude) const {
0066   // Assumption in algorithm is that arithmetic shifts are used for ints (as opposed to logical shifts)
0067   static_assert(((int)-1) >> 3 == (int)-1,
0068                 "Signed ints need to use arithmetic shifts for this algorithm to work properly!");
0069
0070   // Input checks
0071   // Input is in 2QN format, and for xilinx
0072   // the max is +- 1.0000...
0073   assert(abs(xInput) <= (1 << (inputBits_ - 1)));
0074   assert(abs(yInput) <= (1 << (inputBits_ - 1)));
0075
0076   // Rotation to get from current vector to origin
0077   // must invert to get aPhi
0078   int rotation(0);
0079   int x, y;
0080
0081   // Debug tool
0082   auto printVals = [&x, &y, &rotation, this] {
0083     printf("x: % 8d y: % 8d phi: % 8d outphi: % 8d float phi = % f\n",
0084            x,
0085            y,
0086            rotation,
0087            (abs(rotation) >> (internalBits_ - outputBits_)) * ((rotation > 0) ? -1 : 1),
0088            rotation / pow(2., internalBits_ - 3));
0089   };
0090
0091   // Convert to internal precision
0092   if (internalBits_ > inputBits_) {
0093     x = xInput << (internalBits_ - inputBits_);
0094     y = yInput << (internalBits_ - inputBits_);
0095   } else {
0096     x = xInput >> (inputBits_ - internalBits_);
0097     y = yInput >> (inputBits_ - internalBits_);
0098   }
0099   if (debug_)
0100     printVals();
0101
0102   // Coarse rotate to [-pi/4,pi/4)
0103   if (x - y >= 0) {
0104     if (x + y >= 0) {
0105       // East (Correct) quadrant
0106     } else {
0107       // South, rotate by +pi/2
0108       int xtmp = -y;
0109       int ytmp = x;
0110       x = xtmp;
0111       y = ytmp;
0112       rotation += encodedAngles_[HalfPi];
0113     }
0114   } else {
0115     if (x + y >= 0) {
0116       // North, rotate by -pi/2
0117       int xtmp = y;
0118       int ytmp = -x;
0119       x = xtmp;
0120       y = ytmp;
0121       rotation += encodedAngles_[NHalfPi];
0122     } else {
0123       // West, rotate by pi
0124       x = -x;
0125       y = -y;
0126       rotation += encodedAngles_[Pi];
0127     }
0128   }
0129   if (debug_)
0130     std::cout << "Coarse rotate" << std::endl;
0131   if (debug_)
0132     printVals();
0133
0134   if (debug_)
0135     std::cout << "Starting iterations" << std::endl;
0136   for (int i = 1; i <= iterations_; ++i) {
0137     int sign = (y >= 0) ? -1 : 1;
0138     int xtmp = x - sign * (y >> i);
0139     int ytmp = y + sign * (x >> i);
0140     x = xtmp;
0141     y = ytmp;
0142     rotation += sign * rotations_[i - 1];
0143     if (debug_)
0144       printVals();
0145   }
0146
0147   // need a little extra room for the last multiplication
0148   aMagnitude = ((long)x * (long)scaleFactor_) >> (2 * internalBits_ - outputBits_ - 1);
0149
0150   // Xilinx seems to just mod to [-pi,pi]
0151   if (rotation > encodedAngles_[Pi])
0152     rotation -= 2 * encodedAngles_[Pi] + 1;
0153   aPhi = (-rotation) >> (internalBits_ - outputBits_);
0154 }