diff --git a/CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_sys.c b/CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_sys.c
index 3608e36..9ec9509 100644
--- a/CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_sys.c
+++ b/CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_sys.c
@@ -369,28 +369,3 @@ void mDelaymS(uint16_t t)
     }
 }
 
-#ifdef DEBUG
-int _write(int fd, char *buf, int size)
-{
-    int i;
-    for(i = 0; i < size; i++)
-    {
-#if DEBUG == Debug_UART0
-        while(R8_UART0_TFC == UART_FIFO_SIZE);                  /* 等待数据发送 */
-        R8_UART0_THR = *buf++; /* 发送数据 */
-#elif DEBUG == Debug_UART1
-        while(R8_UART1_TFC == UART_FIFO_SIZE);                  /* 等待数据发送 */
-        R8_UART1_THR = *buf++; /* 发送数据 */
-#elif DEBUG == Debug_UART2
-        while(R8_UART2_TFC == UART_FIFO_SIZE);                  /* 等待数据发送 */
-        R8_UART2_THR = *buf++; /* 发送数据 */
-#elif DEBUG == Debug_UART3       
-        while(R8_UART3_TFC == UART_FIFO_SIZE);                  /* 等待数据发送 */
-        R8_UART3_THR = *buf++; /* 发送数据 */
-#endif
-    }
-    return size;
-}
-
-#endif
-
diff --git a/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH583SFR.h b/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH583SFR.h
index 86fa854..05ee89d 100644
--- a/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH583SFR.h
+++ b/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH583SFR.h
@@ -123,12 +123,6 @@ typedef volatile unsigned long long  *PUINT64V;
 #define max(a,b)                (((a) > (b)) ? (a) : (b))
 #endif
 
-#ifdef  DEBUG
-#define PRINT(X...) printf(X)
-#else
-#define PRINT(X...)
-#endif
-
 /* Calculate the byte offset of a field in a structure of type */
 #define FIELD_OFFSET(Type, Field)    ((UINT16)&(((Type *)0)->Field))
 
diff --git a/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH58x_common.h b/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH58x_common.h
index 965cac6..b51fe14 100644
--- a/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH58x_common.h
+++ b/CH5xx_ble_firmware_library/StdPeriphDriver/inc/CH58x_common.h
@@ -36,15 +36,6 @@
 #endif
 #endif
 
-#define Debug_UART0        0
-#define Debug_UART1        1
-#define Debug_UART2        2
-#define Debug_UART3        3
-
-#ifdef DEBUG
-#include <stdio.h>
-#endif
-
 /**
  * @brief  系统主频时钟（Hz）
  */
diff --git a/Makefile b/Makefile
index e81e396..745f9f8 100644
--- a/Makefile
+++ b/Makefile
@@ -47,25 +47,27 @@ CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_clk.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_uart0.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_timer1.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_pwm.c \
-CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_usbhostClass.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_adc.c \
-CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_usbhostBase.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_timer3.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_timer0.c \
-CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_usb2hostClass.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_flash.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_uart1.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_usb2dev.c \
-CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_usb2hostBase.c \
 CH5xx_ble_firmware_library/StdPeriphDriver/CH58x_spi1.c \
 CH5xx_ble_firmware_library/RVMSIS/core_riscv.c \
 src/main.c \
+src/debug.c \
 src/leddrv.c \
 src/button.c \
 src/bmlist.c \
 src/ble/profile/legacy.c \
 src/ble/profile/batt.c \
 src/ble/profile/devinfo.c \
+src/ble/profile/ng.c \
+src/config.c \
+src/legacyctrl.c \
+src/util/crc.c \
+src/ngctrl.c \
 src/ble/setup.c \
 src/ble/peripheral.c \
 src/data.c \
diff --git a/src/animation.c b/src/animation.c
index 3606314..9a55660 100644
--- a/src/animation.c
+++ b/src/animation.c
@@ -3,6 +3,7 @@
 #include "xbm.h"
 #include "leddrv.h"
 #include "bmlist.h"
+#include "debug.h"
 
 #define ANI_ANIMATION_STEPS     (5) // steps
 #define ANI_FIXED_STEPS         (LED_COLS) // steps
@@ -404,7 +405,7 @@ static void picture(bm_t *bm, uint16_t *fb, int step, int frame)
 	int hc = LED_COLS / 2;
 	int range = mod(step - 1, LED_COLS);
 
-	if (range > LED_COLS/2) {
+	if (range > hc) {
 		still(bm, fb, frame);
 		return;
 	}
@@ -418,15 +419,10 @@ static void picture(bm_t *bm, uint16_t *fb, int step, int frame)
 				0 : bm->buf[hc - i + frame];
 	}
 
-	if (i >= LED_COLS)
+	if (i >= hc)
 		return;
 	fb[hc + i - 1] = -1;
 	fb[hc - i] = -1;
-
-	for (i++; i< LED_COLS; i++) {
-		fb[hc + i - 1] = 0;
-		fb[hc - i] = 0;
-	}
 }
 
 static void picture_out(bm_t *bm, uint16_t *fb, int step)
@@ -442,7 +438,7 @@ static void picture_out(bm_t *bm, uint16_t *fb, int step)
 		fb[hc + i - 1] = 0;
 		fb[hc - i] = 0;
 	}
-	if (i >= LED_COLS)
+	if (i >= hc)
 		return;
 	fb[hc + i - 1] = -1;
 	fb[hc - i] = -1;
diff --git a/src/ble/peripheral.c b/src/ble/peripheral.c
index f9c5e58..4ed9566 100644
--- a/src/ble/peripheral.c
+++ b/src/ble/peripheral.c
@@ -1,5 +1,6 @@
 #include "CH58xBLE_LIB.h"
 #include "setup.h"
+#include "../config.h"
 
 #define ADV_UUID    (0xFEE0)
 
@@ -24,29 +25,6 @@ typedef struct
 
 #define CONN_TIMEOUT        100 // Supervision timeout (units of 10ms)
 
-// GAP - SCAN RSP data (max size = 31 bytes)
-static uint8 scanRspData[] = {
-	// complete name
-	16, // length of this section
-	GAP_ADTYPE_LOCAL_NAME_COMPLETE,
-	'L', 'E','D', ' ',
-	'B', 'a','d', 'g', 'e', ' ',
-	'M', 'a','g', 'i', 'c',
-
-	// connection interval range
-	0x05, // length of this section
-	GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE,
-	LO_UINT16(MIN_CONN_INTERVAL),
-	HI_UINT16(MIN_CONN_INTERVAL),
-	LO_UINT16(MAX_CONN_INTERVAL),
-	HI_UINT16(MAX_CONN_INTERVAL),
-
-	// Tx power level
-	0x02, // length of this data
-	GAP_ADTYPE_POWER_LEVEL,
-	9 // 9dBm
-};
-
 // GAP - Advertisement data (max size = 31 bytes)
 // keep short, save energy, save the planet
 static uint8 advertData[] = {
@@ -61,9 +39,6 @@ static uint8 advertData[] = {
 	HI_UINT16(ADV_UUID)
 };
 
-// GAP GATT Attributes
-static uint8 devName[GAP_DEVICE_NAME_LEN] = "LED Badge Magic";
-
 // Connection item list
 static peripheralConnItem_t conn_list;
 
@@ -210,21 +185,58 @@ void ble_disable_advertise()
 	GAPRole_SetParameter(GAPROLE_ADVERT_ENABLED, sizeof(uint8), &e);
 }
 
