Remove atomic register bit manipulation functions from i.MX and s3c target code and introduce generic functions for ARM (bitmod32, bitset32, and bitclr32). Multiprocessor support is possible but just not implemented at the moment, only interrupt lockout.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@27188 a1c6a512-1295-4272-9138-f99709370657
diff --git a/firmware/SOURCES b/firmware/SOURCES
index 53c38e0..ea309a1 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -893,6 +893,7 @@
 
 #ifdef GIGABEAT_F
 #ifndef SIMULATOR
+target/arm/bits-armv4.S
 target/arm/lcd-as-memframe.S
 target/arm/mmu-arm.S
 target/arm/s3c2440/adc-s3c2440.c
@@ -918,6 +919,7 @@
 
 #ifdef GIGABEAT_S
 #ifndef SIMULATOR
+target/arm/bits-armv6.S
 target/arm/lcd-as-memframe.S
 target/arm/mmu-armv6.S
 target/arm/imx31/ata-imx31.c
@@ -1553,6 +1555,7 @@
 
 #if defined(MINI2440)
 #ifndef SIMULATOR
+target/arm/bits-armv4.S
 target/arm/lcd-as-memframe.S
 target/arm/mmu-arm.S
 target/arm/s3c2440/debug-s3c2440.c
diff --git a/firmware/export/system.h b/firmware/export/system.h
index fe8121c..ee668c8 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -214,6 +214,11 @@
 uint32_t isolate_first_bit(uint32_t val)
     { return val & -val; }
 
+/* Functions to set and clear register or variable bits atomically */
+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);
+
 /* gcc 3.4 changed the format of the constraints */
 #if (__GNUC__ >= 3) && (__GNUC_MINOR__ > 3) || (__GNUC__ >= 4)
 #define I_CONSTRAINT "I08"
diff --git a/firmware/target/arm/bits-armv4.S b/firmware/target/arm/bits-armv4.S
new file mode 100644
index 0000000..05d61b8
--- /dev/null
+++ b/firmware/target/arm/bits-armv4.S
@@ -0,0 +1,77 @@
+/***************************************************************************
+*             __________               __   ___.
+*   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+*   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+*   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+*   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+*                     \/            \/     \/    \/            \/
+* $Id$
+*
+* Copyright (C) 2010 by Michael Sevakis
+*
+* This program is free software; you can redistribute it and/or
+* modify it under the terms of the GNU General Public License
+* as published by the Free Software Foundation; either version 2
+* of the License, or (at your option) any later version.
+*
+* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+* KIND, either express or implied.
+*
+****************************************************************************/
+
+/***************************************************************************
+ * void bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitmod32
+    .type       bitmod32, %function
+bitmod32:
+    mrs    r12, cpsr
+    orr    r3, r12, #0xc0
+    msr    cpsr_c, r3
+    ldr    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'
+    str    r3, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitmod32, .-bitmod32
+
+/***************************************************************************
+ * void bitset32(volatile uint32_t *addr, uint32_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitset32
+    .type       bitset32, %function
+bitset32:
+    mrs    r12, cpsr
+    orr    r2, r12, #0xc0
+    msr    cpsr_c, r2
+    ldr    r2, [r0]
+    orr    r2, r2, r1
+    str    r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitset32, .-bitset32
+
+
+/***************************************************************************
+ * void bitclr32(volatile uint32_t *addr, uint32_t mask)
+ */
+     .section    .text, "ax", %progbits
+     .align      2
+     .global     bitclr32
+     .type       bitclr32, %function
+bitclr32:
+    mrs    r12, cpsr
+    orr    r2, r12, #0xc0
+    msr    cpsr_c, r2
+    ldr    r2, [r0]
+    bic    r2, r2, r1
+    str    r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitclr32, .-bitclr32
diff --git a/firmware/target/arm/bits-armv6.S b/firmware/target/arm/bits-armv6.S
new file mode 100644
index 0000000..0a15805
--- /dev/null
+++ b/firmware/target/arm/bits-armv6.S
@@ -0,0 +1,74 @@
+/***************************************************************************
+*             __________               __   ___.
+*   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+*   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+*   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+*   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+*                     \/            \/     \/    \/            \/
+* $Id$
+*
+* Copyright (C) 2010 by Michael Sevakis
+*
+* This program is free software; you can redistribute it and/or
+* modify it under the terms of the GNU General Public License
+* as published by the Free Software Foundation; either version 2
+* of the License, or (at your option) any later version.
+*
+* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+* KIND, either express or implied.
+*
+****************************************************************************/
+
+/***************************************************************************
+ * void bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitmod32
+    .type       bitmod32, %function
+bitmod32:
+    mrs    r12, cpsr
+    cpsid  if
+    ldr    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'
+    str    r3, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitmod32, .-bitmod32
+
+/***************************************************************************
+ * void bitset32(volatile uint32_t *addr, uint32_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitset32
+    .type       bitset32, %function
+bitset32:
+    mrs    r12, cpsr
+    cpsid  if
+    ldr    r2, [r0]
+    orr    r2, r2, r1
+    str    r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitset32, .-bitset32
+
+
+/***************************************************************************
+ * void bitclr32(volatile uint32_t *addr, uint32_t mask)
+ */
+     .section    .text, "ax", %progbits
+     .align      2
+     .global     bitclr32
+     .type       bitclr32, %function
+bitclr32:
+    mrs    r12, cpsr
+    cpsid  if
+    ldr    r2, [r0]
+    bic    r2, r2, r1
+    str    r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitclr32, .-bitclr32
diff --git a/firmware/target/arm/imx31/ccm-imx31.c b/firmware/target/arm/imx31/ccm-imx31.c
index 2cf2080..00a7d85 100644
--- a/firmware/target/arm/imx31/ccm-imx31.c
+++ b/firmware/target/arm/imx31/ccm-imx31.c
@@ -43,7 +43,7 @@
     shift = 2*(cg % 16);        /* Get field shift */
     mask = CG_MASK << shift;    /* Select field */
 
