From c5357940ab0108b4102442d07825c44d5be0d22f Mon Sep 17 00:00:00 2001
From: Amaury Pouly <amaury.pouly@gmail.com>
Date: Thu, 13 Jun 2013 02:02:53 +0200
Subject: hwstub: major improvement in the stub and the tools

Fix the stub in many way to correctly detect the STMP family and
act upon that. Drop some unused commands and bump version.
Rewrite the tool to allows scripting in lua and load the register
description from an XML file using the regtools. Introduce a new
tool to load and run code using the hwstub (either binary format
or Rockbox additive scramble format). Also switch to an optimise
version of the memcpy/move/set functions to correctly handle
alignement issue (like writing a full word/half-word when
possible for registers which is crucial)

Change-Id: Id1d5cfe0b1b47e8b43900d32c5cd6eafae6414f6
---
 utils/hwstub/stmp/Makefile   |  87 +++----
 utils/hwstub/stmp/config.h   |  29 ++-
 utils/hwstub/stmp/format.h   |   6 +-
 utils/hwstub/stmp/hwemul.db  |  31 ---
 utils/hwstub/stmp/hwemul.lds |  70 ------
 utils/hwstub/stmp/hwstub.db  |  31 +++
 utils/hwstub/stmp/hwstub.lds |  71 ++++++
 utils/hwstub/stmp/link.lds   |  49 ----
 utils/hwstub/stmp/logf.h     |   6 +-
 utils/hwstub/stmp/main.c     | 536 ++++++++++---------------------------------
 utils/hwstub/stmp/memcpy.S   | 176 ++++++++++++++
 utils/hwstub/stmp/memmove.S  | 190 +++++++++++++++
 utils/hwstub/stmp/memset.S   |  98 ++++++++
 utils/hwstub/stmp/protocol.h |   2 +-
 utils/hwstub/stmp/stddef.h   |   6 +-
 utils/hwstub/stmp/string.h   |   6 +-
 utils/hwstub/stmp/system.h   |   6 +-
 17 files changed, 758 insertions(+), 642 deletions(-)
 delete mode 100644 utils/hwstub/stmp/hwemul.db
 delete mode 100644 utils/hwstub/stmp/hwemul.lds
 create mode 100644 utils/hwstub/stmp/hwstub.db
 create mode 100644 utils/hwstub/stmp/hwstub.lds
 delete mode 100644 utils/hwstub/stmp/link.lds
 create mode 100644 utils/hwstub/stmp/memcpy.S
 create mode 100644 utils/hwstub/stmp/memmove.S
 create mode 100644 utils/hwstub/stmp/memset.S

(limited to 'utils/hwstub/stmp')

diff --git a/utils/hwstub/stmp/Makefile b/utils/hwstub/stmp/Makefile
index ca61fe392d..7fd33c3d9e 100644
--- a/utils/hwstub/stmp/Makefile
+++ b/utils/hwstub/stmp/Makefile
@@ -1,58 +1,51 @@
-PREFIX?=arm-elf-eabi-
-CC=$(PREFIX)gcc
-LD=$(PREFIX)gcc
-AS=$(PREFIX)gcc
-OC=$(PREFIX)objcopy
-SBTOOLS=../../sbtools/
+#
+# common
+#
+CC=arm-elf-eabi-gcc
+LD=arm-elf-eabi-gcc
+AS=arm-elf-eabi-gcc
+OC=arm-elf-eabi-objcopy
 CFLAGS=-W -Wall -Wundef -O -nostdlib -ffreestanding -Wstrict-prototypes -pipe -std=gnu99 -mcpu=arm926ej-s -fomit-frame-pointer -Wno-pointer-sign -Wno-override-init -ffunction-sections
-CFLAGS_3700=$(CFLAGS) -DHAVE_STMP3700
 ASFLAGS=$(CFLAGS) -D__ASSEMBLER__
-ASFLAGS_3700=$(CFLAGS_3700) -D__ASSEMBLER__
 OCFLAGS=
-LINKER_FILE=hwemul.lds
-LDFLAGS=-lgcc -Os -nostdlib -Tlink.lds -Wl,-Map,hwemul.map
-LDFLAGS_3700=-lgcc -Os -nostdlib -Tlink.lds -Wl,-Map,hwemul3700.map
+LINKER_FILE=hwstub.lds
+TMP_LDS=link.lds
+TMP_MAP=hwstub.map
+LDFLAGS=-lgcc -Os -nostdlib -T$(TMP_LDS) -Wl,-Map,$(TMP_MAP)
 SRC_C=$(wildcard *.c)
 SRC_S=$(wildcard *.S)
 OBJ_C=$(SRC_C:.c=.o)
 OBJ_S=$(SRC_S:.S=.o)
-OBJ_C_3700=$(SRC_C:.c=.3700.o)
-OBJ_S_3700=$(SRC_S:.S=.3700.o)
 OBJ=$(OBJ_C) $(OBJ_S)
-OBJ_3700=$(OBJ_C_3700) $(OBJ_S_3700)
 OBJ_EXCEPT_CRT0=$(filter-out crt0.o,$(OBJ))
-OBJ_EXCEPT_CRT0_3700=$(filter-out crt0.3700.o,$(OBJ_3700))
 DEPS=$(OBJ:.o=.d)
-EXEC_ELF=hwemul.elf
-EXEC_SB=hwemul.sb
-EXEC_ELF_3700=hwemul3700.elf
-EXEC_SB_3700=hwemul3700.sb
+EXEC_ELF=hwstub.elf
 
+#
+# image production
+#
+TOOLS=../../../tools
+SBTOOLS=../../imxtools/sbtools
+
+# sb (stmp37xx)
+EXEC_SB=hwstub.sb
 ELF2SB=$(SBTOOLS)/elftosb -d
-ELF2SB_CMD=-c hwemul.db
-ELF2SB_KEY=-z
-SBLOADER=$(SBTOOLS)/sbloader
-SBLOADER_CMD=0 $(EXEC_SB)
-SBLOADER_CMD_3700=0 $(EXEC_SB_3700)
+ELF2SB_CMD=-c hwstub.db
+ELF2SB_KEY?=-z
 
-TOOLS=../../../../tools/
-SCRAMBLE=$(TOOLS)/scramble
+# sb1 (stmp36xx)
+EXEC_SB1=hwstub.sb1
+ELF2SB1_CMD=-loadjump $(EXEC_ELF)
+ELF2SB1_KEY?=
+ELF2SB1=$(SBTOOLS)/elftosb1 -d
 
-EXEC=$(EXEC_SB) $(EXEC_SB_3700) $(EXEC_ELF) $(EXEC_ELF_3700)
+EXEC=$(EXEC_ELF) $(EXEC_SB) $(EXEC_SB1)
 
 all: $(EXEC)
 
 # pull in dependency info for *existing* .o files
 -include $(DEPS)
 
-%.3700.o: %.c
-	$(CC) $(CFLAGS_3700) -c -o $@ $<
-	$(CC) -MM $(CFLAGS_3700) $*.c > $*.d
-	@cp -f $*.d $*.d.tmp
-	@sed -e 's/.*://' -e 's/\\$$//' < $*.d.tmp | fmt -1 | \
-	  sed -e 's/^ *//' -e 's/$$/:/' >> $*.d
-	@rm -f $*.d.tmp
-
 %.o: %.c
 	$(CC) $(CFLAGS) -c -o $@ $<
 	$(CC) -MM $(CFLAGS) $*.c > $*.d
@@ -61,32 +54,20 @@ all: $(EXEC)
 	  sed -e 's/^ *//' -e 's/$$/:/' >> $*.d
 	@rm -f $*.d.tmp
 
-%.3700.o: %.S
-	$(AS) $(ASFLAGS_3700) -c -o $@ $<
-
 %.o: %.S
 	$(AS) $(ASFLAGS) -c -o $@ $<
 
-link.lds: $(LINKER_FILE)
+$(TMP_LDS): $(LINKER_FILE)
 	$(CC) -E -x c - < $< | sed '/#/d' > $@
-	
-$(EXEC_ELF): $(OBJ) link.lds
+
+$(EXEC_ELF): $(OBJ) $(TMP_LDS)
 	$(LD) $(LDFLAGS) -o $@ $(OBJ_EXCEPT_CRT0)
 
 $(EXEC_SB): $(EXEC_ELF)
 	$(ELF2SB) $(ELF2SB_CMD) $(ELF2SB_KEY) -o $@
 
-$(EXEC_ELF_3700): $(OBJ_3700) link.lds
-	$(LD) $(LDFLAGS_3700) -o $@ $(OBJ_EXCEPT_CRT0_3700)
-
-$(EXEC_SB_3700): $(EXEC_ELF_3700)
-	$(ELF2SB) $(ELF2SB_CMD) $(ELF2SB_KEY) -o $@
-
-sbload: $(EXEC_SB)
-	$(SBLOADER) $(SBLOADER_CMD)
-
-sbload3700: $(EXEC_SB_3700)
-	$(SBLOADER) $(SBLOADER_CMD_3700)
+$(EXEC_SB1): $(EXEC_ELF)
+	$(ELF2SB1) $(ELF2SB1_CMD) $(ELF2SB1_KEY) -o $@
 
 clean:
