You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

213 lines
4.1KB

  1. /* Teensyduino Audio Memcpy
  2. * Copyright (c) 2016 Frank Bösing
  3. *
  4. * Permission is hereby granted, free of charge, to any person obtaining
  5. * a copy of this software and associated documentation files (the
  6. * "Software"), to deal in the Software without restriction, including
  7. * without limitation the rights to use, copy, modify, merge, publish,
  8. * distribute, sublicense, and/or sell copies of the Software, and to
  9. * permit persons to whom the Software is furnished to do so, subject to
  10. * the following conditions:
  11. *
  12. * 1. The above copyright notice and this permission notice shall be
  13. * included in all copies or substantial portions of the Software.
  14. *
  15. * 2. If the Software is incorporated into a build system that allows
  16. * selection among a list of target devices, then similar target
  17. * devices manufactured by PJRC.COM must be included in the list of
  18. * target devices and selectable in the same manner.
  19. *
  20. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  21. * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  22. * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  23. * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
  24. * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
  25. * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  26. * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  27. * SOFTWARE.
  28. */
  29. #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
  30. #include <AudioStream.h>
  31. .cpu cortex-m4
  32. .syntax unified
  33. .thumb
  34. .text
  35. .align 2
  36. /* void memcpy_tointerleave(short *dst, short *srcL, short *srcR); */
  37. .global memcpy_tointerleaveLR
  38. .thumb_func
  39. memcpy_tointerleaveLR:
  40. @ r0: dst
  41. @ r1: srcL
  42. @ r2: srcR
  43. push {r4-r11,r14}
  44. add r14,r0,#(AUDIO_BLOCK_SAMPLES*2)
  45. .align 2
  46. .loopLR:
  47. .irp offset, 1,2
  48. //Load 2*4 words
  49. ldmia r1!, {r5,r7,r9,r11} //1+4
  50. ldmia r2!, {r6,r8,r10,r12} //1+4
  51. pkhbt r3,r5,r6,LSL #16 //1
  52. pkhtb r4,r6,r5,ASR #16 //1
  53. pkhbt r5,r7,r8,LSL #16 //1
  54. pkhtb r6,r8,r7,ASR #16 //1
  55. pkhbt r7,r9,r10,LSL #16 //1
  56. pkhtb r8,r10,r9,ASR #16 //1
  57. pkhbt r9,r11,r12,LSL #16 //1
  58. pkhtb r10,r12,r11,ASR #16 //1
  59. //Write 8 Words
  60. stmia r0!, {r3,r4,r5,r6,r7,r8,r9,r10} //1+8
  61. .endr //5+5+8+9 = 27 Cycles to interleave 32 bytes.
  62. cmp r14, r0
  63. bne .loopLR
  64. pop {r4-r11,r14}
  65. BX lr
  66. /* void memcpy_tointerleaveL(short *dst, short *srcL); */
  67. .global memcpy_tointerleaveL
  68. .thumb_func
  69. memcpy_tointerleaveL:
  70. @ r0: dst
  71. @ r1: srcL
  72. push {r4-r11}
  73. mov r2, #0
  74. add r12,r0,#(AUDIO_BLOCK_SAMPLES*2)
  75. .align 2
  76. .loopL:
  77. .irp offset, 1,2
  78. //Load 4 words
  79. ldmia r1!, {r5,r7,r9,r11} //1+4
  80. pkhbt r3,r5,r2 //1
  81. pkhtb r4,r2,r5,ASR #16 //1
  82. pkhbt r5,r7,r2 //1
  83. pkhtb r6,r2,r7,ASR #16 //1
  84. pkhbt r7,r9,r2 //1
  85. pkhtb r8,r2,r9,ASR #16 //1
  86. pkhbt r9,r11,r2 //1
  87. pkhtb r10,r2,r11,ASR #16 //1
  88. //Write 8 Words
  89. stmia r0!, {r3,r4,r5,r6,r7,r8,r9,r10} //1+8
  90. .endr
  91. cmp r12, r0
  92. bne .loopL
  93. pop {r4-r11}
  94. BX lr
  95. /* void memcpy_tointerleaveL(short *dst, short *srcR); */
  96. .global memcpy_tointerleaveR
  97. .thumb_func
  98. memcpy_tointerleaveR:
  99. @ r0: dst
  100. @ r1: srcR
  101. push {r4-r11}
  102. mov r2, #0
  103. add r12,r0,#(AUDIO_BLOCK_SAMPLES*2)
  104. .align 2
  105. .loopR:
  106. .irp offset, 1,2
  107. //Load 4 words
  108. ldmia r1!, {r5,r7,r9,r11}
  109. pkhbt r3,r2,r5,LSL #16
  110. pkhtb r4,r5,r2
  111. pkhbt r5,r2,r7,LSL #16
  112. pkhtb r6,r7,r2
  113. pkhbt r7,r2,r9,LSL #16
  114. pkhtb r8,r9,r2
  115. pkhbt r9,r2,r11,LSL #16
  116. pkhtb r10,r11,r2
  117. //Write 8 Words
  118. stmia r0!, {r3,r4,r5,r6,r7,r8,r9,r10}
  119. .endr
  120. cmp r12, r0
  121. bne .loopR
  122. pop {r4-r11}
  123. BX lr
  124. /* void memcpy_tointerleaveQuad(int16_t *dst, const int16_t *src1, const int16_t *src2, const int16_t *src3, const int16_t *src4) */
  125. .global memcpy_tointerleaveQuad
  126. .thumb_func
  127. memcpy_tointerleaveQuad:
  128. @ r0: dst
  129. @ r1: src1
  130. @ r2: src2
  131. @ r3: src3
  132. @ r4: src4
  133. push {r4-r11}
  134. ldr r4, [sp, #(0+32)] //5th parameter is saved on the stack
  135. add r11,r0,#(AUDIO_BLOCK_SAMPLES*4)
  136. .align 2
  137. .loopQuad:
  138. .irp offset, 1,2
  139. ldr r5, [r1],4
  140. ldr r6, [r3],4
  141. pkhbt r7,r5,r6,LSL #16
  142. pkhtb r9,r6,r5,ASR #16
  143. ldr r5, [r2],4
  144. ldr r6, [r4],4
  145. pkhbt r8,r5,r6,LSL #16
  146. pkhtb r10,r6,r5,ASR #16
  147. stmia r0!, {r7-r10}
  148. .endr
  149. cmp r11, r0
  150. bne .loopQuad
  151. pop {r4-r11}
  152. BX lr
  153. .END
  154. #endif