+// len should not exceed 20 chars excluding null-terminate char. Otherwise it
+// will be trimmed off and return -1
+static int setup_scan_rsp(char *name, uint8_t len)
+{
+	int ret = 0;
+	// GAP - SCAN RSP data (max size = 31 bytes)
+	uint8_t scanRspData[31] = {
+		// connection interval range
+		0x05, // length of this section
+		GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE,
+		LO_UINT16(MIN_CONN_INTERVAL),
+		HI_UINT16(MIN_CONN_INTERVAL),
+		LO_UINT16(MAX_CONN_INTERVAL),
+		HI_UINT16(MAX_CONN_INTERVAL),
+
+		// Tx power level
+		0x02, // length of this data
+		GAP_ADTYPE_POWER_LEVEL,
+		9, // 9dBm
+
+		len + 1,
+		GAP_ADTYPE_LOCAL_NAME_COMPLETE,
+	};
+	
+	if (len < (31 - 11)) {
+		tmos_memcpy(&scanRspData[11], name, len);
+		ret = -1;
+	} else {
+		tmos_memcpy(&scanRspData[11], name, 20);
+	}
+	int total_len = 11 + len;
+	tmos_memset(&scanRspData[total_len], 0, 31 - total_len);
+	GAPRole_SetParameter(GAPROLE_SCAN_RSP_DATA, total_len, scanRspData);
+	return ret;
+}
+
 static void gap_init()
 {
 	GAPRole_PeripheralInit();
 
-	static uint16 desired_min_interval = 6;
-	static uint16 desired_max_interval = 500;
+	uint16_t min_interval = 6;
+	uint16_t max_interval = 500;
 
 	// Set the GAP Role Parameters
-	GAPRole_SetParameter(GAPROLE_SCAN_RSP_DATA, sizeof(scanRspData), scanRspData);
+	setup_scan_rsp(badge_cfg.ble_devname, 20);
 	GAPRole_SetParameter(GAPROLE_ADVERT_DATA, sizeof(advertData), advertData);
-	GAPRole_SetParameter(GAPROLE_MIN_CONN_INTERVAL, sizeof(uint16), &desired_min_interval);
-	GAPRole_SetParameter(GAPROLE_MAX_CONN_INTERVAL, sizeof(uint16), &desired_max_interval);
+	GAPRole_SetParameter(GAPROLE_MIN_CONN_INTERVAL, sizeof(uint16), &min_interval);
+	GAPRole_SetParameter(GAPROLE_MAX_CONN_INTERVAL, sizeof(uint16), &max_interval);
 	
 	// Set the GAP Characteristics
-	GGS_SetParameter(GGS_DEVICE_NAME_ATT, GAP_DEVICE_NAME_LEN, devName);
+	GGS_SetParameter(GGS_DEVICE_NAME_ATT, 20,
+				badge_cfg.ble_devname);
 	GAP_SetParamValue(TGAP_DISC_ADV_INT_MIN, MIN_ADV_INTERVAL);
 	GAP_SetParamValue(TGAP_DISC_ADV_INT_MAX, MAX_ADV_INTERVAL);
 
diff --git a/src/ble/profile.h b/src/ble/profile.h
index 1472a3d..39e7e79 100644
--- a/src/ble/profile.h
+++ b/src/ble/profile.h
@@ -4,5 +4,8 @@
 int legacy_registerService();
 int devInfo_registerService();
 int batt_registerService();
+int ng_registerService();
+
+bStatus_t ng_notify(uint8_t *pValue, uint8_t len);
 
 #endif /* __BLE_UART_SERVICE_H__ */
diff --git a/src/ble/profile/legacy.c b/src/ble/profile/legacy.c
index 92ac3f4..2c127a8 100644
--- a/src/ble/profile/legacy.c
+++ b/src/ble/profile/legacy.c
@@ -1,8 +1,6 @@
 #include "utils.h"
 
-#include "../../data.h"
-#include "../../power.h"
-#include "../../leddrv.h"
+#include "../../legacyctrl.h"
 
 static const uint16_t ServiceUUID = 0xFEE0;
 static const gattAttrType_t service = {2, (uint8_t *)&ServiceUUID};
@@ -18,39 +16,6 @@ static gattAttribute_t attr_table[] = {
 	CHAR_VAL_DECLAR(&RxCharUUID, 2, GATT_PERMIT_WRITE, RxCharVal),
 };
 
-static bStatus_t receive(uint8_t *val, uint16_t len)
-{
-	static uint16_t c, data_len, n;
-	static uint8_t *data;
-	if (len != LEGACY_TRANSFER_WIDTH) {
-		return ATT_ERR_INVALID_VALUE_SIZE;
-	}
-	if (c == 0) {
-		if (memcmp(val, "wang\0\0", 6)) {
-			return ATT_ERR_INVALID_VALUE;
-		} else {
-			data = malloc(sizeof(data_legacy_t));
-		}
-	}
-
-	memcpy(data + c * len, val, len);
-
-	if (c == 1) {
-		data_legacy_t *d = (data_legacy_t *)data;
-		n = bigendian16_sum(d->sizes, 8);
-		data_len = LEGACY_HEADER_SIZE + LED_ROWS * n;
-		data = realloc(data, data_len);
-	}
-
-	if (c > 2 && ((c+1) * LEGACY_TRANSFER_WIDTH) >= data_len) {
-		data_flatSave(data, data_len);
-		reset_jump();
-	}
-
-	c++;
-	return SUCCESS;
-}
-
 static bStatus_t write_handler(uint16 connHandle, gattAttribute_t *pAttr,
 				uint8 *pValue, uint16 len, uint16 offset, uint8 method)
 {
@@ -60,7 +25,10 @@ static bStatus_t write_handler(uint16 connHandle, gattAttribute_t *pAttr,
 
 	uint16_t uuid = BUILD_UINT16(pAttr->type.uuid[0], pAttr->type.uuid[1]);
 	if(uuid == RxCharUUID) {
-		return receive(pValue, len);
+		if (legacy_ble_rx(pValue, len)) {
+			return ATT_ERR_UNLIKELY;
+		}
+		return SUCCESS;
 	}
 	return ATT_ERR_ATTR_NOT_FOUND;
 }
diff --git a/src/ble/profile/ng.c b/src/ble/profile/ng.c
new file mode 100644
index 0000000..7ab96ce
--- /dev/null
+++ b/src/ble/profile/ng.c
@@ -0,0 +1,160 @@
+#include "utils.h"
+
+#include "CH583SFR.h"
+
+#include "../../power.h"
+#include "../../ngctrl.h"
+#include "../../debug.h"
+
+static const uint16_t ServiceUUID = 0xF055;
+static const gattAttrType_t service = {2, (uint8_t *)&ServiceUUID};
+
+static const uint16_t TxCharUUID = 0xF056;
+static uint8_t TxCharProps = GATT_PROP_READ | GATT_PROP_NOTIFY;
+static uint8_t TxCharVal[256];
+static uint16_t TxLen;
+
+static const uint16_t RxCharUUID = 0xF057;
+static uint8_t RxCharProps = GATT_PROP_WRITE;
+#define RxCharVal TxCharVal
+static gattCharCfg_t TxCCCD[1];
+
+static gattAttribute_t attr_table[] = {
+	ATTR_DECLAR(primaryServiceUUID, 2, GATT_PERMIT_READ, &service),
+
+	CHAR_DECLAR(&RxCharProps),
+	CHAR_VAL_DECLAR(&RxCharUUID, 2, GATT_PERMIT_WRITE, RxCharVal),
+
+	CHAR_DECLAR(&TxCharProps),
+	CHAR_VAL_DECLAR(&TxCharUUID, 2, GATT_PERMIT_READ, TxCharVal),
+	ATTR_DECLAR(clientCharCfgUUID, 2, GATT_PERMIT_READ | GATT_PERMIT_WRITE, TxCCCD),
+};
+
+#define notiAttr attr_table[4]
+
+
+static bStatus_t write_handler(uint16 connHandle, gattAttribute_t *pAttr,
+				uint8 *pValue, uint16 len, uint16 offset, uint8 method)
+{
+	PRINT("ble: write_handler(): connHandle: %04X\n", connHandle);
+
+	if (!gattPermitWrite(pAttr->permissions)) {
+		return ATT_ERR_WRITE_NOT_PERMITTED;
+	}
+	
+	uint16_t uuid = BUILD_UINT16(pAttr->type.uuid[0], pAttr->type.uuid[1]);
+	if(uuid == GATT_CLIENT_CHAR_CFG_UUID) {
+		bStatus_t ret = GATTServApp_ProcessCCCWriteReq(connHandle, pAttr, pValue, len,
+				offset, GATT_CLIENT_CFG_NOTIFY);
+		PRINT("ble: CCCD changed: %02X\n", TxCCCD->value);
+		return ret;
+	}
+
+	if (uuid == RxCharUUID) {
+		return ng_parse(pValue, len);
+	}
+
+	return ATT_ERR_ATTR_NOT_FOUND;
+}
+
+static bStatus_t read_handler(uint16_t connHandle, gattAttribute_t *pAttr,
+				uint8_t *pValue, uint16_t *pLen, uint16_t offset,
+				uint16_t maxLen, uint8_t method)
+{
+	if (!gattPermitRead(pAttr->permissions)) {
+		return ATT_ERR_READ_NOT_PERMITTED;
+	}
+
+	uint16_t uuid = BUILD_UINT16(pAttr->type.uuid[0], pAttr->type.uuid[1]);
+	if(uuid == GATT_CLIENT_CHAR_CFG_UUID) {
+		*pLen = 2;
+		tmos_memcpy(pValue, pAttr->pValue, *pLen);
+		return SUCCESS;
+	}
+
+	if (uuid == TxCharUUID) {
+		*pLen = MIN(TxLen-offset, maxLen);
+		tmos_memcpy(pValue, &pAttr->pValue[offset], *pLen);
+		return SUCCESS;
+	}
+
+	return ATT_ERR_ATTR_NOT_FOUND;
+}
+
+static gattServiceCBs_t service_handlers = {
+	read_handler,
+	write_handler,
+	NULL
+};
+
+static void connStatus_handler(uint16 connHandle, uint8 changeType)
+{
+	if(connHandle == LOOPBACK_CONNHANDLE)
+		return;
+
+	// Reset ClientCharConfig if connection has dropped
+	if((changeType == LINKDB_STATUS_UPDATE_REMOVED)
+				|| ((changeType == LINKDB_STATUS_UPDATE_STATEFLAGS)
+				&& (!linkDB_Up(connHandle)))) {
+		GATTServApp_InitCharCfg(connHandle, TxCCCD);
+	}
+}
+
+int ng_registerService()
+{
+	uint8 status = SUCCESS;
+	GATTServApp_InitCharCfg(INVALID_CONNHANDLE, TxCCCD);
+	linkDB_Register(connStatus_handler);
+
+	status = GATTServApp_RegisterService(attr_table,
+				GATT_NUM_ATTRS(attr_table),
+				GATT_MAX_ENCRYPT_KEY_SIZE,
+				&service_handlers);
+	return (status);
+}
+
+static uint8 isNotifyEnabled(uint16 connHandle)
+{
+	uint16_t val = GATTServApp_ReadCharCfg(connHandle, TxCCCD);
+	return val & GATT_CLIENT_CFG_NOTIFY;
+}
+/**
+ * @brief   Send notify to client. Currently support one client connection
+ *          only.
+ * 
+ * @param	val Value to be sent
+ * @param	len length of val. This should not be larger than MTU.
+ * @return	bStatus_t
+ */
+bStatus_t ng_notify(uint8_t *val, uint8_t len)
+{
+	if(!isNotifyEnabled(TxCCCD->connHandle)) {
+		PRINT("ble: ng_notify() notify is not enabled\n");
+		return bleIncorrectMode;
+	}
+	if(len > ATT_GetMTU(TxCCCD->connHandle)) {
+		return bleInvalidRange;
+	}
+
+	attHandleValueNoti_t noti = {
+		.handle = notiAttr.handle,
+		.len = len
+	};
+	noti.pValue = GATT_bm_alloc(TxCCCD->connHandle, ATT_HANDLE_VALUE_NOTI,
+				len, NULL, 0);
+	if (noti.pValue == NULL) {
+		return bleMemAllocError;
+	}
+
+	tmos_memcpy(noti.pValue, val, len);
+
+	bStatus_t ret = GATT_Notification(TxCCCD->connHandle, &noti, FALSE);
+	GATT_bm_free((gattMsg_t *)&noti, ATT_HANDLE_VALUE_NOTI);
+	if (ret != SUCCESS) {
+		PRINT("ble: noti sending failed\n");
+		return ret;
+	}
+
+	PRINT("ble: noti sent\n");
+	return SUCCESS;
+}
diff --git a/src/ble/setup.c b/src/ble/setup.c
index 7049acb..8a8e015 100644
--- a/src/ble/setup.c
+++ b/src/ble/setup.c
@@ -6,28 +6,39 @@
 #include "CH58x_common.h"
 
 #ifndef BLE_BUFF_LEN
-#define BLE_BUFF_LEN 27
-#endif
-#ifndef BLE_BUFF_NUM
-#define BLE_BUFF_NUM                        5
+// MTU = 64 but clients should request new MTU, otherwise default will be 23
+#define BLE_BUFF_LEN (64 + 4)
 #endif
+
 #ifndef BLE_TX_NUM_EVENT
 #define BLE_TX_NUM_EVENT                    1
 #endif
+
 #ifndef BLE_TX_POWER
 #define BLE_TX_POWER                        LL_TX_POWEER_6_DBM
+// #define BLE_TX_POWER                        LL_TX_POWEER_MINUS_16_DBM
 #endif
+
 #ifndef BLE_MEMHEAP_SIZE
-#define BLE_MEMHEAP_SIZE                    (1024*6)
+#define BLE_MEMHEAP_SIZE                    (1024 * 6)
 #endif
+
 #ifndef CENTRAL_MAX_CONNECTION
-#define CENTRAL_MAX_CONNECTION              3
+#define CENTRAL_MAX_CONNECTION              1
 #endif
+
+#ifndef BLE_BUFF_NUM
+// The BLE lib automatically stack up Write Long messages in Write handler.
+// A connection will be disconnected if this number is some how not enough.
+#define BLE_BUFF_NUM        (512 / 23)
+#endif
+
 #ifndef PERIPHERAL_MAX_CONNECTION
 #define PERIPHERAL_MAX_CONNECTION           1
 #endif
 
-static __attribute__((aligned(4))) uint32_t MEM_BUF[BLE_MEMHEAP_SIZE / 4];
+static __attribute__((aligned(4), section(".noinit")))
+uint32_t MEM_BUF[BLE_MEMHEAP_SIZE / 4];
 
 static void lsi_calib(void)
 {
diff --git a/src/bmlist.c b/src/bmlist.c
index d9a942d..420386e 100644
--- a/src/bmlist.c
+++ b/src/bmlist.c
@@ -68,6 +68,7 @@ bm_t *bmlist_drop(bm_t *bm)
 		head = bm->next;
 	if (bm == tail)
 		tail = bm->prev;
+	free(bm);
 	return bm->next;
 }
 
diff --git a/src/config.c b/src/config.c
new file mode 100644
index 0000000..44c7c7c
--- /dev/null
+++ b/src/config.c
@@ -0,0 +1,87 @@
+/**
+ * TODO: needs constraints checking as some of the config fields may have
+ * invalid values which will make the badge broke.
+ */
+#include "config.h"
+#include "resource.h"
+#include "res/foss-asia-2.xbm"
+#include "debug.h"
+#include "util/crc.h"
+
+#include "ISP583.h"
+#include <stdlib.h>
+
+#define CFG_SIZE sizeof(badge_cfg_t)
+#define CFG_DEF_FLASH_OFFS (EEPROM_MAX_SIZE - CFG_SIZE - 1) // Default offset
+
+badge_cfg_t badge_cfg;
+
+/* In case of first time firmware upgrading */
+void cfg_fallback()
+{
+	badge_cfg.ble_always_on = 1;
+	memcpy(badge_cfg.ble_devname, "LED Badge Magic\0\0\0\0", 20);
+	/* OEM app testing: */
+	// memcpy(badge_cfg.ble_devname, "LSLED\0\0\0\0\0\0\0\0\0\0\0\0\0\0", 20);
+
+	badge_cfg.led_brightness = 0;
+	badge_cfg.led_scan_freq = 2000;
+
+	badge_cfg.splash_speedT = 30; // ms
+
+	int splash_size = ALIGN_1BYTE(splash.w) * splash.h;
+	memcpy(badge_cfg.splash_bm_bits, splash.bits, splash_size);
+	badge_cfg.splash_bm_w = splash.w;
+	badge_cfg.splash_bm_h = splash.h;
+	badge_cfg.splash_bm_fh = splash.fh;
+
+	badge_cfg.reset_rx = FALSE;
+}
+
+void cfg_update_crc(badge_cfg_t *cfg)
+{
+	cfg->crc = crc_cal((uint8_t *)cfg, CFG_SIZE - 1);
+	PRINT(__func__);
+	PRINT(": crc: %02X\n", cfg->crc);
+}
+
+int cfg_writeflash(uint16_t flash_offs, badge_cfg_t *cfg)
+{
+	if (flash_offs + CFG_SIZE > EEPROM_MAX_SIZE) {
+		return -1;
+	}
+	cfg_update_crc(&badge_cfg);
+	return EEPROM_WRITE(flash_offs, cfg, CFG_SIZE);
+}
+
+int cfg_readflash(uint16_t flash_offs, badge_cfg_t *cfg)
+{
+	int ret = EEPROM_READ(flash_offs, cfg, CFG_SIZE);
+	if (ret)
+		return ret;
+
+	return crc_cal((uint8_t *)cfg, CFG_SIZE);
+}
+
+int cfg_readflash_def(badge_cfg_t *cfg)
+{
+	return cfg_readflash(CFG_DEF_FLASH_OFFS, cfg);
+}
+
+int cfg_writeflash_def(badge_cfg_t *cfg)
+{
+	return cfg_writeflash(CFG_DEF_FLASH_OFFS, cfg);
+}
+
+void cfg_init()
+{
+	badge_cfg_t cfg;
+	int r = cfg_readflash_def(&cfg);
+	if (r) {
+		cfg_fallback();
+		PRINT("configuration falling back: %02x\n", r);
+	} else {
+		memcpy(&badge_cfg, &cfg, CFG_SIZE);
+		PRINT("configuration read from flash successfully\n");
+	}
+}
\ No newline at end of file
diff --git a/src/config.h b/src/config.h
new file mode 100644
index 0000000..b6ce545
--- /dev/null
+++ b/src/config.h
@@ -0,0 +1,42 @@
+#ifndef __CONFIG_H__
+#define __CONFIG_H__
+
+#include <stdint.h>
+
+#include "xbm.h"
+#include "leddrv.h"
+
+#define SPLASH_MAX_WIDTH (48) // pixels
+#define SPLASH_MAX_HEIGHT (44) // pixels
+#define SPLASH_MAX_SIZE (ALIGN_1BYTE(SPLASH_MAX_WIDTH) * SPLASH_MAX_HEIGHT)
+
+typedef struct {
+	// Turn on Bluetooth while in Normal mode and disable Downloads mode
+	uint8_t ble_always_on;
+	char ble_devname[20];
+
+	uint8_t led_brightness;
+	uint16_t led_scan_freq; // Scan frequency
+
+	uint8_t splash_bm_bits[SPLASH_MAX_SIZE]; // Up to 48x44 (Width x Height)
+	uint8_t splash_bm_w;
+	uint8_t splash_bm_h;
+	uint8_t splash_bm_fh;
+
+	// Speed in period of micro second. The lower value, the higher speed
+	uint16_t splash_speedT;
+	uint8_t reset_rx; // Reset after bitmap received
+
+	uint8_t crc;
+} __attribute__((packed)) badge_cfg_t;
+
+extern badge_cfg_t badge_cfg;
+
+void cfg_init();
+int cfg_writeflash(uint16_t flash_offs, badge_cfg_t *cfg);
+int cfg_readflash(uint16_t flash_offs, badge_cfg_t *cfg);
+int cfg_writeflash_def(badge_cfg_t *cfg);
+int cfg_readflash_def(badge_cfg_t *cfg);
+void cfg_fallback();
+
+#endif /* __CONFIG_H__ */
diff --git a/src/data.c b/src/data.c
index 32c1dce..1261c90 100644
--- a/src/data.c
+++ b/src/data.c
@@ -24,34 +24,25 @@ uint32_t data_flatSave(uint8_t *data, uint32_t len)
 	return EEPROM_WRITE(0, data, len);
 }
 
-data_legacy_t *data_get_header(int read_anyway)
+void data_get_header(data_legacy_t *buf)
 {
-	static data_legacy_t *cache;
-
-	if (cache == NULL) {
-		cache = malloc(sizeof(data_legacy_t));
-		read_anyway = 1;
-	}
-
-	if (read_anyway) {
-		EEPROM_READ(0, cache, LEGACY_HEADER_SIZE);
-	}
-	return cache;
+	EEPROM_READ(0, buf, LEGACY_HEADER_SIZE);
 }
 
 uint16_t data_flash2newmem(uint8_t **chunk, uint32_t n)
 {
-	data_legacy_t *header = data_get_header(0);
+	data_legacy_t header;
+	data_get_header(&header);
 
-	if (memcmp(header->header, "wang", 5))
+	if (memcmp(header.header, "wang", 5))
 		return 0;
 
-	uint16_t size = bswap16(header->sizes[n]) * LED_ROWS;
+	uint16_t size = bswap16(header.sizes[n]) * LED_ROWS;
 	if (size == 0)
 		return 0;
 
 	uint16_t offs = LEGACY_HEADER_SIZE
-		+ bigendian16_sum(header->sizes, n) * LED_ROWS;
+		+ bigendian16_sum(header.sizes, n) * LED_ROWS;
 
 	*chunk = malloc(size);
 	EEPROM_READ(offs, *chunk, size);
@@ -98,11 +89,12 @@ bm_t *flash2newbm(uint32_t n)
 		return NULL;
 
 	bm_t *bm = chunk2newbm(buf, size);
-	data_legacy_t *header = data_get_header(0);
+	data_legacy_t header;
+	data_get_header(&header);
 
-	bm->is_flash = (header->flash & (1 << n)) != 0;
-	bm->is_marquee = (header->marquee & (1 << n)) != 0;
-	bm->modes = header->modes[n];
+	bm->is_flash = (header.flash & (1 << n)) != 0;
+	bm->is_marquee = (header.marquee & (1 << n)) != 0;
+	bm->modes = header.modes[n];
 
 	free(buf);
 	return bm;
diff --git a/src/data.h b/src/data.h
index ebdf104..ef4050c 100644
--- a/src/data.h
+++ b/src/data.h
@@ -20,7 +20,7 @@ typedef struct {
 	uint8_t separator[16];
 
 	uint8_t *bitmapdata;
-} data_legacy_t;
+}  __attribute__((packed)) data_legacy_t;
 
 #define LEGACY_TRANSFER_WIDTH (16)
 #define LEGACY_HEADER_SIZE (sizeof(data_legacy_t) - sizeof(uint8_t *))
@@ -47,7 +47,7 @@ uint32_t bigendian16_sum(uint16_t *s, int len);
 uint32_t data_flatSave(uint8_t *data, uint32_t len);
 uint16_t data_flash2newmem(uint8_t **chunk, uint32_t n);
 
-data_legacy_t *data_get_header(int read_anyway);
+void data_get_header(data_legacy_t *buf);
 
 void chunk2buffer(uint8_t *chunk, uint16_t size, uint16_t *buf);
 void chunk2bm(uint8_t *chunk, uint16_t size, bm_t *bm);
diff --git a/src/debug.c b/src/debug.c
new file mode 100644
index 0000000..e0137af
--- /dev/null
+++ b/src/debug.c
@@ -0,0 +1,35 @@
+#include "debug.h"
+#include "CH583SFR.h"
+
+#ifdef DEBUG
+
+#define Debug_UART0        0
+#define Debug_UART1        1
+#define Debug_UART2        2
+#define Debug_UART3        3
+
+int _write(int fd, char *buf, int size)
+{
+    int i;
+    for(i = 0; i < size; i++)
+    {
+#if DEBUG == Debug_UART0
+        while(R8_UART0_TFC == UART_FIFO_SIZE);
+        R8_UART0_THR = *buf++;
+#elif DEBUG == Debug_UART1
+        while(R8_UART1_TFC == UART_FIFO_SIZE);
+        R8_UART1_THR = *buf++;
+#elif DEBUG == Debug_UART2
+        while(R8_UART2_TFC == UART_FIFO_SIZE);
+        R8_UART2_THR = *buf++;
+#elif DEBUG == Debug_UART3       
+        while(R8_UART3_TFC == UART_FIFO_SIZE);
+        R8_UART3_THR = *buf++;
+#else
+#error "Please specify a debug port for uart"
+#endif
+    }
+    return size;
+}
+
+#endif
\ No newline at end of file
diff --git a/src/debug.h b/src/debug.h
new file mode 100644
index 0000000..6378f4b
--- /dev/null
+++ b/src/debug.h
@@ -0,0 +1,15 @@
+#ifndef __DEBUG_H__
+#define __DEBUG_H__
+
+#include <stdio.h>
+
+#ifdef  DEBUG
+#define PRINT(X...) printf(X)
+#else
+#define PRINT(X...)
+#endif
+
+#define _TRACE() PRINT("> "); PRINT(__FILE__); PRINT(": "); PRINT(__func__); \
+				PRINT("()\n")
+
+#endif /* __DEBUG_H__ */
diff --git a/src/legacyctrl.c b/src/legacyctrl.c
new file mode 100644
index 0000000..582d6e4
--- /dev/null
+++ b/src/legacyctrl.c
@@ -0,0 +1,110 @@
+#include "data.h"
+#include "power.h"
+#include "leddrv.h"
+#include "debug.h"
+#include "legacyctrl.h"
+
+int legacy_ble_rx(uint8_t *val, uint16_t len)
+{
+	_TRACE();
+	static uint16_t c, data_len, n;
+	static uint8_t *data;
+
+	if (len != LEGACY_TRANSFER_WIDTH) {
+		PRINT("Transfer width is not matched\n");
+		return -1;
+	}
+
+	PRINT("val[%d]: ", len);
+	for (int i=0; i<len; i++) {
+		PRINT("%02X ", val[i]);
+	}
+	PRINT("\n");
+
+	if (c == 0) {
+		if (memcmp(val, "wang", 5)) {
+			PRINT("Not a header\n");
+			return -2;
+		} else {
+			free(data);
+			data = malloc(sizeof(data_legacy_t));
+			if (!data) {
+				PRINT("insufficient memory\n");
+				return -3;
+			}
+		}
+	} else { // Re attempt after a failed transfer
+		if (!memcmp(val, "wang", 5)) {
+			free(data);
+			c = 0;
+			data = malloc(sizeof(data_legacy_t));
+			if (!data) {
+				PRINT("insufficient memory\n");
+				return -3;
+			}
+		}
+	}
+
+	PRINT("Copying BLE value\n");
+	memcpy(data + c * len, val, len);
+
+	if (c == 1) {
+		data_legacy_t *d = (data_legacy_t *)data;
+		n = bigendian16_sum(d->sizes, 8);
+		data_len = LEGACY_HEADER_SIZE + LED_ROWS * n;
+		PRINT("Data len: %d\n", data_len);
+		data = realloc(data, data_len);
+		if (!data) {
+			PRINT("insufficient memory\n");
+			return -3;
+		}
+	}
+
+	if (c > 2 && ((c+1) * LEGACY_TRANSFER_WIDTH) >= data_len) {
+		PRINT("All bitmaps data received successfully\nWriting to flash.. ");
+		data_flatSave(data, data_len);
+		free(data);
+		data = NULL;
+		handle_after_rx();
+		PRINT("Done\n");
+	}
+
+	c++;
+	return 0;
+}
+
+int legacy_usb_rx(uint8_t *buf, uint16_t len)
+{
+	static uint16_t rx_len, data_len;
+	static uint8_t *data;
+
+	PRINT("dump first 8 bytes: %02x %02x %02x %02x %02x %02x %02x %02x\n",
+				buf[0], buf[1], buf[2], buf[3],
+				buf[4], buf[5], buf[6], buf[7]);
+
+	if (rx_len == 0) {
+		if (memcmp(buf, "wang", 5))
+			return -1;
+
+		int init_len = len > LEGACY_HEADER_SIZE ? len : sizeof(data_legacy_t);
+		init_len += MAX_PACKET_SIZE;
+		data = malloc(init_len);
+	}
+
+	memcpy(data + rx_len, buf, len);
+	rx_len += len;
+
+	if (!data_len) {
+		data_legacy_t *d = (data_legacy_t *)data;
+		uint16_t n = bigendian16_sum(d->sizes, 8);
+		data_len = LEGACY_HEADER_SIZE + LED_ROWS * n;
+		data = realloc(data, data_len);
+	}
+
+	if ((rx_len > LEGACY_HEADER_SIZE) && rx_len >= data_len) {
+		data_flatSave(data, data_len);
+		free(data);
+		handle_after_rx();
+	}
+	return 0;
+}
\ No newline at end of file
diff --git a/src/legacyctrl.h b/src/legacyctrl.h
new file mode 100644
index 0000000..c661fd7
--- /dev/null
+++ b/src/legacyctrl.h
@@ -0,0 +1,10 @@
+#ifndef __LEGACYCTRL_H__
+#define __LEGACYCTRL_H__
+
+#include <stdint.h>
+
+void handle_after_rx();
+int legacy_ble_rx(uint8_t *val, uint16_t len);
+int legacy_usb_rx(uint8_t *buf, uint16_t len);
+
+#endif /* __LEGACYCTRL_H__ */
diff --git a/src/main.c b/src/main.c
index d08ae40..82a2a95 100644
--- a/src/main.c
+++ b/src/main.c
@@ -11,14 +11,14 @@
 
 #include "power.h"
 #include "data.h"
+#include "config.h"
+#include "debug.h"
 
 #include "ble/setup.h"
 #include "ble/profile.h"
 
 #include "usb/usb.h"
-
-#define SCAN_F          (2000)
-#define SCAN_T          (FREQ_SYS / SCAN_F)
+#include "legacyctrl.h"
 
 #define NEXT_STATE(v, min, max) \
 				(v)++; \
@@ -60,10 +60,22 @@ static void change_brightness()
 	NEXT_STATE(brightness, 0, BRIGHTNESS_LEVELS);
 }
 