-	rm -rf $(OBJ) $(OBJ_3700) $(DEPS) $(EXEC) *.map
+	rm -rf $(OBJ) $(DEPS) $(EXEC) $(TMP_LDS) $(TMP_MAP)
\ No newline at end of file
diff --git a/utils/hwstub/stmp/config.h b/utils/hwstub/stmp/config.h
index 6bd995e147..9d6de07f33 100644
--- a/utils/hwstub/stmp/config.h
+++ b/utils/hwstub/stmp/config.h
@@ -18,8 +18,8 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
-#ifndef __HWEMUL_CONFIG__
-#define __HWEMUL_CONFIG__
+#ifndef __HWSTUB_CONFIG__
+#define __HWSTUB_CONFIG__
 
 #define MEMORYSIZE      0
 #define STACK_SIZE      0x1000
@@ -30,4 +30,27 @@
 #define DRAM_ORIG       0x40000000
 #define DRAM_SIZE       (MEMORYSIZE * 0x100000)
 
-#endif /* __HWEMUL_CONFIG__ */
+#define CPU_ARM
+#define ARM_ARCH    5
+
+#if defined(CPU_ARM) && defined(__ASSEMBLER__)
+/* ARMv4T doesn't switch the T bit when popping pc directly, we must use BX */
+.macro ldmpc cond="", order="ia", regs
+#if ARM_ARCH == 4 && defined(USE_THUMB)
+    ldm\cond\order sp!, { \regs, lr }
+    bx\cond lr
+#else
+    ldm\cond\order sp!, { \regs, pc }
+#endif
+.endm
+.macro ldrpc cond=""
+#if ARM_ARCH == 4 && defined(USE_THUMB)
+    ldr\cond lr, [sp], #4
+    bx\cond  lr
+#else
+    ldr\cond pc, [sp], #4
+#endif
+.endm
+#endif
+
+#endif /* __HWSTUB_CONFIG__ */
diff --git a/utils/hwstub/stmp/format.h b/utils/hwstub/stmp/format.h
index a514c882ba..2ad4229f1e 100644
--- a/utils/hwstub/stmp/format.h
+++ b/utils/hwstub/stmp/format.h
@@ -18,12 +18,12 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
-#ifndef __HWEMUL_FORMAT__
-#define __HWEMUL_FORMAT__
+#ifndef __HWSTUB_FORMAT__
+#define __HWSTUB_FORMAT__
 
 #include <stdarg.h>
 
 void vuprintf(int (*push)(void *userp, unsigned char data),
               void *userp, const char *fmt, va_list ap);
 
-#endif /* __HWEMUL_FORMAT__ */
+#endif /* __HWSTUB_FORMAT__ */
diff --git a/utils/hwstub/stmp/hwemul.db b/utils/hwstub/stmp/hwemul.db
deleted file mode 100644
index 7a6f930f57..0000000000
--- a/utils/hwstub/stmp/hwemul.db
+++ /dev/null
@@ -1,31 +0,0 @@
-/***************************************************************************
- *             __________               __   ___.
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
- *                     \/            \/     \/    \/            \/
- * $Id$
- *
- * Copyright (C) 2012 by Amaury Pouly
- *
- * 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.
- *
- ****************************************************************************/
-sources
-{
-    hwemul = "hwemul.elf";
-}
-
-section(0)
-{
-    load hwemul;
-    jump hwemul(1);
-}
-
diff --git a/utils/hwstub/stmp/hwemul.lds b/utils/hwstub/stmp/hwemul.lds
deleted file mode 100644
index 7e3ac747a2..0000000000
--- a/utils/hwstub/stmp/hwemul.lds
+++ /dev/null
@@ -1,70 +0,0 @@
-/***************************************************************************
- *             __________               __   ___.
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
- *                     \/            \/     \/    \/            \/
- * $Id$
- *
- * Copyright (C) 2012 by Amaury Pouly
- *
- * 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.
- *
- ****************************************************************************/
-#include "config.h"
-
-ENTRY(start)
-OUTPUT_FORMAT(elf32-littlearm)
-OUTPUT_ARCH(arm)
-STARTUP(crt0.o)
-
-#define IRAM_END_ADDR   (IRAM_ORIG + IRAM_SIZE)
-
-MEMORY
-{
-    OCRAM : ORIGIN = IRAM_ORIG, LENGTH = IRAM_SIZE
-}
-
-SECTIONS
-{
-    .octext :
-    {
-        oc_codestart = .;
-        *(.text*)
-        *(.data*)
-        *(.rodata*)
-    } > OCRAM
-
-    .bss (NOLOAD) :
-    {
-        bss_start = .;
-        *(.bss)
-        bss_end = .;
-    } > OCRAM
-
-    .stack (NOLOAD) :
-    {
-        oc_codeend = .;
-        oc_stackstart = .;
-        . += STACK_SIZE;
-        oc_stackend = .;
-        oc_bufferstart = .;
-    } > OCRAM
-
-    .ocend IRAM_END_ADDR (NOLOAD) :
-    {
-        oc_bufferend = .;
-    } > OCRAM
-
-    /DISCARD/ :
-    {
-        *(.eh_frame)
-    } 
-}
diff --git a/utils/hwstub/stmp/hwstub.db b/utils/hwstub/stmp/hwstub.db
new file mode 100644
index 0000000000..1a085da50a
--- /dev/null
+++ b/utils/hwstub/stmp/hwstub.db
@@ -0,0 +1,31 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2012 by Amaury Pouly
+ *
+ * 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.
+ *
+ ****************************************************************************/
+sources
+{
+    hwstub = "hwstub.elf";
+}
+
+section(0)
+{
+    load hwstub;
+    jump hwstub(1);
+}
+
diff --git a/utils/hwstub/stmp/hwstub.lds b/utils/hwstub/stmp/hwstub.lds
new file mode 100644
index 0000000000..61504a3e75
--- /dev/null
+++ b/utils/hwstub/stmp/hwstub.lds
@@ -0,0 +1,71 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2012 by Amaury Pouly
+ *
+ * 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.
+ *
+ ****************************************************************************/
+#include "config.h"
+
+ENTRY(start)
+OUTPUT_FORMAT(elf32-littlearm)
+OUTPUT_ARCH(arm)
+STARTUP(crt0.o)
+
+#define IRAM_END_ADDR   (IRAM_ORIG + IRAM_SIZE)
+
+MEMORY
+{
+    OCRAM : ORIGIN = IRAM_ORIG, LENGTH = IRAM_SIZE
+}
+
+SECTIONS
+{
+    .octext :
+    {
+        oc_codestart = .;
+        *(.text*)
+        *(.icode*)
+        *(.data*)
+        *(.rodata*)
+    } > OCRAM
+
+    .bss (NOLOAD) :
+    {
+        bss_start = .;
+        *(.bss)
+        bss_end = .;
+    } > OCRAM
+
+    .stack (NOLOAD) :
+    {
+        oc_codeend = .;
+        oc_stackstart = .;
+        . += STACK_SIZE;
+        oc_stackend = .;
+        oc_bufferstart = .;
+    } > OCRAM
+
+    .ocend IRAM_END_ADDR (NOLOAD) :
+    {
+        oc_bufferend = .;
+    } > OCRAM
+
+    /DISCARD/ :
+    {
+        *(.eh_frame)
+    } 
+}
diff --git a/utils/hwstub/stmp/link.lds b/utils/hwstub/stmp/link.lds
deleted file mode 100644
index 97b259955f..0000000000
--- a/utils/hwstub/stmp/link.lds
+++ /dev/null
@@ -1,49 +0,0 @@
-
-ENTRY(start)
-OUTPUT_FORMAT(elf32-littlearm)
-OUTPUT_ARCH(arm)
-STARTUP(crt0.o)
-
-
-
-MEMORY
-{
-    OCRAM : ORIGIN = 0, LENGTH = 0x8000
-}
-
-SECTIONS
-{
-    .octext :
-    {
-        oc_codestart = .;
-        *(.text*)
-        *(.data*)
-        *(.rodata*)
-    } > OCRAM
-
-    .bss (NOLOAD) :
-    {
-        bss_start = .;
-        *(.bss)
-        bss_end = .;
-    } > OCRAM
-
-    .stack (NOLOAD) :
-    {
-        oc_codeend = .;
-        oc_stackstart = .;
-        . += 0x1000;
-        oc_stackend = .;
-        oc_bufferstart = .;
-    } > OCRAM
-
-    .ocend (0 + 0x8000) (NOLOAD) :
-    {
-        oc_bufferend = .;
-    } > OCRAM
-
-    /DISCARD/ :
-    {
-        *(.eh_frame)
-    }
-}
diff --git a/utils/hwstub/stmp/logf.h b/utils/hwstub/stmp/logf.h
index 5aa882a630..48c8c2c9b9 100644
--- a/utils/hwstub/stmp/logf.h
+++ b/utils/hwstub/stmp/logf.h
@@ -18,8 +18,8 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
-#ifndef __HWEMUL_LOGF__
-#define __HWEMUL_LOGF__
+#ifndef __HWSTUB_LOGF__
+#define __HWSTUB_LOGF__
 
 #include "stddef.h"
 #include <stdarg.h>
