diff --git a/Makefile b/Makefile
index 8499dc1db7486f8a13be612e477d253d48b3418f..2aef96b8b25cc361a0faa761004b3dd138c45b44 100755
--- a/Makefile
+++ b/Makefile
@@ -2,23 +2,31 @@
 # modified by zerom for WinARM 8/2010
 
 PROJECT_NAME=bioloid_firmware
-TARGET_FILES=$(wildcard src/*.c)
+#TARGET_FILES=$(wildcard src/*.c)
+TARGET_FILES=src/bioloid_stm32.c
+TARGET_FILES+=src/system_stm32f4xx.c
+TARGET_FILES+=src/stm32f4xx_hal_msp.c
+TARGET_FILES+=src/gpio.c
 TARGET_PROCESSOR=STM32F407VG
 
-include ../../STM32_processor/libraries/f4/select_processor.mk
+HAL_PATH=../../STM32_processor/hal/f4
 
-STM32_LIB_PATH = ../../STM32_processor/libraries/f4/stm32_lib
-STM32_DSP_LIB_PATH = ../../STM32_processor/libraries/f4/stm32_dsp_lib
-DYNAMIXEL_LIB_PATH = ../../STM32_processor/libraries
-STM32_STARTUP_FILES_PATH = ../../STM32_processor/startup/f4
-STM32_LINKER_SCRIPTS_PATH = ../../STM32_processor/libraries/f4/linker_scripts
+include $(HAL_PATH)/select_processor.mk
+
+STM32_STARTUP_FILES_PATH = $(HAL_PATH)/startup_code/
+STM32_LINKER_SCRIPTS_PATH = $(HAL_PATH)/linker_scripts
+UTILS_PATH=../../STM32_processor/libraries/utils
+COMM_PATH=../../STM32_processor/libraries/comm
+USART_PATH=../../STM32_processor/libraries/f4/usart
+DYNAMIXEL_PATH=../../STM32_processor/libraries/dynamixel_base
 BUILD_PATH=build
 
 COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m4 -mthumb -mthumb-interwork 
-COMPILE_OPTS += -Wall -g -O2 -fno-common 
+COMPILE_OPTS += -Wall -O2 -fno-common 
 COMPILE_OPTS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
 COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DARM_MATH_CM4 -D__FPU_PRESENT -DHSE_VALUE=8000000
-INCLUDE_DIRS = -I$(STM32_LIB_PATH)/include -I$(STM32_LIB_PATH)/include/core -I$(STM32_LIB_PATH)/include/devices -I$(STM32_DSP_LIB_PATH)/include -I${DYNAMIXEL_LIB_PATH}/include/dynamixel -I./include
+INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices
+INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include
 LIBRARY_DIRS = -L$(STM32_LIB_PATH)/lib -L$(STM32_DSP_LIB_PATH)/lib -L$(DYNAMIXEL_LIB_PATH)/lib -L/usr/arm-none-eabi/lib/fpu/
 
 TCHAIN_PREFIX=arm-none-eabi-
@@ -30,8 +38,8 @@ AS = $(TCHAIN_PREFIX)gcc
 ASFLAGS = $(COMPILE_OPTS) -c 
 
 LD = $(TCHAIN_PREFIX)gcc
-#LDFLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) $(LIBRARY_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/$(LINKER_SCRIPT_FILE) -nostartfiles 
-LDFLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) $(LIBRARY_DIRS) -T linker_script/STM32F4_1024FLASH_192RAM.ld -nostartfiles 
+LDFLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/$(LINKER_SCRIPT_FILE) --specs=nosys.specs
+EXT_LIB = $(COMM_PATH)/lib/comm_m4_fpu.a $(UTILS_PATH)/lib/utils_m4_fpu.a $(DYNAMIXEL_PATH)/lib/dynamixel_m4_fpu.a
 
 OBJCP = $(TCHAIN_PREFIX)objcopy
 OBJCPFLAGS_HEX = -O ihex
@@ -44,6 +52,19 @@ MAIN_OUT_HEX = $(BUILD_PATH)/$(PROJECT_NAME).hex
 MAIN_OUT_BIN = $(BUILD_PATH)/$(PROJECT_NAME).bin
 MAIN_OUT_LSS = $(BUILD_PATH)/$(PROJECT_NAME).lss
 
+# add STM32 src files
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_gpio.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_cortex.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_rcc.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_rcc_ex.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_pwr.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_tim.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_tim_ex.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_flash.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_flash_ex.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_dma.c
+TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal.c
+
 BIOLOID_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
 BIOLOID_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(BIOLOID_OBJS_TMP))
 
@@ -52,11 +73,17 @@ all: $(MAIN_OUT_ELF) $(MAIN_OUT_HEX) $(MAIN_OUT_BIN) $(MAIN_OUT_LSS)
 mkdir_build: 
 	mkdir -p $(BUILD_PATH)
 
-$(BUILD_PATH)/%.o: src/%.c 
+$(BUILD_PATH)/%.o: src/%.c
+	$(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(HAL_PATH)/src/devices/%.c
+	$(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(HAL_PATH)/src/%.c
+	$(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
 	$(CC) -c $(CFLAGS) -o $@ $<
 
-$(MAIN_OUT_ELF): mkdir_build $(BIOLOID_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_LIB_PATH)/lib/$(LIBRARY) 
-	$(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_LIB_PATH)/lib/$(LIBRARY) $(STM32_DSP_LIB_PATH)/lib/libstm32f4_dsp.a ${DYNAMIXEL_LIB_PATH}/lib/dynamixel_hard.a -lm --output $@
+$(MAIN_OUT_ELF): mkdir_build $(BIOLOID_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) 
+	$(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) --output $@
 
 $(MAIN_OUT_HEX): $(MAIN_OUT_ELF)	
 	$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
@@ -69,6 +96,7 @@ $(MAIN_OUT_LSS): $(MAIN_OUT_ELF)
 
 $(BUID_PATH)/$(STARTUP_FILE:.s=.o): $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE)
 	$(AS) $(ASFLAGS) -o $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE) > $(BUILD_PATH)/$(STARTUP_FILE:.s=.lst)
+
 clean:
 	-rm -rf $(BUILD_PATH)
 
diff --git a/include/gpio.h b/include/gpio.h
index 8fea176627702e56e994c46522bdd37bbf40c203..544b7278aec1fa6bec869ce5621cf6639f3e5cd6 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -1,11 +1,11 @@
 #ifndef _GPIO_H
 #define _GPIO_H
 
-#include "stm32f4xx.h"
+#include "stm32f4xx_hal.h"
 
-typedef enum {NORTH_LED,SOUTH_LED,EAST_LED,WEST_LED} led_t;
+typedef enum {USER1_LED,USER2_LED,RXD_LED,TXD_LED} led_t;
 
-typedef enum {NORTH_PB,SOUTH_PB,EAST_PB,WEST_PB} pushbutton_t;
+typedef enum {USER_PB,RESET_PB,START_PB,MODE_PB} pushbutton_t;
 
 void gpio_init(void);
 // LED functions
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 950f0871aca7020bf086d4028314096b9b16763a..db35b7b43f5a51feb5bed8f55aff51eee2915840 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -1,60 +1,10 @@
-#include "stm32f4xx.h"
-#include "system_stm32f4xx.h"
-
-#include "stm32_time.h"
-#include "dynamixel.h"
-#include "dynamixel_master_uart_dma.h"
-#include "dynamixel_slave_uart_dma.h"
-#include "eeprom.h"
-#include "imu_9dof_dma.h"
+#include "stm32f4xx_hal.h"
 #include "gpio.h"
-#include "ram.h"
-#include "adc_dma.h"
-#include "motion_pages.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "comm.h"
 
 int32_t main(void)
 {
-  uint16_t eeprom_data,action_period,mm_period;
-
-  /* initialize EEPROM */
-  EE_Init();
-  // initialize the Dynamixel RAM memory space
-  ram_init();
-  /* initialize the 1ms system timer */
-  time_init();
   /* initialize the gpio */
   gpio_init();
-  /* initialize the dynamixel master interface */
-  dyn_master_init();  
-  dyn_master_set_timeout(20);
-  /* initialize the dynamixel slave interface*/
-  dyn_slave_init();
-  EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data);
-  dyn_slave_set_address((uint8_t)eeprom_data);
-  EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data);
-  dyn_slave_set_return_delay((uint8_t)eeprom_data);
-  EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
-  dyn_slave_set_return_level((uint8_t)eeprom_data);
-  /* initialize the IMU */
-//  imu_init();
-  // initialize the Analog to digital converter
-//  adc_init();
-  // initialize motion manager
-//  EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
-//  action_period=eeprom_data&0x00FF;
-//  EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
-//  action_period+=((eeprom_data&0x00FF)<<8);
-//  mm_period=(action_period*1000000)>>16;
-//  manager_init(mm_period);
-  // initialize the action module
-//  action_init(action_period);
-  comm_init();
-  comm_start();
-
-  gpio_blink_led(NORTH_LED,1000);
 
   while(1);
 }