+static void mode_setup_download();
+static void mode_setup_normal();
+
 __HIGH_CODE
 static void change_mode()
 {
 	NEXT_STATE(mode, 0, MODES_COUNT);
+	const static void (*modes[])(void) = {
+		NULL,
+		mode_setup_normal,
+		mode_setup_download,
+		poweroff
+	};
+
+	if (modes[mode])
+		modes[mode]();
 }
 
 __HIGH_CODE
@@ -81,17 +93,19 @@ static void bm_transition()
 		return;
 	}
 }
-void play_splash(xbm_t *xbm, int col, int row)
+void play_splash(xbm_t *xbm, int col, int row, int spT)
 {
 	while (ani_xbm_scrollup_pad(xbm, 11, 11, 11, fb, 0, 0) != 0) {
-		DelayMs(30);
+		DelayMs(spT);
 	}
 }
 
 void load_bmlist()
 {
-	if (data_get_header(0) == 0) // There is no bitmap stored in flash
-		return; // skip
+	data_legacy_t header;
+	data_get_header(&header);
+	if (memcmp(header.header, "wang", 5))
+		return; // There is no bitmap stored in flash
 
 	bm_t *curr_bm = bmlist_current();
 
@@ -205,48 +219,18 @@ void ble_setup()
 	tmos_clockInit();
 
 	peripheral_init();
-	ble_disable_advertise();
+
+	if (! badge_cfg.ble_always_on) {
+		ble_disable_advertise();
+	}
 
 	devInfo_registerService();
 	legacy_registerService();
 	batt_registerService();
+	ng_registerService();
 }
 