@@ -28,4 +28,4 @@ void enable_logf(bool en);
 void logf(const char *fmt, ...);
 size_t logf_readback(char *buf, size_t max_size);
 
-#endif /* __HWEMUL_LOGF__ */
+#endif /* __HWSTUB_LOGF__ */
diff --git a/utils/hwstub/stmp/main.c b/utils/hwstub/stmp/main.c
index 09bb6c7714..845f3842ea 100644
--- a/utils/hwstub/stmp/main.c
+++ b/utils/hwstub/stmp/main.c
@@ -34,118 +34,20 @@ extern unsigned char oc_bufferend[];
 
 /**
  *
- * Pin control
+ * Global
  *
  */
 
-#define HW_PINCTRL_BASE         0x80018000
-
-#define HW_PINCTRL_CTRL         (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x0))
-#define HW_PINCTRL_MUXSEL(i)    (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x100 + (i) * 0x10))
-#define HW_PINCTRL_DRIVE(i)     (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x200 + (i) * 0x10))
-#ifdef HAVE_STMP3700
-#define HW_PINCTRL_PULL(i)      (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x300 + (i) * 0x10))
-#define HW_PINCTRL_DOUT(i)      (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x400 + (i) * 0x10))
-#define HW_PINCTRL_DIN(i)       (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x500 + (i) * 0x10))
-#define HW_PINCTRL_DOE(i)       (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x600 + (i) * 0x10))
-#define HW_PINCTRL_PIN2IRQ(i)   (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x700 + (i) * 0x10))
-#define HW_PINCTRL_IRQEN(i)     (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x800 + (i) * 0x10))
-#define HW_PINCTRL_IRQLEVEL(i)  (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x900 + (i) * 0x10))
-#define HW_PINCTRL_IRQPOL(i)    (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0xa00 + (i) * 0x10))
-#define HW_PINCTRL_IRQSTAT(i)   (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0xb00 + (i) * 0x10))
-#else
-#define HW_PINCTRL_PULL(i)      (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x400 + (i) * 0x10))
-#define HW_PINCTRL_DOUT(i)      (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x500 + (i) * 0x10))
-#define HW_PINCTRL_DIN(i)       (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x600 + (i) * 0x10))
-#define HW_PINCTRL_DOE(i)       (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x700 + (i) * 0x10))
-#define HW_PINCTRL_PIN2IRQ(i)   (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x800 + (i) * 0x10))
-#define HW_PINCTRL_IRQEN(i)     (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0x900 + (i) * 0x10))
-#define HW_PINCTRL_IRQLEVEL(i)  (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0xa00 + (i) * 0x10))
-#define HW_PINCTRL_IRQPOL(i)    (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0xb00 + (i) * 0x10))
-#define HW_PINCTRL_IRQSTAT(i)   (*(volatile uint32_t *)(HW_PINCTRL_BASE + 0xc00 + (i) * 0x10))
-#endif
-
-#define PINCTRL_FUNCTION_MAIN   0
-#define PINCTRL_FUNCTION_ALT1   1
-#define PINCTRL_FUNCTION_ALT2   2
-#define PINCTRL_FUNCTION_GPIO   3
-
-#define PINCTRL_DRIVE_4mA       0
-#define PINCTRL_DRIVE_8mA       1
-#define PINCTRL_DRIVE_12mA      2
-#define PINCTRL_DRIVE_16mA      3 /* not available on all pins */
-
-typedef void (*pin_irq_cb_t)(int bank, int pin);
-
-static inline void imx233_pinctrl_init(void)
+enum stmp_family_t
 {
-    __REG_CLR(HW_PINCTRL_CTRL) = __BLOCK_CLKGATE | __BLOCK_SFTRST;
-}
-
-static inline void imx233_set_pin_drive_strength(unsigned bank, unsigned pin, unsigned strength)
-{
-    __REG_CLR(HW_PINCTRL_DRIVE(4 * bank + pin / 8)) = 3 << (4 * (pin % 8));
-    __REG_SET(HW_PINCTRL_DRIVE(4 * bank + pin / 8)) = strength << (4 * (pin % 8));
-}
-
-static inline void imx233_enable_gpio_output(unsigned bank, unsigned pin, bool enable)
-{
-    if(enable)
-        __REG_SET(HW_PINCTRL_DOE(bank)) = 1 << pin;
-    else
-        __REG_CLR(HW_PINCTRL_DOE(bank)) = 1 << pin;
-}
-
-static inline void imx233_enable_gpio_output_mask(unsigned bank, uint32_t pin_mask, bool enable)
-{
-    if(enable)
-        __REG_SET(HW_PINCTRL_DOE(bank)) = pin_mask;
-    else
-        __REG_CLR(HW_PINCTRL_DOE(bank)) = pin_mask;
-}
-
-static inline void imx233_set_gpio_output(unsigned bank, unsigned pin, bool value)
-{
-    if(value)
-        __REG_SET(HW_PINCTRL_DOUT(bank)) = 1 << pin;
-    else
-        __REG_CLR(HW_PINCTRL_DOUT(bank)) = 1 << pin;
-}
-
-static inline void imx233_set_gpio_output_mask(unsigned bank, uint32_t pin_mask, bool value)
-{
-    if(value)
-        __REG_SET(HW_PINCTRL_DOUT(bank)) = pin_mask;
-    else
-        __REG_CLR(HW_PINCTRL_DOUT(bank)) = pin_mask;
-}
-
-static inline uint32_t imx233_get_gpio_input_mask(unsigned bank, uint32_t pin_mask)
-{
-    return HW_PINCTRL_DIN(bank) & pin_mask;
-}
-
-static inline void imx233_set_pin_function(unsigned bank, unsigned pin, unsigned function)
-{
-    __REG_CLR(HW_PINCTRL_MUXSEL(2 * bank + pin / 16)) = 3 << (2 * (pin % 16));
-    __REG_SET(HW_PINCTRL_MUXSEL(2 * bank + pin / 16)) = function << (2 * (pin % 16));
-}
-
-static inline void imx233_enable_pin_pullup(unsigned bank, unsigned pin, bool enable)
-{
-    if(enable)
-        __REG_SET(HW_PINCTRL_PULL(bank)) = 1 << pin;
-    else
-        __REG_CLR(HW_PINCTRL_PULL(bank)) = 1 << pin;
-}
+    UNKNOWN,
+    STMP3600,
+    STMP3700,
+    STMP3770,
+    STMP3780
+};
 
-static inline void imx233_enable_pin_pullup_mask(unsigned bank, uint32_t pin_msk, bool enable)
-{
-    if(enable)
-        __REG_SET(HW_PINCTRL_PULL(bank)) = pin_msk;
-    else
-        __REG_CLR(HW_PINCTRL_PULL(bank)) = pin_msk;
-}
+enum stmp_family_t g_stmp_family = UNKNOWN;
 
 /**
  *
@@ -458,287 +360,38 @@ static void usb_drv_configure_endpoint(int ep_num, int type)
 
 /**
  *
- * Clock control
+ * Clkctrl
  *
- **/
-#define __CLK_CLKGATE   (1 << 31)
-#define __CLK_BUSY      (1 << 29)
+ */
 
 #define HW_CLKCTRL_BASE     0x80040000
 
 #define HW_CLKCTRL_PLLCTRL0 (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x0))
+#define HW_CLKCTRL_PLLCTRL0__BYPASS         (1 << 17) /* STMP3600 only */
 #define HW_CLKCTRL_PLLCTRL0__POWER          (1 << 16)
 #define HW_CLKCTRL_PLLCTRL0__EN_USB_CLKS    (1 << 18)
-#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BP     20
-#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BM     (3 << 20)
 
 #define HW_CLKCTRL_PLLCTRL1 (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x10))
+#define HW_CLKCTRL_PLLCTRL1__LOCK           (1 << 31)
 