diff --git a/src/gpio.c b/src/gpio.c
index 55969faa30bc120f7eebaadcd0f4759331b58dbf..78f8e747460320c2365ceb5f9769c37e74e11b21 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -1,153 +1,69 @@
 #include "gpio.h"
 #include "ram.h"
 
-#define LED1_GPIO_CLK             RCC_AHB1Periph_GPIOE
-#define LED1_PIN                  GPIO_Pin_2                
-#define LED1_GPIO_PORT            GPIOE                       
-#define LED1_SOURCE               GPIO_PinSource2
-
-#define LED2_GPIO_CLK             RCC_AHB1Periph_GPIOE
-#define LED2_PIN                  GPIO_Pin_3                
-#define LED2_GPIO_PORT            GPIOE                       
-#define LED2_SOURCE               GPIO_PinSource3
-
-#define LED3_GPIO_CLK             RCC_AHB1Periph_GPIOE
-#define LED3_PIN                  GPIO_Pin_4            
-#define LED3_GPIO_PORT            GPIOE                       
-#define LED3_SOURCE               GPIO_PinSource4
-
-#define LED4_GPIO_CLK             RCC_AHB1Periph_GPIOE
-#define LED4_PIN                  GPIO_Pin_5            
-#define LED4_GPIO_PORT            GPIOE                       
-#define LED4_SOURCE               GPIO_PinSource5
-
-#define PUSH_BUTTON1_GPIO_CLK     RCC_AHB1Periph_GPIOE
-#define PUSH_BUTTON1_PIN          GPIO_Pin_6                
-#define PUSH_BUTTON1_GPIO_PORT    GPIOE                       
-#define PUSH_BUTTON1_SOURCE       GPIO_PinSource6
-#define PUSH_BUTTON1_EXTI_PORT    EXTI_PortSourceGPIOE
-#define PUSH_BUTTON1_EXTI_PIN     EXTI_PinSource6
-#define PUSH_BUTTON1_EXTI_LINE    EXTI_Line6
-
-#define PUSH_BUTTON2_GPIO_CLK     RCC_AHB1Periph_GPIOC
-#define PUSH_BUTTON2_PIN          GPIO_Pin_13                
-#define PUSH_BUTTON2_GPIO_PORT    GPIOC                       
-#define PUSH_BUTTON2_SOURCE       GPIO_PinSource13
-#define PUSH_BUTTON2_EXTI_PORT    EXTI_PortSourceGPIOC
-#define PUSH_BUTTON2_EXTI_PIN     EXTI_PinSource13
-#define PUSH_BUTTON2_EXTI_LINE    EXTI_Line13
-
-#define PUSH_BUTTON3_GPIO_CLK     RCC_AHB1Periph_GPIOC
-#define PUSH_BUTTON3_PIN          GPIO_Pin_14                
-#define PUSH_BUTTON3_GPIO_PORT    GPIOC                       
-#define PUSH_BUTTON3_SOURCE       GPIO_PinSource14
-#define PUSH_BUTTON3_EXTI_PORT    EXTI_PortSourceGPIOC
-#define PUSH_BUTTON3_EXTI_PIN     EXTI_PinSource14
-#define PUSH_BUTTON3_EXTI_LINE    EXTI_Line14
-
-#define PUSH_BUTTON4_GPIO_CLK     RCC_AHB1Periph_GPIOC
-#define PUSH_BUTTON4_PIN          GPIO_Pin_15
-#define PUSH_BUTTON4_GPIO_PORT    GPIOC                       
-#define PUSH_BUTTON4_SOURCE       GPIO_PinSource15
-#define PUSH_BUTTON4_EXTI_PORT    EXTI_PortSourceGPIOC
-#define PUSH_BUTTON4_EXTI_PIN     EXTI_PinSource15
-#define PUSH_BUTTON4_EXTI_LINE    EXTI_Line15
-
-#define GPO_TIMER                 TIM2
-#define GPO_TIMER_CLK             RCC_APB1Periph_TIM2
-#define GPO_TIMER_IRQn            TIM2_IRQn
-#define GPO_TIMER_IRQHandler      TIM2_IRQHandler
-
-#define GPI_EXTI1_IRQn            EXTI9_5_IRQn
-#define GPI_EXTI1_IRQHandler      EXTI9_5_IRQHandler
-#define GPI_EXTI2_IRQn            EXTI15_10_IRQn
-#define GPI_EXTI2_IRQHandler      EXTI15_10_IRQHandler
+#define ENABLE_LED_TX_GPIO_CLK      __HAL_RCC_GPIOE_CLK_ENABLE()
+#define LED_TX_PIN                  GPIO_PIN_10
+#define LED_TX_GPIO_PORT            GPIOE
+
+#define ENABLE_LED_RX_GPIO_CLK      __HAL_RCC_GPIOE_CLK_ENABLE()
+#define LED_RX_PIN                  GPIO_PIN_2
+#define LED_RX_GPIO_PORT            GPIOE
+
+#define ENABLE_LED_USR1_GPIO_CLK    __HAL_RCC_GPIOA_CLK_ENABLE()
+#define LED_USR1_PIN                GPIO_PIN_13
+#define LED_USR1_GPIO_PORT          GPIOA
+
+#define ENABLE_LED_USR2_GPIO_CLK    __HAL_RCC_GPIOE_CLK_ENABLE()
+#define LED_USR2_PIN                GPIO_PIN_9                
+#define LED_USR2_GPIO_PORT          GPIOE
+
+#define ENABLE_START_PB_GPIO_CLK    __HAL_RCC_GPIOE_CLK_ENABLE()
+#define START_PB_PIN                GPIO_PIN_3
+#define START_PB_GPIO_PORT          GPIOE
+                       
+#define ENABLE_MODE_PB_GPIO_CLK     __HAL_RCC_GPIOE_CLK_ENABLE()
+#define MODE_PB_PIN                 GPIO_PIN_4
+#define MODE_PB_GPIO_PORT           GPIOE
+                       
+#define ENABLE_RESET_PB_GPIO_CLK    __HAL_RCC_GPIOA_CLK_ENABLE()
+#define RESET_PB_PIN                GPIO_PIN_12
+#define RESET_PB_GPIO_PORT          GPIOA
+                       
+#define ENABLE_USER_PB_GPIO_CLK     __HAL_RCC_GPIOA_CLK_ENABLE()
+#define USER_PB_PIN                 GPIO_PIN_11
+#define USER_PB_GPIO_PORT           GPIOA
+
+#define GPI_EXTI1_IRQn              EXTI15_10_IRQn
+#define GPI_EXTI1_IRQHandler        EXTI15_10_IRQHandler
+
+#define GPI_EXTI2_IRQn              EXTI4_IRQn
+#define GPI_EXTI2_IRQHandler        EXTI3_IRQHandler
+
+#define GPI_EXTI3_IRQn              EXTI4_IRQn
+#define GPI_EXTI3_IRQHandler        EXTI4_IRQHandler
+
+#define GPO_TIMER                   TIM2
+#define ENABLE_GPO_TIMER_CLK        __HAL_RCC_TIM2_CLK_ENABLE()
+#define GPO_TIMER_IRQn              TIM2_IRQn
+#define GPO_TIMER_IRQHandler        TIM2_IRQHandler
 
 // private variables