-static void usb_receive(uint8_t *buf, uint16_t len)
-{
-	static uint16_t rx_len, data_len;
-	static uint8_t *data;
-
-	PRINT("dump first 8 bytes: %02x %02x %02x %02x %02x %02x %02x %02x\n",
-				buf[0], buf[1], buf[2], buf[3],
-				buf[4], buf[5], buf[6], buf[7]);
-
-	if (rx_len == 0) {
-		if (memcmp(buf, "wang", 5))
-			return;
-
-		int init_len = len > LEGACY_HEADER_SIZE ? len : sizeof(data_legacy_t);
-		init_len += MAX_PACKET_SIZE;
-		data = malloc(init_len);
-	}
-
-	memcpy(data + rx_len, buf, len);
-	rx_len += len;
-
-	if (!data_len) {
-		data_legacy_t *d = (data_legacy_t *)data;
-		uint16_t n = bigendian16_sum(d->sizes, 8);
-		data_len = LEGACY_HEADER_SIZE + LED_ROWS * n;
-		data = realloc(data, data_len);
-	}
-
-	if ((rx_len > LEGACY_HEADER_SIZE) && rx_len >= data_len) {
-		data_flatSave(data, data_len);
-		SYS_ResetExecute();
-	}
-}
-
-void spawn_tasks()
+static void spawn_tasks()
 {
 	common_taskid = TMOS_ProcessEventRegister(common_tasks);
 
@@ -257,10 +241,8 @@ void spawn_tasks()
 	tmos_start_task(common_taskid, ANI_NEXT_STEP, 500000 / 625);
 }
 
