From: Karl Palsson <karlp@tweak.net.au>
Date: Sat, 22 Oct 2011 02:11:04 +0000 (+0000)
Subject: Merge upstream texane/master
X-Git-Url: https://git.gag.com/?a=commitdiff_plain;h=9bc72d4519e63fb1cf657a6c93008eaa8989fd01;p=fw%2Fstlink

Merge upstream texane/master
---

diff --git a/.gitignore b/.gitignore
index 78d3fb5..44d2e4d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@
 libstlink.a
 test_usb
 test_sg
+gdbserver/st-utils
\ No newline at end of file
diff --git a/AUTHORS b/AUTHORS
index ca1df75..0abaac6 100644
--- a/AUTHORS
+++ b/AUTHORS
@@ -7,3 +7,5 @@ karlp@tweak.net.au
 h0rr0rrdrag@gmail.com
 mstempin@com1.fr
 bon@elektron.ikp.physik.tu-darmstadt.de
+nelsonjm@macpod.neta
+ned@bike-nomad.com
\ No newline at end of file
diff --git a/Makefile b/Makefile
index 4c3b44b..c5bb1d9 100644
--- a/Makefile
+++ b/Makefile
@@ -1,21 +1,36 @@
-
+# make ... for both libusb and libsg
+#
+# make CONFIG_USE_LIBSG=0 ...
+# for just libusb
+#
 VPATH=src
 
-SOURCES_LIB=stlink-common.c stlink-usb.c stlink-sg.c
+SOURCES_LIB=stlink-common.c stlink-usb.c
 OBJS_LIB=$(SOURCES_LIB:.c=.o)
+TEST_PROGRAMS=test_usb
+LDFLAGS=-lusb-1.0 -L. -lstlink
+
+ifeq ($(CONFIG_USE_LIBSG),)
+CONFIG_USE_LIBSG=1
+endif
+
+ifneq ($(CONFIG_USE_LIBSG),0)
+SOURCES_LIB+=stlink-sg.c
+CFLAGS+=-DCONFIG_USE_LIBSG=1
+LDFLAGS+=-lsgutils2
+TEST_PROGRAMS+=test_sg
+endif
 
 CFLAGS+=-g
-CFLAGS+=-DCONFIG_USE_LIBUSB
-CFLAGS+=-DCONFIG_USE_LIBSG
-CFLAGS+=-DDEBUG
+CFLAGS+=-DCONFIG_USE_LIBUSB=1
+CFLAGS+=-DDEBUG=1
 CFLAGS+=-std=gnu99
 CFLAGS+=-Wall -Wextra
 
-LDFLAGS=-lstlink -lusb-1.0 -lsgutils2 -L.
 
 LIBRARY=libstlink.a
 
-all:  $(LIBRARY) test_usb test_sg 
+all:  $(LIBRARY) flash gdbserver $(TEST_PROGRAMS)
 
 $(LIBRARY): $(OBJS_LIB)
 	@echo "objs are $(OBJS_LIB)"
@@ -42,5 +57,15 @@ clean:
 	rm -rf $(LIBRARY)
 	rm -rf test_usb*
 	rm -rf test_sg*
+
+distclean: clean
+	$(MAKE) -C flash clean
+	$(MAKE) -C gdbserver clean
 	
-.PHONY: clean all
+flash:
+	$(MAKE) -C flash CONFIG_USE_LIBSG="$(CONFIG_USE_LIBSG)" 
+
+gdbserver:
+	$(MAKE) -C gdbserver
+
+.PHONY: clean all flash gdbserver
diff --git a/TODO b/TODO
index 4d1929c..e3795fc 100644
--- a/TODO
+++ b/TODO
@@ -1,10 +1,17 @@
-. flash writing is not working from GDB
-add a standalone application, flash/
+. flash tool
+ . test support for reading
+ . writing is not working. refer to the specs, may have changed for stm32l
+ . then test with blink_flash example
+ . then update the documentation
 
-. add a startup.S based example
+. documentation
+ . make README points to doc/tutorial
 
 . remove libsg dependency using:
 https://github.com/afaerber/stlink/tree/master/libstlink
 
 . compile and test a realtime kernel, for instance:
-http://www.chibios.org/dokuwiki/doku.php?id=start
+http://www.chibios.org/dokuwiki/doku.php?id=chibios:articles:stm32l_discovery
+svn checkout https://chibios.svn.sourceforge.net/svnroot/chibios/trunk ;
+cd chibios/trunk/demos/ARMCM3-STM32L152-DISCOVERY ;
+make ;
diff --git a/doc/tutorial/tutorial.pdf b/doc/tutorial/tutorial.pdf
index b84d061..9202ed4 100644
Binary files a/doc/tutorial/tutorial.pdf and b/doc/tutorial/tutorial.pdf differ
diff --git a/doc/tutorial/tutorial.tex b/doc/tutorial/tutorial.tex
index cef3632..d1ba4fd 100644
--- a/doc/tutorial/tutorial.tex
+++ b/doc/tutorial/tutorial.tex
@@ -56,30 +56,37 @@ point of view, those versions differ only in the transport layer used to communi
 (v1 uses SCSI passthru commands, while v2 uses raw USB).
 
 \paragraph{}
-Before continuing, the following dependencies are required:
+Before continuing, the following dependencies must be met:
 \begin{itemize}
 \item libusb-1.0
-\item libsg2
+\item libsgutils2 (optionnal)
 \end{itemize}
 
+\paragraph{}
+STLINK should run on any system meeting the above constraints.
+
 \paragraph{}
 The STLINK software source code is retrieved using:\\
 \begin{small}
 \begin{lstlisting}[frame=tb]
-git clone https://github.com/texane/stlink stlink.git
+$> git clone https://github.com/texane/stlink stlink.git
 \end{lstlisting}
 \end{small}
 
 \paragraph{}
-The GDB server is called st-util and is built using:\\
+Everything can be built from the top directory:\\
 \begin{small}
 \begin{lstlisting}[frame=tb]
-$> cd stlink.git;
-$> make ;
-$> cd gdbserver ;
-$> make ;
+$> cd stlink.git
+$> make CONFIG_USE_LIBSG=0
 \end{lstlisting}
 \end{small}
+It includes:
+\begin{itemize}
+\item a communication library (stlink.git/libstlink.a),
+\item a GDB server (stlink.git/gdbserver/st-util),
+\item a flash manipulation tool (stlink.git/flash/flash).
+\end{itemize}
 
 
 \newpage
@@ -88,8 +95,9 @@ $> make ;
 A simple LED blinking example is provided in the example directory. It is built using:\\
 \begin{small}
 \begin{lstlisting}[frame=tb]
+# update the make option accordingly to your architecture
 cd stlink.git/example/blink ;
-PATH=$TOOLCHAIN_PATH/bin:$PATH make ;
+PATH=$TOOLCHAIN_PATH/bin:$PATH make CONFIG_STM32L_DISCOVERY=1;
 \end{lstlisting}
 \end{small}
 
@@ -102,7 +110,8 @@ are using, you must run one of the 2 commands:\\
 $> sudo ./st-util /dev/sg2
 
 # STM32L discovery kit
-$> sudo ./st-util
+# 2 dummy command line arguments needed, will be fixed soon
+$> sudo ./st-util fu bar
 \end{lstlisting}
 \end{small}
 
@@ -144,6 +153,31 @@ $> continue
 The board BLUE and GREEN leds should be blinking (those leds are near the user and reset buttons).
 
 
+\newpage
+\section{Reading and writing to flash}
+\paragraph{}
+Flash memory reading and writing is done by a separate tool. A binary running in flash is assumed to
+be linked against address 0x8000000. The flash tool is then used as shown below:\\
+\begin{small}
+\begin{lstlisting}[frame=tb]
+# change to the flash tool directory
+$> cd stlink.git/flash ;
+
+# stlinkv1 command to read 4096 from flash into out.bin
+$> ./flash read /dev/sg2 out.bin 0x8000000 4096
+
+# stlinkv2 command
+$> ./flash read out.bin 0x8000000 4096
+
+# stlinkv1 command to write the file in.bin into flash
+$> ./flash write /dev/sg2 in.bin 0x8000000
+
+# stlinkv2 command
+$> ./flash write in.bin 0x8000000
+\end{lstlisting}
+\end{small}
+
+
 \newpage
 \section{Notes}
 
@@ -187,6 +221,17 @@ $> make
 \end{lstlisting}
 \end{small}
 
+\subsection{STM32VL support}
+\paragraph{}
+It seems support for STM32VL is quite broken. If it does not work, try build STLINK using libsg:
+\begin{small}
+\begin{lstlisting}[frame=tb]
+$> cd stlink.git
+$> make CONFIG_USE_LIBSG=1
+\end{lstlisting}
+\end{small}
+
+
 \newpage
 \section{References}
 \begin{itemize}
diff --git a/example/blink/Makefile b/example/blink/Makefile
index 0f4f712..e5f7045 100644
--- a/example/blink/Makefile
+++ b/example/blink/Makefile
@@ -4,8 +4,15 @@ BIN_IMAGE=blink.bin
 CC=arm-none-eabi-gcc
 OBJCOPY=arm-none-eabi-objcopy
 
-CFLAGS=-O2 -mlittle-endian -mthumb -g
-CFLAGS+=-mcpu=cortex-m3 -ffreestanding -nostdlib -nostdinc
+CFLAGS=-g -O2 -mlittle-endian -mthumb
+ifeq ($(CONFIG_STM32L_DISCOVERY), 1)
+	CFLAGS+=-mcpu=cortex-m3 -DCONFIG_STM32L_DISCOVERY
+else ifeq ($(CONFIG_STM32VL_DISCOVERY), 1)
+	CFLAGS+=-mcpu=cortex-m3 -DCONFIG_STM32VL_DISCOVERY=1
+else ifeq ($(CONFIG_STM32F4_DISCOVERY), 1)
+	CFLAGS+=-mcpu=cortex-m4 -DCONFIG_STM32F4_DISCOVERY=1	
+endif
+	CFLAGS+=-ffreestanding -nostdlib -nostdinc
 
 # to run from SRAM
 CFLAGS+=-Wl,-Ttext,0x20000000 -Wl,-e,0x20000000
diff --git a/example/blink/main.c b/example/blink/main.c
index 929e3b9..0889b81 100644
--- a/example/blink/main.c
+++ b/example/blink/main.c
@@ -5,10 +5,6 @@ typedef unsigned int uint32_t;
 
 /* hardware configuration */
 