-// LED blink periods
-__IO uint16_t north_led_period;
-__IO uint16_t south_led_period;
-__IO uint16_t east_led_period;
-__IO uint16_t west_led_period;
-// Pushbuttons callbacks
-void (*north_pb_callback)(void);
-void (*south_pb_callback)(void);
-void (*east_pb_callback)(void);
-void (*west_pb_callback)(void);
 
 // IRQ handler functions
 void GPI_EXTI1_IRQHandler(void)
 {
-  if(EXTI_GetITStatus(PUSH_BUTTON1_EXTI_LINE) != RESET)
-  {
-    if(north_pb_callback!=0)  
-      north_pb_callback();
-    ram_set_bit(BIOLOID_PUSHBUTTON,0);
-    /* Clear the EXTI line 0 pending bit */
-    EXTI_ClearITPendingBit(PUSH_BUTTON1_EXTI_LINE);
-  }
 }
 
 void GPI_EXTI2_IRQHandler(void)
 {
-  if(EXTI_GetITStatus(PUSH_BUTTON2_EXTI_LINE) != RESET)
-  {
-    if(south_pb_callback!=0)  
-      south_pb_callback();
-    ram_set_bit(BIOLOID_PUSHBUTTON,1);
-    /* Clear the EXTI line 0 pending bit */
-    EXTI_ClearITPendingBit(PUSH_BUTTON2_EXTI_LINE);
-  }
-  if(EXTI_GetITStatus(PUSH_BUTTON3_EXTI_LINE) != RESET)
-  {
-    if(east_pb_callback!=0)  
-      east_pb_callback();
-    ram_set_bit(BIOLOID_PUSHBUTTON,2);
-    /* Clear the EXTI line 0 pending bit */
-    EXTI_ClearITPendingBit(PUSH_BUTTON3_EXTI_LINE);
-  }
-  if(EXTI_GetITStatus(PUSH_BUTTON4_EXTI_LINE) != RESET)
-  {
-    if(west_pb_callback!=0)  
-      west_pb_callback();
-    ram_set_bit(BIOLOID_PUSHBUTTON,3);
-    /* Clear the EXTI line 0 pending bit */
-    EXTI_ClearITPendingBit(PUSH_BUTTON4_EXTI_LINE);
-  }
 }
 
-void GPO_TIMER_IRQHandler(void)
+void GPI_EXTI3_IRQHandler(void)
 {
-  uint16_t capture;
+}
 
-  if(TIM_GetITStatus(GPO_TIMER, TIM_IT_CC1)!=RESET)
-  {
-    TIM_ClearITPendingBit(GPO_TIMER,TIM_IT_CC1);
-    GPIO_ToggleBits(LED1_GPIO_PORT,LED1_PIN);
-    capture = TIM_GetCapture1(GPO_TIMER);
-    TIM_SetCompare1(GPO_TIMER, capture + north_led_period);
-  }
-  if(TIM_GetITStatus(GPO_TIMER, TIM_IT_CC2)!=RESET)
-  {
-    TIM_ClearITPendingBit(GPO_TIMER,TIM_IT_CC2);
-    GPIO_ToggleBits(LED2_GPIO_PORT,LED2_PIN);
-    capture = TIM_GetCapture2(GPO_TIMER);
-    TIM_SetCompare2(GPO_TIMER, capture + south_led_period);
-  }
-  if(TIM_GetITStatus(GPO_TIMER, TIM_IT_CC3)!=RESET)
-  {
-    TIM_ClearITPendingBit(GPO_TIMER,TIM_IT_CC3);
-    GPIO_ToggleBits(LED3_GPIO_PORT,LED3_PIN);
-    capture = TIM_GetCapture3(GPO_TIMER);
-    TIM_SetCompare3(GPO_TIMER, capture + east_led_period);
-  }
-  if(TIM_GetITStatus(GPO_TIMER, TIM_IT_CC4)!=RESET)
-  {
-    TIM_ClearITPendingBit(GPO_TIMER,TIM_IT_CC4);
-    GPIO_ToggleBits(LED4_GPIO_PORT,LED4_PIN);
-    capture = TIM_GetCapture4(GPO_TIMER);
-    TIM_SetCompare4(GPO_TIMER, capture + west_led_period);
-  }
+void GPO_TIMER_IRQHandler(void)
+{
 }
 
 // private functions