-#define HW_CLKCTRL_CPU      (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x20))
-#define HW_CLKCTRL_CPU__DIV_CPU_BP  0
-#define HW_CLKCTRL_CPU__DIV_CPU_BM  0x3f
-#define HW_CLKCTRL_CPU__INTERRUPT_WAIT  (1 << 12)
-#define HW_CLKCTRL_CPU__DIV_XTAL_BP 16
-#define HW_CLKCTRL_CPU__DIV_XTAL_BM (0x3ff << 16)
-#define HW_CLKCTRL_CPU__DIV_XTAL_FRAC_EN    (1 << 26)
-#define HW_CLKCTRL_CPU__BUSY_REF_CPU    (1 << 28)
-
-#define HW_CLKCTRL_HBUS     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x30))
-#define HW_CLKCTRL_HBUS__DIV_BP         0
-#define HW_CLKCTRL_HBUS__DIV_BM         0x1f
-#define HW_CLKCTRL_HBUS__DIV_FRAC_EN    (1 << 5)
-#define HW_CLKCTRL_HBUS__SLOW_DIV_BP    16
-#define HW_CLKCTRL_HBUS__SLOW_DIV_BM    (0x7 << 16)
-#define HW_CLKCTRL_HBUS__AUTO_SLOW_MODE (1 << 20)
-
-#define HW_CLKCTRL_XBUS     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x40))
-#define HW_CLKCTRL_XBUS__DIV_BP     0
-#define HW_CLKCTRL_XBUS__DIV_BM     0x3ff
-#define HW_CLKCTRL_XBUS__BUSY       (1 << 31)
-
-#define HW_CLKCTRL_XTAL     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x50))
-#define HW_CLKCTRL_XTAL__TIMROT_CLK32K_GATE (1 << 26)
-#define HW_CLKCTRL_XTAL__DRI_CLK24M_GATE    (1 << 28)
-#define HW_CLKCTRL_XTAL__PWM_CLK24M_GATE    (1 << 29)
-#define HW_CLKCTRL_XTAL__FILT_CLK24M_GATE   (1 << 30)
-
-#define HW_CLKCTRL_PIX      (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x60))
-#define HW_CLKCTRL_PIX__DIV_BP  0
-#define HW_CLKCTRL_PIX__DIV_BM  0xfff
-
-#define HW_CLKCTRL_SSP      (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x70))
-#define HW_CLKCTRL_SSP__DIV_BP  0
-#define HW_CLKCTRL_SSP__DIV_BM  0x1ff
-
-#define HW_CLKCTRL_EMI      (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0xa0))
-#define HW_CLKCTRL_EMI__DIV_EMI_BP  0
-#define HW_CLKCTRL_EMI__DIV_EMI_BM  0x3f
-#define HW_CLKCTRL_EMI__DIV_XTAL_BP 8
-#define HW_CLKCTRL_EMI__DIV_XTAL_BM (0xf << 8)
-#define HW_CLKCTRL_EMI__BUSY_REF_EMI    (1 << 28)
-#define HW_CLKCTRL_EMI__SYNC_MODE_EN    (1 << 30)
-#define HW_CLKCTRL_EMI__CLKGATE     (1 << 31)
-
-#ifdef HAVE_STMP3770
-#define HW_CLKCTRL_CLKSEQ   (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0xe0))
-#else
-#define HW_CLKCTRL_CLKSEQ   (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x110))
-#endif
-#define HW_CLKCTRL_CLKSEQ__BYPASS_PIX   (1 << 1)
-#define HW_CLKCTRL_CLKSEQ__BYPASS_SSP   (1 << 5)
-#define HW_CLKCTRL_CLKSEQ__BYPASS_EMI   (1 << 6)
-#define HW_CLKCTRL_CLKSEQ__BYPASS_CPU   (1 << 7)
-
-#ifdef HAVE_STMP3770
-#define HW_CLKCTRL_FRAC     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0xd0))
-#else
-#define HW_CLKCTRL_FRAC     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0xf0))
-#endif
-#define HW_CLKCTRL_FRAC_CPU (*(volatile uint8_t *)(HW_CLKCTRL_BASE + 0xf0))
-#define HW_CLKCTRL_FRAC_EMI (*(volatile uint8_t *)(HW_CLKCTRL_BASE + 0xf1))
-#define HW_CLKCTRL_FRAC_PIX (*(volatile uint8_t *)(HW_CLKCTRL_BASE + 0xf2))
-#define HW_CLKCTRL_FRAC_IO  (*(volatile uint8_t *)(HW_CLKCTRL_BASE + 0xf3))
-#define HW_CLKCTRL_FRAC_XX__XXDIV_BM    0x3f
-#define HW_CLKCTRL_FRAC_XX__XX_STABLE   (1 << 6)
-#define HW_CLKCTRL_FRAC_XX__CLKGATEXX   (1 << 7)
-
-#define HW_CLKCTRL_RESET    (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x120))
-#define HW_CLKCTRL_RESET_CHIP   0x2
-#define HW_CLKCTRL_RESET_DIG    0x1
-
-/**
- *
- * DMA
- *
- */
-
-/********
- * APHB *
- ********/
-
-#define HW_APBH_BASE        0x80004000
-
-/* APHB channels */
-#define HW_APBH_SSP(ssp)    ssp
-
-#define HW_APBH_CTRL0       (*(volatile uint32_t *)(HW_APBH_BASE + 0x0))
-#define HW_APBH_CTRL0__FREEZE_CHANNEL(i)    (1 << (i))
-#define HW_APBH_CTRL0__CLKGATE_CHANNEL(i)   (1 << ((i) + 8))
-#define HW_APBH_CTRL0__RESET_CHANNEL(i)     (1 << ((i) + 16))
-#define HW_APBH_CTRL0__APB_BURST4_EN        (1 << 28)
-#define HW_APBH_CTRL0__APB_BURST8_EN        (1 << 29)
-
-#define HW_APBH_CTRL1       (*(volatile uint32_t *)(HW_APBH_BASE + 0x10))
-#define HW_APBH_CTRL1__CHx_CMDCMPLT_IRQ(i)      (1 << (i))
-#define HW_APBH_CTRL1__CHx_CMDCMPLT_IRQ_EN(i)   (1 << ((i) + 16))
-
-#define HW_APBH_CTRL2       (*(volatile uint32_t *)(HW_APBH_BASE + 0x20))
-#define HW_APBH_CTRL2__CHx_ERROR_IRQ(i)         (1 << (i))
-#define HW_APBH_CTRL2__CHx_ERROR_STATUS(i)      (1 << ((i) + 16))
-
-#define HW_APBH_CHx_CURCMDAR(i) (*(volatile uint32_t *)(HW_APBH_BASE + 0x40 + 0x70 * (i)))
-
-#define HW_APBH_CHx_NXTCMDAR(i) (*(volatile uint32_t *)(HW_APBH_BASE + 0x50 + 0x70 * (i)))
-
-#define HW_APBH_CHx_CMD(i)      (*(volatile uint32_t *)(HW_APBH_BASE + 0x60 + 0x70 * (i)))
-
-#define HW_APBH_CHx_BAR(i)      (*(volatile uint32_t *)(HW_APBH_BASE + 0x70 + 0x70 * (i)))
-
-#define HW_APBH_CHx_SEMA(i)     (*(volatile uint32_t *)(HW_APBH_BASE + 0x80 + 0x70 * (i)))
-
-#define HW_APBH_CHx_DEBUG1(i)   (*(volatile uint32_t *)(HW_APBH_BASE + 0x90 + 0x70 * (i)))
-
-#define HW_APBH_CHx_DEBUG2(i)   (*(volatile uint32_t *)(HW_APBH_BASE + 0xa0 + 0x70 * (i)))
-#define HW_APBH_CHx_DEBUG2__AHB_BYTES_BP    0
-#define HW_APBH_CHx_DEBUG2__AHB_BYTES_BM    0xffff
-#define HW_APBH_CHx_DEBUG2__APB_BYTES_BP    16
-#define HW_APBH_CHx_DEBUG2__APB_BYTES_BM    0xffff0000
-
-/********
- * APHX *
- ********/
-
-/* APHX channels */
-#define HW_APBX_AUDIO_ADC   0
-#define HW_APBX_AUDIO_DAC   1
-#define HW_APBX_I2C         3
-
-#define HW_APBX_BASE        0x80024000
-
-#define HW_APBX_CTRL0       (*(volatile uint32_t *)(HW_APBX_BASE + 0x0))
-
-#define HW_APBX_CTRL1       (*(volatile uint32_t *)(HW_APBX_BASE + 0x10))
-#define HW_APBX_CTRL1__CHx_CMDCMPLT_IRQ(i)      (1 << (i))
-#define HW_APBX_CTRL1__CHx_CMDCMPLT_IRQ_EN(i)   (1 << ((i) + 16))
-
-#define HW_APBX_CTRL2       (*(volatile uint32_t *)(HW_APBX_BASE + 0x20))
-#define HW_APBX_CTRL2__CHx_ERROR_IRQ(i)         (1 << (i))
-#define HW_APBX_CTRL2__CHx_ERROR_STATUS(i)      (1 << ((i) + 16))
+/* STMP3600 only */
+#define HW_CLKCTRL_CPUCLKCTRL   (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x20))
+#define HW_CLKCTRL_CPUCLKCTRL__DIV_BP   0
+#define HW_CLKCTRL_CPUCLKCTRL__DIV_BM   0x3ff
+#define HW_CLKCTRL_CPUCLKCTRL__WAIT_PLL_LOCK    (1 << 30)
 
-#define HW_APBX_CHANNEL_CTRL    (*(volatile uint32_t *)(HW_APBX_BASE + 0x30))
-#define HW_APBX_CHANNEL_CTRL__FREEZE_CHANNEL(i) (1 << (i))
-#define HW_APBX_CHANNEL_CTRL__RESET_CHANNEL(i)  (1 << ((i) + 16))
+/* STMP3600 */
+#define HW_CLKCTRL_HBUSCLKCTRL  (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x30))
 
-#define HW_APBX_CHx_CURCMDAR(i) (*(volatile uint32_t *)(HW_APBX_BASE + 0x100 + (i) * 0x70))
+/* STMP3600 only */
+#define HW_CLKCTRL_XBUSCLKCTRL  (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x40))
+#define HW_CLKCTRL_XBUSCLKCTRL__DIV_BP   0
+#define HW_CLKCTRL_XBUSCLKCTRL__DIV_BM   0x3ff
 
