diff --git a/Makefile b/Makefile
index 50953d6..25e97dd 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,7 @@
 
 # compile options
 # you can enable ('1') or disable ('0') any/all of these you like
+ENABLE_SWD                    := 0
 ENABLE_AIRCOPY                := 0
 ENABLE_FMRADIO                := 0
 ENABLE_OVERLAY                := 1
@@ -132,10 +133,13 @@ ifeq ($(ENABLE_OVERLAY),1)
 	ASFLAGS += -DENABLE_OVERLAY
 endif
 
-#CFLAGS = -Os -Wall -Werror -mcpu=cortex-m0 -fno-builtin -fshort-enums -fno-delete-null-pointer-checks -std=c11 -MMD
-CFLAGS = -Os -Wall -Werror -mcpu=cortex-m0 -fno-builtin -fshort-enums -fno-delete-null-pointer-checks -std=c11 -MMD -fdata-sections -ffunction-sections
+CFLAGS = -Os -Wall -Werror -mcpu=cortex-m0 -fno-builtin -fshort-enums -fno-delete-null-pointer-checks -std=c11 -MMD
+#CFLAGS = -Os -Wall -Werror -mcpu=cortex-m0 -fno-builtin -fshort-enums -fno-delete-null-pointer-checks -std=c11 -MMD -fdata-sections -ffunction-sections
 CFLAGS += -DPRINTF_INCLUDE_CONFIG_H
 CFLAGS += -DGIT_HASH=\"$(GIT_HASH)\"
+ifeq ($(ENABLE_SWD),1)
+	CFLAGS += -DENABLE_SWD
+endif
 ifeq ($(ENABLE_AIRCOPY),1)
 	CFLAGS += -DENABLE_AIRCOPY
 endif
diff --git a/app/app.c b/app/app.c
index 35acf93..3f4515c 100644
--- a/app/app.c
+++ b/app/app.c
@@ -52,7 +52,9 @@
 #include "misc.h"
 #include "radio.h"
 #include "settings.h"
-#include "sram-overlay.h"
+#if defined(ENABLE_OVERLAY)
+	#include "sram-overlay.h"
+#endif
 #include "ui/battery.h"
 #include "ui/inputbox.h"
 #include "ui/menu.h"
@@ -727,7 +729,8 @@ void APP_CheckRadioInterrupts(void)
 }
 
 void APP_EndTransmission(void)
-{
+{	// back to RX mode
+
 	RADIO_SendEndOfTransmission();
 
 	if (gCurrentVfo->pTX->CodeType != CODE_TYPE_OFF)
@@ -1710,6 +1713,7 @@ void APP_TimeSlice500ms(void)
 		SYSTEM_DelayMs(5);
 
 		RADIO_SetupRegisters(true);
+
 		gRequestDisplayScreen = DISPLAY_MAIN;
 	}
 #endif
diff --git a/app/menu.c b/app/menu.c
index 5f82114..14a6535 100644
--- a/app/menu.c
+++ b/app/menu.c
@@ -16,6 +16,9 @@
 
 #include <string.h>
 
+#if !defined(ENABLE_OVERLAY)
+	#include "ARMCM0.h"
+#endif
 #include "app/dtmf.h"
 #include "app/generic.h"
 #include "app/menu.h"
@@ -31,7 +34,9 @@
 #include "frequencies.h"
 #include "misc.h"
 #include "settings.h"
-#include "sram-overlay.h"
+#if defined(ENABLE_OVERLAY)
+	#include "sram-overlay.h"
+#endif
 #include "ui/inputbox.h"
 #include "ui/menu.h"
 #include "ui/ui.h"
@@ -1234,7 +1239,12 @@ static void MENU_Key_MENU(bool bKeyPressed, bool bKeyHeld)
 								AUDIO_PlaySingleVoice(true);
 							#endif
 							MENU_AcceptSetting();
-							overlay_FLASH_RebootToBootloader();
+	
+							#if defined(ENABLE_OVERLAY)
+								overlay_FLASH_RebootToBootloader();
+							#else
+								NVIC_SystemReset();
+							#endif
 						}
 
 						gFlagAcceptSetting  = true;
diff --git a/app/uart.c b/app/uart.c
index a4f163a..96fc79f 100644
--- a/app/uart.c
+++ b/app/uart.c
@@ -16,6 +16,9 @@
 
 #include <string.h>
 
+#if !defined(ENABLE_OVERLAY)
+	#include "ARMCM0.h"
+#endif
 #ifdef ENABLE_FMRADIO
 	#include "app/fm.h"
 #endif
@@ -32,7 +35,9 @@
 #include "functions.h"
 #include "misc.h"
 #include "settings.h"
-#include "sram-overlay.h"
+#if defined(ENABLE_OVERLAY)
+	#include "sram-overlay.h"
+#endif
 #include "version.h"
 
 #define DMA_INDEX(x, y) (((x) + (y)) % sizeof(UART_DMA_Buffer))
@@ -524,7 +529,11 @@ void UART_HandleCommand(void)
 			break;
 	
 		case 0x05DD:
-			overlay_FLASH_RebootToBootloader();
+			#if defined(ENABLE_OVERLAY)
+				overlay_FLASH_RebootToBootloader();
+			#else
+				NVIC_SystemReset();
+			#endif
 			break;
 	}
 }
diff --git a/bitmaps.c b/bitmaps.c
index 8f3c633..225c96f 100644
--- a/bitmaps.c
+++ b/bitmaps.c
@@ -21,7 +21,6 @@ const uint8_t BITMAP_BatteryLevel1[] =
 	0b00000000,
 	0b00111110,
 	0b00100010,