-    imx31_regmod32(reg, mode << shift, mask);
+    bitmod32(reg, mode << shift, mask);
 }
 
 /* Decode PLL output frequency from register value */
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.c b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
index 6bacc20..aa8d0f5 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.c
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
@@ -275,7 +275,7 @@
 
 #ifndef DVFS_NO_PWRRDY
     /* Configure PWRRDY signal pin. */
-    imx31_regclr32(&GPIO1_GDIR, (1 << 5));
+    bitclr32(&GPIO1_GDIR, (1 << 5));
     iomuxc_set_pin_mux(IOMUXC_GPIO1_5,
                        IOMUXC_MUX_OUT_FUNCTIONAL | IOMUXC_MUX_IN_FUNCTIONAL);
 #endif
@@ -289,27 +289,26 @@
     }
 
     /* Set up LTR0. */
-    imx31_regmod32(&CCM_LTR0,
-                   DVFS_UPTHR << CCM_LTR0_UPTHR_POS |
-                   DVFS_DNTHR << CCM_LTR0_DNTHR_POS |
-                   DVFS_DIV3CK << CCM_LTR0_DIV3CK_POS,
-                   CCM_LTR0_UPTHR | CCM_LTR0_DNTHR | CCM_LTR0_DIV3CK);
+    bitmod32(&CCM_LTR0,
+             DVFS_UPTHR << CCM_LTR0_UPTHR_POS |
+             DVFS_DNTHR << CCM_LTR0_DNTHR_POS |
+             DVFS_DIV3CK << CCM_LTR0_DIV3CK_POS,
+             CCM_LTR0_UPTHR | CCM_LTR0_DNTHR | CCM_LTR0_DIV3CK);
 
     /* Set up LTR1. */
-    imx31_regmod32(&CCM_LTR1,
-                   DVFS_DNCNT << CCM_LTR1_DNCNT_POS |
-                   DVFS_UPCNT << CCM_LTR1_UPCNT_POS |
-                   DVFS_PNCTHR << CCM_LTR1_PNCTHR_POS |
-                   CCM_LTR1_LTBRSR,
-                   CCM_LTR1_DNCNT | CCM_LTR1_UPCNT |
-                   CCM_LTR1_PNCTHR | CCM_LTR1_LTBRSR);
+    bitmod32(&CCM_LTR1,
+             DVFS_DNCNT << CCM_LTR1_DNCNT_POS |
+             DVFS_UPCNT << CCM_LTR1_UPCNT_POS |
+             DVFS_PNCTHR << CCM_LTR1_PNCTHR_POS |
+             CCM_LTR1_LTBRSR,
+             CCM_LTR1_DNCNT | CCM_LTR1_UPCNT |
+             CCM_LTR1_PNCTHR | CCM_LTR1_LTBRSR);
 
     /* Set up LTR2-- EMA configuration. */
-    imx31_regmod32(&CCM_LTR2, DVFS_EMAC << CCM_LTR2_EMAC_POS,
-                   CCM_LTR2_EMAC);
+    bitmod32(&CCM_LTR2, DVFS_EMAC << CCM_LTR2_EMAC_POS, CCM_LTR2_EMAC);
 
     /* DVFS interrupt goes to MCU. Mask load buffer full interrupt. */
-    imx31_regset32(&CCM_PMCR0, CCM_PMCR0_DVFIS | CCM_PMCR0_LBMI);
+    bitset32(&CCM_PMCR0, CCM_PMCR0_DVFIS | CCM_PMCR0_LBMI);
 
     /* Initialize current core PLL and dividers for default level. Assumes
      * clocking scheme has been set up appropriately in other init code. */
@@ -517,8 +516,8 @@
 static void INIT_ATTR dptc_init(void)
 {
     /* Force DPTC off if running for some reason. */
-    imx31_regmod32(&CCM_PMCR0, CCM_PMCR0_PTVAIM,
-                   CCM_PMCR0_PTVAIM | CCM_PMCR0_DPTEN);
+    bitmod32(&CCM_PMCR0, CCM_PMCR0_PTVAIM,
+             CCM_PMCR0_PTVAIM | CCM_PMCR0_DPTEN);
 
      /* Shadow the regulator registers */
     mc13783_read_regs(dptc_pmic_regs, dptc_reg_shadows, 2);
@@ -528,14 +527,14 @@
 
     /* Interrupt goes to MCU, specified reference circuits enabled when
      * DPTC is active. */
-    imx31_regset32(&CCM_PMCR0, CCM_PMCR0_PTVIS);
+    bitset32(&CCM_PMCR0, CCM_PMCR0_PTVIS);
 
-    imx31_regmod32(&CCM_PMCR0, DPTC_DRCE_MASK,
-                   CCM_PMCR0_DRCE0 | CCM_PMCR0_DRCE1 |
-                   CCM_PMCR0_DRCE2 | CCM_PMCR0_DRCE3);
+    bitmod32(&CCM_PMCR0, DPTC_DRCE_MASK,
+             CCM_PMCR0_DRCE0 | CCM_PMCR0_DRCE1 |
+             CCM_PMCR0_DRCE2 | CCM_PMCR0_DRCE3);
 
     /* DPTC counting range = 256 system clocks */
-    imx31_regclr32(&CCM_PMCR0, CCM_PMCR0_DCR);
+    bitclr32(&CCM_PMCR0, CCM_PMCR0_DCR);
 
     logf("DPTC: Initialized");
 }
@@ -629,7 +628,7 @@
         shift -= 16; /* Bits 13:11, 16:14 ... 31:29 */
     }
 
-    imx31_regmod32(reg_p, value << shift, 0x7 << shift);
+    bitmod32(reg_p, value << shift, 0x7 << shift);
 }
 
 
@@ -643,7 +642,7 @@
     else if ((unsigned)index < 16)
         bit = 1ul << (index + 29);
 
-    imx31_regmod32(&CCM_LTR0, edge ? bit : 0, bit);
+    bitmod32(&CCM_LTR0, edge ? bit : 0, bit);
 }
 
 