-#define HW_APBX_CHx_NXTCMDAR(i) (*(volatile uint32_t *)(HW_APBX_BASE + 0x110 + (i) * 0x70))
-
-#define HW_APBX_CHx_CMD(i)      (*(volatile uint32_t *)(HW_APBX_BASE + 0x120 + (i) * 0x70))
-
-#define HW_APBX_CHx_BAR(i)      (*(volatile uint32_t *)(HW_APBX_BASE + 0x130 + (i) * 0x70))
-
-#define HW_APBX_CHx_SEMA(i)     (*(volatile uint32_t *)(HW_APBX_BASE + 0x140 + (i) * 0x70))
-
-#define HW_APBX_CHx_DEBUG1(i)   (*(volatile uint32_t *)(HW_APBX_BASE + 0x150 + (i) * 0x70))
-
-#define HW_APBX_CHx_DEBUG2(i)   (*(volatile uint32_t *)(HW_APBX_BASE + 0x160 + (i) * 0x70))
-#define HW_APBX_CHx_DEBUG2__AHB_BYTES_BP    0
-#define HW_APBX_CHx_DEBUG2__AHB_BYTES_BM    0xffff
-#define HW_APBX_CHx_DEBUG2__APB_BYTES_BP    16
-#define HW_APBX_CHx_DEBUG2__APB_BYTES_BM    0xffff0000
-
-/**********
- * COMMON *
- **********/
-
-struct apb_dma_command_t
-{
-    struct apb_dma_command_t *next;
-    uint32_t cmd;
-    void *buffer;
-    /* PIO words follow */
-};
-
-#define APBH_DMA_CHANNEL(i)     i
-#define APBX_DMA_CHANNEL(i)     ((i) | 0x10)
-#define APB_IS_APBX_CHANNEL(x)  ((x) & 0x10)
-#define APB_GET_DMA_CHANNEL(x)  ((x) & 0xf)
-
-#define APB_SSP(ssp)        APBH_DMA_CHANNEL(HW_APBH_SSP(ssp))
-#define APB_AUDIO_ADC       APBX_DMA_CHANNEL(HW_APBX_AUDIO_ADC)
-#define APB_AUDIO_DAC       APBX_DMA_CHANNEL(HW_APBX_AUDIO_DAC)
-#define APB_I2C             APBX_DMA_CHANNEL(HW_APBX_I2C)
-
-#define HW_APB_CHx_CMD__COMMAND_BM         0x3
-#define HW_APB_CHx_CMD__COMMAND__NO_XFER   0
-#define HW_APB_CHx_CMD__COMMAND__WRITE     1
-#define HW_APB_CHx_CMD__COMMAND__READ      2
-#define HW_APB_CHx_CMD__COMMAND__SENSE     3
-#define HW_APB_CHx_CMD__CHAIN              (1 << 2)
-#define HW_APB_CHx_CMD__IRQONCMPLT         (1 << 3)
-/* those two are only available on APHB */
-#define HW_APBH_CHx_CMD__NANDLOCK          (1 << 4)
-#define HW_APBH_CHx_CMD__NANDWAIT4READY    (1 << 5)
-#define HW_APB_CHx_CMD__SEMAPHORE          (1 << 6)
-#define HW_APB_CHx_CMD__WAIT4ENDCMD        (1 << 7)
-/* An errata advise not to use it */
-//#define HW_APB_CHx_CMD__HALTONTERMINATE    (1 << 8)
-#define HW_APB_CHx_CMD__CMDWORDS_BM         0xf000
-#define HW_APB_CHx_CMD__CMDWORDS_BP         12
-#define HW_APB_CHx_CMD__XFER_COUNT_BM       0xffff0000
-#define HW_APB_CHx_CMD__XFER_COUNT_BP       16
-/* For software use */
-#define HW_APB_CHx_CMD__UNUSED_BP           8
-#define HW_APB_CHx_CMD__UNUSED_BM           (0xf << 8)
-#define HW_APB_CHx_CMD__UNUSED_MAGIC        (0xa << 8)
-
-#define HW_APB_CHx_SEMA__PHORE_BM           0xff0000
-#define HW_APB_CHx_SEMA__PHORE_BP           16
-
-/* A single descriptor cannot transfer more than 2^16 bytes */
-#define IMX233_MAX_SINGLE_DMA_XFER_SIZE     (1 << 16)
-
-static void imx233_dma_init(void)
-{
-    __REG_CLR(HW_APBH_CTRL0) = __BLOCK_CLKGATE | __BLOCK_SFTRST;
-    __REG_CLR(HW_APBX_CTRL0) = __BLOCK_CLKGATE | __BLOCK_SFTRST;
-}
-
-static void imx233_dma_reset_channel(unsigned chan)
-{
-    volatile uint32_t *ptr;
-    uint32_t bm;
-    if(APB_IS_APBX_CHANNEL(chan))
-    {
-        ptr = &HW_APBX_CHANNEL_CTRL;
-        bm = HW_APBX_CHANNEL_CTRL__RESET_CHANNEL(APB_GET_DMA_CHANNEL(chan));
-    }
-    else
-    {
-        ptr = &HW_APBH_CTRL0;
-        bm = HW_APBH_CTRL0__RESET_CHANNEL(APB_GET_DMA_CHANNEL(chan));
-    }
-    __REG_SET(*ptr) = bm;
-    /* wait for end of reset */
-    while(*ptr & bm)
-        ;
-}
-
-static void imx233_dma_start_command(unsigned chan, struct apb_dma_command_t *cmd)
-{
-    if(APB_IS_APBX_CHANNEL(chan))
-    {
-        HW_APBX_CHx_NXTCMDAR(APB_GET_DMA_CHANNEL(chan)) = (uint32_t)cmd;
-        HW_APBX_CHx_SEMA(APB_GET_DMA_CHANNEL(chan)) = 1;
-    }
-    else
-    {
-        HW_APBH_CHx_NXTCMDAR(APB_GET_DMA_CHANNEL(chan)) = (uint32_t)cmd;
-        HW_APBH_CHx_SEMA(APB_GET_DMA_CHANNEL(chan)) = 1;
-    }
-}
-
-static void imx233_dma_wait_completion(unsigned chan)
-{
-    volatile uint32_t *sema;
-    if(APB_IS_APBX_CHANNEL(chan))
-        sema = &HW_APBX_CHx_SEMA(APB_GET_DMA_CHANNEL(chan));
-    else
-        sema = &HW_APBH_CHx_SEMA(APB_GET_DMA_CHANNEL(chan));
-
-    while(*sema & HW_APB_CHx_SEMA__PHORE_BM)
-        ;
-}
+/* STMP3600 only */
+#define HW_CLKCTRL_UTMICLKCTRL  (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x70))
+#define HW_CLKCTRL_UTMICLKCTRL__UTMI_CLK30M_GATE    (1 << 30)
+#define HW_CLKCTRL_UTMICLKCTRL__UTMI_CLK120M_GATE   (1 << 31)
 
 /**
  *
@@ -751,8 +404,6 @@ static void imx233_dma_wait_completion(unsigned chan)
 #define HW_DIGCTL_CTRL          (*(volatile uint32_t *)(HW_DIGCTL_BASE + 0))
 #define HW_DIGCTL_CTRL__USB_CLKGATE (1 << 2)
 
-#define HW_DIGCTL_HCLKCOUNT     (*(volatile uint32_t *)(HW_DIGCTL_BASE + 0x20))
-
 #define HW_DIGCTL_MICROSECONDS  (*(volatile uint32_t *)(HW_DIGCTL_BASE + 0xC0))
 
 #define HW_DIGCTL_CHIPID        (*(volatile uint32_t *)(HW_DIGCTL_BASE + 0x310))
@@ -786,7 +437,6 @@ static void udelay(unsigned us)
 /* USB Phy */
 #define HW_USBPHY_BASE          0x8007C000
 #define HW_USBPHY_PWD           (*(volatile uint32_t *)(HW_USBPHY_BASE + 0))
-#define HW_USBPHY_PWD__ALL      (7 << 10 | 0xf << 17)
 
 #define HW_USBPHY_CTRL          (*(volatile uint32_t *)(HW_USBPHY_BASE + 0x30))
 
@@ -852,17 +502,9 @@ struct dcp_packet_t
  *
  */
 