-#define CONFIG_STM32L_DISCOVERY 1
-#define CONFIG_STM32VL_DISCOVERY 0
-
-
 #if CONFIG_STM32VL_DISCOVERY
 
 # define GPIOC 0x40011000 /* port C */
@@ -58,6 +54,36 @@ static inline void switch_leds_off(void)
   *(volatile uint32_t*)GPIOB_ODR = 0;
 }
 
+#elif CONFIG_STM32F4_DISCOVERY
+
+#define GPIOD 0x40020C00 /* port D */
+# define GPIOD_MODER (GPIOD + 0x00) /* port mode register */
+# define GPIOD_ODR (GPIOD + 0x14) /* port output data register */
+
+# define LED_GREEN (1 << 12) /* port B, pin 12 */
+# define LED_ORANGE (1 << 13) /* port B, pin 13 */
+# define LED_RED (1 << 14) /* port B, pin 14 */
+# define LED_BLUE (1 << 15) /* port B, pin 15 */
+
+static inline void setup_leds(void)
+{
+  *(volatile uint32_t*)GPIOD_MODER |= (1 << (12 * 2)) | (1 << (13 * 2)) |
+  	(1 << (13 * 2)) | (1 << (14 * 2)) | (1 << (15 * 2));
+}
+
+
+static inline void switch_leds_on(void)
+{
+  *(volatile uint32_t*)GPIOD_ODR = LED_GREEN | LED_ORANGE | LED_RED | LED_BLUE;
+}
+
+static inline void switch_leds_off(void)
+{
+  *(volatile uint32_t*)GPIOD_ODR = 0;
+}
+
+#else
+#error "Architecture must be defined!"
 #endif /* otherwise, error */
 
 