@@ -652,7 +651,7 @@
     if ((unsigned)dvgp <= 3)
     {
         unsigned long bit = 1ul << dvgp;
-        imx31_regmod32(&CCM_PMCR1, assert ? bit : 0, bit);
+        bitmod32(&CCM_PMCR1, assert ? bit : 0, bit);
     }
 }
 
@@ -660,8 +659,7 @@
 /* Turn the wait-for-interrupt monitoring on or off */
 void dvfs_wfi_monitor(bool on)
 {
-    imx31_regmod32(&CCM_PMCR0, on ? 0 : CCM_PMCR0_WFIM,
-                   CCM_PMCR0_WFIM);
+    bitmod32(&CCM_PMCR0, on ? 0 : CCM_PMCR0_WFIM, CCM_PMCR0_WFIM);
 }
 
 
diff --git a/firmware/target/arm/imx31/gigabeat-s/fmradio-i2c-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/fmradio-i2c-gigabeat-s.c
index f720921..39a2cda 100644
--- a/firmware/target/arm/imx31/gigabeat-s/fmradio-i2c-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/fmradio-i2c-gigabeat-s.c
@@ -38,14 +38,14 @@
 void fmradio_i2c_init(void)
 {
     /* RST: LOW */
-    imx31_regclr32(&GPIO1_DR, (1 << 26));
+    bitclr32(&GPIO1_DR, (1 << 26));
     /* RST: OUT */
-    imx31_regset32(&GPIO1_GDIR, (1 << 26));
+    bitset32(&GPIO1_GDIR, (1 << 26));
 
     /* I2C2 SCL: IN, I2C2: SDA IN */
-    imx31_regclr32(&GPIO2_GDIR, (3 << 14));
+    bitclr32(&GPIO2_GDIR, (3 << 14));
     /* I2C2 SCL LO, I2C2 SDA LO */
-    imx31_regclr32(&GPIO2_DR, (3 << 14));
+    bitclr32(&GPIO2_DR, (3 << 14));
 
     /* open-drain pins - external pullups on PCB. Pullup default but
      * disabled */
@@ -73,17 +73,17 @@
     {
         /* place in GPIO mode to hold SDIO low during RESET release,
          * SEN1 should be high already (pullup) and GPIO3 left alone */
-        imx31_regset32(&GPIO2_GDIR, (1 << 15)); /* SDIO OUT */
+        bitset32(&GPIO2_GDIR, (1 << 15)); /* SDIO OUT */
         /* I2C2_SDA => MCU2_15 */ 
         iomuxc_set_pin_mux(IOMUXC_DCD_DTE1,
                            IOMUXC_MUX_OUT_GPIO | IOMUXC_MUX_IN_GPIO);
         /* enable CLK32KMCU clock */
         mc13783_set(MC13783_POWER_CONTROL0, MC13783_CLK32KMCUEN);
         /* enable the fm chip (release RESET) */
-        imx31_regset32(&GPIO1_DR, (1 << 26));
+        bitset32(&GPIO1_DR, (1 << 26));
         sleep(HZ/100);
         /* busmode should be selected - OK to release SDIO */
-        imx31_regclr32(&GPIO2_GDIR, (1 << 15)); /* SDIO IN */
+        bitclr32(&GPIO2_GDIR, (1 << 15)); /* SDIO IN */
         /* restore pin mux (MCU2_15 => I2C2_SDA) */
         iomuxc_set_pin_mux(IOMUXC_DCD_DTE1,
                            IOMUXC_MUX_OUT_ALT2 | IOMUXC_MUX_IN_ALT2);
@@ -97,7 +97,7 @@
            we can diable the i2c module when not in use */
         i2c_enable_node(&si4700_i2c_node, false);
         /* disable the fm chip */
-        imx31_regclr32(&GPIO1_DR, (1 << 26));
+        bitclr32(&GPIO1_DR, (1 << 26));
         /* disable CLK32KMCU clock */
         mc13783_clear(MC13783_POWER_CONTROL0, MC13783_CLK32KMCUEN);
     }
diff --git a/firmware/target/arm/imx31/gigabeat-s/i2s-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/i2s-gigabeat-s.c
index c2ec0d6..6291a9c 100644
--- a/firmware/target/arm/imx31/gigabeat-s/i2s-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/i2s-gigabeat-s.c
@@ -35,11 +35,11 @@
      * WM Codec post divider (MCLKDIV=1.5):
      * INT_BIT_CLK (MCLK) / 1.5 = 11289600Hz = 256*fs = SYSCLK
      */
-    imx31_regmod32(&CCM_PDR1,
-                   ((1-1) << CCM_PDR1_SSI1_PRE_PODF_POS) |
-                   ((5-1) << CCM_PDR1_SSI1_PODF_POS) |
-                   ((8-1) << CCM_PDR1_SSI2_PRE_PODF_POS) |
-                   ((64-1) << CCM_PDR1_SSI2_PODF_POS),
-                   CCM_PDR1_SSI1_PODF | CCM_PDR1_SSI2_PODF |
-                   CCM_PDR1_SSI1_PRE_PODF | CCM_PDR1_SSI2_PRE_PODF);
+    bitmod32(&CCM_PDR1,
+             ((1-1) << CCM_PDR1_SSI1_PRE_PODF_POS) |
+             ((5-1) << CCM_PDR1_SSI1_PODF_POS) |
+             ((8-1) << CCM_PDR1_SSI2_PRE_PODF_POS) |
+             ((64-1) << CCM_PDR1_SSI2_PODF_POS),
+             CCM_PDR1_SSI1_PODF | CCM_PDR1_SSI2_PODF |
+             CCM_PDR1_SSI1_PRE_PODF | CCM_PDR1_SSI2_PRE_PODF);
 }
diff --git a/firmware/target/arm/imx31/gigabeat-s/lcd-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/lcd-gigabeat-s.c
index cadd0e7..cd583ec 100644
--- a/firmware/target/arm/imx31/gigabeat-s/lcd-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/lcd-gigabeat-s.c
@@ -133,13 +133,13 @@
         lcd_powered = false;
         lcd_write_reg(0x04, 0x00);
         lcd_enable_interface(false);
