diff --git a/bootloader/targets/f2/target.mk b/bootloader/targets/f2/target.mk
index 53f9302eb..fb93cda26 100644
--- a/bootloader/targets/f2/target.mk
+++ b/bootloader/targets/f2/target.mk
@@ -5,7 +5,7 @@ FW_ADDRESS		= 0x08008000
 OS_OFFSET		= 0x00008000
 FLASH_ADDRESS	= 0x08000000
 
-DEBUG_AGENT		= openocd -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32l4x.cfg -c "init" -c "adapter speed 4000"
+OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32l4x.cfg -c "init" -c "adapter speed 4000"
 BOOT_CFLAGS		= -DBOOT_ADDRESS=$(BOOT_ADDRESS) -DFW_ADDRESS=$(FW_ADDRESS) -DOS_OFFSET=$(OS_OFFSET)
 MCU_FLAGS		= -mcpu=cortex-m4 -mthumb -mfpu=fpv4-sp-d16 -mfloat-abi=hard
 
diff --git a/bootloader/targets/f3/target.c b/bootloader/targets/f3/target.c
index 74ec9a1f9..d8d5a8793 100644
--- a/bootloader/targets/f3/target.c
+++ b/bootloader/targets/f3/target.c
@@ -157,4 +157,4 @@ void target_switch2os() {
     LL_GPIO_ResetOutputPin(LED_RED_PORT, LED_RED_PIN);
     SCB->VTOR = BOOT_ADDRESS + OS_OFFSET;
     target_switch((void*)(BOOT_ADDRESS + OS_OFFSET));
-}
\ No newline at end of file
+}
diff --git a/bootloader/targets/f3/target.mk b/bootloader/targets/f3/target.mk
index 45c2bbe67..47a6a1cdb 100644
--- a/bootloader/targets/f3/target.mk
+++ b/bootloader/targets/f3/target.mk
@@ -5,7 +5,7 @@ FW_ADDRESS		= 0x08008000
 OS_OFFSET		= 0x00008000
 FLASH_ADDRESS	= 0x08000000
 
-DEBUG_AGENT		= openocd -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32wbx.cfg -c "init" -c "adapter speed 4000"
+OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32wbx.cfg -c "init" -c "adapter speed 4000"
 BOOT_CFLAGS		= -DBOOT_ADDRESS=$(BOOT_ADDRESS) -DFW_ADDRESS=$(FW_ADDRESS) -DOS_OFFSET=$(OS_OFFSET)
 MCU_FLAGS		= -mcpu=cortex-m4 -mthumb -mfpu=fpv4-sp-d16 -mfloat-abi=hard
 
diff --git a/firmware/targets/f2/target.mk b/firmware/targets/f2/target.mk
index ba58e50b8..50c94ee30 100644
--- a/firmware/targets/f2/target.mk
+++ b/firmware/targets/f2/target.mk
@@ -1,6 +1,6 @@
 TOOLCHAIN = arm
 
-DEBUG_AGENT		= openocd -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32l4x.cfg -c "init" -c "adapter speed 4000"
+OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32l4x.cfg -c "init" -c "adapter speed 4000"
 
 BOOT_ADDRESS	= 0x08000000
 FW_ADDRESS		= 0x08008000
diff --git a/firmware/targets/f3/stm32wb55xx_flash_cm4.ld b/firmware/targets/f3/stm32wb55xx_flash_cm4.ld
index c8946a8ee..ce970b161 100644
--- a/firmware/targets/f3/stm32wb55xx_flash_cm4.ld
+++ b/firmware/targets/f3/stm32wb55xx_flash_cm4.ld
@@ -55,7 +55,7 @@ _Min_Stack_Size = 0x400; /* required amount of stack */
 /* Specify the memory areas */
 MEMORY
 {
-FLASH (rx)                 : ORIGIN = 0x08008000, LENGTH = 480K
+FLASH (rx)                 : ORIGIN = 0x08000000, LENGTH = 512K
 RAM1 (xrw)                 : ORIGIN = 0x20000004, LENGTH = 0x2FFFC
 RAM_SHARED (xrw)           : ORIGIN = 0x20030000, LENGTH = 10K
 }
