1 files changed, 159 insertions, 0 deletions

diff --git a/lib/chibios-contrib/os/hal/ports/NUMICRO/LLD/USBv1/usb_memcpy.S b/lib/chibios-contrib/os/hal/ports/NUMICRO/LLD/USBv1/usb_memcpy.S
new file mode 100644
index 000000000..7b4958a1f
--- /dev/null
+++ b/lib/chibios-contrib/os/hal/ports/NUMICRO/LLD/USBv1/usb_memcpy.S
@@ -0,0 +1,159 @@
+/*
+    ChibiOS - Copyright (C) 2020 Alex Lewontin
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    usb_memcpy.S
+ * @brief   Definition of the usb_memcpy function
+ *
+ * @addtogroup USB
+ * @{
+ */
+
+#if !defined(__DOXYGEN__)
+
+    .syntax     unified
+    .cpu        cortex-m0
+    .thumb
+
+    .section    .text, "ax"
+    .align      4
+    .global     usb_memcpy
+
+/* size_t usb_memcpy(volatile void* destination, const volatile void* source, size_t num); */
+usb_memcpy:
+  cmp     r2, #0
+  beq.n   zero_return
+  push    {r2, r4, r5, r6, r7, lr}
+
+  movs    r3, #3
+
+check_alignment:
+  tst     r0, r3
+  bne.n   offset
+  tst     r1, r3
+  bne.n   unaligned
+
+aligned:
+/* r0: dest, r1: src, r2: num, r3: #3 */
+  movs    r4, r2
+
+  bics    r4, r3
+  movs    r6, r4
+
+  b.n     aligned_loop_check
+aligned_loop_top:
+  subs    r4, r4, #4
+  ldr     r5, [r1, r4]
+  str     r5, [r0, r4]
+aligned_loop_check:
+  bne.n   aligned_loop_top
+
+
+leftover_check:
+  ands    r2, r3
+  beq.n   return
+
+/* if there are leftovers, we can read them all in one */
+
+/* byte mode */
+  ldr     r4, =0x40060000
+  ldr     r5, [r4, #16]
+  movs    r7, #128 
+  lsls    r7, r7, #3 /* USBD_ATTR_BYTEM_Msk */
+  orrs    r5, r7
+  str     r5, [r4, #16]
+  adds    r1, r1, r6
+  adds    r0, r0, r6
+
+/*
+ * r0: dest
+ * r1: src
+ * r2 is 1, 2, or 3 (# of bytes left)
+ * r3: #3
+ * r4: &USBD
+ * r5: USBD->ATTR
+ * r6: leftover offset base
+ * r7: USBD_ATTR_BYTEM_Msk
+ */
+  ldrb    r3, [r1, #0]
+  strb    r3, [r0, #0]
+
+  cmp     r2, #1
+  beq.n   word_mode_return
+
+  ldrb    r3, [r1, #1]
+  strb    r3, [r0, #1]
+
+  cmp     r2, #2
+  beq.n   word_mode_return
+
+  ldrb    r3, [r1, #2]
+  strb    r3, [r0, #2]
+
+word_mode_return:
+  ldr     r5, [r4, #16]
+  bics    r5, r7
+  str     r5, [r4, #16]
+return:
+  pop     {r0, r4, r5, r6, r7, pc}
+zero_return:
+  movs    r0, #0
+  bx.n    lr
+
+offset:
+  ldr     r4, =0x40060000
+  ldr     r5, [r4, #16]
+  movs    r7, #128 
+  lsls    r7, r7, #3 /* USBD_ATTR_BYTEM_Msk */
+  orrs    r5, r7
+  str     r5, [r4, #16]
+
+offset_loop:
+  ldrb r6, [r1, #0]
+  strb r6, [r0, #0]
+  adds r0, r0, #1
+  adds r1, r1, #1
+  subs r2, r2, #1
+
+  beq.n   word_mode_return
+  tst     r0, r3
+  bne.n   offset_loop
+  tst     r1, r3
+  bne.n   unaligned_loop_top
+
+  ldr     r5, [r4, #16]
+  bics    r5, r7
+  str     r5, [r4, #16]
+  b.n     aligned
+
+unaligned:
+  ldr     r4, =0x40060000
+  ldr     r5, [r4, #16]
+  movs    r7, #128 
+  lsls    r7, r7, #3 /* USBD_ATTR_BYTEM_Msk */
+  orrs    r5, r7
+  str     r5, [r4, #16]
+
+unaligned_loop_top:
+  subs    r2, r2, #1
+  ldrb    r5, [r1, r2]
+  strb    r5, [r0, r2]
+unaligned_loop_check:
+  bne.n   unaligned_loop_top
+
+  b.n     word_mode_return
+
+#endif