From 1091013c1f27c0eddf58b94d9ce3fbdefebdefdb Mon Sep 17 00:00:00 2001
From: pvvx <pvvx@mail.ru>
Date: Tue, 16 Jan 2024 16:06:44 +0300
Subject: [PATCH] add config & command

---
 bthome_phy6222/Makefile                 |   4 +-
 bthome_phy6222/SDK/misc/ota_phy6222.ld  | 140 ++++++++
 bthome_phy6222/source/cmd_parcer.c      | 119 +++++++
 bthome_phy6222/source/cmd_parcer.h      |  85 +++++
 bthome_phy6222/source/config.c          |  87 +++++
 bthome_phy6222/source/config.h          |  32 ++
 bthome_phy6222/source/devinfoservice.c  |  10 +-
 bthome_phy6222/source/devinfoservice.h  |   2 +-
 bthome_phy6222/source/flash_eep.h       |  14 +-
 bthome_phy6222/source/main.c            |   3 +-
 bthome_phy6222/source/osal_peripheral.c |   4 +-
 bthome_phy6222/source/sbp_profile.c     | 423 ++++++++++++++++++++++++
 bthome_phy6222/source/sbp_profile.h     |  85 +++++
 bthome_phy6222/source/sensor.h          |  12 +-
 bthome_phy6222/source/sensors.c         |  28 +-
 bthome_phy6222/source/thb2_main.c       |  95 ++----
 bthome_phy6222/source/thb2_peripheral.h |   1 +
 bthome_phy6222/source/thservice.c       |   2 +-
 18 files changed, 1050 insertions(+), 96 deletions(-)
 create mode 100644 bthome_phy6222/SDK/misc/ota_phy6222.ld
 create mode 100644 bthome_phy6222/source/cmd_parcer.c
 create mode 100644 bthome_phy6222/source/cmd_parcer.h
 create mode 100644 bthome_phy6222/source/config.c
 create mode 100644 bthome_phy6222/source/sbp_profile.c
 create mode 100644 bthome_phy6222/source/sbp_profile.h

diff --git a/bthome_phy6222/Makefile b/bthome_phy6222/Makefile
index dffc774..10626f0 100644
--- a/bthome_phy6222/Makefile
+++ b/bthome_phy6222/Makefile
@@ -14,9 +14,11 @@ SRC_PRJ += battservice.c
 SRC_PRJ += bthome_beacon.c
 SRC_PRJ += osal_peripheral.c
 SRC_PRJ += peripheral_main.c
-#SRC_PRJ += sbp_profile_ota.c
+SRC_PRJ += sbp_profile.c
 SRC_PRJ += devinfoservice.c
 SRC_PRJ += sensors.c
+SRC_PRJ += config.c
+SRC_PRJ += cmd_parcer.c
 SRC_PRJ += thb2_main.c
 SRC_PRJ += thb2_peripheral.c
 SRC_PRJ += thservice.c