diff --git a/example/blink_flash/Makefile b/example/blink_flash/Makefile
new file mode 100644
index 0000000..7776471
--- /dev/null
+++ b/example/blink_flash/Makefile
@@ -0,0 +1,36 @@
+EXECUTABLE=blink.elf
+BIN_IMAGE=blink.bin
+
+CC=arm-none-eabi-gcc
+OBJCOPY=arm-none-eabi-objcopy
+
+CFLAGS=-O2 -mlittle-endian -mthumb
+
+CFLAGS=-g -O2 -mlittle-endian -mthumb
+ifeq ($(CONFIG_STM32L_DISCOVERY), 1)
+	CFLAGS+=-mcpu=cortex-m3 -DCONFIG_STM32L_DISCOVERY
+else ifeq ($(CONFIG_STM32VL_DISCOVERY), 1)
+	CFLAGS+=-mcpu=cortex-m3 -DCONFIG_STM32VL_DISCOVERY=1
+else ifeq ($(CONFIG_STM32F4_DISCOVERY), 1)
+	CFLAGS+=-mcpu=cortex-m4 -DCONFIG_STM32F4_DISCOVERY=1	
+else
+$(error "must specify CONFIG_ for board!")
+endif
+	CFLAGS+=-ffreestanding -nostdlib -nostdinc
+
+# to run from FLASH
+CFLAGS+=-Wl,-T,stm32_flash.ld
+
+all: $(BIN_IMAGE)
+
+$(BIN_IMAGE): $(EXECUTABLE)
+	$(OBJCOPY) -O binary $^ $@
+
+$(EXECUTABLE): main.c startup_stm32l1xx_md.s
+	$(CC) $(CFLAGS) $^ -o $@
+
+clean:
+	rm -rf $(EXECUTABLE)
+	rm -rf $(BIN_IMAGE)
+
+.PHONY: all clean
diff --git a/example/blink_flash/main.c b/example/blink_flash/main.c
new file mode 100644
index 0000000..e93fd72
--- /dev/null
+++ b/example/blink_flash/main.c
@@ -0,0 +1,82 @@
+/* missing type */
+
+typedef unsigned int uint32_t;
+
+
+/* hardware configuration */
+
+#define CONFIG_STM32L_DISCOVERY 1
+#define CONFIG_STM32VL_DISCOVERY 0
+
+
+#if CONFIG_STM32VL_DISCOVERY
+
+# define GPIOC 0x40011000 /* port C */
+# define GPIOC_CRH (GPIOC + 0x04) /* port configuration register high */
+# define GPIOC_ODR (GPIOC + 0x0c) /* port output data register */
+
+# define LED_BLUE (1 << 8) /* port C, pin 8 */
+# define LED_GREEN (1 << 9) /* port C, pin 9 */
+
+static inline void setup_leds(void)
+{
+  *(volatile uint32_t*)GPIOC_CRH = 0x44444411;
+}
+
+static inline void switch_leds_on(void)
+{
+  *(volatile uint32_t*)GPIOC_ODR = LED_BLUE | LED_GREEN;
+}
+
+static inline void switch_leds_off(void)
+{
+  *(volatile uint32_t*)GPIOC_ODR = 0;
+}
+
+#elif CONFIG_STM32L_DISCOVERY
+
+# define GPIOB 0x40020400 /* port B */
+# define GPIOB_MODER (GPIOB + 0x00) /* port mode register */
+# define GPIOB_ODR (GPIOB + 0x14) /* port output data register */
+
+# define LED_BLUE (1 << 6) /* port B, pin 6 */
+# define LED_GREEN (1 << 7) /* port B, pin 7 */
+
+static inline void setup_leds(void)
+{
+  /* configure port 6 and 7 as output */
+  *(volatile uint32_t*)GPIOB_MODER |= (1 << (7 * 2)) | (1 << (6 * 2));
+}
+
+static inline void switch_leds_on(void)
+{
+  *(volatile uint32_t*)GPIOB_ODR = LED_BLUE | LED_GREEN;
+}
+
+static inline void switch_leds_off(void)
+{
+  *(volatile uint32_t*)GPIOB_ODR = 0;
+}
+
+#endif /* otherwise, error */
+
+
+#define delay()						\
+do {							\
+  register unsigned int i;				\
+  for (i = 0; i < 1000000; ++i)				\
+    __asm__ __volatile__ ("nop\n\t":::"memory");	\
+} while (0)
+
+void main(void)
+{
+  setup_leds();
+
+  while (1)
+  {
+    switch_leds_on();
+    delay();
+    switch_leds_off();
+    delay();
+  }
+}
diff --git a/example/blink_flash/startup_stm32l1xx_md.s b/example/blink_flash/startup_stm32l1xx_md.s
new file mode 100644
index 0000000..4ec8203
--- /dev/null
+++ b/example/blink_flash/startup_stm32l1xx_md.s
@@ -0,0 +1,366 @@
+/**
+ ******************************************************************************
+ * @file      startup_stm32l1xx_md.s
+ * @author    MCD Application Team
+ * @version   V1.0.0
+ * @date      31-December-2010
+ * @brief     STM32L1xx Ultra Low Power Medium-density Devices vector table for 
+ *            RIDE7 toolchain.
+ *            This module performs:
+ *                - Set the initial SP
+ *                - Set the initial PC == Reset_Handler,
+ *                - Set the vector table entries with the exceptions ISR address
+ *                - Branches to main in the C library (which eventually
+ *                  calls main()).
+ *            After Reset the Cortex-M3 processor is in Thread mode,
+ *            priority is Privileged, and the Stack is set to Main.
+ *******************************************************************************
+ * @attention
+ *
+ * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * <h2><center>&copy; COPYRIGHT 2010 STMicroelectronics</center></h2>
+ ******************************************************************************* 
+ */
+    
+  .syntax unified
+  .cpu cortex-m3
+  .fpu softvfp
+  .thumb
+
+.global g_pfnVectors
+.global Default_Handler
+
+/* start address for the initialization values of the .data section. 
+defined in linker script */
+.word _sidata
+/* start address for the .data section. defined in linker script */  
+.word _sdata
+/* end address for the .data section. defined in linker script */
+.word _edata
+/* start address for the .bss section. defined in linker script */
+.word _sbss
+/* end address for the .bss section. defined in linker script */
+.word _ebss
+
+.equ  BootRAM, 0xF108F85F
+/**
+ * @brief  This is the code that gets called when the processor first
+ *          starts execution following a reset event. Only the absolutely
+ *          necessary set is performed, after which the application
+ *          supplied main() routine is called. 
+ * @param  None
+ * @retval : None
+*/
+
+    .section .text.Reset_Handler
+  .weak Reset_Handler
+  .type Reset_Handler, %function
+Reset_Handler:
+
+/* Copy the data segment initializers from flash to SRAM */  
+  movs r1, #0
+  b LoopCopyDataInit
+
+CopyDataInit:
+  ldr r3, =_sidata
+  ldr r3, [r3, r1]
+  str r3, [r0, r1]
+  adds r1, r1, #4
+    
+LoopCopyDataInit:
+  ldr r0, =_sdata
+  ldr r3, =_edata
+  adds r2, r0, r1
+  cmp r2, r3
+  bcc CopyDataInit
+  ldr r2, =_sbss
+  b LoopFillZerobss
+/* Zero fill the bss segment. */  
+FillZerobss:
+  movs r3, #0
+  str r3, [r2], #4
+    
+LoopFillZerobss:
+  ldr r3, = _ebss
+  cmp r2, r3
+  bcc FillZerobss
+/* Call the clock system intitialization function.*/
+/* let main do the system initialization */
+/* bl  SystemInit */
+/* Call the application's entry point.*/
+  bl main
+  bx lr
+.size Reset_Handler, .-Reset_Handler
+
+/**
+ * @brief  This is the code that gets called when the processor receives an 
+ *         unexpected interrupt.  This simply enters an infinite loop, preserving
+ *         the system state for examination by a debugger.
+ *
+ * @param  None     
+ * @retval None       
+*/
+    .section .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+  b Infinite_Loop
+  .size Default_Handler, .-Default_Handler
+/*******************************************************************************
+*
+* The minimal vector table for a Cortex M3. Note that the proper constructs
+* must be placed on this to ensure that it ends up at physical address
+* 0x0000.0000.
+*******************************************************************************/    
+  .section .isr_vector,"a",%progbits
+  .type g_pfnVectors, %object
+  .size g_pfnVectors, .-g_pfnVectors
+    
+    
+g_pfnVectors:
+  .word _estack
+  .word Reset_Handler
+  .word NMI_Handler
+  .word HardFault_Handler
+  .word MemManage_Handler
+  .word BusFault_Handler
+  .word UsageFault_Handler
+  .word 0
+  .word 0
+  .word 0
+  .word 0
+  .word SVC_Handler
+  .word DebugMon_Handler
+  .word 0
+  .word PendSV_Handler
+  .word SysTick_Handler
+  .word WWDG_IRQHandler
+  .word PVD_IRQHandler
+  .word TAMPER_STAMP_IRQHandler
+  .word RTC_WKUP_IRQHandler
+  .word FLASH_IRQHandler
+  .word RCC_IRQHandler
+  .word EXTI0_IRQHandler
+  .word EXTI1_IRQHandler
+  .word EXTI2_IRQHandler
+  .word EXTI3_IRQHandler
+  .word EXTI4_IRQHandler
+  .word DMA1_Channel1_IRQHandler
+  .word DMA1_Channel2_IRQHandler
+  .word DMA1_Channel3_IRQHandler
+  .word DMA1_Channel4_IRQHandler
+  .word DMA1_Channel5_IRQHandler
+  .word DMA1_Channel6_IRQHandler
+  .word DMA1_Channel7_IRQHandler
+  .word ADC1_IRQHandler
+  .word USB_HP_IRQHandler
+  .word USB_LP_IRQHandler
+  .word DAC_IRQHandler
+  .word COMP_IRQHandler
+  .word EXTI9_5_IRQHandler
+  .word LCD_IRQHandler
+  .word TIM9_IRQHandler
+  .word TIM10_IRQHandler
+  .word TIM11_IRQHandler
+  .word TIM2_IRQHandler
+  .word TIM3_IRQHandler
+  .word TIM4_IRQHandler
+  .word I2C1_EV_IRQHandler
+  .word I2C1_ER_IRQHandler
+  .word I2C2_EV_IRQHandler
+  .word I2C2_ER_IRQHandler
+  .word SPI1_IRQHandler
+  .word SPI2_IRQHandler
+  .word USART1_IRQHandler
+  .word USART2_IRQHandler
+  .word USART3_IRQHandler
+  .word EXTI15_10_IRQHandler
+  .word RTC_Alarm_IRQHandler
+  .word USB_FS_WKUP_IRQHandler
+  .word TIM6_IRQHandler
+  .word TIM7_IRQHandler
+  .word 0
+  .word 0
+  .word 0
+  .word 0
+  .word 0
+  .word BootRAM          /* @0x108. This is for boot in RAM mode for 
+                            STM32L15x ULtra Low Power Medium-density devices. */
+   
+/*******************************************************************************
+*
+* Provide weak aliases for each Exception handler to the Default_Handler. 
+* As they are weak aliases, any function with the same name will override 
+* this definition.
+*
+*******************************************************************************/
+    
+  .weak NMI_Handler
+  .thumb_set NMI_Handler,Default_Handler
+
+  .weak HardFault_Handler
+  .thumb_set HardFault_Handler,Default_Handler
+
+  .weak MemManage_Handler
+  .thumb_set MemManage_Handler,Default_Handler
+
+  .weak BusFault_Handler
+  .thumb_set BusFault_Handler,Default_Handler
+
+  .weak UsageFault_Handler
+  .thumb_set UsageFault_Handler,Default_Handler
+
+  .weak SVC_Handler
+  .thumb_set SVC_Handler,Default_Handler
+
+  .weak DebugMon_Handler
+  .thumb_set DebugMon_Handler,Default_Handler
+
+  .weak PendSV_Handler
+  .thumb_set PendSV_Handler,Default_Handler
+
+  .weak SysTick_Handler
+  .thumb_set SysTick_Handler,Default_Handler
+
+  .weak WWDG_IRQHandler
+  .thumb_set WWDG_IRQHandler,Default_Handler
+
+  .weak PVD_IRQHandler
+  .thumb_set PVD_IRQHandler,Default_Handler
+
+  .weak TAMPER_STAMP_IRQHandler
+  .thumb_set TAMPER_STAMP_IRQHandler,Default_Handler
+
+  .weak RTC_WKUP_IRQHandler
+  .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+
+  .weak FLASH_IRQHandler
+  .thumb_set FLASH_IRQHandler,Default_Handler
+
+  .weak RCC_IRQHandler
+  .thumb_set RCC_IRQHandler,Default_Handler
+
+  .weak EXTI0_IRQHandler
+  .thumb_set EXTI0_IRQHandler,Default_Handler
+
+  .weak EXTI1_IRQHandler
+  .thumb_set EXTI1_IRQHandler,Default_Handler
+
+  .weak EXTI2_IRQHandler
+  .thumb_set EXTI2_IRQHandler,Default_Handler
+
+  .weak EXTI3_IRQHandler
+  .thumb_set EXTI3_IRQHandler,Default_Handler
+
+  .weak EXTI4_IRQHandler
+  .thumb_set EXTI4_IRQHandler,Default_Handler
+
+  .weak DMA1_Channel1_IRQHandler
+  .thumb_set DMA1_Channel1_IRQHandler,Default_Handler
+
+  .weak DMA1_Channel2_IRQHandler
+  .thumb_set DMA1_Channel2_IRQHandler,Default_Handler
+
+  .weak DMA1_Channel3_IRQHandler
+  .thumb_set DMA1_Channel3_IRQHandler,Default_Handler
+
+  .weak DMA1_Channel4_IRQHandler
+  .thumb_set DMA1_Channel4_IRQHandler,Default_Handler
+
+  .weak DMA1_Channel5_IRQHandler
+  .thumb_set DMA1_Channel5_IRQHandler,Default_Handler
+
+  .weak DMA1_Channel6_IRQHandler
+  .thumb_set DMA1_Channel6_IRQHandler,Default_Handler
+
+  .weak DMA1_Channel7_IRQHandler
+  .thumb_set DMA1_Channel7_IRQHandler,Default_Handler
+
+  .weak ADC1_IRQHandler
+  .thumb_set ADC1_IRQHandler,Default_Handler
+
+  .weak USB_HP_IRQHandler
+  .thumb_set USB_HP_IRQHandler,Default_Handler
+
+  .weak USB_LP_IRQHandler
+  .thumb_set USB_LP_IRQHandler,Default_Handler
+
+  .weak DAC_IRQHandler
+  .thumb_set DAC_IRQHandler,Default_Handler
+
+  .weak COMP_IRQHandler
+  .thumb_set COMP_IRQHandler,Default_Handler
+
+  .weak EXTI9_5_IRQHandler
+  .thumb_set EXTI9_5_IRQHandler,Default_Handler
+
+  .weak LCD_IRQHandler
+  .thumb_set LCD_IRQHandler,Default_Handler
+  
+  .weak TIM9_IRQHandler
+  .thumb_set TIM9_IRQHandler,Default_Handler
+
+  .weak TIM10_IRQHandler
+  .thumb_set TIM10_IRQHandler,Default_Handler
+
+  .weak TIM11_IRQHandler
+  .thumb_set TIM11_IRQHandler,Default_Handler
+
+  .weak TIM2_IRQHandler
+  .thumb_set TIM2_IRQHandler,Default_Handler
+
+  .weak TIM3_IRQHandler
+  .thumb_set TIM3_IRQHandler,Default_Handler
+
+  .weak TIM4_IRQHandler
+  .thumb_set TIM4_IRQHandler,Default_Handler
+
+  .weak I2C1_EV_IRQHandler
+  .thumb_set I2C1_EV_IRQHandler,Default_Handler
+
+  .weak I2C1_ER_IRQHandler
+  .thumb_set I2C1_ER_IRQHandler,Default_Handler
+
+  .weak I2C2_EV_IRQHandler
+  .thumb_set I2C2_EV_IRQHandler,Default_Handler
+
+  .weak I2C2_ER_IRQHandler
+  .thumb_set I2C2_ER_IRQHandler,Default_Handler
+
+  .weak SPI1_IRQHandler
+  .thumb_set SPI1_IRQHandler,Default_Handler
+
+  .weak SPI2_IRQHandler
+  .thumb_set SPI2_IRQHandler,Default_Handler
+
+  .weak USART1_IRQHandler
+  .thumb_set USART1_IRQHandler,Default_Handler
+
+  .weak USART2_IRQHandler
+  .thumb_set USART2_IRQHandler,Default_Handler
+
+  .weak USART3_IRQHandler
+  .thumb_set USART3_IRQHandler,Default_Handler
+
+  .weak EXTI15_10_IRQHandler
+  .thumb_set EXTI15_10_IRQHandler,Default_Handler
+
+  .weak RTC_Alarm_IRQHandler
+  .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+
+  .weak USB_FS_WKUP_IRQHandler
+  .thumb_set USB_FS_WKUP_IRQHandler,Default_Handler
+
+  .weak TIM6_IRQHandler
+  .thumb_set TIM6_IRQHandler,Default_Handler
+
+  .weak TIM7_IRQHandler
+  .thumb_set TIM7_IRQHandler,Default_Handler
+
+/******************** (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE***/
+
diff --git a/example/blink_flash/stm32_flash.ld b/example/blink_flash/stm32_flash.ld
new file mode 100644
index 0000000..146b16e
--- /dev/null
+++ b/example/blink_flash/stm32_flash.ld
@@ -0,0 +1,173 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash.ld
+**
+**  Abstract    : Linker script for STM32L152RB Device with
+**                128KByte FLASH, 16KByte RAM
+**
+**                Set heap size, stack size and stack location according
+**                to application requirements.
+**
+**                Set memory bank area and size if external memory is used.
+**
+**  Target      : STMicroelectronics STM32
+**
+**  Environment : Atollic TrueSTUDIO(R)
+**
+**  Distribution: The file is distributed “as is,” without any warranty
+**                of any kind.
+**
+**  (c)Copyright Atollic AB.
+**  You may use this file as-is or modify it according to the needs of your
+**  project. Distribution of this file (unmodified or modified) is not
+**  permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the
+**  rights to distribute the assembled, compiled & linked contents of this
+**  file as part of an application binary file, provided that it is built
+**  using the Atollic TrueSTUDIO(R) toolchain.
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_estack = 0x20004000;    /* end of 16K RAM */
+
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0;      /* required amount of heap  */
+_Min_Stack_Size = 0x80; /* required amount of stack */
+
+/* Specify the memory areas */
+MEMORY
+{
+  FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 128K
+  RAM (xrw)       : ORIGIN = 0x20000000, LENGTH = 16K
+  MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+  RW_EEPROM (rw)   : ORIGIN = 0x08080000, LENGTH = 32
+}
+
+/* Define output sections */
+SECTIONS
+{
+  /* The startup code goes first into FLASH */
+  .isr_vector :
+  {
+    . = ALIGN(4);
+    KEEP(*(.isr_vector)) /* Startup code */
+    . = ALIGN(4);
+  } >FLASH
+
+  /* The program code and other data goes into FLASH */
+  .text :
+  {
+    . = ALIGN(4);
+    *(.text)           /* .text sections (code) */
+    *(.text*)          /* .text* sections (code) */
+    *(.rodata)         /* .rodata sections (constants, strings, etc.) */
+    *(.rodata*)        /* .rodata* sections (constants, strings, etc.) */
+    *(.glue_7)         /* glue arm to thumb code */
+    *(.glue_7t)        /* glue thumb to arm code */
+	*(.eh_frame)
+
+    KEEP (*(.init))
+    KEEP (*(.fini))
+
+    . = ALIGN(4);
+    _etext = .;        /* define a global symbols at end of code */
+  } >FLASH
+
+
+   .ARM.extab   : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
+    .ARM : {
+    __exidx_start = .;
+      *(.ARM.exidx*)
+      __exidx_end = .;
+    } >FLASH
+
+  .preinit_array     :
+  {
+    PROVIDE_HIDDEN (__preinit_array_start = .);
+    KEEP (*(.preinit_array*))
+    PROVIDE_HIDDEN (__preinit_array_end = .);
+  } >FLASH
+  .init_array :
+  {
+    PROVIDE_HIDDEN (__init_array_start = .);
+    KEEP (*(SORT(.init_array.*)))
+    KEEP (*(.init_array*))
+    PROVIDE_HIDDEN (__init_array_end = .);
+  } >FLASH
+  .fini_array :
+  {
+    PROVIDE_HIDDEN (__fini_array_start = .);
+    KEEP (*(.fini_array*))
+    KEEP (*(SORT(.fini_array.*)))
+    PROVIDE_HIDDEN (__fini_array_end = .);
+  } >FLASH
+
+  /* used by the startup to initialize data */
+  _sidata = .;
+
+  /* Initialized data sections goes into RAM, load LMA copy after code */
+  .data : AT ( _sidata )
+  {
+    . = ALIGN(4);
+    _sdata = .;        /* create a global symbol at data start */
+    *(.data)           /* .data sections */
+    *(.data*)          /* .data* sections */
+
+    . = ALIGN(4);
+    _edata = .;        /* define a global symbol at data end */
+  } >RAM
+
+  /* Uninitialized data section */
+  . = ALIGN(4);
+  .bss :
+  {
+    /* This is used by the startup in order to initialize the .bss secion */
+    _sbss = .;         /* define a global symbol at bss start */
+    __bss_start__ = _sbss;
+    *(.bss)
+    *(.bss*)
+    *(COMMON)
+
+    . = ALIGN(4);
+    _ebss = .;         /* define a global symbol at bss end */
+    __bss_end__ = _ebss;
+  } >RAM
+
+  /* User_heap_stack section, used to check that there is enough RAM left */
+  ._user_heap_stack :
+  {
+    . = ALIGN(4);
+    PROVIDE ( end = . );
+    PROVIDE ( _end = . );
+    . = . + _Min_Heap_Size;
+    . = . + _Min_Stack_Size;
+    . = ALIGN(4);
+  } >RAM
+
+  /* MEMORY_bank1 section, code must be located here explicitly            */
+  /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */
+  .memory_b1_text :
+  {
+    *(.mb1text)        /* .mb1text sections (code) */
+    *(.mb1text*)       /* .mb1text* sections (code)  */
+    *(.mb1rodata)      /* read-only data (constants) */
+    *(.mb1rodata*)
+  } >MEMORY_B1
+
+  /* Remove information from the standard libraries */
+  /DISCARD/ :
+  {
+    libc.a ( * )
+    libm.a ( * )
+    libgcc.a ( * )
+  }
+
+  .ARM.attributes 0 : { *(.ARM.attributes) }
+  
+  .DataFlash (NOLOAD): {*(.DataFlash)} >RW_EEPROM
+}
diff --git a/example/blink_flash/system_stm32l1xx.c b/example/blink_flash/system_stm32l1xx.c
new file mode 100644
index 0000000..6deab32
--- /dev/null
+++ b/example/blink_flash/system_stm32l1xx.c
@@ -0,0 +1,367 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32l1xx.c
+  * @author  MCD Application Team
+  * @version V1.0.0
+  * @date    2-June-2011
+  * @brief   CMSIS Cortex-M3 Device Peripheral Access Layer System Source File.
+  *          This file contains the system clock configuration for STM32L1xx Ultra
+  *          Low Medium-density devices, and is generated by the clock configuration
+  *          tool "STM32L1xx_Clock_Configuration_V1.0.0.xls".
+  *             
+  * 1.  This file provides two functions and one global variable to be called from 
+  *     user application:
+  *      - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
+  *                      and Divider factors, AHB/APBx prescalers and Flash settings),
+  *                      depending on the configuration made in the clock xls tool. 
+  *                      This function is called at startup just after reset and 
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32l1xx_md.s" file.
+  *                        
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick 
+  *                                  timer or configure other parameters.
+  *                                     
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.   
+  *      
+  * 2. After each device reset the MSI (2.1 MHz Range) is used as system clock source.
+  *    Then SystemInit() function is called, in "startup_stm32l1xx_md.s" file, to
+  *    configure the system clock before to branch to main program.    
+  *    
+  * 3. If the system clock source selected by user fails to startup, the SystemInit()
+  *    function will do nothing and MSI still used as system clock source. User can 
+  *    add some code to deal with this issue inside the SetSysClock() function.       
+  * 
+  * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define
+  *    in "stm32l1xx.h" file. When HSE is used as system clock source, directly or
+  *    through PLL, and you are using different crystal you have to adapt the HSE
+  *    value to your own configuration.
+  * 
+  * 5. This file configures the system clock as follows:  
+  *=============================================================================
+  *                         System Clock Configuration
+  *=============================================================================
+  *        System clock source          | HSI
+  *----------------------------------------------------------------------------- 
+  *        SYSCLK                       | 16000000 Hz
+  *----------------------------------------------------------------------------- 
+  *        HCLK                         | 16000000 Hz
+  *----------------------------------------------------------------------------- 
+  *        AHB Prescaler                | 1
+  *----------------------------------------------------------------------------- 
+  *        APB1 Prescaler               | 1
+  *----------------------------------------------------------------------------- 
+  *        APB2 Prescaler               | 1
+  *----------------------------------------------------------------------------- 
+  *        HSE Frequency                | 8000000 Hz
+  *----------------------------------------------------------------------------- 
+  *        PLL DIV                      | Not Used
+  *----------------------------------------------------------------------------- 
+  *        PLL MUL                      | Not Used
+  *----------------------------------------------------------------------------- 
+  *        VDD                          | 3.3 V
+  *----------------------------------------------------------------------------- 
+  *        Vcore                        | 1.8 V (Range 1)
+  *----------------------------------------------------------------------------- 
+  *        Flash Latency                | 0 WS
+  *----------------------------------------------------------------------------- 
+  *        Require 48MHz for USB clock  | Disabled
+  *----------------------------------------------------------------------------- 
+  *=============================================================================
+  ****************************************************************************** 
+  * @attention
+  *
+  * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+  * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+  * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
+  * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+  * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+  * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+  *
+  * <h2><center>&copy; COPYRIGHT 2010 STMicroelectronics</center></h2>
+  ******************************************************************************  
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32l1xx_system
+  * @{
+  */  
+  
+/** @addtogroup STM32L1xx_System_Private_Includes
+  * @{
+  */
+
+#include "stm32l1xx.h"
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32L1xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32L1xx_System_Private_Defines
+  * @{
+  */
+/*!< Uncomment the following line if you need to relocate your vector Table in
+     Internal SRAM. */ 
+/* #define VECT_TAB_SRAM */
+#define VECT_TAB_OFFSET  0x0 /*!< Vector Table base offset field. 
+                                  This value must be a multiple of 0x200. */
+/**
+  * @}
+  */
+
+/** @addtogroup STM32L1xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32L1xx_System_Private_Variables
+  * @{
+  */
+uint32_t SystemCoreClock    = 16000000;
+__I uint8_t PLLMulTable[9] = {3, 4, 6, 8, 12, 16, 24, 32, 48};
+__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32L1xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+static void SetSysClock(void);
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32L1xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system.
+  *         Initialize the Embedded Flash Interface, the PLL and update the 
+  *         SystemCoreClock variable.
+  * @param  None
+  * @retval None
+  */
+void SystemInit (void)
+{
+  /*!< Set MSION bit */
+  RCC->CR |= (uint32_t)0x00000100;
+
+  /*!< Reset SW[1:0], HPRE[3:0], PPRE1[2:0], PPRE2[2:0], MCOSEL[2:0] and MCOPRE[2:0] bits */
+  RCC->CFGR &= (uint32_t)0x88FFC00C;
+  
+  /*!< Reset HSION, HSEON, CSSON and PLLON bits */
+  RCC->CR &= (uint32_t)0xEEFEFFFE;
+
+  /*!< Reset HSEBYP bit */
+  RCC->CR &= (uint32_t)0xFFFBFFFF;
+
+  /*!< Reset PLLSRC, PLLMUL[3:0] and PLLDIV[1:0] bits */
+  RCC->CFGR &= (uint32_t)0xFF02FFFF;
+
+  /*!< Disable all interrupts */
+  RCC->CIR = 0x00000000;
+    
+  /* Configure the System clock frequency, AHB/APBx prescalers and Flash settings */
+  SetSysClock();
+
+#ifdef VECT_TAB_SRAM
+  SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */
+#else
+  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */
+#endif
+}
+
+/**
+  * @brief  Update SystemCoreClock according to Clock Register Values
+  * @note   - The system frequency computed by this function is not the real 
+  *           frequency in the chip. It is calculated based on the predefined 
+  *           constant and the selected clock source:
+  *             
+  *           - If SYSCLK source is MSI, SystemCoreClock will contain the MSI 
+  *             value as defined by the MSI range.
+  *                                   
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+  *                                              
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+  *                          
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 
+  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.
+  *         
+  *         (*) HSI_VALUE is a constant defined in stm32l1xx.h file (default value
+  *             16 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.   
+  *    
+  *         (**) HSE_VALUE is a constant defined in stm32l1xx.h file (default value
+  *              8 MHz), user has to ensure that HSE_VALUE is same as the real
+  *              frequency of the crystal used. Otherwise, this function may
+  *              have wrong result.
+  *                
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.  
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate (void)
+{
+  uint32_t tmp = 0, pllmul = 0, plldiv = 0, pllsource = 0, msirange = 0;
+
+  /* Get SYSCLK source -------------------------------------------------------*/
+  tmp = RCC->CFGR & RCC_CFGR_SWS;
+  
+  switch (tmp)
+  {
+    case 0x00:  /* MSI used as system clock */
+      msirange = (RCC->ICSCR & RCC_ICSCR_MSIRANGE) >> 13;
+      SystemCoreClock = (32768 * (1 << (msirange + 1)));
+      break;
+    case 0x04:  /* HSI used as system clock */
+      SystemCoreClock = HSI_VALUE;
+      break;
+    case 0x08:  /* HSE used as system clock */
+      SystemCoreClock = HSE_VALUE;
+      break;
+    case 0x0C:  /* PLL used as system clock */
+      /* Get PLL clock source and multiplication factor ----------------------*/
+      pllmul = RCC->CFGR & RCC_CFGR_PLLMUL;
+      plldiv = RCC->CFGR & RCC_CFGR_PLLDIV;
+      pllmul = PLLMulTable[(pllmul >> 18)];
+      plldiv = (plldiv >> 22) + 1;
+      
+      pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
+
+      if (pllsource == 0x00)
+      {
+        /* HSI oscillator clock selected as PLL clock entry */
+        SystemCoreClock = (((HSI_VALUE) * pllmul) / plldiv);
+      }
+      else
+      {
+        /* HSE selected as PLL clock entry */
+        SystemCoreClock = (((HSE_VALUE) * pllmul) / plldiv);
+      }
+      break;
+    default: /* MSI used as system clock */
+      msirange = (RCC->ICSCR & RCC_ICSCR_MSIRANGE) >> 13;
+      SystemCoreClock = (32768 * (1 << (msirange + 1)));
+      break;
+  }
+  /* Compute HCLK clock frequency --------------------------------------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+  /* HCLK clock frequency */
+  SystemCoreClock >>= tmp;
+}
+
+/**
+  * @brief  Configures the System clock frequency, AHB/APBx prescalers and Flash 
+  *         settings.
+  * @note   This function should be called only once the RCC clock configuration  
+  *         is reset to the default reset state (done in SystemInit() function).             
+  * @param  None
+  * @retval None
+  */
+static void SetSysClock(void)
+{
+  __IO uint32_t StartUpCounter = 0, HSIStatus = 0;
+  
+  /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration ---------------------------*/
+  /* Enable HSI */
+  RCC->CR |= ((uint32_t)RCC_CR_HSION);
+ 
+  /* Wait till HSI is ready and if Time out is reached exit */
+  do
+  {
+    HSIStatus = RCC->CR & RCC_CR_HSIRDY;
+  } while((HSIStatus == 0) && (StartUpCounter != HSI_STARTUP_TIMEOUT));
+
+  if ((RCC->CR & RCC_CR_HSIRDY) != RESET)
+  {
+    HSIStatus = (uint32_t)0x01;
+  }
+  else
+  {
+    HSIStatus = (uint32_t)0x00;
+  }
+    
+  if (HSIStatus == (uint32_t)0x01)
+  {
+    /* Flash 0 wait state */
+    FLASH->ACR &= ~FLASH_ACR_LATENCY;
+    
+    /* Disable Prefetch Buffer */
+    FLASH->ACR &= ~FLASH_ACR_PRFTEN;
+
+    /* Disable 64-bit access */
+    FLASH->ACR &= ~FLASH_ACR_ACC64;
+    
+
+    /* Power enable */
+    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
+  
+    /* Select the Voltage Range 1 (1.8 V) */
+    PWR->CR = PWR_CR_VOS_0;
+  
+  
+    /* Wait Until the Voltage Regulator is ready */
+    while((PWR->CSR & PWR_CSR_VOSF) != RESET)
+    {
+    }
+      
+    /* HCLK = SYSCLK /1*/
+    RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
+    /* PCLK2 = HCLK /1*/
+    RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1;
+    
+    /* PCLK1 = HCLK /1*/
+    RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV1;
+    
+    /* Select HSI as system clock source */
+    RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
+    RCC->CFGR |= (uint32_t)RCC_CFGR_SW_HSI;
+
+    /* Wait till HSI is used as system clock source */
+    while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_HSI)
+    {
+    }
+  }
+  else
+  {
+    /* If HSI fails to start-up, the application will have wrong clock
+       configuration. User can add here some code to deal with this error */
+  }
+}
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/
diff --git a/flash/Makefile b/flash/Makefile
index fe0dff7..8a30b41 100644
--- a/flash/Makefile
+++ b/flash/Makefile
@@ -1,12 +1,27 @@
+# make ... for both libusb and libsg
+#
+# make CONFIG_USE_LIBSG=0 ...
+# for just libusb
+#
+CC=gcc
+
 CFLAGS+=-g