-        imx31_regclr32(&GPIO3_DR, (1 << 12));
+        bitclr32(&GPIO3_DR, (1 << 12));
         mc13783_clear(MC13783_REGULATOR_MODE1, MC13783_VCAMEN);
     }
     else
     {
         mc13783_set(MC13783_REGULATOR_MODE1, MC13783_VCAMEN);
-        imx31_regset32(&GPIO3_DR, (1 << 12));
+        bitset32(&GPIO3_DR, (1 << 12));
         lcd_enable_interface(true);
         lcd_write_reg(0x04, 0x01);
         lcd_powered = true;
diff --git a/firmware/target/arm/imx31/gigabeat-s/pcm-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/pcm-gigabeat-s.c
index cfd83f0..15f9d0b 100644
--- a/firmware/target/arm/imx31/gigabeat-s/pcm-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/pcm-gigabeat-s.c
@@ -458,7 +458,7 @@
     /* Stop receiving data */
     sdma_channel_stop(DMA_REC_CH_NUM);
 
-    imx31_regclr32(&SSI_SIER1, SSI_SIER_RDMAE);
+    bitclr32(&SSI_SIER1, SSI_SIER_RDMAE);
 
     SSI_SCR1 &= ~SSI_SCR_RE;      /* Disable RX */
     SSI_SRCR1 &= ~SSI_SRCR_RFEN0; /* Disable RX FIFO */
diff --git a/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
index 4540be6..9d7d305 100644
--- a/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
@@ -85,17 +85,17 @@
     if (!on)
     {
         /* Bus must be isolated before power off */
-        imx31_regset32(&GPIO2_DR, (1 << 16));
+        bitset32(&GPIO2_DR, (1 << 16));
     }
 
     /* HD power switch */
-    imx31_regmod32(&GPIO3_DR, on ? (1 << 5) : 0, (1 << 5));
+    bitmod32(&GPIO3_DR, on ? (1 << 5) : 0, (1 << 5));
 
     if (on)
     {
         /* Bus switch may be turned on after powerup */
         sleep(HZ/10);
-        imx31_regclr32(&GPIO2_DR, (1 << 16));
+        bitclr32(&GPIO2_DR, (1 << 16));
     }
 }
 
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
index 80b6f22..f458561 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
@@ -186,18 +186,18 @@
     cpu_frequency = ccm_get_mcu_clk();
 
     /* MCR WFI enables wait mode (CCM_CCMR_LPM_WAIT_MODE = 0) */
-    imx31_regclr32(&CCM_CCMR, CCM_CCMR_LPM);
+    bitclr32(&CCM_CCMR, CCM_CCMR_LPM);
 
     iim_init();
 
-    imx31_regset32(&SDHC1_CLOCK_CONTROL, STOP_CLK);
-    imx31_regset32(&SDHC2_CLOCK_CONTROL, STOP_CLK);
-    imx31_regset32(&RNGA_CONTROL, RNGA_CONTROL_SLEEP);
-    imx31_regclr32(&UCR1_1, EUARTUCR1_UARTEN);
-    imx31_regclr32(&UCR1_2, EUARTUCR1_UARTEN);
-    imx31_regclr32(&UCR1_3, EUARTUCR1_UARTEN);
-    imx31_regclr32(&UCR1_4, EUARTUCR1_UARTEN);
-    imx31_regclr32(&UCR1_5, EUARTUCR1_UARTEN);
+    bitset32(&SDHC1_CLOCK_CONTROL, STOP_CLK);
+    bitset32(&SDHC2_CLOCK_CONTROL, STOP_CLK);
+    bitset32(&RNGA_CONTROL, RNGA_CONTROL_SLEEP);
+    bitclr32(&UCR1_1, EUARTUCR1_UARTEN);
+    bitclr32(&UCR1_2, EUARTUCR1_UARTEN);
+    bitclr32(&UCR1_3, EUARTUCR1_UARTEN);
+    bitclr32(&UCR1_4, EUARTUCR1_UARTEN);
+    bitclr32(&UCR1_5, EUARTUCR1_UARTEN);
 
     for (i = 0; i < ARRAYLEN(disable_clocks); i++)
         ccm_module_clock_gating(disable_clocks[i], CGM_OFF);
@@ -207,49 +207,6 @@
     gpio_init();
 }
 