-	0b01111111,
 	0b01000001,
 	0b01000001,
 	0b01000001,
@@ -35,7 +34,8 @@ const uint8_t BITMAP_BatteryLevel1[] =
 	0b01000001,
 	0b01000001,
 	0b01000001,
-	0b01100011
+	0b01000001,
+	0b01111111
 };
 
 const uint8_t BITMAP_BatteryLevel2[] =
@@ -43,10 +43,7 @@ const uint8_t BITMAP_BatteryLevel2[] =
 	0b00000000,
 	0b00111110,
 	0b00100010,
-	0b01111111,
 	0b01000001,
-	0b01011101,
-	0b01011101,
 	0b01000001,
 	0b01000001,
 	0b01000001,
@@ -57,7 +54,10 @@ const uint8_t BITMAP_BatteryLevel2[] =
 	0b01000001,
 	0b01000001,
 	0b01000001,
-	0b01100011
+	0b01011101,
+	0b01011101,
+	0b01000001,
+	0b01111111
 };
 
 const uint8_t BITMAP_BatteryLevel3[] =
@@ -65,7 +65,13 @@ const uint8_t BITMAP_BatteryLevel3[] =
 	0b00000000,
 	0b00111110,
 	0b00100010,
-	0b01111111,
+	0b01000001,
+	0b01000001,
+	0b01000001,
+	0b01000001,
+	0b01000001,
+	0b01000001,
+	0b01000001,
 	0b01000001,
 	0b01011101,
 	0b01011101,
@@ -73,13 +79,7 @@ const uint8_t BITMAP_BatteryLevel3[] =
 	0b01011101,
 	0b01011101,
 	0b01000001,
-	0b01000001,
-	0b01000001,
-	0b01000001,
-	0b01000001,
-	0b01000001,
-	0b01000001,
-	0b01100011
+	0b01111111
 };
 
 const uint8_t BITMAP_BatteryLevel4[] =
@@ -87,7 +87,10 @@ const uint8_t BITMAP_BatteryLevel4[] =
 	0b00000000,
 	0b00111110,
 	0b00100010,
-	0b01111111,
+	0b01000001,
+	0b01000001,
+	0b01000001,
+	0b01000001,
 	0b01000001,
 	0b01011101,
 	0b01011101,
@@ -98,10 +101,7 @@ const uint8_t BITMAP_BatteryLevel4[] =
 	0b01011101,
 	0b01011101,
 	0b01000001,
-	0b01000001,
-	0b01000001,
-	0b01000001,
-	0b01100011
+	0b01111111
 };
 
 const uint8_t BITMAP_BatteryLevel5[] =
@@ -109,7 +109,10 @@ const uint8_t BITMAP_BatteryLevel5[] =
 	0b00000000,
 	0b00111110,
 	0b00100010,
-	0b01111111,
+	0b01000001,
+	0b01011101,
+	0b01011101,
+	0b01011101,
 	0b01000001,
 	0b01011101,
 	0b01011101,
@@ -120,10 +123,7 @@ const uint8_t BITMAP_BatteryLevel5[] =
 	0b01011101,
 	0b01011101,
 	0b01000001,
-	0b01011101,
-	0b01011101,
-	0b01000001,
-	0b01100011
+	0b01111111
 };
 
 const uint8_t BITMAP_USB_C[] =
diff --git a/board.c b/board.c
index d697913..2b341b8 100644
--- a/board.c
+++ b/board.c
@@ -41,7 +41,9 @@
 #include "helper/battery.h"
 #include "misc.h"
 #include "settings.h"
-#include "sram-overlay.h"
+#if defined(ENABLE_OVERLAY)
+	#include "sram-overlay.h"
+#endif
 
 #ifndef ARRAY_SIZE
 	#define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0]))
@@ -56,236 +58,420 @@ static const uint32_t gDefaultFrequencyTable[] =
 	43350000     //
 };
 
-// BAND1_50MHz
-// BAND2_108MHz
-// BAND3_136MHz
-// BAND4_174MHz
-// BAND5_350MHz
-// BAND6_400MHz
-// BAND7_470MHz
-
-void BOARD_FLASH_Init(void)
-{
-	FLASH_Init(FLASH_READ_MODE_1_CYCLE);
-	FLASH_ConfigureTrimValues();
-	SYSTEM_ConfigureClocks();
-
-	overlay_FLASH_MainClock       = 48000000;
-	overlay_FLASH_ClockMultiplier = 48;
-
-	FLASH_Init(FLASH_READ_MODE_2_CYCLE);
-}
+#if defined(ENABLE_OVERLAY)
+	void BOARD_FLASH_Init(void)
+	{
+		FLASH_Init(FLASH_READ_MODE_1_CYCLE);
+		FLASH_ConfigureTrimValues();
+		SYSTEM_ConfigureClocks();
+	
+		overlay_FLASH_MainClock       = 48000000;
+		overlay_FLASH_ClockMultiplier = 48;
+	
+		FLASH_Init(FLASH_READ_MODE_2_CYCLE);
+	}
+#endif
 
 void BOARD_GPIO_Init(void)
 {
 	GPIOA->DIR |= 0
+		// A7 = UART1 TX default as OUTPUT from bootloader!
+		// A8 = UART1 RX default as INPUT from bootloader!
+		// Key pad + I2C
 		| GPIO_DIR_10_BITS_OUTPUT
+		// Key pad + I2C
 		| GPIO_DIR_11_BITS_OUTPUT
+		// Key pad + Voice chip
 		| GPIO_DIR_12_BITS_OUTPUT
-		| GPIO_DIR_13_BITS_OUTPUT;
+		// Key pad + Voice chip
+		| GPIO_DIR_13_BITS_OUTPUT
+		;
 	GPIOA->DIR &= ~(0
-		| GPIO_DIR_3_MASK
-		| GPIO_DIR_4_MASK
-		| GPIO_DIR_5_MASK
-		| GPIO_DIR_6_MASK);
+		// Key pad
+		| GPIO_DIR_3_MASK // INPUT
+		// Key pad
+		| GPIO_DIR_4_MASK // INPUT
+		// Key pad
+		| GPIO_DIR_5_MASK // INPUT
+		// Key pad
+		| GPIO_DIR_6_MASK // INPUT
+		);
 	GPIOB->DIR |= 0