-void ble_start()
+static void start_ble_animation()
 {
-	ble_enable_advertise();
-
 	tmos_stop_task(common_taskid, ANI_NEXT_STEP);
 	tmos_stop_task(common_taskid, ANI_MARQUE);
 	tmos_stop_task(common_taskid, ANI_FLASH);
@@ -269,35 +251,54 @@ void ble_start()
 	tmos_start_reload_task(common_taskid, BLE_NEXT_STEP, 500000 / 625);
 }
 
-void handle_mode_transition()
+static void start_normal_animation()
 {
-	static int prev_mode;
-	if (prev_mode == mode) return;
+	tmos_start_reload_task(common_taskid, ANI_MARQUE, ANI_MARQUE_SPEED_T / 625);
+	tmos_start_reload_task(common_taskid, ANI_FLASH, ANI_FLASH_SPEED_T / 625);
+	tmos_start_task(common_taskid, ANI_NEXT_STEP, 500000 / 625);
+	tmos_stop_task(common_taskid, BLE_NEXT_STEP);
+}
 
-	switch (mode)
-	{
-	case DOWNLOAD:
-		// Disable bitmap transition while in download mode
-		btn_onOnePress(KEY2, NULL);
-
-		// Take control of the current bitmap to display
-		// the Bluetooth animation
-		ble_start();
-		while (mode == DOWNLOAD) {
-			TMOS_SystemProcess();
-		}
-		// If not being flashed, pressing KEY1 again will
-		// make the badge goes off:
-
-		// fallthrough
-	case POWER_OFF:
-		poweroff();
-		break;
-
-	default:
-		break;
+static void resume_from_streaming()
+{
+	if (badge_cfg.ble_always_on) {
+		start_normal_animation();
+	} else {
+		start_ble_animation();
 	}
-	prev_mode = mode;
+}
+
+static void stop_all_animation()
+{
+	tmos_stop_task(common_taskid, ANI_NEXT_STEP);
+	tmos_stop_task(common_taskid, ANI_MARQUE);
+	tmos_stop_task(common_taskid, ANI_FLASH);
+	tmos_stop_task(common_taskid, BLE_NEXT_STEP);
+	memset(fb, 0, sizeof(fb));
+}
+
+int streaming_enabled;
+
+uint8_t streaming_setting(uint8_t *params, uint16_t len)
+{
+	if (params[0] == 0x00) { // enter streaming mode
+		stop_all_animation();
+		streaming_enabled = 1;
+	} else if (params[0] == 0x01) { // return to normal mode
+		resume_from_streaming();
+		streaming_enabled = 0;
+	}
+	return 0;
+}
+
+uint8_t stream_bitmap(uint8_t *params, uint16_t len)
+{
+	if (! streaming_enabled) {
+		return -1;
+	}
+
+	tmos_memcpy(fb, params, min(LED_COLS, len));
+	return 0;
 }
 
 static void debug_init()
@@ -363,6 +364,51 @@ static void disp_charging()
 	}
 }
 
