From 8e8f700842f7b9737bfd62651372db0656cfa52d Mon Sep 17 00:00:00 2001
From: Karl Kurbjun <kkurbjun@gmail.com>
Date: Sun, 6 Feb 2011 19:43:45 +0000
Subject: Add ARMv4 16-bit(set, mod, clr) operations used on DM320.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@29221 a1c6a512-1295-4272-9138-f99709370657
---
 firmware/target/arm/bits-armv4.S | 58 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)

(limited to 'firmware/target')

diff --git a/firmware/target/arm/bits-armv4.S b/firmware/target/arm/bits-armv4.S
index 05d61b8b7e..542e362f77 100644
--- a/firmware/target/arm/bits-armv4.S
+++ b/firmware/target/arm/bits-armv4.S
@@ -75,3 +75,61 @@ bitclr32:
     msr    cpsr_c, r12
     bx     lr
     .size  bitclr32, .-bitclr32
+
+/***************************************************************************
+ * void bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitmod16
+    .type       bitmod16, %function
+bitmod16:
+    mrs    r12, cpsr
+    orr    r3, r12, #0xc0
+    msr    cpsr_c, r3
+    ldrh   r3, [r0]
+    and    r1, r1, r2               @ Only allow mod of bits in 'mask'
+    bic    r3, r3, r2               @ Clear mask bits
+    orr    r3, r3, r1               @ Set according to 'bits'
+    strh   r3, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitmod16, .-bitmod16
+
+/***************************************************************************
+ * void bitset16(volatile uint16_t *addr, uint16_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitset16
+    .type       bitset16, %function
+bitset16:
+    mrs    r12, cpsr
+    orr    r2, r12, #0xc0
+    msr    cpsr_c, r2
+    ldrh   r2, [r0]
+    orr    r2, r2, r1
+    strh   r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitset16, .-bitset16
+
+
+/***************************************************************************
+ * void bitclr16(volatile uint16_t *addr, uint16_t mask)
+ */
+     .section    .text, "ax", %progbits
+     .align      2
+     .global     bitclr16
+     .type       bitclr16, %function
+bitclr16:
+    mrs    r12, cpsr
+    orr    r2, r12, #0xc0
+    msr    cpsr_c, r2
+    ldrh   r2, [r0]
+    bic    r2, r2, r1
+    strh   r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitclr16, .-bitclr16
+
-- 
cgit v1.2.3