+		// Back light
 		| GPIO_DIR_6_BITS_OUTPUT
+		// ST7565
 		| GPIO_DIR_9_BITS_OUTPUT
+		// ST7565 + SWD IO
 		| GPIO_DIR_11_BITS_OUTPUT
-		| GPIO_DIR_15_BITS_OUTPUT;
+		// B14 = SWD_CLK assumed INPUT by default
+		// BK1080
+		| GPIO_DIR_15_BITS_OUTPUT
+		;
 	GPIOC->DIR |= 0
+		// BK4819 SCN
 		| GPIO_DIR_0_BITS_OUTPUT
+		// BK4819 SCL
 		| GPIO_DIR_1_BITS_OUTPUT
+		// BK4819 SDA
 		| GPIO_DIR_2_BITS_OUTPUT
+		// Flash light
 		| GPIO_DIR_3_BITS_OUTPUT
-		| GPIO_DIR_4_BITS_OUTPUT;
-	GPIOC->DIR &= ~(GPIO_DIR_5_MASK);
+		// Speaker
+		| GPIO_DIR_4_BITS_OUTPUT
+		;
+	GPIOC->DIR &= ~(0
+		// PTT button
+		| GPIO_DIR_5_MASK // INPUT
+		);
 
-	GPIO_SetBit(&GPIOB->DATA, GPIOB_PIN_BK1080);
+	#if defined(ENABLE_FMRADIO)
+		GPIO_SetBit(&GPIOB->DATA, GPIOB_PIN_BK1080);
+	#endif
 }
 
 void BOARD_PORTCON_Init(void)
 {
-	// TODO: Need to redo these macros to make more sense.
-
 	// PORT A pin selection
 
-	PORTCON_PORTA_SEL0 &= 0
-		| PORTCON_PORTA_SEL0_A0_MASK
-		| PORTCON_PORTA_SEL0_A1_MASK
-		| PORTCON_PORTA_SEL0_A2_MASK
-		| PORTCON_PORTA_SEL0_A7_MASK;
+	PORTCON_PORTA_SEL0 &= ~(0
+		// Key pad
+		| PORTCON_PORTA_SEL0_A3_MASK
+		// Key pad
+		| PORTCON_PORTA_SEL0_A4_MASK
+		// Key pad
+		| PORTCON_PORTA_SEL0_A5_MASK
+		// Key pad
+		| PORTCON_PORTA_SEL0_A6_MASK
+		);
 	PORTCON_PORTA_SEL0 |= 0
-		| PORTCON_PORTA_SEL0_A0_BITS_GPIOA0
-		| PORTCON_PORTA_SEL0_A1_BITS_GPIOA1
-		| PORTCON_PORTA_SEL0_A2_BITS_GPIOA2
-		| PORTCON_PORTA_SEL0_A7_BITS_UART1_TX;
+		// Key pad
+		| PORTCON_PORTA_SEL0_A3_BITS_GPIOA3
+		// Key pad
+		| PORTCON_PORTA_SEL0_A4_BITS_GPIOA4
+		// Key pad
+		| PORTCON_PORTA_SEL0_A5_BITS_GPIOA5
+		// Key pad
+		| PORTCON_PORTA_SEL0_A6_BITS_GPIOA6
+		// UART1 TX, wasn't cleared in previous step / relying on default value!
+		| PORTCON_PORTA_SEL0_A7_BITS_UART1_TX
+		;
 
-	PORTCON_PORTA_SEL1 &= 0
-		| PORTCON_PORTA_SEL1_A8_MASK
-		| PORTCON_PORTA_SEL1_A9_MASK
-		| PORTCON_PORTA_SEL1_A14_MASK
-		| PORTCON_PORTA_SEL1_A15_MASK;
+	PORTCON_PORTA_SEL1 &= ~(0
+		// Key pad + I2C
+		| PORTCON_PORTA_SEL1_A10_MASK
+		// Key pad + I2C
+		| PORTCON_PORTA_SEL1_A11_MASK
+		// Key pad + Voice chip
+		| PORTCON_PORTA_SEL1_A12_MASK
+		// Key pad + Voice chip
+		| PORTCON_PORTA_SEL1_A13_MASK
+		);
 	PORTCON_PORTA_SEL1 |= 0
+		// UART1 RX, wasn't cleared in previous step / relying on default value!
 		| PORTCON_PORTA_SEL1_A8_BITS_UART1_RX
+		// Battery voltage, wasn't cleared in previous step / relying on default value!
 		| PORTCON_PORTA_SEL1_A9_BITS_SARADC_CH4
+		// Key pad + I2C
+		| PORTCON_PORTA_SEL1_A10_BITS_GPIOA10
+		// Key pad + I2C
+		| PORTCON_PORTA_SEL1_A11_BITS_GPIOA11
+		// Key pad + Voice chip
+		| PORTCON_PORTA_SEL1_A12_BITS_GPIOA12
+		// Key pad + Voice chip
+		| PORTCON_PORTA_SEL1_A13_BITS_GPIOA13
+		// Battery Current, wasn't cleared in previous step / relying on default value!
 		| PORTCON_PORTA_SEL1_A14_BITS_SARADC_CH9
-		| PORTCON_PORTA_SEL1_A15_BITS_GPIOA15;
+		;
 
 	// PORT B pin selection
 
-	PORTCON_PORTB_SEL0 &= 0
-		| PORTCON_PORTB_SEL0_B0_MASK
-		| PORTCON_PORTB_SEL0_B1_MASK
-		| PORTCON_PORTB_SEL0_B2_MASK
-		| PORTCON_PORTB_SEL0_B3_MASK
-		| PORTCON_PORTB_SEL0_B4_MASK
-		| PORTCON_PORTB_SEL0_B5_MASK;
+	PORTCON_PORTB_SEL0 &= ~(0
+		// Back light
+		| PORTCON_PORTB_SEL0_B6_MASK
+		// SPI0 SSN
+		| PORTCON_PORTB_SEL0_B7_MASK
+		);
 	PORTCON_PORTB_SEL0 |= 0
-		| PORTCON_PORTB_SEL0_B0_BITS_GPIOB0
-		| PORTCON_PORTB_SEL0_B1_BITS_GPIOB1
-		| PORTCON_PORTB_SEL0_B2_BITS_GPIOB2
-		| PORTCON_PORTB_SEL0_B3_BITS_GPIOB3
-		| PORTCON_PORTB_SEL0_B4_BITS_GPIOB4
-		| PORTCON_PORTB_SEL0_B5_BITS_GPIOB5
-		| PORTCON_PORTB_SEL0_B7_BITS_SPI0_SSN;
+		// Back light
+		| PORTCON_PORTB_SEL0_B6_BITS_GPIOB6
+		// SPI0 SSN
+		| PORTCON_PORTB_SEL0_B7_BITS_SPI0_SSN
+		;
 
-	PORTCON_PORTB_SEL1 &= 0
-		| PORTCON_PORTB_SEL1_B8_MASK
-		| PORTCON_PORTB_SEL1_B10_MASK
-		| PORTCON_PORTB_SEL1_B12_MASK
-		| PORTCON_PORTB_SEL1_B13_MASK;
+	PORTCON_PORTB_SEL1 &= ~(0
+		// ST7565
+		| PORTCON_PORTB_SEL1_B9_MASK
+		// ST7565 + SWD IO
+		| PORTCON_PORTB_SEL1_B11_MASK
+		// SWD CLK
+		| PORTCON_PORTB_SEL1_B14_MASK
+		// BK1080
+		| PORTCON_PORTB_SEL1_B15_MASK
+		);
 	PORTCON_PORTB_SEL1 |= 0
+		// SPI0 CLK, wasn't cleared in previous step / relying on default value!
 		| PORTCON_PORTB_SEL1_B8_BITS_SPI0_CLK
+		// ST7565
+		| PORTCON_PORTB_SEL1_B9_BITS_GPIOB9
+		// SPI0 MOSI, wasn't cleared in previous step / relying on default value!
 		| PORTCON_PORTB_SEL1_B10_BITS_SPI0_MOSI
+#if defined(ENABLE_SWD)
+		// SWD IO
 		| PORTCON_PORTB_SEL1_B11_BITS_SWDIO
-		| PORTCON_PORTB_SEL1_B12_BITS_GPIOB12
-		| PORTCON_PORTB_SEL1_B13_BITS_GPIOB13
-		| PORTCON_PORTB_SEL1_B14_BITS_SWCLK;
+		// SWD CLK
+		| PORTCON_PORTB_SEL1_B14_BITS_SWCLK
+#else
+		// ST7565
+		| PORTCON_PORTB_SEL1_B11_BITS_GPIOB11
+#endif
+		;
 
 	// PORT C pin selection
 
-	PORTCON_PORTC_SEL0 &= 0
-		| PORTCON_PORTC_SEL0_C6_MASK
-		| PORTCON_PORTC_SEL0_C7_MASK;
+	PORTCON_PORTC_SEL0 &= ~(0
+		// BK4819 SCN
+		| PORTCON_PORTC_SEL0_C0_MASK
+		// BK4819 SCL
+		| PORTCON_PORTC_SEL0_C1_MASK
+		// BK4819 SDA
+		| PORTCON_PORTC_SEL0_C2_MASK
+		// Flash light
+		| PORTCON_PORTC_SEL0_C3_MASK
+		// Speaker
+		| PORTCON_PORTC_SEL0_C4_MASK
+		// PTT button
+		| PORTCON_PORTC_SEL0_C5_MASK
+		);
 
 	// PORT A pin configuration
 
 	PORTCON_PORTA_IE |= 0
+		// Keypad
 		| PORTCON_PORTA_IE_A3_BITS_ENABLE
+		// Keypad
 		| PORTCON_PORTA_IE_A4_BITS_ENABLE
+		// Keypad
 		| PORTCON_PORTA_IE_A5_BITS_ENABLE
+		// Keypad
 		| PORTCON_PORTA_IE_A6_BITS_ENABLE
-		| PORTCON_PORTA_IE_A8_BITS_ENABLE;
+		// A7 = UART1 TX disabled by default
+		// UART1 RX
+		| PORTCON_PORTA_IE_A8_BITS_ENABLE
+		;
 	PORTCON_PORTA_IE &= ~(0
+		// Keypad + I2C
 		| PORTCON_PORTA_IE_A10_MASK
+		// Keypad + I2C
 		| PORTCON_PORTA_IE_A11_MASK
+		// Keypad + Voice chip
 		| PORTCON_PORTA_IE_A12_MASK
-		| PORTCON_PORTA_IE_A13_MASK);
+		// Keypad + Voice chip
+		| PORTCON_PORTA_IE_A13_MASK
+		);
 
 	PORTCON_PORTA_PU |= 0