@@ -155,277 +71,28 @@ void GPO_TIMER_IRQHandler(void)
 // public functions
 void gpio_init(void)
 {
-  GPIO_InitTypeDef GPIO_InitStructure;
-  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-  NVIC_InitTypeDef NVIC_InitStructure;
-  EXTI_InitTypeDef EXTI_InitStructure;
-
-  /* enable clocks */
-  RCC_AHB1PeriphClockCmd(LED1_GPIO_CLK | LED2_GPIO_CLK | LED3_GPIO_CLK | LED4_GPIO_CLK, ENABLE);
-  RCC_AHB1PeriphClockCmd(PUSH_BUTTON1_GPIO_CLK | PUSH_BUTTON2_GPIO_CLK | PUSH_BUTTON3_GPIO_CLK | PUSH_BUTTON4_GPIO_CLK, ENABLE);
-  RCC_APB1PeriphClockCmd(GPO_TIMER_CLK,ENABLE);
-  RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
-
-  /* GPIO Configuration */
-  GPIO_InitStructure.GPIO_Pin = LED1_PIN;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
-  GPIO_Init(LED1_GPIO_PORT, &GPIO_InitStructure);
-     
-  GPIO_InitStructure.GPIO_Pin = LED2_PIN;
-  GPIO_Init(LED2_GPIO_PORT, &GPIO_InitStructure);
-
-  GPIO_InitStructure.GPIO_Pin = LED3_PIN;
-  GPIO_Init(LED3_GPIO_PORT, &GPIO_InitStructure);
-
-  GPIO_InitStructure.GPIO_Pin = LED4_PIN;
-  GPIO_Init(LED4_GPIO_PORT, &GPIO_InitStructure);
-
-  GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON1_PIN;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
-  GPIO_Init(PUSH_BUTTON1_GPIO_PORT, &GPIO_InitStructure);
-
-  GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON2_PIN;
-  GPIO_Init(PUSH_BUTTON2_GPIO_PORT, &GPIO_InitStructure);
-
-  GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON3_PIN;
-  GPIO_Init(PUSH_BUTTON3_GPIO_PORT, &GPIO_InitStructure);
-
-  GPIO_InitStructure.GPIO_Pin = PUSH_BUTTON4_PIN;
-  GPIO_Init(PUSH_BUTTON4_GPIO_PORT, &GPIO_InitStructure);
-
-  // initialize private variables
-  north_led_period=0;
-  south_led_period=0;
-  east_led_period=0;
-  west_led_period=0;
-  north_pb_callback=0;
-  south_pb_callback=0;
-  east_pb_callback=0;
-  west_pb_callback=0;
-
-  // initialize the timer interrupts
-  NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
-  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-  NVIC_Init(&NVIC_InitStructure);
-
-  /* LED's timer configuration */
-  TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
-  TIM_TimeBaseStructure.TIM_Prescaler = 0;
-  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-  TIM_TimeBaseInit(GPO_TIMER,&TIM_TimeBaseStructure);
-  TIM_Cmd(GPO_TIMER, ENABLE);
-  TIM_PrescalerConfig(GPO_TIMER, 42000, TIM_PSCReloadMode_Immediate);
-  TIM_SetClockDivision(GPO_TIMER,TIM_CKD_DIV2);
-
-  /* Connect external interrupts */
-  SYSCFG_EXTILineConfig(PUSH_BUTTON1_EXTI_PORT,PUSH_BUTTON1_EXTI_PIN);
-  SYSCFG_EXTILineConfig(PUSH_BUTTON2_EXTI_PORT,PUSH_BUTTON2_EXTI_PIN);
-  SYSCFG_EXTILineConfig(PUSH_BUTTON3_EXTI_PORT,PUSH_BUTTON3_EXTI_PIN);
-  SYSCFG_EXTILineConfig(PUSH_BUTTON4_EXTI_PORT,PUSH_BUTTON4_EXTI_PIN);
-
-  /* configure external interrupts */
-  EXTI_InitStructure.EXTI_Line = PUSH_BUTTON1_EXTI_LINE | PUSH_BUTTON2_EXTI_LINE | PUSH_BUTTON3_EXTI_LINE | PUSH_BUTTON4_EXTI_LINE;
-  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
-  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
-  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
-  EXTI_Init(&EXTI_InitStructure);
-
-  NVIC_InitStructure.NVIC_IRQChannel = GPI_EXTI1_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
-  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-  NVIC_Init(&NVIC_InitStructure);
-
-  NVIC_InitStructure.NVIC_IRQChannel = GPI_EXTI2_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
-  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-  NVIC_Init(&NVIC_InitStructure);
 }
 
 void gpio_set_led(led_t led_id)
 {
-  switch(led_id)
-  {
-    case NORTH_LED:
-      GPIO_SetBits(LED1_GPIO_PORT,LED1_PIN);
-      break;
-    case SOUTH_LED:
-      GPIO_SetBits(LED2_GPIO_PORT,LED2_PIN);
-      break;
-    case EAST_LED:
-      GPIO_SetBits(LED3_GPIO_PORT,LED3_PIN);
-      break;
-    case WEST_LED:
-      GPIO_SetBits(LED4_GPIO_PORT,LED4_PIN);
-      break;
-  }
 }
 
 void gpio_clear_led(led_t led_id)
 {
-  switch(led_id)
-  {
-    case NORTH_LED:
-      GPIO_ResetBits(LED1_GPIO_PORT,LED1_PIN);
-      break;
-    case SOUTH_LED:
-      GPIO_ResetBits(LED2_GPIO_PORT,LED2_PIN);
-      break;
-    case EAST_LED:
-      GPIO_ResetBits(LED3_GPIO_PORT,LED3_PIN);
-      break;
-    case WEST_LED:
-      GPIO_ResetBits(LED4_GPIO_PORT,LED4_PIN);
-      break;
-  }
 }
 
 void gpio_toggle_led(led_t led_id)
 {
-  switch(led_id)
-  {
-    case NORTH_LED:
-      GPIO_ToggleBits(LED1_GPIO_PORT,LED1_PIN);
-      break;
-    case SOUTH_LED:
-      GPIO_ToggleBits(LED2_GPIO_PORT,LED2_PIN);
-      break;
-    case EAST_LED:
-      GPIO_ToggleBits(LED3_GPIO_PORT,LED3_PIN);
-      break;
-    case WEST_LED:
-      GPIO_ToggleBits(LED4_GPIO_PORT,LED4_PIN);
-      break;
-  }
 }
 
 void gpio_blink_led(led_t led_id, int16_t period_ms)
 {
-  TIM_OCInitTypeDef  TIM_OCInitStructure;
-  uint16_t capture;
-
-  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
-  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
-  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
-  switch(led_id)
-  {
-    case NORTH_LED:
-      if(period_ms>1)
-      {
-        north_led_period=period_ms;
-        TIM_OCInitStructure.TIM_Pulse = north_led_period;
-        TIM_OC1Init(GPO_TIMER, &TIM_OCInitStructure);
-        TIM_OC1PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
-        capture = TIM_GetCounter(GPO_TIMER);
-        TIM_SetCompare1(GPO_TIMER, capture + north_led_period);
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC1, ENABLE);
-      }
-      else
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC1, DISABLE);
-      break;
-    case SOUTH_LED:
-      if(period_ms>1)
-      {
-        south_led_period=period_ms;
-        TIM_OCInitStructure.TIM_Pulse = south_led_period;
-        TIM_OC2Init(GPO_TIMER, &TIM_OCInitStructure);
-        TIM_OC2PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
-        capture = TIM_GetCounter(GPO_TIMER);
-        TIM_SetCompare2(GPO_TIMER, capture + south_led_period);
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC2, ENABLE);
-      }
-      else
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC2, DISABLE);
-      break;
-    case EAST_LED:
-      if(period_ms>1)
-      {
-        east_led_period=period_ms;
-        TIM_OCInitStructure.TIM_Pulse = east_led_period;
-        TIM_OC3Init(GPO_TIMER, &TIM_OCInitStructure);
-        TIM_OC3PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
-        capture = TIM_GetCounter(GPO_TIMER);
-        TIM_SetCompare3(GPO_TIMER, capture + east_led_period);
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC3, ENABLE);
-      }
-      else
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC3, DISABLE);
-      break;
-    case WEST_LED:
-      if(period_ms>1)
-      {
-        west_led_period=period_ms;
-        TIM_OCInitStructure.TIM_Pulse = west_led_period;
-        TIM_OC4Init(GPO_TIMER, &TIM_OCInitStructure);
-        TIM_OC4PreloadConfig(GPO_TIMER, TIM_OCPreload_Disable);
-        capture = TIM_GetCounter(GPO_TIMER);
-        TIM_SetCompare4(GPO_TIMER, capture + west_led_period);
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC4, ENABLE);
-      }
-      else
-        TIM_ITConfig(GPO_TIMER, TIM_IT_CC4, DISABLE);
-      break;
-  }
 }
 
 uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id)
 {
-  switch(pb_id)
-  {
-    case NORTH_PB:
-      if(GPIO_ReadInputDataBit(PUSH_BUTTON1_GPIO_PORT,PUSH_BUTTON1_PIN)==Bit_SET)
-        return 0x01;
-      else
-        return 0x00;
-      break;
-    case SOUTH_PB:
-      if(GPIO_ReadInputDataBit(PUSH_BUTTON2_GPIO_PORT,PUSH_BUTTON2_PIN)==Bit_SET)
-        return 0x01;
-      else
-        return 0x00;
-      break;
-    case EAST_PB:
-      if(GPIO_ReadInputDataBit(PUSH_BUTTON3_GPIO_PORT,PUSH_BUTTON3_PIN)==Bit_SET)
-        return 0x01;
-      else
-        return 0x00;
-      break;
-    case WEST_PB:
-      if(GPIO_ReadInputDataBit(PUSH_BUTTON4_GPIO_PORT,PUSH_BUTTON4_PIN)==Bit_SET)
-        return 0x01;
-      else
-        return 0x00;
-      break;
-  }
-
-  return 0x00;
 }
 
 void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void))
 {
-  switch(pb_id)
-  {
-    case NORTH_PB:
-      north_pb_callback=callback;
-      break;
-    case SOUTH_PB:
-      south_pb_callback=callback;
-      break;
-    case EAST_PB:
-      east_pb_callback=callback;
-      break;
-    case WEST_PB:
-      west_pb_callback=callback;
-      break;
-  }
 }
diff --git a/src/system_stm32f4xx.c b/src/system_stm32f4xx.c
index 7290a9aca28d73a57011380df2d52a330faab09f..318a217adf28a83a5e60427b9e31da9c0061f43f 100644
--- a/src/system_stm32f4xx.c
+++ b/src/system_stm32f4xx.c
@@ -2,17 +2,13 @@
   ******************************************************************************
   * @file    system_stm32f4xx.c
   * @author  MCD Application Team
-  * @version V1.3.0
-  * @date    08-November-2013
+  * @version V2.2.0
+  * @date    15-December-2014
   * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
-  *          This file contains the system clock configuration for STM32F4xx devices.
-  *             
-  * 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 
+  *
+  *   This file provides two functions and one global variable to be called from 
+  *   user application:
+  *      - SystemInit(): This function is called at startup just after reset and 
   *                      before branch to main program. This call is made inside
   *                      the "startup_stm32f4xx.s" file.
   *
@@ -24,84 +20,33 @@
   *                                 be called whenever the core clock is changed
   *                                 during program execution.
   *