+static void mode_setup_download()
+{
+	// If always-on BLE is enabled, then skip this mode, jump to next mode
+	if (badge_cfg.ble_always_on) {
+		change_mode();
+	}
+
+	// Disable bitmap transition while in download mode
+	btn_onOnePress(KEY2, NULL);
+
+	// Take control of the current bitmap to display
+	// the Bluetooth animation
+	ble_enable_advertise();
+	start_ble_animation();
+}
+
+void clean_bmlist()
+{
+	bm_t *curr_bm = bmlist_current();
+	while (curr_bm->next != curr_bm)
+		bmlist_drop(curr_bm->next);
+}
+
+void reload_bmlist()
+{
+	clean_bmlist();
+	load_bmlist();
+}
+
+static void mode_setup_normal()
+{
+	btn_onOnePress(KEY2, bm_transition);
+	reload_bmlist();
+	start_normal_animation();
+}
+
+void handle_after_rx()
+{
+	if (badge_cfg.reset_rx) {
+		SYS_ResetExecute();
+	} else {
+		mode_setup_normal();
+	}
+}
+
 int main()
 {
 	SetSysClock(CLK_SOURCE_PLL_60MHz);
@@ -370,12 +416,12 @@ int main()
 	debug_init();
 	PRINT("\nDebug console is on UART%d\n", DEBUG);
 
-	cdc_onWrite(usb_receive);
-	hiddev_onWrite(usb_receive);
+	cdc_onWrite(legacy_usb_rx);
+	hiddev_onWrite(legacy_usb_rx);
 	usb_start();
 
 	led_init();
-	TMR0_TimerInit(SCAN_T / 4);
+	TMR0_TimerInit((FREQ_SYS / 2000) / 2);
 	TMR0_ITCfg(ENABLE, TMR0_3_IT_CYC_END);
 	PFIC_EnableIRQ(TMR0_IRQn);
 
@@ -388,8 +434,14 @@ int main()
 
 	power_init();
 	disp_charging();
-
-	play_splash(&splash, 0, 0);
+	cfg_init();
+	xbm_t spl = {
+		.bits = &(badge_cfg.splash_bm_bits),
+		.w = badge_cfg.splash_bm_w,
+		.h = badge_cfg.splash_bm_h,
+		.fh = badge_cfg.splash_bm_fh,
+	};
+	play_splash(&spl, 0, 0, badge_cfg.splash_speedT);
 
 	load_bmlist();
 
@@ -399,7 +451,6 @@ int main()
 
 	mode = NORMAL;
 	while (1) {
-		handle_mode_transition();
 		TMOS_SystemProcess();
 	}
 }