+		// Keypad
 		| PORTCON_PORTA_PU_A3_BITS_ENABLE
+		// Keypad
 		| PORTCON_PORTA_PU_A4_BITS_ENABLE
+		// Keypad
 		| PORTCON_PORTA_PU_A5_BITS_ENABLE
-		| PORTCON_PORTA_PU_A6_BITS_ENABLE;
+		// Keypad
+		| PORTCON_PORTA_PU_A6_BITS_ENABLE
+		;
 	PORTCON_PORTA_PU &= ~(0
+		// Keypad + I2C
 		| PORTCON_PORTA_PU_A10_MASK
+		// Keypad + I2C
 		| PORTCON_PORTA_PU_A11_MASK
+		// Keypad + Voice chip
 		| PORTCON_PORTA_PU_A12_MASK
-		| PORTCON_PORTA_PU_A13_MASK);
+		// Keypad + Voice chip
+		| PORTCON_PORTA_PU_A13_MASK
+		);
 
 	PORTCON_PORTA_PD &= ~(0
+		// Keypad
 		| PORTCON_PORTA_PD_A3_MASK
+		// Keypad
 		| PORTCON_PORTA_PD_A4_MASK
+		// Keypad
 		| PORTCON_PORTA_PD_A5_MASK
+		// Keypad
 		| PORTCON_PORTA_PD_A6_MASK
+		// Keypad + I2C
 		| PORTCON_PORTA_PD_A10_MASK
+		// Keypad + I2C
 		| PORTCON_PORTA_PD_A11_MASK
+		// Keypad + Voice chip
 		| PORTCON_PORTA_PD_A12_MASK
-		| PORTCON_PORTA_PD_A13_MASK);
+		// Keypad + Voice chip
+		| PORTCON_PORTA_PD_A13_MASK
+		);
 
 	PORTCON_PORTA_OD |= 0