-  * 2. After each device reset the HSI (16 MHz) is used as system clock source.
-  *    Then SystemInit() function is called, in "startup_stm32f4xx.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 HSI 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 25MHz, refer to "HSE_VALUE" define
-  *    in "stm32f4xx.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:
-  *=============================================================================
-  *=============================================================================
-  *                    Supported STM32F40xxx/41xxx devices
-  *-----------------------------------------------------------------------------
-  *        System Clock source                    | PLL (HSE)
-  *-----------------------------------------------------------------------------
-  *        SYSCLK(Hz)                             | 168000000
-  *-----------------------------------------------------------------------------
-  *        HCLK(Hz)                               | 168000000
-  *-----------------------------------------------------------------------------
-  *        AHB Prescaler                          | 1
-  *-----------------------------------------------------------------------------
-  *        APB1 Prescaler                         | 4
-  *-----------------------------------------------------------------------------
-  *        APB2 Prescaler                         | 2
-  *-----------------------------------------------------------------------------
-  *        HSE Frequency(Hz)                      | 8000000
-  *-----------------------------------------------------------------------------
-  *        PLL_M                                  | 8
-  *-----------------------------------------------------------------------------
-  *        PLL_N                                  | 336
-  *-----------------------------------------------------------------------------
-  *        PLL_P                                  | 2
-  *-----------------------------------------------------------------------------
-  *        PLL_Q                                  | 7
-  *-----------------------------------------------------------------------------
-  *        PLLI2S_N                               | NA
-  *-----------------------------------------------------------------------------
-  *        PLLI2S_R                               | NA
-  *-----------------------------------------------------------------------------
-  *        I2S input clock                        | NA
-  *-----------------------------------------------------------------------------
-  *        VDD(V)                                 | 3.3
-  *-----------------------------------------------------------------------------
-  *        Main regulator output voltage          | Scale1 mode
-  *-----------------------------------------------------------------------------
-  *        Flash Latency(WS)                      | 5
-  *-----------------------------------------------------------------------------
-  *        Prefetch Buffer                        | ON
-  *-----------------------------------------------------------------------------
-  *        Instruction cache                      | ON
-  *-----------------------------------------------------------------------------
-  *        Data cache                             | ON
-  *-----------------------------------------------------------------------------
-  *        Require 48MHz for USB OTG FS,          | Disabled
-  *        SDIO and RNG clock                     |
-  *-----------------------------------------------------------------------------
-  *=============================================================================
-  ****************************************************************************** 
+  ******************************************************************************
   * @attention
   *
-  * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  *
-  * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
-  * You may not use this file except in compliance with the License.
-  * You may obtain a copy of the License at:
+  * <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
   *
-  *        http://www.st.com/software_license_agreement_liberty_v2
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. Redistributions in binary form must reproduce the above copyright notice,
+  *      this list of conditions and the following disclaimer in the documentation
+  *      and/or other materials provided with the distribution.
+  *   3. Neither the name of STMicroelectronics nor the names of its contributors
+  *      may be used to endorse or promote products derived from this software
+  *      without specific prior written permission.
   *
-  * Unless required by applicable law or agreed to in writing, software 
-  * distributed under the License is distributed on an "AS IS" BASIS, 
-  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-  * See the License for the specific language governing permissions and
-  * limitations under the License.
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   *
   ******************************************************************************
   */
@@ -118,8 +63,17 @@
   * @{
   */
 
+
 #include "stm32f4xx.h"
 
+#if !defined  (HSE_VALUE) 
+  #define HSE_VALUE    ((uint32_t)8000000) /*!< Default value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE    ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
 /**
   * @}
   */
@@ -138,15 +92,18 @@
 
 /************************* Miscellaneous Configuration ************************/
 /*!< Uncomment the following line if you need to use external SRAM or SDRAM mounted
-     on STM324xG_EVAL/STM324x7I_EVAL/STM324x9I_EVAL boards as data memory  */
-     
-#if defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F429_439xx)
+     on STM324xG_EVAL/STM324x9I_EVAL boards as data memory  */
+#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx) || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
 /* #define DATA_IN_ExtSRAM */
-#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx */
-
-#if defined (STM32F427_437xx) || defined (STM32F429_439xx)
+#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */
+ 
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
 /* #define DATA_IN_ExtSDRAM */
-#endif /* STM32F427_437x || STM32F429_439xx */ 
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */
+
+#if defined(DATA_IN_ExtSRAM) && defined(DATA_IN_ExtSDRAM)
+ #error "Please select DATA_IN_ExtSRAM or DATA_IN_ExtSDRAM " 
+#endif /* DATA_IN_ExtSRAM && DATA_IN_ExtSDRAM */
 
 /*!< Uncomment the following line if you need to relocate your vector Table in
      Internal SRAM. */
@@ -155,20 +112,6 @@
                                    This value must be a multiple of 0x200. */
 /******************************************************************************/
 
-/************************* PLL Parameters *************************************/
-/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */
-#define PLL_M      8
-/* USB OTG FS, SDIO and RNG Clock =  PLL_VCO / PLLQ */
-#define PLL_Q      7
-
-#if defined (STM32F40_41xxx)
-#define PLL_N      336
-/* SYSCLK = PLL_VCO / PLL_P */
-#define PLL_P      2
-#endif /* STM32F40_41xxx */
-
-/******************************************************************************/
-
 /**
   * @}
   */
@@ -184,12 +127,16 @@
 /** @addtogroup STM32F4xx_System_Private_Variables
   * @{
   */
-
-#if defined (STM32F40_41xxx)
-  uint32_t SystemCoreClock = 168000000;
-#endif /* STM32F40_41xxx */
-
-  __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+  /* This variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+  uint32_t SystemCoreClock = 16000000;
+  __IO const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
 
 /**
   * @}
@@ -199,10 +146,8 @@
   * @{
   */
 
-static void SetSysClock(void);
-
 #if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
-static void SystemInit_ExtMemCtl(void); 
+  static void SystemInit_ExtMemCtl(void); 
 #endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
 
 /**
@@ -215,8 +160,8 @@ static void SystemInit_ExtMemCtl(void);
 
 /**
   * @brief  Setup the microcontroller system
-  *         Initialize the Embedded Flash Interface, the PLL and update the 
-  *         SystemFrequency variable.
+  *         Initialize the FPU setting, vector table location and External memory 
+  *         configuration.
   * @param  None
   * @retval None
   */
@@ -227,7 +172,6 @@ void SystemInit(void)
     SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */
   #endif
   /* Reset the RCC clock configuration to the default reset state ------------*/
-  RCC_SetHSEValue(HSE_VALUE);
   /* Set HSION bit */
   RCC->CR |= (uint32_t)0x00000001;
 
@@ -249,10 +193,6 @@ void SystemInit(void)
 #if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
   SystemInit_ExtMemCtl(); 
 #endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
-         
-  /* Configure the System clock source, PLL Multiplier and Divider factors, 
-     AHB/APBx prescalers and Flash settings ----------------------------------*/
-  SetSysClock();
 
   /* Configure the Vector Table location add offset address ------------------*/
 #ifdef VECT_TAB_SRAM
@@ -260,8 +200,6 @@ void SystemInit(void)
 #else
   SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
 #endif
-
-  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
 }
 
 /**
@@ -285,14 +223,14 @@ void SystemInit(void)
   *           - 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 stm32f4xx.h file (default value
+  *         (*) HSI_VALUE is a constant defined in stm32f4xx_hal_conf.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 stm32f4xx.h file (default value
-  *              25 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.
+  *         (**) HSE_VALUE is a constant defined in stm32f4xx_hal_conf.h file (its value
+  *              depends on the application requirements), 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.
@@ -331,7 +269,7 @@ void SystemCoreClockUpdate(void)
       else
       {
         /* HSI used as PLL clock source */
-        pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);      
+        pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
       }
 
       pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
