imxtools: move hwemul to its own directory

The hwemul is only partly imx specific: the stub is stmp specific
but could be ported to other targets, the computer side and the
protocol are mostly stmp independent (or should be).

Change-Id: If88febffe591b0de86ea11cb740455ba20ddc401

25 files changed, 3727 insertions, 0 deletions

diff --git a/utils/hwstub/hwemul_protocol.h b/utils/hwstub/hwemul_protocol.h
new file mode 100644
index 0000000000..f11fd91352
--- /dev/null
+++ b/utils/hwstub/hwemul_protocol.h
@@ -0,0 +1,127 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL_PROTOCOL__
+#define __HWEMUL_PROTOCOL__
+
+#define HWEMUL_CLASS        0xfe
+#define HWEMUL_SUBCLASS     0xac
+#define HWEMUL_PROTOCOL     0x1d
+
+#define HWEMUL_VERSION_MAJOR    2
+#define HWEMUL_VERSION_MINOR    8
+#define HWEMUL_VERSION_REV      2
+
+#define HWEMUL_USB_VID  0xfee1
+#define HWEMUL_USB_PID  0xdead
+
+/**
+ * Control commands
+ *
+ * These commands are sent to the device, using the standard bRequest field
+ * of the SETUP packet. This is to take advantage of both wIndex and wValue
+ * although it would have been more correct to send them to the interface.
+ */
+
+/* list of commands */
+#define HWEMUL_GET_INFO     0 /* mandatory */
+#define HWEMUL_GET_LOG      1 /* optional */
+#define HWEMUL_RW_MEM       2 /* optional */
+#define HWEMUL_CALL         3 /* optional */
+#define HWEMUL_JUMP         4 /* optional */
+#define HWEMUL_AES_OTP      5 /* optional */
+
+/**
+ * HWEMUL_GET_INFO: get some information about an aspect of the device.
+ * The wIndex field of the SETUP specifies which information to get. */
+
+/* list of possible information */
+#define HWEMUL_INFO_VERSION     0
+#define HWEMUL_INFO_LAYOUT      1
+#define HWEMUL_INFO_STMP        2
+#define HWEMUL_INFO_FEATURES    3
+
+struct usb_resp_info_version_t
+{
+    uint8_t major;
+    uint8_t minor;
+    uint8_t revision;
+} __attribute__((packed));
+
+struct usb_resp_info_layout_t
+{
+    /* describe the range of memory used by the running code */
+    uint32_t oc_code_start;
+    uint32_t oc_code_size;
+    /* describe the range of memory used by the stack */
+    uint32_t oc_stack_start;
+    uint32_t oc_stack_size;
+    /* describe the range of memory available as a buffer */
+    uint32_t oc_buffer_start;
+    uint32_t oc_buffer_size;
+} __attribute__((packed));
+
+struct usb_resp_info_stmp_t
+{
+    uint16_t chipid; /* 0x3780 for STMP3780 for example */
+    uint8_t rev; /* 0=TA1 on STMP3780 for example */
+    uint8_t is_supported; /* 1 if the chip is supported */
+} __attribute__((packed));
+
+/* list of possible features */
+#define HWEMUL_FEATURE_LOG      (1 << 0)
+#define HWEMUL_FEATURE_MEM      (1 << 1)
+#define HWEMUL_FEATURE_CALL     (1 << 2)
+#define HWEMUL_FEATURE_JUMP     (1 << 2)
+#define HWEMUL_FEATURE_AES_OTP  (1 << 3)
+
+struct usb_resp_info_features_t
+{
+    uint32_t feature_mask;
+};
+
+/**
+ * HWEMUL_GET_LOG: only if has HWEMUL_FEATURE_LOG.
+ * The log is returned as part of the control transfer.
+ */
+
+/**
+ * HWEMUL_RW_MEM: only if has HWEMUL_FEATURE_MEM.
+ * The 32-bit address is split into two parts.
+ * The low 16-bit are stored in wValue and the upper
+ * 16-bit are stored in wIndex. Depending on the transfer direction,
+ * the transfer is either a read or a write. */
+
+/**
+ * HWEMUL_x: only if has HWEMUL_FEATURE_x where x=CALL or JUMP.
+ * The 32-bit address is split into two parts.
+ * The low 16-bit are stored in wValue and the upper
+ * 16-bit are stored in wIndex. Depending on the transfer direction,
+ * the transfer is either a read or a write. */
+
+/**
+ * HWEMUL_AES_OTP: only if has HWEMUL_FEATURE_AES_OTP.
+ * The control transfer contains the data to be en/decrypted and the data
+ * is sent back on the interrupt endpoint. The first 16-bytes of the data
+ * are interpreted as the IV. The output format is the same.
+ * The wValue field contains the parameters of the process. */
+#define HWEMUL_AES_OTP_ENCRYPT  (1 << 0)
+
+#endif /* __HWEMUL_PROTOCOL__ */
diff --git a/utils/hwstub/lib/Makefile b/utils/hwstub/lib/Makefile
new file mode 100644
index 0000000000..7280fe8e38
--- /dev/null
+++ b/utils/hwstub/lib/Makefile
@@ -0,0 +1,27 @@
+CC=gcc
+AR=ar
+CFLAGS=-W -Wall -O2 `pkg-config --cflags libusb-1.0` -std=c99 -g -fPIC
+LDFLAGS=`pkg-config --libs libusb-1.0` -fPIC
+LIB=libhwemul.a
+REGTOOLS=../../regtools
+DESC=$(REGTOOLS)/desc
+HWEMULGEN=$(REGTOOLS)/hwemulgen
+HWEMULSOC_PREFIX=hwemul_soc
+SRC=$(wildcard *.c) $(HWEMULSOC_PREFIX).c
+OBJ=$(SRC:.c=.o)
+
+all: $(LIB) $(EXEC)
+
+$(HWEMULSOC_PREFIX).c $(HWEMULSOC_PREFIX).h:
+        $(HWEMULGEN) $(DESC)/*.xml $(HWEMULSOC_PREFIX)
+
+%.o: %.c $(HWEMULSOC_PREFIX).h
+        $(CC) $(CFLAGS) -c -o $@ $<
+
+$(LIB): $(OBJ)
+        $(AR) rcs $@ $^
+
+clean:
+        rm -rf $(OBJ) $(LIB) $(HWEMULSOC_PREFIX).c $(HWEMULSOC_PREFIX).h
+
+
diff --git a/utils/hwstub/lib/hwemul.c b/utils/hwstub/lib/hwemul.c
new file mode 100644
index 0000000000..3e2e6de38a
--- /dev/null
+++ b/utils/hwstub/lib/hwemul.c
@@ -0,0 +1,175 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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 "hwemul.h"
+#include "hwemul_soc.h"
+
+#ifndef MIN
+#define MIN(a,b) ((a) < (b) ? (a) : (b))
+#endif
+
+/* requires then ->handle field only */
+int hwemul_probe(struct hwemul_device_t *dev)
+{
+    libusb_device *mydev = libusb_get_device(dev->handle);
+
+    int config_id;
+    libusb_get_configuration(dev->handle, &config_id);
+    struct libusb_config_descriptor *config;
+    libusb_get_active_config_descriptor(mydev, &config);
+
+    const struct libusb_endpoint_descriptor *endp = NULL;
+    int intf;
+    for(intf = 0; intf < config->bNumInterfaces; intf++)
+    {
+        if(config->interface[intf].num_altsetting != 1)
+            continue;
+        const struct libusb_interface_descriptor *interface =
+            &config->interface[intf].altsetting[0];
+        if(interface->bNumEndpoints != 3 ||
+                interface->bInterfaceClass != HWEMUL_CLASS ||
+                interface->bInterfaceSubClass != HWEMUL_SUBCLASS ||
+                interface->bInterfaceProtocol != HWEMUL_PROTOCOL)
+            continue;
+        dev->intf = intf;
+        dev->bulk_in = dev->bulk_out = dev->int_in = -1;
+        for(int ep = 0; ep < interface->bNumEndpoints; ep++)
+        {
+            endp = &interface->endpoint[ep];
+            if((endp->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) == LIBUSB_TRANSFER_TYPE_INTERRUPT &&
+                    (endp->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) == LIBUSB_ENDPOINT_IN)
+                dev->int_in = endp->bEndpointAddress;
+            if((endp->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) == LIBUSB_TRANSFER_TYPE_BULK &&
+                    (endp->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) == LIBUSB_ENDPOINT_IN)
+                dev->bulk_in = endp->bEndpointAddress;
+            if((endp->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) == LIBUSB_TRANSFER_TYPE_BULK &&
+                    (endp->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) == LIBUSB_ENDPOINT_OUT)
+                dev->bulk_out = endp->bEndpointAddress;
+        }
+        if(dev->bulk_in == -1 || dev->bulk_out == -1 || dev->int_in == -1)
+            continue;
+        break;
+    }
+    if(intf == config->bNumInterfaces)
+        return 1;
+
+    return libusb_claim_interface(dev->handle, intf);
+}
+
+int hwemul_release(struct hwemul_device_t *dev)
+{
+    return libusb_release_interface(dev->handle, dev->intf);
+}
+
+int hwemul_get_info(struct hwemul_device_t *dev, uint16_t idx, void *info, size_t sz)
+{
+    return libusb_control_transfer(dev->handle,
+        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN,
+        HWEMUL_GET_INFO, 0, idx, info, sz, 1000);
+}
+
+int hwemul_get_log(struct hwemul_device_t *dev, void *buf, size_t sz)
+{
+    return libusb_control_transfer(dev->handle,
+        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN,
+        HWEMUL_GET_LOG, 0, 0, buf, sz, 1000);
+}
+
+int hwemul_rw_mem(struct hwemul_device_t *dev, int read, uint32_t addr, void *buf, size_t sz)
+{
+    size_t tot_sz = 0;
+    while(sz)
+    {
+        uint16_t xfer = MIN(1 * 1024, sz);
+        int ret = libusb_control_transfer(dev->handle,
+            LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE |
+            (read ? LIBUSB_ENDPOINT_IN : LIBUSB_ENDPOINT_OUT),
+            HWEMUL_RW_MEM, addr & 0xffff, addr >> 16, buf, xfer, 1000);
+        if(ret != xfer)
+            return ret;
+        sz -= xfer;
+        addr += xfer;
+        buf += xfer;
+        tot_sz += xfer;
+    }
+    return tot_sz;
+}
+
+int hwemul_call(struct hwemul_device_t *dev, uint32_t addr)
+{
+    return libusb_control_transfer(dev->handle,
+            LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE |
+            LIBUSB_ENDPOINT_OUT, HWEMUL_CALL, addr & 0xffff, addr >> 16, NULL, 0,
+            1000);
+}
+
+int hwemul_jump(struct hwemul_device_t *dev, uint32_t addr)
+{
+    return libusb_control_transfer(dev->handle,
+            LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE |
+            LIBUSB_ENDPOINT_OUT, HWEMUL_JUMP, addr & 0xffff, addr >> 16, NULL, 0,
+            1000);
+}
+
+const char *hwemul_get_product_string(struct usb_resp_info_stmp_t *stmp)
+{
+    switch(stmp->chipid)
+    {
+        case 0x3700: return "STMP 3700";
+        case 0x37b0: return "STMP 3770";
+        case 0x3780: return "STMP 3780 / i.MX233";
+        default: return "unknown";
+    }
+}
+
+const char *hwemul_get_rev_string(struct usb_resp_info_stmp_t *stmp)
+{
+    switch(stmp->chipid)
+    {
+        case 0x37b0:
+        case 0x3780:
+            switch(stmp->rev)
+            {
+                case 0: return "TA1";
+                case 1: return "TA2";
+                case 2: return "TA3";
+                case 3: return "TA4";
+                default: return "unknown";
+            }
+            break;
+        default:
+            return "unknown";
+    }
+}
+
+int hwemul_aes_otp(struct hwemul_device_t *dev, void *buf, size_t sz, uint16_t param)
+{
+    int ret = libusb_control_transfer(dev->handle,
+            LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE |
+            LIBUSB_ENDPOINT_OUT, HWEMUL_AES_OTP, param, 0, buf, sz,
+            1000);
+    if(ret <0 || (unsigned)ret != sz)
+        return -1;
+    int xfer;
+    ret = libusb_interrupt_transfer(dev->handle, dev->int_in, buf, sz, &xfer, 1000);
+    if(ret < 0 || (unsigned)xfer != sz)
+        return -1;
+    return ret;
+}
diff --git a/utils/hwstub/lib/hwemul.h b/utils/hwstub/lib/hwemul.h
new file mode 100644
index 0000000000..376ba65381
--- /dev/null
+++ b/utils/hwstub/lib/hwemul.h
@@ -0,0 +1,63 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL__
+#define __HWEMUL__
+
+#include <libusb.h>
+#include "hwemul_protocol.h"
+#include "hwemul_soc.h"
+
+/**
+ *
+ * Low-Level interface
+ *
+ */
+
+struct hwemul_device_t
+{
+    libusb_device_handle *handle;
+    int intf;
+    int bulk_in;
+    int bulk_out;
+    int int_in;
+};
+
+/* Requires then ->handle field only. Returns 0 on success */
+int hwemul_probe(struct hwemul_device_t *dev);
+/* Returns 0 on success */
+int hwemul_release(struct hwemul_device_t *dev);
+
+/* Returns number of bytes filled */
+int hwemul_get_info(struct hwemul_device_t *dev, uint16_t idx, void *info, size_t sz);
+/* Returns number of bytes filled */
+int hwemul_get_log(struct hwemul_device_t *dev, void *buf, size_t sz);
+/* Returns number of bytes written/read or <0 on error */
+int hwemul_rw_mem(struct hwemul_device_t *dev, int read, uint32_t addr, void *buf, size_t sz);
+/* Returns <0 on error */
+int hwemul_call(struct hwemul_device_t *dev, uint32_t addr);
+int hwemul_jump(struct hwemul_device_t *dev, uint32_t addr);
+/* Returns <0 on error. The size must be a multiple of 16. */
+int hwemul_aes_otp(struct hwemul_device_t *dev, void *buf, size_t sz, uint16_t param);
+
+const char *hwemul_get_product_string(struct usb_resp_info_stmp_t *stmp);
+const char *hwemul_get_rev_string(struct usb_resp_info_stmp_t *stmp);
+
+#endif /* __HWEMUL__ */
\ No newline at end of file
diff --git a/utils/hwstub/lib/hwemul_protocol.h b/utils/hwstub/lib/hwemul_protocol.h
new file mode 100644
index 0000000000..d3ffb6ce00
--- /dev/null
+++ b/utils/hwstub/lib/hwemul_protocol.h
@@ -0,0 +1 @@
+#include "../hwemul_protocol.h"
diff --git a/utils/hwstub/stmp/Makefile b/utils/hwstub/stmp/Makefile
new file mode 100644
index 0000000000..ca61fe392d
--- /dev/null
+++ b/utils/hwstub/stmp/Makefile
@@ -0,0 +1,92 @@
+PREFIX?=arm-elf-eabi-
+CC=$(PREFIX)gcc
+LD=$(PREFIX)gcc
+AS=$(PREFIX)gcc
+OC=$(PREFIX)objcopy
+SBTOOLS=../../sbtools/
+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
+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
+
+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)
+
+TOOLS=../../../../tools/
+SCRAMBLE=$(TOOLS)/scramble
+
+EXEC=$(EXEC_SB) $(EXEC_SB_3700) $(EXEC_ELF) $(EXEC_ELF_3700)
+
+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
+        @cp -f $*.d $*.d.tmp
+        @sed -e 's/.*://' -e 's/\\$$//' < $*.d.tmp | fmt -1 | \
+          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)
+        $(CC) -E -x c - < $< | sed '/#/d' > $@
+        
+$(EXEC_ELF): $(OBJ) link.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)
+
+clean:
+        rm -rf $(OBJ) $(OBJ_3700) $(DEPS) $(EXEC) *.map
diff --git a/utils/hwstub/stmp/config.h b/utils/hwstub/stmp/config.h
new file mode 100644
index 0000000000..6bd995e147
--- /dev/null
+++ b/utils/hwstub/stmp/config.h
@@ -0,0 +1,33 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL_CONFIG__
+#define __HWEMUL_CONFIG__
+
+#define MEMORYSIZE      0
+#define STACK_SIZE      0x1000
+#define MAX_LOGF_SIZE   128
+
+#define IRAM_ORIG       0
+#define IRAM_SIZE       0x8000
+#define DRAM_ORIG       0x40000000
+#define DRAM_SIZE       (MEMORYSIZE * 0x100000)
+
+#endif /* __HWEMUL_CONFIG__ */
diff --git a/utils/hwstub/stmp/crt0.S b/utils/hwstub/stmp/crt0.S
new file mode 100644
index 0000000000..e2d4742d36
--- /dev/null
+++ b/utils/hwstub/stmp/crt0.S
@@ -0,0 +1,17 @@
+.section    .text,"ax",%progbits
+.code       32
+.align      0x04
+.global     start
+start:
+    msr     cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */
+    ldr     sp, =oc_stackend
+    /* clear bss */
+    ldr     r2, =bss_start
+    ldr     r3, =bss_end
+    mov     r4, #0
+1:
+    cmp     r3, r2
+    strhi   r4, [r2], #4
+    bhi     1b
+    /* jump to C code */
+    b       main
diff --git a/utils/hwstub/stmp/format.c b/utils/hwstub/stmp/format.c
new file mode 100644
index 0000000000..f5783159c0
--- /dev/null
+++ b/utils/hwstub/stmp/format.c
@@ -0,0 +1,223 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2002 by Gary Czvitkovicz
+ *
+ * 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 <stdarg.h>
+#include <limits.h>
+#include "stddef.h"
+#include "string.h"
+#include "format.h"
+
+static const char hexdigit[] = "0123456789ABCDEF";
+
+void vuprintf(
+    /* call 'push()' for each output letter */
+    int (*push)(void *userp, unsigned char data),
+    void *userp,
+    const char *fmt,
+    va_list ap)
+{
+    char *str;
+    char tmpbuf[12], pad;
+    int ch, width, val, sign, precision;
+    long lval, lsign;
+    unsigned int uval;
+    unsigned long ulval;
+    size_t uszval;
+    ssize_t szval, szsign;
+    bool ok = true;
+
+    tmpbuf[sizeof tmpbuf - 1] = '\0';
+
+    while ((ch = *fmt++) != '\0' && ok)
+    {
+    if (ch == '%')
+    {
+        ch = *fmt++;
+        pad = ' ';
+        if (ch == '0')
+        pad = '0';
+
+        width = 0;
+        while (ch >= '0' && ch <= '9')
+        {
+        width = 10*width + ch - '0';
+        ch = *fmt++;
+        }
+        
+        precision = 0;
+        if(ch == '.')
+        {
+            ch = *fmt++;
+            while (ch >= '0' && ch <= '9')
+            {
+                precision = 10*precision + ch - '0';
+                ch = *fmt++;
+            }
+        } else {
+            precision = INT_MAX;
+        }
+
+        str = tmpbuf + sizeof tmpbuf - 1;
+        switch (ch)
+        {
+        case 'c':
+            *--str = va_arg (ap, int);
+            break;
+
+        case 's':
+            str = va_arg (ap, char*);
+            break;
+
+        case 'd':
+            val = sign = va_arg (ap, int);
+            if (val < 0)
+            val = -val;
+            do
+            {
+            *--str = (val % 10) + '0';
+            val /= 10;
+            }
+            while (val > 0);
+            if (sign < 0)
+            *--str = '-';
+            break;
+
+        case 'u':
+            uval = va_arg(ap, unsigned int);
+            do
+            {
+            *--str = (uval % 10) + '0';
+            uval /= 10;
+            }
+            while (uval > 0);
+            break;
+
+        case 'x':
+        case 'X':
+            pad='0';
+            uval = va_arg (ap, int);
+            do
+            {
+            *--str = hexdigit[uval & 0xf];
+            uval >>= 4;
+            }
+            while (uval);
+            break;
+
+        case 'l':
+            ch = *fmt++;
+            switch(ch) {
+                case 'x':
+                case 'X':
+                    pad='0';
+                    ulval = va_arg (ap, long);
+                    do
+                    {
+                        *--str = hexdigit[ulval & 0xf];
+                        ulval >>= 4;
+                    }
+                    while (ulval);
+                    break;
+                case 'd':
+                    lval = lsign = va_arg (ap, long);
+                    if (lval < 0)
+                        lval = -lval;
+                    do
+                    {
+                        *--str = (lval % 10) + '0';
+                        lval /= 10;
+                    }
+                    while (lval > 0);
+                    if (lsign < 0)
+                        *--str = '-';
+                    break;
+
+                case 'u':
+                    ulval = va_arg(ap, unsigned long);
+                    do
+                    {
+                        *--str = (ulval % 10) + '0';
+                        ulval /= 10;
+                    }
+                    while (ulval > 0);
+                    break;
+
+                default:
+                    *--str = 'l';
+                    *--str = ch;
+            }
+
+            break;
+
+        case 'z':
+            ch = *fmt++;
+            switch(ch) {
+                case 'd':
+                    szval = szsign = va_arg (ap, ssize_t);
+                    if (szval < 0)
+                        szval = -szval;
+                    do
+                    {
+                        *--str = (szval % 10) + '0';
+                        szval /= 10;
+                    }
+                    while (szval > 0);
+                    if (szsign < 0)
+                        *--str = '-';
+                    break;
+
+                case 'u':
+                    uszval = va_arg(ap, size_t);
+                    do
+                    {
+                        *--str = (uszval % 10) + '0';
+                        uszval /= 10;
+                    }
+                    while (uszval > 0);
+                    break;
+
+                default:
+                    *--str = 'z';
+                    *--str = ch;
+            }
+
+            break;
+
+        default:
+            *--str = ch;
+            break;
+        }
+
+        if (width > 0)
+        {
+        width -= strlen (str);
+        while (width-- > 0 && ok)
+            ok=push(userp, pad);
+        }
+        while (*str != '\0' && ok && precision--)
+                ok=push(userp, *str++);
+    }
+    else
+        ok=push(userp, ch);
+    }
+}
+
diff --git a/utils/hwstub/stmp/format.h b/utils/hwstub/stmp/format.h
new file mode 100644
index 0000000000..a514c882ba
--- /dev/null
+++ b/utils/hwstub/stmp/format.h
@@ -0,0 +1,29 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL_FORMAT__
+#define __HWEMUL_FORMAT__
+
+#include <stdarg.h>
+
+void vuprintf(int (*push)(void *userp, unsigned char data),
+              void *userp, const char *fmt, va_list ap);
+
+#endif /* __HWEMUL_FORMAT__ */
diff --git a/utils/hwstub/stmp/hwemul.db b/utils/hwstub/stmp/hwemul.db
new file mode 100644
index 0000000000..7a6f930f57
--- /dev/null
+++ b/utils/hwstub/stmp/hwemul.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
+{
+    hwemul = "hwemul.elf";
+}
+
+section(0)
+{
+    load hwemul;
+    jump hwemul(1);
+}
+
diff --git a/utils/hwstub/stmp/hwemul.lds b/utils/hwstub/stmp/hwemul.lds
new file mode 100644
index 0000000000..7e3ac747a2
--- /dev/null
+++ b/utils/hwstub/stmp/hwemul.lds
@@ -0,0 +1,70 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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/link.lds b/utils/hwstub/stmp/link.lds
new file mode 100644
index 0000000000..97b259955f
--- /dev/null
+++ b/utils/hwstub/stmp/link.lds
@@ -0,0 +1,49 @@
+
+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.c b/utils/hwstub/stmp/logf.c
new file mode 100644
index 0000000000..3ccc5c5e9f
--- /dev/null
+++ b/utils/hwstub/stmp/logf.c
@@ -0,0 +1,68 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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"
+#include "logf.h"
+#include "format.h"
+#include "string.h"
+
+static unsigned char logfbuffer[MAX_LOGF_SIZE];
+static int logfread = 0;
+static int logfwrite = 0;
+static int logfen = true;
+
+void enable_logf(bool en)
+{
+    logfen = en;
+}
+
+static int logf_push(void *userp, unsigned char c)
+{
+    (void)userp;
+    
+    logfbuffer[logfwrite++] = c;
+    if(logfwrite == MAX_LOGF_SIZE)
+        logfwrite = 0;
+    return true;
+}
+
+void logf(const char *fmt, ...)
+{
+    if(!logfen) return;
+    va_list ap;
+    va_start(ap, fmt);
+    vuprintf(logf_push, NULL, fmt, ap);
+    va_end(ap);
+}
+
+size_t logf_readback(char *buf, size_t max_size)
+{
+    if(logfread == logfwrite)
+        return 0;
+    if(logfread < logfwrite)
+        max_size = MIN(max_size, (size_t)(logfwrite - logfread));
+    else
+        max_size = MIN(max_size, (size_t)(MAX_LOGF_SIZE - logfread));
+    memcpy(buf, &logfbuffer[logfread], max_size);
+    logfread += max_size;
+    if(logfread == MAX_LOGF_SIZE)
+        logfread = 0;
+    return max_size;
+}
diff --git a/utils/hwstub/stmp/logf.h b/utils/hwstub/stmp/logf.h
new file mode 100644
index 0000000000..5aa882a630
--- /dev/null
+++ b/utils/hwstub/stmp/logf.h
@@ -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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL_LOGF__
+#define __HWEMUL_LOGF__
+
+#include "stddef.h"
+#include <stdarg.h>
+
+void enable_logf(bool en);
+void logf(const char *fmt, ...);
+size_t logf_readback(char *buf, size_t max_size);
+
+#endif /* __HWEMUL_LOGF__ */
diff --git a/utils/hwstub/stmp/main.c b/utils/hwstub/stmp/main.c
new file mode 100644
index 0000000000..09bb6c7714
--- /dev/null
+++ b/utils/hwstub/stmp/main.c
@@ -0,0 +1,1409 @@
+#include "stddef.h"
+#include "protocol.h"
+#include "logf.h"
+#include "usb_ch9.h"
+
+extern unsigned char oc_codestart[];
+extern unsigned char oc_codeend[];
+extern unsigned char oc_stackstart[];
+extern unsigned char oc_stackend[];
+extern unsigned char oc_bufferstart[];
+extern unsigned char oc_bufferend[];
+
+#define oc_codesize ((size_t)(oc_codeend - oc_codestart))
+#define oc_stacksize ((size_t)(oc_stackend - oc_stackstart))
+#define oc_buffersize ((size_t)(oc_bufferend - oc_bufferstart))
+
+/**
+ *
+ * Common
+ *
+ */
+#define MIN(a,b) ((a) < (b) ? (a) : (b))
+
+#define __REG_SET(reg)  (*((volatile uint32_t *)(&reg + 1)))
+#define __REG_CLR(reg)  (*((volatile uint32_t *)(&reg + 2)))
+#define __REG_TOG(reg)  (*((volatile uint32_t *)(&reg + 3)))
+
+#define __BLOCK_SFTRST  (1 << 31)
+#define __BLOCK_CLKGATE (1 << 30)
+
+#define __XTRACT(reg, field)    ((reg & reg##__##field##_BM) >> reg##__##field##_BP)
+#define __XTRACT_EX(val, field)    (((val) & field##_BM) >> field##_BP)
+#define __FIELD_SET(reg, field, val) reg = (reg & ~reg##__##field##_BM) | (val << reg##__##field##_BP)
+
+/**
+ *
+ * Pin control
+ *
+ */
+
+#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)
+{
+    __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;
+}
+
+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;
+}
+
+/**
+ *
+ * USB subsystem
+ *
+ */
+
+#define USB_BASE            0x80080000
+#define USB_NUM_ENDPOINTS   2
+#define MAX_PKT_SIZE        1024
+#define MAX_PKT_SIZE_EP0    64
+
+/* USB device mode registers (Little Endian) */
+#define REG_USBCMD           (*(volatile unsigned int *)(USB_BASE+0x140))
+#define REG_DEVICEADDR       (*(volatile unsigned int *)(USB_BASE+0x154))
+#define REG_ENDPOINTLISTADDR (*(volatile unsigned int *)(USB_BASE+0x158))
+#define REG_PORTSC1          (*(volatile unsigned int *)(USB_BASE+0x184))
+#define REG_USBMODE          (*(volatile unsigned int *)(USB_BASE+0x1a8))
+#define REG_ENDPTSETUPSTAT   (*(volatile unsigned int *)(USB_BASE+0x1ac))
+#define REG_ENDPTPRIME       (*(volatile unsigned int *)(USB_BASE+0x1b0))
+#define REG_ENDPTSTATUS      (*(volatile unsigned int *)(USB_BASE+0x1b8))
+#define REG_ENDPTCOMPLETE    (*(volatile unsigned int *)(USB_BASE+0x1bc))
+#define REG_ENDPTCTRL0       (*(volatile unsigned int *)(USB_BASE+0x1c0))
+#define REG_ENDPTCTRL1       (*(volatile unsigned int *)(USB_BASE+0x1c4))
+#define REG_ENDPTCTRL2       (*(volatile unsigned int *)(USB_BASE+0x1c8))
+#define REG_ENDPTCTRL(_x_)   (*(volatile unsigned int *)(USB_BASE+0x1c0+4*(_x_)))
+
+/* USB CMD  Register Bit Masks */
+#define USBCMD_RUN                            (0x00000001)
+#define USBCMD_CTRL_RESET                     (0x00000002)
+#define USBCMD_PERIODIC_SCHEDULE_EN           (0x00000010)
+#define USBCMD_ASYNC_SCHEDULE_EN              (0x00000020)
+#define USBCMD_INT_AA_DOORBELL                (0x00000040)
+#define USBCMD_ASP                            (0x00000300)
+#define USBCMD_ASYNC_SCH_PARK_EN              (0x00000800)
+#define USBCMD_SUTW                           (0x00002000)
+#define USBCMD_ATDTW                          (0x00004000)
+#define USBCMD_ITC                            (0x00FF0000)
+
+/* Device Address bit masks */
+#define USBDEVICEADDRESS_MASK                 (0xFE000000)
+#define USBDEVICEADDRESS_BIT_POS              (25)
+
+/* Endpoint Setup Status bit masks */
+#define EPSETUP_STATUS_EP0                    (0x00000001)
+
+/* PORTSCX  Register Bit Masks */
+#define PORTSCX_CURRENT_CONNECT_STATUS         (0x00000001)
+#define PORTSCX_CONNECT_STATUS_CHANGE          (0x00000002)
+#define PORTSCX_PORT_ENABLE                    (0x00000004)
+#define PORTSCX_PORT_EN_DIS_CHANGE             (0x00000008)
+#define PORTSCX_OVER_CURRENT_ACT               (0x00000010)
+#define PORTSCX_OVER_CURRENT_CHG               (0x00000020)
+#define PORTSCX_PORT_FORCE_RESUME              (0x00000040)
+#define PORTSCX_PORT_SUSPEND                   (0x00000080)
+#define PORTSCX_PORT_RESET                     (0x00000100)
+#define PORTSCX_LINE_STATUS_BITS               (0x00000C00)
+#define PORTSCX_PORT_POWER                     (0x00001000)
+#define PORTSCX_PORT_INDICTOR_CTRL             (0x0000C000)
+#define PORTSCX_PORT_TEST_CTRL                 (0x000F0000)
+#define PORTSCX_WAKE_ON_CONNECT_EN             (0x00100000)
+#define PORTSCX_WAKE_ON_CONNECT_DIS            (0x00200000)
+#define PORTSCX_WAKE_ON_OVER_CURRENT           (0x00400000)
+#define PORTSCX_PHY_LOW_POWER_SPD              (0x00800000)
+#define PORTSCX_PORT_FORCE_FULL_SPEED          (0x01000000)
+#define PORTSCX_PORT_SPEED_MASK                (0x0C000000)
+#define PORTSCX_PORT_WIDTH                     (0x10000000)
+#define PORTSCX_PHY_TYPE_SEL                   (0xC0000000)
+
+/* bit 11-10 are line status */
+#define PORTSCX_LINE_STATUS_SE0                (0x00000000)
+#define PORTSCX_LINE_STATUS_JSTATE             (0x00000400)
+#define PORTSCX_LINE_STATUS_KSTATE             (0x00000800)
+#define PORTSCX_LINE_STATUS_UNDEF              (0x00000C00)
+#define PORTSCX_LINE_STATUS_BIT_POS            (10)
+
+/* bit 15-14 are port indicator control */
+#define PORTSCX_PIC_OFF                        (0x00000000)
+#define PORTSCX_PIC_AMBER                      (0x00004000)
+#define PORTSCX_PIC_GREEN                      (0x00008000)
+#define PORTSCX_PIC_UNDEF                      (0x0000C000)
+#define PORTSCX_PIC_BIT_POS                    (14)
+
+/* bit 19-16 are port test control */
+#define PORTSCX_PTC_DISABLE                    (0x00000000)
+#define PORTSCX_PTC_JSTATE                     (0x00010000)
+#define PORTSCX_PTC_KSTATE                     (0x00020000)
+#define PORTSCX_PTC_SE0NAK                     (0x00030000)
+#define PORTSCX_PTC_PACKET                     (0x00040000)
+#define PORTSCX_PTC_FORCE_EN                   (0x00050000)
+#define PORTSCX_PTC_BIT_POS                    (16)
+
+/* bit 27-26 are port speed */
+#define PORTSCX_PORT_SPEED_FULL                (0x00000000)
+#define PORTSCX_PORT_SPEED_LOW                 (0x04000000)
+#define PORTSCX_PORT_SPEED_HIGH                (0x08000000)
+#define PORTSCX_PORT_SPEED_UNDEF               (0x0C000000)
+#define PORTSCX_SPEED_BIT_POS                  (26)
+
+/* bit 28 is parallel transceiver width for UTMI interface */
+#define PORTSCX_PTW                            (0x10000000)
+#define PORTSCX_PTW_8BIT                       (0x00000000)
+#define PORTSCX_PTW_16BIT                      (0x10000000)
+
+/* bit 31-30 are port transceiver select */
+#define PORTSCX_PTS_UTMI                       (0x00000000)
+#define PORTSCX_PTS_CLASSIC                    (0x40000000)
+#define PORTSCX_PTS_ULPI                       (0x80000000)
+#define PORTSCX_PTS_FSLS                       (0xC0000000)
+#define PORTSCX_PTS_BIT_POS                    (30)
+
+/* USB MODE Register Bit Masks */
+#define USBMODE_CTRL_MODE_IDLE                (0x00000000)
+#define USBMODE_CTRL_MODE_DEVICE              (0x00000002)
+#define USBMODE_CTRL_MODE_HOST                (0x00000003)
+#define USBMODE_CTRL_MODE_RSV                 (0x00000001)
+#define USBMODE_SETUP_LOCK_OFF                (0x00000008)
+#define USBMODE_STREAM_DISABLE                (0x00000010)
+
+/* ENDPOINTCTRLx  Register Bit Masks */
+#define EPCTRL_TX_ENABLE                       (0x00800000)
+#define EPCTRL_TX_DATA_TOGGLE_RST              (0x00400000)    /* Not EP0 */
+#define EPCTRL_TX_DATA_TOGGLE_INH              (0x00200000)    /* Not EP0 */
+#define EPCTRL_TX_TYPE                         (0x000C0000)
+#define EPCTRL_TX_DATA_SOURCE                  (0x00020000)    /* Not EP0 */
+#define EPCTRL_TX_EP_STALL                     (0x00010000)
+#define EPCTRL_RX_ENABLE                       (0x00000080)
+#define EPCTRL_RX_DATA_TOGGLE_RST              (0x00000040)    /* Not EP0 */
+#define EPCTRL_RX_DATA_TOGGLE_INH              (0x00000020)    /* Not EP0 */
+#define EPCTRL_RX_TYPE                         (0x0000000C)
+#define EPCTRL_RX_DATA_SINK                    (0x00000002)    /* Not EP0 */
+#define EPCTRL_RX_EP_STALL                     (0x00000001)
+
+/* bit 19-18 and 3-2 are endpoint type */
+#define EPCTRL_TX_EP_TYPE_SHIFT                (18)
+#define EPCTRL_RX_EP_TYPE_SHIFT                (2)
+
+#define QH_MULT_POS                            (30)
+#define QH_ZLT_SEL                             (0x20000000)
+#define QH_MAX_PKT_LEN_POS                     (16)
+#define QH_IOS                                 (0x00008000)
+#define QH_NEXT_TERMINATE                      (0x00000001)
+#define QH_IOC                                 (0x00008000)
+#define QH_MULTO                               (0x00000C00)
+#define QH_STATUS_HALT                         (0x00000040)
+#define QH_STATUS_ACTIVE                       (0x00000080)
+#define EP_QUEUE_CURRENT_OFFSET_MASK         (0x00000FFF)
+#define EP_QUEUE_HEAD_NEXT_POINTER_MASK      (0xFFFFFFE0)
+#define EP_QUEUE_FRINDEX_MASK                (0x000007FF)
+#define EP_MAX_LENGTH_TRANSFER               (0x4000)
+
+#define DTD_NEXT_TERMINATE                   (0x00000001)
+#define DTD_IOC                              (0x00008000)
+#define DTD_STATUS_ACTIVE                    (0x00000080)
+#define DTD_STATUS_HALTED                    (0x00000040)
+#define DTD_STATUS_DATA_BUFF_ERR             (0x00000020)
+#define DTD_STATUS_TRANSACTION_ERR           (0x00000008)
+#define DTD_RESERVED_FIELDS                  (0x80007300)
+#define DTD_ADDR_MASK                        (0xFFFFFFE0)
+#define DTD_PACKET_SIZE                      (0x7FFF0000)
+#define DTD_LENGTH_BIT_POS                   (16)
+#define DTD_ERROR_MASK                       (DTD_STATUS_HALTED | \
+                                               DTD_STATUS_DATA_BUFF_ERR | \
+                                               DTD_STATUS_TRANSACTION_ERR)
+/*-------------------------------------------------------------------------*/
+/* manual: 32.13.2 Endpoint Transfer Descriptor (dTD) */
+struct transfer_descriptor {
+    unsigned int next_td_ptr;           /* Next TD pointer(31-5), T(0) set
+                                           indicate invalid */
+    unsigned int size_ioc_sts;          /* Total bytes (30-16), IOC (15),
+                                           MultO(11-10), STS (7-0)  */
+    unsigned int buff_ptr0;             /* Buffer pointer Page 0 */
+    unsigned int buff_ptr1;             /* Buffer pointer Page 1 */
+    unsigned int buff_ptr2;             /* Buffer pointer Page 2 */
+    unsigned int buff_ptr3;             /* Buffer pointer Page 3 */
+    unsigned int buff_ptr4;             /* Buffer pointer Page 4 */
+    unsigned int reserved;
+} __attribute__ ((packed));
+
+static struct transfer_descriptor td_array[USB_NUM_ENDPOINTS*2]
+    __attribute__((aligned(32)));
+
+/* manual: 32.13.1 Endpoint Queue Head (dQH) */
+struct queue_head {
+    unsigned int max_pkt_length;    /* Mult(31-30) , Zlt(29) , Max Pkt len
+                                       and IOS(15) */
+    unsigned int curr_dtd_ptr;      /* Current dTD Pointer(31-5) */
+    struct transfer_descriptor dtd; /* dTD overlay */
+    unsigned int setup_buffer[2];   /* Setup data 8 bytes */
+    unsigned int reserved;          /* for software use, pointer to the first TD */
+    unsigned int status;            /* for software use, status of chain in progress */
+    unsigned int length;            /* for software use, transfered bytes of chain in progress */
+    unsigned int wait;              /* for softwate use, indicates if the transfer is blocking */
+} __attribute__((packed));
+
+static struct queue_head qh_array[USB_NUM_ENDPOINTS*2] __attribute__((aligned(2048)));
+
+static const unsigned int pipe2mask[] = {
+    0x01, 0x010000,
+    0x02, 0x020000,
+    0x04, 0x040000,
+    0x08, 0x080000,
+    0x10, 0x100000,
+};
+
+/* return transfered size if wait=true */
+static int prime_transfer(int ep_num, void *ptr, int len, bool send, bool wait)
+{
+    int pipe = ep_num * 2 + (send ? 1 : 0);
+    unsigned mask = pipe2mask[pipe];
+    struct transfer_descriptor *td = &td_array[pipe];
+    struct queue_head* qh = &qh_array[pipe];
+    
+    /* prepare TD */
+    td->next_td_ptr = DTD_NEXT_TERMINATE;
+    td->size_ioc_sts = (len<< DTD_LENGTH_BIT_POS) | DTD_STATUS_ACTIVE;
+    td->buff_ptr0 = (unsigned int)ptr;
+    td->buff_ptr1 = ((unsigned int)ptr & 0xfffff000) + 0x1000;
+    td->buff_ptr2 = ((unsigned int)ptr & 0xfffff000) + 0x2000;
+    td->buff_ptr3 = ((unsigned int)ptr & 0xfffff000) + 0x3000;
+    td->buff_ptr4 = ((unsigned int)ptr & 0xfffff000) + 0x4000;
+    td->reserved = 0;
+    /* prime */
+    qh->dtd.next_td_ptr = (unsigned int)td;
+    qh->dtd.size_ioc_sts &= ~(QH_STATUS_HALT | QH_STATUS_ACTIVE);
+    REG_ENDPTPRIME |= mask;
+    /* wait for priming to be taken into account */
+    while(!(REG_ENDPTSTATUS & mask));
+    /* wait for completion */
+    if(wait)
+    {
+        while(!(REG_ENDPTCOMPLETE & mask));
+        REG_ENDPTCOMPLETE = mask;
+        /* memory barrier */
+        asm volatile("":::"memory");
+        /* return transfered size */
+        return len - (td->size_ioc_sts >> DTD_LENGTH_BIT_POS);
+    }
+    else
+        return 0;
+}
+
+void usb_drv_set_address(int address)
+{
+    REG_DEVICEADDR = address << USBDEVICEADDRESS_BIT_POS;
+}
+
+/* endpoints */
+#define EP_CONTROL 0
+
+#define DIR_OUT 0
+#define DIR_IN 1
+
+#define EP_DIR(ep) (((ep) & USB_ENDPOINT_DIR_MASK) ? DIR_IN : DIR_OUT)
+#define EP_NUM(ep) ((ep) & USB_ENDPOINT_NUMBER_MASK)
+
+static int usb_drv_send_nonblocking(int endpoint, void* ptr, int length)
+{
+    return prime_transfer(EP_NUM(endpoint), ptr, length, true, false);
+}
+
+static int usb_drv_send(int endpoint, void* ptr, int length)
+{
+    return prime_transfer(EP_NUM(endpoint), ptr, length, true, true);
+}
+
+static int usb_drv_recv(int endpoint, void* ptr, int length)
+{
+    return prime_transfer(EP_NUM(endpoint), ptr, length, false, true);
+}
+
+static int usb_drv_recv_nonblocking(int endpoint, void* ptr, int length)
+{
+    return prime_transfer(EP_NUM(endpoint), ptr, length, false, false);
+}
+
+static int usb_drv_port_speed(void)
+{
+    return (REG_PORTSC1 & 0x08000000) ? 1 : 0;
+}
+
+static void usb_drv_stall(int endpoint, bool stall, bool in)
+{
+    int ep_num = EP_NUM(endpoint);
+
+    if(in)
+    {
+        if(stall)
+            REG_ENDPTCTRL(ep_num) |= EPCTRL_TX_EP_STALL;
+        else
+            REG_ENDPTCTRL(ep_num) &= ~EPCTRL_TX_EP_STALL;
+    }
+    else
+    {
+        if (stall)
+            REG_ENDPTCTRL(ep_num) |= EPCTRL_RX_EP_STALL;
+        else
+            REG_ENDPTCTRL(ep_num) &= ~EPCTRL_RX_EP_STALL;
+    }
+}
+
+static void usb_drv_configure_endpoint(int ep_num, int type)
+{
+    REG_ENDPTCTRL(ep_num) =
+        EPCTRL_RX_DATA_TOGGLE_RST | EPCTRL_RX_ENABLE |
+        EPCTRL_TX_DATA_TOGGLE_RST | EPCTRL_TX_ENABLE |
+            (type << EPCTRL_RX_EP_TYPE_SHIFT) |
+            (type << EPCTRL_TX_EP_TYPE_SHIFT);
+}
+
+/**
+ *
+ * Clock control
+ *
+ **/
+#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__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_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))
+
+#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))
+
+#define HW_APBX_CHx_CURCMDAR(i) (*(volatile uint32_t *)(HW_APBX_BASE + 0x100 + (i) * 0x70))
+
+#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)
+        ;
+}
+
+/**
+ *
+ * Digctl
+ *
+ */
+
+/* Digital control */
+#define HW_DIGCTL_BASE          0x8001C000
+#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))
+#define HW_DIGCTL_CHIPID__PRODUCT_CODE_BP   16
+#define HW_DIGCTL_CHIPID__PRODUCT_CODE_BM   0xffff0000
+#define HW_DIGCTL_CHIPID__REVISION_BP       0
+#define HW_DIGCTL_CHIPID__REVISION_BM       0xff
+
+static bool imx233_us_elapsed(uint32_t ref, unsigned us_delay)
+{
+    uint32_t cur = HW_DIGCTL_MICROSECONDS;
+    if(ref + us_delay <= ref)
+        return !(cur > ref) && !(cur < (ref + us_delay));
+    else
+        return (cur < ref) || cur >= (ref + us_delay);
+}
+
+static void udelay(unsigned us)
+{
+    uint32_t ref = HW_DIGCTL_MICROSECONDS;
+    while(!imx233_us_elapsed(ref, us));
+}
+
+#define HZ  1000000
+
+/**
+ *
+ * USB PHY
+ *
+ */
+/* 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))
+
+/**
+ *
+ * DCP
+ *
+ */
+#define HW_DCP_BASE         0x80028000
+
+#define HW_DCP_CTRL         (*(volatile unsigned long *)(HW_DCP_BASE + 0x0))
+
+#define HW_DCP_STAT         (*(volatile unsigned long *)(HW_DCP_BASE + 0x10))
+#define HW_DCP_STAT__IRQ(x) (1 << (x))
+
+#define HW_DCP_CHANNELCTRL  (*(volatile unsigned long *)(HW_DCP_BASE + 0x20))
+#define HW_DCP_CHANNELCTRL__ENABLE_CHANNEL(x)   (1 << (x))
+
+#define HW_DCP_CH0CMDPTR    (*(volatile unsigned long *)(HW_DCP_BASE + 0x100))
+
+#define HW_DCP_CH0SEMA      (*(volatile unsigned long *)(HW_DCP_BASE + 0x110))
+#define HW_DCP_CH0SEMA__INCREMENT(x)    (x)
+#define HW_DCP_CH0SEMA__VALUE_BP        16
+#define HW_DCP_CH0SEMA__VALUE_BM        (0xff << 16)
+#define HW_DCP_CH0STAT      (*(volatile unsigned long *)(HW_DCP_BASE + 0x120))
+
+#define HW_DCP_CTRL0__INTERRUPT_ENABLE  (1 << 0)
+#define HW_DCP_CTRL0__DECR_SEMAPHORE    (1 << 1)
+#define HW_DCP_CTRL0__ENABLE_MEMCOPY    (1 << 4)
+#define HW_DCP_CTRL0__ENABLE_CIPHER     (1 << 5)
+#define HW_DCP_CTRL0__ENABLE_HASH       (1 << 6)
+#define HW_DCP_CTRL0__CIPHER_ENCRYPT    (1 << 8)
+#define HW_DCP_CTRL0__CIPHER_INIT       (1 << 9)
+#define HW_DCP_CTRL0__OTP_KEY           (1 << 10)
+#define HW_DCP_CTRL0__HASH_INIT         (1 << 12)
+#define HW_DCP_CTRL0__HASH_TERM         (1 << 13)
+#define HW_DCP_CTRL0__HASH_OUTPUT       (1 << 15)
+
+#define HW_DCP_CTRL1__CIPHER_SELECT_BP  0
+#define HW_DCP_CTRL1__CIPHER_SELECT_BM  0xf
+#define HW_DCP_CTRL1__CIPHER_SELECT__AES128 0
+#define HW_DCP_CTRL1__CIPHER_MODE_BP    4
+#define HW_DCP_CTRL1__CIPHER_MODE_BM    0xf0
+#define HW_DCP_CTRL1__CIPHER_MODE__CBC  (1 << 4)
+#define HW_DCP_CTRL1__HASH_SELECT_BP    4
+#define HW_DCP_CTRL1__HASH_SELECT_BM    0xf00
+
+struct dcp_packet_t
+{
+    unsigned long next;
+    unsigned long ctrl0;
+    unsigned long ctrl1;
+    unsigned long src_buf;
+    unsigned long dst_buf;
+    unsigned long buf_sz;
+    unsigned long payload_ptr;
+    unsigned long status;
+} __attribute__((packed));
+
+/**
+ *
+ * Misc
+ *
+ */
+
+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;
+}
+
+/**
+ * 
+ * USB stack
+ * 
+ */
+
+static struct usb_device_descriptor __attribute__((aligned(2)))
+    device_descriptor=
+{
+    .bLength            = sizeof(struct usb_device_descriptor),
+    .bDescriptorType    = USB_DT_DEVICE,
+    .bcdUSB             = 0x0200,
+    .bDeviceClass       = USB_CLASS_PER_INTERFACE,
+    .bDeviceSubClass    = 0,
+    .bDeviceProtocol    = 0,
+    .bMaxPacketSize0    = 64,
+    .idVendor           = HWEMUL_USB_VID,
+    .idProduct          = HWEMUL_USB_PID,
+    .bcdDevice          = HWEMUL_VERSION_MAJOR << 8 | HWEMUL_VERSION_MINOR,
+    .iManufacturer      = 1,
+    .iProduct           = 2,
+    .iSerialNumber      = 3,
+    .bNumConfigurations = 1
+};
+
+#define USB_MAX_CURRENT 200
+
+static struct usb_config_descriptor __attribute__((aligned(2)))
+                                    config_descriptor =
+{
+    .bLength             = sizeof(struct usb_config_descriptor),
+    .bDescriptorType     = USB_DT_CONFIG,
+    .wTotalLength        = 0, /* will be filled in later */
+    .bNumInterfaces      = 1,
+    .bConfigurationValue = 1,
+    .iConfiguration      = 0,
+    .bmAttributes        = USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
+    .bMaxPower           = (USB_MAX_CURRENT + 1) / 2, /* In 2mA units */
+};
+
+/* main interface */
+static struct usb_interface_descriptor __attribute__((aligned(2)))
+    interface_descriptor =
+{
+    .bLength            = sizeof(struct usb_interface_descriptor),
+    .bDescriptorType    = USB_DT_INTERFACE,
+    .bInterfaceNumber   = 0,
+    .bAlternateSetting  = 0,
+    .bNumEndpoints      = 3,
+    .bInterfaceClass    = HWEMUL_CLASS,
+    .bInterfaceSubClass = HWEMUL_SUBCLASS,
+    .bInterfaceProtocol = HWEMUL_PROTOCOL,
+    .iInterface         = 4
+};
+
+
+static struct usb_endpoint_descriptor __attribute__((aligned(2)))
+    endpoint_descriptor =
+{
+    .bLength          = sizeof(struct usb_endpoint_descriptor),
+    .bDescriptorType  = USB_DT_ENDPOINT,
+    .bEndpointAddress = 0,
+    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
+    .wMaxPacketSize   = 0,
+    .bInterval        = 0
+};
+
+static const struct usb_string_descriptor __attribute__((aligned(2)))
+    usb_string_iManufacturer =
+{
+    24,
+    USB_DT_STRING,
+    {'R', 'o', 'c', 'k', 'b', 'o', 'x', '.', 'o', 'r', 'g'}
+};
+
+static const struct usb_string_descriptor __attribute__((aligned(2)))
+    usb_string_iProduct =
+{
+    52,
+    USB_DT_STRING,
+    {'R', 'o', 'c', 'k', 'b', 'o', 'x', ' ',
+     'h', 'a', 'r', 'd', 'w', 'a', 'r', 'e', ' ',
+     'e', 'm', 'u', 'l', 'a', 't', 'e', 'r'}
+};
+
+static struct usb_string_descriptor __attribute__((aligned(2)))
+    usb_string_iSerial =
+{
+    84,
+    USB_DT_STRING,
+    {'0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
+     '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
+     '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0',
+     '0', '0', '0', '0', '0', '0', '0', '0'}
+};
+
+static struct usb_string_descriptor __attribute__((aligned(2)))
+    usb_string_iInterface =
+{
+    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) }
+};
+
+/* this is stringid #0: languages supported */
+static const struct usb_string_descriptor __attribute__((aligned(2)))
+    lang_descriptor =
+{
+    4,
+    USB_DT_STRING,
+    {0x0409} /* LANGID US English */
+};
+
+#define USB_NUM_STRINGS 5
+
+static const struct usb_string_descriptor* const usb_strings[USB_NUM_STRINGS] =
+{
+   &lang_descriptor,
+   &usb_string_iManufacturer,
+   &usb_string_iProduct,
+   &usb_string_iSerial,
+   &usb_string_iInterface
+};
+
+uint8_t *usb_buffer = oc_bufferstart;
+uint32_t usb_buffer_size = 0;
+
+#define EP_BULK 1
+#define EP_INT  2
+
+static void set_config(void)
+{
+    usb_drv_configure_endpoint(EP_BULK, USB_ENDPOINT_XFER_BULK);
+    usb_drv_configure_endpoint(EP_INT, USB_ENDPOINT_XFER_INT);
+}
+
+static void handle_std_dev_desc(struct usb_ctrlrequest *req)
+{
+    int size;
+    const void* ptr = NULL;
+    unsigned index = req->wValue & 0xff;
+
+    switch(req->wValue >> 8)
+    {
+        case USB_DT_DEVICE:
+            ptr = &device_descriptor;
+            size = sizeof(struct usb_device_descriptor);
+            break;
+        case USB_DT_OTHER_SPEED_CONFIG:
+        case USB_DT_CONFIG:
+        {
+            int max_packet_size;
+
+            /* config desc */
+            if((req->wValue >> 8) ==USB_DT_CONFIG)
+            {
+                max_packet_size = (usb_drv_port_speed() ? 512 : 64);
+                config_descriptor.bDescriptorType = USB_DT_CONFIG;
+            }
+            else
+            {
+                max_packet_size=(usb_drv_port_speed() ? 64 : 512);
+                config_descriptor.bDescriptorType = USB_DT_OTHER_SPEED_CONFIG;
+            }
+            size = sizeof(struct usb_config_descriptor);
+
+            /* interface */
+            memcpy(usb_buffer + size, (void *)&interface_descriptor,
+                sizeof(interface_descriptor));
+            size += sizeof(interface_descriptor);
+            /* endpoint 1: bulk out */
+            endpoint_descriptor.bEndpointAddress = EP_BULK | USB_DIR_OUT;
+            endpoint_descriptor.bmAttributes = USB_ENDPOINT_XFER_BULK;
+            endpoint_descriptor.wMaxPacketSize = 512;
+            memcpy(usb_buffer + size, (void *)&endpoint_descriptor,
+                sizeof(endpoint_descriptor));
+            size += sizeof(endpoint_descriptor);
+            /* endpoint 2: bulk in */
+            endpoint_descriptor.bEndpointAddress = EP_BULK | USB_DIR_IN;
+            endpoint_descriptor.bmAttributes = USB_ENDPOINT_XFER_BULK;
+            endpoint_descriptor.wMaxPacketSize = 512;
+            memcpy(usb_buffer + size, (void *)&endpoint_descriptor,
+                sizeof(endpoint_descriptor));
+            size += sizeof(endpoint_descriptor);
+            /* endpoint 3: int in */
+            endpoint_descriptor.bEndpointAddress = EP_INT | USB_DIR_IN;
+            endpoint_descriptor.bmAttributes = USB_ENDPOINT_XFER_INT;
+            endpoint_descriptor.wMaxPacketSize = 1024;
+            memcpy(usb_buffer + size, (void *)&endpoint_descriptor,
+                sizeof(endpoint_descriptor));
+            size += sizeof(endpoint_descriptor);
+
+            /* fix config descriptor */
+            config_descriptor.bNumInterfaces = 1;
+            config_descriptor.wTotalLength = size;
+            memcpy(usb_buffer, (void *)&config_descriptor, sizeof(config_descriptor));
+
+            ptr = usb_buffer;
+            break;
+        }
+        case USB_DT_STRING:
+            if(index < USB_NUM_STRINGS)
+            {
+                size = usb_strings[index]->bLength;
+                ptr = usb_strings[index];
+            }
+            else
+                usb_drv_stall(EP_CONTROL, true, true);
+            break;
+        default:
+            break;
+    }
+
+    if(ptr)
+    {
+        int length = MIN(size, req->wLength);
+
+        if(ptr != usb_buffer)
+            memcpy(usb_buffer, ptr, length);
+
+        usb_drv_send(EP_CONTROL, usb_buffer, length);
+        usb_drv_recv(EP_CONTROL, NULL, 0);
+    }
+    else
+        usb_drv_stall(EP_CONTROL, true, true);
+}
+
+static void handle_std_dev_req(struct usb_ctrlrequest *req)
+{
+    switch(req->bRequest)
+    {
+        case USB_REQ_GET_CONFIGURATION:
+            usb_buffer[0] = 1;
+            usb_drv_send(EP_CONTROL, usb_buffer, 1);
+            usb_drv_recv(EP_CONTROL, NULL, 0);
+            break;
+        case USB_REQ_SET_CONFIGURATION:
+            usb_drv_send(EP_CONTROL, NULL, 0);
+            set_config();
+            break;
+        case USB_REQ_GET_DESCRIPTOR:
+            handle_std_dev_desc(req);
+            break;
+        case USB_REQ_SET_ADDRESS:
+            usb_drv_send(EP_CONTROL, NULL, 0);
+            usb_drv_set_address(req->wValue);
+            break;
+        case USB_REQ_GET_STATUS:
+            usb_buffer[0] = 0;
+            usb_buffer[1] = 0;
+            usb_drv_send(EP_CONTROL, usb_buffer, 2);
+            usb_drv_recv(EP_CONTROL, NULL, 0);
+            break;
+        default:
+            usb_drv_stall(EP_CONTROL, true, true);
+    }
+}
+
+static void handle_std_req(struct usb_ctrlrequest *req)
+{
+    switch(req->bRequestType & USB_RECIP_MASK)
+    {
+        case USB_RECIP_DEVICE:
+            return handle_std_dev_req(req);
+        default:
+            usb_drv_stall(EP_CONTROL, true, true);
+    }
+}
+
+struct usb_resp_info_version_t g_version =
+{
+    .major = HWEMUL_VERSION_MAJOR,
+    .minor = HWEMUL_VERSION_MINOR,
+    .revision = HWEMUL_VERSION_REV
+};
+
+struct usb_resp_info_layout_t g_layout;
+
+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
+};
+
+static void fill_layout_info(void)
+{
+    g_layout.oc_code_start = (uint32_t)oc_codestart;
+    g_layout.oc_code_size = oc_codesize;
+    g_layout.oc_stack_start = (uint32_t)oc_stackstart;
+    g_layout.oc_stack_size = oc_stacksize;
+    g_layout.oc_buffer_start = (uint32_t)oc_bufferstart;
+    g_layout.oc_buffer_size = oc_buffersize;
+}
+
+static void fill_stmp_info(void)
+{
+    g_stmp.chipid = __XTRACT(HW_DIGCTL_CHIPID, PRODUCT_CODE);
+    g_stmp.rev = __XTRACT(HW_DIGCTL_CHIPID, REVISION);
+    g_stmp.is_supported = g_stmp.chipid == 0x3780 || g_stmp.chipid == 0x3700 ||
+        g_stmp.chipid == 0x3b00;
+}
+
+static void handle_get_info(struct usb_ctrlrequest *req)
+{
+    void *ptr = NULL;
+    int size = 0;
+    switch(req->wIndex)
+    {
+        case HWEMUL_INFO_VERSION:
+            ptr = &g_version;
+            size = sizeof(g_version);
+            break;
+        case HWEMUL_INFO_LAYOUT:
+            fill_layout_info();
+            ptr = &g_layout;
+            size = sizeof(g_layout);
+            break;
+        case HWEMUL_INFO_STMP:
+            fill_stmp_info();
+            ptr = &g_stmp;
+            size = sizeof(g_stmp);
+            break;
+        case HWEMUL_INFO_FEATURES:
+            ptr = &g_features;
+            size = sizeof(g_features);
+            break;
+        default:
+            usb_drv_stall(EP_CONTROL, true, true);
+    }
+
+    if(ptr)
+    {
+        int length = MIN(size, req->wLength);
+        
+        if(ptr != usb_buffer)
+            memcpy(usb_buffer, ptr, length);
+        usb_drv_send(EP_CONTROL, usb_buffer, length);
+        usb_drv_recv(EP_CONTROL, NULL, 0);
+    }
+}
+
+static void handle_get_log(struct usb_ctrlrequest *req)
+{
+    enable_logf(false);
+    int length = logf_readback(usb_buffer, MIN(req->wLength, usb_buffer_size));
+    usb_drv_send(EP_CONTROL, usb_buffer, length);
+    usb_drv_recv(EP_CONTROL, NULL, 0);
+    enable_logf(true);
+}
+
+static void handle_rw_mem(struct usb_ctrlrequest *req)
+{
+    uint32_t addr = req->wValue | req->wIndex << 16;
+    uint16_t length = req->wLength;
+    
+    if(req->bRequestType & USB_DIR_IN)
+    {
+        memcpy(usb_buffer, (void *)addr, length);
+        asm volatile("nop" : : : "memory");
+        usb_drv_send(EP_CONTROL, usb_buffer, length);
+        usb_drv_recv(EP_CONTROL, NULL, 0);
+    }
+    else
+    {
+        int size = usb_drv_recv(EP_CONTROL, usb_buffer, length);
+        asm volatile("nop" : : : "memory");
+        if(size != length)
+            usb_drv_stall(EP_CONTROL, true, true);
+        else
+        {
+            memcpy((void *)addr, usb_buffer, length);
+            usb_drv_send(EP_CONTROL, NULL, 0);
+        }
+    }
+}
+
+static void handle_call_jump(struct usb_ctrlrequest *req)
+{
+    uint32_t addr = req->wValue | req->wIndex << 16;
+
+    if(req->bRequest == HWEMUL_CALL)
+        ((void (*)(void))addr)();
+    else
+        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);
+    /* reset DCP */
+    __REG_SET(HW_DCP_CTRL) = 0x80000000;
+    /* clear clock gate */
+    __REG_CLR(HW_DCP_CTRL) = 0xc0000000;
+    /* enable dma for channel 0 */
+    __REG_SET(HW_DCP_CHANNELCTRL) = HW_DCP_CHANNELCTRL__ENABLE_CHANNEL(0);
+    /* prepare packet */
+    dcp_packet.next = 0;
+
+    dcp_packet.ctrl0 = HW_DCP_CTRL0__INTERRUPT_ENABLE |
+        HW_DCP_CTRL0__DECR_SEMAPHORE | HW_DCP_CTRL0__CIPHER_INIT |
+        HW_DCP_CTRL0__ENABLE_CIPHER | HW_DCP_CTRL0__OTP_KEY |
+        (encrypt ? HW_DCP_CTRL0__CIPHER_ENCRYPT : 0);
+    dcp_packet.ctrl1 = HW_DCP_CTRL1__CIPHER_SELECT__AES128 |
+        HW_DCP_CTRL1__CIPHER_MODE__CBC;
+    dcp_packet.src_buf = (unsigned long)buffer + 16;
+    dcp_packet.dst_buf = (unsigned long)buffer + 16;
+    dcp_packet.buf_sz = length - 16;
+    dcp_packet.payload_ptr = (unsigned long)buffer;
+    dcp_packet.status = 0;
+
+    asm volatile("":::"memory");
+    /* kick */
+    HW_DCP_CH0CMDPTR = (unsigned long)&dcp_packet;
+    HW_DCP_CH0SEMA = HW_DCP_CH0SEMA__INCREMENT(1);
+    /* wait */
+    while(!(HW_DCP_STAT & HW_DCP_STAT__IRQ(0)));
+
+    usb_drv_send_nonblocking(EP_INT, buffer, length);
+}
+
+static void handle_aes_otp(struct usb_ctrlrequest *req)
+{
+    uint16_t length = req->wLength;
+
+    int size = usb_drv_recv(EP_CONTROL, usb_buffer, length);
+    if(size != length)
+        usb_drv_stall(EP_CONTROL, true, true);
+    else
+        usb_drv_send(EP_CONTROL, NULL, 0);
+    do_aes_otp(usb_buffer, length, req->wValue);
+}
+
+static void handle_class_dev_req(struct usb_ctrlrequest *req)
+{
+    switch(req->bRequest)
+    {
+        case HWEMUL_GET_INFO:
+            handle_get_info(req);
+            break;
+        case HWEMUL_GET_LOG:
+            handle_get_log(req);
+            break;
+        case HWEMUL_RW_MEM:
+            handle_rw_mem(req);
+            break;
+        case HWEMUL_CALL:
+        case HWEMUL_JUMP:
+            handle_call_jump(req);
+            break;
+        case HWEMUL_AES_OTP:
+            handle_aes_otp(req);
+            break;
+        default:
+            usb_drv_stall(EP_CONTROL, true, true);
+    }
+}
+
+static void handle_class_req(struct usb_ctrlrequest *req)
+{
+    switch(req->bRequestType & USB_RECIP_MASK)
+    {
+        case USB_RECIP_DEVICE:
+            return handle_class_dev_req(req);
+        default:
+            usb_drv_stall(EP_CONTROL, true, true);
+    }
+}
+
+/**
+ * 
+ * Main
+ * 
+ */
+
+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("argument: 0x%08x\n", arg);
+
+    /* 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;
+    /* 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;
+    /* reset the controller */
+    REG_USBCMD |= USBCMD_CTRL_RESET;
+    while (REG_USBCMD & USBCMD_CTRL_RESET);
+    /* put it in device mode */
+    REG_USBMODE = USBMODE_CTRL_MODE_DEVICE;
+    /* reset address */
+    REG_DEVICEADDR = 0;
+    /* prepare qh array */
+    qh_array[0].max_pkt_length = 1 << 29 | MAX_PKT_SIZE_EP0 << 16;
+    qh_array[1].max_pkt_length = 1 << 29 | MAX_PKT_SIZE_EP0 << 16;
+    qh_array[2].max_pkt_length = 1 << 29 | MAX_PKT_SIZE << 16;
+    qh_array[3].max_pkt_length = 1 << 29 | MAX_PKT_SIZE << 16;
+    /* setup qh */
+    REG_ENDPOINTLISTADDR = (unsigned int)qh_array;
+    /* clear setup status */
+    REG_ENDPTSETUPSTAT = EPSETUP_STATUS_EP0;
+    /* run! */
+    REG_USBCMD |= USBCMD_RUN;
+    
+    while(1)
+    {
+        /* wait for setup */
+        while(!(REG_ENDPTSETUPSTAT & EPSETUP_STATUS_EP0))
+            ;
+        /* clear setup status */
+        REG_ENDPTSETUPSTAT = EPSETUP_STATUS_EP0;
+        /* check request */
+        asm volatile("":::"memory");
+        struct usb_ctrlrequest *req = (void *)&qh_array[0].setup_buffer[0];
+        
+        switch(req->bRequestType & USB_TYPE_MASK)
+        {
+            case USB_TYPE_STANDARD:
+                handle_std_req(req);
+                break;
+            case USB_TYPE_CLASS:
+                handle_class_req(req);
+                break;
+            default:
+                usb_drv_stall(EP_CONTROL, true, true);
+        }
+    }
+}
diff --git a/utils/hwstub/stmp/protocol.h b/utils/hwstub/stmp/protocol.h
new file mode 100644
index 0000000000..d3ffb6ce00
--- /dev/null
+++ b/utils/hwstub/stmp/protocol.h
@@ -0,0 +1 @@
+#include "../hwemul_protocol.h"
diff --git a/utils/hwstub/stmp/stddef.h b/utils/hwstub/stmp/stddef.h
new file mode 100644
index 0000000000..9d59d2913c
--- /dev/null
+++ b/utils/hwstub/stmp/stddef.h
@@ -0,0 +1,32 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL_STDDEF__
+#define __HWEMUL_STDDEF__
+
+#include "stdint.h"
+
+typedef uint32_t size_t;
+typedef int32_t ssize_t;
+
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+
+#endif /* __HWEMUL_STDDEF__ */
diff --git a/utils/hwstub/stmp/stdint.h b/utils/hwstub/stmp/stdint.h
new file mode 100644
index 0000000000..4fe3702c86
--- /dev/null
+++ b/utils/hwstub/stmp/stdint.h
@@ -0,0 +1,38 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2005 by Dave Chapman
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __STDINT_H__
+#define __STDINT_H__
+
+typedef signed char int8_t;
+typedef unsigned char uint8_t;
+typedef short int16_t;
+typedef unsigned short uint16_t;
+typedef long int32_t;
+typedef unsigned long uint32_t;
+typedef char bool;
+
+#define true        1
+#define false       0
+
+#define NULL        (void *)0
+
+#endif /* __STDINT_H__ */
diff --git a/utils/hwstub/stmp/string.c b/utils/hwstub/stmp/string.c
new file mode 100644
index 0000000000..1f8c415a99
--- /dev/null
+++ b/utils/hwstub/stmp/string.c
@@ -0,0 +1,29 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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 "string.h"
+
+size_t strlen(const char *s)
+{
+    size_t len = 0;
+    while(*s++)
+        len++;
+    return len;
+}
diff --git a/utils/hwstub/stmp/string.h b/utils/hwstub/stmp/string.h
new file mode 100644
index 0000000000..7ef460ea6e
--- /dev/null
+++ b/utils/hwstub/stmp/string.h
@@ -0,0 +1,30 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL_STRING__
+#define __HWEMUL_STRING__
+
+#include "stddef.h"
+
+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__ */
diff --git a/utils/hwstub/stmp/system.h b/utils/hwstub/stmp/system.h
new file mode 100644
index 0000000000..c1babe7d87
--- /dev/null
+++ b/utils/hwstub/stmp/system.h
@@ -0,0 +1,118 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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.
+ *
+ ****************************************************************************/
+#ifndef __HWEMUL_SYSTEM__
+#define __HWEMUL_SYSTEM__
+
+#define IRQ_ENABLED      0x00
+#define IRQ_DISABLED     0x80
+#define IRQ_STATUS       0x80
+#define FIQ_ENABLED      0x00
+#define FIQ_DISABLED     0x40
+#define FIQ_STATUS       0x40
+#define IRQ_FIQ_ENABLED  0x00
+#define IRQ_FIQ_DISABLED 0xc0
+#define IRQ_FIQ_STATUS   0xc0
+#define HIGHEST_IRQ_LEVEL IRQ_DISABLED
+
+#define set_irq_level(status) \
+    set_interrupt_status((status), IRQ_STATUS)
+#define set_fiq_status(status) \
+    set_interrupt_status((status), FIQ_STATUS)
+
+#define disable_irq_save() \
+    disable_interrupt_save(IRQ_STATUS)
+#define disable_fiq_save() \
+    disable_interrupt_save(FIQ_STATUS)
+
+#define restore_irq(cpsr) \
+    restore_interrupt(cpsr)
+#define restore_fiq(cpsr) \
+    restore_interrupt(cpsr)
+
+#define disable_irq() \
+    disable_interrupt(IRQ_STATUS)
+#define enable_irq() \
+    enable_interrupt(IRQ_STATUS)
+#define disable_fiq() \
+    disable_interrupt(FIQ_STATUS)
+#define enable_fiq() \
+    enable_interrupt(FIQ_STATUS)
+
+static inline int set_interrupt_status(int status, int mask)
+{
+    unsigned long cpsr;
+    int oldstatus;
+    /* Read the old levels and set the new ones */
+    asm volatile (
+        "mrs    %1, cpsr        \n"
+        "bic    %0, %1, %[mask] \n"
+        "orr    %0, %0, %2      \n"
+        "msr    cpsr_c, %0      \n"
+        : "=&r,r"(cpsr), "=&r,r"(oldstatus)
+        : "r,i"(status & mask), [mask]"i,i"(mask));
+
+    return oldstatus;
+}
+
+static inline void restore_interrupt(int cpsr)
+{
+    /* Set cpsr_c from value returned by disable_interrupt_save
+     * or set_interrupt_status */
+    asm volatile ("msr cpsr_c, %0" : : "r"(cpsr));
+}
+
+static inline void enable_interrupt(int mask)
+{
+    /* Clear I and/or F disable bit */
+    int tmp;
+    asm volatile (
+        "mrs     %0, cpsr   \n"
+        "bic     %0, %0, %1 \n"
+        "msr     cpsr_c, %0 \n"
+        : "=&r"(tmp) : "i"(mask));
+}
+
+static inline void disable_interrupt(int mask)
+{
+    /* Set I and/or F disable bit */
+    int tmp;
+    asm volatile (
+        "mrs     %0, cpsr   \n"
+        "orr     %0, %0, %1 \n"
+        "msr     cpsr_c, %0 \n"
+        : "=&r"(tmp) : "i"(mask));
+}
+
+static inline int disable_interrupt_save(int mask)
+{
+    /* Set I and/or F disable bit and return old cpsr value */
+    int cpsr, tmp;
+    asm volatile (
+        "mrs     %1, cpsr   \n"
+        "orr     %0, %1, %2 \n"
+        "msr     cpsr_c, %0 \n"
+        : "=&r"(tmp), "=&r"(cpsr)
+        : "i"(mask));
+    return cpsr;
+}
+
+#endif /* __HWEMUL_SYSTEM__ */
+ 
diff --git a/utils/hwstub/stmp/usb_ch9.h b/utils/hwstub/stmp/usb_ch9.h
new file mode 100644
index 0000000000..09141b93bd
--- /dev/null
+++ b/utils/hwstub/stmp/usb_ch9.h
@@ -0,0 +1,454 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) by Linux Kernel Developers
+ *
+ * Based on code from the Linux Kernel
+ * available at http://www.kernel.org
+ * Original file: <kernel>/include/linux/usb/ch9.h
+ * 
+ * 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.
+ *
+ ****************************************************************************/
+
+/*
+ * This file holds USB constants and structures that are needed for
+ * USB device APIs.  These are used by the USB device model, which is
+ * defined in chapter 9 of the USB 2.0 specification and in the
+ * Wireless USB 1.0 (spread around).  Linux has several APIs in C that
+ * need these:
+ *
+ * - the master/host side Linux-USB kernel driver API;
+ * - the "usbfs" user space API; and
+ * - the Linux "gadget" slave/device/peripheral side driver API.
+ *
+ * USB 2.0 adds an additional "On The Go" (OTG) mode, which lets systems
+ * act either as a USB master/host or as a USB slave/device.  That means
+ * the master and slave side APIs benefit from working well together.
+ *
+ * There's also "Wireless USB", using low power short range radios for
+ * peripheral interconnection but otherwise building on the USB framework.
+ *
+ * Note all descriptors are declared '__attribute__((packed))' so that:
+ *
+ * [a] they never get padded, either internally (USB spec writers
+ *     probably handled that) or externally;
+ *
+ * [b] so that accessing bigger-than-a-bytes fields will never
+ *     generate bus errors on any platform, even when the location of
+ *     its descriptor inside a bundle isn't "naturally aligned", and
+ *
+ * [c] for consistency, removing all doubt even when it appears to
+ *     someone that the two other points are non-issues for that
+ *     particular descriptor type.
+ */
+
+#ifndef _CH9_H_
+#define _CH9_H_
+
+#include "stdint.h"
+
+/*-------------------------------------------------------------------------*/
+
+/* CONTROL REQUEST SUPPORT */
+
+/*
+ * USB directions
+ *
+ * This bit flag is used in endpoint descriptors' bEndpointAddress field.
+ * It's also one of three fields in control requests bRequestType.
+ */
+#define USB_DIR_OUT                     0               /* to device */
+#define USB_DIR_IN                      0x80            /* to host */
+
+/*
+ * USB types, the second of three bRequestType fields
+ */
+#define USB_TYPE_MASK                   (0x03 << 5)
+#define USB_TYPE_STANDARD               (0x00 << 5)
+#define USB_TYPE_CLASS                  (0x01 << 5)
+#define USB_TYPE_VENDOR                 (0x02 << 5)
+#define USB_TYPE_RESERVED               (0x03 << 5)
+
+/*
+ * USB recipients, the third of three bRequestType fields
+ */
+#define USB_RECIP_MASK                  0x1f
+#define USB_RECIP_DEVICE                0x00
+#define USB_RECIP_INTERFACE             0x01
+#define USB_RECIP_ENDPOINT              0x02
+#define USB_RECIP_OTHER                 0x03
+
+/*
+ * Standard requests, for the bRequest field of a SETUP packet.
+ *
+ * These are qualified by the bRequestType field, so that for example
+ * TYPE_CLASS or TYPE_VENDOR specific feature flags could be retrieved
+ * by a GET_STATUS request.
+ */
+#define USB_REQ_GET_STATUS              0x00
+#define USB_REQ_CLEAR_FEATURE           0x01
+#define USB_REQ_SET_FEATURE             0x03
+#define USB_REQ_SET_ADDRESS             0x05
+#define USB_REQ_GET_DESCRIPTOR          0x06
+#define USB_REQ_SET_DESCRIPTOR          0x07
+#define USB_REQ_GET_CONFIGURATION       0x08
+#define USB_REQ_SET_CONFIGURATION       0x09
+#define USB_REQ_GET_INTERFACE           0x0A
+#define USB_REQ_SET_INTERFACE           0x0B
+#define USB_REQ_SYNCH_FRAME             0x0C
+/*
+ * USB feature flags are written using USB_REQ_{CLEAR,SET}_FEATURE, and
+ * are read as a bit array returned by USB_REQ_GET_STATUS.  (So there
+ * are at most sixteen features of each type.)  Hubs may also support a
+ * new USB_REQ_TEST_AND_SET_FEATURE to put ports into L1 suspend.
+ */
+#define USB_DEVICE_SELF_POWERED         0   /* (read only) */
+#define USB_DEVICE_REMOTE_WAKEUP        1   /* dev may initiate wakeup */
+#define USB_DEVICE_TEST_MODE            2   /* (wired high speed only) */
+#define USB_DEVICE_BATTERY              2   /* (wireless) */
+#define USB_DEVICE_B_HNP_ENABLE         3   /* (otg) dev may initiate HNP */
+#define USB_DEVICE_WUSB_DEVICE          3   /* (wireless)*/
+#define USB_DEVICE_A_HNP_SUPPORT        4   /* (otg) RH port supports HNP */
+#define USB_DEVICE_A_ALT_HNP_SUPPORT    5   /* (otg) other RH port does */
+#define USB_DEVICE_DEBUG_MODE           6   /* (special devices only) */
+
+#define USB_ENDPOINT_HALT       0   /* IN/OUT will STALL */
+
+
+/**
+ * struct usb_ctrlrequest - SETUP data for a USB device control request
+ * @bRequestType: matches the USB bmRequestType field
+ * @bRequest: matches the USB bRequest field
+ * @wValue: matches the USB wValue field (le16 byte order)
+ * @wIndex: matches the USB wIndex field (le16 byte order)
+ * @wLength: matches the USB wLength field (le16 byte order)
+ *
+ * This structure is used to send control requests to a USB device.  It matches
+ * the different fields of the USB 2.0 Spec section 9.3, table 9-2.  See the
+ * USB spec for a fuller description of the different fields, and what they are
+ * used for.
+ *
+ * Note that the driver for any interface can issue control requests.
+ * For most devices, interfaces don't coordinate with each other, so
+ * such requests may be made at any time.
+ */
+struct usb_ctrlrequest {
+        uint8_t bRequestType;
+        uint8_t bRequest;
+        uint16_t wValue;
+        uint16_t wIndex;
+        uint16_t wLength;
+} __attribute__ ((packed));
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * STANDARD DESCRIPTORS ... as returned by GET_DESCRIPTOR, or
+ * (rarely) accepted by SET_DESCRIPTOR.
+ *
+ * Note that all multi-byte values here are encoded in little endian
+ * byte order "on the wire".  But when exposed through Linux-USB APIs,
+ * they've been converted to cpu byte order.
+ */
+
+/*
+ * Descriptor types ... USB 2.0 spec table 9.5
+ */
+#define USB_DT_DEVICE                   0x01
+#define USB_DT_CONFIG                   0x02
+#define USB_DT_STRING                   0x03
+#define USB_DT_INTERFACE                0x04
+#define USB_DT_ENDPOINT                 0x05
+#define USB_DT_DEVICE_QUALIFIER         0x06
+#define USB_DT_OTHER_SPEED_CONFIG       0x07
+#define USB_DT_INTERFACE_POWER          0x08
+/* these are from a minor usb 2.0 revision (ECN) */
+#define USB_DT_OTG                      0x09
+#define USB_DT_DEBUG                    0x0a
+#define USB_DT_INTERFACE_ASSOCIATION    0x0b
+/* these are from the Wireless USB spec */
+#define USB_DT_SECURITY                 0x0c
+#define USB_DT_KEY                      0x0d
+#define USB_DT_ENCRYPTION_TYPE          0x0e
+#define USB_DT_BOS                      0x0f
+#define USB_DT_DEVICE_CAPABILITY        0x10
+#define USB_DT_WIRELESS_ENDPOINT_COMP   0x11
+#define USB_DT_WIRE_ADAPTER             0x21
+#define USB_DT_RPIPE                    0x22
+#define USB_DT_CS_RADIO_CONTROL         0x23
+
+/* Conventional codes for class-specific descriptors.  The convention is
+ * defined in the USB "Common Class" Spec (3.11).  Individual class specs
+ * are authoritative for their usage, not the "common class" writeup.
+ */
+#define USB_DT_CS_DEVICE                (USB_TYPE_CLASS | USB_DT_DEVICE)
+#define USB_DT_CS_CONFIG                (USB_TYPE_CLASS | USB_DT_CONFIG)
+#define USB_DT_CS_STRING                (USB_TYPE_CLASS | USB_DT_STRING)
+#define USB_DT_CS_INTERFACE             (USB_TYPE_CLASS | USB_DT_INTERFACE)
+#define USB_DT_CS_ENDPOINT              (USB_TYPE_CLASS | USB_DT_ENDPOINT)
+
+/* All standard descriptors have these 2 fields at the beginning */
+struct usb_descriptor_header {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+} __attribute__ ((packed));
+
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_DEVICE: Device descriptor */
+struct usb_device_descriptor {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+
+        uint16_t bcdUSB;
+        uint8_t  bDeviceClass;
+        uint8_t  bDeviceSubClass;
+        uint8_t  bDeviceProtocol;
+        uint8_t  bMaxPacketSize0;
+        uint16_t idVendor;
+        uint16_t idProduct;
+        uint16_t bcdDevice;
+        uint8_t  iManufacturer;
+        uint8_t  iProduct;
+        uint8_t  iSerialNumber;
+        uint8_t  bNumConfigurations;
+} __attribute__ ((packed));
+
+#define USB_DT_DEVICE_SIZE              18
+
+
+/*
+ * Device and/or Interface Class codes
+ * as found in bDeviceClass or bInterfaceClass
+ * and defined by www.usb.org documents
+ */
+#define USB_CLASS_PER_INTERFACE         0       /* for DeviceClass */
+#define USB_CLASS_AUDIO                 1
+#define USB_CLASS_COMM                  2
+#define USB_CLASS_HID                   3
+#define USB_CLASS_PHYSICAL              5
+#define USB_CLASS_STILL_IMAGE           6
+#define USB_CLASS_PRINTER               7
+#define USB_CLASS_MASS_STORAGE          8
+#define USB_CLASS_HUB                   9
+#define USB_CLASS_CDC_DATA              0x0a
+#define USB_CLASS_CSCID                 0x0b    /* chip+ smart card */
+#define USB_CLASS_CONTENT_SEC           0x0d    /* content security */
+#define USB_CLASS_VIDEO                 0x0e
+#define USB_CLASS_WIRELESS_CONTROLLER   0xe0
+#define USB_CLASS_MISC                  0xef
+#define USB_CLASS_APP_SPEC              0xfe
+#define USB_CLASS_VENDOR_SPEC           0xff
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_CONFIG: Configuration descriptor information.
+ *
+ * USB_DT_OTHER_SPEED_CONFIG is the same descriptor, except that the
+ * descriptor type is different.  Highspeed-capable devices can look
+ * different depending on what speed they're currently running.  Only
+ * devices with a USB_DT_DEVICE_QUALIFIER have any OTHER_SPEED_CONFIG
+ * descriptors.
+ */
+struct usb_config_descriptor {
+       uint8_t  bLength;
+       uint8_t  bDescriptorType;
+
+       uint16_t wTotalLength;
+       uint8_t  bNumInterfaces;
+       uint8_t  bConfigurationValue;
+       uint8_t  iConfiguration;
+       uint8_t  bmAttributes;
+       uint8_t  bMaxPower;
+} __attribute__ ((packed));
+
+#define USB_DT_CONFIG_SIZE              9
+
+/* from config descriptor bmAttributes */
+#define USB_CONFIG_ATT_ONE              (1 << 7)        /* must be set */
+#define USB_CONFIG_ATT_SELFPOWER        (1 << 6)        /* self powered */
+#define USB_CONFIG_ATT_WAKEUP           (1 << 5)        /* can wakeup */
+#define USB_CONFIG_ATT_BATTERY          (1 << 4)        /* battery powered */
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_STRING: String descriptor */
+struct usb_string_descriptor {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+
+        uint16_t wString[];             /* UTF-16LE encoded */
+} __attribute__ ((packed));
+
+/* note that "string" zero is special, it holds language codes that
+ * the device supports, not Unicode characters.
+ */
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_INTERFACE: Interface descriptor */
+struct usb_interface_descriptor {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+
+        uint8_t  bInterfaceNumber;
+        uint8_t  bAlternateSetting;
+        uint8_t  bNumEndpoints;
+        uint8_t  bInterfaceClass;
+        uint8_t  bInterfaceSubClass;
+        uint8_t  bInterfaceProtocol;
+        uint8_t  iInterface;
+} __attribute__ ((packed));
+
+#define USB_DT_INTERFACE_SIZE           9
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_ENDPOINT: Endpoint descriptor */
+struct usb_endpoint_descriptor {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+
+        uint8_t  bEndpointAddress;
+        uint8_t  bmAttributes;
+        uint16_t wMaxPacketSize;
+        uint8_t  bInterval;
+} __attribute__ ((packed));
+
+#define USB_DT_ENDPOINT_SIZE            7
+#define USB_DT_ENDPOINT_AUDIO_SIZE      9       /* Audio extension */
+
+
+/*
+ * Endpoints
+ */
+#define USB_ENDPOINT_NUMBER_MASK        0x0f    /* in bEndpointAddress */
+#define USB_ENDPOINT_DIR_MASK           0x80
+
+#define USB_ENDPOINT_XFERTYPE_MASK      0x03    /* in bmAttributes */
+#define USB_ENDPOINT_XFER_CONTROL       0
+#define USB_ENDPOINT_XFER_ISOC          1
+#define USB_ENDPOINT_XFER_BULK          2
+#define USB_ENDPOINT_XFER_INT           3
+#define USB_ENDPOINT_MAX_ADJUSTABLE     0x80
+
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_DEVICE_QUALIFIER: Device Qualifier descriptor */
+struct usb_qualifier_descriptor {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+
+        uint16_t bcdUSB;
+        uint8_t  bDeviceClass;
+        uint8_t  bDeviceSubClass;
+        uint8_t  bDeviceProtocol;
+        uint8_t  bMaxPacketSize0;
+        uint8_t  bNumConfigurations;
+        uint8_t  bRESERVED;
+} __attribute__ ((packed));
+
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_OTG (from OTG 1.0a supplement) */
+struct usb_otg_descriptor {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+
+        uint8_t  bmAttributes;  /* support for HNP, SRP, etc */
+} __attribute__ ((packed));
+
+/* from usb_otg_descriptor.bmAttributes */
+#define USB_OTG_SRP             (1 << 0)
+#define USB_OTG_HNP             (1 << 1)        /* swap host/device roles */
+
+/*-------------------------------------------------------------------------*/
+
+/* USB_DT_DEBUG:  for special highspeed devices, replacing serial console */
+struct usb_debug_descriptor {
+        uint8_t  bLength;
+        uint8_t  bDescriptorType;
+
+        /* bulk endpoints with 8 byte maxpacket */
+        uint8_t  bDebugInEndpoint;
+        uint8_t  bDebugOutEndpoint;
+} __attribute__((packed));
+
+/*-------------------------------------------------------------------------*/
+/* USB 2.0 defines three speeds, here's how Linux identifies them */
+
+enum usb_device_speed {
+        USB_SPEED_UNKNOWN = 0,                  /* enumerating */
+        USB_SPEED_LOW, USB_SPEED_FULL,          /* usb 1.1 */
+        USB_SPEED_HIGH,                         /* usb 2.0 */
+        USB_SPEED_VARIABLE,                     /* wireless (usb 2.5) */
+};
+
+enum usb_device_state {
+        /* NOTATTACHED isn't in the USB spec, and this state acts
+         * the same as ATTACHED ... but it's clearer this way.
+         */
+        USB_STATE_NOTATTACHED = 0,
+
+        /* chapter 9 and authentication (wireless) device states */
+        USB_STATE_ATTACHED,
+        USB_STATE_POWERED,                      /* wired */
+        USB_STATE_UNAUTHENTICATED,              /* auth */
+        USB_STATE_RECONNECTING,                 /* auth */
+        USB_STATE_DEFAULT,                      /* limited function */
+        USB_STATE_ADDRESS,
+        USB_STATE_CONFIGURED,                   /* most functions */
+
+        USB_STATE_SUSPENDED
+
+        /* NOTE:  there are actually four different SUSPENDED
+         * states, returning to POWERED, DEFAULT, ADDRESS, or
+         * CONFIGURED respectively when SOF tokens flow again.
+         * At this level there's no difference between L1 and L2
+         * suspend states.  (L2 being original USB 1.1 suspend.)
+         */
+};
+
+/**
+ * struct usb_string - wraps a C string and its USB id
+ * @id:the (nonzero) ID for this string
+ * @s:the string, in UTF-8 encoding
+ *
+ * If you're using usb_gadget_get_string(), use this to wrap a string
+ * together with its ID.
+ */
+struct usb_string {
+        uint8_t id;
+        const char* s;
+};
+
+/**
+ * struct usb_gadget_strings - a set of USB strings in a given language
+ * @language:identifies the strings' language (0x0409 for en-us)
+ * @strings:array of strings with their ids
+ *
+ * If you're using usb_gadget_get_string(), use this to wrap all the
+ * strings for a given language.
+ */
+struct usb_gadget_strings {
+        uint16_t language;                      /* 0x0409 for en-us */
+        struct usb_string* strings;
+};
+
+#endif /*_CH9_H_*/
diff --git a/utils/hwstub/tools/Makefile b/utils/hwstub/tools/Makefile
new file mode 100644
index 0000000000..3466a4e776
--- /dev/null
+++ b/utils/hwstub/tools/Makefile
@@ -0,0 +1,22 @@
+CC=gcc
+AR=ar
+HWEMUL_LIB_DIR=../lib
+CFLAGS=-W -Wall -O2 `pkg-config --cflags libusb-1.0` -std=c99 -g -I$(HWEMUL_LIB_DIR)
+LDFLAGS=`pkg-config --libs libusb-1.0` -lreadline
+EXEC=hwemul_tool
+HWEMUL_LIB=$(HWEMUL_LIB_DIR)/libhwemul.a
+SRC=$(wildcard *.c)
+OBJ=$(SRC:.c=.o)
+
+all: $(EXEC)
+
+%.o: %.c
+        $(CC) $(CFLAGS) -c -o $@ $<
+
+hwemul_tool: hwemul_tool.o $(HWEMUL_LIB)
+        $(CC) -o $@ $^ $(LDFLAGS)
+
+clean:
+        rm -rf $(OBJ) $(LIB)
+
+
diff --git a/utils/hwstub/tools/hwemul_tool.c b/utils/hwstub/tools/hwemul_tool.c
new file mode 100644
index 0000000000..d75cd7a957
--- /dev/null
+++ b/utils/hwstub/tools/hwemul_tool.c
@@ -0,0 +1,558 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   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 "hwemul.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <stdbool.h>
+#include <readline/readline.h>
+#include <readline/history.h>
+
+bool g_quiet = false;
+struct hwemul_device_t hwdev;
+struct hwemul_soc_t *cur_soc = NULL;
+
+void print_log(struct hwemul_device_t *hwdev)
+{
+    do
+    {
+        char buffer[128];
+        int length = hwemul_get_log(hwdev, buffer, sizeof(buffer) - 1);
+        if(length <= 0)
+            break;
+        buffer[length] = 0;
+        printf("%s", buffer);
+    }while(1);
+}
+
+int print_help()
+{
+    printf("Commands:\n");
+    printf("  help\t\tDisplay this help\n");
+    printf("  call <addr>\tCall address <addr>\n");
+    printf("  quit\t\tQuit this session\n");
+    printf("  read32 <addr>\tRead a 32-bit word at <addr>\n");
+    printf("  write32 <value> <addr>\tRead the 32-bit word <value> at <addr>\n");
+    printf("  read <regname>\tRead a register by name\n");
+    printf("  read <regname>.<field>\tRead a register field by name\n");
+    printf("  soc <socname>\tSelect the soc description to use\n");
+    printf("  write <value> <regname>\tWrite a register by name\n");
+    printf("  write <value <regname>.<field>\tWrite a register field by name\n");
+    printf("    NOTE: if the register is SCT variant, no read is performed.\n");
+    return 1;
+}
+
+int syntax_error(char *str)
+{
+    printf("Syntax error at '%s'. Type 'help' to get some help.\n", str);
+    return 1;
+}
+
+int parse_uint32(char *str, uint32_t *u)
+{
+    char *end;
+    *u = strtoul(str, &end, 0);
+    return *end == 0;
+}
+
+int do_call(uint32_t a)
+{
+    hwemul_call(&hwdev, a);
+    return 1;
+}
+
+int parse_call()
+{
+    char *arg = strtok(NULL, " ");
+    uint32_t addr;
+    if(arg && parse_uint32(arg, &addr))
+        return do_call(addr);
+    else
+        return syntax_error(arg);
+}
+
+int do_read32(uint32_t a)
+{
+    uint32_t val;
+    if(hwemul_rw_mem(&hwdev, 1, a, &val, sizeof(val)) == sizeof(val))
+        printf("%#x = %#x\n", a, val);
+    else
+        printf("read error at %#x\n", a);
+    return 1;
+}
+
+int parse_read32()
+{
+    char *arg = strtok(NULL, " ");
+    uint32_t addr;
+    if(arg && parse_uint32(arg, &addr))
+        return do_read32(addr);
+    else
+        return syntax_error(arg);
+}
+
+int do_write32(uint32_t val, uint32_t a)
+{
+    if(hwemul_rw_mem(&hwdev, 0, a, &val, sizeof(val)) == sizeof(val))
+        printf("data written\n");
+    else
+        printf("write error at %#x\n", a);
+    return 1;
+}
+
+int parse_write32()
+{
+    char *arg = strtok(NULL, " ");
+    uint32_t val;
+    if(!arg || !parse_uint32(arg, &val))
+        return syntax_error(arg);
+    uint32_t addr;
+    arg = strtok(NULL, " ");
+    if(arg && parse_uint32(arg, &addr))
+        return do_write32(val, addr);
+    else
+        return syntax_error(arg);
+}
+
+struct hwemul_soc_t *find_soc_by_name(const char *soc)
+{
+    struct hwemul_soc_list_t *list = hwemul_get_soc_list();
+    for(size_t i = 0; i < list->nr_socs; i++)
+        if(strcmp(soc, list->socs[i]->name) == 0)
+            return list->socs[i];
+    return NULL;
+}
+
+struct hwemul_soc_reg_t *find_reg_by_name(struct hwemul_soc_t *soc, const char *reg)
+{
+    for(size_t i = 0; i < soc->nr_regs; i++)
+        if(strcmp(reg, soc->regs_by_name[i]->name) == 0)
+            return soc->regs_by_name[i];
+    return NULL;
+}
+
+struct hwemul_soc_reg_field_t *find_field_by_name(struct hwemul_soc_reg_t *reg, const char *field)
+{
+    for(size_t i = 0; i < reg->nr_fields; i++)
+        if(strcmp(field, reg->fields_by_name[i]->name) == 0)
+            return reg->fields_by_name[i];
+    return NULL;
+}
+
+
+int do_read(char *regname)
+{
+    char *dot = strchr(regname, '.');
+    if(dot != NULL)
+        *dot++ = 0;
+    if(cur_soc == NULL)
+    {
+        printf("No soc selected!\n");
+        return 1;
+    }
+    struct hwemul_soc_reg_t *reg = find_reg_by_name(cur_soc, regname);
+    if(reg == NULL)
+    {
+        printf("no reg '%s' found\n", regname);
+        return 1;
+    }
+    uint32_t val;
+    if(hwemul_rw_mem(&hwdev, 1, reg->addr, &val, sizeof(val)) != sizeof(val))
+    {
+        printf("read error at %#x\n", reg->addr);
+        return 1;
+    }
+    if(dot)
+    {
+        struct hwemul_soc_reg_field_t *field = find_field_by_name(reg, dot);
+        if(field == NULL)
+        {
+            printf("no field '%s' found\n", dot);
+            return 1;
+        }
+        val >>= field->first_bit;
+        val &= (1 << (field->last_bit - field->first_bit + 1)) - 1;
+        printf("%s.%s = %#x\n", regname, dot, val);
+    }
+    else
+        printf("%s = %#x\n", regname, val);
+    return 1;
+}
+
+int parse_read()
+{
+    char *arg = strtok(NULL, " ");
+    if(arg)
+        return do_read(arg);
+    else
+        return syntax_error(arg);
+}
+
+int do_soc(char *soc)
+{
+    struct hwemul_soc_t *s = find_soc_by_name(soc);
+    if(s == NULL)
+        printf("no soc '%s' found\n", soc);
+    else
+        cur_soc = s;
+    return 1;
+}
+
+int parse_soc()
+{
+    char *arg = strtok(NULL, " ");
+    if(arg)
+        return do_soc(arg);
+    else
+        return syntax_error(arg);
+}
+
+int do_write(uint32_t val, char *regname)
+{
+    char *dot = strchr(regname, '.');
+    if(dot != NULL)
+        *dot++ = 0;
+    if(cur_soc == NULL)
+    {
+        printf("No soc selected!\n");
+        return 1;
+    }
+    struct hwemul_soc_reg_t *reg = find_reg_by_name(cur_soc, regname);
+    int is_sct = 0;
+    uint32_t addr_off = 0;
+    if(reg == NULL)
+    {
+        size_t len = strlen(regname);
+        /* try SCT variant */
+        if(strcmp(regname + len - 4, "_SET") == 0)
+            addr_off = 4;
+        else if(strcmp(regname + len - 4, "_CLR") == 0)
+            addr_off = 8;
+        else if(strcmp(regname + len - 4, "_TOG") == 0)
+            addr_off = 12;
+        else
+        {
+            printf("no reg '%s' found\n", regname);
+            return 1;
+        }
+        is_sct = 1;
+        regname[len - 4] = 0;
+        reg = find_reg_by_name(cur_soc, regname);
+        if(reg == NULL)
+        {
+            printf("no reg '%s' found\n", regname);
+            return 1;
+        }
+    }
+    if(dot)
+    {
+        struct hwemul_soc_reg_field_t *field = find_field_by_name(reg, dot);
+        if(field == NULL)
+        {
+            printf("no field '%s' found\n", dot);
+            return 1;
+        }
+        uint32_t actual_val = 0;
+        if(!is_sct)
+        {
+            if(hwemul_rw_mem(&hwdev, 1, reg->addr, &actual_val, sizeof(actual_val)) != sizeof(actual_val))
+            {
+                printf("read error at %#x\n", reg->addr);
+                return 1;
+            }
+            printf("read %#x at %#x\n", actual_val, reg->addr);
+        }
+        uint32_t mask = ((1 << (field->last_bit - field->first_bit + 1)) - 1) << field->first_bit;
+        printf("mask=%#x\n", mask);
+        val = (actual_val & ~mask) | ((val << field->first_bit) & mask);
+    }
+    printf("write %#x to %#x\n", val, reg->addr + addr_off);
+    if(hwemul_rw_mem(&hwdev, 0, reg->addr + addr_off, &val, sizeof(val)) != sizeof(val))
+    {
+        printf("write error at %#x\n", reg->addr);
+        return 1;
+    }
+    return 1;
+}
+
+int parse_write()
+{
+    char *arg = strtok(NULL, " ");
+    uint32_t val;
+    if(!arg || !parse_uint32(arg, &val))
+        return syntax_error(arg);
+    arg = strtok(NULL, " ");
+    if(arg)
+        return do_write(val, arg);
+    else
+        return syntax_error(arg);
+}
+
+int parse_command(char *cmd)
+{
+    if(strcmp(cmd, "help") == 0)
+        return print_help();
+    if(strcmp(cmd, "quit") == 0)
+        return 0;
+    if(strcmp(cmd, "call") == 0)
+        return parse_call();
+    if(strcmp(cmd, "read32") == 0)
+        return parse_read32();
+    if(strcmp(cmd, "write32") == 0)
+        return parse_write32();
+    if(strcmp(cmd, "read") == 0)
+        return parse_read();
+    if(strcmp(cmd, "soc") == 0)
+        return parse_soc();
+    if(strcmp(cmd, "write") == 0)
+        return parse_write();
+    return syntax_error(cmd);
+}
+
+void interactive_mode(void)
+{
+    rl_bind_key('\t', rl_complete);
+    while(1)
+    {
+        char *input = readline("> ");
+        if(!input)
+            break;
+        add_history(input);
+        int ret = parse_command(input);
+        free(input);
+        if(ret == 0)
+            break;
+    }
+}
+
+void usage(void)
+{
+    printf("hwemul_tool, compiled with hwemul %d.%d.%d\n",
+        HWEMUL_VERSION_MAJOR, HWEMUL_VERSION_MINOR, HWEMUL_VERSION_REV);
+    printf("available soc descriptions:");
+    for(unsigned i = 0; i < hwemul_get_soc_list()->nr_socs; i++)
+        printf(" %s", hwemul_get_soc_list()->socs[i]->name);
+    printf("\n");
+    printf("usage: hwemul_tool [options]\n");
+    printf("options:\n");
+    printf("  --help/-?\tDisplay this help\n");
+    printf("  --quiet/-q\tQuiet non-command messages\n");
+    exit(1);
+}
+
+int main(int argc, char **argv)
+{
+    while(1)
+    {
+        static struct option long_options[] =
+        {
+            {"help", no_argument, 0, '?'},
+            {"quiet", no_argument, 0, 'q'},
+            {0, 0, 0, 0}
+        };
+
+        int c = getopt_long(argc, argv, "?q", long_options, NULL);
+        if(c == -1)
+            break;
+        switch(c)
+        {
+            case -1:
+                break;
+            case 'q':
+                g_quiet = true;
+                break;
+            case '?':
+                usage();
+                break;
+            default:
+                abort();
+        }
+    }
+
+    if(argc - optind != 0)
+    {
+        usage();
+        return 1;
+    }
+    
+    libusb_context *ctx;
+    libusb_init(&ctx);
+    libusb_set_debug(ctx, 3);
+
+    if(!g_quiet)
+        printf("Looking for device %#04x:%#04x...\n", HWEMUL_USB_VID, HWEMUL_USB_PID);
+
+    libusb_device_handle *handle = libusb_open_device_with_vid_pid(ctx,
+        HWEMUL_USB_VID, HWEMUL_USB_PID);
+    if(handle == NULL)
+    {
+        printf("No device found\n");
+        return 1;
+    }
+
+    libusb_device *mydev = libusb_get_device(handle);
+    if(!g_quiet)
+    {
+        printf("device found at %d:%d\n",
+            libusb_get_bus_number(mydev),
+            libusb_get_device_address(mydev));
+    }
+    hwdev.handle = handle;
+    if(hwemul_probe(&hwdev))
+    {
+        printf("Cannot probe device!\n");
+        return 1;
+    }
+
+    struct usb_resp_info_version_t ver;
+    int ret = hwemul_get_info(&hwdev, HWEMUL_INFO_VERSION, &ver, sizeof(ver));
+    if(ret != sizeof(ver))
+    {
+        printf("Cannot get version!\n");
+        goto Lerr;
+    }
+    if(!g_quiet)
+        printf("Device version: %d.%d.%d\n", ver.major, ver.minor, ver.revision);
+
+    struct usb_resp_info_layout_t layout;
+    ret = hwemul_get_info(&hwdev, HWEMUL_INFO_LAYOUT, &layout, sizeof(layout));
+    if(ret != sizeof(layout))
+    {
+        printf("Cannot get layout: %d\n", ret);
+        goto Lerr;
+    }
+    if(!g_quiet)
+    {
+        printf("Device layout:\n");
+        printf("  Code: 0x%x (0x%x)\n", layout.oc_code_start, layout.oc_code_size);
+        printf("  Stack: 0x%x (0x%x)\n", layout.oc_stack_start, layout.oc_stack_size);
+        printf("  Buffer: 0x%x (0x%x)\n", layout.oc_buffer_start, layout.oc_buffer_size);
+    }
+
+    struct usb_resp_info_features_t features;
+    ret = hwemul_get_info(&hwdev, HWEMUL_INFO_FEATURES, &features, sizeof(features));
+    if(ret != sizeof(features))
+    {
+        printf("Cannot get features: %d\n", ret);
+        goto Lerr;
+    }
+    if(!g_quiet)
+    {
+        printf("Device features:");
+        if(features.feature_mask & HWEMUL_FEATURE_LOG)
+            printf(" log");
+        if(features.feature_mask & HWEMUL_FEATURE_MEM)
+            printf(" mem");
+        if(features.feature_mask & HWEMUL_FEATURE_CALL)
+            printf(" call");
+        if(features.feature_mask & HWEMUL_FEATURE_JUMP)
+            printf(" jump");
+        if(features.feature_mask & HWEMUL_FEATURE_AES_OTP)
+            printf(" aes_otp");
+        printf("\n");
+    }
+
+    struct usb_resp_info_stmp_t stmp;
+    ret = hwemul_get_info(&hwdev, HWEMUL_INFO_STMP, &stmp, sizeof(stmp));
+    if(ret != sizeof(stmp))
+    {
+        printf("Cannot get stmp: %d\n", ret);
+        goto Lerr;
+    }
+    if(!g_quiet)
+    {
+        printf("Device stmp:\n");
+        printf("  chip ID: %x (%s)\n", stmp.chipid,hwemul_get_product_string(&stmp));
+        printf("  revision: %d (%s)\n", stmp.rev, hwemul_get_rev_string(&stmp));
+        printf("  supported: %d\n", stmp.is_supported);
+    }
+
+    if(!g_quiet)
+    {
+        void *rom = malloc(64 * 1024);
+        ret = hwemul_rw_mem(&hwdev, 1, 0xc0000000, rom, 64 * 1024);
+        if(ret != 64 * 1024)
+        {
+            printf("Cannot read ROM: %d\n", ret);
+            goto Lerr;
+        }
+    
+        printf("ROM successfully read!\n");
+        FILE *f = fopen("rom.bin", "wb");
+        fwrite(rom, 64 * 1024, 1, f);
+        fclose(f);
+    }
+
+    if(!g_quiet)
+    {
+        struct
+        {
+            uint8_t iv[16];
+            uint8_t data[16];
+        } __attribute__((packed)) dcp_test;
+
+        for(int i = 0; i < 16; i++)
+            dcp_test.iv[i] = rand();
+        for(int i = 0; i < 16; i++)
+            dcp_test.data[i] = rand();
+        printf("DCP\n");
+        printf("  IN\n");
+        printf("    IV:");
+        for(int i = 0; i < 16; i++)
+            printf(" %02x", dcp_test.iv[i]);
+        printf("\n");
+        printf("    IV:");
+        for(int i = 0; i < 16; i++)
+            printf(" %02x", dcp_test.data[i]);
+        printf("\n");
+
+        if(!hwemul_aes_otp(&hwdev, &dcp_test, sizeof(dcp_test), HWEMUL_AES_OTP_ENCRYPT))
+        {
+            printf("  OUT\n");
+            printf("    IV:");
+            for(int i = 0; i < 16; i++)
+                printf(" %02x", dcp_test.iv[i]);
+            printf("\n");
+            printf("    IV:");
+            for(int i = 0; i < 16; i++)
+                printf(" %02x", dcp_test.data[i]);
+            printf("\n");
+        }
+        else
+            printf("DCP error!\n");
+    }
+
+    if(!g_quiet)
+        printf("Starting interactive session. Type 'help' to get help.\n");
+
+    interactive_mode();
+
+    Lerr:
+    if(features.feature_mask & HWEMUL_FEATURE_LOG)
+    {
+        if(!g_quiet)
+            printf("Device log:\n");
+        print_log(&hwdev);
+    }
+    hwemul_release(&hwdev);
+    return 1;
+}