math.c 3.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166
  1. /*
  2. * Math library
  3. * Contains functions math operation that are not directly supported by the ALU
  4. */
  5. // Divide two signed integer numbers using MU
  6. word MATH_div(word dividend, word divisor)
  7. {
  8. word retval = 0;
  9. asm(
  10. "load32 0xC02744 r2 ; r2 = addr idiv_writea\n"
  11. "write 0 r2 r4 ; write a to divider\n"
  12. "write 1 r2 r5 ; write b to divider and perform signed division\n"
  13. "read 1 r2 r2 ; read result to r2\n"
  14. "write -4 r14 r2 ; write result to stack for return\n"
  15. );
  16. return retval;
  17. }
  18. // Modulo from division of two signed integer numbers using MU
  19. word MATH_mod(word dividend, word divisor)
  20. {
  21. word retval = 0;
  22. asm(
  23. "load32 0xC02744 r2 ; r2 = addr idiv_writea\n"
  24. "write 0 r2 r4 ; write a to divider\n"
  25. "write 3 r2 r5 ; write b to divider and perform signed modulo\n"
  26. "read 3 r2 r2 ; read remainder to r2\n"
  27. "write -4 r14 r2 ; write result to stack for return\n"
  28. );
  29. return retval;
  30. }
  31. // Divide two unsigned integer numbers using MU
  32. word MATH_divU(word dividend, word divisor)
  33. {
  34. word retval = 0;
  35. asm(
  36. "load32 0xC02744 r2 ; r2 = addr idiv_writea\n"
  37. "write 0 r2 r4 ; write a to divider\n"
  38. "write 2 r2 r5 ; write b to divider and perform unsigned division\n"
  39. "read 2 r2 r2 ; read result to r2\n"
  40. "write -4 r14 r2 ; write result to stack for return\n"
  41. );
  42. return retval;
  43. }
  44. // Modulo from division of two unsigned integer numbers using MU
  45. word MATH_modU(word dividend, word divisor)
  46. {
  47. word retval = 0;
  48. asm(
  49. "load32 0xC02744 r2 ; r2 = addr idiv_writea\n"
  50. "write 0 r2 r4 ; write a to divider\n"
  51. "write 4 r2 r5 ; write b to divider and perform unsiged modulo\n"
  52. "read 4 r2 r2 ; read remainder to r2\n"
  53. "write -4 r14 r2 ; write result to stack for return\n"
  54. );
  55. return retval;
  56. }
  57. // Signed Division and Modulo without / and %
  58. word MATH_SW_divmod(word dividend, word divisor, word* rem)
  59. {
  60. word quotient = 1;
  61. word neg = 1;
  62. if ((dividend>0 &&divisor<0)||(dividend<0 && divisor>0))
  63. neg = -1;
  64. // Convert to positive
  65. word tempdividend = (dividend < 0) ? -dividend : dividend;
  66. word tempdivisor = (divisor < 0) ? -divisor : divisor;
  67. if (tempdivisor == tempdividend) {
  68. *rem = 0;
  69. return 1*neg;
  70. }
  71. else if (tempdividend < tempdivisor) {
  72. if (dividend < 0)
  73. *rem = tempdividend*neg;
  74. else
  75. *rem = tempdividend;
  76. return 0;
  77. }
  78. while (tempdivisor<<1 <= tempdividend)
  79. {
  80. tempdivisor = tempdivisor << 1;
  81. quotient = quotient << 1;
  82. }
  83. // Call division recursively
  84. if(dividend < 0)
  85. quotient = quotient*neg + MATH_SW_divmod(-(tempdividend-tempdivisor), divisor, rem);
  86. else
  87. quotient = quotient*neg + MATH_SW_divmod(tempdividend-tempdivisor, divisor, rem);
  88. return quotient;
  89. }
  90. word MATH_SW_div(word dividend, word divisor)
  91. {
  92. word rem = 0;
  93. return MATH_SW_divmod(dividend, divisor, &rem);
  94. }
  95. word MATH_SW_mod(word dividend, word divisor)
  96. {
  97. word rem = 0;
  98. MATH_SW_divmod(dividend, divisor, &rem);
  99. return rem;
  100. }
  101. // Unsigned Division and Modulo without / and %
  102. word MATH_SW_divmodU(word dividend, word divisor, word mod)
  103. {
  104. word quotient = 0;
  105. word remainder = 0;
  106. if(divisor == 0)
  107. return 0;
  108. word i;
  109. for(i = 31 ; i >= 0 ; i--)
  110. {
  111. quotient = quotient << 1;
  112. remainder = remainder << 1;
  113. remainder = remainder | ((unsigned) (dividend & (1 << i)) >> i);
  114. if((unsigned int) remainder >= (unsigned int) divisor)
  115. {
  116. remainder = remainder - divisor;
  117. quotient = quotient | 1;
  118. }
  119. if (i == 0)
  120. if (mod == 1)
  121. return remainder;
  122. else
  123. return quotient;
  124. }
  125. return 0;
  126. }
  127. // Unsigned positive integer division
  128. word MATH_SW_divU(word dividend, word divisor)
  129. {
  130. return MATH_SW_divmodU(dividend, divisor, 0);
  131. }
  132. // Unsigned positive integer modulo
  133. word MATH_SW_modU(word dividend, word divisor)
  134. {
  135. return MATH_SW_divmodU(dividend, divisor, 1);
  136. }
  137. // Returns absolute value
  138. word MATH_abs(word x)
  139. {
  140. if (x >= 0)
  141. return x;
  142. else
  143. return -x;
  144. }