@@ -348,309 +286,25 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-/**
-  * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
-  *         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)
-{
-/******************************************************************************/
-/*            PLL (clocked by HSE) used as System clock source                */
-/******************************************************************************/
-  __IO uint32_t StartUpCounter = 0, HSEStatus = 0;
-  
-  /* Enable HSE */
-  RCC->CR |= ((uint32_t)RCC_CR_HSEON);
- 
-  /* Wait till HSE is ready and if Time out is reached exit */
-  do
-  {
-    HSEStatus = RCC->CR & RCC_CR_HSERDY;
-    StartUpCounter++;
-  } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
-
-  if ((RCC->CR & RCC_CR_HSERDY) != RESET)
-  {
-    HSEStatus = (uint32_t)0x01;
-  }
-  else
-  {
-    HSEStatus = (uint32_t)0x00;
-  }
-
-  if (HSEStatus == (uint32_t)0x01)
-  {
-    /* Select regulator voltage output Scale 1 mode */
-    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
-    PWR->CR |= PWR_CR_VOS;
-
-    /* HCLK = SYSCLK / 1*/
-    RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
-
-#if defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F429_439xx)      
-    /* PCLK2 = HCLK / 2*/
-    RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
-    
-    /* PCLK1 = HCLK / 4*/
-    RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
-#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx */
-
-#if defined (STM32F401xx)
-    /* PCLK2 = HCLK / 2*/
-    RCC->CFGR |= RCC_CFGR_PPRE2_DIV1;
-    
-    /* PCLK1 = HCLK / 4*/
-    RCC->CFGR |= RCC_CFGR_PPRE1_DIV2;
-#endif /* STM32F401xx */
-   
-    /* Configure the main PLL */
-    RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
-                   (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
-
-    /* Enable the main PLL */
-    RCC->CR |= RCC_CR_PLLON;
-
-    /* Wait till the main PLL is ready */
-    while((RCC->CR & RCC_CR_PLLRDY) == 0)
-    {
-    }
-   
-#if defined (STM32F427_437xx) || defined (STM32F429_439xx)
-    /* Enable the Over-drive to extend the clock frequency to 180 Mhz */
-    PWR->CR |= PWR_CR_ODEN;
-    while((PWR->CSR & PWR_CSR_ODRDY) == 0)
-    {
-    }
-    PWR->CR |= PWR_CR_ODSWEN;
-    while((PWR->CSR & PWR_CSR_ODSWRDY) == 0)
-    {
-    }      
-    /* Configure Flash prefetch, Instruction cache, Data cache and wait state */
-    FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
-#endif /* STM32F427_437x || STM32F429_439xx  */
-
-#if defined (STM32F40_41xxx)     
-    /* Configure Flash prefetch, Instruction cache, Data cache and wait state */
-    FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
-#endif /* STM32F40_41xxx  */
-
-#if defined (STM32F401xx)
-    /* Configure Flash prefetch, Instruction cache, Data cache and wait state */
-    FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS;
-#endif /* STM32F401xx */
-
-    /* Select the main PLL as system clock source */
-    RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
-    RCC->CFGR |= RCC_CFGR_SW_PLL;
-
-    /* Wait till the main PLL is used as system clock source */
-    while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
-    {
-    }
-  }
-  else
-  { /* If HSE fails to start-up, the application will have wrong clock
-         configuration. User can add here some code to deal with this error */
-  }
-
-}
-
-/**
-  * @brief  Setup the external memory controller. Called in startup_stm32f4xx.s 
-  *          before jump to __main
-  * @param  None
-  * @retval None
-  */ 
-#ifdef DATA_IN_ExtSRAM
-/**
-  * @brief  Setup the external memory controller.
-  *         Called in startup_stm32f4xx.s before jump to main.
-  *         This function configures the external SRAM mounted on STM324xG_EVAL/STM324x7I boards
-  *         This SRAM will be used as program data memory (including heap and stack).
-  * @param  None
-  * @retval None
-  */
-void SystemInit_ExtMemCtl(void)
-{
-/*-- GPIOs Configuration -----------------------------------------------------*/
-/*
- +-------------------+--------------------+------------------+--------------+
- +                       SRAM pins assignment                               +
- +-------------------+--------------------+------------------+--------------+
- | PD0  <-> FMC_D2  | PE0  <-> FMC_NBL0 | PF0  <-> FMC_A0 | PG0 <-> FMC_A10 | 
- | PD1  <-> FMC_D3  | PE1  <-> FMC_NBL1 | PF1  <-> FMC_A1 | PG1 <-> FMC_A11 | 
- | PD4  <-> FMC_NOE | PE3  <-> FMC_A19  | PF2  <-> FMC_A2 | PG2 <-> FMC_A12 | 
- | PD5  <-> FMC_NWE | PE4  <-> FMC_A20  | PF3  <-> FMC_A3 | PG3 <-> FMC_A13 | 
- | PD8  <-> FMC_D13 | PE7  <-> FMC_D4   | PF4  <-> FMC_A4 | PG4 <-> FMC_A14 | 
- | PD9  <-> FMC_D14 | PE8  <-> FMC_D5   | PF5  <-> FMC_A5 | PG5 <-> FMC_A15 | 
- | PD10 <-> FMC_D15 | PE9  <-> FMC_D6   | PF12 <-> FMC_A6 | PG9 <-> FMC_NE2 | 
- | PD11 <-> FMC_A16 | PE10 <-> FMC_D7   | PF13 <-> FMC_A7 |-----------------+
- | PD12 <-> FMC_A17 | PE11 <-> FMC_D8   | PF14 <-> FMC_A8 | 
- | PD13 <-> FMC_A18 | PE12 <-> FMC_D9   | PF15 <-> FMC_A9 | 
- | PD14 <-> FMC_D0  | PE13 <-> FMC_D10  |-----------------+
- | PD15 <-> FMC_D1  | PE14 <-> FMC_D11  |
- |                  | PE15 <-> FMC_D12  |
- +------------------+------------------+
-*/
-   /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
-  RCC->AHB1ENR   |= 0x00000078;
-  
-  /* Connect PDx pins to FMC Alternate function */
-  GPIOD->AFR[0]  = 0x00cc00cc;
-  GPIOD->AFR[1]  = 0xcccccccc;
-  /* Configure PDx pins in Alternate function mode */  
-  GPIOD->MODER   = 0xaaaa0a0a;
-  /* Configure PDx pins speed to 100 MHz */  
-  GPIOD->OSPEEDR = 0xffff0f0f;
-  /* Configure PDx pins Output type to push-pull */  
-  GPIOD->OTYPER  = 0x00000000;
-  /* No pull-up, pull-down for PDx pins */ 
-  GPIOD->PUPDR   = 0x00000000;
-
-  /* Connect PEx pins to FMC Alternate function */
-  GPIOE->AFR[0]  = 0xcccccccc;
-  GPIOE->AFR[1]  = 0xcccccccc;
-  /* Configure PEx pins in Alternate function mode */ 
-  GPIOE->MODER   = 0xaaaaaaaa;
-  /* Configure PEx pins speed to 100 MHz */ 
-  GPIOE->OSPEEDR = 0xffffffff;
-  /* Configure PEx pins Output type to push-pull */  
-  GPIOE->OTYPER  = 0x00000000;
-  /* No pull-up, pull-down for PEx pins */ 
-  GPIOE->PUPDR   = 0x00000000;
-
-  /* Connect PFx pins to FMC Alternate function */
-  GPIOF->AFR[0]  = 0x00cccccc;
-  GPIOF->AFR[1]  = 0xcccc0000;
-  /* Configure PFx pins in Alternate function mode */   
-  GPIOF->MODER   = 0xaa000aaa;
-  /* Configure PFx pins speed to 100 MHz */ 
-  GPIOF->OSPEEDR = 0xff000fff;
-  /* Configure PFx pins Output type to push-pull */  
-  GPIOF->OTYPER  = 0x00000000;
-  /* No pull-up, pull-down for PFx pins */ 
-  GPIOF->PUPDR   = 0x00000000;
-
-  /* Connect PGx pins to FMC Alternate function */
-  GPIOG->AFR[0]  = 0x00cccccc;
-  GPIOG->AFR[1]  = 0x000000c0;
-  /* Configure PGx pins in Alternate function mode */ 
-  GPIOG->MODER   = 0x00080aaa;
-  /* Configure PGx pins speed to 100 MHz */ 
-  GPIOG->OSPEEDR = 0x000c0fff;
-  /* Configure PGx pins Output type to push-pull */  
-  GPIOG->OTYPER  = 0x00000000;
-  /* No pull-up, pull-down for PGx pins */ 
-  GPIOG->PUPDR   = 0x00000000;
-  
-/*-- FMC Configuration ------------------------------------------------------*/
-  /* Enable the FMC/FSMC interface clock */
-  RCC->AHB3ENR         |= 0x00000001;
-  
-#if defined (STM32F427_437xx) || defined (STM32F429_439xx)
-  /* Configure and enable Bank1_SRAM2 */
-  FMC_Bank1->BTCR[2]  = 0x00001011;
-  FMC_Bank1->BTCR[3]  = 0x00000201;
-  FMC_Bank1E->BWTR[2] = 0x0fffffff;
-#endif /* STM32F427_437xx || STM32F429_439xx */ 
-
-#if defined (STM32F40_41xxx)
-  /* Configure and enable Bank1_SRAM2 */
-  FSMC_Bank1->BTCR[2]  = 0x00001011;
-  FSMC_Bank1->BTCR[3]  = 0x00000201;
-  FSMC_Bank1E->BWTR[2] = 0x0fffffff;
-#endif  /* STM32F40_41xxx */
-
-/*
-  Bank1_SRAM2 is configured as follow:
-  In case of FSMC configuration 
-  NORSRAMTimingStructure.FSMC_AddressSetupTime = 1;
-  NORSRAMTimingStructure.FSMC_AddressHoldTime = 0;
-  NORSRAMTimingStructure.FSMC_DataSetupTime = 2;
-  NORSRAMTimingStructure.FSMC_BusTurnAroundDuration = 0;
-  NORSRAMTimingStructure.FSMC_CLKDivision = 0;
-  NORSRAMTimingStructure.FSMC_DataLatency = 0;
-  NORSRAMTimingStructure.FSMC_AccessMode = FMC_AccessMode_A;
-
-  FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2;
-  FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable;
-  FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_SRAM;
-  FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b;
-  FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable;
-  FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable;  
-  FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low;
-  FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable;
-  FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState;
-  FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable;
-  FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable;
-  FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable;
-  FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
-  FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &NORSRAMTimingStructure;
-  FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &NORSRAMTimingStructure;
-
-  In case of FMC configuration   
-  NORSRAMTimingStructure.FMC_AddressSetupTime = 1;
-  NORSRAMTimingStructure.FMC_AddressHoldTime = 0;
-  NORSRAMTimingStructure.FMC_DataSetupTime = 2;
-  NORSRAMTimingStructure.FMC_BusTurnAroundDuration = 0;
-  NORSRAMTimingStructure.FMC_CLKDivision = 0;
-  NORSRAMTimingStructure.FMC_DataLatency = 0;
-  NORSRAMTimingStructure.FMC_AccessMode = FMC_AccessMode_A;
-
-  FMC_NORSRAMInitStructure.FMC_Bank = FMC_Bank1_NORSRAM2;
-  FMC_NORSRAMInitStructure.FMC_DataAddressMux = FMC_DataAddressMux_Disable;
-  FMC_NORSRAMInitStructure.FMC_MemoryType = FMC_MemoryType_SRAM;
-  FMC_NORSRAMInitStructure.FMC_MemoryDataWidth = FMC_MemoryDataWidth_16b;
-  FMC_NORSRAMInitStructure.FMC_BurstAccessMode = FMC_BurstAccessMode_Disable;
-  FMC_NORSRAMInitStructure.FMC_AsynchronousWait = FMC_AsynchronousWait_Disable;  
-  FMC_NORSRAMInitStructure.FMC_WaitSignalPolarity = FMC_WaitSignalPolarity_Low;
-  FMC_NORSRAMInitStructure.FMC_WrapMode = FMC_WrapMode_Disable;
-  FMC_NORSRAMInitStructure.FMC_WaitSignalActive = FMC_WaitSignalActive_BeforeWaitState;
-  FMC_NORSRAMInitStructure.FMC_WriteOperation = FMC_WriteOperation_Enable;
-  FMC_NORSRAMInitStructure.FMC_WaitSignal = FMC_WaitSignal_Disable;
-  FMC_NORSRAMInitStructure.FMC_ExtendedMode = FMC_ExtendedMode_Disable;
-  FMC_NORSRAMInitStructure.FMC_WriteBurst = FMC_WriteBurst_Disable;
-  FMC_NORSRAMInitStructure.FMC_ContinousClock = FMC_CClock_SyncOnly;
-  FMC_NORSRAMInitStructure.FMC_ReadWriteTimingStruct = &NORSRAMTimingStructure;
-  FMC_NORSRAMInitStructure.FMC_WriteTimingStruct = &NORSRAMTimingStructure;
-*/
-  
-}
-#endif /* DATA_IN_ExtSRAM */
-  
-#ifdef DATA_IN_ExtSDRAM
+#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
 /**
   * @brief  Setup the external memory controller.
   *         Called in startup_stm32f4xx.s before jump to main.
-  *         This function configures the external SDRAM mounted on STM324x9I_EVAL board
-  *         This SDRAM will be used as program data memory (including heap and stack).
+  *         This function configures the external memories (SRAM/SDRAM)
+  *         This SRAM/SDRAM will be used as program data memory (including heap and stack).
   * @param  None
   * @retval None
   */
 void SystemInit_ExtMemCtl(void)
 {
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
+#if defined (DATA_IN_ExtSDRAM)
   register uint32_t tmpreg = 0, timeout = 0xFFFF;
   register uint32_t index;
 
   /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface 
       clock */
-  RCC->AHB1ENR |= 0x000001FC;
-  
-  /* Connect PCx pins to FMC Alternate function */
-  GPIOC->AFR[0]  = 0x0000000c;
-  GPIOC->AFR[1]  = 0x00007700;
-  /* Configure PCx pins in Alternate function mode */  
-  GPIOC->MODER   = 0x00a00002;
-  /* Configure PCx pins speed to 50 MHz */  
-  GPIOC->OSPEEDR = 0x00a00002;
-  /* Configure PCx pins Output type to push-pull */  
-  GPIOC->OTYPER  = 0x00000000;
-  /* No pull-up, pull-down for PCx pins */ 
-  GPIOC->PUPDR   = 0x00500000;
+  RCC->AHB1ENR |= 0x000001F8;
   
   /* Connect PDx pins to FMC Alternate function */
   GPIOD->AFR[0]  = 0x000000CC;
@@ -677,8 +331,8 @@ void SystemInit_ExtMemCtl(void)
   GPIOE->PUPDR   = 0x00000000;
 
   /* Connect PFx pins to FMC Alternate function */
-  GPIOF->AFR[0]  = 0xcccccccc;
-  GPIOF->AFR[1]  = 0xcccccccc;
+  GPIOF->AFR[0]  = 0xCCCCCCCC;
+  GPIOF->AFR[1]  = 0xCCCCCCCC;
   /* Configure PFx pins in Alternate function mode */   
   GPIOF->MODER   = 0xAA800AAA;
   /* Configure PFx pins speed to 50 MHz */ 
@@ -689,12 +343,12 @@ void SystemInit_ExtMemCtl(void)
   GPIOF->PUPDR   = 0x00000000;
 
   /* Connect PGx pins to FMC Alternate function */
-  GPIOG->AFR[0]  = 0xcccccccc;
-  GPIOG->AFR[1]  = 0xcccccccc;
+  GPIOG->AFR[0]  = 0xCCCCCCCC;
+  GPIOG->AFR[1]  = 0xCCCCCCCC;
   /* Configure PGx pins in Alternate function mode */ 
-  GPIOG->MODER   = 0xaaaaaaaa;
+  GPIOG->MODER   = 0xAAAAAAAA;
   /* Configure PGx pins speed to 50 MHz */ 
-  GPIOG->OSPEEDR = 0xaaaaaaaa;
+  GPIOG->OSPEEDR = 0xAAAAAAAA;
   /* Configure PGx pins Output type to push-pull */  
   GPIOG->OTYPER  = 0x00000000;
   /* No pull-up, pull-down for PGx pins */ 
@@ -729,43 +383,43 @@ void SystemInit_ExtMemCtl(void)
   RCC->AHB3ENR |= 0x00000001;
   
   /* Configure and enable SDRAM bank1 */
-  FMC_Bank5_6->SDCR[0] = 0x000039D0;
+  FMC_Bank5_6->SDCR[0] = 0x000019E4;
   FMC_Bank5_6->SDTR[0] = 0x01115351;      
   
   /* SDRAM initialization sequence */
   /* Clock enable command */
   FMC_Bank5_6->SDCMR = 0x00000011; 
   tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
-  while((tmpreg != 0) & (timeout-- > 0))
+  while((tmpreg != 0) && (timeout-- > 0))
   {
     tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
   }
-  
+
   /* Delay */
   for (index = 0; index<1000; index++);
   
   /* PALL command */
   FMC_Bank5_6->SDCMR = 0x00000012;           
   timeout = 0xFFFF;
-  while((tmpreg != 0) & (timeout-- > 0))
+  while((tmpreg != 0) && (timeout-- > 0))
   {
-  tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
   }
   
   /* Auto refresh command */
   FMC_Bank5_6->SDCMR = 0x00000073;
   timeout = 0xFFFF;
-  while((tmpreg != 0) & (timeout-- > 0))
+  while((tmpreg != 0) && (timeout-- > 0))
   {
-  tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
   }
  
   /* MRD register program */
   FMC_Bank5_6->SDCMR = 0x00046014;
   timeout = 0xFFFF;
-  while((tmpreg != 0) & (timeout-- > 0))
+  while((tmpreg != 0) && (timeout-- > 0))
   {
-  tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
+    tmpreg = FMC_Bank5_6->SDSR & 0x00000020; 
   } 
   
   /* Set refresh count */
@@ -775,35 +429,85 @@ void SystemInit_ExtMemCtl(void)
   /* Disable write protection */
   tmpreg = FMC_Bank5_6->SDCR[0]; 
   FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF);