-void  __attribute__((naked)) imx31_regmod32(volatile uint32_t *reg_p,
-                                            uint32_t value,
-                                            uint32_t mask)
-{
-    asm volatile("and    r1, r1, r2 \n"
-                 "mrs    ip, cpsr   \n"
-                 "cpsid  if         \n"
-                 "ldr    r3, [r0]   \n"
-                 "bic    r3, r3, r2 \n" 
-                 "orr    r3, r3, r1 \n"
-                 "str    r3, [r0]   \n"
-                 "msr    cpsr_c, ip \n"
-                 "bx     lr         \n");
-    (void)reg_p; (void)value; (void)mask;
-}
-
-void __attribute__((naked)) imx31_regset32(volatile uint32_t *reg_p,
-                                           uint32_t mask)
-{
-    asm volatile("mrs    r3, cpsr   \n"
-                 "cpsid  if         \n"
-                 "ldr    r2, [r0]   \n"
-                 "orr    r2, r2, r1 \n"
-                 "str    r2, [r0]   \n"
-                 "msr    cpsr_c, r3 \n"
-                 "bx     lr         \n");
-    (void)reg_p; (void)mask;
-}
-
-void __attribute__((naked)) imx31_regclr32(volatile uint32_t *reg_p,
-                                           uint32_t mask)
-{
-    asm volatile("mrs    r3, cpsr   \n"
-                 "cpsid  if         \n"
-                 "ldr    r2, [r0]   \n"
-                 "bic    r2, r2, r1 \n"
-                 "str    r2, [r0]   \n"
-                 "msr    cpsr_c, r3 \n"
-                 "bx     lr         \n");
-    (void)reg_p; (void)mask;
-}
-
-
 void system_prepare_fw_start(void)
 {
     dvfs_dptc_stop();
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-target.h b/firmware/target/arm/imx31/gigabeat-s/system-target.h
index af95471..a13f87a 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/system-target.h
@@ -51,11 +51,6 @@
 void kernel_device_init(void);
 void system_halt(void);
 
-void imx31_regmod32(volatile uint32_t *reg_p, uint32_t value,
-                    uint32_t mask);
-void imx31_regset32(volatile uint32_t *reg_p, uint32_t mask);
-void imx31_regclr32(volatile uint32_t *reg_p, uint32_t mask);
-
 #define KDEV_INIT
 
 struct ARM_REGS {
diff --git a/firmware/target/arm/imx31/gigabeat-s/usb-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/usb-gigabeat-s.c
index d873c19..016f24f 100644
--- a/firmware/target/arm/imx31/gigabeat-s/usb-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/usb-gigabeat-s.c
@@ -40,16 +40,16 @@
     {
         if (GPIO1_DR & (1 << 30))
         {
-            imx31_regclr32(&GPIO3_DR, (1 << 16)); /* Reset ISP1504 */
+            bitclr32(&GPIO3_DR, (1 << 16)); /* Reset ISP1504 */
             sleep(HZ/100);
-            imx31_regset32(&GPIO3_DR, (1 << 16));
+            bitset32(&GPIO3_DR, (1 << 16));
             sleep(HZ/10);
-            imx31_regclr32(&GPIO1_DR, (1 << 30)); /* Select ISP1504 */
+            bitclr32(&GPIO1_DR, (1 << 30)); /* Select ISP1504 */
         }
     }
     else
     {
-        imx31_regset32(&GPIO1_DR, (1 << 30)); /* Deselect ISP1504 */
+        bitset32(&GPIO1_DR, (1 << 30)); /* Deselect ISP1504 */
     }
 }
 
diff --git a/firmware/target/arm/imx31/gigabeat-s/wmcodec-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/wmcodec-gigabeat-s.c
index 96324cc..36ab33a 100644
--- a/firmware/target/arm/imx31/gigabeat-s/wmcodec-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/wmcodec-gigabeat-s.c
@@ -48,13 +48,13 @@
 
     audiohw_preinit();
 
-    imx31_regset32(&GPIO3_DR, (1 << 21)); /* Turn on analogue LDO */
+    bitset32(&GPIO3_DR, (1 << 21)); /* Turn on analogue LDO */
 }
 
 void audiohw_enable_headphone_jack(bool enable)
 {
     /* Turn headphone jack output on or off. */
-    imx31_regmod32(&GPIO3_DR, enable ? (1 << 22) : 0, (1 << 22));
+    bitmod32(&GPIO3_DR, enable ? (1 << 22) : 0, (1 << 22));
 }
 
 void wmcodec_write(int reg, int data)
diff --git a/firmware/target/arm/imx31/iomuxc-imx31.c b/firmware/target/arm/imx31/iomuxc-imx31.c
index 876b8b2..412693e 100644
--- a/firmware/target/arm/imx31/iomuxc-imx31.c
+++ b/firmware/target/arm/imx31/iomuxc-imx31.c
@@ -32,8 +32,8 @@
     unsigned long index = pin / 4;
     unsigned int shift = 8*(pin % 4);
 
-    imx31_regmod32((unsigned long *)(IOMUXC_BASE_ADDR + 0xc) + index,
-                   mux << shift, IOMUXC_MUX_MASK << shift);
+    bitmod32((unsigned long *)(IOMUXC_BASE_ADDR + 0xc) + index,
+              mux << shift, IOMUXC_MUX_MASK << shift);
 }
 
 
@@ -45,6 +45,6 @@
     unsigned long index = padoffs / 3;
     unsigned int shift = 10*(padoffs % 3);
 
-    imx31_regmod32((unsigned long *)(IOMUXC_BASE_ADDR + 0x154) + index,
-                   config << shift, IOMUXC_PAD_MASK << shift);
+    bitmod32((unsigned long *)(IOMUXC_BASE_ADDR + 0x154) + index,
+             config << shift, IOMUXC_PAD_MASK << shift);
 }
diff --git a/firmware/target/arm/imx31/mc13783-imx31.c b/firmware/target/arm/imx31/mc13783-imx31.c
index 6e982af..b236dcd 100644
--- a/firmware/target/arm/imx31/mc13783-imx31.c
+++ b/firmware/target/arm/imx31/mc13783-imx31.c
@@ -106,7 +106,7 @@
          * acknowledged. Reenable interrupt and if anything was still
          * pending or became pending again, another signal will be
          * generated. */
-        imx31_regset32(&MC13783_GPIO_IMR, 1ul << MC13783_GPIO_LINE);
+        bitset32(&MC13783_GPIO_IMR, 1ul << MC13783_GPIO_LINE);
 
         event = mc13783_events;
         event_last = event + MC13783_NUM_EVENTS;
@@ -138,7 +138,7 @@
 void mc13783_event(void)
 {
     /* Mask the interrupt (unmasked when PMIC thread services it). */
-    imx31_regclr32(&MC13783_GPIO_IMR, 1ul << MC13783_GPIO_LINE);
+    bitclr32(&MC13783_GPIO_IMR, 1ul << MC13783_GPIO_LINE);
     MC13783_GPIO_ISR = (1ul << MC13783_GPIO_LINE);
     wakeup_signal(&mc13783_svc_wake);
 }
diff --git a/firmware/target/arm/imx31/sdma-imx31.c b/firmware/target/arm/imx31/sdma-imx31.c
index 4928108..381e589 100644
--- a/firmware/target/arm/imx31/sdma-imx31.c
+++ b/firmware/target/arm/imx31/sdma-imx31.c
@@ -414,12 +414,12 @@
 
     /* DSP side */
 #if 0 /* Not using this */