-void memcpy(uint8_t *dst, const uint8_t *src, uint32_t length)
-{
-    for(uint32_t i = 0; i < length; i++)
-        dst[i] = src[i];
-}
-
-void memset(uint8_t *dst, uint8_t fill, uint32_t length)
-{
-    for(uint32_t i = 0; i < length; i++)
-        dst[i] = fill;
-}
+void memcpy(void *dest, const void *src, size_t n);
+void memmove(void *dest, const void *src, size_t n);
+void memset(void *dst, int value, size_t n);
 
 /**
  * 
@@ -880,9 +522,9 @@ static struct usb_device_descriptor __attribute__((aligned(2)))
     .bDeviceSubClass    = 0,
     .bDeviceProtocol    = 0,
     .bMaxPacketSize0    = 64,
-    .idVendor           = HWEMUL_USB_VID,
-    .idProduct          = HWEMUL_USB_PID,
-    .bcdDevice          = HWEMUL_VERSION_MAJOR << 8 | HWEMUL_VERSION_MINOR,
+    .idVendor           = HWSTUB_USB_VID,
+    .idProduct          = HWSTUB_USB_PID,
+    .bcdDevice          = HWSTUB_VERSION_MAJOR << 8 | HWSTUB_VERSION_MINOR,
     .iManufacturer      = 1,
     .iProduct           = 2,
     .iSerialNumber      = 3,
@@ -913,9 +555,9 @@ static struct usb_interface_descriptor __attribute__((aligned(2)))
     .bInterfaceNumber   = 0,
     .bAlternateSetting  = 0,
     .bNumEndpoints      = 3,
-    .bInterfaceClass    = HWEMUL_CLASS,
-    .bInterfaceSubClass = HWEMUL_SUBCLASS,
-    .bInterfaceProtocol = HWEMUL_PROTOCOL,
+    .bInterfaceClass    = HWSTUB_CLASS,
+    .bInterfaceSubClass = HWSTUB_SUBCLASS,
+    .bInterfaceProtocol = HWSTUB_PROTOCOL,
     .iInterface         = 4
 };
 
@@ -966,9 +608,9 @@ static struct usb_string_descriptor __attribute__((aligned(2)))
     28,
     USB_DT_STRING,
     {'A', 'c', 'i', 'd', ' ',
-     '0' + (HWEMUL_VERSION_MAJOR >> 4), '0' + (HWEMUL_VERSION_MAJOR & 0xf), '.',
-     '0' + (HWEMUL_VERSION_MINOR >> 4), '0' + (HWEMUL_VERSION_MINOR & 0xf), '.',
-     '0' + (HWEMUL_VERSION_REV >> 4), '0' + (HWEMUL_VERSION_REV & 0xf) }
+     '0' + (HWSTUB_VERSION_MAJOR >> 4), '0' + (HWSTUB_VERSION_MAJOR & 0xf), '.',
+     '0' + (HWSTUB_VERSION_MINOR >> 4), '0' + (HWSTUB_VERSION_MINOR & 0xf), '.',
+     '0' + (HWSTUB_VERSION_REV >> 4), '0' + (HWSTUB_VERSION_REV & 0xf) }
 };
 
 /* this is stringid #0: languages supported */
@@ -1138,9 +780,9 @@ static void handle_std_req(struct usb_ctrlrequest *req)
 
 struct usb_resp_info_version_t g_version =
 {
-    .major = HWEMUL_VERSION_MAJOR,
-    .minor = HWEMUL_VERSION_MINOR,
-    .revision = HWEMUL_VERSION_REV
+    .major = HWSTUB_VERSION_MAJOR,
+    .minor = HWSTUB_VERSION_MINOR,
+    .revision = HWSTUB_VERSION_REV
 };
 
 struct usb_resp_info_layout_t g_layout;
@@ -1149,8 +791,8 @@ struct usb_resp_info_stmp_t g_stmp;
 
 struct usb_resp_info_features_t g_features =
 {
-    .feature_mask = HWEMUL_FEATURE_LOG | HWEMUL_FEATURE_MEM |
-        HWEMUL_FEATURE_CALL | HWEMUL_FEATURE_JUMP | HWEMUL_FEATURE_AES_OTP
+    .feature_mask = HWSTUB_FEATURE_LOG | HWSTUB_FEATURE_MEM |
+        HWSTUB_FEATURE_CALL | HWSTUB_FEATURE_JUMP | HWSTUB_FEATURE_AES_OTP
 };
 
 static void fill_layout_info(void)