diff --git a/src/ngctrl.c b/src/ngctrl.c
new file mode 100644
index 0000000..88eb7ee
--- /dev/null
+++ b/src/ngctrl.c
@@ -0,0 +1,161 @@
+#include "ngctrl.h"
+
+#include "CH58xBLE_LIB.h"
+#include "CH583SFR.h"
+
+#include "ble/profile.h"
+#include "power.h"
+#include "debug.h"
+#include "config.h"
+
+// TODO: Some of configs can be added, just listing:
+// - Remote brighness adjusting
+// - Upload bitmap to ram
+
+uint8_t next_packet(uint8_t *val, uint16_t len)
+{
+	PRINT(__func__);
+	PRINT(": to be implemented\n");
+	return 0;
+}
+
+static void cfg_reset_rx(uint8_t *state, uint16_t len)
+{
+	badge_cfg.reset_rx = !!state[0];
+}
+
+inline static void __poweroff(uint8_t *val, uint16_t len)
+{
+	poweroff();
+}
+
+inline static void __reset(uint8_t *val, uint16_t len)
+{
+	SYS_ResetExecute();
+}
+
+// Power off, reset after bitmap received.
+uint8_t power_setting(uint8_t *val, uint16_t len)
+{
+	PRINT(__func__);
+	PRINT("\n");
+
+	const void (*ble_lut[])(uint8_t *, uint16_t) = {
+		__poweroff,
+		cfg_reset_rx,
+		__reset,
+	};
+
+	uint8_t fn = val[0];
+	if (fn >= (sizeof(ble_lut) / sizeof(ble_lut[0])))
+		return -1;
+
+	ble_lut[fn](&val[1], len - 1);
+	return 0;
+}
+
+static void cfg_ble_devname(uint8_t *name, uint16_t len)
+{
+	tmos_memcpy(badge_cfg.ble_devname, name, len);
+}
+
+static void cfg_ble_alwayon(uint8_t *val, uint16_t len)
+{
+	badge_cfg.ble_always_on = val[0] != 0;
+}
+
+/* always-on, adv name
+Screen off/on can be archived by entering/leaving streaming mode
+ */
+uint8_t ble_setting(uint8_t *val, uint16_t len)
+{
+	PRINT(__func__);
+	PRINT("\n");
+
+	const void (*ble_lut[])(uint8_t *, uint16_t) = {
+		cfg_ble_alwayon,
+		cfg_ble_devname
+	};
+
+	uint8_t fn = val[0];
+	if (fn >= (sizeof(ble_lut) / sizeof(ble_lut[0])))
+		return -1;
+
+	ble_lut[fn](&val[1], len - 1);
+
+	return 0;
+}
+
+uint8_t flash_splash_screen(uint8_t *val, uint16_t len)
+{
+	PRINT(__func__);
+	PRINT("\n");
+	
+	uint8_t w = val[0];
+	uint8_t h = val[1];
+	uint8_t fh = val[2];
+	uint8_t sz = len - 3;
+
+	if (w > SPLASH_MAX_WIDTH)
+		return -1;
+	if (h > SPLASH_MAX_HEIGHT)
+		return -2;
+	if (sz > SPLASH_MAX_SIZE)
+		return -3;
+	if (len < 3)
+		return -4;
+
+	tmos_memcpy(badge_cfg.splash_bm_bits, &val[3], sz);
+	badge_cfg.splash_bm_w = w;
+	badge_cfg.splash_bm_h = h;
+	badge_cfg.splash_bm_fh = fh;
+
+	return 0;
+}
+
+uint8_t save_cfg(uint8_t *val, uint16_t len)
+{
+	PRINT(__func__);
+	PRINT("\n");
+	return (uint8_t)cfg_writeflash_def(&badge_cfg);
+}
+
+uint8_t load_fallback_cfg(uint8_t *val, uint16_t len)
+{
+	PRINT(__func__);
+	PRINT("\n");
+	cfg_fallback(&badge_cfg);
+	return 0;
+}
+/* TODO: add a way to read configs */
+const uint8_t (*cmd_lut[])(uint8_t *val, uint16_t len) = {
+	next_packet, // Unsure if we need this
+	power_setting,
+	streaming_setting,
+	stream_bitmap,
+	ble_setting,
+	flash_splash_screen,
+	save_cfg,
+	load_fallback_cfg,
+};
+
+#define CMD_LUT_LEN (sizeof(cmd_lut) / sizeof(cmd_lut[0]))
+
+uint8_t ng_parse(uint8_t *val, uint16_t len)
+{
+	uint8_t cmd = val[0];
+	PRINT("LUT_LEN: %02x \n", CMD_LUT_LEN);
+	if (cmd >= CMD_LUT_LEN) {
+		PRINT("invalid command!\n");
+		return bleInvalidRange;
+	}
+
+	if (cmd_lut[cmd]) {
+		PRINT("executing [cmd %02x] \n", cmd);
+		uint8_t ret = (*cmd_lut[cmd])(val + 1, len - 1);
+		ng_notify(&ret, 1); // response to the client app
+	} else {
+		PRINT("function is not defined!\n");
+	}
+	return SUCCESS;
+}
\ No newline at end of file
diff --git a/src/ngctrl.h b/src/ngctrl.h
new file mode 100644
index 0000000..460af28
--- /dev/null
+++ b/src/ngctrl.h
@@ -0,0 +1,20 @@
+#ifndef __NG_CTRL_H__
+#define __NG_CTRL_H__
+
+#include <stdint.h>
+
+// enum NG_STATUS {
+	
+// };
+
+uint8_t next_packet(uint8_t *val, uint16_t len);
+uint8_t power_setting(uint8_t *val, uint16_t len);
+uint8_t ble_setting(uint8_t *val, uint16_t len);
+uint8_t streaming_setting(uint8_t *val, uint16_t len);
+uint8_t stream_bitmap(uint8_t *val, uint16_t len);
+uint8_t flash_splash_screen(uint8_t *val, uint16_t len);
+
+extern const uint8_t (*cmd_lut[])(uint8_t *val, uint16_t len);
+uint8_t ng_parse(uint8_t *val, uint16_t len);
+
+#endif /* __NG_CTRL_H__ */
diff --git a/src/power.c b/src/power.c
index 84cc76d..da6ec0f 100644
--- a/src/power.c
+++ b/src/power.c
@@ -1,7 +1,8 @@
-#include <CH58xBLE_LIB.h>
+#include <CH58x_common.h>
 
 #include "power.h"
 #include "button.h"