+		// Keypad
 		| PORTCON_PORTA_OD_A3_BITS_ENABLE
+		// Keypad
 		| PORTCON_PORTA_OD_A4_BITS_ENABLE
+		// Keypad
 		| PORTCON_PORTA_OD_A5_BITS_ENABLE
-		| PORTCON_PORTA_OD_A6_BITS_ENABLE;
+		// Keypad
+		| PORTCON_PORTA_OD_A6_BITS_ENABLE
+		;
 	PORTCON_PORTA_OD &= ~(0
+		// Keypad + I2C
 		| PORTCON_PORTA_OD_A10_MASK
+		// Keypad + I2C
 		| PORTCON_PORTA_OD_A11_MASK
+		// Keypad + Voice chip
 		| PORTCON_PORTA_OD_A12_MASK
-		| PORTCON_PORTA_OD_A13_MASK);
+		// Keypad + Voice chip
+		| PORTCON_PORTA_OD_A13_MASK
+		);
 
 	// PORT B pin configuration
 
 	PORTCON_PORTB_IE |= 0
-		| PORTCON_PORTB_IE_B14_BITS_ENABLE;
+		| PORTCON_PORTB_IE_B14_BITS_ENABLE
+		;
 	PORTCON_PORTB_IE &= ~(0
+		// Back light
 		| PORTCON_PORTB_IE_B6_MASK
+		// UART1
 		| PORTCON_PORTB_IE_B7_MASK
 		| PORTCON_PORTB_IE_B8_MASK
+		// ST7565
 		| PORTCON_PORTB_IE_B9_MASK
+		// SPI0 MOSI
 		| PORTCON_PORTB_IE_B10_MASK
-		| PORTCON_PORTB_IE_B15_MASK);
+#if !defined(ENABLE_SWD)
+		// ST7565
+		| PORTCON_PORTB_IE_B11_MASK
+#endif
+		// BK1080
+		| PORTCON_PORTB_IE_B15_MASK
+		);
 
 	PORTCON_PORTB_PU &= ~(0
+		// Back light
 		| PORTCON_PORTB_PU_B6_MASK
+		// ST7565
 		| PORTCON_PORTB_PU_B9_MASK
+		// ST7565 + SWD IO
 		| PORTCON_PORTB_PU_B11_MASK
+		// SWD CLK
 		| PORTCON_PORTB_PU_B14_MASK
-		| PORTCON_PORTB_PU_B15_MASK);
+		// BK1080
+		| PORTCON_PORTB_PU_B15_MASK
+		);
 
 	PORTCON_PORTB_PD &= ~(0
+		// Back light
 		| PORTCON_PORTB_PD_B6_MASK
+		// ST7565
 		| PORTCON_PORTB_PD_B9_MASK
+		// ST7565 + SWD IO
 		| PORTCON_PORTB_PD_B11_MASK
+		// SWD CLK
 		| PORTCON_PORTB_PD_B14_MASK
-		| PORTCON_PORTB_PD_B15_MASK);
+		// BK1080
+		| PORTCON_PORTB_PD_B15_MASK
+		);
 
 	PORTCON_PORTB_OD &= ~(0
+		// Back light
 		| PORTCON_PORTB_OD_B6_MASK
+		// ST7565
 		| PORTCON_PORTB_OD_B9_MASK
+		// ST7565 + SWD IO
 		| PORTCON_PORTB_OD_B11_MASK
-		| PORTCON_PORTB_OD_B15_MASK);
+		// BK1080
+		| PORTCON_PORTB_OD_B15_MASK
+		);
 
 	PORTCON_PORTB_OD |= 0