@@ -1177,21 +819,21 @@ static void handle_get_info(struct usb_ctrlrequest *req)
     int size = 0;
     switch(req->wIndex)
     {
-        case HWEMUL_INFO_VERSION:
+        case HWSTUB_INFO_VERSION:
             ptr = &g_version;
             size = sizeof(g_version);
             break;
-        case HWEMUL_INFO_LAYOUT:
+        case HWSTUB_INFO_LAYOUT:
             fill_layout_info();
             ptr = &g_layout;
             size = sizeof(g_layout);
             break;
-        case HWEMUL_INFO_STMP:
+        case HWSTUB_INFO_STMP:
             fill_stmp_info();
             ptr = &g_stmp;
             size = sizeof(g_stmp);
             break;
-        case HWEMUL_INFO_FEATURES:
+        case HWSTUB_INFO_FEATURES:
             ptr = &g_features;
             size = sizeof(g_features);
             break;
@@ -1249,17 +891,22 @@ static void handle_call_jump(struct usb_ctrlrequest *req)
 {
     uint32_t addr = req->wValue | req->wIndex << 16;
 
-    if(req->bRequest == HWEMUL_CALL)
+    if(req->bRequest == HWSTUB_CALL)
         ((void (*)(void))addr)();
     else
+    {
+        /* disconnect to make sure usb/dma won't interfere */
+        REG_USBCMD &= ~USBCMD_RUN;
+        REG_USBCMD |= USBCMD_CTRL_RESET;
         asm volatile("bx %0\n" : : "r" (addr) : "memory");
+    }
 }
 
 static void do_aes_otp(void *buffer, unsigned length, unsigned params)
 {
     static struct dcp_packet_t dcp_packet;
 
-    bool encrypt = !!(params & HWEMUL_AES_OTP_ENCRYPT);
+    bool encrypt = !!(params & HWSTUB_AES_OTP_ENCRYPT);
     /* reset DCP */
     __REG_SET(HW_DCP_CTRL) = 0x80000000;
     /* clear clock gate */
@@ -1307,20 +954,20 @@ static void handle_class_dev_req(struct usb_ctrlrequest *req)
 {
     switch(req->bRequest)
     {
-        case HWEMUL_GET_INFO:
+        case HWSTUB_GET_INFO:
             handle_get_info(req);
             break;
-        case HWEMUL_GET_LOG:
+        case HWSTUB_GET_LOG:
             handle_get_log(req);
             break;
-        case HWEMUL_RW_MEM:
+        case HWSTUB_RW_MEM:
             handle_rw_mem(req);
             break;
-        case HWEMUL_CALL:
-        case HWEMUL_JUMP:
+        case HWSTUB_CALL:
+        case HWSTUB_JUMP:
             handle_call_jump(req);
             break;
-        case HWEMUL_AES_OTP:
+        case HWSTUB_AES_OTP:
             handle_aes_otp(req);
             break;
         default:
@@ -1348,19 +995,68 @@ static void handle_class_req(struct usb_ctrlrequest *req)
 void main(uint32_t arg)
 {
     usb_buffer_size = oc_buffersize;
-    
-    logf("hwemul %d.%d.%d\n", HWEMUL_VERSION_MAJOR, HWEMUL_VERSION_MINOR,
-         HWEMUL_VERSION_REV);
+
+    logf("hwstub %d.%d.%d\n", HWSTUB_VERSION_MAJOR, HWSTUB_VERSION_MINOR,
+         HWSTUB_VERSION_REV);
     logf("argument: 0x%08x\n", arg);
 
+    /* detect family */
+    uint16_t product_code = __XTRACT(HW_DIGCTL_CHIPID, PRODUCT_CODE);
+    if(product_code >= 0x3600 && product_code < 0x3700)
+    {
+        logf("identified STMP3600 family\n");
+        g_stmp_family = STMP3600;
+    }
+    else if(product_code == 0x3700)
+    {
+        logf("identified STMP3700 family\n");
+        g_stmp_family = STMP3700;
+    }
+    else if(product_code == 0x37b0)
+    {
+        logf("identified STMP3770 family\n");
+        g_stmp_family = STMP3770;
+    }
+    else if(product_code == 0x3780)
+    {
+        logf("identified STMP3780 family\n");
+        g_stmp_family = STMP3780;
+    }
+    else
+        logf("cannot identify family: 0x%x\n", product_code);
+
     /* we don't know if USB was connected or not. In USB recovery mode it will
      * but in other cases it might not be. In doubt, disconnect */
     REG_USBCMD &= ~USBCMD_RUN;
+    if(g_stmp_family == STMP3600)
+    {
+        /* CPU clock is always derived from PLL, if we switch to PLL, cpu will
+         * run at 480 MHz unprepared ! That's bad so prepare to run at slow sleed
+         * (1.2MHz) for a safe transition */
+        HW_CLKCTRL_CPUCLKCTRL = HW_CLKCTRL_CPUCLKCTRL__WAIT_PLL_LOCK | 400;
+        /* We need to ensure that XBUS < HBUS but HBUS will be 1.2 MHz after the
+         * switch so lower XBUS too */
+        HW_CLKCTRL_XBUSCLKCTRL = 20;
+        /* Power PLL */
+        __REG_SET(HW_CLKCTRL_PLLCTRL0) = HW_CLKCTRL_PLLCTRL0__POWER;
+        HW_CLKCTRL_PLLCTRL0 = (HW_CLKCTRL_PLLCTRL0 & ~0x3ff) | 480;
+        /* Wait lock */
+        while(!(HW_CLKCTRL_PLLCTRL1 & HW_CLKCTRL_PLLCTRL1__LOCK));
+        /* Switch to PLL source */
+        __REG_CLR(HW_CLKCTRL_PLLCTRL0) = HW_CLKCTRL_PLLCTRL0__BYPASS;
+        /* Get back XBUS = 24 MHz and CPU = HBUS = 64MHz */
+        HW_CLKCTRL_CPUCLKCTRL = 7;
+        HW_CLKCTRL_HBUSCLKCTRL = 7;
+        HW_CLKCTRL_XBUSCLKCTRL = 1;
+        __REG_CLR(HW_CLKCTRL_UTMICLKCTRL) = HW_CLKCTRL_UTMICLKCTRL__UTMI_CLK120M_GATE;
+        __REG_CLR(HW_CLKCTRL_UTMICLKCTRL) = HW_CLKCTRL_UTMICLKCTRL__UTMI_CLK30M_GATE;
+    }
+    else
+        __REG_SET(HW_CLKCTRL_PLLCTRL0) = HW_CLKCTRL_PLLCTRL0__POWER;
     /* enable USB PHY PLL */
     __REG_SET(HW_CLKCTRL_PLLCTRL0) = HW_CLKCTRL_PLLCTRL0__EN_USB_CLKS;
     /* power up USB PHY */
     __REG_CLR(HW_USBPHY_CTRL) = __BLOCK_CLKGATE | __BLOCK_SFTRST;
-    //__REG_CLR(HW_USBPHY_PWD) = HW_USBPHY_PWD__ALL;
     HW_USBPHY_PWD = 0;
     /* enable USB controller */
     __REG_CLR(HW_DIGCTL_CTRL) = HW_DIGCTL_CTRL__USB_CLKGATE;
@@ -1382,7 +1078,7 @@ void main(uint32_t arg)
     REG_ENDPTSETUPSTAT = EPSETUP_STATUS_EP0;
     /* run! */
     REG_USBCMD |= USBCMD_RUN;
-    
+
     while(1)
     {
         /* wait for setup */
diff --git a/utils/hwstub/stmp/memcpy.S b/utils/hwstub/stmp/memcpy.S
new file mode 100644
index 0000000000..2a55fb5656
--- /dev/null
+++ b/utils/hwstub/stmp/memcpy.S
@@ -0,0 +1,176 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2006 Free Software Foundation, Inc.
+ * This file was originally part of the GNU C Library
+ * Contributed to glibc by MontaVista Software, Inc. (written by Nicolas Pitre)
+ * Adapted for Rockbox by Daniel Ankers
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#include "config.h"
+
+/*
+ * Endian independent macros for shifting bytes within registers.
+ */
+#ifndef __ARMEB__
+#define pull            lsr
+#define push            lsl
+#else
+#define pull            lsl
+#define push            lsr
+#endif
+
+/* Prototype: void *memcpy(void *dest, const void *src, size_t n); */
+
+    .section    .icode,"ax",%progbits
+
+    .align      2
+    .global     memcpy
+    .type       memcpy,%function
+
+memcpy:
+        stmfd   sp!, {r0, r4, lr}
+
+        subs    r2, r2, #4
+        blt 8f
+        ands    ip, r0, #3
+        bne 9f
+        ands    ip, r1, #3
+        bne 10f
+
+1:      subs    r2, r2, #(28)
+        stmfd   sp!, {r5 - r8}
+        blt 5f
+
+2:
+3:
+4:      ldmia   r1!, {r3, r4, r5, r6, r7, r8, ip, lr}
+        subs    r2, r2, #32
+        stmia   r0!, {r3, r4, r5, r6, r7, r8, ip, lr}
+        bge 3b
+
+5:      ands    ip, r2, #28
+        rsb ip, ip, #32
+        addne   pc, pc, ip      @ C is always clear here
+        b   7f
+6:      nop
+        ldr r3, [r1], #4
+        ldr r4, [r1], #4
+        ldr r5, [r1], #4
+        ldr r6, [r1], #4
+        ldr r7, [r1], #4
+        ldr r8, [r1], #4
+        ldr lr, [r1], #4
+
+        add pc, pc, ip
+        nop
+        nop
+        str r3, [r0], #4
+        str r4, [r0], #4
+        str r5, [r0], #4
+        str r6, [r0], #4
+        str r7, [r0], #4
+        str r8, [r0], #4
+        str lr, [r0], #4
+
+7:      ldmfd   sp!, {r5 - r8}
+
+8:      movs    r2, r2, lsl #31
+        ldrneb  r3, [r1], #1
+        ldrcsb  r4, [r1], #1
+        ldrcsb  ip, [r1]
+        strneb  r3, [r0], #1
+        strcsb  r4, [r0], #1
+        strcsb  ip, [r0]
+
+        ldmpc   regs="r0, r4"
+
+9:      rsb ip, ip, #4
+        cmp ip, #2
+        ldrgtb  r3, [r1], #1
+        ldrgeb  r4, [r1], #1
+        ldrb    lr, [r1], #1
+        strgtb  r3, [r0], #1
+        strgeb  r4, [r0], #1
+        subs    r2, r2, ip
+        strb    lr, [r0], #1
+        blt 8b
+        ands    ip, r1, #3
+        beq 1b
+
+10:     bic r1, r1, #3
+        cmp ip, #2
+        ldr lr, [r1], #4
+        beq 17f
+        bgt 18f
+
+
+        .macro  forward_copy_shift pull push
+
+        subs    r2, r2, #28
+        blt 14f
+
+11:     stmfd   sp!, {r5 - r9}
+
+12:
+13:     ldmia   r1!, {r4, r5, r6, r7}
+        mov r3, lr, pull #\pull
+        subs    r2, r2, #32
+        ldmia   r1!, {r8, r9, ip, lr}
+        orr r3, r3, r4, push #\push
+        mov r4, r4, pull #\pull
+        orr r4, r4, r5, push #\push
+        mov r5, r5, pull #\pull
+        orr r5, r5, r6, push #\push
+        mov r6, r6, pull #\pull
+        orr r6, r6, r7, push #\push
+        mov r7, r7, pull #\pull
+        orr r7, r7, r8, push #\push
+        mov r8, r8, pull #\pull
+        orr r8, r8, r9, push #\push
+        mov r9, r9, pull #\pull
+        orr r9, r9, ip, push #\push
+        mov ip, ip, pull #\pull
+        orr ip, ip, lr, push #\push
+        stmia   r0!, {r3, r4, r5, r6, r7, r8, r9, ip}
+        bge 12b
+
+        ldmfd   sp!, {r5 - r9}
+
+14:     ands    ip, r2, #28
+        beq 16f
+
+15:     mov r3, lr, pull #\pull
+        ldr lr, [r1], #4
+        subs    ip, ip, #4
+        orr r3, r3, lr, push #\push
+        str r3, [r0], #4
+        bgt 15b
+
+16:     sub r1, r1, #(\push / 8)
+        b   8b
+
+        .endm
+
+
+        forward_copy_shift  pull=8  push=24
+
+17:     forward_copy_shift  pull=16 push=16
+
+18:     forward_copy_shift  pull=24 push=8
+
diff --git a/utils/hwstub/stmp/memmove.S b/utils/hwstub/stmp/memmove.S
new file mode 100644
index 0000000000..d8cab048be
--- /dev/null
+++ b/utils/hwstub/stmp/memmove.S
@@ -0,0 +1,190 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2006 Free Software Foundation, Inc.
+ * This file was originally part of the GNU C Library
+ * Contributed to glibc by MontaVista Software, Inc. (written by Nicolas Pitre)
+ * Adapted for Rockbox by Daniel Ankers
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#include "config.h"
+
+/*
+ * Endian independent macros for shifting bytes within registers.
+ */
+#ifndef __ARMEB__
+#define pull            lsr
+#define push            lsl
+#else
+#define pull            lsl
+#define push            lsr
+#endif
+
+        .text
+
+/*
+ * Prototype: void *memmove(void *dest, const void *src, size_t n);
+ *
+ * Note:
+ *
+ * If the memory regions don't overlap, we simply branch to memcpy which is
+ * normally a bit faster. Otherwise the copy is done going downwards.
+ */
+
+    .section    .icode,"ax",%progbits
+
+    .align      2
+    .global     memmove
+    .type       memmove,%function
+
+memmove:
+
+        subs    ip, r0, r1
+        cmphi   r2, ip
+        bls memcpy
+
+        stmfd   sp!, {r0, r4, lr}
+        add r1, r1, r2
+        add r0, r0, r2
+        subs    r2, r2, #4
+        blt 8f
+        ands    ip, r0, #3
+        bne 9f
+        ands    ip, r1, #3
+        bne 10f
+
+1:      subs    r2, r2, #(28)
+        stmfd   sp!, {r5 - r8}
+        blt 5f
+
+2:
+3:
+4:      ldmdb   r1!, {r3, r4, r5, r6, r7, r8, ip, lr}
+        subs    r2, r2, #32
+        stmdb   r0!, {r3, r4, r5, r6, r7, r8, ip, lr}
+        bge 3b
+
+5:      ands    ip, r2, #28
+        rsb ip, ip, #32
+        addne   pc, pc, ip      @ C is always clear here
+        b   7f
+6:      nop
+        ldr r3, [r1, #-4]!
+        ldr r4, [r1, #-4]!
+        ldr r5, [r1, #-4]!
+        ldr r6, [r1, #-4]!
+        ldr r7, [r1, #-4]!
+        ldr r8, [r1, #-4]!
+        ldr lr, [r1, #-4]!
+
+        add pc, pc, ip
+        nop
+        nop
+        str r3, [r0, #-4]!
+        str r4, [r0, #-4]!
+        str r5, [r0, #-4]!
+        str r6, [r0, #-4]!
+        str r7, [r0, #-4]!
+        str r8, [r0, #-4]!
+        str lr, [r0, #-4]!
+
+7:      ldmfd   sp!, {r5 - r8}
+
+8:      movs    r2, r2, lsl #31
+        ldrneb  r3, [r1, #-1]!
+        ldrcsb  r4, [r1, #-1]!
+        ldrcsb  ip, [r1, #-1]
+        strneb  r3, [r0, #-1]!
+        strcsb  r4, [r0, #-1]!
+        strcsb  ip, [r0, #-1]
+        ldmpc   regs="r0, r4"
+
+9:      cmp ip, #2
+        ldrgtb  r3, [r1, #-1]!
+        ldrgeb  r4, [r1, #-1]!
+        ldrb    lr, [r1, #-1]!
+        strgtb  r3, [r0, #-1]!
+        strgeb  r4, [r0, #-1]!
+        subs    r2, r2, ip
+        strb    lr, [r0, #-1]!
+        blt 8b
+        ands    ip, r1, #3
+        beq 1b
+
+10:     bic r1, r1, #3
+        cmp ip, #2
+        ldr r3, [r1, #0]
+        beq 17f
+        blt 18f
+
+
+        .macro  backward_copy_shift push pull
+
+        subs    r2, r2, #28
+        blt 14f
+
+11:     stmfd   sp!, {r5 - r9}
+
+12:
+13:     ldmdb   r1!, {r7, r8, r9, ip}
+        mov     lr, r3, push #\push
+        subs    r2, r2, #32
+        ldmdb   r1!, {r3, r4, r5, r6}
+        orr     lr, lr, ip, pull #\pull
+        mov     ip, ip, push #\push
+        orr     ip, ip, r9, pull #\pull
+        mov     r9, r9, push #\push
+        orr     r9, r9, r8, pull #\pull
+        mov     r8, r8, push #\push
+        orr     r8, r8, r7, pull #\pull
+        mov     r7, r7, push #\push
+        orr     r7, r7, r6, pull #\pull
+        mov     r6, r6, push #\push
+        orr     r6, r6, r5, pull #\pull
+        mov     r5, r5, push #\push
+        orr     r5, r5, r4, pull #\pull
+        mov     r4, r4, push #\push
+        orr     r4, r4, r3, pull #\pull
+        stmdb   r0!, {r4 - r9, ip, lr}
+        bge 12b
+
+        ldmfd   sp!, {r5 - r9}
+
+14:     ands    ip, r2, #28
+        beq 16f
+
+15:     mov     lr, r3, push #\push
+        ldr r3, [r1, #-4]!
+        subs    ip, ip, #4
+        orr lr, lr, r3, pull #\pull
+        str lr, [r0, #-4]!
+        bgt 15b
+
+16:     add r1, r1, #(\pull / 8)
+        b   8b
+
+        .endm
+
+
+        backward_copy_shift push=8  pull=24
+
+17:     backward_copy_shift push=16 pull=16
+
+18:     backward_copy_shift push=24 pull=8
+
+
diff --git a/utils/hwstub/stmp/memset.S b/utils/hwstub/stmp/memset.S
new file mode 100644
index 0000000000..682da874ce
--- /dev/null
+++ b/utils/hwstub/stmp/memset.S
@@ -0,0 +1,98 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2006 by Thom Johansen
+ *
+ * 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.
+ *
+ ****************************************************************************/
+#include "config.h"
+
+    .section    .icode,"ax",%progbits
+
+    .align      2
+
+/*  The following code is based on code found in Linux kernel version 2.6.15.3
+ *  linux/arch/arm/lib/memset.S
+ *
+ *  Copyright (C) 1995-2000 Russell King
+ */
+
+/* This code will align a pointer for memset, if needed */
+1:      cmp     r2, #4                  @ 1 do we have enough
+        blt     5f                      @ 1 bytes to align with?
+        cmp     r3, #2                  @ 1
+        strgtb  r1, [r0, #-1]!          @ 1
+        strgeb  r1, [r0, #-1]!          @ 1
+        strb    r1, [r0, #-1]!          @ 1
+        sub     r2, r2, r3              @ 1 r2 = r2 - r3
+        b 2f
+
+        .global     memset
+        .type       memset,%function
+memset:
+        add     r0, r0, r2              @ we'll write backwards in memory
+        ands    r3, r0, #3              @ 1 unaligned?
+        bne     1b                      @ 1
+2:
+/*
+ * we know that the pointer in r0 is aligned to a word boundary.
+ */
+        orr     r1, r1, r1, lsl #8
+        orr     r1, r1, r1, lsl #16
+        mov     r3, r1
+        cmp     r2, #16
+        blt     5f
+/*
+ * We need an extra register for this loop - save the return address and
+ * use the LR
+ */
+        str     lr, [sp, #-4]!
+        mov     ip, r1
+        mov     lr, r1
+
+3:      subs    r2, r2, #64
+        stmgedb r0!, {r1, r3, ip, lr}   @ 64 bytes at a time.
+        stmgedb r0!, {r1, r3, ip, lr}
+        stmgedb r0!, {r1, r3, ip, lr}
+        stmgedb r0!, {r1, r3, ip, lr}
+        bgt     3b
+        ldrpc   cond=eq                 @ Now <64 bytes to go.
+/*
+ * No need to correct the count; we're only testing bits from now on
+ */
+        tst     r2, #32
+        stmnedb r0!, {r1, r3, ip, lr}
+        stmnedb r0!, {r1, r3, ip, lr}
+        tst     r2, #16
+        stmnedb r0!, {r1, r3, ip, lr}
+        ldr     lr, [sp], #4
+
+5:      tst     r2, #8
+        stmnedb r0!, {r1, r3}
+        tst     r2, #4
+        strne   r1, [r0, #-4]!
+/*
+ * When we get here, we've got less than 4 bytes to zero.  We
+ * may have an unaligned pointer as well.
+ */
+6:      tst     r2, #2
+        strneb  r1, [r0, #-1]!
+        strneb  r1, [r0, #-1]!
+        tst     r2, #1
+        strneb  r1, [r0, #-1]!
+        bx      lr
+.end:
+        .size   memset,.end-memset
diff --git a/utils/hwstub/stmp/protocol.h b/utils/hwstub/stmp/protocol.h
index d3ffb6ce00..35510fa9b2 100644
--- a/utils/hwstub/stmp/protocol.h
+++ b/utils/hwstub/stmp/protocol.h
@@ -1 +1 @@
-#include "../hwemul_protocol.h"
+#include "../hwstub_protocol.h"
diff --git a/utils/hwstub/stmp/stddef.h b/utils/hwstub/stmp/stddef.h
index 9d59d2913c..9bfd767750 100644
--- a/utils/hwstub/stmp/stddef.h
+++ b/utils/hwstub/stmp/stddef.h
@@ -18,8 +18,8 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
-#ifndef __HWEMUL_STDDEF__
-#define __HWEMUL_STDDEF__
+#ifndef __HWSTUB_STDDEF__
+#define __HWSTUB_STDDEF__
 
 #include "stdint.h"
 
@@ -29,4 +29,4 @@ typedef int32_t ssize_t;
 #define MIN(a, b) ((a) < (b) ? (a) : (b))
 #define MAX(a, b) ((a) > (b) ? (a) : (b))
 
-#endif /* __HWEMUL_STDDEF__ */
+#endif /* __HWSTUB_STDDEF__ */
diff --git a/utils/hwstub/stmp/string.h b/utils/hwstub/stmp/string.h
index 7ef460ea6e..184144e525 100644
--- a/utils/hwstub/stmp/string.h
+++ b/utils/hwstub/stmp/string.h
@@ -18,8 +18,8 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
-#ifndef __HWEMUL_STRING__
-#define __HWEMUL_STRING__
+#ifndef __HWSTUB_STRING__
+#define __HWSTUB_STRING__
 
 #include "stddef.h"
 
@@ -27,4 +27,4 @@ void memset(void *dst, int c, size_t n);
 void memcpy(void *dst, const void *src, size_t n);
 size_t strlen(const char *s);
 
-#endif /* __HWEMUL_STRING__ */
+#endif /* __HWSTUB_STRING__ */
diff --git a/utils/hwstub/stmp/system.h b/utils/hwstub/stmp/system.h
index c1babe7d87..e5aea12051 100644
--- a/utils/hwstub/stmp/system.h
+++ b/utils/hwstub/stmp/system.h
@@ -18,8 +18,8 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
-#ifndef __HWEMUL_SYSTEM__
-#define __HWEMUL_SYSTEM__
+#ifndef __HWSTUB_SYSTEM__
+#define __HWSTUB_SYSTEM__
 
 #define IRQ_ENABLED      0x00
 #define IRQ_DISABLED     0x80
@@ -114,5 +114,5 @@ static inline int disable_interrupt_save(int mask)
     return cpsr;
 }
 
-#endif /* __HWEMUL_SYSTEM__ */
+#endif /* __HWSTUB_SYSTEM__ */
  
-- 
cgit v1.2.3