3 files changed, 67 insertions, 0 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index d48d3c623a..817574da83 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -985,6 +985,7 @@ target/arm/imx31/gigabeat-s/timer-gigabeat-s.c
 #endif /* GIGABEAT_S */
 
 #if CONFIG_CPU == DM320
+target/arm/bits-armv4.S
 target/arm/tms320dm320/debug-dm320.c
 target/arm/tms320dm320/dsp-dm320.c
 target/arm/tms320dm320/i2c-dm320.c
@@ -998,14 +999,18 @@ target/arm/tms320dm320/uart-dm320.c
 #ifdef MROBE_500
 #ifndef SIMULATOR
 target/arm/ata-as-arm.S
+#if !defined(LCD_USE_DMA)
 target/arm/lcd-as-memframe.S
+#endif
 target/arm/mmu-arm.S
+target/arm/tms320dm320/mrobe-500/crt0-board.S
 target/arm/tms320dm320/mrobe-500/adc-mr500.c
 target/arm/tms320dm320/mrobe-500/ata-mr500.c
 target/arm/tms320dm320/mrobe-500/backlight-mr500.c
 target/arm/tms320dm320/mrobe-500/button-mr500.c
 target/arm/tms320dm320/mrobe-500/dm320codec-mr500.c
 target/arm/tms320dm320/mrobe-500/lcd-mr500.c
+target/arm/tms320dm320/mrobe-500/lcd-as-mr500.S
 #if defined(HAVE_REMOTE_LCD)
 target/arm/tms320dm320/mrobe-500/lcd-remote-mr500.c
 #endif
diff --git a/firmware/export/system.h b/firmware/export/system.h
index b246d154a4..65db721bb4 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -152,6 +152,10 @@ uint32_t isolate_first_bit(uint32_t val)
     { return val & -val; }
 
 /* Functions to set and clear register or variable bits atomically */
+void bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask);
+void bitset16(volatile uint16_t *addr, uint16_t mask);
+void bitclr16(volatile uint16_t *addr, uint16_t mask);
+
 void bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask);
 void bitset32(volatile uint32_t *addr, uint32_t mask);
 void bitclr32(volatile uint32_t *addr, uint32_t mask);
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
+