-    imx31_regmod32(&SDMA_DSPOVR, (config & CH_OWNSHP_DSP) ? 0 : bit, bit);
+    bitmod32(&SDMA_DSPOVR, (config & CH_OWNSHP_DSP) ? 0 : bit, bit);
 #endif
     /* Event */
-    imx31_regmod32(&SDMA_EVTOVR, (config & CH_OWNSHP_EVT) ? 0 : bit, bit);
+    bitmod32(&SDMA_EVTOVR, (config & CH_OWNSHP_EVT) ? 0 : bit, bit);
     /* MCU side */
-    imx31_regmod32(&SDMA_HOSTOVR, (config & CH_OWNSHP_MCU) ? 0 : bit, bit);
+    bitmod32(&SDMA_HOSTOVR, (config & CH_OWNSHP_MCU) ? 0 : bit, bit);
 }
 
 static bool setup_channel(struct channel_control_block *ccb_p)
@@ -485,12 +485,12 @@
     if (channel_cfg & CH_OWNSHP_EVT)
     {
         /* Set event ID to channel activation bitmapping */
-        imx31_regset32(&SDMA_CHNENBL(cd_p->event_id1), 1ul << channel);
+        bitset32(&SDMA_CHNENBL(cd_p->event_id1), 1ul << channel);
 
         if (cd_p->per_type == SDMA_PER_ATA)
         {
             /* ATA has two */
-            imx31_regset32(&SDMA_CHNENBL(cd_p->event_id2), 1ul << channel);
+            bitset32(&SDMA_CHNENBL(cd_p->event_id2), 1ul << channel);
         }
     }
 
@@ -676,7 +676,7 @@
 
     /* Unlock callback if it was set */
     if (intmsk & chmsk)
-        imx31_regset32(&sdma_enabled_ints, chmsk);
+        bitset32(&sdma_enabled_ints, chmsk);
 
     logf("SDMA ch closed: %d", channel);
 }
@@ -721,7 +721,7 @@
 
     /* Enable interrupt if a callback is specified. */
     if (cd_p->callback != NULL)
-        imx31_regset32(&sdma_enabled_ints, 1ul << channel);
+        bitset32(&sdma_enabled_ints, 1ul << channel);
 
     /* Minimum schedulable = 1 */
     sdma_channel_set_priority(channel, 1);
@@ -741,7 +741,7 @@
     ccb_p = &ccb_array[channel];
 
     /* Block callbacks (if not initialized, it won't be set). */
-    imx31_regclr32(&sdma_enabled_ints, 1ul << channel);
+    bitclr32(&sdma_enabled_ints, 1ul << channel);
 
     if (ccb_p->status.opened_init == 0)
         return;
diff --git a/firmware/target/arm/s3c2440/adc-s3c2440.c b/firmware/target/arm/s3c2440/adc-s3c2440.c
index f42a3d2..2e0cf8a 100644
--- a/firmware/target/arm/s3c2440/adc-s3c2440.c
+++ b/firmware/target/arm/s3c2440/adc-s3c2440.c
@@ -39,7 +39,7 @@
     int i;
 
     /* Turn on the ADC PCLK */
-    s3c_regset32(&CLKCON, 1<<15);
+    bitset32(&CLKCON, 1<<15);
 
     /* Set channel 0, normal mode, disable "start by read" */
     ADCCON &= ~(0x3F);
diff --git a/firmware/target/arm/s3c2440/dma-s3c2440.c b/firmware/target/arm/s3c2440/dma-s3c2440.c
index b83897c..c8df4fc 100644
--- a/firmware/target/arm/s3c2440/dma-s3c2440.c
+++ b/firmware/target/arm/s3c2440/dma-s3c2440.c
@@ -76,8 +76,8 @@
     INTPND = DMA0_MASK | DMA1_MASK | DMA2_MASK | DMA3_MASK;
     
     /* Enable interrupt in controller */
-    s3c_regclr32(&INTMOD, DMA0_MASK | DMA1_MASK | DMA2_MASK | DMA3_MASK);
-    s3c_regclr32(&INTMSK, DMA0_MASK | DMA1_MASK | DMA2_MASK | DMA3_MASK);
+    bitclr32(&INTMOD, DMA0_MASK | DMA1_MASK | DMA2_MASK | DMA3_MASK);
+    bitclr32(&INTMSK, DMA0_MASK | DMA1_MASK | DMA2_MASK | DMA3_MASK);
 }
 
 void dma_retain(void)
diff --git a/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c b/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
index e9f5547..c1c9017 100644
--- a/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
+++ b/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
@@ -50,14 +50,14 @@
 void pcm_play_lock(void)
 {
     if (++dma_play_lock.locked == 1)
-        s3c_regset32(&INTMSK, DMA2_MASK);
+        bitset32(&INTMSK, DMA2_MASK);
 }
 
 /* Unmask the DMA interrupt if enabled */
 void pcm_play_unlock(void)
 {
     if (--dma_play_lock.locked == 0)
-        s3c_regclr32(&INTMSK, dma_play_lock.state);
+        bitclr32(&INTMSK, dma_play_lock.state);
 }
 
 void pcm_play_dma_init(void)
@@ -65,7 +65,7 @@
     /* There seem to be problems when changing the IIS interface configuration
      * when a clock is not present.
      */
-    s3c_regset32(&CLKCON, 1<<17);
+    bitset32(&CLKCON, 1<<17);
     /* slave, transmit mode, 16 bit samples - MCLK 384fs - use 16.9344Mhz -
        BCLK 32fs */
     IISMOD = (1<<9) | (1<<8) | (2<<6) | (1<<3) | (1<<2) | (1<<0);
@@ -73,7 +73,7 @@
     /* RX,TX off,on */
     IISCON |= (1<<3) | (1<<2);
 
-    s3c_regclr32(&CLKCON, 1<<17);
+    bitclr32(&CLKCON, 1<<17);
 
     audiohw_init();
 
@@ -86,11 +86,11 @@
     /* Do not service DMA requests, yet */
 
     /* clear any pending int and mask it */
-    s3c_regset32(&INTMSK, DMA2_MASK);
+    bitset32(&INTMSK, DMA2_MASK);
     SRCPND = DMA2_MASK;
 
     /* connect to FIQ */