-		| PORTCON_PORTB_OD_B14_BITS_ENABLE;
+		// SWD CLK
+		| PORTCON_PORTB_OD_B14_BITS_ENABLE
+		;
 
 	// PORT C pin configuration
 
 	PORTCON_PORTC_IE |= 0
-		| PORTCON_PORTC_IE_C5_BITS_ENABLE;
+		// PTT button
+		| PORTCON_PORTC_IE_C5_BITS_ENABLE
+		;
 	PORTCON_PORTC_IE &= ~(0
+		// BK4819 SCN
 		| PORTCON_PORTC_IE_C0_MASK
+		// BK4819 SCL
 		| PORTCON_PORTC_IE_C1_MASK
+		// BK4819 SDA
 		| PORTCON_PORTC_IE_C2_MASK
+		// Flash Light
 		| PORTCON_PORTC_IE_C3_MASK
-		| PORTCON_PORTC_IE_C4_MASK);
+		// Speaker
+		| PORTCON_PORTC_IE_C4_MASK
+		);
 
 	PORTCON_PORTC_PU |= 0
-		| PORTCON_PORTC_PU_C5_BITS_ENABLE;
+		// PTT button
+		| PORTCON_PORTC_PU_C5_BITS_ENABLE
+		;
 	PORTCON_PORTC_PU &= ~(0
+		// BK4819 SCN
 		| PORTCON_PORTC_PU_C0_MASK
+		// BK4819 SCL
 		| PORTCON_PORTC_PU_C1_MASK
+		// BK4819 SDA
 		| PORTCON_PORTC_PU_C2_MASK
+		// Flash Light
 		| PORTCON_PORTC_PU_C3_MASK
-		| PORTCON_PORTC_PU_C4_MASK);
+		// Speaker
+		| PORTCON_PORTC_PU_C4_MASK
+		);
 
 	PORTCON_PORTC_PD &= ~(0
+		// BK4819 SCN
 		| PORTCON_PORTC_PD_C0_MASK
+		// BK4819 SCL
 		| PORTCON_PORTC_PD_C1_MASK
+		// BK4819 SDA
 		| PORTCON_PORTC_PD_C2_MASK
+		// Flash Light
 		| PORTCON_PORTC_PD_C3_MASK
+		// Speaker
 		| PORTCON_PORTC_PD_C4_MASK
-		| PORTCON_PORTC_PD_C5_MASK);
+		// PTT Button
+		| PORTCON_PORTC_PD_C5_MASK
+		);
 
 	PORTCON_PORTC_OD &= ~(0
+		// BK4819 SCN
 		| PORTCON_PORTC_OD_C0_MASK
+		// BK4819 SCL
 		| PORTCON_PORTC_OD_C1_MASK
+		// BK4819 SDA
 		| PORTCON_PORTC_OD_C2_MASK
+		// Flash Light
 		| PORTCON_PORTC_OD_C3_MASK
-		| PORTCON_PORTC_OD_C4_MASK);
+		// Speaker
+		| PORTCON_PORTC_OD_C4_MASK
+		);
 	PORTCON_PORTC_OD |= 0
-		| PORTCON_PORTC_OD_C5_BITS_ENABLE;
+		// BK4819 SCN
+		| PORTCON_PORTC_OD_C0_BITS_DISABLE
+		// BK4819 SCL
+		| PORTCON_PORTC_OD_C1_BITS_DISABLE
+		// BK4819 SDA
+		| PORTCON_PORTC_OD_C2_BITS_DISABLE
+		// Flash Light
+		| PORTCON_PORTC_OD_C3_BITS_DISABLE
+		// Speaker
+		| PORTCON_PORTC_OD_C4_BITS_DISABLE
+		// PTT button
+		| PORTCON_PORTC_OD_C5_BITS_ENABLE
+		;
 }
 
 void BOARD_ADC_Init(void)
diff --git a/driver/bk4819.c b/driver/bk4819.c
index 02ee2fa..1d4ac39 100644
--- a/driver/bk4819.c
+++ b/driver/bk4819.c
@@ -29,9 +29,8 @@ bool gRxIdleMode;
 
 __inline uint16_t scale_freq(const uint16_t freq)
 {
-//	return (uint16_t)(freq * 10.32444);   // argh - floating point
-//	return ((uint32_t)freq * 1032444u) / 100000u;
-	return ((uint32_t)freq * 1353245u) >> 17;
+//	return (((uint32_t)freq * 1032444u) + 50000u) / 100000u;   // with rounding
+	return (((uint32_t)freq * 1353245u) + (1u << 16)) >> 17;   // with rounding
 }
 
 void BK4819_Init(void)