+#include "debug.h"
 
 void poweroff()
 {
diff --git a/src/usb/debug.c b/src/usb/debug.c
index 0c9f443..95d756e 100644
--- a/src/usb/debug.c
+++ b/src/usb/debug.c
@@ -1,4 +1,4 @@
-#include "CH58x_common.h"
+#include "debug.h"
 
 void print_setuppk(USB_SETUP_REQ *request)
 {
diff --git a/src/usb/debug.h b/src/usb/debug.h
index 1279115..0aa0ebb 100644
--- a/src/usb/debug.h
+++ b/src/usb/debug.h
@@ -1,14 +1,11 @@
-#ifndef __DEBUG_H__
-#define __DEBUG_H__
+#ifndef __USB_DEBUG_H__
+#define __USB_DEBUG_H__
 
 #include "CH58x_common.h"
-
-
-#define _TRACE() PRINT("> "); PRINT(__FILE__);PRINT(": "); PRINT(__func__); \
-				PRINT("()\n")
+#include "../debug.h"
 
 void print_setuppk(USB_SETUP_REQ *setup_req_pk);
 void print_status_reg();
 void print_intflag_reg();
 
-#endif /* __DEBUG_H__ */
+#endif /* __USB_DEBUG_H__ */
diff --git a/src/util/crc.c b/src/util/crc.c
new file mode 100644
index 0000000..7efce15
--- /dev/null
+++ b/src/util/crc.c
@@ -0,0 +1,25 @@
+#include "crc.h"
+
+// Polynomial: x^8 + x^2 + x + 1 (0xE0)
+uint8_t crc8_ccitt_update (uint8_t inCrc, uint8_t inData)
+{
+	uint8_t data = inCrc ^ inData;
+	for (int i = 0; i < 8; i++) {
+		if (( data & 0x80 ) != 0 ) {
+			data <<= 1;
+			data ^= 0x07;
+		} else {
+			data <<= 1;
+		}
+	}
+	return data;
+}
+
+uint8_t crc_cal(uint8_t *a, int len)
+{
+	uint8_t crc = 0x00;
+	for (int i=0; i<len; i++) {
+		crc = crc8_ccitt_update(crc, a[i]);
+	}
+	return crc;
+}
\ No newline at end of file
diff --git a/src/util/crc.h b/src/util/crc.h
new file mode 100644
index 0000000..2a28813
--- /dev/null
+++ b/src/util/crc.h
@@ -0,0 +1,9 @@
+#ifndef __UTIL_CRC_H__
+#define __UTIL_CRC_H__
+
+#include <stdint.h>
+
+uint8_t crc8_ccitt_update (uint8_t inCrc, uint8_t inData);
+uint8_t crc_cal(uint8_t *a, int len);
+
+#endif /* __UTIL_CRC_H__ */
diff --git a/src/xbm.c b/src/xbm.c
index 94566d9..48cde4b 100644
--- a/src/xbm.c
+++ b/src/xbm.c
@@ -2,15 +2,13 @@
 #include <stdlib.h>
 #include <memory.h>
 
-#define ALIGNBIT(x) (((x % 8)>0)*8 + (x / 8) * 8)
-
 /**
  * Draw bitmap file to fb at (col, row)
  * if bitmap file larger than fb, it will be stripped out
  */
 void xbm2fb(xbm_t *xbm, uint16_t *fb, int col, int row)
 {
-	int W = ALIGNBIT(xbm->w);
+	int W = ALIGN_8BIT(xbm->w);
 	uint16_t *tmpfb = malloc(W * sizeof(uint16_t));
 	memset(tmpfb, 0, W * sizeof(uint16_t));
 
@@ -32,7 +30,7 @@ void xbm2fb(xbm_t *xbm, uint16_t *fb, int col, int row)
 void xbm2fb_dirty(xbm_t *xbm, uint16_t *fb, int col, int row)
 {
 	// Byte align for xbm bytes
-	int W = ALIGNBIT(xbm->w);
+	int W = ALIGN_8BIT(xbm->w);
 
 	for (int h = 0; h < xbm->h; h++) {
 		for (int w = 0; w < W; w++) {
@@ -48,8 +46,8 @@ void xbm2fb_dirty(xbm_t *xbm, uint16_t *fb, int col, int row)
  */
 xbm_t *xbm_croph(xbm_t *xbm, xbm_t *frame, int from_row, int to_row)
 {
-	int rb = xbm->w / 8 + ((xbm->w % 8)>0); // Number of bytes in a row
-	if (to_row * rb > rb * xbm->h)
+	int rb = ALIGN_1BYTE(xbm->w); // Number of bytes in a row
+	if (to_row > xbm->h)
 		return NULL;
 
 	frame->bits = &xbm->bits[rb * from_row];
diff --git a/src/xbm.h b/src/xbm.h
index b3f2c52..7bf9530 100644
--- a/src/xbm.h
+++ b/src/xbm.h
@@ -3,6 +3,9 @@
 
 #include <stdint.h>
 
+#define ALIGN_1BYTE(x) (((x % 8)>0) + (x / 8))
+#define ALIGN_8BIT(x) (ALIGN_1BYTE(x) * 8)
+
 typedef struct {
 	uint8_t *bits;
 	int     w; // Width