diff --git a/firmware/targets/f3/stm32wb55xx_flash_cm4_boot.ld b/firmware/targets/f3/stm32wb55xx_flash_cm4_boot.ld
new file mode 100644
index 000000000..c8946a8ee
--- /dev/null
+++ b/firmware/targets/f3/stm32wb55xx_flash_cm4_boot.ld
@@ -0,0 +1,187 @@
+/**
+*****************************************************************************
+**
+**  File        : stm32wb55xx_flash_cm4.ld
+**
+**  Abstract    : System Workbench Minimal System calls file
+**
+** 		          For more information about which c-functions
+**                need which of these lowlevel functions
+**                please consult the Newlib libc-manual
+**
+**  Environment : System Workbench for MCU
+**
+**  Distribution: The file is distributed “as is,” without any warranty
+**                of any kind.
+**
+*****************************************************************************
+**
+** <h2><center>&copy; COPYRIGHT(c) 2019 Ac6</center></h2>
+**
+** 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 Ac6 nor the names of its contributors
+**      may be used to endorse or promote products derived from this software
+**      without specific prior written permission.
+**
+** 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.
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_estack = 0x20030000;    /* end of RAM */
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0x200;      /* required amount of heap  */
+_Min_Stack_Size = 0x400; /* required amount of stack */
+
+/* Specify the memory areas */
+MEMORY
+{
+FLASH (rx)                 : ORIGIN = 0x08008000, LENGTH = 480K
+RAM1 (xrw)                 : ORIGIN = 0x20000004, LENGTH = 0x2FFFC
+RAM_SHARED (xrw)           : ORIGIN = 0x20030000, LENGTH = 10K
+}
+
+/* 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) */
+    *(.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
+
+  /* Constant data goes into FLASH */
+  .rodata :
+  {
+    . = ALIGN(4);
+    *(.rodata)         /* .rodata sections (constants, strings, etc.) */
+    *(.rodata*)        /* .rodata* sections (constants, strings, etc.) */
+    . = ALIGN(4);
+  } >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 (*(SORT(.fini_array.*)))
+    KEEP (*(.fini_array*))
+    PROVIDE_HIDDEN (__fini_array_end = .);
+  } >FLASH
+
+  /* used by the startup to initialize data */
+  _sidata = LOADADDR(.data);
+
+  /* Initialized data sections goes into RAM, load LMA copy after code */
+  .data : 
+  {
+    . = 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 */
+  } >RAM1 AT> FLASH
+
+  
+  /* 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;
+  } >RAM1
+
+  /* User_heap_stack section, used to check that there is enough RAM left */
+  ._user_heap_stack :
+  {
+    . = ALIGN(8);
+    PROVIDE ( end = . );
+    PROVIDE ( _end = . );
+    . = . + _Min_Heap_Size;
+    . = . + _Min_Stack_Size;
+    . = ALIGN(8);
+  } >RAM1
+
+  
+
+  /* Remove information from the standard libraries */
+  /DISCARD/ :
+  {
+    libc.a ( * )
+    libm.a ( * )
+    libgcc.a ( * )
+  }
+
+  .ARM.attributes 0       : { *(.ARM.attributes) }
+   MAPPING_TABLE (NOLOAD) : { *(MAPPING_TABLE) } >RAM_SHARED
+   MB_MEM1 (NOLOAD)       : { *(MB_MEM1) } >RAM_SHARED
+   MB_MEM2 (NOLOAD)       : { _sMB_MEM2 = . ; *(MB_MEM2) ; _eMB_MEM2 = . ; } >RAM_SHARED
+}
+
+
diff --git a/firmware/targets/f3/target.mk b/firmware/targets/f3/target.mk
index 8b5da7994..957e8df2d 100644
--- a/firmware/targets/f3/target.mk
+++ b/firmware/targets/f3/target.mk
@@ -1,6 +1,6 @@
 TOOLCHAIN = arm
 