@@ -754,72 +753,33 @@ void BK4819_EnableTXLink(void)
 
 void BK4819_PlayDTMF(char Code)
 {
+	uint16_t tone1 = 0;
+	uint16_t tone2 = 0;
+
 	switch (Code)
 	{
-		case '0':
-			BK4819_WriteRegister(BK4819_REG_71, 0x25F3);
-			BK4819_WriteRegister(BK4819_REG_72, 0x35E1);
-			break;
-		case '1':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1C1C);
-			BK4819_WriteRegister(BK4819_REG_72, 0x30C2);
-			break;
-		case '2':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1C1C);
-			BK4819_WriteRegister(BK4819_REG_72, 0x35E1);
-			break;
-		case '3':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1C1C);
-			BK4819_WriteRegister(BK4819_REG_72, 0x3B91);
-			break;
-		case '4':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1F0E);
-			BK4819_WriteRegister(BK4819_REG_72, 0x30C2);
-			break;
-		case '5':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1F0E);
-			BK4819_WriteRegister(BK4819_REG_72, 0x35E1);
-			break;
-		case '6':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1F0E);
-			BK4819_WriteRegister(BK4819_REG_72, 0x3B91);
-			break;
-		case '7':
-			BK4819_WriteRegister(BK4819_REG_71, 0x225C);
-			BK4819_WriteRegister(BK4819_REG_72, 0x30C2);
-			break;
-		case '8':
-			BK4819_WriteRegister(BK4819_REG_71, 0x225c);
-			BK4819_WriteRegister(BK4819_REG_72, 0x35E1);
-			break;
-		case '9':
-			BK4819_WriteRegister(BK4819_REG_71, 0x225C);
-			BK4819_WriteRegister(BK4819_REG_72, 0x3B91);
-			break;
-		case 'A':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1C1C);
-			BK4819_WriteRegister(BK4819_REG_72, 0x41DC);
-			break;
-		case 'B':
-			BK4819_WriteRegister(BK4819_REG_71, 0x1F0E);
-			BK4819_WriteRegister(BK4819_REG_72, 0x41DC);
-			break;
-		case 'C':
-			BK4819_WriteRegister(BK4819_REG_71, 0x225C);
-			BK4819_WriteRegister(BK4819_REG_72, 0x41DC);
-			break;
-		case 'D':
-			BK4819_WriteRegister(BK4819_REG_71, 0x25F3);
-			BK4819_WriteRegister(BK4819_REG_72, 0x41DC);
-			break;
-		case '*':
-			BK4819_WriteRegister(BK4819_REG_71, 0x25F3);
-			BK4819_WriteRegister(BK4819_REG_72, 0x30C2);
-			break;
-		case '#':
-			BK4819_WriteRegister(BK4819_REG_71, 0x25F3);
-			BK4819_WriteRegister(BK4819_REG_72, 0x3B91);
-			break;
+		case '0': tone1 = 9715; tone2 = 13793; break;   //  941Hz  1336Hz
+		case '1': tone1 = 7196; tone2 = 12482; break;   //  679Hz  1209Hz
+		case '2': tone1 = 7196; tone2 = 13793; break;   //  697Hz  1336Hz
+		case '3': tone1 = 7196; tone2 = 15249; break;   //  679Hz  1477Hz
+		case '4': tone1 = 7950; tone2 = 12482; break;   //  770Hz  1209Hz
+		case '5': tone1 = 7950; tone2 = 13793; break;   //  770Hz  1336Hz
+		case '6': tone1 = 7950; tone2 = 15249; break;   //  770Hz  1477Hz
+		case '7': tone1 = 8796; tone2 = 12482; break;   //  852Hz  1209Hz
+		case '8': tone1 = 8796; tone2 = 13793; break;   //  852Hz  1336Hz
+		case '9': tone1 = 8796; tone2 = 15249; break;   //  852Hz  1477Hz
+		case 'A': tone1 = 7196; tone2 = 16860; break;   //  679Hz  1633Hz
+		case 'B': tone1 = 7950; tone2 = 16860; break;   //  770Hz  1633Hz
+		case 'C': tone1 = 8796; tone2 = 16860; break;   //  852Hz  1633Hz
+		case 'D': tone1 = 9715; tone2 = 16860; break;   //  941Hz  1633Hz
+		case '*': tone1 = 9715; tone2 = 12482; break;   //  941Hz  1209Hz
+		case '#': tone1 = 9715; tone2 = 15249; break;   //  941Hz  1477Hz
+	}
+	
+	if (tone1 > 0 && tone2 > 0)
+	{
+		BK4819_WriteRegister(BK4819_REG_71, tone1);
+		BK4819_WriteRegister(BK4819_REG_72, tone2);
 	}
 }
 
@@ -1117,30 +1077,36 @@ void BK4819_PrepareFSKReceive(void)
 
 void BK4819_PlayRoger(void)
 {
+	#if 0
+		const uint32_t tone1_Hz = 500;
+		const uint32_t tone2_Hz = 700;
+	#else
+		// motorola type
+		const uint32_t tone1_Hz = 1540;
+		const uint32_t tone2_Hz = 1310;
+	#endif
+	
 	BK4819_EnterTxMute();
-
 	BK4819_SetAF(BK4819_AF_MUTE);
-	BK4819_WriteRegister(BK4819_REG_70, 0xE000);
+	BK4819_WriteRegister(BK4819_REG_70, 0xE000);  // 1110 0000 0000 0000 
 
 	BK4819_EnableTXLink();
-
 	SYSTEM_DelayMs(50);
 
-	BK4819_WriteRegister(BK4819_REG_71, 0x142A);
+	BK4819_WriteRegister(BK4819_REG_71, scale_freq(tone1_Hz));
+
 	BK4819_ExitTxMute();
-
 	SYSTEM_DelayMs(80);
-
 	BK4819_EnterTxMute();
-	BK4819_WriteRegister(BK4819_REG_71, 0x1C3B);
+
+	BK4819_WriteRegister(BK4819_REG_71, scale_freq(tone2_Hz));
+
 	BK4819_ExitTxMute();
-
 	SYSTEM_DelayMs(80);
-
 	BK4819_EnterTxMute();
 
 	BK4819_WriteRegister(BK4819_REG_70, 0x0000);
-	BK4819_WriteRegister(BK4819_REG_30, 0xC1FE);
+	BK4819_WriteRegister(BK4819_REG_30, 0xC1FE);   // 1 1 0000 0 1 1111 1 1 1 0
 }
 
 void BK4819_PlayRogerMDC(void)