+#endif /* DATA_IN_ExtSDRAM */
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */
+
+#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx) || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
+#if defined(DATA_IN_ExtSRAM)
+/*-- GPIOs Configuration -----------------------------------------------------*/
+   /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
+  RCC->AHB1ENR   |= 0x00000078;
+  
+  /* Connect PDx pins to FMC Alternate function */
+  GPIOD->AFR[0]  = 0x00CCC0CC;
+  GPIOD->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PDx pins in Alternate function mode */  
+  GPIOD->MODER   = 0xAAAA0A8A;
+  /* Configure PDx pins speed to 100 MHz */  
+  GPIOD->OSPEEDR = 0xFFFF0FCF;
+  /* Configure PDx pins Output type to push-pull */  
+  GPIOD->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PDx pins */ 
+  GPIOD->PUPDR   = 0x00000000;
+
+  /* Connect PEx pins to FMC Alternate function */
+  GPIOE->AFR[0]  = 0xC00CC0CC;
+  GPIOE->AFR[1]  = 0xCCCCCCCC;
+  /* Configure PEx pins in Alternate function mode */ 
+  GPIOE->MODER   = 0xAAAA828A;
+  /* Configure PEx pins speed to 100 MHz */ 
+  GPIOE->OSPEEDR = 0xFFFFC3CF;
+  /* Configure PEx pins Output type to push-pull */  
+  GPIOE->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PEx pins */ 
+  GPIOE->PUPDR   = 0x00000000;
+
+  /* Connect PFx pins to FMC Alternate function */
+  GPIOF->AFR[0]  = 0x00CCCCCC;
+  GPIOF->AFR[1]  = 0xCCCC0000;
+  /* Configure PFx pins in Alternate function mode */   
+  GPIOF->MODER   = 0xAA000AAA;
+  /* Configure PFx pins speed to 100 MHz */ 
+  GPIOF->OSPEEDR = 0xFF000FFF;
+  /* Configure PFx pins Output type to push-pull */  
+  GPIOF->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PFx pins */ 
+  GPIOF->PUPDR   = 0x00000000;
+
+  /* Connect PGx pins to FMC Alternate function */
+  GPIOG->AFR[0]  = 0x00CCCCCC;
+  GPIOG->AFR[1]  = 0x000000C0;
+  /* Configure PGx pins in Alternate function mode */ 
+  GPIOG->MODER   = 0x00085AAA;
+  /* Configure PGx pins speed to 100 MHz */ 
+  GPIOG->OSPEEDR = 0x000CAFFF;
+  /* Configure PGx pins Output type to push-pull */  
+  GPIOG->OTYPER  = 0x00000000;
+  /* No pull-up, pull-down for PGx pins */ 
+  GPIOG->PUPDR   = 0x00000000;
   
