diff --git a/run_debugserver.sh b/run_debugserver.sh
index d87b0bf..a73c30f 100755
--- a/run_debugserver.sh
+++ b/run_debugserver.sh
@@ -1,38 +1,36 @@
 #!/bin/bash
 
 run_jlinkserver() {
-	JLinkGDBServerCLExe -if SWD -device STM32H750VBTx \
-		-ir -localhostonly -nogui \
+	JLinkGDBServerCLExe -if SWD -device NUC220LE3AN \
+		-ir -nogui \
+		-localhostonly -port 3333 \
 		-rtos GDBServer/RTOSPlugin_FreeRTOS.so
 }
 
-run_openocd_stlink() {
-	openocd -f "interface/stlink.cfg" -f "target/stm32h7x.cfg"
-}
-
 run_openocd_jlink() {
-	openocd -f "interface/jlink.cfg" -c "transport select swd" -f "target/stm32h7x.cfg"
+	openocd -f "interface/jlink.cfg" -c "transport select swd" -f "target/numicro.cfg"
 }
 
 run_pyocd() {
-	pyocd gdbserver -t stm32h750vbtx -f 24m --persist
+	echo "Note: pyOCD uses low level DAP APIs provided by J-LINK," \
+		"speed settings and other functions may not available."
+	pyocd gdbserver -t stm32h750vbtx -f 4m --persist
 }
 
 case $1 in
-	jlink)
-		run_jlinkserver
-		;;
+jlink)
+	run_jlinkserver
+	;;
 
-	pyocd)
-		run_pyocd
-		;;
+pyocd)
+	run_pyocd
+	;;
 
-	openocd-stlink)
-		run_openocd_stlink
-		;;
+openocd-jlink)
+	run_openocd_jlink
+	;;
 
-	openocd-jlink)
-		run_openocd_jlink
-		;;
-
-esac
\ No newline at end of file
+*)
+	echo "Usage: run_debugserver.sh jlink|pyocd|openocd-jlink"
+	;;
+esac
diff --git a/src/main.c b/src/main.c
index d4ed1fd..c86faba 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,8 +1,84 @@
 #include "NUC200Series.h"
 
-int main(int argc, const char *argv[]) {
-    SystemCoreClockUpdate();
+/**
+ * This function enables external crystal oscillator,
+ * configures internal PLL and switch CPU and Bus clock
+ * to PLL frequency.
+ */
+static void system_clock_config(void) {
+    /**
+     * Configure External crystal input
+     * Note: This register is guarded.
+     */
+    SYS_UnlockReg();
+    CLK->PWRCON |= CLK_PWRCON_XTL12M_EN_Msk; /* Enable XTL12M */
+    SYS_LockReg();
 
-    for(;;) {
+    while (!(CLK->CLKSTATUS & CLK_CLKSTATUS_XTL12M_STB_Msk)) {
+        /* Wait until external crystal stabilized */
+    }
+
+    /**
+     * Configure PLL
+     * NUC200 series has 1 PLL, up to 50MHz.
+     * We set up PLL running at 48MHz, which is default.
+     * PLLCON register:
+     * OUT_DV =  3,  NO =  4
+     *  IN_DV =  1,  NR =  3
+     *  FB_DV = 46,  NF = 48
+     *  PLL FOUT = FIN * (NF / NR) * (1 / NO) = 12MHz * (48 / 3) * (1 / 4) = 48MHz
+     */
+    CLK->PLLCON = (CLK_PLLCON_IN_DV_Msk & (1U << CLK_PLLCON_IN_DV_Pos)) |
+                  (CLK_PLLCON_FB_DV_Msk & (46U << CLK_PLLCON_FB_DV_Pos)) |
+                  (CLK_PLLCON_OUT_DV_Msk & (3U << CLK_PLLCON_OUT_DV_Pos));
+
+    CLK->PLLCON &= ~CLK_PLLCON_OE_Msk; /* Enable PLL FOUT output. NOTE: This bit is active-low! */
+
+    while (!(CLK->CLKSTATUS & CLK_CLKSTATUS_PLL_STB_Msk)) {
+        /* Wait until PLL clock stabilized */
+    }
+
+    /**
+     * Configure Clock dividers.
+     * NUC200 series has 2 divider registers, one for SC peripherals,
+     * another for everything else.
+     *   ADC: 12MHz
+     *  UART: 12MHz
+     *   USB: 48MHz
+     *  HCLK: 48MHz
+     * SC0-2: 12MHz
+     */
+    CLK->CLKDIV  = CLK_CLKDIV_ADC(4) | CLK_CLKDIV_UART(4) | CLK_CLKDIV_USB(1) | CLK_CLKDIV_HCLK(1);
+    CLK->CLKDIV1 = CLK_CLKDIV1_SC2(4) | CLK_CLKDIV1_SC1(4) | CLK_CLKDIV1_SC0(4);
+
+    /**
+     * Configure Clock selectors.
+     * Some of these register bits (in CLKSEL0) are guarded by a latch register,
+     * however it will not cleared automatically by write operations.
+     * Clear the latch manually after clock changes.
+     */
+    SYS_UnlockReg();
+    CLK->CLKSEL0 = CLK_CLKSEL0_STCLK_S_HCLK | CLK_CLKSEL0_HCLK_S_PLL; /* SysTick and HCLK */
+    SYS_LockReg();
+
+    CLK->CLKSEL1 = 0xFFFFFFFF; /* Reset value */
+    CLK->CLKSEL1 &= ~CLK_CLKSEL1_SPI0_S_Msk;
+    CLK->CLKSEL1 |= CLK_CLKSEL1_SPI0_S_HCLK;
+
+    CLK->CLKSEL2 = 0x200FFU; /* Reset value */
+    CLK->CLKSEL3 = 0x3FU;    /* Reset value */
+
+    SystemCoreClockUpdate();
+}
+
+static void pinmux_config(void) {
+    // Nothing for now.
+}
+
+int main(int argc, const char *argv[]) {
+    system_clock_config();
+    pinmux_config();
+
+    for (;;) {
     }
 }
\ No newline at end of file