-    s3c_regset32(&INTMOD, DMA2_MASK);
+    bitset32(&INTMOD, DMA2_MASK);
 }
 
 void pcm_postinit(void)
@@ -132,7 +132,7 @@
 static void play_stop_pcm(void)
 {
     /* Mask DMA interrupt */
-    s3c_regset32(&INTMSK, DMA2_MASK);
+    bitset32(&INTMSK, DMA2_MASK);
 
     /* De-Activate the DMA channel */
     DMASKTRIG2 = 0x4;
@@ -160,7 +160,7 @@
 void pcm_play_dma_start(const void *addr, size_t size)
 {
     /* Enable the IIS clock */
-    s3c_regset32(&CLKCON, 1<<17);
+    bitset32(&CLKCON, 1<<17);
 
     /* stop any DMA in progress - idle IIS */
     play_stop_pcm();
@@ -191,7 +191,7 @@
     play_stop_pcm();
 
     /* Disconnect the IIS clock */
-    s3c_regclr32(&CLKCON, 1<<17);
+    bitclr32(&CLKCON, 1<<17);
 }
 
 void pcm_play_dma_pause(bool pause)
diff --git a/firmware/target/arm/s3c2440/i2c-s3c2440.c b/firmware/target/arm/s3c2440/i2c-s3c2440.c
index 4669186..155eb2f 100644
--- a/firmware/target/arm/s3c2440/i2c-s3c2440.c
+++ b/firmware/target/arm/s3c2440/i2c-s3c2440.c
@@ -43,7 +43,7 @@
     mutex_lock(&i2c_mtx);
 
     /* Turn on I2C clock */
-    s3c_regset32(&CLKCON, 1 << 16);
+    bitset32(&CLKCON, 1 << 16);
 
     /* Set mode to master transmitter and enable lines */
     IICSTAT = I2C_MODE_MASTER | I2C_MODE_TX | I2C_RXTX_ENB;
@@ -76,7 +76,7 @@
     IICSTAT = 0;
 
     /* Turn off I2C clock */
-    s3c_regclr32(&CLKCON, 1 << 16);
+    bitclr32(&CLKCON, 1 << 16);
 
     mutex_unlock(&i2c_mtx);
 }
@@ -92,11 +92,11 @@
     INTPND = IIC_MASK;
 
     /* Enable i2c interrupt in controller */
-    s3c_regclr32(&INTMOD, IIC_MASK);
-    s3c_regclr32(&INTMSK, IIC_MASK);
+    bitclr32(&INTMOD, IIC_MASK);
+    bitclr32(&INTMSK, IIC_MASK);
 
     /* Turn on I2C clock */
-    s3c_regset32(&CLKCON, 1 << 16);
+    bitset32(&CLKCON, 1 << 16);
 
     /* Set GPE15 (IICSDA) and GPE14 (IICSCL) to IIC */
     GPECON = (GPECON & ~((3 << 30) | (3 << 28))) |
@@ -110,7 +110,7 @@
     IICLC = (0 << 0);
 
     /* Turn off I2C clock */
-    s3c_regclr32(&CLKCON, 1 << 16);
+    bitclr32(&CLKCON, 1 << 16);
 }
 
 void IIC(void)
diff --git a/firmware/target/arm/s3c2440/kernel-s3c2440.c b/firmware/target/arm/s3c2440/kernel-s3c2440.c
index 6cabc8d..892758e 100644
--- a/firmware/target/arm/s3c2440/kernel-s3c2440.c
+++ b/firmware/target/arm/s3c2440/kernel-s3c2440.c
@@ -62,7 +62,7 @@
 #ifdef BOOTLOADER
 void tick_stop(void)
 {
-    s3c_regset32(&INTMSK, TIMER4_MASK);
+    bitset32(&INTMSK, TIMER4_MASK);
     TCON &= ~(1 << 20);
     SRCPND = TIMER4_MASK;
     INTPND = TIMER4_MASK;
diff --git a/firmware/target/arm/s3c2440/lcd-s3c2440.c b/firmware/target/arm/s3c2440/lcd-s3c2440.c
index b9f7d3e..77be29f 100644
--- a/firmware/target/arm/s3c2440/lcd-s3c2440.c
+++ b/firmware/target/arm/s3c2440/lcd-s3c2440.c
@@ -101,7 +101,7 @@
         GPDCON  |= 0xAAA0AAA0;
         GPDUP   |= 0xFCFC;
 
-        s3c_regset32(&CLKCON, 0x20);  /* enable LCD clock */
+        bitset32(&CLKCON, 0x20);  /* enable LCD clock */
         LCDCON1 |= LCD_ENVID;
     }
     else
@@ -113,7 +113,7 @@
         GPDUP   &= ~0xFCFC;
 
         LCDCON1 &= ~LCD_ENVID;        /* Must disable first or bus may freeze */
-        s3c_regclr32(&CLKCON, 0x20);  /* disable LCD clock */
+        bitclr32(&CLKCON, 0x20);  /* disable LCD clock */
     }
 }
 
@@ -165,7 +165,7 @@
 
 static void LCD_SPI_start(void)
 {
-    s3c_regset32(&CLKCON, 0x40000);   /* enable SPI clock */
+    bitset32(&CLKCON, 0x40000);   /* enable SPI clock */
     LCD_SPI_SS(false);
     SPCON0=0x3E;
     SPPRE0=24;
@@ -179,7 +179,7 @@
     LCD_SPI_SS(false);
 
     SPCON0 &= ~0x10;
-    s3c_regclr32(&CLKCON, 0x40000);    /* disable SPI clock */
+    bitclr32(&CLKCON, 0x40000);    /* disable SPI clock */
 }
 
 static void LCD_SPI_init(void)
@@ -253,7 +253,7 @@
     GPBUP   |= 0x181;
 #endif
 
-    s3c_regset32(&CLKCON, 0x20);  /* enable LCD clock */
+    bitset32(&CLKCON, 0x20);  /* enable LCD clock */
 
     LCD_CTRL_setup();
 #ifdef GIGABEAT_F
