cordic.c 2.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101
  1. /*
  2. * Copyright (c) 2011 Broadcom Corporation
  3. *
  4. * Permission to use, copy, modify, and/or distribute this software for any
  5. * purpose with or without fee is hereby granted, provided that the above
  6. * copyright notice and this permission notice appear in all copies.
  7. *
  8. * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
  9. * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10. * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
  11. * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12. * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
  13. * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  14. * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15. */
  16. #include <linux/module.h>
  17. #include <linux/cordic.h>
  18. #define CORDIC_ANGLE_GEN 39797
  19. #define CORDIC_PRECISION_SHIFT 16
  20. #define CORDIC_NUM_ITER (CORDIC_PRECISION_SHIFT + 2)
  21. #define FIXED(X) ((s32)((X) << CORDIC_PRECISION_SHIFT))
  22. #define FLOAT(X) (((X) >= 0) \
  23. ? ((((X) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1) \
  24. : -((((-(X)) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1))
  25. static const s32 arctan_table[] = {
  26. 2949120,
  27. 1740967,
  28. 919879,
  29. 466945,
  30. 234379,
  31. 117304,
  32. 58666,
  33. 29335,
  34. 14668,
  35. 7334,
  36. 3667,
  37. 1833,
  38. 917,
  39. 458,
  40. 229,
  41. 115,
  42. 57,
  43. 29
  44. };
  45. /*
  46. * cordic_calc_iq() - calculates the i/q coordinate for given angle
  47. *
  48. * theta: angle in degrees for which i/q coordinate is to be calculated
  49. * coord: function output parameter holding the i/q coordinate
  50. */
  51. struct cordic_iq cordic_calc_iq(s32 theta)
  52. {
  53. struct cordic_iq coord;
  54. s32 angle, valtmp;
  55. unsigned iter;
  56. int signx = 1;
  57. int signtheta;
  58. coord.i = CORDIC_ANGLE_GEN;
  59. coord.q = 0;
  60. angle = 0;
  61. theta = FIXED(theta);
  62. signtheta = (theta < 0) ? -1 : 1;
  63. theta = ((theta + FIXED(180) * signtheta) % FIXED(360)) -
  64. FIXED(180) * signtheta;
  65. if (FLOAT(theta) > 90) {
  66. theta -= FIXED(180);
  67. signx = -1;
  68. } else if (FLOAT(theta) < -90) {
  69. theta += FIXED(180);
  70. signx = -1;
  71. }
  72. for (iter = 0; iter < CORDIC_NUM_ITER; iter++) {
  73. if (theta > angle) {
  74. valtmp = coord.i - (coord.q >> iter);
  75. coord.q += (coord.i >> iter);
  76. angle += arctan_table[iter];
  77. } else {
  78. valtmp = coord.i + (coord.q >> iter);
  79. coord.q -= (coord.i >> iter);
  80. angle -= arctan_table[iter];
  81. }
  82. coord.i = valtmp;
  83. }
  84. coord.i *= signx;
  85. coord.q *= signx;
  86. return coord;
  87. }
  88. EXPORT_SYMBOL(cordic_calc_iq);
  89. MODULE_DESCRIPTION("CORDIC algorithm");
  90. MODULE_AUTHOR("Broadcom Corporation");
  91. MODULE_LICENSE("Dual BSD/GPL");