diff --git a/bthome_phy6222/SDK/misc/ota_phy6222.ld b/bthome_phy6222/SDK/misc/ota_phy6222.ld
new file mode 100644
index 0000000..66cebe6
--- /dev/null
+++ b/bthome_phy6222/SDK/misc/ota_phy6222.ld
@@ -0,0 +1,140 @@
+
+
+MEMORY
+{
+  jumptbl (rwx) : ORIGIN = 0x1fff0000, LENGTH = 0x00400
+  gcfgtbl (rw)  : ORIGIN = 0x1fff0400, LENGTH = 0x00400
+  flash (rx)    : ORIGIN = 0x11005000, LENGTH = 0x0B000
+  sram (rwx)  	: ORIGIN = 0x1fff1838, LENGTH = 0x0E7C8
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(__start)
+
+SECTIONS
+{
+    .jumptbl : {
+       KEEP(*(jump_table_mem_area))
+    } > jumptbl  
+
+
+    .gcfgtbl : {
+      *(global_config_area)
+    } > gcfgtbl
+
+    .textentry : {
+        *(*.isr_vector)
+    } > sram
+
+
+    .init_section : {
+        _sinit = ABSOLUTE(.);
+        *(.init_array .init_array.*)
+        _einit = ABSOLUTE(.);
+    } > flash
+
+    .ARM.extab : {
+        *(.ARM.extab*)
+    } > flash
+
+    __exidx_start = ABSOLUTE(.);
+    .ARM.exidx : {
+        *(.ARM.exidx*)
+    } > flash
+    __exidx_end = ABSOLUTE(.);
+
+ 
+    ._sjtblsstore : {
+       _sjtblss = ABSOLUTE(.);
+    } > flash   
+
+
+    .data : {
+        _sdata = ABSOLUTE(.);
+        _stextram = ABSOLUTE(.);
+        *phy6222_start.o(.text)
+        *.o(_section_standby_code_)
+        *.o(_section_sram_code_)
+
+        *patch.o(.text.*)
+        *patch_ext_adv.o(.text.*)
+
+        *rf_phy_driver.o(.text.*)
+        *pwrmgr.o(.text .text.*)
+        *timer.o(.text .text.*)
+
+        *flash.o(.text .text.*)
+        *clock.o(.text.*)
+
+        *phy_sec_ext.o(.text .text.*)
+
+        *libc.a:lib_a-memset.o(.text.*)
+        *libc.a:lib_a-memcpy-stub.o(.text.*)
+        *libgcc.a:_udivsi3.o(.text)
+        *libgcc.a:_divsi3.o(.text)
+        *libgcc.a:_dvmd_tls.o(.text)
+/*        *libgcc.a:_thumb1_case_sqi.o(.text)
+        *libgcc.a:_thumb1_case_uqi.o(.text) */
+   
+        _etextram = ABSOLUTE(.);
+
+        *(.data .data.*)
+        *(.gnu.linkonce.d.*)
+        CONSTRUCTORS
+        . = ALIGN(4);
+        _edata = ABSOLUTE(.);
+    } > sram 
+
+    .bss : {
+        _sbss = ABSOLUTE(.);
+        *(.bss .bss.*)
+        *(.gnu.linkonce.b.*)
+        *(COMMON)
+        . = ALIGN(4);
+        _ebss = ABSOLUTE(.);
+    } > sram
+ 
+ /*   
+    .int_stack : {
+        . = ALIGN(4);
+        *(int_stack)
+        . = ALIGN(4);
+        _stack_top = ABSOLUTE(.);
+    } > sram
+*/
+    /* stack 32 kbytes */
+    g_top_irqstack = ORIGIN(sram) + LENGTH(sram);
+
+   .common_text : {
+        _stext = ABSOLUTE(.);   
+       *(.text .text.*)
+       *(.rodata .rodata.*)
+       *(.fixup)
+        *(.gnu.warning)
+        *(.rodata .rodata.*)
+        *(.gnu.linkonce.t.*)
+        *(.glue_7)
+        *(.glue_7t)
+        *(.got)
+        *(.gcc_except_table)
+        *(.gnu.linkonce.r.*)
+        _etext = ABSOLUTE(.);
+   } > flash   
+ 
+    /* Stabs debugging sections. */
+    .stab 0 : { *(.stab) }
+    .stabstr 0 : { *(.stabstr) }
+    .stab.excl 0 : { *(.stab.excl) }
+    .stab.exclstr 0 : { *(.stab.exclstr) }
+    .stab.index 0 : { *(.stab.index) }
+    .stab.indexstr 0 : { *(.stab.indexstr) }
+    .comment 0 : { *(.comment) }
+    .debug_abbrev 0 : { *(.debug_abbrev) }
+    .debug_info 0 : { *(.debug_info) }
+    .debug_line 0 : { *(.debug_line) }
+    .debug_pubnames 0 : { *(.debug_pubnames) }
+    .debug_aranges 0 : { *(.debug_aranges) }
+    
+}
+
diff --git a/bthome_phy6222/source/cmd_parcer.c b/bthome_phy6222/source/cmd_parcer.c
new file mode 100644
index 0000000..984aae0
--- /dev/null
+++ b/bthome_phy6222/source/cmd_parcer.c
@@ -0,0 +1,119 @@
+/*
+ * cmd_parcer.c
+ *
+ *  Created on: 16 янв. 2024 г.
+ *      Author: pvvx
+ */
+
+/*********************************************************************
+	INCLUDES
+*/
+#include "bcomdef.h"
+#include "config.h"
+#include "OSAL.h"
+#include "linkdb.h"
+#include "att.h"
+#include "gatt.h"
+#include "gatt_uuid.h"
+#include "gattservapp.h"
+#include "gapbondmgr.h"
+#include "flash_eep.h"
+#include "bleperipheral.h"
+#include "sbp_profile.h"
+#include "sensor.h"
+#include "cmd_parcer.h"
+
+/*********************************************************************/
+#define SEND_DATA_SIZE	18
+
+int cmd_parser(uint8_t * obuf, uint8_t * ibuf, uint32_t len) {
+	int olen = 0;
+	if (len) {
+		uint8_t cmd = ibuf[0];
+		obuf[0] = cmd;
+		obuf[1] = 0; // no err
+		if (cmd == CMD_ID_DEVID) { // Get DEV_ID
+			pdev_id_t p = (pdev_id_t) obuf;
+			//p->revision = 0; // already = 0
+			p->hw_version = 0x01;
+			p->sw_version = APP_VERSION;
+			p->dev_spec_data = 0;
+			p->services = 0;
+			olen = sizeof(dev_id_t);
+		} else if (cmd == CMD_ID_CFG) {		// Get/Set device config
+			if (--len > sizeof(cfg))
+				len = sizeof(cfg);
+			if (len) {
+				osal_memcpy(&cfg, &ibuf[1], len);
+				test_config();
+				flash_write_cfg(&cfg, EEP_ID_CFG, sizeof(cfg));
+			}
+			osal_memcpy(&obuf[1], &cfg, sizeof(cfg));
+			olen = sizeof(cfg) + 1;
+		} else if (cmd == CMD_ID_CFG_DEF) { // Set default device config
+			osal_memcpy(&cfg, &def_cfg, sizeof(cfg));
+			test_config();
+			flash_write_cfg(&cfg, EEP_ID_CFG, sizeof(cfg));
+			osal_memcpy(&obuf[1], &cfg, sizeof(cfg));
+			olen = sizeof(cfg) + 1;
+		} else if (cmd == CMD_ID_CFS) {	// Get/Set sensor config
+			if (--len > sizeof(thsensor_cfg.coef))
+				len = sizeof(thsensor_cfg.coef);
+			if (len) {
+				osal_memcpy(&thsensor_cfg.coef, &ibuf[1], len);
+				flash_write_cfg(&thsensor_cfg.coef, EEP_ID_CFS, sizeof(thsensor_cfg.coef));
+			}
+			osal_memcpy(&obuf[1], &thsensor_cfg.coef, sizeof(thsensor_cfg.coef));
+			olen = sizeof(thsensor_cfg.coef) + 1;
+		} else if (cmd == CMD_ID_CFS_DEF) {	// Get/Set default sensor config
+			osal_memcpy(&thsensor_cfg.coef, &def_thcoef, sizeof(thsensor_cfg.coef));
+			flash_write_cfg(&thsensor_cfg.coef, EEP_ID_CFS, sizeof(thsensor_cfg.coef));
+			osal_memcpy(&obuf[1], &thsensor_cfg.coef, sizeof(thsensor_cfg.coef));
+			olen = sizeof(thsensor_cfg.coef) + 1;
+
+	//---------- Debug commands (unsupported in different versions!):
+
+		} else if (cmd == CMD_ID_EEP_RW && len > 2) {
+			obuf[1] = ibuf[1];
+			obuf[2] = ibuf[2];
+			uint16_t id = ibuf[1] | (ibuf[2] << 8);
+			if(len > 3) {
+				flash_write_cfg(&ibuf[3], id, len - 3);
+			}
+			int16_t i = flash_read_cfg(&obuf[3], id, SEND_DATA_SIZE);
+			if(i < 0) {
+				obuf[1] = (uint8_t)(i & 0xff); // Error
+				olen = 2;
+			} else
+				olen = i + 3;
+		} else if (cmd == CMD_ID_MEM_RW && len > 4) { // Read/Write memory
+			uint8_t *p = (uint8_t *)(ibuf[1] | (ibuf[2]<<8) | (ibuf[3]<<16) | (ibuf[4]<<24));
+			if(len > 5) {
+				len -= 5;
+				osal_memcpy(p, &ibuf[5], len);
+			} else
+				len = SEND_DATA_SIZE;
+			osal_memcpy(&obuf, &ibuf, 5);
+			osal_memcpy(&obuf[5], p, len);
+			olen = len + 1 + 4;
+		} else if (cmd == CMD_ID_REG_RW && len > 4) { // Read/Write register
+			volatile uint32_t *p = (volatile uint32_t *)(ibuf[1] | (ibuf[2]<<8) | (ibuf[3]<<16) | (ibuf[4]<<24));
+			uint32_t tmp;
+			if(len > 8) {
+				tmp = ibuf[5] | (ibuf[6]<<8) | (ibuf[7]<<16) | (ibuf[8]<<24);
+				*p = tmp;
+			} else {
+				obuf[1] = 0xfe; // Error size
+				olen = 2;
+			}
+			tmp = *p;
+			osal_memcpy(&obuf, &ibuf, 5);
+			osal_memcpy(&obuf[5], &tmp, 4);
+			olen = 1 + 4 + 4;
+		} else {
+			obuf[1] = 0xff; // Error cmd
+			olen = 2;
+		}
+	}
+	return olen;
+}
diff --git a/bthome_phy6222/source/cmd_parcer.h b/bthome_phy6222/source/cmd_parcer.h
new file mode 100644
index 0000000..a4a20bb
--- /dev/null
+++ b/bthome_phy6222/source/cmd_parcer.h
@@ -0,0 +1,85 @@
+/*
+ * cmd_parcer.h
+ *
+ *  Created on: 16 янв. 2024 г.
+ *      Author: pvvx
+ */
+
+#ifndef _CMD_PARCER_H_
+#define _CMD_PARCER_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+//#include "types.h"
+/*********************************************************************
+ * CONSTANTS
+ */
+// A complete list of interface commands for different devices.
+// Not all commands are supported by a specific device (!)
+enum CMD_ID_KEYS {
+	CMD_ID_DEVID 	= 0x00, // Get dev id, version, services
+	CMD_ID_DNAME    = 0x01, // Get/Set device name, "\0" - default: THB2_xxxx
+	CMD_ID_GDEVS 	= 0x02, // Get address devices
+	CMD_ID_I2C_SCAN = 0x03, // I2C scan
+	CMD_ID_I2C_UTR	= 0x04, // Universal I2C/SMBUS read-write
+	CMD_ID_SEN_ID	= 0x05,	// Get sensor ID
+	CMD_ID_FLASH_ID	= 0x06,	// Get Flash JEDEC ID
+	CMD_ID_DEV_MAC	= 0x10, // Get/Set MAC [+RandMAC], [size][mac[6][randmac[2]]]
+	CMD_ID_BKEY		= 0x18, // Get/Set beacon bindkey in EEP
+	CMD_ID_COMFORT  = 0x20, // Get/Set comfort parameters
+	CMD_ID_EXTDATA  = 0x22, // Get/Set show ext. data
+	CMD_ID_UTC_TIME = 0x23, // Get/Set utc time (if USE_CLOCK = 1)
+	CMD_ID_TADJUST  = 0x24, // Get/Set adjust time clock delta (in 1/16 us for 1 sec)
+	CMD_ID_CFS  	= 0x25, // Get/Set sensor config
+	CMD_ID_CFS_DEF 	= 0x26, // Get/Set default sensor config
+	CMD_ID_MEASURE  = 0x33, // Start/stop notify measures in connection mode
+	CMD_ID_LOGGER   = 0x35, // Read memory measures
+	CMD_ID_CLRLOG	= 0x36, // Clear memory measures
+	CMD_ID_RDS      = 0x40, // Get/Set Reed switch config (DIY devices)
+	CMD_ID_TRG      = 0x44, // Get/Set trg and Reed switch data config
+	CMD_ID_TRG_OUT  = 0x45, // Get/Set trg out, Send Reed switch and trg data
+	CMD_ID_HXC      = 0x49, // Get/Set HX71X config
+	CMD_ID_CFG      = 0x55,	// Get/Set device config
+	CMD_ID_CFG_DEF  = 0x56,	// Set default device config
+	CMD_ID_LCD_DUMP = 0x60, // Get/Set lcd buf
+	CMD_ID_LCD_FLG  = 0x61, // Start/Stop notify lcd dump and ...
+	CMD_ID_PINCODE  = 0x70, // Set new PinCode 0..999999
+	CMD_ID_MTU		= 0x71, // Request Mtu Size Exchange (23..255)
+	CMD_ID_REBOOT	= 0x72, // Set Reboot on disconnect
+	CMD_ID_SET_OTA	= 0x73, // Extension BigOTA: Get/set address and size OTA, erase sectors
+
+	// Debug commands (unsupported in different versions!):
+
+	CMD_ID_OTAC		= 0xD1,	// OTA clear
+	CMD_ID_WRFB		= 0xD3,	// Write Flash
+	CMD_ID_RDFB		= 0xD4,	// Read Flash Block
+	CMD_ID_ERFB		= 0xD5,	// Erase Flash Sector
+	CMD_ID_CHGB		= 0xD7,	// Change boot
+	CMD_ID_REG_RW	= 0xDA,	// Read/Write Registers
+	CMD_ID_MEM_RW	= 0xDB,	// Read/Write memory
+	CMD_ID_EEP_RW	= 0xDC,	// Get/set EEP
+	CMD_ID_LR_RESET = 0xDD,	// Reset Long Range
+	CMD_ID_DEBUG    = 0xDE	// Test/Debug
+
+};
+
+
+// CMD_ID_DEV_ID
+typedef struct _dev_id_t{
+	uint8_t pid;			// packet identifier = CMD_ID_DEV_ID
+	uint8_t revision;		// protocol version/revision
+	uint16_t hw_version;	// hardware version
+	uint16_t sw_version;	// software version (BCD)
+	uint16_t dev_spec_data;	// device-specific data
+	uint32_t services;		// supported services by the device
+} dev_id_t, * pdev_id_t;
+
+int cmd_parser(uint8_t * obuf, uint8_t * ibuf, uint32_t len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _CMD_PARCER_H_ */
diff --git a/bthome_phy6222/source/config.c b/bthome_phy6222/source/config.c
new file mode 100644
index 0000000..22d997a
--- /dev/null
+++ b/bthome_phy6222/source/config.c
@@ -0,0 +1,87 @@
+/*
+ * config.c
+ *
+ *  Created on: 16 янв. 2024 г.
+ *      Author: pvvx
+ */
+/*********************************************************************
+ * INCLUDES
+ */
+#include "bcomdef.h"
+#include "config.h"
+#include "rf_phy_driver.h"
+#include "global_config.h"
+#include "OSAL.h"
+#include "OSAL_PwrMgr.h"
+#include "gatt.h"
+#include "hci.h"
+#include "gapgattserver.h"
+#include "gattservapp.h"
+#include "devinfoservice.h"
+#include "ota_app_service.h"
+#include "thb2_peripheral.h"
+#include "gapbondmgr.h"
+#include "pwrmgr.h"
+#include "gpio.h"
+#include "bleperipheral.h"
+#include "ll.h"
+#include "ll_hw_drv.h"
+#include "ll_def.h"
+#include "hci_tl.h"
+#include "flash.h"
+//#include "fs.h"
+#include "flash_eep.h"
+#include "battservice.h"
+#include "thservice.h"
+#include "thb2_peripheral.h"
+#include "bthome_beacon.h"
+#include "sensor.h"
+#include "battery.h"
+#include "sbp_profile.h"
+
+extern gapPeriConnectParams_t periConnParameters;
+
+cfg_t cfg;
+
+const cfg_t def_cfg = {
+		.rf_tx_power = RF_PHY_TX_POWER_0DBM,
+		.advertising_interval = 80, // 80 * 62.5 = 5000 ms
+		.measure_interval = 2,  // 5 * 2 = 10 sec
+		.batt_interval = 6, // 60 sec
+		.connect_latency = 29,	// 30*30 = 900 ms
+		.averaging_measurements = 180 // 180*10 = 1800 sec, 30 min
+};
+
+void test_config(void) {
+	if (cfg.rf_tx_power > RF_PHY_TX_POWER_EXTRA_MAX)
+		cfg.rf_tx_power = RF_PHY_TX_POWER_EXTRA_MAX;
+	g_rfPhyTxPower = cfg.rf_tx_power;
+
+	gapRole_MinConnInterval = periConnParameters.intervalMin = DEFAULT_DESIRED_MIN_CONN_INTERVAL;
+	gapRole_MaxConnInterval = periConnParameters.intervalMax = DEFAULT_DESIRED_MAX_CONN_INTERVAL;
+	gapRole_SlaveLatency = periConnParameters.latency = cfg.connect_latency;
+	periConnParameters.timeout = (cfg.connect_latency + 1) * 3 * 4;
+	if(periConnParameters.timeout > 2048)
+		periConnParameters.timeout = 2048; // 20.48 sec мax
+	gapRole_TimeoutMultiplier = periConnParameters.timeout;
+
+	if(cfg.advertising_interval == 0)
+		cfg.advertising_interval = 1;
+	if(cfg.measure_interval == 0)
+		cfg.measure_interval = 1;
+	adv_wrk.measure_interval_ms = cfg.advertising_interval * cfg.measure_interval * 625 / 10;
+}
+
+void load_eep_config(void) {
+	if(!flash_supported_eep_ver(0, APP_VERSION)) {
+		osal_memcpy(&cfg, &def_cfg, sizeof(cfg));
+		osal_memcpy(&thsensor_cfg.coef, &def_thcoef, sizeof(thsensor_cfg.coef));
+	} else {
+		if (flash_read_cfg(&cfg, EEP_ID_CFG, sizeof(cfg)) != sizeof(cfg))
+			osal_memcpy(&cfg, &def_cfg, sizeof(cfg));
+		if (flash_read_cfg(&thsensor_cfg.coef, EEP_ID_CFS, sizeof(thsensor_cfg.coef)) != sizeof(thsensor_cfg.coef))
+			osal_memcpy(&thsensor_cfg.coef, &def_thcoef, sizeof(thsensor_cfg.coef));
+	}
+	test_config();
+}
+
diff --git a/bthome_phy6222/source/config.h b/bthome_phy6222/source/config.h
index f5e693c..83aff16 100644
--- a/bthome_phy6222/source/config.h
+++ b/bthome_phy6222/source/config.h
@@ -8,6 +8,8 @@
 #ifndef SOURCE_CONFIG_H_
 #define SOURCE_CONFIG_H_
 
+#include "types.h"
+
 #ifndef APP_VERSION
 #define APP_VERSION	0x05	// BCD
 #endif
@@ -72,4 +74,34 @@
 // Supervision timeout value (units of 10ms, 1000=10s) if automatic parameter update request is enabled
 #define DEFAULT_DESIRED_CONN_TIMEOUT			400 // 4s
 
+typedef struct /*__attribute__((packed))*/ _cfg_t {
+	uint32_t flg;
+
+	uint8_t rf_tx_power; //
+	uint8_t advertising_interval; // multiply by 62.5 for value in ms (1..160,  62.5 ms .. 10 sec)
+	uint8_t connect_latency; // +1 x 0.03 sec ( = connection interval), Tmin = 1*30 = 30 ms, Tmax = 256 * 30 = 7680 ms
+	uint8_t reserved1;
+
+	uint8_t measure_interval; // measure_interval
+	uint8_t batt_interval; // * measure_interval
+	uint8_t averaging_measurements; // * measure_interval, 0 - off, 1..255 * measure_interval
+	uint8_t reserved2;
+
+}cfg_t;
+
+extern cfg_t cfg;
+extern const cfg_t def_cfg;
+
+typedef struct _adv_work_t {
+	uint32_t	measure_interval_ms;
+	uint8_t		adv_count;
+	uint8_t		adv_batt_count;
+	uint8_t 	adv_con_count;
+} adv_work_t;
+
+extern adv_work_t adv_wrk;
+
+void test_config(void);
+void load_eep_config(void);
+
 #endif /* SOURCE_CONFIG_H_ */
diff --git a/bthome_phy6222/source/devinfoservice.c b/bthome_phy6222/source/devinfoservice.c
index 8ff74ca..4505488 100644
--- a/bthome_phy6222/source/devinfoservice.c
+++ b/bthome_phy6222/source/devinfoservice.c
@@ -19,7 +19,6 @@
 #include "gatt_profile_uuid.h"
 #include "gattservapp.h"
 
-
 #include "devinfoservice.h"
 
 /*********************************************************************
@@ -157,7 +156,7 @@ const uint8 devInfoModelNumber[]		=	DEF_MODEL_NUMBER_STR;
 #if	SERIAL_NUMBER_STR_ENABLE
 // Serial Number String characteristic
 static uint8 devInfoSerialNumberProps	=	GATT_PROP_READ;
-uint8 devInfoSerialNumber[19]; // = "000000-00000000-0000"; // FLASH_ID-SENSOR_ID-EFUSE
+uint8 devInfoSerialNumber[21]; // = "000000-00000000-0000"; // FLASH_ID-SENSOR_ID-EFUSE
 #endif
 
 #if FIRMWARE_REVISION_ENABLE
@@ -346,11 +345,11 @@ bStatus_t DevInfo_SetParameter( uint8 param, uint8 len, void* value )
 
 	switch ( param )
 	{
-	#if	SYSTEM_ID_ENABLE
+#if	SYSTEM_ID_ENABLE
 		case DEVINFO_SYSTEM_ID:
 		osal_memcpy(devInfoSystemId, value, len);
 		break;
-	#endif
+#endif
 
 		default:
 		ret = INVALIDPARAMETER;
@@ -599,6 +598,3 @@ static uint8 devInfo_ReadAttrCB( uint16 connHandle, gattAttribute_t* pAttr,
 	return ( status );
 }
 
-
-/*********************************************************************
-*********************************************************************/
diff --git a/bthome_phy6222/source/devinfoservice.h b/bthome_phy6222/source/devinfoservice.h
index 3d0ed80..a6d22c1 100644
--- a/bthome_phy6222/source/devinfoservice.h
+++ b/bthome_phy6222/source/devinfoservice.h
@@ -99,7 +99,7 @@ bStatus_t DevInfo_SetParameter( uint8 param, uint8 len, void* value );
 extern bStatus_t DevInfo_GetParameter( uint8 param, void* value );
 
 
-extern uint8 devInfoSerialNumber[19];
+extern uint8 devInfoSerialNumber[21];
 extern const uint8 devInfoModelNumber[];
 /*********************************************************************
 *********************************************************************/
diff --git a/bthome_phy6222/source/flash_eep.h b/bthome_phy6222/source/flash_eep.h
index c67cf8b..3ae3dfc 100644
--- a/bthome_phy6222/source/flash_eep.h
+++ b/bthome_phy6222/source/flash_eep.h
@@ -15,13 +15,13 @@ extern "C" {
 // EEPROM IDs
 #define EEP_ID_MAC (0xACAD) // EEP ID MAC
 #define EEP_ID_CFG (0x0CFC) // EEP ID config data
-#define EEP_ID_TRG (0x0DFE) // EEP ID trigger data
-#define EEP_ID_RPC (0x0DF5) // EEP ID reed switch pulse counter
-#define EEP_ID_PCD (0xC0DE) // EEP ID pincode
-#define EEP_ID_CMF (0x0FCC) // EEP ID comfort data
-#define EEP_ID_DVN (0xDEAE) // EEP ID device name
-#define EEP_ID_TIM (0x0ADA) // EEP ID time adjust
-#define EEP_ID_KEY (0xBC0D) // EEP ID bkey
+#define EEP_ID_CFS (0x0CF5) // EEP ID sensor coefficients
+//#define EEP_ID_RPC (0x0DF5) // EEP ID reed switch pulse counter
+//#define EEP_ID_PCD (0xC0DE) // EEP ID pincode
+//#define EEP_ID_CMF (0x0FCC) // EEP ID comfort data
+//#define EEP_ID_DVN (0xDEAE) // EEP ID device name
+//#define EEP_ID_TIM (0x0ADA) // EEP ID time adjust
+//#define EEP_ID_KEY (0xBC0D) // EEP ID bkey
 #define EEP_ID_VER (0x5555) // EEP ID blk: unsigned int = minimum supported version
 //-----------------------------------------------------------------------------
 #ifndef FLASH_BASE_ADDR
diff --git a/bthome_phy6222/source/main.c b/bthome_phy6222/source/main.c
index 13ec72e..5ccd14f 100644
--- a/bthome_phy6222/source/main.c
+++ b/bthome_phy6222/source/main.c
@@ -275,7 +275,6 @@ static void hal_init(void) {
 	hal_gpio_init();
 	LOG_INIT();
 	//hal_fs_init(0x1103C000, 2);
-	flash_supported_eep_ver(0, APP_VERSION);
 	hal_adc_init();
 }
 
@@ -312,6 +311,8 @@ int main(void) {
 	hal_rfphy_init();
 	hal_init();
 
+	load_eep_config();
+
 	LOG("SDK Version ID %08x \n",SDK_VER_RELEASE_ID);
 	LOG("rfClk %d rcClk %d sysClk %d tpCap[%02x %02x]\n",g_rfPhyClkSel,g_clk32K_config,g_system_clk,g_rfPhyTpCal0,g_rfPhyTpCal1);
 	LOG("sizeof(struct ll_pkt_desc) = %d, buf size = %d\n", sizeof(struct ll_pkt_desc), BLE_CONN_BUF_SIZE);
diff --git a/bthome_phy6222/source/osal_peripheral.c b/bthome_phy6222/source/osal_peripheral.c
index 2e78922..6114469 100644
--- a/bthome_phy6222/source/osal_peripheral.c
+++ b/bthome_phy6222/source/osal_peripheral.c
@@ -92,11 +92,11 @@ void osalInitTasks( void )
 	LL_Init( taskID++ );
 	/* HCI Task */
 	HCI_Init( taskID++ );
-	#if defined ( OSAL_CBTIMER_NUM_TASKS )
+#if defined ( OSAL_CBTIMER_NUM_TASKS )
 	/* Callback Timer Tasks */
 	osal_CbTimerInit( taskID );
 	taskID += OSAL_CBTIMER_NUM_TASKS;
-	#endif
+#endif
 	/* L2CAP Task */
 	L2CAP_Init( taskID++ );
 	/* SM Task */
diff --git a/bthome_phy6222/source/sbp_profile.c b/bthome_phy6222/source/sbp_profile.c
new file mode 100644
index 0000000..a70820f
--- /dev/null
+++ b/bthome_phy6222/source/sbp_profile.c
@@ -0,0 +1,423 @@
+/**************************************************************************************************
+	Filename:		sbpProfile.c
+	Revised:
+	Revision:
+	Description:	This file contains the Simple GATT profile sample GATT service
+				  profile for use with the BLE sample application.
+**************************************************************************************************/
+
+/*********************************************************************
+	INCLUDES
+*/
+#include "bcomdef.h"
+#include "config.h"
+#include "OSAL.h"
+#include "linkdb.h"
+#include "att.h"
+#include "gatt.h"
+#include "gatt_uuid.h"
+#include "gattservapp.h"
+#include "gapbondmgr.h"
+//#include "log.h"
+#include "thb2_peripheral.h"
+#include "bleperipheral.h"
+#include "sbp_profile.h"
+#include "cmd_parcer.h"
+
+/*********************************************************************
+ * MACROS
+ */
+
+/*********************************************************************
+ * CONSTANTS
+ */
+/*********************************************************************
+ * TYPEDEFS
+ */
+
+/*********************************************************************
+ * GLOBAL VARIABLES
+ */
+// Simple GATT Profile Service UUID: 0xFFF0
+CONST uint8_t simpleProfileServUUID[ATT_BT_UUID_SIZE] =
+{
+	LO_UINT16(SIMPLEPROFILE_SERV_UUID), HI_UINT16(SIMPLEPROFILE_SERV_UUID)
+};
+
+// Characteristic 1 UUID: 0xFFF3
+CONST uint8_t simpleProfilechar1UUID[ATT_BT_UUID_SIZE] =
+{
+	LO_UINT16(SIMPLEPROFILE_CHAR1_UUID), HI_UINT16(SIMPLEPROFILE_CHAR1_UUID)
+};
+
+// Characteristic 2 UUID: 0xFFF4
+CONST uint8_t simpleProfilechar2UUID[ATT_BT_UUID_SIZE] =
+{
+	LO_UINT16(SIMPLEPROFILE_CHAR2_UUID), HI_UINT16(SIMPLEPROFILE_CHAR2_UUID)
+};
+
+/*********************************************************************
+ * EXTERNAL VARIABLES
+ */
+
+/*********************************************************************
+ * EXTERNAL FUNCTIONS
+ */
+
+/*********************************************************************
+ * LOCAL VARIABLES
+ */
+
+static simpleProfileCBs_t *simpleProfile_AppCBs = NULL;
+//static	uint8_t	ReadNotify_Len		=	0;
+
+/*********************************************************************
+ * Profile Attributes - variables
+ */
+
+// Simple Profile Service attribute 0xFFF0
+static CONST gattAttrType_t simpleProfileService = { ATT_BT_UUID_SIZE, simpleProfileServUUID };
+
+
+// Simple Profile Characteristic 1 Properties
+static CONST uint8_t simpleProfileChar1Props			=	GATT_PROP_READ | GATT_PROP_WRITE | GATT_PROP_WRITE_NO_RSP;
+//static CONST uint8_t simpleProfileChar1UserDesp[]		=	"OTA\0";	// Simple Profile Characteristic 1 User Description
+
+static uint8_t ota_in_buffer[20];			// Characteristic 1 Value
+static uint8_t ota_in_len;
+
+// Simple Profile Characteristic 2 Properties
+static CONST uint8_t simpleProfileChar2Props			=	GATT_PROP_READ | GATT_PROP_WRITE | GATT_PROP_WRITE_NO_RSP | GATT_PROP_NOTIFY;
+//static CONST uint8_t simpleProfileChar2UserDesp[]		=	"CMD\0";	// Simple Profile Characteristic 2 User Description
+static gattCharCfg_t simpleProfileChar2Config[GATT_MAX_NUM_CONN];		//
+
+static uint8_t cmd_in_buffer[32];			// Characteristic 2 Value
+static uint8_t cmd_in_len;							// Characteristic 2 Value
+
+/*********************************************************************
+ * Profile Attributes - Table
+ */
+
+#define SERVAPP_NUM_ATTR_SUPPORTED		  6
+#define OTA_DATA_ATTR_IDX				  2 // Position of OTA in attribute array
+#define CDM_DATA_ATTR_IDX				  4 // Position of CMD in attribute array
+
+static gattAttribute_t simpleProfileAttrTbl[SERVAPP_NUM_ATTR_SUPPORTED] =
+{
+	// Simple Profile Service
+	{
+			{ ATT_BT_UUID_SIZE, primaryServiceUUID },	/* type */
+			GATT_PERMIT_READ,							/* permissions */
+			0,											/* handle */
+			(uint8_t *)&simpleProfileService			/* pValue */
+	},
+
+	// Characteristic 1 Declaration
+	{
+			{ ATT_BT_UUID_SIZE, characterUUID },
+			GATT_PERMIT_READ,
+			0,
+			(uint8_t *)&simpleProfileChar1Props
+	},
+	// Characteristic Value 1
+	{
+			{ ATT_BT_UUID_SIZE, simpleProfilechar1UUID },
+			GATT_PERMIT_READ | GATT_PERMIT_WRITE,
+			0,
+			(uint8_t *)&ota_in_buffer[0]
+	},
+#if 0
+	// Characteristic 1 User Description
+	{
+			{ ATT_BT_UUID_SIZE, charUserDescUUID },
+			GATT_PERMIT_READ,
+			0,
+			(uint8_t *)simpleProfileChar1UserDesp
+	},
+#endif
+	// Characteristic 2 Declaration
+	{
+			{ ATT_BT_UUID_SIZE, characterUUID },
+			GATT_PERMIT_READ,
+			0,
+			(uint8_t *)&simpleProfileChar2Props
+	},
+	// Characteristic Value 2
+	{
+			{ ATT_BT_UUID_SIZE, simpleProfilechar2UUID },
+			GATT_PERMIT_READ | GATT_PERMIT_WRITE,
+			0,
+			(uint8_t *)&cmd_in_buffer[0]
+	},
+	// Characteristic 2 configuration
+	{
+			{ ATT_BT_UUID_SIZE, clientCharCfgUUID },
+			GATT_PERMIT_READ | GATT_PERMIT_WRITE,
+			0,
+			(uint8_t *)simpleProfileChar2Config
+	},
+#if 0
+	// Characteristic 2 User Description
+	{
+			{ ATT_BT_UUID_SIZE, charUserDescUUID },
+			GATT_PERMIT_READ,
+			0,
+			(uint8_t *)simpleProfileChar2UserDesp
+	},
+#endif
+};
+
+
+/*********************************************************************
+ * LOCAL FUNCTIONS
+ */
+static bStatus_t	simpleProfile_ReadAttrCB( uint16_t connHandle, gattAttribute_t *pAttr,uint8_t *pValue, uint16_t *pLen, uint16_t offset, uint8_t maxLen );
+static bStatus_t	simpleProfile_WriteAttrCB( uint16_t connHandle, gattAttribute_t *pAttr,uint8_t *pValue, uint16_t len, uint16_t offset );
+static void			simpleProfile_HandleConnStatusCB( uint16_t connHandle, uint8_t changeType );
+/*********************************************************************
+ * PROFILE CALLBACKS
+ */
+// Simple Profile Service Callbacks
+CONST gattServiceCBs_t simpleProfileCBs =
+{
+	simpleProfile_ReadAttrCB,  // Read callback function pointer
+	simpleProfile_WriteAttrCB, // Write callback function pointer
+	NULL					   // Authorization callback function pointer
+};
+
+/*********************************************************************
+ * PUBLIC FUNCTIONS
+ */
+
+/*********************************************************************
+ * @fn		SimpleProfile_AddService
+ *
+ * @brief	Initializes the Simple Profile service by registering
+ *			GATT attributes with the GATT server.
+ *
+ * @param	services - services to add. This is a bit map and can
+ *					   contain more than one service.
+ *
+ * @return	Success or Failure
+ */
+bStatus_t SimpleProfile_AddService( uint32_t services )
+{
+	uint8_t status = SUCCESS;
+
+	// Initialize Client Characteristic Configuration attributes
+	GATTServApp_InitCharCfg( INVALID_CONNHANDLE, simpleProfileChar2Config );
+
+	// Register with Link DB to receive link status change callback
+	VOID linkDB_Register( simpleProfile_HandleConnStatusCB );
+
+	if ( services & SIMPLEPROFILE_SERVICE )
+	{
+		// Register GATT attribute list and CBs with GATT Server App
+		status = GATTServApp_RegisterService( simpleProfileAttrTbl,
+					GATT_NUM_ATTRS( simpleProfileAttrTbl ),
+					&simpleProfileCBs );
+	}
+
+	return ( status );
+}
+
+
+/*********************************************************************
+ * @fn		SimpleProfile_RegisterAppCBs
+ *
+ * @brief	Registers the application callback function. Only call
+ *			this function once.
+ *
+ * @param	callbacks - pointer to application callbacks.
+ *
+ * @return	SUCCESS or bleAlreadyInRequestedMode
+ */
+bStatus_t SimpleProfile_RegisterAppCBs( simpleProfileCBs_t *appCallbacks )
+{
+	if ( appCallbacks ){
+		simpleProfile_AppCBs = appCallbacks;
+		return ( SUCCESS );
+	}else{
+		return ( bleAlreadyInRequestedMode );
+	}
+}
+
+
+/*********************************************************************
+ * @fn			simpleProfile_ReadAttrCB
+ *
+ * @brief		Read an attribute.
+ *
+ * @param		connHandle - connection message was received on
+ * @param		pAttr - pointer to attribute
+ * @param		pValue - pointer to data to be read
+ * @param		pLen - length of data to be read
+ * @param		offset - offset of the first octet to be read
+ * @param		maxLen - maximum length of data to be read
+ *
+ * @return		Success or Failure
+ */
+static bStatus_t simpleProfile_ReadAttrCB( uint16_t connHandle, gattAttribute_t *pAttr,
+							uint8_t *pValue, uint16_t *pLen, uint16_t offset, uint8_t maxLen )
+{
+	(void)connHandle;
+	(void)maxLen;
+	bStatus_t status = SUCCESS;
+
+	// If attribute permissions require authorization to read, return error
+	if ( gattPermitAuthorRead( pAttr->permissions ) )
+	{
+		// Insufficient authorization
+		return ( ATT_ERR_INSUFFICIENT_AUTHOR );
+	}
+
+	// Make sure it's not a blob operation (no attributes in the profile are long)
+	if ( offset > 0 )
+	{
+		return ( ATT_ERR_ATTR_NOT_LONG );
+	}
+
+	if ( pAttr->type.len == ATT_BT_UUID_SIZE )
+	{
+		// 16-bit UUID
+		uint16_t uuid = BUILD_UINT16( pAttr->type.uuid[0], pAttr->type.uuid[1]);
+		switch ( uuid )
+		{
+			// No need for "GATT_SERVICE_UUID" or "GATT_CLIENT_CHAR_CFG_UUID" cases;
+			// gattserverapp handles those reads
+			case SIMPLEPROFILE_CHAR1_UUID:
+				*pLen = sizeof(ota_in_buffer);
+				osal_memcpy( pValue, pAttr->pValue, *pLen );
+				LOG("Read_UUID1:\n");
+				break;
+			case SIMPLEPROFILE_CHAR2_UUID:
+				*pLen = sizeof(cfg);
+				osal_memcpy( pValue, &cfg, *pLen );
+				LOG("Read_UUID2:\n");
+				break;
+			default:
+				// Should never get here! (characteristics 3 and 4 do not have read permissions)
+				*pLen = 0;
+				status = ATT_ERR_ATTR_NOT_FOUND;
+			break;
+		}
+	} else {
+		// 128-bit UUID
+		*pLen = 0;
+		status = ATT_ERR_INVALID_HANDLE;
+	}
+	return ( status );
+}
+
+/*********************************************************************
+ * @fn		simpleProfile_WriteAttrCB
+ *
+ * @brief	Validate attribute data prior to a write operation
+ *
+ * @param	connHandle - connection message was received on
+ * @param	pAttr - pointer to attribute
+ * @param	pValue - pointer to data to be written
+ * @param	len - length of data
+ * @param	offset - offset of the first octet to be written
+ *
+ * @return	Success or Failure
+ */
+ static bStatus_t simpleProfile_WriteAttrCB( uint16_t connHandle, gattAttribute_t *pAttr,
+								 uint8_t *pValue, uint16_t len, uint16_t offset )
+{
+	bStatus_t status = SUCCESS;
+
+	// If attribute permissions require authorization to write, return error
+	if ( gattPermitAuthorWrite( pAttr->permissions ) )
+	{
+		// Insufficient authorization
+		return ( ATT_ERR_INSUFFICIENT_AUTHOR );
+	}
+
+	if ( pAttr->type.len == ATT_BT_UUID_SIZE )
+	{
+		// 16-bit UUID
+		uint16_t uuid = BUILD_UINT16( pAttr->type.uuid[0], pAttr->type.uuid[1]);
+		switch ( uuid )
+		{
+			case SIMPLEPROFILE_CHAR1_UUID:
+	            //Validate the value
+	            // Make sure it's not a blob oper
+	            if ( offset == 0 ) {
+					if ( len > sizeof(ota_in_buffer))
+						status = ATT_ERR_INVALID_VALUE_SIZE;
+	            } else
+	                status = ATT_ERR_ATTR_NOT_LONG;
+
+	            //Write the value
+	            if ( status == SUCCESS) {
+	            	osal_memcpy( pAttr->pValue, pValue, len );
+	            	ota_in_len = len;
+	            }
+				break;
+
+			case SIMPLEPROFILE_CHAR2_UUID:
+				// Validate the value
+				// Make sure it's not a blob oper
+				if ( offset == 0 ) {
+					if ( len > sizeof(cmd_in_buffer))
+						status = ATT_ERR_INVALID_VALUE_SIZE;
+				} else
+					status = ATT_ERR_ATTR_NOT_LONG;
+				// Write the value
+				if ( status == SUCCESS ) {
+					osal_memcpy(pAttr->pValue, pValue, len );
+					cmd_in_len = len;
+					LOG("receive data = 0x ");
+					LOG_DUMP_BYTE(pCurValue, len);
+					osal_set_event(simpleBLEPeripheral_TaskID, SBP_DEALDATA);
+				}
+				break;
+
+			case GATT_CLIENT_CHAR_CFG_UUID:
+				LOG("Enable/Disable Notity\n");
+				status = GATTServApp_ProcessCCCWriteReq( connHandle, pAttr, pValue, len,
+													 offset, GATT_CLIENT_CFG_NOTIFY );
+			break;
+
+			default:
+				// Should never get here! (characteristics 2 and 4 do not have write permissions)
+				status = ATT_ERR_ATTR_NOT_FOUND;
+			break;
+		}
+	}else{
+		// 128-bit UUID
+		status = ATT_ERR_INVALID_HANDLE;
+	}
+	return ( status );
+}
+
+/*********************************************************************
+ * @fn			simpleProfile_HandleConnStatusCB
+ *
+ * @brief		Simple Profile link status change handler function.
+ *
+ * @param		connHandle - connection handle
+ * @param		changeType - type of change
+ *
+ * @return		none
+ */
+static void simpleProfile_HandleConnStatusCB( uint16_t connHandle, uint8_t changeType )
+{
+	// Make sure this is not loopback connection
+	if ( connHandle != LOOPBACK_CONNHANDLE ){
+		// Reset Client Char Config if connection has dropped
+		if ( ( changeType == LINKDB_STATUS_UPDATE_REMOVED )||( ( changeType == LINKDB_STATUS_UPDATE_STATEFLAGS ) &&( !linkDB_Up( connHandle ) ) ) ){
+			GATTServApp_InitCharCfg( connHandle, simpleProfileChar2Config );
+		}
+	}
+}
+
+
+void new_cmd(void) {
+	attHandleValueNoti_t noti;
+	noti.handle = simpleProfileAttrTbl[CDM_DATA_ATTR_IDX].handle;
+	noti.len = cmd_parser(noti.value, cmd_in_buffer, cmd_in_len);
+	if(noti.len)
+		GATT_Notification(gapRole_ConnectionHandle, &noti, FALSE );
+}
diff --git a/bthome_phy6222/source/sbp_profile.h b/bthome_phy6222/source/sbp_profile.h
new file mode 100644
index 0000000..5c2c1cc
--- /dev/null
+++ b/bthome_phy6222/source/sbp_profile.h
@@ -0,0 +1,85 @@
+/**************************************************************************************************
+	Filename:		sbpProfile.h
+	Revised:
+	Revision:
+	Description:	This file contains the Simple GATT profile definitions and
+				  prototypes.
+ **************************************************************************************************/
+
+#ifndef _SBPPROFILE_H_
+#define _SBPPROFILE_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/*********************************************************************
+ * INCLUDES
+ */
+
+/*********************************************************************
+ * CONSTANTS
+ */
+
+// Profile Parameters
+#define SIMPLEPROFILE_CHAR1						0  // RW uint8 - Profile Characteristic 1 value 
+#define SIMPLEPROFILE_CHAR2						1  // RW uint8 - Profile Characteristic 2 value
+
+// Simple Profile Service UUID
+#define SIMPLEPROFILE_SERV_UUID					0xFFF0
+	
+// OTA UUID
+#define SIMPLEPROFILE_CHAR1_UUID				0xFFF3
+// CMD UUID
+#define SIMPLEPROFILE_CHAR2_UUID				0xFFF4
+  
+// Simple X Profile Services bit fields
+#define SIMPLEPROFILE_SERVICE					0x00000001
+
+/*********************************************************************
+ * TYPEDEFS
+ */
+  
+/*********************************************************************
+ * MACROS
+ */
+
+/*********************************************************************
+ * Profile Callbacks
+ */
+
+// Callback when a characteristic value has changed
+typedef void (*simpleProfileChange_t)( uint8_t paramID );
+
+typedef struct
+{
+	simpleProfileChange_t		 pfnSimpleProfileChange;  // Called when characteristic value changes
+} simpleProfileCBs_t;
+
+	
+
+/*********************************************************************
+ * API FUNCTIONS 
+ */
+
+
+/*
+ * SimpleProfile_AddService- Initializes the Simple GATT Profile service by registering
+ *			GATT attributes with the GATT server.
+ *
+ * @param	services - services to add. This is a bit map and can
+ *					   contain more than one service.
+ */
+
+extern bStatus_t SimpleProfile_AddService( uint32_t services );
+
+//extern bStatus_t simpleProfile_Notify( uint8_t param, uint8_t len, void *value );
+
+void new_cmd(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _SBPPROFILE_H_ */
diff --git a/bthome_phy6222/source/sensor.h b/bthome_phy6222/source/sensor.h
index 30dace0..42e8ad1 100644
--- a/bthome_phy6222/source/sensor.h
+++ b/bthome_phy6222/source/sensor.h
@@ -117,13 +117,21 @@ typedef struct _measured_data_t {
 	uint8_t		battery; // 0..100 % 
 } measured_data_t;
 
+typedef struct _thsensor_coef_t {
+	uint32_t temp_k;
+	uint32_t humi_k;
+	int16_t temp_z;
+	int16_t humi_z;
+} thsensor_coef_t;
+
+extern const thsensor_coef_t def_thcoef;
+
 typedef struct _thsensor_cfg_t {
+	thsensor_coef_t coef;
 	union {
 		uint32_t id;
 		uint16_t _id[2];
 	};
-	int16_t temp_offset;
-	int16_t humi_offset;
 	uint8_t 	i2c_addr;
 } thsensor_cfg_t;
 
diff --git a/bthome_phy6222/source/sensors.c b/bthome_phy6222/source/sensors.c
index 93c7b96..a5eacea 100644
--- a/bthome_phy6222/source/sensors.c
+++ b/bthome_phy6222/source/sensors.c
@@ -16,6 +16,18 @@
 measured_data_t measured_data;
 thsensor_cfg_t thsensor_cfg;
 
+const thsensor_coef_t def_thcoef = {
+#if DEVICE == DEVICE_THB2
+		.temp_k = 25606,
+		.humi_k = 20000,
+#elif DEVICE == DEVICE_BTH01
+		.temp_k = 16500,
+		.humi_k = 10000,
+#endif
+		.temp_z = 0,
+		.humi_z = 0
+};
+
 void init_i2c(void) {
 	hal_gpio_fmux_set(I2C_SCL, FMUX_IIC0_SCL);
 	hal_gpio_fmux_set(I2C_SDA, FMUX_IIC0_SDA);
@@ -186,15 +198,15 @@ int send_i2c_wreg(uint8 addr, uint8 reg, uint16 data) {
 __ATTR_SECTION_XIP__ void init_sensor(void) {
 	init_i2c();
 #if DEVICE == DEVICE_THB2
-
 	send_i2c_byte(0, 0x06); // Reset command using the general call address
 	WaitMs(SENSOR_RESET_TIMEOUT_ms);
 	thsensor_cfg.i2c_addr = CHT8310_I2C_ADDR0;
 	read_i2c_bytes(thsensor_cfg.i2c_addr, CHT8310_REG_MID, (uint8 *)&thsensor_cfg._id[0], 2); // 0x5959
-	read_i2c_bytes(thsensor_cfg.i2c_addr, CHT8310_REG_VID, (uint8 *)&thsensor_cfg._id[1], 2);
+	read_i2c_bytes(thsensor_cfg.i2c_addr, CHT8310_REG_VID, (uint8 *)&thsensor_cfg._id[1], 2); // 0x8215
 	//WaitMs(1);
-	send_i2c_wreg(CHT8310_I2C_ADDR0, CHT8310_REG_CRT, 0x0300); // Set conversion ratio 5 sec
-
+	if(adv_wrk.measure_interval_ms >= 5000) // > 5 sec
+		send_i2c_wreg(CHT8310_I2C_ADDR0, CHT8310_REG_CRT, 0x0300); // Set conversion ratio 5 sec
+	// else 1 sec
 #elif DEVICE == DEVICE_BTH01
 #define USE_DEFAULT_SETS_SENSOR	0 // for CHT8305
 #if USE_DEFAULT_SETS_SENSOR
@@ -237,9 +249,9 @@ int read_sensor(void) {
 	deinit_i2c();
 	if (!_r32) {
 		_r16 = (reg_data[0] << 8) | reg_data[1];
-		measured_data.temp = ((int32)(_r16 * 25606 + 0x7fff) >> 16)  + thsensor_cfg.temp_offset;; // x 0.01 C
+		measured_data.temp = ((int32)(_r16 * thsensor_cfg.coef.temp_k + 0x7fff) >> 16)  + thsensor_cfg.coef.temp_z;; // x 0.01 C
 		_r32 = ((reg_data[2] << 8) | reg_data[3]) & 0x7fff;
-		measured_data.humi = ((uint32)(_r32 * 20000 + 0x7fff) >> 16) +  + thsensor_cfg.humi_offset; // x 0.01 %
+		measured_data.humi = ((uint32)(_r32 * thsensor_cfg.coef.humi_k + 0x7fff) >> 16) +  + thsensor_cfg.coef.humi_z; // x 0.01 %
 		if (measured_data.humi < 0)
 			measured_data.humi = 0;
 		else if (measured_data.humi > 9999)
@@ -251,9 +263,9 @@ int read_sensor(void) {
 	uint16_t _temp;
 	if (!read_noreg_i2c_bytes(thsensor_cfg.i2c_addr, reg_data, 4)) {
 		_temp = (reg_data[0] << 8) | reg_data[1];
-		measured_data.temp = ((uint32_t)(_temp * 16500) >> 16) - 4000 + thsensor_cfg.temp_offset; // x 0.01 C
+		measured_data.temp = ((uint32_t)(_temp * thsensor_cfg.coef.temp_k) >> 16) - 4000 + thsensor_cfg.coef.temp_z; // x 0.01 C
 		_temp = (reg_data[2] << 8) | reg_data[3];
-		measured_data.humi = ((uint32_t)(_temp * 10000) >> 16) + thsensor_cfg.humi_offset; // x 0.01 %
+		measured_data.humi = ((uint32_t)(_temp * thsensor_cfg.coef.humi_k) >> 16) + thsensor_cfg.coef.humi_z; // x 0.01 %
 		if (measured_data.humi < 0)
 			measured_data.humi = 0;
 		else if (measured_data.humi > 9999)
diff --git a/bthome_phy6222/source/thb2_main.c b/bthome_phy6222/source/thb2_main.c
index b2f9cd1..a7820ee 100644
--- a/bthome_phy6222/source/thb2_main.c
+++ b/bthome_phy6222/source/thb2_main.c
@@ -21,7 +21,6 @@
 #include "gapgattserver.h"
 #include "gattservapp.h"
 #include "devinfoservice.h"
-#include "sbp_profile_ota.h"
 #include "ota_app_service.h"
 #include "thb2_peripheral.h"
 #include "gapbondmgr.h"
@@ -41,6 +40,7 @@
 #include "bthome_beacon.h"
 #include "sensor.h"
 #include "battery.h"
+#include "sbp_profile.h"
 /*********************************************************************
  * MACROS
  */
@@ -71,9 +71,6 @@
  */
 perStatsByChan_t g_perStatsByChanTest;
 
-uint8 adv_count;
-uint8 adv_con_count;
-
 /*********************************************************************
  * EXTERNAL VARIABLES
  */
@@ -88,6 +85,9 @@ uint8 adv_con_count;
 /*********************************************************************
  * LOCAL VARIABLES
  */
+
+adv_work_t adv_wrk;
+
 uint8	simpleBLEPeripheral_TaskID;	  // Task ID for internal task/event processing
 
 static	gaprole_States_t	gapProfileState	=	GAPROLE_INIT;
@@ -197,20 +197,20 @@ static void set_adv_interval(uint16 advInt)
 
 static void adv_measure(void) {
 	if(gapRole_AdvEnabled) {
-		if(++adv_count & 1) {
+		if(++adv_wrk.adv_count >= cfg.measure_interval) {
+			adv_wrk.adv_count = 0;
 			read_sensor();
+			if(++adv_wrk.adv_batt_count >= cfg.batt_interval) { // = 60
+				adv_wrk.adv_batt_count = 0;
+				batt_start_measure();
+			}
 			bthome_data_beacon((padv_bthome_ns1_t) gapRole_AdvertData);
 			LL_SetAdvData(sizeof(adv_bthome_ns1_t), gapRole_AdvertData);
-			if(adv_con_count) {
-				if(--adv_con_count == 0) {
-					set_new_adv_interval(DEF_ADV_INERVAL);
-				}
-			}
 		}
-		else if(adv_count >= BATT_TIMER_MEASURE_INTERVAL/DEF_ADV_INERVAL_MS) // = 60
-		{
-			adv_count = 0;
-			batt_start_measure();
+		if(adv_wrk.adv_con_count) {
+			if(--adv_wrk.adv_con_count == 0) {
+				set_new_adv_interval(cfg.advertising_interval * 100);
+			}
 		}
 	}
 }
@@ -223,7 +223,7 @@ static void posedge_int_wakeup_cb(GPIO_Pin_e pin, IO_Wakeup_Pol_e type)
 	(void) pin;
 	if(type == POSEDGE)
 	{
-		adv_con_count = 30000/DEF_CON_ADV_INERVAL_MS; // 60 sec
+		adv_wrk.adv_con_count = 30000/DEF_CON_ADV_INERVAL_MS; // 60 sec
 		LOG("int or wakeup(pos):gpio:%d type:%d\n", pin, type);
 		hal_gpio_write(GPIO_LED, LED_OFF);
 		if(gapRole_AdvEnabled) {
@@ -294,7 +294,6 @@ void gatrole_advert_enable(bool enable) {
 /*********************************************************************
  * PROFILE CALLBACKS
  */
-
 // GAP Role Callbacks
 static gapRolesCBs_t simpleBLEPeripheral_PeripheralCBs =
 {
@@ -309,14 +308,6 @@ static gapBondCBs_t simpleBLEPeripheral_BondMgrCBs =
 	NULL					  // Pairing / Bonding state Callback (not used by application)
 };
 #endif
-// Simple GATT Profile Callbacks
-//static
-#if 0
-simpleProfileCBs_t simpleBLEPeripheral_SimpleProfileCBs =
-{
-	simpleProfileChangeCB	 // Charactersitic value change callback
-};
-#endif
 /*********************************************************************
  * PUBLIC FUNCTIONS
  */
@@ -401,17 +392,17 @@ void SimpleBLEPeripheral_Init( uint8 task_id )
 	GGS_AddService( GATT_ALL_SERVICES );				//	GAP
 	GATTServApp_AddService( GATT_ALL_SERVICES );		//	GATT attributes
 	DevInfo_AddService();								//	Device Information Service
-	//SimpleProfile_AddService( GATT_ALL_SERVICES );	//	Simple GATT Profile
 	Batt_AddService();
-	//Batt_Register(NULL);
 	TH_AddService();
+	//Batt_Register(NULL);
+	SimpleProfile_AddService( GATT_ALL_SERVICES );		//	Simple GATT Profile
 
 	//uint8	OTA_Passward_AscII[8]	=	{'1','2','3','4','5','6','7','8'};
 	//ota_app_AddService_UseKey(8, OTA_Passward_AscII);
 	// ota_app_AddService();
 
 #if (1)
-#if 1 // CODED PHY not work?
+#if 0 // CODED PHY not work?
 	deviceFeatureSet.featureSet[1] |= (uint8)(
 			LL_FEATURE_2M_PHY
 			| LL_FEATURE_CODED_PHY
@@ -426,7 +417,7 @@ void SimpleBLEPeripheral_Init( uint8 task_id )
 	llInitFeatureSet2MPHY(FALSE);
 	llInitFeatureSetDLE(FALSE);
 #endif
-    HCI_LE_SetDefaultPhyMode(0,0x03,0x01,0x01);
+//    HCI_LE_SetDefaultPhyMode(0,0xff,0x01,0x01);
 
 #ifdef MTU_SIZE
 	ATT_SetMTUSizeMax(MTU_SIZE);
@@ -486,8 +477,7 @@ uint16 BLEPeripheral_ProcessEvent( uint8 task_id, uint16 events )
 	if ( events & SBP_RESET_ADV_EVT )
 	{
 		LOG("SBP_RESET_ADV_EVT\n");
-		adv_count = 0;
-		//adv_con_count = 1;
+		adv_wrk.adv_count = 0;
 		// set_new_adv_interval(DEF_ADV_INERVAL); // actual time = advInt * 625us
 		gatrole_advert_enable(TRUE);
 		return ( events ^ SBP_RESET_ADV_EVT );
@@ -498,8 +488,8 @@ uint16 BLEPeripheral_ProcessEvent( uint8 task_id, uint16 events )
 		read_sensor();
 		// TH Notify
 		TH_NotifyLevel();
-		if(++adv_count >= 6) { // 60 sec
-			adv_count = 0;
+		if(++adv_wrk.adv_batt_count >= cfg.batt_interval) { // 60 sec
+			adv_wrk.adv_batt_count = 0;
 			batt_start_measure();
 		}
 		// return unprocessed events
@@ -531,6 +521,7 @@ uint16 BLEPeripheral_ProcessEvent( uint8 task_id, uint16 events )
 	if(events & SBP_DEALDATA)
 	{
 		LOG("\ndeal app datas in events!\n");
+		new_cmd();
 		// return unprocessed events
 		return(events ^ SBP_DEALDATA);
 	}
@@ -600,7 +591,6 @@ static void peripheralStateReadRssiCB( int8	 rssi )
 	{
 		case GAPROLE_STARTED:
 		{
-			// set_mac();
 			LOG("Gaprole_start\n");
 			osal_set_event(simpleBLEPeripheral_TaskID, SBP_RESET_ADV_EVT);
 		}
@@ -610,16 +600,14 @@ static void peripheralStateReadRssiCB( int8	 rssi )
 		{
 			LOG("Gaprole_adversting\n");
 			osal_stop_timerEx(simpleBLEPeripheral_TaskID, TIMER_BATT_EVT);
-			adv_count = 0;
-			//bthome_data_beacon((padv_bthome_ns1_t) gapRole_AdvertData);
-			//LL_SetAdvData(sizeof(adv_bthome_ns1_t), gapRole_AdvertData);
+			adv_wrk.adv_count = 0;
 		}
 		break;
 
 		case GAPROLE_CONNECTED:
-			adv_con_count = 0;
-			//osal_start_timerEx(simpleBLEPeripheral_TaskID, TIMER_BATT_EVT, 1000000);
-			osal_start_reload_timer(simpleBLEPeripheral_TaskID, TIMER_BATT_EVT, 10000); // 2*DEF_ADV_INERVAL_MS); // 5000 ms
+			adv_wrk.adv_count = 0;
+			adv_wrk.adv_con_count = 0;
+			osal_start_reload_timer(simpleBLEPeripheral_TaskID, TIMER_BATT_EVT, adv_wrk.measure_interval_ms); // 10000 ms
 			HCI_PPLUS_ConnEventDoneNoticeCmd(simpleBLEPeripheral_TaskID, NULL);
 			LOG("Gaprole_Connected\n");
 		break;
@@ -630,8 +618,9 @@ static void peripheralStateReadRssiCB( int8	 rssi )
 
 		case GAPROLE_WAITING:
 			LOG("Gaprole_Disconnection\n");
-			adv_con_count = 1;
+			adv_wrk.adv_con_count = 1;
 			osal_stop_timerEx(simpleBLEPeripheral_TaskID, TIMER_BATT_EVT);
+			adv_wrk.adv_count = 0;
 		break;
 
 		case GAPROLE_WAITING_AFTER_TIMEOUT:
@@ -651,29 +640,3 @@ static void peripheralStateReadRssiCB( int8	 rssi )
 	VOID gapProfileState;
 }
 
-#if 0
-/*********************************************************************
- * @fn		simpleProfileChangeCB
- *
- * @brief	Callback from SimpleBLEProfile indicating a value change
- *
- * @param	paramID - parameter ID of the value that was changed.
- *
- * @return	none
- */
-static void simpleProfileChangeCB( uint8 paramID )
-{
-
-	switch( paramID )
-	{
-		case SIMPLEPROFILE_CHAR1:
-
-		break;
-
-		default:
-			// not process other attribute change
-		break;
-	}
-}
-#endif
-
diff --git a/bthome_phy6222/source/thb2_peripheral.h b/bthome_phy6222/source/thb2_peripheral.h
index 90d2154..755e3b3 100644
--- a/bthome_phy6222/source/thb2_peripheral.h
+++ b/bthome_phy6222/source/thb2_peripheral.h
@@ -330,6 +330,7 @@ extern uint16 gapRole_MinConnInterval;
 extern uint16 gapRole_MaxConnInterval;
 extern uint16 gapRole_SlaveLatency;
 extern uint16 gapRole_TimeoutMultiplier;
+extern uint16 gapRole_ConnectionHandle;
 
 
 #ifdef __cplusplus
diff --git a/bthome_phy6222/source/thservice.c b/bthome_phy6222/source/thservice.c
index bee3fce..a5c036e 100644
--- a/bthome_phy6222/source/thservice.c
+++ b/bthome_phy6222/source/thservice.c
@@ -349,7 +349,7 @@ static void thNotifyCB( linkDBItem_t* pLinkItem )
 /*********************************************************************
 	@fn		 thNotifyLevelState
 
-	@brief	 Send a notification of the battery level state
+	@brief	 Send a notification of the th state
 			characteristic if a connection is established.
 
 	@return	 None.