-CFLAGS+=-DCONFIG_USE_LIBUSB
-CFLAGS+=-DCONFIG_USE_LIBSG
+CFLAGS+=-DCONFIG_USE_LIBUSB=1
 CFLAGS+=-DDEBUG
 CFLAGS+=-std=gnu99
 CFLAGS+=-Wall -Wextra
 CFLAGS+=-I../src
 
-LDFLAGS=-L.. -lstlink -lusb-1.0 -lsgutils2
+LDFLAGS=-lusb-1.0 -L.. -lstlink 
+
+ifeq ($(CONFIG_USE_LIBSG),)
+CONFIG_USE_LIBSG=1
+endif
+
+ifneq ($(CONFIG_USE_LIBSG),0)
+CFLAGS+=-DCONFIG_USE_LIBSG=1
+LDFLAGS+=-lsgutils2
+endif
 
 SRCS=main.c
 OBJS=$(SRCS:.c=.o)
diff --git a/flash/main.c b/flash/main.c
index c8b15e0..e474174 100644
--- a/flash/main.c
+++ b/flash/main.c
@@ -3,47 +3,134 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
 #include "stlink-common.h"
 
 
-int main(int ac, char** av)
+struct opts
 {
-  /* stlinkv1 command line: ./flash /dev/sgX path addr */
-  /* stlinkv2 command line: ./flash path addr */
-
-  stlink_t* sl = NULL;
+  unsigned int do_read;
+  const char* devname;
+  const char* filename;
   stm32_addr_t addr;
-  const char* path;
-  int err;
+  size_t size;
+};
+
+static void usage(void)
+{
+    puts("stlinkv1 command line: ./flash {read|write} /dev/sgX path addr <size>");
+    puts("stlinkv2 command line: ./flash {read|write} path addr <size>");
+}
+
+static int get_opts(struct opts* o, int ac, char** av)
+{
+  /* stlinkv1 command line: ./flash {read|write} /dev/sgX path addr <size> */
+  /* stlinkv2 command line: ./flash {read|write} path addr <size> */
+
+  unsigned int i = 0;
+
+  if (ac < 3) return -1;
 
-  if (ac == 4) /* stlinkv1 */
+  /* stlinkv2 */
+  o->devname = NULL;
+
+  if (strcmp(av[0], "read") == 0)
   {
-    static const int scsi_verbose = 2;
-    sl = stlink_quirk_open(av[1], scsi_verbose);
-    path = av[2];
-    addr = strtoul(av[3], NULL, 16);
+    o->do_read = 1;
+
+    /* stlinkv1 mode */
+    if (ac == 5)
+    {
+      o->devname = av[1];
+      i = 1;
+    }
+
+    o->size = strtoul(av[i + 3], NULL, 10);
   }
-  else if (ac == 3) /* stlinkv2 */
+  else if (strcmp(av[0], "write") == 0)
   {
-    sl = stlink_open_usb(NULL, 10);
-    path = av[1];
-    addr = strtoul(av[2], NULL, 16);
+    o->do_read = 0;
+
+    /* stlinkv1 mode */
+    if (ac == 4)
+    {
+      o->devname = av[1];
+      i = 1;
+    }
   }
-  else /* invalid */
+  else
+  {
+    return -1;
+  }
+
+  o->filename = av[i + 1];
+  o->addr = strtoul(av[i + 2], NULL, 16);
+
+  return 0;
+} 
+
+
+int main(int ac, char** av)
+{
+  stlink_t* sl = NULL;
+  struct opts o;
+  int err = -1;
+
+  if (get_opts(&o, ac - 1, av + 1) == -1)
   {
     printf("invalid command line\n");
+    usage();
     goto on_error;
   }
 
-  if (sl == NULL) goto on_error;
-
-  err = stlink_fwrite_flash(sl, path, addr);
-  if (err == -1)
+  if (o.devname != NULL) /* stlinkv1 */
   {
-    printf("stlink_fwrite_flash() == -1\n");
+#if CONFIG_USE_LIBSG
+    static const int scsi_verbose = 2;
+    sl = stlink_quirk_open(o.devname, scsi_verbose);
+    if (sl == NULL) goto on_error;
+#else
+    printf("not compiled for use with STLink/V1");
     goto on_error;
+#endif
+  }
+  else /* stlinkv2 */
+  {
+    sl = stlink_open_usb(10);
+    if (sl == NULL) goto on_error;
   }
 
+  if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE)
+    stlink_exit_dfu_mode(sl);
+
+  if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE)
+    stlink_enter_swd_mode(sl);
+
+  stlink_reset(sl);
+
+  if (o.do_read == 0) /* write */
+  {
+    err = stlink_fwrite_flash(sl, o.filename, o.addr);
+    if (err == -1)
+    {
+      printf("stlink_fwrite_flash() == -1\n");
+      goto on_error;
+    }
+  }
+  else /* read */
+  {
+    err = stlink_fread(sl, o.filename, o.addr, o.size);
+    if (err == -1)
+    {
+      printf("stlink_fread() == -1\n");
+      goto on_error;
+    }
+  }
+
+  /* success */
+  err = 0;
+
  on_error:
   if (sl != NULL) stlink_close(sl);
 