diff --git a/driver/st7565.c b/driver/st7565.c
index 46fdd25..b5a1df9 100644
--- a/driver/st7565.c
+++ b/driver/st7565.c
@@ -151,7 +151,10 @@ void ST7565_Init(void)
 	ST7565_WriteByte(0xA2);   // bias 9
 	ST7565_WriteByte(0xC0);   // com normal
 	ST7565_WriteByte(0xA1);   // reverse ?
-	ST7565_WriteByte(0xA6);   // normal ?
+	
+	ST7565_WriteByte(0xA6);   // normal screen ?
+//	ST7565_WriteByte(0xA7);   // inverse screen ?
+
 	ST7565_WriteByte(0xA4);   // all points normal
 	ST7565_WriteByte(0x24);   //
 	ST7565_WriteByte(0x81);   // volume first ?
diff --git a/firmware b/firmware
index bd1f835..eb197f0 100644
Binary files a/firmware and b/firmware differ
diff --git a/firmware.bin b/firmware.bin
index 963c3c0..a757ad8 100644
Binary files a/firmware.bin and b/firmware.bin differ
diff --git a/firmware.packed.bin b/firmware.packed.bin
index fcfe12d..c87eb3b 100644
Binary files a/firmware.packed.bin and b/firmware.packed.bin differ
diff --git a/functions.c b/functions.c
index d93c738..cfcea61 100644
--- a/functions.c
+++ b/functions.c
@@ -129,7 +129,7 @@ void FUNCTION_Select(FUNCTION_Type_t Function)
 			BK4819_Sleep();
 			BK4819_ToggleGpioOut(BK4819_GPIO6_PIN2, false);
 			gBatterySaveCountdownExpired = false;
-			gUpdateStatus = true;
+			gUpdateStatus                = true;
 			GUI_SelectNextDisplay(DISPLAY_MAIN);
 			return;
 	
diff --git a/radio.c b/radio.c
index 0a1505f..ed44d0c 100644
--- a/radio.c
+++ b/radio.c
@@ -569,21 +569,18 @@ void RADIO_SetupRegisters(bool bSwitchToFunction0)
 	BK4819_SetFrequency(Frequency);
 
 	BK4819_SetupSquelch(
-		gRxVfo->SquelchOpenRSSIThresh,
-		gRxVfo->SquelchCloseRSSIThresh,
-		gRxVfo->SquelchOpenNoiseThresh,
-		gRxVfo->SquelchCloseNoiseThresh,
-		gRxVfo->SquelchCloseGlitchThresh,
-		gRxVfo->SquelchOpenGlitchThresh);
+		gRxVfo->SquelchOpenRSSIThresh,    gRxVfo->SquelchCloseRSSIThresh,
+		gRxVfo->SquelchOpenNoiseThresh,   gRxVfo->SquelchCloseNoiseThresh,
+		gRxVfo->SquelchCloseGlitchThresh, gRxVfo->SquelchOpenGlitchThresh);
 
 	BK4819_PickRXFilterPathBasedOnFrequency(Frequency);
 
 	BK4819_ToggleGpioOut(BK4819_GPIO6_PIN2, true);
 
 	// AF Rx Gain and DAC
-	BK4819_WriteRegister(BK4819_REG_48, 0xB3A8);
+	BK4819_WriteRegister(BK4819_REG_48, 0xB3A8);  // 1011 00 111010 1000
 
-	InterruptMask = 0 | BK4819_REG_3F_SQUELCH_FOUND | BK4819_REG_3F_SQUELCH_LOST;
+	InterruptMask = BK4819_REG_3F_SQUELCH_FOUND | BK4819_REG_3F_SQUELCH_LOST;
 
 	#ifdef ENABLE_NOAA
 		if (IS_NOT_NOAA_CHANNEL(gRxVfo->CHANNEL_SAVE))
@@ -681,13 +678,13 @@ void RADIO_SetupRegisters(bool bSwitchToFunction0)
 	#endif
 	{
 		BK4819_EnableVox(gEeprom.VOX1_THRESHOLD, gEeprom.VOX0_THRESHOLD);
-		InterruptMask |= 0 | BK4819_REG_3F_VOX_FOUND | BK4819_REG_3F_VOX_LOST;
+		InterruptMask |= BK4819_REG_3F_VOX_FOUND | BK4819_REG_3F_VOX_LOST;
 	}
 	else
 		BK4819_DisableVox();
 
 	#ifdef ENABLE_COMPANDER
-		if (gRxVfo->Compander)
+		if (gRxVfo->Compander && !gRxVfo->IsAM)
 			BK4819_EnableCompander();
 		else
 			BK4819_DisableCompander();
@@ -775,7 +772,7 @@ void RADIO_SetTxParameters(void)
 	BK4819_SetFrequency(gCurrentVfo->pTX->Frequency);
 
 	#ifdef ENABLE_COMPANDER
-		if (gCurrentVfo->Compander)
+		if (gCurrentVfo->Compander && !gCurrentVfo->IsAM)
 			BK4819_EnableCompander();
 		else
 			BK4819_DisableCompander();
diff --git a/start.S b/start.S
index 7399bb9..eb29dd7 100644
--- a/start.S
+++ b/start.S
@@ -245,7 +245,9 @@ HandlerReset:
 	mov	sp, r0
 	bl	DATA_Init
 	bl	BSS_Init
+#if defined(ENABLE_OVERLAY)
 	bl	BOARD_FLASH_Init
+#endif
 	bl	Main
 	b 	.