visus
/
teensy-audio
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
717 Commits
2 Branches
Tree: f90eb6b81c
teensy-audio/utility/dspinst.h

371 lines
13KB
Raw Blame History 

  1. /* Audio Library for Teensy 3.X

  2.  * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com

  3.  *

  4.  * Development of this audio library was funded by PJRC.COM, LLC by sales of

  5.  * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop

  6.  * open source software by purchasing Teensy or other PJRC products.

  7.  *

  8.  * Permission is hereby granted, free of charge, to any person obtaining a copy

  9.  * of this software and associated documentation files (the "Software"), to deal

  10.  * in the Software without restriction, including without limitation the rights

  11.  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

  12.  * copies of the Software, and to permit persons to whom the Software is

  13.  * furnished to do so, subject to the following conditions:

  14.  *

  15.  * The above copyright notice, development funding notice, and this permission

  16.  * notice shall be included in all copies or substantial portions of the Software.

  17.  *

  18.  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

  19.  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

  20.  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

  21.  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

  22.  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

  23.  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

  24.  * THE SOFTWARE.

  25.  */

  26. 

  27. #ifndef dspinst_h_

  28. #define dspinst_h_

  29. 

  30. #include <stdint.h>

  31. 

  32. // computes limit((val >> rshift), 2**bits)

  33. static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift) __attribute__((always_inline, unused));

  34. static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift)

  35. {

  36. #if defined(KINETISK)

  37. 	int32_t out;

  38. 	asm volatile("ssat %0, %1, %2, asr %3" : "=r" (out) : "I" (bits), "r" (val), "I" (rshift));

  39. 	return out;

  40. #elif defined(KINETISL)

  41. 	int32_t out, max;

  42. 	out = val >> rshift;

  43. 	max = 1 << (bits - 1);

  44. 	if (out >= 0) {

  45. 		if (out > max - 1) out = max - 1;

  46. 	} else {

  47. 		if (out < -max) out = -max;

  48. 	}

  49. 	return out;

  50. #endif

  51. }

  52. 

  53. // computes limit(val, 2**bits)

  54. static inline int16_t saturate16(int32_t val) __attribute__((always_inline, unused));

  55. static inline int16_t saturate16(int32_t val)

  56. {

  57. #if defined(KINETISK)

  58. 	int16_t out;

  59. 	int32_t tmp;

  60. 	asm volatile("ssat %0, %1, %2" : "=r" (tmp) : "I" (16), "r" (val) );

  61. 	out = (int16_t) (tmp & 0xffff); // not sure if the & 0xffff is necessary. test.

  62. 	return out;

  63. #elif defined(KINETISL)

  64. 	return 0; // TODO....

  65. #endif

  66. }

  67. 

  68. // computes ((a[31:0] * b[15:0]) >> 16)

  69. static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b) __attribute__((always_inline, unused));

  70. static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b)

  71. {

  72. #if defined(KINETISK)

  73. 	int32_t out;

  74. 	asm volatile("smulwb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  75. 	return out;

  76. #elif defined(KINETISL)

  77. 	return ((int64_t)a * (int16_t)(b & 0xFFFF)) >> 16;

  78. #endif

  79. }

  80. 

  81. // computes ((a[31:0] * b[31:16]) >> 16)

  82. static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b) __attribute__((always_inline, unused));

  83. static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b)

  84. {

  85. #if defined(KINETISK)

  86. 	int32_t out;

  87. 	asm volatile("smulwt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  88. 	return out;

  89. #elif defined(KINETISL)

  90. 	return ((int64_t)a * (int16_t)(b >> 16)) >> 16;

  91. #endif

  92. }

  93. 

  94. // computes (((int64_t)a[31:0] * (int64_t)b[31:0]) >> 32)

  95. static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b) __attribute__((always_inline, unused));

  96. static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b)

  97. {

  98. #if defined(KINETISK)

  99. 	int32_t out;

  100. 	asm volatile("smmul %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  101. 	return out;

  102. #elif defined(KINETISL)

  103. 	return 0; // TODO....

  104. #endif

  105. }

  106. 

  107. // computes (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32)

  108. static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b) __attribute__((always_inline, unused));

  109. static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b)

  110. {

  111. #if defined(KINETISK)

  112. 	int32_t out;

  113. 	asm volatile("smmulr %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  114. 	return out;

  115. #elif defined(KINETISL)

  116. 	return 0; // TODO....

  117. #endif

  118. }

  119. 

  120. // computes sum + (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32)

  121. static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline, unused));

  122. static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b)

  123. {

  124. #if defined(KINETISK)

  125. 	int32_t out;

  126. 	asm volatile("smmlar %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));

  127. 	return out;

  128. #elif defined(KINETISL)

  129. 	return 0; // TODO....

  130. #endif

  131. }

  132. 

  133. // computes sum - (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32)

  134. static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline, unused));

  135. static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b)

  136. {

  137. #if defined(KINETISK)

  138. 	int32_t out;

  139. 	asm volatile("smmlsr %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));

  140. 	return out;

  141. #elif defined(KINETISL)

  142. 	return 0; // TODO....

  143. #endif

  144. }

  145. 

  146. 

  147. // computes (a[31:16] | (b[31:16] >> 16))

  148. static inline uint32_t pack_16t_16t(int32_t a, int32_t b) __attribute__((always_inline, unused));

  149. static inline uint32_t pack_16t_16t(int32_t a, int32_t b)

  150. {

  151. #if defined(KINETISK)

  152. 	int32_t out;

  153. 	asm volatile("pkhtb %0, %1, %2, asr #16" : "=r" (out) : "r" (a), "r" (b));

  154. 	return out;

  155. #elif defined(KINETISL)

  156. 	return (a & 0xFFFF0000) | ((uint32_t)b >> 16);

  157. #endif

  158. }

  159. 

  160. // computes (a[31:16] | b[15:0])

  161. static inline uint32_t pack_16t_16b(int32_t a, int32_t b) __attribute__((always_inline, unused));

  162. static inline uint32_t pack_16t_16b(int32_t a, int32_t b)

  163. {

  164. #if defined(KINETISK)

  165. 	int32_t out;

  166. 	asm volatile("pkhtb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  167. 	return out;

  168. #elif defined(KINETISL)

  169. 	return (a & 0xFFFF0000) | (b & 0x0000FFFF);

  170. #endif

  171. }

  172. 

  173. // computes ((a[15:0] << 16) | b[15:0])

  174. static inline uint32_t pack_16b_16b(int32_t a, int32_t b) __attribute__((always_inline, unused));

  175. static inline uint32_t pack_16b_16b(int32_t a, int32_t b)

  176. {

  177. #if defined(KINETISK)

  178. 	int32_t out;

  179. 	asm volatile("pkhbt %0, %1, %2, lsl #16" : "=r" (out) : "r" (b), "r" (a));

  180. 	return out;

  181. #elif defined(KINETISL)

  182. 	return (a << 16) | (b & 0x0000FFFF);

  183. #endif

  184. }

  185. 

  186. // computes ((a[15:0] << 16) | b[15:0])

  187. /*

  188. static inline uint32_t pack_16x16(int32_t a, int32_t b) __attribute__((always_inline, unused));

  189. static inline uint32_t pack_16x16(int32_t a, int32_t b)

  190. {

  191. 	int32_t out;

  192. 	asm volatile("pkhbt %0, %1, %2, lsl #16" : "=r" (out) : "r" (b), "r" (a));

  193. 	return out;

  194. }

  195. */

  196. 

  197. // computes (((a[31:16] + b[31:16]) << 16) | (a[15:0 + b[15:0]))  (saturates)

  198. static inline uint32_t signed_add_16_and_16(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  199. static inline uint32_t signed_add_16_and_16(uint32_t a, uint32_t b)

  200. {

  201. 	int32_t out;

  202. 	asm volatile("qadd16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  203. 	return out;

  204. }

  205. 

  206. // computes (((a[31:16] - b[31:16]) << 16) | (a[15:0 - b[15:0]))  (saturates)

  207. static inline int32_t signed_subtract_16_and_16(int32_t a, int32_t b) __attribute__((always_inline, unused));

  208. static inline int32_t signed_subtract_16_and_16(int32_t a, int32_t b)

  209. {

  210. 	int32_t out;

  211. 	asm volatile("qsub16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  212. 	return out;

  213. }

  214. 

  215. // computes out = (((a[31:16]+b[31:16])/2) <<16) | ((a[15:0]+b[15:0])/2)

  216. static inline int32_t signed_halving_add_16_and_16(int32_t a, int32_t b) __attribute__((always_inline, unused));

  217. static inline int32_t signed_halving_add_16_and_16(int32_t a, int32_t b)

  218. {

  219. 	int32_t out;

  220. 	asm volatile("shadd16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  221. 	return out;

  222. }

  223. 

  224. // computes out = (((a[31:16]-b[31:16])/2) <<16) | ((a[15:0]-b[15:0])/2)

  225. static inline int32_t signed_halving_subtract_16_and_16(int32_t a, int32_t b) __attribute__((always_inline, unused));

  226. static inline int32_t signed_halving_subtract_16_and_16(int32_t a, int32_t b)

  227. {

  228. 	int32_t out;

  229. 	asm volatile("shsub16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  230. 	return out;

  231. }

  232. 

  233. // computes (sum + ((a[31:0] * b[15:0]) >> 16))

  234. static inline int32_t signed_multiply_accumulate_32x16b(int32_t sum, int32_t a, uint32_t b) __attribute__((always_inline, unused));

  235. static inline int32_t signed_multiply_accumulate_32x16b(int32_t sum, int32_t a, uint32_t b)

  236. {

  237. 	int32_t out;

  238. 	asm volatile("smlawb %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));

  239. 	return out;

  240. }

  241. 

  242. // computes (sum + ((a[31:0] * b[31:16]) >> 16))

  243. static inline int32_t signed_multiply_accumulate_32x16t(int32_t sum, int32_t a, uint32_t b) __attribute__((always_inline, unused));

  244. static inline int32_t signed_multiply_accumulate_32x16t(int32_t sum, int32_t a, uint32_t b)

  245. {

  246. 	int32_t out;

  247. 	asm volatile("smlawt %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));

  248. 	return out;

  249. }

  250. 

  251. // computes logical and, forces compiler to allocate register and use single cycle instruction

  252. static inline uint32_t logical_and(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  253. static inline uint32_t logical_and(uint32_t a, uint32_t b)

  254. {

  255. 	asm volatile("and %0, %1" : "+r" (a) : "r" (b));

  256. 	return a;

  257. }

  258. 

  259. // computes ((a[15:0] * b[15:0]) + (a[31:16] * b[31:16]))

  260. static inline int32_t multiply_16tx16t_add_16bx16b(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  261. static inline int32_t multiply_16tx16t_add_16bx16b(uint32_t a, uint32_t b)

  262. {

  263. 	int32_t out;

  264. 	asm volatile("smuad %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  265. 	return out;

  266. }

  267. 

  268. // computes ((a[15:0] * b[31:16]) + (a[31:16] * b[15:0]))

  269. static inline int32_t multiply_16tx16b_add_16bx16t(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  270. static inline int32_t multiply_16tx16b_add_16bx16t(uint32_t a, uint32_t b)

  271. {

  272. 	int32_t out;

  273. 	asm volatile("smuadx %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  274. 	return out;

  275. }

  276. 

  277. // // computes sum += ((a[15:0] * b[15:0]) + (a[31:16] * b[31:16]))

  278. static inline int64_t multiply_accumulate_16tx16t_add_16bx16b(int64_t sum, uint32_t a, uint32_t b)

  279. {

  280. 	asm volatile("smlald %Q0, %R0, %1, %2" : "+r" (sum) : "r" (a), "r" (b));

  281. 	return sum;

  282. }

  283. 

  284. // // computes sum += ((a[15:0] * b[31:16]) + (a[31:16] * b[15:0]))

  285. static inline int64_t multiply_accumulate_16tx16b_add_16bx16t(int64_t sum, uint32_t a, uint32_t b)

  286. {

  287. 	asm volatile("smlaldx %Q0, %R0, %1, %2" : "+r" (sum) : "r" (a), "r" (b));

  288. 	return sum;

  289. }

  290. 

  291. // computes ((a[15:0] * b[15:0])

  292. static inline int32_t multiply_16bx16b(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  293. static inline int32_t multiply_16bx16b(uint32_t a, uint32_t b)

  294. {

  295. 	int32_t out;

  296. 	asm volatile("smulbb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  297. 	return out;

  298. }

  299. 

  300. // computes ((a[15:0] * b[31:16])

  301. static inline int32_t multiply_16bx16t(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  302. static inline int32_t multiply_16bx16t(uint32_t a, uint32_t b)

  303. {

  304. 	int32_t out;

  305. 	asm volatile("smulbt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  306. 	return out;

  307. }

  308. 

  309. // computes ((a[31:16] * b[15:0])

  310. static inline int32_t multiply_16tx16b(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  311. static inline int32_t multiply_16tx16b(uint32_t a, uint32_t b)

  312. {

  313. 	int32_t out;

  314. 	asm volatile("smultb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  315. 	return out;

  316. }

  317. 

  318. // computes ((a[31:16] * b[31:16])

  319. static inline int32_t multiply_16tx16t(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  320. static inline int32_t multiply_16tx16t(uint32_t a, uint32_t b)

  321. {

  322. 	int32_t out;

  323. 	asm volatile("smultt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  324. 	return out;

  325. }

  326. 

  327. // computes (a - b), result saturated to 32 bit integer range

  328. static inline int32_t substract_32_saturate(uint32_t a, uint32_t b) __attribute__((always_inline, unused));

  329. static inline int32_t substract_32_saturate(uint32_t a, uint32_t b)

  330. {

  331. 	int32_t out;

  332. 	asm volatile("qsub %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));

  333. 	return out;

  334. }

  335. 

  336. //get Q from PSR

  337. static inline uint32_t get_q_psr(void) __attribute__((always_inline, unused));

  338. static inline uint32_t get_q_psr(void)

  339. {

  340.   uint32_t out;

  341.   asm ("mrs %0, APSR" : "=r" (out));

  342.   return (out & 0x8000000)>>27;

  343. }

  344. 

  345. //clear Q BIT in PSR

  346. static inline void clr_q_psr(void) __attribute__((always_inline, unused));

  347. static inline void clr_q_psr(void)

  348. {

  349.   uint32_t t;

  350.   asm ("mov %[t],#0\n"

  351.        "msr APSR_nzcvq,%0\n" : [t] "=&r" (t)::"cc");

  352. }

  353. 

  354. // Multiply two S.31 fractional integers, and return the 32 most significant

  355. // bits after a shift left by the constant z.

  356. // This comes from rockbox.org

  357. 

  358. static inline int32_t FRACMUL_SHL(int32_t x, int32_t y, int z)

  359. {

  360.     int32_t t, t2;

  361.     asm ("smull    %[t], %[t2], %[a], %[b]\n\t"

  362.          "mov      %[t2], %[t2], asl %[c]\n\t"

  363.          "orr      %[t], %[t2], %[t], lsr %[d]\n\t"

  364.          : [t] "=&r" (t), [t2] "=&r" (t2)

  365.          : [a] "r" (x), [b] "r" (y),

  366.            [c] "Mr" ((z) + 1), [d] "Mr" (31 - (z)));

  367.     return t;

  368. }

  369. 

  370. #endif