diff --git a/gdbserver/Makefile b/gdbserver/Makefile
index e9d2774..a8d1b90 100644
--- a/gdbserver/Makefile
+++ b/gdbserver/Makefile
@@ -1,13 +1,24 @@
+# make ... for both libusb and libsg
+#
+# make CONFIG_USE_LIBSG=0 ...
+# for just libusb
+#
 
 PRG := st-util
 OBJS = gdb-remote.o gdb-server.o
 
 CFLAGS+=-g -Wall -Werror -std=gnu99 -I../src
-CFLAGS+=-DCONFIG_USE_LIBUSB
-CFLAGS+=-DCONFIG_USE_LIBSG
-LIBS := -lstlink -lusb-1.0 -lsgutils2
-LDFLAGS+=$(LIBS) -L..
+CFLAGS+=-DCONFIG_USE_LIBUSB=1
+LDFLAGS=-lusb-1.0 -L.. -lstlink 
 
+ifeq ($(CONFIG_USE_LIBSG),)
+CONFIG_USE_LIBSG=1
+endif
+
+ifneq ($(CONFIG_USE_LIBSG),0)
+CFLAGS+=-DCONFIG_USE_LIBSG=1
+LDFLAGS+=-lsgutils2
+endif
 
 all: $(PRG)
 