diff --git a/firmware/target/arm/s3c2440/mini2440/pcm-mini2440.c b/firmware/target/arm/s3c2440/mini2440/pcm-mini2440.c
index 30db29c4..8a6b62f 100644
--- a/firmware/target/arm/s3c2440/mini2440/pcm-mini2440.c
+++ b/firmware/target/arm/s3c2440/mini2440/pcm-mini2440.c
@@ -62,14 +62,14 @@
 void pcm_play_lock(void)
 {
     if (++dma_play_lock.locked == 1)
-        s3c_regset32(&INTMSK, DMA2_MASK);
+        bitset32(&INTMSK, DMA2_MASK);
 }
 
 /* Unmask the DMA interrupt if enabled */
 void pcm_play_unlock(void)
 {
     if (--dma_play_lock.locked == 0)
-        s3c_regclr32(&INTMSK, dma_play_lock.state);
+        bitclr32(&INTMSK, dma_play_lock.state);
 }
 
 void pcm_play_dma_init(void)
@@ -77,7 +77,7 @@
     /* There seem to be problems when changing the IIS interface configuration
      * when a clock is not present.
      */
-    s3c_regset32(&CLKCON, 1<<17);
+    bitset32(&CLKCON, 1<<17);
     
 #ifdef HAVE_UDA1341
     /* master, transmit mode, 16 bit samples, BCLK 32fs, PCLK */
@@ -95,7 +95,7 @@
     IISCON |= (1<<3) | (1<<2);
 #endif
 
-    s3c_regclr32(&CLKCON, 1<<17);
+    bitclr32(&CLKCON, 1<<17);
 
     audiohw_init();
 
@@ -112,11 +112,11 @@
     /* Do not service DMA requests, yet */
 
     /* clear any pending int and mask it */
-    s3c_regset32(&INTMSK, DMA2_MASK);
+    bitset32(&INTMSK, DMA2_MASK);
     SRCPND = DMA2_MASK;
 
     /* connect to FIQ */
-    s3c_regset32(&INTMOD, DMA2_MASK);
+    bitset32(&INTMOD, DMA2_MASK);
 }
 
 void pcm_postinit(void)
@@ -172,7 +172,7 @@
 static void play_stop_pcm(void)
 {
     /* Mask DMA interrupt */
-    s3c_regset32(&INTMSK, DMA2_MASK);
+    bitset32(&INTMSK, DMA2_MASK);
 
     /* De-Activate the DMA channel */
     DMASKTRIG2 = 0x4;
@@ -200,7 +200,7 @@
 void pcm_play_dma_start(const void *addr, size_t size)
 {
     /* Enable the IIS clock */
-    s3c_regset32(&CLKCON, 1<<17);
+    bitset32(&CLKCON, 1<<17);
 
     /* stop any DMA in progress - idle IIS */
     play_stop_pcm();
@@ -231,7 +231,7 @@
     play_stop_pcm();
 
     /* Disconnect the IIS clock */
-    s3c_regclr32(&CLKCON, 1<<17);
+    bitclr32(&CLKCON, 1<<17);
 }
 
 void pcm_play_dma_pause(bool pause)
diff --git a/firmware/target/arm/s3c2440/sd-s3c2440.c b/firmware/target/arm/s3c2440/sd-s3c2440.c
index f4c8a4f..d42405d 100644
--- a/firmware/target/arm/s3c2440/sd-s3c2440.c
+++ b/firmware/target/arm/s3c2440/sd-s3c2440.c
@@ -299,8 +299,8 @@
 
 #if 1
     /* Enable interrupt in controller */
-    s3c_regclr32(&INTMOD, SDI_MASK);
-    s3c_regclr32(&INTMSK, SDI_MASK);
+    bitclr32(&INTMOD, SDI_MASK);
+    bitclr32(&INTMSK, SDI_MASK);
     
     SDIIMSK |= S3C2410_SDIIMSK_DATAFINISH 
                | S3C2410_SDIIMSK_DATATIMEOUT
diff --git a/firmware/target/arm/s3c2440/system-s3c2440.c b/firmware/target/arm/s3c2440/system-s3c2440.c
index cb273ad..577b469 100644
--- a/firmware/target/arm/s3c2440/system-s3c2440.c
+++ b/firmware/target/arm/s3c2440/system-s3c2440.c
@@ -136,24 +136,6 @@
     enable_mmu();
 }
 
-void s3c_regmod32(volatile unsigned long *reg, unsigned long bits,
-                  unsigned long mask)
-{
-    int oldstatus = disable_interrupt_save(IRQ_FIQ_STATUS);
-    *reg = (*reg & ~mask) | (bits & mask);
-    restore_interrupt(oldstatus);
-}
-
-void s3c_regset32(volatile unsigned long *reg, unsigned long bits)
-{
-    s3c_regmod32(reg, bits, bits);
-}
-
-void s3c_regclr32(volatile unsigned long *reg, unsigned long bits)
-{
-    s3c_regmod32(reg, 0, bits);
-}
-
 #ifdef BOOTLOADER
 void system_prepare_fw_start(void)
 {
diff --git a/firmware/target/arm/s3c2440/system-target.h b/firmware/target/arm/s3c2440/system-target.h
index ad32f89..c48a62c 100644
--- a/firmware/target/arm/s3c2440/system-target.h
+++ b/firmware/target/arm/s3c2440/system-target.h
@@ -69,14 +69,4 @@
 void system_prepare_fw_start(void);
 void tick_stop(void);
 
-/* Functions to set and clear register bits atomically */
-
-/* Set and clear register bits */
-void s3c_regmod32(volatile unsigned long *reg, unsigned long bits,
-                  unsigned long mask);
-/* Set register bits */
-void s3c_regset32(volatile unsigned long *reg, unsigned long bits);
-/* Clear register bits */
-void s3c_regclr32(volatile unsigned long *reg, unsigned long bits);
-
 #endif /* SYSTEM_TARGET_H */