diff --git a/Makefile b/Makefile
index 24fe08f..2189409 100755
--- a/Makefile
+++ b/Makefile
@@ -84,6 +84,7 @@ SIZE = arm-none-eabi-size
 ASFLAGS = -mcpu=cortex-m0
 CFLAGS = -Os -Wall -Werror -mcpu=cortex-m0 -fno-builtin -fshort-enums -std=c11 -MMD
 CFLAGS += -DPRINTF_INCLUDE_CONFIG_H
+CFLAGS_LTO = -flto
 LDFLAGS = -mcpu=cortex-m0 -nostartfiles -Wl,-T,firmware.ld
 
 OVERLAY_CFLAGS = $(CFLAGS) -fno-inline -fno-toplevel-reorder
@@ -138,7 +139,7 @@ $(TARGET): $(OBJS)
 bsp/dp32g030/%.h: hardware/dp32g030/%.def
 
 %.o: %.c | $(BSP_HEADERS)
-	$(CC) $(CFLAGS) $(INC) -c $< -o $@
+	$(CC) $(CFLAGS) $(CFLAGS_LTO) $(INC) -c $< -o $@
 
 %.o: %.S
 	$(AS) $(ASFLAGS) $< -o $@
diff --git a/app/app.c b/app/app.c
index bbc623a..578f668 100644
--- a/app/app.c
+++ b/app/app.c
@@ -540,7 +540,7 @@ void APP_CheckRadioInterrupts(void)
 		Mask = BK4819_GetRegister(BK4819_REG_02);
 		if (Mask & BK4819_REG_02_DTMF_5TONE_FOUND) {
 			gDTMF_RequestPending = true;
-			g_20000442 = 5;
+			gDTMF_RecvTimeout = 5;
 			if (gDTMF_WriteIndex > 15) {
 				uint8_t i;
 				for (i = 0; i < sizeof(gDTMF_Received) - 1; i++) {
@@ -1225,9 +1225,9 @@ LAB_00004b08:
 		}
 	}
 
-	if (g_20000442) {
-		g_20000442--;
-		if (g_20000442 == 0) {
+	if (gDTMF_RecvTimeout) {
+		gDTMF_RecvTimeout--;
+		if (gDTMF_RecvTimeout == 0) {
 			gDTMF_WriteIndex = 0;
 			memset(gDTMF_Received, 0, sizeof(gDTMF_Received));
 		}
diff --git a/app/dtmf.c b/app/dtmf.c
index df4d1c0..8fc668a 100644
--- a/app/dtmf.c
+++ b/app/dtmf.c
@@ -16,7 +16,11 @@
 
 #include <string.h>
 #include "app/fm.h"
+#include "bsp/dp32g030/gpio.h"
+#include "driver/bk4819.h"
 #include "driver/eeprom.h"
+#include "driver/gpio.h"
+#include "driver/system.h"
 #include "dtmf.h"
 #include "external/printf/printf.h"
 #include "misc.h"
@@ -28,8 +32,8 @@ char gDTMF_InputBox[15];
 char gDTMF_Received[16];
 bool gIsDtmfContactValid;
 char gDTMF_ID[4];
-char gDTMF_Contact0[4];
-char gDTMF_Contact1[4];
+char gDTMF_Caller[4];
+char gDTMF_Callee[4];
 uint8_t gDTMF_State;
 bool gDTMF_DecodeRing;
 uint8_t gDTMF_DecodeRingCountdown;
@@ -38,6 +42,7 @@ uint8_t gDTMF_WriteIndex;
 uint8_t gDTMF_AUTO_RESET_TIME;
 uint8_t gDTMF_InputIndex;
 bool gDTMF_InputMode;
+uint8_t gDTMF_RecvTimeout;
 
 bool DTMF_ValidateCodes(char *pCode, uint8_t Size)
 {
@@ -210,7 +215,7 @@ void DTMF_HandleRequest(void)
 	}
 
 	if (gDTMF_WriteIndex >= 2) {
-		if (DTMF_CompareMessage(gDTMF_Received + gDTMF_WriteIndex - 2, "AB", 2, true)) {
+		if (DTMF_CompareMessage(gDTMF_Received + (gDTMF_WriteIndex - 2), "AB", 2, true)) {
 			gDTMF_State = 1;
 			gUpdateDisplay = true;
 			return;
@@ -240,8 +245,8 @@ void DTMF_HandleRequest(void)
 		return;
 	}
 	g_200003BC = 2;
-	memcpy(gDTMF_Contact1, gDTMF_Received + Offset, 3);
-	memcpy(gDTMF_Contact0, gDTMF_Received + Offset + 4, 3);
+	memcpy(gDTMF_Callee, gDTMF_Received + Offset, 3);
+	memcpy(gDTMF_Caller, gDTMF_Received + Offset + 4, 3);
 
 	gUpdateDisplay = true;
 
@@ -268,3 +273,65 @@ void DTMF_HandleRequest(void)
 	}
 }
 
+void DTMF_Reply(void)
+{
+	char String[20];
+	const char *pString;
+	uint16_t Delay;
+
+	switch (g_200003BE) {
+	case 1:
+		if (g_20000438 == 2) {
+			pString = gDTMF_String;
+		} else {
+			sprintf(String, "%s%c%s", gDTMF_String, gEeprom.DTMF_SEPARATE_CODE, gEeprom.ANI_DTMF_ID);
+			pString = String;
+		}
+		break;
+
+	case 2:
+		pString = "AB";
+		break;
+
+	case 3:
+		sprintf(String, "%s%c%s", gEeprom.ANI_DTMF_ID, gEeprom.DTMF_SEPARATE_CODE, "AAAAA");
+		pString = String;
+		break;
+
+	default:
+		if (g_200003BC || (gCrossTxRadioInfo->DTMF_PTT_ID_TX_MODE != 1 && gCrossTxRadioInfo->DTMF_PTT_ID_TX_MODE != 3)) {
+			g_200003BE = 0;
+			return;
+		}
+		pString = gEeprom.DTMF_UP_CODE;
+		break;
+	}
+
+	g_200003BE = 0;
+	Delay = gEeprom.DTMF_PRELOAD_TIME;
+	if (gEeprom.DTMF_SIDE_TONE) {
+		GPIO_SetBit(&GPIOC->DATA, GPIOC_PIN_AUDIO_PATH);
+		gEnableSpeaker = true;
+		Delay = gEeprom.DTMF_PRELOAD_TIME;
+		if (gEeprom.DTMF_PRELOAD_TIME < 60) {
+			Delay = 60;
+		}
+	}
+	SYSTEM_DelayMs(Delay);
+
+	BK4819_EnterDTMF_TX(gEeprom.DTMF_SIDE_TONE);
+
+	BK4819_PlayDTMFString(
+		pString,
+		1,
+		gEeprom.DTMF_FIRST_CODE_PERSIST_TIME,
+		gEeprom.DTMF_HASH_CODE_PERSIST_TIME,
+		gEeprom.DTMF_CODE_PERSIST_TIME,
+		gEeprom.DTMF_CODE_INTERVAL_TIME);
+
+	GPIO_ClearBit(&GPIOC->DATA, GPIOC_PIN_AUDIO_PATH);
+
+	gEnableSpeaker = false;
+	BK4819_ExitDTMF_TX(false);
+}
+
diff --git a/app/dtmf.h b/app/dtmf.h
index 322cd33..ae6b5c9 100644
--- a/app/dtmf.h
+++ b/app/dtmf.h
@@ -25,8 +25,8 @@ extern char gDTMF_InputBox[15];
 extern char gDTMF_Received[16];
 extern bool gIsDtmfContactValid;
 extern char gDTMF_ID[4];
-extern char gDTMF_Contact0[4];
-extern char gDTMF_Contact1[4];
+extern char gDTMF_Caller[4];
+extern char gDTMF_Callee[4];
 extern uint8_t gDTMF_State;
 extern bool gDTMF_DecodeRing;
 extern uint8_t gDTMF_DecodeRingCountdown;
@@ -35,6 +35,7 @@ extern uint8_t gDTMF_WriteIndex;
 extern uint8_t gDTMF_AUTO_RESET_TIME;
 extern uint8_t gDTMF_InputIndex;
 extern bool gDTMF_InputMode;
+extern uint8_t gDTMF_RecvTimeout;
 
 bool DTMF_ValidateCodes(char *pCode, uint8_t Size);
 bool DTMF_GetContact(uint8_t Index, char *pContact);
@@ -44,6 +45,7 @@ bool DTMF_CompareMessage(const char *pDTMF, const char *pTemplate, uint8_t Size,
 bool DTMF_IsGroupCall(const char *pDTMF, uint32_t Size);
 void DTMF_Append(char Code);
 void DTMF_HandleRequest(void);
+void DTMF_Reply(void);
 
 #endif
 
diff --git a/driver/bk4819.c b/driver/bk4819.c
index cf8175f..50b121b 100644
--- a/driver/bk4819.c
+++ b/driver/bk4819.c
@@ -661,7 +661,7 @@ void BK4819_PlayDTMF(char Code)
 	}
 }
 
-void BK4819_PlayDTMFString(char *pString, bool bDelayFirst, uint16_t FirstCodePersistTime, uint16_t HashCodePersistTime, uint16_t CodePersistTime, uint16_t CodeInternalTime)
+void BK4819_PlayDTMFString(const char *pString, bool bDelayFirst, uint16_t FirstCodePersistTime, uint16_t HashCodePersistTime, uint16_t CodePersistTime, uint16_t CodeInternalTime)
 {
 	uint8_t i;
 	uint16_t Delay;
diff --git a/driver/bk4819.h b/driver/bk4819.h
index 28f201c..e6b8a61 100644
--- a/driver/bk4819.h
+++ b/driver/bk4819.h
@@ -100,7 +100,7 @@ void BK4819_ExitDTMF_TX(bool bKeep);
 void BK4819_EnableTXLink(void);
 
 void BK4819_PlayDTMF(char Code);
-void BK4819_PlayDTMFString(char *pString, bool bDelayFirst, uint16_t FirstCodePersistTime, uint16_t HashCodePersistTime, uint16_t CodePersistTime, uint16_t CodeInternalTime);
+void BK4819_PlayDTMFString(const char *pString, bool bDelayFirst, uint16_t FirstCodePersistTime, uint16_t HashCodePersistTime, uint16_t CodePersistTime, uint16_t CodeInternalTime);
 
 void BK4819_TransmitTone(bool bLocalLoopback, uint32_t Frequency);
 
diff --git a/functions.c b/functions.c
index d6d21b2..718a315 100644
--- a/functions.c
+++ b/functions.c
@@ -70,9 +70,6 @@ void FUNCTION_Select(FUNCTION_Type_t Function)
 {
 	FUNCTION_Type_t PreviousFunction;
 	bool bWasPowerSave;
-	char *pString;
-	char String[16]; // Can be overflown with the right EEPROM values
-	uint16_t Delay;
 
 	PreviousFunction = gCurrentFunction;
 	bWasPowerSave = (PreviousFunction == FUNCTION_POWER_SAVE);
@@ -161,54 +158,8 @@ void FUNCTION_Select(FUNCTION_Type_t Function)
 	RADIO_PrepareTransmit();
 	BK4819_ToggleGpioOut(BK4819_GPIO1_PIN29, true);
 
-	if (g_200003BE == 1) {
-		if (g_20000438 == 2) {
-			pString = gDTMF_String;
-		} else {
-			sprintf(String, "%s%c%s", gDTMF_String, gEeprom.DTMF_SEPARATE_CODE, gEeprom.ANI_DTMF_ID);
-			pString = String;
-		}
-	} else if (g_200003BE == 2) {
-		pString = "AB";
-	} else {
-		if (g_200003BE == 3) {
-			sprintf(String, "%s%c%s", gEeprom.ANI_DTMF_ID, gEeprom.DTMF_SEPARATE_CODE, "AAAAA");
-			pString = String;
-		}
-		if (g_200003BC || (gCrossTxRadioInfo->DTMF_PTT_ID_TX_MODE != 1 && gCrossTxRadioInfo->DTMF_PTT_ID_TX_MODE != 3)) {
-			g_200003BE = 0;
-			goto Skip;
-		}
-		pString = gEeprom.DTMF_UP_CODE;
-	}
-	g_200003BE = 0;
-	Delay = gEeprom.DTMF_PRELOAD_TIME;
-	if (gEeprom.DTMF_SIDE_TONE) {
-		GPIO_SetBit(&GPIOC->DATA, GPIOC_PIN_AUDIO_PATH);
-		gEnableSpeaker = true;
-		Delay = gEeprom.DTMF_PRELOAD_TIME;
-		if (gEeprom.DTMF_PRELOAD_TIME < 60) {
-			Delay = 60;
-		}
-	}
-	SYSTEM_DelayMs(Delay);
+	DTMF_Reply();
 
-	BK4819_EnterDTMF_TX(gEeprom.DTMF_SIDE_TONE);
-
-	BK4819_PlayDTMFString(
-		pString,
-		1,
-		gEeprom.DTMF_FIRST_CODE_PERSIST_TIME,
-		gEeprom.DTMF_HASH_CODE_PERSIST_TIME,
-		gEeprom.DTMF_CODE_PERSIST_TIME,
-		gEeprom.DTMF_CODE_INTERVAL_TIME);
-
-	GPIO_ClearBit(&GPIOC->DATA, GPIOC_PIN_AUDIO_PATH);
-
-	gEnableSpeaker = false;
-	BK4819_ExitDTMF_TX(false);
-
-Skip:
 	if (g_20000383) {
 		if (g_20000383 == 3) {
 			BK4819_TransmitTone(true, 1750);
diff --git a/misc.c b/misc.c
index fea713e..e1d0912 100644
--- a/misc.c
+++ b/misc.c
@@ -136,7 +136,6 @@ uint8_t gPttDebounceCounter;
 uint8_t g_20000438;
 bool g_20000439;
 uint8_t gMenuListCount;
-uint8_t g_20000442;
 uint8_t g_20000458;
 uint8_t gBackupCROSS_BAND_RX_TX;
 uint8_t g_2000045C;
diff --git a/misc.h b/misc.h
index c26a3f9..e396ec5 100644
--- a/misc.h
+++ b/misc.h
@@ -161,7 +161,6 @@ extern uint8_t gPttDebounceCounter;
 extern uint8_t g_20000438;
 extern bool g_20000439;
 extern uint8_t gMenuListCount;
-extern uint8_t g_20000442;
 extern uint8_t g_20000458;
 extern uint8_t gBackupCROSS_BAND_RX_TX;
 extern uint8_t g_2000045C;
diff --git a/ui/main.c b/ui/main.c
index 70c5804..6e69719 100644
--- a/ui/main.c
+++ b/ui/main.c
@@ -29,7 +29,6 @@
 void UI_DisplayMain(void)
 {
 	char String[16];
-	char String2[16];
 	uint8_t i;
 
 	memset(gFrameBuffer, 0, sizeof(gFrameBuffer));
@@ -66,6 +65,8 @@ void UI_DisplayMain(void)
 
 		if (Channel != i) {
 			if (g_200003BC || g_200003BD || gDTMF_InputMode) {
+				char Contact[16];
+
 				if (!gDTMF_InputMode) {
 					if (g_200003BC == 1) {
 						if (gDTMF_State == 2) {
@@ -74,10 +75,10 @@ void UI_DisplayMain(void)
 							strcpy(String, "CALL OUT");
 						}
 					} else if (g_200003BC == 2) {
-						if (DTMF_FindContact(gDTMF_Contact0, String2)) {
-							sprintf(String, "CALL:%s", String2);
+						if (DTMF_FindContact(gDTMF_Caller, Contact)) {
+							sprintf(String, "CALL:%s", Contact);
 						} else {
-							sprintf(String, "CALL:%s", gDTMF_Contact0);
+							sprintf(String, "CALL:%s", gDTMF_Caller);
 						}
 					} else if (g_200003BD == 1) {
 						if (gDTMF_State == 1) {
@@ -92,20 +93,20 @@ void UI_DisplayMain(void)
 				UI_PrintString(String, 2, 127, i * 3, 8, false);
 
 				memset(String, 0, sizeof(String));
-				memset(String2, 0, sizeof(String2));
+				memset(Contact, 0, sizeof(Contact));
 
 				if (!gDTMF_InputMode) {
 					if (g_200003BC == 1) {
-						if (DTMF_FindContact(gDTMF_String, String2)) {
-							sprintf(String, ">%s", String2);
+						if (DTMF_FindContact(gDTMF_String, Contact)) {
+							sprintf(String, ">%s", Contact);
 						} else {
 							sprintf(String, ">%s", gDTMF_String);
 						}
 					} else if (g_200003BC == 2) {
-						if (DTMF_FindContact(gDTMF_Contact1, String2)) {
-							sprintf(String, ">%s", String2);
+						if (DTMF_FindContact(gDTMF_Callee, Contact)) {
+							sprintf(String, ">%s", Contact);
 						} else {
-							sprintf(String, ">%s", gDTMF_Contact1);
+							sprintf(String, ">%s", gDTMF_Callee);
 						}
 					} else if (g_200003BD == 1) {
 						sprintf(String, ">%s", gDTMF_String);