diff --git a/gdbserver/gdb-server.c b/gdbserver/gdb-server.c
index 72f433f..67f0be0 100644
--- a/gdbserver/gdb-server.c
+++ b/gdbserver/gdb-server.c
@@ -14,6 +14,7 @@
 #include <sys/socket.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
+#include <signal.h>
 
 #include <stlink-common.h>
 
@@ -24,6 +25,12 @@
 #define FLASH_PAGE_MASK (~((1 << 10) - 1))
 #define FLASH_SIZE (FLASH_PAGE * 128)
 
+volatile int do_exit = 0;
+void ctrl_c(int sig)
+{
+  do_exit = 1;
+}
+
 static const char hex[] = "0123456789abcdef";
 
 static const char* current_memory_map = NULL;
@@ -32,6 +39,11 @@ static const char* current_memory_map = NULL;
  * Chip IDs are explained in the appropriate programming manual for the
  * DBGMCU_IDCODE register (0xE0042000)
  */
+
+#define CORE_M3_R1 0x1BA00477
+#define CORE_M3_R2 0x4BA00477
+#define CORE_M4_R0 0x2BA01477
+
 struct chip_params {
 	uint32_t chip_id;
 	char* description;
@@ -43,11 +55,11 @@ struct chip_params {
 	{ 0x410, "F1 Medium-density device", 0x1ffff7e0,
 	  0x20000,  0x400, 0x5000,  0x1ffff000, 0x800  }, // table 2, pm0063
 	{ 0x411, "F2 device", 0, /* No flash size register found in the docs*/
-	  0x100000,   0x20000, 0x20000, 0x1ff00000, 0x7800  }, // table 1, pm0059
+	  0x100000,   0x20000, 0x20000, 0x1fff0000, 0x7800  }, // table 1, pm0059
 	{ 0x412, "F1 Low-density device", 0x1ffff7e0,
 	  0x8000,   0x400, 0x2800,  0x1ffff000, 0x800  }, // table 1, pm0063
 	{ 0x413, "F4 device", 0x1FFF7A10,
-	  0x100000,   0x20000, 0x20000,  0x1ff00000, 0x7800  }, // table 1, pm0081
+	  0x100000,   0x20000, 0x30000,  0x1fff0000, 0x7800  }, // table 1, pm0081
 	{ 0x414, "F1 High-density device", 0x1ffff7e0,
 	  0x80000,  0x800, 0x10000, 0x1ffff000, 0x800  },  // table 3 pm0063 
           // This ignores the EEPROM! (and uses the page erase size,
@@ -79,15 +91,10 @@ int main(int argc, char** argv) {
 
 	switch(argc) {
 
-		default: {
-			fprintf(stderr, HelpStr, NULL);
-			return 1;
-		}
-
 		case 3 : {
 			//sl = stlink_quirk_open(argv[2], 0);
-                        // FIXME - hardcoded to usb....
-                        sl = stlink_open_usb(argv[2], 10);
+			// FIXME - hardcoded to usb....
+			sl = stlink_open_usb(10);
 			if(sl == NULL) return 1;
 			break;
 		}
@@ -99,6 +106,7 @@ int main(int argc, char** argv) {
 			}
 		}
 
+#if CONFIG_USE_LIBSG
 		case 1 : { // Search ST-LINK (from /dev/sg0 to /dev/sg99)
 			const int DevNumMax = 99;
 			int ExistDevCount = 0;
@@ -136,18 +144,32 @@ int main(int argc, char** argv) {
 			}
 			break;
 		}
+#endif
+
+		default: {
+			fprintf(stderr, HelpStr, NULL);
+			return 1;
+		}
 	}
 
-        if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
-            stlink_exit_dfu_mode(sl);
-        }
+	if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
+		stlink_exit_dfu_mode(sl);
+	}
 
 	if(stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
 	  stlink_enter_swd_mode(sl);
 	}
 
 	uint32_t chip_id = stlink_chip_id(sl);
-	printf("Chip ID is %08x.\n", chip_id);
+	uint32_t core_id = stlink_core_id(sl);
+
+	/* Fix chip_id for F4 */
+	if (((chip_id & 0xFFF) == 0x411) && (core_id == CORE_M4_R0)) {
+	  printf("Fixing wrong chip_id for STM32F4 Rev A errata\n");
+	  chip_id = 0x413;
+	}
+
+	printf("Chip ID is %08x, Core ID is  %08x.\n", chip_id, core_id);
 
 	const struct chip_params* params = NULL;
 