-/*
-  Bank1_SDRAM is configured as follow:
-
-  FMC_SDRAMTimingInitStructure.FMC_LoadToActiveDelay = 2;      
-  FMC_SDRAMTimingInitStructure.FMC_ExitSelfRefreshDelay = 6;  
-  FMC_SDRAMTimingInitStructure.FMC_SelfRefreshTime = 4;        
-  FMC_SDRAMTimingInitStructure.FMC_RowCycleDelay = 6;         
-  FMC_SDRAMTimingInitStructure.FMC_WriteRecoveryTime = 2;      
-  FMC_SDRAMTimingInitStructure.FMC_RPDelay = 2;                
-  FMC_SDRAMTimingInitStructure.FMC_RCDDelay = 2;               
-
-  FMC_SDRAMInitStructure.FMC_Bank = SDRAM_BANK;
-  FMC_SDRAMInitStructure.FMC_ColumnBitsNumber = FMC_ColumnBits_Number_8b;
-  FMC_SDRAMInitStructure.FMC_RowBitsNumber = FMC_RowBits_Number_11b;
-  FMC_SDRAMInitStructure.FMC_SDMemoryDataWidth = FMC_SDMemory_Width_16b;
-  FMC_SDRAMInitStructure.FMC_InternalBankNumber = FMC_InternalBank_Number_4;
-  FMC_SDRAMInitStructure.FMC_CASLatency = FMC_CAS_Latency_3; 
-  FMC_SDRAMInitStructure.FMC_WriteProtection = FMC_Write_Protection_Disable;
-  FMC_SDRAMInitStructure.FMC_SDClockPeriod = FMC_SDClock_Period_2;
-  FMC_SDRAMInitStructure.FMC_ReadBurst = FMC_Read_Burst_disable;
-  FMC_SDRAMInitStructure.FMC_ReadPipeDelay = FMC_ReadPipe_Delay_1;
-  FMC_SDRAMInitStructure.FMC_SDRAMTimingStruct = &FMC_SDRAMTimingInitStructure;
-*/
+/*-- FMC/FSMC Configuration --------------------------------------------------*/                                                                               
+  /* Enable the FMC/FSMC interface clock */
+  RCC->AHB3ENR         |= 0x00000001;
   
-}
-#endif /* DATA_IN_ExtSDRAM */
+#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx)|| defined(STM32F439xx) 
+  /* Configure and enable Bank1_SRAM2 */
+  FMC_Bank1->BTCR[2]  = 0x00001011;
+  FMC_Bank1->BTCR[3]  = 0x00000201;
+  FMC_Bank1E->BWTR[2] = 0x0fffffff;
+#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ 
 
+#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx)|| defined(STM32F417xx)
+  /* Configure and enable Bank1_SRAM2 */
+  FSMC_Bank1->BTCR[2]  = 0x00001011;
+  FSMC_Bank1->BTCR[3]  = 0x00000201;
+  FSMC_Bank1E->BWTR[2] = 0x0FFFFFFF;
+#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx */
 
+#endif /* DATA_IN_ExtSRAM */
+#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ 
+}
+#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
 /**
   * @}
   */