-DEBUG_AGENT		= openocd -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32wbx.cfg -c "init" -c "adapter speed 4000"
+OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f target/stm32wbx.cfg -c "init" -c "adapter speed 4000"
 
 BOOT_ADDRESS	= 0x08000000
 FW_ADDRESS		= 0x08008000
@@ -94,7 +94,7 @@ CFLAGS			+= \
 ifeq ($(NO_BOOTLOADER), 1)
 LDFLAGS			+= -T$(MXPROJECT_DIR)/stm32wb55xx_flash_cm4_no_boot.ld
 else
-LDFLAGS			+= -T$(MXPROJECT_DIR)/stm32wb55xx_flash_cm4.ld
+LDFLAGS			+= -T$(MXPROJECT_DIR)/stm32wb55xx_flash_cm4_boot.ld
 endif
 
 CFLAGS += \
diff --git a/lib/drivers/bq25896.c b/lib/drivers/bq25896.c
index 32c5f8c3d..cccc7bdf3 100644
--- a/lib/drivers/bq25896.c
+++ b/lib/drivers/bq25896.c
@@ -61,14 +61,20 @@ typedef struct {
 static bq25896_regs_t bq25896_regs;
 
 void bq25896_init() {
-    bq25896_read(0x00, (uint8_t*)&bq25896_regs, sizeof(bq25896_regs));
-
-    bq25896_regs.r09.BATFET_DIS = 0;
-    bq25896_write_reg(0x09, (uint8_t*)&bq25896_regs.r09);
-
     bq25896_regs.r14.REG_RST = 1;
     bq25896_write_reg(0x14, (uint8_t*)&bq25896_regs.r14);
 
+    // Readout all registers
+    bq25896_read(0x00, (uint8_t*)&bq25896_regs, sizeof(bq25896_regs));
+
+    // Poll ADC forever
+    bq25896_regs.r02.CONV_START = 1;
+    bq25896_regs.r02.CONV_RATE = 1;
+    bq25896_write_reg(0x02, (uint8_t*)&bq25896_regs.r02);
+
+    bq25896_regs.r07.WATCHDOG = WatchdogDisable;
+    bq25896_write_reg(0x07, (uint8_t*)&bq25896_regs.r07);
+
     bq25896_read(0x00, (uint8_t*)&bq25896_regs, sizeof(bq25896_regs));
 }
 
@@ -78,6 +84,7 @@ void bq25896_poweroff() {
 }
 
 bool bq25896_is_charging() {
+    bq25896_read(0x00, (uint8_t*)&bq25896_regs, sizeof(bq25896_regs));
     bq25896_read_reg(0x0B, (uint8_t*)&bq25896_regs.r0B);
     return bq25896_regs.r0B.CHRG_STAT != ChrgStatNo;
 }
@@ -92,16 +99,7 @@ void bq25896_disable_otg() {
     bq25896_write_reg(0x03, (uint8_t*)&bq25896_regs.r03);
 }
 
-void bq25896_adc_sample() {
-    bq25896_regs.r02.CONV_START = 1;
-    bq25896_write_reg(0x02, (uint8_t*)&bq25896_regs.r02);
-    while(bq25896_regs.r02.CONV_START == 1) {
-        bq25896_read_reg(0x02, (uint8_t*)&bq25896_regs.r02);
-    }
-}
-
 uint16_t bq25896_get_vbus_voltage() {
-    bq25896_adc_sample();
     bq25896_read_reg(0x11, (uint8_t*)&bq25896_regs.r11);
     if (bq25896_regs.r11.VBUS_GD) {
         return (uint16_t)bq25896_regs.r11.VBUSV * 100 + 2600;
@@ -111,25 +109,21 @@ uint16_t bq25896_get_vbus_voltage() {
 }
 
 uint16_t bq25896_get_vsys_voltage() {
-    bq25896_adc_sample();
     bq25896_read_reg(0x0F, (uint8_t*)&bq25896_regs.r0F);
     return (uint16_t)bq25896_regs.r0F.SYSV * 20 + 2304;
 }
 
 uint16_t bq25896_get_vbat_voltage() {
-    bq25896_adc_sample();
     bq25896_read_reg(0x0E, (uint8_t*)&bq25896_regs.r0E);
     return (uint16_t)bq25896_regs.r0E.BATV * 20 + 2304;
 }
 
 uint16_t bq25896_get_vbat_current() {
-    bq25896_adc_sample();
     bq25896_read_reg(0x12, (uint8_t*)&bq25896_regs.r12);
     return (uint16_t)bq25896_regs.r12.ICHGR * 50;
 }
 
 uint32_t bq25896_get_ntc_mpct() {
-    bq25896_adc_sample();
     bq25896_read_reg(0x10, (uint8_t*)&bq25896_regs.r10);
     return (uint32_t)bq25896_regs.r10.TSPCT * 465+21000;
 }
diff --git a/lib/drivers/bq25896_reg.h b/lib/drivers/bq25896_reg.h
index 97b064612..b37041061 100644
--- a/lib/drivers/bq25896_reg.h
+++ b/lib/drivers/bq25896_reg.h
@@ -9,12 +9,6 @@
 
 #define BQ25896_ADDRESS 0xD6
 
-typedef struct {
-    uint8_t IINLIM:6;       // Input Current Limit, mA, offset: +100mA
-    bool EN_ILIM:1;         // Enable ILIM Pin
-    bool EN_HIZ:1;          // Enable HIZ Mode
-} REG00;
-
 #define IILIM_1600          (1<<5)
 #define IILIM_800           (1<<4)
 #define IILIM_400           (1<<3)
@@ -23,10 +17,11 @@ typedef struct {
 #define IILIM_50            (1<<0)
 
 typedef struct {
-    uint8_t VINDPM_OS:5;    // Input Voltage Limit Offset, mV
-    bool BCOLD:1;           // Boost Mode Cold Temperature Monitor Threshold
-    uint8_t BHOT:2;         // Boost Mode Hot Temperature Monitor Threshold
-} REG01;
+    uint8_t IINLIM:6;       // Input Current Limit, mA, offset: +100mA
+    bool EN_ILIM:1;         // Enable ILIM Pin
+    bool EN_HIZ:1;          // Enable HIZ Mode
+} REG00;
+
 
 #define VINDPM_OS_1600      (1<<4)
 #define VINDPM_OS_800       (1<<3)
@@ -34,6 +29,19 @@ typedef struct {
 #define VINDPM_OS_200       (1<<1)
 #define VINDPM_OS_100       (1<<0)
 
+typedef enum {
+    Bhot34 = 0b00,         // – VBHOT1 Threshold (34.75%) (default)
+    Bhot37 = 0b01,         // – VBHOT0 Threshold (Typ. 37.75%)
+    Bhot31 = 0b10,         // – VBHOT2 Threshold (Typ. 31.25%)
+    BhotDisable = 0b11,    // – Disable boost mode thermal protection
+} Bhot;
+
+typedef struct {
+    uint8_t VINDPM_OS:5;    // Input Voltage Limit Offset, mV
+    bool BCOLD:1;           // Boost Mode Cold Temperature Monitor Threshold
+    Bhot BHOT:2;            // Boost Mode Hot Temperature Monitor Threshold
+} REG01;
+
 
 typedef struct {
     bool AUTO_DPDM_EN:1;    // Automatic Input Detection Enable
@@ -46,6 +54,10 @@ typedef struct {
 } REG02;
 
 
+#define SYS_MIN_400         (1<<2)
+#define SYS_MIN_200         (1<<1)
+#define SYS_MIN_100         (1<<0)
+
 typedef struct {
     bool MIN_VBAT_SEL:1;    // Minimum Battery Voltage (falling) to exit boost mode
     uint8_t SYS_MIN:3;      // Minimum System Voltage Limit, mV, offset: +3000mV
@@ -55,14 +67,6 @@ typedef struct {
     bool BAT_LOADEN:1;      // Battery Load (IBATLOAD) Enable
 } REG03;
 
-#define SYS_MIN_400         (1<<2)
-#define SYS_MIN_200         (1<<1)
-#define SYS_MIN_100         (1<<0)
-
-typedef struct {
-    uint8_t ICHG:7;         // Fast Charge Current Limit, mA
-    bool EN_PUMPX:1;        // Current pulse control Enable
-} REG04;
 
 #define ICHG_4096           (1<<6)
 #define ICHG_2048           (1<<5)
@@ -73,9 +77,10 @@ typedef struct {
 #define ICHG_64             (1<<0)
 
 typedef struct {
-    uint8_t ITERM:4;        // Termination Current Limit, offset: +64mA
-    uint8_t IPRECHG:4;      // Precharge Current Limit, offset: +64mA
-} REG05;
+    uint8_t ICHG:7;         // Fast Charge Current Limit, mA
+    bool EN_PUMPX:1;        // Current pulse control Enable
+} REG04;
+
 
 #define IPRETERM_512        (1<<3)
 #define IPRETERM_256        (1<<2)
@@ -83,10 +88,10 @@ typedef struct {
 #define IPRETERM_64         (1<<0)
 
 typedef struct {
-    bool VRECHG:1;          // Battery Recharge Threshold Offset
-    bool BATLOWV:1;         // Battery Precharge to Fast Charge Threshold
-    uint8_t VREG:6;         // Charge Voltage Limit, offset: +3840mV
-} REG06;
+    uint8_t ITERM:4;        // Termination Current Limit, offset: +64mA
+    uint8_t IPRECHG:4;      // Precharge Current Limit, offset: +64mA
+} REG05;
+
 
 #define VREG_512            (1<<5)
 #define VREG_256            (1<<4)
@@ -95,30 +100,36 @@ typedef struct {
 #define VREG_32             (1<<1)
 #define VREG_16             (1<<0)
 
+typedef struct {
+    bool VRECHG:1;          // Battery Recharge Threshold Offset
+    bool BATLOWV:1;         // Battery Precharge to Fast Charge Threshold
+    uint8_t VREG:6;         // Charge Voltage Limit, offset: +3840mV
+} REG06;
+
+
+typedef enum {
+    WatchdogDisable = 0b00,
+    Watchdog40 = 0b01,
+    Watchdog80 = 0b10,
+    Watchdog160 = 0b11,
+} Watchdog;
+
+typedef enum {
+    ChgTimer5 = 0b00,
+    ChgTimer8 = 0b01,
+    ChgTimer12 = 0b10,
+    ChgTimer20 = 0b11,
+} ChgTimer;
+
 typedef struct {
     bool JEITA_ISET:1;      // JEITA Low Temperature Current Setting
-    uint8_t CHG_TIMER:2;    // Fast Charge Timer Setting
+    ChgTimer CHG_TIMER:2;    // Fast Charge Timer Setting
     bool EN_TIMER:1;        // Charging Safety Timer Enable
-    uint8_t WATCHDOG:2;     // I2C Watchdog Timer Setting
+    Watchdog WATCHDOG:2;     // I2C Watchdog Timer Setting
     bool STAT_DIS:1;        // STAT Pin Disable
     bool EN_TERM:1;         // Charging Termination Enable
 } REG07;
 
-#define WATCHDOG_DIS        (0b00)
-#define WATCHDOG_40         (0b01)
-#define WATCHDOG_80         (0b10)
-#define WATCHDOG_160        (0b11)
-
-#define CHG_TIMER_5         (0b00)
-#define CHG_TIMER_8         (0b01)
-#define CHG_TIMER_12        (0b10)
-#define CHG_TIMER_20        (0b11)
-
-typedef struct {
-    uint8_t TREG:2;         // Thermal Regulation Threshold
-    uint8_t VCLAMP:3;       // IR Compensation Voltage Clamp
-    uint8_t BAT_COMP:3;     // IR Compensation Resistor Setting 
-} REG08;
 
 #define BAT_COMP_80         (1<<2)
 #define BAT_COMP_40         (1<<1)
@@ -133,6 +144,13 @@ typedef struct {
 #define TREG_100            (0b10)
 #define TREG_120            (0b11)
 
+typedef struct {
+    uint8_t TREG:2;         // Thermal Regulation Threshold
+    uint8_t VCLAMP:3;       // IR Compensation Voltage Clamp
+    uint8_t BAT_COMP:3;     // IR Compensation Resistor Setting 
+} REG08;
+
+
 typedef struct {
     bool PUMPX_DN:1;        // Current pulse control voltage down enable
     bool PUMPX_UP:1;        // Current pulse control voltage up enable
@@ -144,11 +162,6 @@ typedef struct {
     bool FORCE_ICO:1;       // Force Start Input Current Optimizer 
 } REG09;
 
-typedef struct {
-    uint8_t BOOST_LIM:3;    // Boost Mode Current Limit
-    bool PFM_OTG_DIS:1;     // PFM mode allowed in boost mode
-    uint8_t BOOSTV:4;       // Boost Mode Voltage Regulation, offset: +4550mV
-} REG0A;
 
 #define BOOSTV_512          (1<<3)
 #define BOOSTV_256          (1<<2)
@@ -164,6 +177,12 @@ typedef struct {
 #define BOOST_LIM_2150      (0b110)
 #define BOOST_LIM_RSVD      (0b111)
 
+typedef struct {
+    uint8_t BOOST_LIM:3;    // Boost Mode Current Limit
+    bool PFM_OTG_DIS:1;     // PFM mode allowed in boost mode
+    uint8_t BOOSTV:4;       // Boost Mode Voltage Regulation, offset: +4550mV
+} REG0A;
+
 
 typedef enum {
     VBusStatNo = 0b000,
@@ -187,6 +206,7 @@ typedef struct {
     VBusStat VBUS_STAT:3;    // VBUS Status register
 } REG0B;
 
+
 typedef enum {
     ChrgFaultNO = 0b00,
     ChrgFaultIN = 0b01,
@@ -210,10 +230,6 @@ typedef struct {
     bool WATCHDOG_FAULT:1;  // Watchdog Fault Status
 } REG0C;
 
-typedef struct {
-    uint8_t VINDPM:7;       // Absolute VINDPM Threshold, offset: +2600mV
-    bool FORCE_VINDPM:1;    // VINDPM Threshold Setting Method
-} REG0D;
 
 #define VINDPM_6400         (1<<6)
 #define VINDPM_3200         (1<<5)
@@ -223,37 +239,49 @@ typedef struct {
 #define VINDPM_200          (1<<1)
 #define VINDPM_100          (1<<0)
 
+typedef struct {
+    uint8_t VINDPM:7;       // Absolute VINDPM Threshold, offset: +2600mV
+    bool FORCE_VINDPM:1;    // VINDPM Threshold Setting Method
+} REG0D;
+
+
 typedef struct {
     uint8_t BATV:7;         // ADC conversion of Battery Voltage (VBAT), offset: +2304mV
     bool THERM_STAT:1;      // Thermal Regulation Status
 } REG0E;
 
+
 typedef struct {
     uint8_t SYSV:7;         // ADDC conversion of System Voltage (VSYS), offset: +2304mV
     uint8_t RES:1;          // Reserved: Always reads 0
 } REG0F;
 
+
 typedef struct {
     uint8_t TSPCT:7;        // ADC conversion of TS Voltage (TS) as percentage of REGN, offset: +21%
     uint8_t RES:1;          // Reserved: Always reads 0
 } REG10;
 
+
 typedef struct {
     uint8_t VBUSV:7;        // ADC conversion of VBUS voltage (VBUS), offset: +2600mV
     bool VBUS_GD:1;         // VBUS Good Status
 } REG11;
 
+
 typedef struct {
     uint8_t ICHGR:7;        // ADC conversion of Charge Current (IBAT) when VBAT > VBATSHORT
     uint8_t RES:1;          // Reserved: Always reads 0
 } REG12;
 
+
 typedef struct {
     uint8_t IDPM_LIM:6;     // Input Current Limit in effect while Input Current Optimizer (ICO) is enabled, offset: 100mA (default)
     bool IDPM_STAT:1;       // IINDPM Status
     bool VDPM_STAT:1;       // VINDPM Status
 } REG13;
 
+
 typedef struct {
     uint8_t DEV_REV:2;      // Device Revision
     bool TS_PROFILE:1;      // Temperature Profile
diff --git a/make/base.mk b/make/base.mk
index 2bfe50904..696c4294c 100644
--- a/make/base.mk
+++ b/make/base.mk
@@ -3,7 +3,7 @@ ASM_SOURCES		=
 C_SOURCES		= 
 CPP_SOURCES		= 
 ASSETS			= 
-DEBUG_AGENT		= 
+OPENOCD_OPTS	= 
 
 CFLAGS			= 
 CPPFLAGS		= 
diff --git a/make/rules.mk b/make/rules.mk
index b87981731..c09643eaa 100644
--- a/make/rules.mk
+++ b/make/rules.mk
@@ -55,11 +55,11 @@ $(OBJ_DIR)/%.o: %.cpp $(OBJ_DIR)/BUILD_FLAGS $(ASSETS)
 	@$(CPP) $(CFLAGS) $(CPPFLAGS) -c $< -o $@
 
 $(OBJ_DIR)/flash: $(OBJ_DIR)/$(PROJECT).bin
-	$(DEBUG_AGENT) -c "program $(OBJ_DIR)/$(PROJECT).bin reset exit $(FLASH_ADDRESS)" 
+	openocd $(OPENOCD_OPTS) -c "program $(OBJ_DIR)/$(PROJECT).bin reset exit $(FLASH_ADDRESS)" 
 	touch $@
 
 $(OBJ_DIR)/upload: $(OBJ_DIR)/$(PROJECT).bin
-	dfu-util -D $(OBJ_DIR)/$(PROJECT).bin -a 0 -s $(FLASH_ADDRESS) -S $(DFU_SERIAL)
+	dfu-util -D $(OBJ_DIR)/$(PROJECT).bin -a 0 -s $(FLASH_ADDRESS)
 	touch $@
 
 $(ASSETS): $(ASSETS_SOURCES)
@@ -71,17 +71,14 @@ flash: $(OBJ_DIR)/flash
 upload: $(OBJ_DIR)/upload
 
 debug: flash
-	$(DEBUG_AGENT) & echo $$! > $(OBJ_DIR)/agent.PID
 	arm-none-eabi-gdb \
-		-ex "target extended-remote 127.0.0.1:3333" \
+		-ex 'target extended-remote | openocd -c "gdb_port pipe" $(OPENOCD_OPTS)' \
 		-ex "set confirm off" \
 		-ex "source ../debug/FreeRTOS/FreeRTOS.py" \
 		-ex "source ../debug/PyCortexMDebug/scripts/gdb.py" \
 		-ex "svd_load $(SVD_FILE)" \
 		-ex "compare-sections" \
 		$(OBJ_DIR)/$(PROJECT).elf; \
-	echo "reset; shutdown;" | nc 127.0.0.1 4444 > /dev/null
-	kill `cat $(OBJ_DIR)/agent.PID`; rm $(OBJ_DIR)/agent.PID > /dev/null
 
 bm_debug: flash
 	set -m; blackmagic & echo $$! > $(OBJ_DIR)/agent.PID