@@ -182,6 +204,9 @@ int main(int argc, char** argv) {
 
 	while(serve(sl, port) == 0);
 
+	/* Switch back to mass storage mode before closing. */
+	stlink_run(sl);
+	stlink_exit_debug_mode(sl);
 	stlink_close(sl);
 
 	return 0;
@@ -578,7 +603,9 @@ int serve(stlink_t *sl, int port) {
 
 	printf("Listening at *:%d...\n", port);
 
+	(void) signal (SIGINT, ctrl_c);
 	int client = accept(sock, NULL, NULL);
+	signal (SIGINT, SIG_DFL);
 	if(client < 0) {
 		perror("accept");
 		return 1;
diff --git a/src/stlink-common.h b/src/stlink-common.h
index cb6c6cc..9481609 100644
--- a/src/stlink-common.h
+++ b/src/stlink-common.h
@@ -175,6 +175,7 @@ extern "C" {
         /* sram settings */
 #define STM32_SRAM_BASE 0x20000000
 #define STM32_SRAM_SIZE (8 * 1024)
+#define STM32L_SRAM_SIZE (16 * 1024)
         stm32_addr_t sram_base;
         size_t sram_size;
 
diff --git a/src/stlink-sg.c b/src/stlink-sg.c
index f8865e1..b8220de 100644
--- a/src/stlink-sg.c
+++ b/src/stlink-sg.c
@@ -80,12 +80,14 @@
 #include <sys/stat.h>
 #include <sys/mman.h>
 
+#include "stlink-common.h"
+
+#if CONFIG_USE_LIBSG
 // sgutils2 (apt-get install libsgutils2-dev)
 #include <scsi/sg_lib.h>
 #include <scsi/sg_pt.h>
-
-#include "stlink-common.h"
 #include "stlink-sg.h"
+#endif
 
 
 // Suspends execution of the calling process for
diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 2433d6c..ff9ef71 100644
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -1,4 +1,3 @@
-#include <assert.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -9,6 +8,8 @@
 #include "stlink-common.h"
 #include "stlink-usb.h"
 
+enum SCSI_Generic_Direction {SG_DXFER_TO_DEV=0, SG_DXFER_FROM_DEV=0x80};
+
 void _stlink_usb_close(stlink_t* sl) {
     struct stlink_libusb * const handle = sl->backend_data;
     // maybe we couldn't even get the usb device?
@@ -19,8 +20,9 @@ void _stlink_usb_close(stlink_t* sl) {
         if (handle->rep_trans != NULL)
             libusb_free_transfer(handle->rep_trans);
 
-        if (handle->usb_handle != NULL)
+        if (handle->usb_handle != NULL) {
             libusb_close(handle->usb_handle);
+        }
 
         libusb_exit(handle->libusb_ctx);
         free(handle);
@@ -88,10 +90,11 @@ int submit_wait(struct stlink_libusb *slu, struct libusb_transfer * trans) {
     return 0;
 }
 
-ssize_t send_recv(struct stlink_libusb* handle,
+ssize_t send_recv(struct stlink_libusb* handle, int terminate,
         unsigned char* txbuf, size_t txsize,
         unsigned char* rxbuf, size_t rxsize) {
     /* note: txbuf and rxbuf can point to the same area */
+    int res = 0;
 
     libusb_fill_bulk_transfer(handle->req_trans, handle->usb_handle,
             handle->ep_req,
@@ -100,122 +103,146 @@ ssize_t send_recv(struct stlink_libusb* handle,
             0
             );
 
-    printf("submit_wait(req)\n");
-
     if (submit_wait(handle, handle->req_trans)) return -1;
 
     /* send_only */
-    if (rxsize == 0) return 0;
-
-    /* read the response */
-
-    libusb_fill_bulk_transfer(handle->rep_trans, handle->usb_handle,
-            handle->ep_rep, rxbuf, rxsize, NULL, NULL, 0);
-
-    printf("submit_wait(rep)\n");
-
-    if (submit_wait(handle, handle->rep_trans)) return -1;
+    if (rxsize != 0) {
+
+        /* read the response */
+        
+        libusb_fill_bulk_transfer(handle->rep_trans, handle->usb_handle,
+                                  handle->ep_rep, rxbuf, rxsize, NULL, NULL, 0);
+        
+        if (submit_wait(handle, handle->rep_trans)) return -1;
+        res = handle->rep_trans->actual_length;
+    }
+    if ((handle->protocoll == 1) && terminate) {
+        /* Read the SG reply */
+        unsigned char sg_buf[13];
+        libusb_fill_bulk_transfer
+            (handle->rep_trans, handle->usb_handle,
+             handle->ep_rep, sg_buf, 13, NULL, NULL, 0);
+        res = submit_wait(handle, handle->rep_trans);
+	/* The STLink doesn't seem to evaluate the sequence number */
+        handle->sg_transfer_idx++;
+        if (res ) return -1;
+    }
 
     return handle->rep_trans->actual_length;
 }
 
 static inline int send_only
-(struct stlink_libusb* handle, unsigned char* txbuf, size_t txsize) {
-    return send_recv(handle, txbuf, txsize, NULL, 0);
+(struct stlink_libusb* handle, int terminate,
+ unsigned char* txbuf, size_t txsize) {
+    return send_recv(handle, terminate, txbuf, txsize, NULL, 0);
 }
 
 
-// KARL - fixme, common code! (or, one per backend)
-// candidate for common code...
-
-
-static int is_stlink_device(libusb_device * dev) {
+/* Search for a STLINK device, either any or teh one with the given PID
+ * Return the protocoll version
+ */
+static int is_stlink_device(libusb_device * dev, uint16_t pid) {
     struct libusb_device_descriptor desc;
+    int version;
 
     if (libusb_get_device_descriptor(dev, &desc))
         return 0;
 
-    printf("device: 0x%04x, 0x%04x\n", desc.idVendor, desc.idProduct);
-
     if (desc.idVendor != USB_ST_VID)
         return 0;
 
-    if (desc.idProduct != USB_STLINK_32L_PID)
+    if ((desc.idProduct != USB_STLINK_32L_PID) && 
+        (desc.idProduct != USB_STLINK_PID ))
+        return 0;
+
+    if(pid && (pid != desc.idProduct))
         return 0;
+    if (desc.idProduct == USB_STLINK_PID )
+        version = 1;
+    else
+        version = 2;
 
-    return 1;
+    return version;
+}
+
+static int fill_command
+(stlink_t * sl, enum SCSI_Generic_Direction dir, uint32_t len) {
+    struct stlink_libusb * const slu = sl->backend_data;
+    unsigned char* const cmd = sl->c_buf;
+    int i = 0;
+    memset(cmd, 0, sizeof (sl->c_buf));
+    if(slu->protocoll == 1) {
+        cmd[i++] = 'U';
+        cmd[i++] = 'S';
+        cmd[i++] = 'B';
+        cmd[i++] = 'C';
+        write_uint32(&cmd[i], slu->sg_transfer_idx);
+        write_uint32(&cmd[i + 4], len);
+        i += 8;
+        cmd[i++] = (dir == SG_DXFER_FROM_DEV)?0x80:0;
+        cmd[i++] = 0; /* Logical unit */
+        cmd[i++] = 0xa; /* Command length */
+    }
+    return i;
 }
 
 void _stlink_usb_version(stlink_t *sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
     ssize_t size;
+    uint32_t rep_len = 6;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_GET_VERSION;
-    buf[1] = 0x80;
+    cmd[i++] = STLINK_GET_VERSION;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 6);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
     }
-
-#if 1 /* DEBUG */
-    {
-        ssize_t i;
-        for (i = 0; i < size; ++i) printf("%02x", buf[i]);
-        printf("\n");
-    }
-#endif /* DEBUG */
 }
 
 void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
-    unsigned char *cmd_buf = sl->c_buf;
-
-    memset(cmd_buf, 0, STLINK_CMD_SIZE);
-    cmd_buf[0] = STLINK_DEBUG_COMMAND;
-    cmd_buf[1] =  STLINK_DEBUG_WRITEMEM_32BIT;
-    write_uint32(cmd_buf + 2, addr);
-    write_uint16(cmd_buf + 6, len);
-    send_only(slu, cmd_buf, STLINK_CMD_SIZE);
-
-#if Q_BUF_LEN < UINT16_MAX
-    assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
-#endif
-    assert((len & 3) == 0); 
-    send_only(slu, buf, len);
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
+
+    int i = fill_command(sl, SG_DXFER_TO_DEV, len);
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_WRITEMEM_32BIT;
+    write_uint32(&cmd[i], addr);
+    write_uint16(&cmd[i + 4], len);
+    send_only(slu, 0, cmd, slu->cmd_len);
 
+    send_only(slu, 1, data, len);
 }
 
 void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
-    unsigned char *cmd_buf = sl->c_buf;
-
-    memset(cmd_buf, 0, STLINK_CMD_SIZE);
-    cmd_buf[0] = STLINK_DEBUG_COMMAND;
-    cmd_buf[1] =  STLINK_DEBUG_WRITEMEM_8BIT;
-    write_uint32(cmd_buf + 2, addr);
-    write_uint16(cmd_buf + 6, len);
-    send_only(slu, cmd_buf, STLINK_CMD_SIZE);
-
-#if Q_BUF_LEN < UINT16_MAX
-    assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
-#endif
-    send_only(slu, buf, len);
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
+
+    int i = fill_command(sl, SG_DXFER_TO_DEV, 0);
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
+    write_uint32(&cmd[i], addr);
+    write_uint16(&cmd[i + 4], len);
+    send_only(slu, 0, cmd, slu->cmd_len);
+    send_only(slu, 1, data, len);
 }
 
 
 int _stlink_usb_current_mode(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
+    unsigned char* const data = sl->q_buf;
     ssize_t size;
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_GET_CURRENT_MODE;
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
+    int rep_len = 2;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
+    
+    cmd[i++] = STLINK_GET_CURRENT_MODE;
+    size = send_recv(slu, 1, cmd,  slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return -1;
@@ -225,76 +252,71 @@ int _stlink_usb_current_mode(stlink_t * sl) {
 
 void _stlink_usb_core_id(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
+    unsigned char* const data = sl->q_buf;
     ssize_t size;
+    int rep_len = 4;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_READCOREID;
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_READCOREID;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 4);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
     }
 
-    sl->core_id = read_uint32(buf, 0);
+    sl->core_id = read_uint32(data, 0);
 }
 
 void _stlink_usb_status(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
     ssize_t size;
+    int rep_len = 2;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_GETSTATUS;
 
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_GETSTATUS;
-
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
     }
-
-    /* todo: stlink_core_stat */
-
-    // FIXME - decode into sl->core_stat
-#if 1 /* DEBUG */
-    printf("status == 0x%x\n", buf[0]);
-#endif /* DEBUG */
-
 }
 
 void _stlink_usb_force_debug(stlink_t *sl) {
     struct stlink_libusb *slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
     ssize_t size;
+    int rep_len = 2;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_FORCEDEBUG;
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
     }
 }
 
-
 void _stlink_usb_enter_swd_mode(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
     ssize_t size;
+    const int rep_len = 0;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_ENTER;
+    cmd[i++] = STLINK_DEBUG_ENTER_SWD;
 
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_SWD_ENTER;
-    buf[2] = STLINK_DEBUG_ENTER_SWD;
-
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
+    size = send_only(slu, 1, cmd, slu->cmd_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -303,14 +325,14 @@ void _stlink_usb_enter_swd_mode(stlink_t * sl) {
 
 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const cmd = sl->c_buf;
     ssize_t size;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_DFU_COMMAND;
-    buf[1] = STLINK_DFU_EXIT;
+    cmd[i++] = STLINK_DFU_COMMAND;
+    cmd[i++] = STLINK_DFU_EXIT;
 
-    size = send_only(slu, buf, 16);
+    size = send_only(slu, 1, cmd, slu->cmd_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -323,14 +345,16 @@ void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
  */
 void _stlink_usb_reset(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd = sl->c_buf;
     ssize_t size;
+    int rep_len = 2;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_RESETSYS;
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_RESETSYS;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -340,14 +364,16 @@ void _stlink_usb_reset(stlink_t * sl) {
 
 void _stlink_usb_step(stlink_t* sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd = sl->c_buf;
     ssize_t size;
+    int rep_len = 2;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_STEPCORE;
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_STEPCORE;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -360,31 +386,32 @@ void _stlink_usb_step(stlink_t* sl) {
  */
 void _stlink_usb_run(stlink_t* sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd = sl->c_buf;
     ssize_t size;
+    int rep_len = 2;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_RUNCORE;
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_RUNCORE;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
     }
-
 }
 
 void _stlink_usb_exit_debug_mode(stlink_t *sl) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const cmd = sl->c_buf;
     ssize_t size;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_EXIT;
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_EXIT;
 
-    size = send_only(slu, buf, 16);
+    size = send_only(slu, 1, cmd, slu->cmd_len);
     if (size == -1) {
         printf("[!] send_only\n");
         return;
@@ -393,21 +420,17 @@ void _stlink_usb_exit_debug_mode(stlink_t *sl) {
 
 void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd = sl->c_buf;
     ssize_t size;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, len);
 
-    /* assume len < sizeof(sl->q_buf) */
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
+    write_uint32(&cmd[i], addr);
+    write_uint16(&cmd[i + 4], len);
 
-    memset(buf, 0, sizeof (sl->q_buf));
-    buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = STLINK_DEBUG_READMEM_32BIT;
-    write_uint32(buf + 2, addr);
-    /* windows usb logs show only one byte is used for length ... */
-    // Presumably, this is because usb transfers can't be 16 bits worth of bytes long...
-    assert (len < 256);
-    buf[6] = (uint8_t) len;
-
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -418,20 +441,17 @@ void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     stlink_print_data(sl);
 }
 
-
-#if 1 /* stlinkv1 */
-
 void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
-    unsigned char* const cmd_buf = sl->c_buf;
+    unsigned char* const cmd = sl->c_buf;
+    unsigned char* const data = sl->q_buf;
     ssize_t size;
-    int i;
+    uint32_t rep_len = 84;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(cmd_buf, 0, STLINK_CMD_SIZE);
-    cmd_buf[0] = STLINK_DEBUG_COMMAND;
-    cmd_buf[1] = STLINK_DEBUG_READALLREGS;
-    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 84);
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_READALLREGS;
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -455,53 +475,19 @@ void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
     DD(sl, "rw2        = 0x%08x\n", read_uint32(sl->q_buf, 80));
 }
 
-#else /* stlinkv2 */
-
-static void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
-    struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
-    unsigned char* const cmd_buf = sl->c_buf;
-    ssize_t size;
-    int i;
-
-#define STLINK_JTAG_COMMAND 0xf2
-#define STLINK_JTAG_READALLREGS2 0x3a
-    memset(cmd_buf, 0, STLINK_CMD_SIZE);
-    cmd_buf[0] = STLINK_JTAG_COMMAND;
-    cmd_buf[1] = STLINK_JTAG_READALLREGS2;
-    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 84);
-
-    if (size == -1) {
-        printf("[!] send_recv\n");
-        return;
-    }
-
-    sl->q_len = (size_t) size;
-
-    for(i=0; i<16; i++)
-      regp->r[i]= read_uint32(sl->q_buf, i*4);
-
-    regp->xpsr       = read_uint32(sl->q_buf, 64);
-    regp->main_sp    = read_uint32(sl->q_buf, 68);
-    regp->process_sp = read_uint32(sl->q_buf, 72);
-    regp->rw         = read_uint32(sl->q_buf, 76);
-    regp->rw2        = read_uint32(sl->q_buf, 80);
-}
-
-#endif /* stlinkv1 */
-
 void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
-    unsigned char* const cmd_buf = sl->c_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
     ssize_t size;
     uint32_t r;
+    uint32_t rep_len = 4;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
-    memset(cmd_buf, 0, STLINK_CMD_SIZE);
-    cmd_buf[0] = STLINK_DEBUG_COMMAND;
-    cmd_buf[1] = STLINK_DEBUG_READREG;
-    cmd_buf[2] = (uint8_t) r_idx;
-    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 4);
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_READREG;
+    cmd[i++] = (uint8_t) r_idx;
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -522,31 +508,29 @@ void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
         regp->process_sp = r;
         break;
     case 19:
-        regp->rw = r; //XXX ?(primask, basemask etc.)
+        regp->rw = r; /* XXX ?(primask, basemask etc.) */
         break;
     case 20:
-        regp->rw2 = r; //XXX ?(primask, basemask etc.)
+        regp->rw2 = r; /* XXX ?(primask, basemask etc.) */
         break;
     default:
         regp->r[r_idx] = r;
     }
 }
 
-
-#if 1 /* stlinkv1 */
-
 void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
     struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
-    unsigned char *cmd_buf = sl->c_buf;
+    unsigned char* const data = sl->q_buf;
+    unsigned char* const cmd  = sl->c_buf;
     ssize_t size;
-
-    memset(cmd_buf, 0, STLINK_CMD_SIZE);
-    cmd_buf[0] = STLINK_DEBUG_COMMAND;
-    cmd_buf[1] = STLINK_DEBUG_WRITEREG;
-    cmd_buf[2] = idx;
-    write_uint32(cmd_buf + 3, reg);
-    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 2);
+    uint32_t rep_len = 2;
+    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
+
+    cmd[i++] = STLINK_DEBUG_COMMAND;
+    cmd[i++] = STLINK_DEBUG_WRITEREG;
+    cmd[i++] = idx;
+    write_uint32(&cmd[i], reg);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -555,32 +539,6 @@ void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
     stlink_print_data(sl);
 }
 
-#else /* stlinkv2 */
-
-void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
-    struct stlink_libusb * const slu = sl->backend_data;
-    unsigned char* const buf = sl->q_buf;
-    unsigned char *cmd_buf = sl->c_buf;
-    ssize_t size;
-
-#define STLINK_JTAG_WRITEREG2 0x34
-    memset(cmd_buf, 0, STLINK_CMD_SIZE);
-    cmd_buf[0] = STLINK_JTAG_COMMAND;
-    cmd_buf[1] = STLINK_JTAG_WRITEREG2;
-    cmd_buf[2] = idx;
-    write_uint32(cmd_buf + 3, reg);
-    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 2);
-    if (size == -1) {
-        printf("[!] send_recv\n");
-        return;
-    }
-    sl->q_len = (size_t) size;
-    stlink_print_data(sl);
-}
-
-#endif /* stlinkv1 */
-
-
 stlink_backend_t _stlink_usb_backend = {
     _stlink_usb_close,
     _stlink_usb_exit_debug_mode,
@@ -604,38 +562,48 @@ stlink_backend_t _stlink_usb_backend = {
 };
 
 
-stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
+stlink_t* stlink_open_usb(const int verbose) {
     stlink_t* sl = NULL;
     struct stlink_libusb* slu = NULL;
-
-    /* unused */
-    dev_name = dev_name;
+    int error = -1;
+    libusb_device** devs = NULL;
+    libusb_device* dev;
+    ssize_t i;
+    ssize_t count;
+    int config;
+    char *iSerial = NULL;
 
     sl = malloc(sizeof (stlink_t));
     slu = malloc(sizeof (struct stlink_libusb));
     if (sl == NULL) goto on_error;
     if (slu == NULL) goto on_error;
+    memset(sl, 0, sizeof (stlink_t));
+    memset(slu, 0, sizeof (struct stlink_libusb));
 
     sl->verbose = verbose;
+    sl->backend = &_stlink_usb_backend;
+    sl->backend_data = slu;
     
-    if (slu->libusb_ctx != NULL) {
-        fprintf(stderr, "reopening with an existing context? undefined behaviour!\n");
-        goto on_error;
-    } else {
-        if (libusb_init(&(slu->libusb_ctx))) {
-            fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
-            goto on_error;
-        }
-    }
+    sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 
-    int error = -1;
+    /* flash memory settings */
+    sl->flash_base = STM32_FLASH_BASE;
+    sl->flash_size = STM32_FLASH_SIZE;
+    sl->flash_pgsz = STM32_FLASH_PGSZ;
 
-    libusb_device** devs = NULL;
-    libusb_device* dev;
-    ssize_t i;
-    ssize_t count;
-    int config;
+    /* system memory */
+    sl->sys_base = STM32_SYSTEM_BASE;
+    sl->sys_size = STM32_SYSTEM_SIZE;
+
+    /* sram memory settings */
+    sl->sram_base = STM32_SRAM_BASE;
+    sl->sram_size = STM32L_SRAM_SIZE;
 
+    if (libusb_init(&(slu->libusb_ctx))) {
+	fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
+	goto on_error;
+    }
+    
     count = libusb_get_device_list(slu->libusb_ctx, &devs);
     if (count < 0) {
         printf("libusb_get_device_list\n");
@@ -644,14 +612,47 @@ stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
 
     for (i = 0; i < count; ++i) {
         dev = devs[i];
-        if (is_stlink_device(dev)) break;
+        slu->protocoll = is_stlink_device(dev, 0);
+        if (slu->protocoll > 0) break;
     }
-    if (i == count) return NULL;
+    if (i == count) goto on_libusb_error;
 
     if (libusb_open(dev, &(slu->usb_handle))) {
         printf("libusb_open()\n");
         goto on_libusb_error;
     }
+    
+    if (iSerial) {
+        unsigned char serial[256];
+        struct libusb_device_descriptor desc;
+        int r;
+
+        r = libusb_get_device_descriptor(dev, &desc);
+        if (r<0) {
+            printf("Can't get descriptor to match Iserial\n");
+            goto on_libusb_error;
+        }
+        r = libusb_get_string_descriptor_ascii
+            (slu->usb_handle, desc.iSerialNumber, serial, 256);
+        if (r<0) {
+            printf("Can't get Serialnumber to match Iserial\n");
+            goto on_libusb_error;
+        }
+        if (strcmp((char*)serial, iSerial)) {
+            printf("Mismatch in serial numbers, dev %s vs given %s\n",
+                   serial, iSerial);
+            goto on_libusb_error;
+        }
+    }
+
+    if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
+        int r;
+        
+        r = libusb_detach_kernel_driver(slu->usb_handle, 0);
+        if (r<0)
+            printf("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
+        goto on_libusb_error;
+    }
 
     if (libusb_get_configuration(slu->usb_handle, &config)) {
         /* this may fail for a previous configured device */
@@ -688,15 +689,20 @@ stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
     slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
     slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 
-    /* libusb_reset_device(slu->usb_handle); */
+    slu->sg_transfer_idx = 0;
+    slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE;
 
     /* success */
+    if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
+      printf("-- exit_dfu_mode\n");
+      stlink_exit_dfu_mode(sl);
+    }
+    stlink_version(sl);
     error = 0;
 
 on_libusb_error:
     if (devs != NULL) {
         libusb_free_device_list(devs, 1);
-        fprintf(stderr, "freed libusb device list\n");
     }
 
     if (error == -1) {
@@ -704,8 +710,6 @@ on_libusb_error:
         return NULL;
     }
 
-    sl->backend = &_stlink_usb_backend;
-    sl->backend_data = slu;
     /* success */
     return sl;
 
diff --git a/src/stlink-usb.h b/src/stlink-usb.h
index 4ed655b..c632a8c 100644
--- a/src/stlink-usb.h
+++ b/src/stlink-usb.h
@@ -15,6 +15,7 @@ extern "C" {
 #include <libusb-1.0/libusb.h>
 #include "stlink-common.h"
     
+#define STLINK_SG_SIZE 31
 #define STLINK_CMD_SIZE 16
 
 #if defined(CONFIG_USE_LIBUSB)
@@ -25,6 +26,9 @@ extern "C" {
         struct libusb_transfer* rep_trans;
         unsigned int ep_req;
         unsigned int ep_rep;
+        int protocoll;
+        unsigned int sg_transfer_idx;
+        unsigned int cmd_len;
     };
 #else
 #error "it's all bad!"
@@ -32,7 +36,7 @@ extern "C" {
 #endif
 
     
-    stlink_t* stlink_open_usb(const char *dev_name, const int verbose);
+    stlink_t* stlink_open_usb(const int verbose);
 
 
 #ifdef	__cplusplus
diff --git a/src/test_sg.c b/src/test_sg.c
index 137eca4..34fbe54 100644
--- a/src/test_sg.c
+++ b/src/test_sg.c
@@ -6,8 +6,10 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#if CONFIG_USE_LIBSG
 #include <scsi/sg_lib.h>
 #include <scsi/sg_pt.h>
+#endif
 #include "stlink-common.h"
 
 int main(int argc, char *argv[]) {
@@ -210,4 +212,4 @@ int main(int argc, char *argv[]) {
 
 	//fflush(stderr); fflush(stdout);
 	return EXIT_SUCCESS;
-}
\ No newline at end of file
+}
diff --git a/src/test_usb.c b/src/test_usb.c
index bd81ec1..343e355 100644
--- a/src/test_usb.c
+++ b/src/test_usb.c
@@ -10,7 +10,7 @@ int main(int ac, char** av) {
     ac = ac;
     av = av;
 
-    sl = stlink_open_usb(NULL, 10);
+    sl = stlink_open_usb(10);
     if (sl != NULL) {
         printf("-- version\n");
         stlink_version(sl);