diff --git a/.gitignore b/.gitignore
index 2e5a65b..03502d6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,6 @@
 build/
 sample/CsmpAgentLib_sample
+sample/CsmpAgentLib_sample.bin
 efr32_wisun/build/
 *.o
 *.a
diff --git a/efr32_wisun.target b/efr32_wisun.target
index 47ceb56..5cf95fd 100644
--- a/efr32_wisun.target
+++ b/efr32_wisun.target
@@ -70,6 +70,7 @@ INCLUDES += -Iinclude\
 
 C_FLAGS           += -Wno-missing-braces
 C_FLAGS_DEBUG     = 
+# C_FLAGS_DEBUG     += -DPRINTDEBUG
 C_FLAGS_RELEASE   = 
 CXX_FLAGS         = 
 CXX_FLAGS_DEBUG   = 
diff --git a/osal/efr32_wisun/osal_efr32_wisun.c b/osal/efr32_wisun/osal_efr32_wisun.c
index 8af5f41..85ef63f 100644
--- a/osal/efr32_wisun/osal_efr32_wisun.c
+++ b/osal/efr32_wisun/osal_efr32_wisun.c
@@ -14,12 +14,28 @@
  *  limitations under the License.
  */
 
+#include <string.h>
 #include "osal.h"
 #include "../../src/lib/debug.h"
+#include "nvm3.h"
+#include "nvm3_hal_flash.h"
+#include "btl_interface.h"
+#include "sl_component_catalog.h"
+
+#if defined(SL_CATALOG_SL_MAIN_PRESENT)
+#include "sl_main_kernel.h"
+#else
 #include "sl_system_kernel.h"
+#endif
 
 #define OSAL_EFR32_WISUN_MIN_STACK_SIZE_WORDS 4096
 
+#define OSAL_EFR32_WISUN_NVM_KEY_BASE         0x00000F0
+#define OSAL_EFR32_WISUN_NVM_KEY_RUN_IMG      (OSAL_EFR32_WISUN_NVM_KEY_BASE + 0x0000U)
+#define OSAL_EFR32_WISUN_NVM_KEY_UPLOAD_IMG   (OSAL_EFR32_WISUN_NVM_KEY_BASE + 0x0001U)
+#define OSAL_EFR32_WISUN_NVM_KEY_BACKUP_IMG   (OSAL_EFR32_WISUN_NVM_KEY_BASE + 0x0002U)
+#define GECKO_BTL_SLOT_CPY_CHUNK_SIZE         1024U
+
 struct trickle_timer {
   uint32_t t0;
   uint32_t tfire;
@@ -32,6 +48,8 @@ struct trickle_timer {
 
 #define __ret_freertos2posix(ret) \
     (ret == pdPASS ? OSAL_SUCCESS : OSAL_FAILURE)
+#define __slotid2gblslotid(slotid) \
+    (slotid == UPLOAD_IMAGE ? GECKO_BTL_UPLOAD_SLOT_ID : GECKO_BTL_BACKUP_SLOT_ID)
 
 extern uint8_t g_csmplib_eui64[8];
 
@@ -44,23 +62,40 @@ static bool m_timert_isrunning = false;
 static void osal_update_timer();
 static void osal_alarm_fired(TimerHandle_t xTimer);
 static void osal_alarm_fired_pend_fnc(void * param1, uint32_t param2);
-
+static void osal_print_csmp_slot_hdr(const Csmp_Slothdr *slot_hdr);
 
 void osal_kernel_start(void)
 {
-    for (BaseType_t i = 0; i < timer_num; i++) {
-        timers[i].is_running = false;
-        timers[i].timer = xTimerCreate("trickle_timer", 
-                                       pdMS_TO_TICKS(m_remaining * 1000), 
-                                       pdTRUE, 
-                                       (void *)i, 
-                                       osal_alarm_fired);
-        assert(timers[i].timer != NULL);
-        xTimerStop(timers[i].timer, 0);
-
-    }
+  static BootloaderStorageInformation_t storage_info = { 0U };
+  static BootloaderStorageSlot_t slot_info = { 0U };
+
+  for (BaseType_t i = 0; i < timer_num; i++) {
+      timers[i].is_running = false;
+      timers[i].timer = xTimerCreate("trickle_timer", 
+                                     pdMS_TO_TICKS(m_remaining * 1000), 
+                                     pdTRUE, 
+                                     (void *)i, 
+                                     osal_alarm_fired);
+      assert(timers[i].timer != NULL);
+      xTimerStop(timers[i].timer, 0);
+  }
+  
+  // Check bootloader storage slots
+  bootloader_getStorageInfo(&storage_info);
+  
+  assert(storage_info.numStorageSlots >= 2);
+  assert(bootloader_getStorageSlotInfo(__slotid2gblslotid(UPLOAD_IMAGE), 
+                                       &slot_info) == BOOTLOADER_OK);
+  assert(slot_info.length >= CSMP_FWMGMT_SLOTIMG_SIZE);
 
-    sl_system_kernel_start();
+  assert(bootloader_getStorageSlotInfo(__slotid2gblslotid(BACKUP_IMAGE), 
+                                       &slot_info) == BOOTLOADER_OK);
+  assert(slot_info.length >= CSMP_FWMGMT_SLOTIMG_SIZE);
+#if defined(SL_CATALOG_SL_MAIN_PRESENT)
+  sl_main_kernel_start();
+#else
+  sl_system_kernel_start();
+#endif
 }
 
 osal_basetype_t osal_task_create(osal_task_t * thread,
@@ -75,7 +110,7 @@ osal_basetype_t osal_task_create(osal_task_t * thread,
                       name, 
                       (OSAL_EFR32_WISUN_MIN_STACK_SIZE_WORDS + stacksize), 
                       arg, 
-                      priority, 
+                      23 + priority, 
                       thread);
 
     return __ret_freertos2posix(ret);
@@ -483,3 +518,254 @@ static void osal_alarm_fired_pend_fnc(void * param1, uint32_t param2)
   (void) param2;
   osal_alarm_fired(NULL);
 }
+
+
+static void osal_print_csmp_slot_hdr(const Csmp_Slothdr *slot_hdr)
+{
+  if (slot_hdr == NULL) {
+    return;
+  }
+
+  DPRINTF("filehash: ");
+  for (int i = 0; i < SHA256_HASH_SIZE; i++) {
+    DPRINTF("%02x,", slot_hdr->filehash[i]);
+  }
+  DPRINTF("\n");
+  DPRINTF("filename: %s\n", slot_hdr->filename);
+  DPRINTF("version: %s\n", slot_hdr->version);
+  DPRINTF("hwid: %s\n", slot_hdr->hwid);
+  DPRINTF("filesize: %lu\n", slot_hdr->filesize);
+  DPRINTF("blocksize: %lu\n", slot_hdr->blocksize);
+  DPRINTF("reportintervalmin: %lu\n", slot_hdr->reportintervalmin);
+  DPRINTF("reportintervalmax: %lu\n", slot_hdr->reportintervalmax);
+  DPRINTF("status: 0x%08lx\n", slot_hdr->status);
+  DPRINTF("nblkmap: ");
+  for (int i = 0; i < CSMP_FWMGMT_BLKMAP_CNT; i++) {
+    DPRINTF("%08lx%c", slot_hdr->nblkmap[i], 
+           ((i % 4) == 3 || i == (CSMP_FWMGMT_BLKMAP_CNT - 1)) ? '\n' : ',');
+  }
+  DPRINTF("\n");
+}
+
+osal_basetype_t osal_read_slothdr(uint8_t slotid, Csmp_Slothdr *slot)
+{
+  sl_status_t ret = SL_STATUS_FAIL;
+  nvm3_ObjectKey_t nvm_key = 0UL;
+  uint32_t nvm_type = 0UL;
+  size_t nvm_size = 0UL;
+
+  if (slot == NULL) {
+    return OSAL_FAILURE;
+  }
+
+  switch(slotid) {
+    case RUN_IMAGE:
+      printf("Reading Run Slot:\n");
+      nvm_key = OSAL_EFR32_WISUN_NVM_KEY_RUN_IMG;
+      break;
+    case UPLOAD_IMAGE:
+    printf("Reading Upload Slot:\n");
+      nvm_key = OSAL_EFR32_WISUN_NVM_KEY_UPLOAD_IMG;
+      break;
+    case BACKUP_IMAGE:
+    printf("Reading Backup Slot:\n");
+      nvm_key = OSAL_EFR32_WISUN_NVM_KEY_BACKUP_IMG;
+      break;
+    default:
+      printf("read_firmware: Invalid slot id\n");
+      return OSAL_FAILURE;
+  }
+  
+  ret = nvm3_getObjectInfo(nvm3_defaultHandle, nvm_key, &nvm_type, &nvm_size);
+  if (ret != SL_STATUS_OK) {
+    printf("nvm3_getObjectInfo failed\n");
+    return OSAL_FAILURE;
+  }
+  
+  ret = nvm3_readData(nvm3_defaultHandle, nvm_key, slot, nvm_size);
+  osal_print_csmp_slot_hdr(slot);
+
+  if (ret != SL_STATUS_OK) {
+    printf("nvm3_readData failed\n");
+    return OSAL_FAILURE;
+  }
+
+  return OSAL_SUCCESS;
+}
+
+osal_basetype_t osal_write_slothdr(uint8_t slotid, Csmp_Slothdr *slot)
+{
+  sl_status_t ret = SL_STATUS_OK;
+  nvm3_ObjectKey_t nvm_key = 0UL;
+
+  if (slot == NULL) {
+    return OSAL_FAILURE;
+  }
+  switch(slotid) {
+    case RUN_IMAGE:
+      printf("Writing Run Slot:\n");
+      nvm_key = OSAL_EFR32_WISUN_NVM_KEY_RUN_IMG;
+      break;
+    case UPLOAD_IMAGE:
+      printf("Writing Upload Slot:\n");
+      nvm_key = OSAL_EFR32_WISUN_NVM_KEY_UPLOAD_IMG;
+      break;
+    case BACKUP_IMAGE:
+      printf("Writing Backup Slot:\n");
+      nvm_key = OSAL_EFR32_WISUN_NVM_KEY_BACKUP_IMG;
+      break;
+    default:
+      printf("read_firmware: Invalid slot id\n");
+      return OSAL_FAILURE;
+  }
+  osal_print_csmp_slot_hdr(slot);
+
+  ret = nvm3_writeData(nvm3_defaultHandle, nvm_key, slot, sizeof(Csmp_Slothdr));
+  if (ret != SL_STATUS_OK) {
+    printf("nvm3_write failed\n");
+    return OSAL_FAILURE;
+  }
+  return OSAL_SUCCESS;
+}
+/****************************************************************************
+ * @fn   osal_read_firmware
+ *
+ * @brief read firmware image from storage(file/flash)
+ *
+ * input parameters
+ *  @param[in] slotid indicating RUN/UPLOAD/BACKUP slot
+ *  @param[in] data pointer to uint8_t data array
+ *  @param[in] size of data in bytes
+ *
+ * output parameters
+ * @return returns 0 on success and -1 on error
+ *****************************************************************************/
+osal_basetype_t osal_read_firmware(uint8_t slotid, uint8_t *data, uint32_t size)
+{
+  (void) slotid;
+  (void) data;
+  (void) size;
+  DPRINTF("osal_read_firmware: Not implemented for EFR32 Wisun platform\n");
+  return OSAL_FAILURE;
+}
+
+/****************************************************************************
+ * @fn   osal_write_firmware
+ *
+ * @brief write firmware image to storage(file/flash)
+ *
+ * input parameters
+ *  @param[in] slotid indicating RUN/UPLOAD/BACKUP slot
+ *  @param[in] data pointer to uint8_t data array
+ *  @param[in] size of data in bytes
+ *
+ * output parameters
+ * @return returns 0 on success and -1 on error
+ *****************************************************************************/
+osal_basetype_t osal_write_firmware(uint8_t slotid, uint8_t *data, uint32_t size)
+{
+  (void) slotid;
+  (void) data;
+  (void) size;
+  DPRINTF("osal_write_firmware: Not implemented for EFR32 Wisun platform\n");
+  return OSAL_FAILURE;
+}
+
+osal_basetype_t osal_system_reboot(struct in6_addr *NMSaddr)
+{
+  (void) NMSaddr;
+
+  DPRINTF("Rebooting system...\n");
+  
+  // Reboot the system
+  NVIC_SystemReset();
+
+  // Should not reach here
+  return OSAL_FAILURE;
+}
+
+osal_basetype_t osal_copy_firmware(uint8_t source_slotid, uint8_t dest_slotid, Csmp_Slothdr *slots) {
+  uint32_t gecko_btl_dst_slot_id = 0UL;
+  uint32_t gecko_btl_src_slot_id = 0UL;
+  static BootloaderStorageSlot_t slotinf = { 0U };
+  uint32_t dst_length = 0UL;
+  uint32_t src_length = 0UL;
+  uint32_t chunk_size = GECKO_BTL_SLOT_CPY_CHUNK_SIZE;
+  uint8_t *chunk = NULL;
+  uint32_t offset = 0;
+
+  if (slots == NULL) {
+    DPRINTF("copy_firmware_slot: slot is NULL\n");
+    return OSAL_FAILURE;
+  }
+
+  if (dest_slotid == source_slotid || 
+      dest_slotid == RUN_IMAGE || 
+      dest_slotid == RUN_IMAGE) {
+    DPRINTF("copy_firmware_slot: src and/or dst slot id is invalid\n");
+    return OSAL_FAILURE;
+  }
+
+
+  // copy slot image
+  gecko_btl_dst_slot_id = __slotid2gblslotid(dest_slotid);
+  gecko_btl_src_slot_id = __slotid2gblslotid(source_slotid);
+
+  if (bootloader_getStorageSlotInfo(gecko_btl_dst_slot_id, &slotinf) != BOOTLOADER_OK) {
+    DPRINTF("copy_firmware_slot: dest bootloader_getStorageSlotInfo failed\n");
+    return OSAL_FAILURE;
+  }
+  dst_length = slotinf.length;
+
+  if (bootloader_getStorageSlotInfo(gecko_btl_src_slot_id, &slotinf) != BOOTLOADER_OK) {
+    DPRINTF("copy_firmware_slot: src bootloader_getStorageSlotInfo failed\n");
+    return OSAL_FAILURE;
+  }
+
+  if (dst_length < slotinf.length) {
+    DPRINTF("copy_firmware_slot: dst slot is smaller than src slot\n");
+    return OSAL_FAILURE;
+  }
+
+  src_length = slotinf.length;
+
+  chunk = osal_malloc(GECKO_BTL_SLOT_CPY_CHUNK_SIZE);
+  if (chunk == NULL) {
+    DPRINTF("copy_firmware_slot: malloc failed\n");
+    return OSAL_FAILURE;
+  }
+
+  DPRINTF("Erasing destination slot...\n");
+  assert(bootloader_eraseStorageSlot(gecko_btl_dst_slot_id) == BOOTLOADER_OK);
+  DPRINTF("Copying source slot to destination slot...\n");
+
+  while(src_length) {
+    chunk_size = src_length < GECKO_BTL_SLOT_CPY_CHUNK_SIZE ? src_length : GECKO_BTL_SLOT_CPY_CHUNK_SIZE;
+    
+    if (bootloader_readStorage(gecko_btl_src_slot_id, offset, chunk, chunk_size) != BOOTLOADER_OK) {
+      DPRINTF("copy_firmware_slot: bootloader_readStorage failed\n");
+      osal_free(chunk);
+      return OSAL_FAILURE;
+    }
+
+    if (bootloader_writeStorage(gecko_btl_dst_slot_id, offset, chunk, chunk_size) != BOOTLOADER_OK) {
+      DPRINTF("copy_firmware_slot: bootloader_writeStorage failed\n");
+      osal_free(chunk);
+      return OSAL_FAILURE;
+    }
+
+    offset += chunk_size;
+    src_length -= chunk_size;
+  }
+
+  osal_free(chunk);
+
+  // copy header
+  memcpy(&slots[dest_slotid], &slots[source_slotid], sizeof(Csmp_Slothdr));
+  if (osal_write_slothdr(dest_slotid, &slots[dest_slotid]) != OSAL_SUCCESS) {
+    DPRINTF("copy_firmware_slot: osal_write_slothdr failed\n");
+    return OSAL_FAILURE;
+  }
+
+  return OSAL_SUCCESS;
+}
\ No newline at end of file
diff --git a/osal/efr32_wisun/osal_platform_types.h b/osal/efr32_wisun/osal_platform_types.h
index 9dfe42b..2715cfd 100644
--- a/osal/efr32_wisun/osal_platform_types.h
+++ b/osal/efr32_wisun/osal_platform_types.h
@@ -32,6 +32,7 @@
 #include "sl_memory_manager.h"
 #include "sl_sleeptimer.h"
 #include "sl_wisun_ntp_timesync_config.h"
+#include "../../src/lib/debug.h"
 
 typedef void (*osal_sighandler_t)(int);
 
@@ -52,4 +53,9 @@ typedef int osal_sd_set_t;
 
 #define s6_addr address
 
+#define GECKO_BTL_UPLOAD_SLOT_ID 0
+#define GECKO_BTL_BACKUP_SLOT_ID 1
+
+#define CSMP_IMAGE_HDR_SIZE 256
+
 #endif
diff --git a/osal/freertos/osal_freertos.c b/osal/freertos/osal_freertos.c
index 2c75ea6..42aa369 100644
--- a/osal/freertos/osal_freertos.c
+++ b/osal/freertos/osal_freertos.c
@@ -16,7 +16,8 @@
 
 #include "osal.h"
 #include "../../src/lib/debug.h"
-
+#include <string.h>
+#include "csmpservice.h"
 
 struct trickle_timer {
   uint32_t t0;
@@ -53,7 +54,7 @@ void osal_kernel_start(void)
                                        pdTRUE, 
                                        (void *)i, 
                                        osal_alarm_fired);
-        DPRINTF("timer%d %s\n", i, timers[i].timer == NULL ? "create failed" : "create success");
+        DPRINTF("timer%ld %s\n", i, timers[i].timer == NULL ? "create failed" : "create success");
         assert(timers[i].timer != NULL);
         xTimerStop(timers[i].timer, 0);
 
@@ -445,3 +446,193 @@ static void osal_alarm_fired_pend_fnc(void * param1, uint32_t param2)
   (void) param2;
   osal_alarm_fired(NULL);
 }
+
+osal_basetype_t osal_system_reboot(struct in6_addr *NMSaddr)
+{
+  DPRINTF("osal_system_reboot: Reboot requested\n");
+  int ret=0;
+  ret = csmp_service_reboot(NMSaddr);
+  if(ret)
+    printf("\n\nosal_system_reboot: success!\nService registration in progress...\n\n");
+  else
+    printf("osal_system_reboot: csmp_service_reboot failed!\\n");
+  return ret;
+}
+
+osal_basetype_t osal_read_firmware(uint8_t slotid, uint8_t *data, uint32_t size) {
+  FILE *file = NULL;
+  switch(slotid){
+    case RUN_IMAGE:
+      file = fopen("opencsmp-run-slot.bin", "rb");
+      break;
+    case UPLOAD_IMAGE:
+      file = fopen("opencsmp-upload-slot.bin", "rb");
+      break;
+    case BACKUP_IMAGE:
+      file = fopen("opencsmp-backup-slot.bin", "rb");
+      break;
+    default:
+      printf("osal_read_firmware: Invalid slotid\n");
+      return OSAL_FAILURE;
+  }
+  if (file == NULL) {
+      printf("osal_read_firmware: Failed to read firmware, slot-id: %u\n", slotid);
+      return OSAL_FAILURE;
+  }
+  fread(data, sizeof(uint8_t), size, file);
+  fclose(file);
+  return OSAL_SUCCESS;
+}
+
+osal_basetype_t osal_write_firmware(uint8_t slotid, uint8_t *data, uint32_t size) {
+  FILE *file = NULL;
+  size_t bytes=0;
+  (void) bytes;
+  switch(slotid){
+    case RUN_IMAGE:
+      file = fopen("opencsmp-run-slot.bin", "wb");
+      break;
+    case UPLOAD_IMAGE:
+      file = fopen("opencsmp-upload-slot.bin", "wb");
+      break;
+    case BACKUP_IMAGE:
+      file = fopen("opencsmp-backup-slot.bin", "wb");
+      break;
+    default:
+      printf("osal_write_firmware: Invalid slotid\n");
+      return OSAL_FAILURE;
+  }
+  if (file == NULL) {
+      printf("osal_read_firmware: Failed to read firmware, slot-id: %u\n", slotid);
+      return OSAL_FAILURE;
+  }
+  bytes = fwrite(data,sizeof(uint8_t),size, file);
+  DPRINTF("osal_write_firmware: Wrote %ld bytes to slot-id: %u\n", bytes, slotid);
+  fclose(file);
+  return OSAL_SUCCESS;
+}
+
+osal_basetype_t osal_write_slothdr(uint8_t slotid, Csmp_Slothdr *slot){
+FILE *file = NULL;
+size_t bytes = 0;
+(void) bytes;
+  switch(slotid){
+    case RUN_IMAGE:
+      file = fopen("opencsmp-run-slothdr.bin", "wb");
+      break;
+    case UPLOAD_IMAGE:
+      file = fopen("opencsmp-upload-slothdr.bin", "wb");
+      break;
+    case BACKUP_IMAGE:
+      file = fopen("opencsmp-backup-slothdr.bin", "wb");
+      break;
+    default:
+      printf("osal_write_slothdr: Invalid slotid\n");
+      return OSAL_FAILURE;
+  }
+  if (file == NULL) {
+      printf("osal_write_slothdr: Failed to write for slothdr, slot-id: %u\n", slotid);
+      return OSAL_FAILURE;
+  }
+
+  // Write CSMP header + firmware to persist slot data across agent reboot
+  bytes = fwrite(&(slot[slotid]), sizeof(Csmp_Slothdr), 1, file);
+  DPRINTF("osal_write_slothdr: Wrote %ld bytes to slot-id: %u\n", bytes, slotid);
+  fclose(file);
+
+  return OSAL_SUCCESS;
+
+}
+
+osal_basetype_t osal_read_slothdr(uint8_t slotid, Csmp_Slothdr *slot){
+  FILE *file = NULL;
+  switch(slotid) {
+    case RUN_IMAGE:
+      file = fopen("opencsmp-run-slothdr.bin", "rb");
+      break;
+    case UPLOAD_IMAGE:
+      file = fopen("opencsmp-upload-slothdr.bin", "rb");
+      break;
+    case BACKUP_IMAGE:
+      file = fopen("opencsmp-backup-slothdr.bin", "rb");
+      break;
+    default:
+      printf("osal_read_slothdr: Invalid slot id\n");
+      return OSAL_FAILURE;
+  }
+  if (file == NULL) {
+      printf("osal_read_slothdr: Failed to read slothdr for slot-id: %u\n", slotid);
+      return OSAL_FAILURE;
+  }
+  fread(&(slot[slotid]), sizeof(Csmp_Slothdr), 1, file);
+  fclose(file);
+  return OSAL_SUCCESS;
+}
+
+
+osal_basetype_t osal_copy_firmware(uint8_t source_slotid, uint8_t dest_slotid, Csmp_Slothdr *slots){
+
+  FILE *source_fw = NULL, *dest_fw = NULL;
+  uint8_t buff[1024] = {0};
+  int ret=0;
+  (void) ret;
+  if(source_slotid == dest_slotid)
+    return OSAL_SUCCESS;
+  switch (source_slotid)
+  {
+  case RUN_IMAGE:
+    source_fw = fopen("opencsmp-run-slot.bin", "rb");
+    break;
+  case UPLOAD_IMAGE:
+    source_fw = fopen("opencsmp-upload-slot.bin", "rb");
+    break;
+  case BACKUP_IMAGE:
+    source_fw = fopen("opencsmp-backup-slot.bin", "rb");
+    break;
+  default:
+      printf("osal_copy_firmware: Invalid slot id\n");
+      return OSAL_FAILURE;
+    break;
+  }
+  if(source_fw == NULL){
+    printf("osal_copy_firmware: Copy function failed, source file could not be opened\n");
+    return OSAL_FAILURE;
+  }
+  switch (dest_slotid)
+  {
+  case RUN_IMAGE:
+    dest_fw = fopen("opencsmp-run-slot.bin", "wb");
+    break;
+  case UPLOAD_IMAGE:
+    dest_fw = fopen("opencsmp-upload-slot.bin", "wb");
+    break;
+  case BACKUP_IMAGE:
+    dest_fw = fopen("opencsmp-backup-slot.bin", "wb");
+    break;
+  default:
+      printf("osal_copy_firmware: Invalid slot id\n");
+      return OSAL_FAILURE;
+    break;
+  }
+  if(dest_fw == NULL){
+    printf("osal_copy_firmware: Copy function failed, dest file could not be opened\n");
+    fclose(source_fw);
+    return OSAL_FAILURE;
+  }
+
+  while ((ret = fread(buff, 1, sizeof(buff), source_fw)) > 0) {
+    fwrite(buff, 1, ret, dest_fw);
+  }
+  fclose(source_fw);
+  fclose(dest_fw);
+  DPRINTF("osal_copy_firmware: Copied Firmware Successfully\n");
+  memcpy(&(slots[dest_slotid]), &(slots[source_slotid]), sizeof(Csmp_Slothdr));
+  if(osal_write_slothdr(dest_slotid, &(slots[dest_slotid])) == OSAL_FAILURE)
+    {
+      printf("osal_copy_firmware: Failed to copy slothdr\n");
+      return OSAL_FAILURE;
+    }
+
+  DPRINTF("osal_copy_firmware: Copied Slothdr Successfully\n");
+  return OSAL_SUCCESS;
+}
\ No newline at end of file
diff --git a/osal/freertos/osal_platform_types.h b/osal/freertos/osal_platform_types.h
index df60eae..597a709 100644
--- a/osal/freertos/osal_platform_types.h
+++ b/osal/freertos/osal_platform_types.h
@@ -35,6 +35,7 @@
 #include "task.h"
 #include "semphr.h"
 #include "timers.h"
+#include "../../src/lib/debug.h"
 
 typedef void (*osal_sighandler_t)(int);
 
diff --git a/osal/osal.h b/osal/osal.h
index d975381..e04cf51 100644
--- a/osal/osal.h
+++ b/osal/osal.h
@@ -37,7 +37,11 @@
 
 // IMAGE SLOT INFO
 #define CSMP_FWMGMT_ACTIVE_SLOTS      3          // 0-RUN, 1-UPLOAD, 2-BACKUP
+#if defined(OSAL_EFR32_WISUN)
+#define CSMP_FWMGMT_SLOTIMG_SIZE      (512*1024)  // ~512 Kb
+#else
 #define CSMP_FWMGMT_SLOTIMG_SIZE      (30*1024)  // ~30 Kb
+#endif
 #define CSMP_FWMGMT_BLKMAP_CNT        (32)
 
 #define REBOOT_DELAY 5
@@ -599,7 +603,8 @@ osal_basetype_t osal_system_reboot(struct in6_addr *NMSaddr);
 /****************************************************************************
  * @fn   osal_read_firmware
  *
- * @brief read firmware image from storage(file/flash)
+ * @brief read firmware image from storage to file. 
+ *        Filesystem support and sufficient memory are required.
  *
  * input parameters
  *  @param[in] slotid indicating RUN/UPLOAD/BACKUP slot
@@ -614,7 +619,8 @@ osal_basetype_t osal_read_firmware(uint8_t slotid, uint8_t *data, uint32_t size)
 /****************************************************************************
  * @fn   osal_write_firmware
  *
- * @brief write firmware image to storage(file/flash)
+ * @brief write firmware image to storage to file.
+ *        Filesystem support and sufficient memory are required.
  *
  * input parameters
  *  @param[in] slotid indicating RUN/UPLOAD/BACKUP slot
@@ -655,16 +661,14 @@ osal_basetype_t osal_read_slothdr(uint8_t slotid, Csmp_Slothdr* slot);
 osal_basetype_t osal_write_slothdr(uint8_t slotid, Csmp_Slothdr* slot);
 
 /****************************************************************************
- * @fn   osal_copy_firmware
- *
- * @brief Copy firmware image and slot header data from source to dest slot
- *
+ * @fn osal_copy_firmware
+ * @brief copy firmware image from source slot to destination slot
  * input parameters
- *  @param[in] source_slotid and dest_slotid indicating RUN/UPLOAD/BACKUP slot
- *  @param[in] pointer to _Csmp_Slothdr slot structure
- *
+ *  @param[in] source_slotid indicating source slot id
+ *  @param[in]  dest_slot_id indicating destination slot id
+ *  @param[in] slots Array of _Csmp_Slothdr slot structures
  * output parameters
  * @return returns 0 on success and -1 on error
  *****************************************************************************/
-osal_basetype_t osal_copy_firmware(uint8_t source_slotid, uint8_t dest_slotid, Csmp_Slothdr *slot);
+osal_basetype_t osal_copy_firmware(uint8_t source_slotid, uint8_t dest_slotid, Csmp_Slothdr *slots);
 #endif
diff --git a/sample/CsmpAgentLib_sample.c b/sample/CsmpAgentLib_sample.c
index c0ea78a..23d7ce3 100644
--- a/sample/CsmpAgentLib_sample.c
+++ b/sample/CsmpAgentLib_sample.c
@@ -28,7 +28,12 @@
 #include "signature_verify.h"
 
 #if defined(OSAL_EFR32_WISUN)
-#include "sl_system_init.h"
+#include "sl_component_catalog.h"
+#if defined(SL_CATALOG_SL_MAIN_PRESENT)
+  #include "sl_main_init.h"
+#else
+  #include "sl_system_init.h"
+#endif
 #include "sl_wisun_app_core_util.h"
 #include "sl_wisun_ntp_timesync.h"
 #endif
@@ -58,9 +63,10 @@ void sample_data_init() {
   int idx=0, ret=0;
   struct timeval tv = {0};
   DPRINTF("sample_data_init: Initialize sample data\n");
-  gettimeofday(&tv, NULL);
+  osal_gettime(&tv, NULL);
   g_init_time = tv.tv_sec;
-  #ifdef OSAL_LINUX
+  #if defined(OSAL_LINUX) || defined(OSAL_FREERTOS_LINUX)
+  #warning "sample_data_init: Linux/Freertos platform, reading/writing image slot data to disk"
     ret=osal_read_slothdr(RUN_IMAGE, g_slothdr);
     if(ret < 0){
       memcpy(&g_slothdr[RUN_IMAGE],&default_run_slot_image, sizeof(Csmp_Slothdr));
@@ -80,8 +86,25 @@ void sample_data_init() {
     }
   #else // Platforms other than linux cuurenlty do not support image read/write to disk function,
         // run-slot will be initialized with default values during boot-up
-      if(!g_reboot_request)
+      
+      ret = osal_read_slothdr(RUN_IMAGE, &g_slothdr[RUN_IMAGE]);
+      if(ret<0){
         memcpy(&g_slothdr[RUN_IMAGE],&default_run_slot_image, sizeof(Csmp_Slothdr));
+        (void) osal_write_slothdr(RUN_IMAGE, &g_slothdr[RUN_IMAGE]);
+        DPRINTF("sample_data_init: Run slot not found!\n");
+      }
+      ret = osal_read_slothdr(UPLOAD_IMAGE, &g_slothdr[UPLOAD_IMAGE]);
+      if(ret<0){
+        memcpy(&g_slothdr[UPLOAD_IMAGE],&default_run_slot_image, sizeof(Csmp_Slothdr));
+        (void) osal_write_slothdr(UPLOAD_IMAGE, &g_slothdr[UPLOAD_IMAGE]);
+        DPRINTF("sample_data_init: Upload slot not found!\n");
+      }
+      ret = osal_read_slothdr(BACKUP_IMAGE, &g_slothdr[BACKUP_IMAGE]);
+      if(ret<0){
+        memcpy(&g_slothdr[BACKUP_IMAGE],&default_run_slot_image, sizeof(Csmp_Slothdr));
+        (void) osal_write_slothdr(BACKUP_IMAGE, &g_slothdr[BACKUP_IMAGE]);
+        DPRINTF("sample_data_init: Backup slot not found!\n");
+      }
   #endif
   // Init sample Vendor Tlv data
   for (idx = 0; idx < VENDOR_MAX_SUBTYPES; idx++) {
@@ -476,8 +499,12 @@ int main(int argc, char **argv)
 #endif
 
 #if defined(OSAL_EFR32_WISUN)
+#if defined(SL_CATALOG_SL_MAIN_PRESENT)
+    sl_main_init();
+#else
     sl_system_init();
 #endif
+#endif
 
 // Create Sample application task
   ret = osal_task_create(&app_task,
diff --git a/sample/CsmpAgentLib_sample.h b/sample/CsmpAgentLib_sample.h
index 8f24df4..228a458 100644
--- a/sample/CsmpAgentLib_sample.h
+++ b/sample/CsmpAgentLib_sample.h
@@ -117,10 +117,10 @@ enum {
 #define CSMP_AGENT_REG_INTERVAL_MAX   100U
 
 /** \brief EUI64 Address of the Agent*/
-#define CSMP_AGENT_EUI64_ADDRESS      "00173bab00100302"
+#define CSMP_AGENT_EUI64_ADDRESS      "00173bab001003fe"
 
 /** \brief NMS Address*/
-#define CSMP_AGENT_NMS_ADDRESS        "fd12:3456::e119:7a97:d047:fe1a"
+#define CSMP_AGENT_NMS_ADDRESS        "fd12:3456::2"
 
 /** \brief Enable/Disable Signature Settings*/
 // #define CSMP_AGENT_SIGNATURE_SETTINGS 1
@@ -212,14 +212,6 @@ extern bool g_reboot_request;
  */
 extern void sample_data_init();
 
-/**
- * @brief Initialize sample data before CSMP service start
- *
- * @param void
- * @return void
- */
-extern void sample_app_reboot();
-
 int str2addr(char *str, uint8_t *addr);
 
 #endif
diff --git a/sample/CsmpAgentLib_sample_tlvs.c b/sample/CsmpAgentLib_sample_tlvs.c
index c578944..ae55425 100644
--- a/sample/CsmpAgentLib_sample_tlvs.c
+++ b/sample/CsmpAgentLib_sample_tlvs.c
@@ -15,7 +15,7 @@
  */
 
 #include "CsmpAgentLib_sample_tlvs.h"
-
+#include <inttypes.h>
 
 /**
  * @brief csmp get TLV request
@@ -58,7 +58,7 @@ void* csmptlvs_get(tlvid_t tlvid, uint32_t *num) {
       return vendorTlv_get(tlvid, num);
 
     default:
-      printf("sample_get: GET un-supported for TLV: %u.%u\n", tlvid.vendor, tlvid.type);
+      printf("sample_get: GET un-supported for TLV: %"PRIuLEAST32".%"PRIuLEAST32"\n", tlvid.vendor, tlvid.type);
       break;
     }
   return NULL;
@@ -76,15 +76,20 @@ void csmptlvs_post(tlvid_t tlvid, void *tlv) {
       currenttime_post((Current_Time*)tlv);
       break;
     case TRANSFER_REQUEST_ID:
-      return transferRequest_post(tlvid, (Transfer_Request*)tlv);
+      transferRequest_post(tlvid, (Transfer_Request*)tlv);
+      break;
     case IMAGE_BLOCK_ID:
-      return imageBlock_post(tlvid, (Image_Block*)tlv);
+      imageBlock_post(tlvid, (Image_Block*)tlv);
+      break;
     case LOAD_REQUEST_ID:
-      return loadRequest_post(tlvid, (Load_Request*)tlv);
+      loadRequest_post(tlvid, (Load_Request*)tlv);
+      break;
     case CANCEL_LOAD_REQUEST_ID:
-      return cancelLoadRequest_post(tlvid, (Cancel_Load_Request*)tlv);
+      cancelLoadRequest_post(tlvid, (Cancel_Load_Request*)tlv);
+      break;
     case SET_BACKUP_REQUEST_ID:
-      return setBackupRequest_post(tlvid, (Set_Backup_Request*)tlv);
+      setBackupRequest_post(tlvid, (Set_Backup_Request*)tlv);
+      break;
     case SIGNATURE_SETTINGS_ID:
       signature_settings_post((Signature_Settings*)tlv);
       break;
@@ -96,7 +101,7 @@ void csmptlvs_post(tlvid_t tlvid, void *tlv) {
       break;
 
     default:
-      printf("sample_post: POST un-supported for TLV: %u.%u\n", tlvid.vendor, tlvid.type);
+      printf("sample_post: POST un-supported for TLV: %"PRIuLEAST32".%"PRIuLEAST32"\n", tlvid.vendor, tlvid.type);
       break;
     }
 }
diff --git a/sample/efr32_wisun.target b/sample/efr32_wisun.target
index e205640..eaa0355 100644
--- a/sample/efr32_wisun.target
+++ b/sample/efr32_wisun.target
@@ -104,7 +104,6 @@ LD      = "$(ARM_GCC_DIR)/bin/arm-none-eabi-gcc"
 # Define a makefile here to add files/settings to the build.       #
 ####################################################################
 CSMP_AGENT_LIB_EFR32_WISUN_PATH = ../Vendors/Silabs
-COPIED_SDK_PATH = $(CSMP_AGENT_LIB_EFR32_WISUN_PATH)/simplicity_sdk_2024.6.0
 
 -include ../Vendors/Silabs/efr32_wisun.mak
 
diff --git a/sample/tlvs/efr32_wisun_tlvs.c b/sample/tlvs/efr32_wisun_tlvs.c
index 77e50c5..fc1da5e 100644
--- a/sample/tlvs/efr32_wisun_tlvs.c
+++ b/sample/tlvs/efr32_wisun_tlvs.c
@@ -26,9 +26,12 @@
 #include "signature_verify.h"
 #include "osal.h"
 #include "sl_wisun_app_core.h"
+#include "btl_interface.h"
+#include "../../src/lib/debug.h"
 
 #define nexthop_IP "fe80::a00:27ff:fe3b:2ab1"
 
+
 /** \brief the hardware information */
 Hardware_Desc g_hardwareDesc = HARDWARE_DESC_INIT;
 
@@ -546,12 +549,12 @@ void* rplinstance_get(uint32_t *num) {
 void* transferRequest_get(tlvid_t tlvid, uint32_t *num) {
   (void)tlvid;
   (void)num;
-  DPRINTF("## sample_firmwaremgmt: GET for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: GET for TLV %ld.\n", tlvid.type);
 
   // Check upload slot download status
   if (g_slothdr[UPLOAD_IMAGE].status != FWHDR_STATUS_DOWNLOAD) {
     g_transferRequest.status = g_slothdr[UPLOAD_IMAGE].status;
-    DPRINTF("sample_firmwaremgmt: Transfer request download status = %u\n",
+    DPRINTF("sample_firmwaremgmt: Transfer request download status = %lu\n",
            g_transferRequest.status);
     return &g_transferRequest;
   }
@@ -566,7 +569,7 @@ void* transferRequest_get(tlvid_t tlvid, uint32_t *num) {
   g_transferRequest.report_int_max = g_slothdr[UPLOAD_IMAGE].reportintervalmax;
   g_transferRequest.status = g_slothdr[UPLOAD_IMAGE].status;
 
-  DPRINTF("## sample_firmwaremgmt: GET for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: GET for TLV %ld done.\n", tlvid.type);
   return &g_transferRequest;
 }
 
@@ -579,7 +582,7 @@ void* transferRequest_get(tlvid_t tlvid, uint32_t *num) {
  */
 void transferRequest_post(tlvid_t tlvid, Transfer_Request *tlv) {
   (void)tlvid;
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld.\n", tlvid.type);
 
   if (!tlv) {
     DPRINTF("sample_firmwaremgmt: Transfer request tlv context is NULL\n");
@@ -598,14 +601,14 @@ void transferRequest_post(tlvid_t tlvid, Transfer_Request *tlv) {
   // Check filehash len
   if (tlv->filehash.len != SHA256_HASH_SIZE) {
     tlv->response = RESPONSE_INVALID_REQ;
-    DPRINTF("sample_firmwaremgmt: Invalid filehash size: %lu\n", tlv->filehash.len);
+    DPRINTF("sample_firmwaremgmt: Invalid filehash size: %u\n", tlv->filehash.len);
     return;
   }
   // Check filesize
   if (tlv->filesize == 0 ||
       tlv->filesize > CSMP_FWMGMT_SLOTIMG_SIZE) {
     tlv->response = RESPONSE_FILE_SIZE_TOO_BIG;
-        DPRINTF("sample_firmwaremgmt: Invalid file size: %u\n", tlv->filesize);
+        DPRINTF("sample_firmwaremgmt: Invalid file size: %lu\n", tlv->filesize);
     return;
   }
   // blocksize should be smaller than csmp's MTU (1024)
@@ -614,7 +617,7 @@ void transferRequest_post(tlvid_t tlvid, Transfer_Request *tlv) {
       tlv->blocksize > BLOCK_SIZE ||
       tlv->blocksize < tlv->filesize/(CSMP_FWMGMT_BLKMAP_CNT * 32)) {
     tlv->response = RESPONSE_INVALID_BLOCK_SIZE;
-    DPRINTF("sample_firmwaremgmt: Invalid block size: %u\n", tlv->blocksize);
+    DPRINTF("sample_firmwaremgmt: Invalid block size: %lu\n", tlv->blocksize);
     return;
   }
   // Check pending reboot
@@ -681,8 +684,11 @@ void transferRequest_post(tlvid_t tlvid, Transfer_Request *tlv) {
 
   // Initiliase new transfer - done
   g_initxfer = false;
+  
+  DPRINTF("Erasing 'UPLOAD' storage slot...\n");
+  assert(bootloader_eraseStorageSlot(GECKO_BTL_UPLOAD_SLOT_ID) == BOOTLOADER_OK);
 
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld done.\n", tlvid.type);
 }
 
 /**
@@ -694,7 +700,7 @@ void transferRequest_post(tlvid_t tlvid, Transfer_Request *tlv) {
  */
 void imageBlock_post(tlvid_t tlvid, Image_Block *tlv) {
   (void)tlvid;
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld.\n", tlvid.type);
 
   if (!tlv) {
     DPRINTF("sample_firmwaremgmt: Image block tlv context is NULL\n");
@@ -720,29 +726,29 @@ void imageBlock_post(tlvid_t tlvid, Image_Block *tlv) {
     uint32_t bit = 31 - (g_imageBlock.blocknum & 31);
     uint32_t mapval = 0xFFFFFFFFUL;
     uint32_t offset = g_imageBlock.blocknum * g_slothdr[UPLOAD_IMAGE].blocksize;
-    DPRINTF("sample_firmwaremgmt: Image block blocknum=%u offset=%u [word=%x bit=%x]\n",
+    DPRINTF("sample_firmwaremgmt: Image block blocknum=%lu offset=%lu [word=%lx bit=%lx]\n",
            g_imageBlock.blocknum, offset, word, bit);
 
     // Check Transfer Request intialised
     if (g_initxfer) {
-      DPRINTF("sample_firmwaremgmt: Transfer still initializing... (Image block %u)\n",
+      DPRINTF("sample_firmwaremgmt: Transfer still initializing... (Image block %lu)\n",
              g_imageBlock.blocknum);
       g_downloadbusy = false;
       return;
     }
     // Check blocknum exceeds bitmap
     if (word >= CSMP_FWMGMT_BLKMAP_CNT) {
-      DPRINTF("sample_firmwaremgmt: Image block %u exceeds bitmap length\n",
+      DPRINTF("sample_firmwaremgmt: Image block %lu exceeds bitmap length\n",
              g_imageBlock.blocknum);
       g_downloadbusy = false;
       return;
     }
 
     mapval = g_slothdr[UPLOAD_IMAGE].nblkmap[word];
-    DPRINTF("sample_firmwaremgmt: Image block mapval=0x%x\n", mapval);
+    DPRINTF("sample_firmwaremgmt: Image block mapval=0x%lx\n", mapval);
 
     if ((mapval & (1 << bit)) == 0) {
-      DPRINTF("sample_firmwaremgmt: Image block %u already written\n",
+      DPRINTF("sample_firmwaremgmt: Image block %lu already written\n",
              g_imageBlock.blocknum);
       // Check for transfer completion
       if(g_slothdr[UPLOAD_IMAGE].status == FWHDR_STATUS_COMPLETE)
@@ -782,30 +788,65 @@ void imageBlock_post(tlvid_t tlvid, Image_Block *tlv) {
       DPRINTF("sample_firmwaremgmt: Image block transfer complete, filehash matched!\n");
       g_slothdr[UPLOAD_IMAGE].status = FWHDR_STATUS_COMPLETE;
       g_downloadbusy = false;
-      if (write_fw_img(UPLOAD_IMAGE) < 0)
-        DPRINTF("sample_firmwaremgmt: Failed to write upload image to file\n");
+      if (osal_write_slothdr(UPLOAD_IMAGE, &g_slothdr[UPLOAD_IMAGE]) < 0)
+        DPRINTF("sample_firmwaremgmt: Failed to write upload image to the nvm3\n");
       else
-        printf("sample_firmwaremgmt: Sucessfully wrote upload image to file\n");
+        printf("sample_firmwaremgmt: Sucessfully wrote upload image to the nvm3\n");
 
       return;
     }
     // Write image block to slot at valid offset
     if (offset < CSMP_FWMGMT_SLOTIMG_SIZE &&
-       ((offset + g_imageBlock.blockdata.len) < CSMP_FWMGMT_SLOTIMG_SIZE)) {
-      DPRINTF("sample_firmwaremgmt: Valid image block %u write offset=%u\n",
-             g_imageBlock.blocknum, offset);
-      memcpy(&g_slothdr[UPLOAD_IMAGE].image[offset], g_imageBlock.blockdata.data,
-                   g_imageBlock.blockdata.len);
+      ((offset + g_imageBlock.blockdata.len) < CSMP_FWMGMT_SLOTIMG_SIZE)) {
+        DPRINTF("sample_firmwaremgmt: Valid image block %lu write offset=%lu\n",
+          g_imageBlock.blocknum, offset);
+          
+      int32_t ret = 0L;
+      uint32_t gecko_btl_slot_offset = offset;
+      uint32_t gecko_btl_chunk_size = g_imageBlock.blockdata.len;
+      uint8_t *gecko_btl_data_ptr = g_imageBlock.blockdata.data;
+
+      // First chunk includes the slot header information
+      if (!g_imageBlock.blocknum) {
+        gecko_btl_slot_offset = 0;
+        assert(g_imageBlock.blockdata.len > CSMP_IMAGE_HDR_SIZE);
+        gecko_btl_chunk_size = g_imageBlock.blockdata.len - CSMP_IMAGE_HDR_SIZE;
+        gecko_btl_data_ptr += CSMP_IMAGE_HDR_SIZE;
+
+      } else {
+        gecko_btl_slot_offset = offset - CSMP_IMAGE_HDR_SIZE;
+        gecko_btl_chunk_size = g_imageBlock.blockdata.len;
+        gecko_btl_data_ptr = g_imageBlock.blockdata.data;
+        
+      }
+
+      ret = bootloader_writeStorage(GECKO_BTL_UPLOAD_SLOT_ID, gecko_btl_slot_offset, gecko_btl_data_ptr, gecko_btl_chunk_size);
+
+      if (ret != BOOTLOADER_OK) {
+        DPRINTF("sample_firmwaremgmt: Failed to write image block %lu to slot\n",
+                g_imageBlock.blocknum);
+        tlv->retval = false;
+        g_downloadbusy = false;
+        return;
+      }
 
+      // Store the header after each 10 blocks
+      if (g_imageBlock.blocknum && !(g_imageBlock.blocknum % 10)) {
+        if (osal_write_slothdr(UPLOAD_IMAGE, &g_slothdr[UPLOAD_IMAGE]) < 0)
+          DPRINTF("sample_firmwaremgmt: Failed to write upload image to the nvm3 (block %lu)\n", g_imageBlock.blocknum);
+        else
+          printf("sample_firmwaremgmt: Sucessfully wrote upload image to the nvm3 (block %lu)\n", g_imageBlock.blocknum);
+      }
+      
       mapval ^= (1 << bit);
       g_slothdr[UPLOAD_IMAGE].nblkmap[word] = mapval;
 
-      DPRINTF("sample_firmwaremgmt: Write image block %u (len=%lu offset=%u) \
-             success! [mapval=0x%x word=0x%x bit=0x%x]\n",
+      DPRINTF("sample_firmwaremgmt: Write image block %lu (len=%u offset=%lu) \
+             success! [mapval=0x%lx word=0x%lx bit=0x%lx]\n",
              g_imageBlock.blocknum, g_imageBlock.blockdata.len, offset, mapval, word, bit);
     } else {
       // Invalid write offset
-      DPRINTF("sample_firmwaremgmt: Invalid image block %u write offset=%u\n",
+      DPRINTF("sample_firmwaremgmt: Invalid image block %lu write offset=%lu\n",
              g_imageBlock.blocknum, offset);
     }
   } else {
@@ -814,7 +855,7 @@ void imageBlock_post(tlvid_t tlvid, Image_Block *tlv) {
   }
 
   g_downloadbusy = false;
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld done.\n", tlvid.type);
 }
 
 /**
@@ -827,7 +868,7 @@ void imageBlock_post(tlvid_t tlvid, Image_Block *tlv) {
 void* loadRequest_get(tlvid_t tlvid, uint32_t *num) {
   (void)tlvid;
   (void)num;
-  DPRINTF("## sample_firmwaremgmt: GET for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: GET for TLV %ld.\n", tlvid.type);
 
   // Check for pending active load requests
   if (!g_initload) {
@@ -852,7 +893,7 @@ void* loadRequest_get(tlvid_t tlvid, uint32_t *num) {
   g_loadRequest.filehash.len = SHA256_HASH_SIZE;
   g_loadRequest.loadtime = g_curloadtime;
 
-  DPRINTF("## sample_firmwaremgmt: GET for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: GET for TLV %ld done.\n", tlvid.type);
   return &g_loadRequest;
 }
 
@@ -860,19 +901,41 @@ void* loadRequest_get(tlvid_t tlvid, uint32_t *num) {
  * @brief   LOAD REQUEST TIMER HANDLER
  */
 void loadreq_timer_fired() {
-  bool ret = false;
-  struct timeval tv = {0};
-  DPRINTF("loadreq_timer_fired: Load request timer fired for slot=%d with delay=%u\n",
+  uint32_t gecko_btl_slot_id = 0;
+  DPRINTF("loadreq_timer_fired: Load request timer fired for slot=%ld with delay=%lu\n",
          g_curloadslot, g_curloadtime);
 
   memcpy(&g_slothdr[RUN_IMAGE], &g_slothdr[g_curloadslot],
          sizeof(g_slothdr[RUN_IMAGE]));
-  g_curloadslot=0xFF;
-  g_curloadtime=0;
+  osal_write_slothdr(RUN_IMAGE, &g_slothdr[RUN_IMAGE]);
   osal_trickle_timer_stop(lrq_timer);
-  sample_app_reboot();
+  gecko_btl_slot_id = (g_curloadslot == UPLOAD_IMAGE) ? GECKO_BTL_UPLOAD_SLOT_ID : GECKO_BTL_BACKUP_SLOT_ID;
+  if (bootloader_verifyImage(gecko_btl_slot_id, NULL) != BOOTLOADER_OK) {
+    DPRINTF("deploy_and_reboot_firmware: bootloader_verifyImage failed\n");
+    assert(false);
+  }
+  if (bootloader_setImageToBootload(gecko_btl_slot_id) != BOOTLOADER_OK) {
+    DPRINTF("deploy_and_reboot_firmware: bootloader_setImageToBootload failed\n");
+    assert(false);
+  }
+
+  bootloader_rebootAndInstall();
 }
 
+
+/**
+ * @brief   REBOOT REQUEST TIMER HANDLER
+ * @return  void
+ */
+void rebootreq_timer_fired() {
+  DPRINTF("rebootreq_timer: Reboot request timer fired\n");
+  osal_trickle_timer_stop(async_timer);
+  sample_data_init(); //Note: This function resets init time and g_slot_hdr to simulate reboot only for linux
+  osal_system_reboot(&g_devconfig.NMSaddr);
+  g_reboot_request = false;
+}
+
+
 /**
  * @brief   POST TLV68 LOAD_REQUEST_TLVID
  *
@@ -884,7 +947,7 @@ void loadRequest_post(tlvid_t tlvid, Load_Request *tlv) {
   (void)tlvid;
   uint32_t newloadslot;
   uint32_t delay = MIN_LOAD_DELAY; // 1 sec
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld.\n", tlvid.type);
 
   if (!tlv) {
     DPRINTF("sample_firmwaremgmt: Load request tlv context is NULL\n");
@@ -949,7 +1012,7 @@ void loadRequest_post(tlvid_t tlvid, Load_Request *tlv) {
   if (g_loadRequest.loadtime != 0) {
     // Get current time to calculate timer delay for load request
     struct timeval tv;
-    gettimeofday(&tv, NULL);
+    osal_gettime(&tv, NULL);
 
     if (g_loadRequest.loadtime > tv.tv_sec) {
       delay = (g_loadRequest.loadtime - tv.tv_sec);
@@ -970,10 +1033,9 @@ void loadRequest_post(tlvid_t tlvid, Load_Request *tlv) {
   g_curloadtime = g_loadRequest.loadtime;
   osal_trickle_timer_start(lrq_timer, delay, delay,
                       (trickle_timer_fired_t)loadreq_timer_fired);
-  DPRINTF("sample_firmwaremgmt: Load request timer started for slot=%d with delay=%u at epoch time =%u s\n",
+  DPRINTF("sample_firmwaremgmt: Load request timer started for slot=%ld with delay=%lu at epoch time =%lu s\n",
          g_curloadslot, delay, g_curloadtime);
-
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld done.\n", tlvid.type);
 }
 
 /**
@@ -986,7 +1048,7 @@ void loadRequest_post(tlvid_t tlvid, Load_Request *tlv) {
 void cancelLoadRequest_post(tlvid_t tlvid, Cancel_Load_Request *tlv) {
   (void)tlvid;
   const uint8_t *filehash = NULL;
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld.\n", tlvid.type);
   if (!tlv) {
     DPRINTF("sample_firmwaremgmt: Cancel load request tlv context is NULL\n");
     return;
@@ -994,22 +1056,22 @@ void cancelLoadRequest_post(tlvid_t tlvid, Cancel_Load_Request *tlv) {
 
   switch (g_curloadslot) {
     case RUN_IMAGE:
-    DPRINTF("sample_firmwaremgmt: Received cancel load request for RUN_IMAGE (%u)\n",
+    DPRINTF("sample_firmwaremgmt: Received cancel load request for RUN_IMAGE (%lu)\n",
            g_curloadslot);
     filehash = g_slothdr[RUN_IMAGE].filehash;
     break;
     case UPLOAD_IMAGE:
-    DPRINTF("sample_firmwaremgmt: Received cancel load request for UPLOAD_IMAGE (%u)\n",
+    DPRINTF("sample_firmwaremgmt: Received cancel load request for UPLOAD_IMAGE (%lu)\n",
            g_curloadslot);
     filehash = g_slothdr[UPLOAD_IMAGE].filehash;
     break;
     case BACKUP_IMAGE:
-    DPRINTF("sample_firmwaremgmt: Received cancel load request for BACKUP_IMAGE (%u)\n",
+    DPRINTF("sample_firmwaremgmt: Received cancel load request for BACKUP_IMAGE (%lu)\n",
            g_curloadslot);
     filehash = g_slothdr[BACKUP_IMAGE].filehash;
     break;
     default:
-    DPRINTF("sample_firmwaremgmt: Received cancel load request for invalid slot (%u)\n",
+    DPRINTF("sample_firmwaremgmt: Received cancel load request for invalid slot (%lu)\n",
            g_curloadslot);
     if (g_curloadslot != 0xFFU){
       tlv->response = RESPONSE_INVALID_REQ;
@@ -1031,7 +1093,7 @@ void cancelLoadRequest_post(tlvid_t tlvid, Cancel_Load_Request *tlv) {
     g_curloadslot = 0xFFU;
   }
 
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld done.\n", tlvid.type);
 }
 
 /**
@@ -1043,7 +1105,7 @@ void cancelLoadRequest_post(tlvid_t tlvid, Cancel_Load_Request *tlv) {
  */
 void setBackupRequest_post(tlvid_t tlvid, Set_Backup_Request *tlv) {
   (void)tlvid;
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld.\n", tlvid.type);
 
   if (!tlv) {
     DPRINTF("sample_firmwaremgmt: Cancel load request tlv context is NULL\n");
@@ -1082,29 +1144,37 @@ void setBackupRequest_post(tlvid_t tlvid, Set_Backup_Request *tlv) {
   if (g_curbackupslot == RUN_IMAGE || g_curbackupslot == UPLOAD_IMAGE) {
     memset(&g_slothdr[BACKUP_IMAGE], 0xFF, sizeof(g_slothdr[BACKUP_IMAGE]));
   }
+
   // Copy target image to backup slot
   switch (g_curbackupslot) {
     case RUN_IMAGE:
       DPRINTF("sample_firmwaremgmt: Backing-up run image to backup slot\n");
-      memcpy(&g_slothdr[BACKUP_IMAGE], &g_slothdr[RUN_IMAGE],
-             sizeof(g_slothdr[BACKUP_IMAGE]));
+      if (osal_copy_firmware(RUN_IMAGE, BACKUP_IMAGE, g_slothdr) != OSAL_SUCCESS) {
+        DPRINTF("sample_firmwaremgmt: Failed to copy run image to backup slot\n");
+        tlv->response = RESPONSE_INVALID_REQ;
+        return;
+      }
       g_slothdr[BACKUP_IMAGE].status = FWHDR_STATUS_COMPLETE;
       break;
     case UPLOAD_IMAGE:
       DPRINTF("sample_firmwaremgmt: Backing-up upload image to backup slot\n");
-      memcpy(&g_slothdr[BACKUP_IMAGE], &g_slothdr[UPLOAD_IMAGE],
-             sizeof(g_slothdr[BACKUP_IMAGE]));
+      if (osal_copy_firmware(UPLOAD_IMAGE, BACKUP_IMAGE, g_slothdr) != OSAL_SUCCESS) {
+        DPRINTF("sample_firmwaremgmt: Failed to copy upload image to backup slot\n");
+        tlv->response = RESPONSE_INVALID_REQ;
+        return;
+      }
       g_slothdr[BACKUP_IMAGE].status = FWHDR_STATUS_COMPLETE;
       break;
     default:
-      DPRINTF("sample_firmwaremgmt: Set backup request from invalid backup slot (%u)\n",
+      DPRINTF("sample_firmwaremgmt: Set backup request from invalid backup slot (%lu)\n",
              g_curbackupslot);
       tlv->response = RESPONSE_INVALID_REQ;
       return;
   }
+  osal_write_slothdr(BACKUP_IMAGE, &g_slothdr[BACKUP_IMAGE]);
   g_curbackupslot = 0xFFU;
 
-  DPRINTF("## sample_firmwaremgmt: POST for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: POST for TLV %ld done.\n", tlvid.type);
 }
 
 /**
@@ -1117,7 +1187,7 @@ void setBackupRequest_post(tlvid_t tlvid, Set_Backup_Request *tlv) {
 void* firmwareImageInfo_get(tlvid_t tlvid, uint32_t *num) {
   (void)tlvid;
   *num = 0;
-  DPRINTF("## sample_firmwaremgmt: GET for TLV %d.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: GET for TLV %ld.\n", tlvid.type);
 
   // Enumerate all active slots
   // Active slots: 0-RUN_IMAGE, 1-UPLOAD_IMAGE, 2-BACKUP_IMAGE
@@ -1127,7 +1197,7 @@ void* firmwareImageInfo_get(tlvid_t tlvid, uint32_t *num) {
 
       // Track number of inuse active slots
       (*num)++;
-      DPRINTF("sample_firmwaremgmt: Reading firmware image info for slot id:%d\n", idx);
+      DPRINTF("sample_firmwaremgmt: Reading firmware image info for slot id:%ld\n", idx);
 
       // Index
       g_firmwareImageInfo[idx].has_index = true;
@@ -1208,7 +1278,7 @@ void* firmwareImageInfo_get(tlvid_t tlvid, uint32_t *num) {
       g_firmwareImageInfo[idx].status = g_slothdr[idx].status;
   }
 
-  DPRINTF("## sample_firmwaremgmt: GET for TLV %d done.\n", tlvid.type);
+  DPRINTF("## sample_firmwaremgmt: GET for TLV %ld done.\n", tlvid.type);
   return &g_firmwareImageInfo;
 }
 
@@ -1267,18 +1337,18 @@ void signature_settings_post(Signature_Settings *tlv) {
  * @return  void* pointer to global g_vendorTlv
  */
 void* vendorTlv_get(tlvid_t tlvid, uint32_t *num) {
-  printf("## sample_vendorTlv: GET for TLV:%u.%u\n", tlvid.vendor, tlvid.type);
+  printf("## sample_vendorTlv: GET for TLV:%lu.%lu\n", tlvid.vendor, tlvid.type);
 
   // Vendor-ID validation
   // Received Vendor-ID to match device's VENDOR_ID
   if (tlvid.vendor != VENDOR_ID) {
-    printf("sample_vendorTlv: csmptlv %d vendor-id mismatch (Expected:%d, Received:%d)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
+    printf("sample_vendorTlv: csmptlv %ld vendor-id mismatch (Expected:%d, Received:%ld)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
     return NULL;
   }
   // Max support subtypes by the vendor
   *num = VENDOR_MAX_SUBTYPES;
 
-  printf("## sample_vendorTlv: GET for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
+  printf("## sample_vendorTlv: GET for TLV:%lu.%lu done\n", tlvid.vendor, tlvid.type);
   return &g_vendorTlv;
 }
 
@@ -1290,14 +1360,14 @@ void* vendorTlv_get(tlvid_t tlvid, uint32_t *num) {
  * @return  void
  */
 void vendorTlv_post(tlvid_t tlvid, Vendor_Tlv *tlv) {
-  printf("## sample_vendorTlv: POST for TLV:%u.%u\n", tlvid.vendor, tlvid.type);
+  printf("## sample_vendorTlv: POST for TLV:%lu.%lu\n", tlvid.vendor, tlvid.type);
 
   int idx;
 
   // Vendor-ID validation
   // Received Vendor-ID to match device's VENDOR_ID
   if (tlvid.vendor != VENDOR_ID) {
-    printf("sample_vendorTlv: csmptlv %d vendor-id mismatch (Expected:%d, Received:%d)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
+    printf("sample_vendorTlv: csmptlv %ld vendor-id mismatch (Expected:%d, Received:%ld)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
     return;
   }
   // Lookup and update subtype
@@ -1305,8 +1375,8 @@ void vendorTlv_post(tlvid_t tlvid, Vendor_Tlv *tlv) {
     if (tlv->subtype == g_vendorTlv[idx].subtype) {
       g_vendorTlv[idx].value.len = tlv->value.len;
       memcpy(g_vendorTlv[idx].value.data, tlv->value.data, g_vendorTlv[idx].value.len);
-      printf("sample_vendorTlv: Updated vendor subtype:%u\n", g_vendorTlv[idx].subtype);
-      printf("## sample_vendorTlv: POST for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
+      printf("sample_vendorTlv: Updated vendor subtype:%lu\n", g_vendorTlv[idx].subtype);
+      printf("## sample_vendorTlv: POST for TLV:%lu.%lu done\n", tlvid.vendor, tlvid.type);
       return;
     }
   }
@@ -1314,7 +1384,27 @@ void vendorTlv_post(tlvid_t tlvid, Vendor_Tlv *tlv) {
   g_vendorTlv[0].subtype = tlv->subtype;
   g_vendorTlv[0].value.len = tlv->value.len;
   memcpy(g_vendorTlv[0].value.data, tlv->value.data, g_vendorTlv[0].value.len);
-  printf("sample_vendorTlv: Added vendor subtype:%u\n", g_vendorTlv[0].subtype);
+  printf("sample_vendorTlv: Added vendor subtype:%lu\n", g_vendorTlv[0].subtype);
 
-  printf("## sample_vendorTlv: POST for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
+  printf("## sample_vendorTlv: POST for TLV:%lu.%lu done\n", tlvid.vendor, tlvid.type);
 }
+
+/**
+ * @brief   POST TLV32 REBOOT_REQUEST_TLVID
+ *
+ * @param   tlvid tlvid structure
+ * @param   tlv Vendor_Tlv structure
+ * @return  void
+ */
+void rebootRequest_post(tlvid_t tlvid, Reboot_Request *tlv) {
+  printf("## app_reboot_request: POST for TLV:%lu.%lu\n", tlvid.vendor, tlvid.type);
+  switch(tlv->flag){
+    case REBOOT:
+      DPRINTF("** app_rebootRequest: Reboot timer initializing...\n");
+      osal_trickle_timer_start(async_timer, REBOOT_DELAY, REBOOT_DELAY, (trickle_timer_fired_t)rebootreq_timer_fired);
+      g_reboot_request = true;
+      break;
+    default:
+      DPRINTF("** app_rebootRequest: Reboot flag not supported!\n");
+  }
+}
\ No newline at end of file
diff --git a/sample/tlvs/freertos_tlvs.c b/sample/tlvs/freertos_tlvs.c
index 7da92a2..34b6260 100644
--- a/sample/tlvs/freertos_tlvs.c
+++ b/sample/tlvs/freertos_tlvs.c
@@ -1,5 +1,5 @@
 /*
- *  Copyright 2021-2024 Cisco Systems, Inc.
+ *  Copyright 2021-2025 Cisco Systems, Inc.
  *
  *  Licensed under the Apache License, Version 2.0 (the "License");
  *  you may not use this file except in compliance with the License.
@@ -108,7 +108,7 @@ uint32_t g_curbackupslot  = 0xFFU;  // Track current backup slot
 
 // Firmware image slots (Slot-id: 0-RUN, 1-UPLOAD, 2-BACKUP)
 Csmp_Slothdr g_slothdr[CSMP_FWMGMT_ACTIVE_SLOTS] = {0};
-
+FILE *upload_slot = NULL;
 /* public key */
 //new key
 static const char pubkey[PUBLIC_KEY_LEN] = {
@@ -158,7 +158,7 @@ void* hardware_desc_get(uint32_t *num) {
     g_hardwareDesc.entphysicalclass = CLASS_MODULE;
     sprintf(g_hardwareDesc.entphysicalname,"lowpan");
     sprintf(g_hardwareDesc.entphysicalhardwarerev,"1.0");
-    sprintf(g_hardwareDesc.entphysicalfirmwarerev,"1.0.0");
+    memcpy(g_hardwareDesc.entphysicalfirmwarerev, g_slothdr[RUN_IMAGE].version, sizeof(g_hardwareDesc.entphysicalfirmwarerev));
     snprintf(g_hardwareDesc.entphysicalserialnum,sizeof(g_hardwareDesc.entphysicalserialnum),
              "%02X%02X%02X%02X%02X%02X%02X%02X",
              g_eui64[0],g_eui64[1],g_eui64[2],g_eui64[3],
@@ -175,12 +175,12 @@ void* hardware_desc_get(uint32_t *num) {
     g_hardwareDesc.entphysicalclass = CLASS_MODULE;
     sprintf(g_hardwareDesc.entphysicalname,"lowpan");
     sprintf(g_hardwareDesc.entphysicalhardwarerev,"1.0");
-    sprintf(g_hardwareDesc.entphysicalfirmwarerev,"6.5(6.5.9)");
+    memcpy(g_hardwareDesc.entphysicalfirmwarerev, g_slothdr[RUN_IMAGE].version, sizeof(g_hardwareDesc.entphysicalfirmwarerev));
     snprintf(g_hardwareDesc.entphysicalserialnum,sizeof(g_hardwareDesc.entphysicalserialnum),
              "%02X%02X%02X%02X%02X%02X%02X%02X",
              g_eui64[0],g_eui64[1],g_eui64[2],g_eui64[3],
              g_eui64[4],g_eui64[5],g_eui64[6],g_eui64[7]);
-            sprintf(g_hardwareDesc.entphysicalmfgname,"Cisco IIoT");
+        sprintf(g_hardwareDesc.entphysicalmfgname,"Cisco IIoT");
     sprintf(g_hardwareDesc.entphysicalmodelname,"CISCO_IR510");
     g_hardwareDesc.entphysicalfunction = FUNCTION_DAG; // Gateway
     break;
@@ -192,7 +192,7 @@ void* hardware_desc_get(uint32_t *num) {
     g_hardwareDesc.entphysicalclass = CLASS_MODULE;
     sprintf(g_hardwareDesc.entphysicalname,"lowpan");
     sprintf(g_hardwareDesc.entphysicalhardwarerev,"3.1");
-    sprintf(g_hardwareDesc.entphysicalfirmwarerev,"6.2(6.2.33)");
+    memcpy(g_hardwareDesc.entphysicalfirmwarerev, g_slothdr[RUN_IMAGE].version, sizeof(g_hardwareDesc.entphysicalfirmwarerev));
     snprintf(g_hardwareDesc.entphysicalserialnum,sizeof(g_hardwareDesc.entphysicalserialnum),
              "%02X%02X%02X%02X%02X%02X%02X%02X",
              g_eui64[0],g_eui64[1],g_eui64[2],g_eui64[3],
@@ -533,6 +533,66 @@ void* rplinstance_get(uint32_t *num) {
   return &g_rplInstance;
 }
 
+/**
+ * @brief   GET TLV127 VENDOR_TLVID
+ *
+ * @param   tlvid tlvid structure
+ * @param   num instances of subtypes or g_vendorTlv
+ * @return  void* pointer to global g_vendorTlv
+ */
+void* vendorTlv_get(tlvid_t tlvid, uint32_t *num) {
+  printf("## sample_vendorTlv: GET for TLV:%u.%u\n", tlvid.vendor, tlvid.type);
+
+  // Vendor-ID validation
+  // Received Vendor-ID to match device's VENDOR_ID
+  if (tlvid.vendor != VENDOR_ID) {
+    printf("sample_vendorTlv: csmptlv %d vendor-id mismatch (Expected:%d, Received:%d)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
+    return NULL;
+  }
+  // Max support subtypes by the vendor
+  *num = VENDOR_MAX_SUBTYPES;
+
+  printf("## sample_vendorTlv: GET for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
+  return &g_vendorTlv;
+}
+
+/**
+ * @brief   POST TLV127 VENDOR_TLVID
+ *
+ * @param   tlvid tlvid structure
+ * @param   tlv Vendor_Tlv structure
+ * @return  void
+ */
+void vendorTlv_post(tlvid_t tlvid, Vendor_Tlv *tlv) {
+  printf("## sample_vendorTlv: POST for TLV:%u.%u\n", tlvid.vendor, tlvid.type);
+
+  int idx;
+
+  // Vendor-ID validation
+  // Received Vendor-ID to match device's VENDOR_ID
+  if (tlvid.vendor != VENDOR_ID) {
+    printf("sample_vendorTlv: csmptlv %d vendor-id mismatch (Expected:%d, Received:%d)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
+    return;
+  }
+  // Lookup and update subtype
+  for (idx = 0; idx < VENDOR_MAX_SUBTYPES; idx++) {
+    if (tlv->subtype == g_vendorTlv[idx].subtype) {
+      g_vendorTlv[idx].value.len = tlv->value.len;
+      memcpy(g_vendorTlv[idx].value.data, tlv->value.data, g_vendorTlv[idx].value.len);
+      printf("sample_vendorTlv: Updated vendor subtype:%u\n", g_vendorTlv[idx].subtype);
+      printf("## sample_vendorTlv: POST for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
+      return;
+    }
+  }
+  // New subtype will be added, overwrites subtype at g_vendorTlv[0]
+  g_vendorTlv[0].subtype = tlv->subtype;
+  g_vendorTlv[0].value.len = tlv->value.len;
+  memcpy(g_vendorTlv[0].value.data, tlv->value.data, g_vendorTlv[0].value.len);
+  printf("sample_vendorTlv: Added vendor subtype:%u\n", g_vendorTlv[0].subtype);
+
+  printf("## sample_vendorTlv: POST for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
+}
+
 /**
  * @brief   GET TLV65 TRANSFER_REQUEST_TLVID
  *
@@ -778,22 +838,29 @@ void imageBlock_post(tlvid_t tlvid, Image_Block *tlv) {
       // Set slot status as complete else bad image
       DPRINTF("sample_firmwaremgmt: Image block transfer complete, filehash matched!\n");
       g_slothdr[UPLOAD_IMAGE].status = FWHDR_STATUS_COMPLETE;
+      fclose(upload_slot);
+      upload_slot = NULL;
       g_downloadbusy = false;
-      if (write_fw_img(UPLOAD_IMAGE) < 0)
-        DPRINTF("sample_firmwaremgmt: Failed to write upload image to file\n");
-      else
-        printf("sample_firmwaremgmt: Sucessfully wrote upload image to file\n");
-
+      osal_write_slothdr(UPLOAD_IMAGE, g_slothdr);
       return;
     }
+    if(upload_slot == NULL){
+      upload_slot = fopen("opencsmp-upload-slot.bin", "wb");
+    }
     // Write image block to slot at valid offset
     if (offset < CSMP_FWMGMT_SLOTIMG_SIZE &&
        ((offset + g_imageBlock.blockdata.len) < CSMP_FWMGMT_SLOTIMG_SIZE)) {
       DPRINTF("sample_firmwaremgmt: Valid image block %u write offset=%u\n",
              g_imageBlock.blocknum, offset);
-      memcpy(&g_slothdr[UPLOAD_IMAGE].image[offset], g_imageBlock.blockdata.data,
-                   g_imageBlock.blockdata.len);
-
+      if(upload_slot != NULL){
+        fseek(upload_slot, offset, SEEK_SET);
+        fwrite(g_imageBlock.blockdata.data, 1, g_imageBlock.blockdata.len, upload_slot);
+        fflush(upload_slot);
+        osal_write_slothdr(UPLOAD_IMAGE, g_slothdr);
+      }
+      else{
+        printf("image block write to file failed!");
+      }
       mapval ^= (1 << bit);
       g_slothdr[UPLOAD_IMAGE].nblkmap[word] = mapval;
 
@@ -855,20 +922,32 @@ void* loadRequest_get(tlvid_t tlvid, uint32_t *num) {
 
 /**
  * @brief   LOAD REQUEST TIMER HANDLER
+ * @return  void
  */
 void loadreq_timer_fired() {
-  bool ret = false;
-  struct timeval tv = {0};
   DPRINTF("loadreq_timer: Load request timer fired for slot=%d with delay=%u\n",
          g_curloadslot, g_curloadtime);
-
-  memcpy(&g_slothdr[RUN_IMAGE], &g_slothdr[g_curloadslot],
-         sizeof(g_slothdr[RUN_IMAGE]));
-
+  DPRINTF("loadreq_timer: Writing Run Slot to disk\n");
+  osal_copy_firmware(g_curloadslot, RUN_IMAGE, g_slothdr);
   g_curloadslot=0xFF;
   g_curloadtime=0;
   osal_trickle_timer_stop(lrq_timer);
-  sample_app_reboot();
+  g_reboot_request = true;
+  sample_data_init(); //Note: This function resets init time and g_slot_hdr to simulate reboot only for linux
+  osal_system_reboot(&g_devconfig.NMSaddr);
+  g_reboot_request = false;
+}
+
+/**
+ * @brief   REBOOT REQUEST TIMER HANDLER
+ * @return  void
+ */
+void rebootreq_timer_fired() {
+  DPRINTF("rebootreq_timer: Reboot request timer fired\n");
+  osal_trickle_timer_stop(async_timer);
+  sample_data_init(); //Note: This function resets init time and g_slot_hdr to simulate reboot only for linux
+  osal_system_reboot(&g_devconfig.NMSaddr);
+  g_reboot_request = false;
 }
 
 /**
@@ -985,6 +1064,7 @@ void cancelLoadRequest_post(tlvid_t tlvid, Cancel_Load_Request *tlv) {
   (void)tlvid;
   const uint8_t *filehash = NULL;
   DPRINTF("## sample_firmwaremgmt: POST for TLV %d.\n", tlvid.type);
+
   if (!tlv) {
     DPRINTF("sample_firmwaremgmt: Cancel load request tlv context is NULL\n");
     return;
@@ -1084,14 +1164,12 @@ void setBackupRequest_post(tlvid_t tlvid, Set_Backup_Request *tlv) {
   switch (g_curbackupslot) {
     case RUN_IMAGE:
       DPRINTF("sample_firmwaremgmt: Backing-up run image to backup slot\n");
-      memcpy(&g_slothdr[BACKUP_IMAGE], &g_slothdr[RUN_IMAGE],
-             sizeof(g_slothdr[BACKUP_IMAGE]));
+      osal_copy_firmware(RUN_IMAGE, BACKUP_IMAGE, g_slothdr);
       g_slothdr[BACKUP_IMAGE].status = FWHDR_STATUS_COMPLETE;
       break;
     case UPLOAD_IMAGE:
       DPRINTF("sample_firmwaremgmt: Backing-up upload image to backup slot\n");
-      memcpy(&g_slothdr[BACKUP_IMAGE], &g_slothdr[UPLOAD_IMAGE],
-             sizeof(g_slothdr[BACKUP_IMAGE]));
+      osal_copy_firmware(UPLOAD_IMAGE, BACKUP_IMAGE, g_slothdr);
       g_slothdr[BACKUP_IMAGE].status = FWHDR_STATUS_COMPLETE;
       break;
     default:
@@ -1210,7 +1288,6 @@ void* firmwareImageInfo_get(tlvid_t tlvid, uint32_t *num) {
   return &g_firmwareImageInfo;
 }
 
-
 /**
  * @brief get the signature settings
  *
@@ -1259,61 +1336,21 @@ void signature_settings_post(Signature_Settings *tlv) {
 }
 
 /**
- * @brief   GET TLV127 VENDOR_TLVID
- *
- * @param   tlvid tlvid structure
- * @param   num instances of subtypes or g_vendorTlv
- * @return  void* pointer to global g_vendorTlv
- */
-void* vendorTlv_get(tlvid_t tlvid, uint32_t *num) {
-  printf("## sample_vendorTlv: GET for TLV:%u.%u\n", tlvid.vendor, tlvid.type);
-
-  // Vendor-ID validation
-  // Received Vendor-ID to match device's VENDOR_ID
-  if (tlvid.vendor != VENDOR_ID) {
-    printf("sample_vendorTlv: csmptlv %d vendor-id mismatch (Expected:%d, Received:%d)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
-    return NULL;
-  }
-  // Max support subtypes by the vendor
-  *num = VENDOR_MAX_SUBTYPES;
-
-  printf("## sample_vendorTlv: GET for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
-  return &g_vendorTlv;
-}
-
-/**
- * @brief   POST TLV127 VENDOR_TLVID
+ * @brief   POST TLV32 REBOOT_REQUEST_TLVID
  *
  * @param   tlvid tlvid structure
  * @param   tlv Vendor_Tlv structure
  * @return  void
  */
-void vendorTlv_post(tlvid_t tlvid, Vendor_Tlv *tlv) {
-  printf("## sample_vendorTlv: POST for TLV:%u.%u\n", tlvid.vendor, tlvid.type);
-
-  int idx;
-
-  // Vendor-ID validation
-  // Received Vendor-ID to match device's VENDOR_ID
-  if (tlvid.vendor != VENDOR_ID) {
-    printf("sample_vendorTlv: csmptlv %d vendor-id mismatch (Expected:%d, Received:%d)\n", tlvid.type, VENDOR_ID, tlvid.vendor);
-    return;
-  }
-  // Lookup and update subtype
-  for (idx = 0; idx < VENDOR_MAX_SUBTYPES; idx++) {
-    if (tlv->subtype == g_vendorTlv[idx].subtype) {
-      g_vendorTlv[idx].value.len = tlv->value.len;
-      memcpy(g_vendorTlv[idx].value.data, tlv->value.data, g_vendorTlv[idx].value.len);
-      printf("sample_vendorTlv: Updated vendor subtype:%u\n", g_vendorTlv[idx].subtype);
-      printf("## sample_vendorTlv: POST for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
-      return;
-    }
+void rebootRequest_post(tlvid_t tlvid, Reboot_Request *tlv) {
+  printf("## app_reboot_request: POST for TLV:%u.%u\n", tlvid.vendor, tlvid.type);
+  switch(tlv->flag){
+    case REBOOT:
+      DPRINTF("** app_rebootRequest: Reboot timer initializing...\n");
+      osal_trickle_timer_start(async_timer, REBOOT_DELAY, REBOOT_DELAY, (trickle_timer_fired_t)rebootreq_timer_fired);
+      g_reboot_request = true;
+      break;
+    default:
+      DPRINTF("** app_rebootRequest: Reboot flag not supported!\n");
   }
-  // New subtype will be added, overwrites subtype at g_vendorTlv[0]
-  g_vendorTlv[0].subtype = tlv->subtype;
-  g_vendorTlv[0].value.len = tlv->value.len;
-  memcpy(g_vendorTlv[0].value.data, tlv->value.data, g_vendorTlv[0].value.len);
-  printf("sample_vendorTlv: Added vendor subtype:%u\n", g_vendorTlv[0].subtype);
-
-  printf("## sample_vendorTlv: POST for TLV:%u.%u done\n", tlvid.vendor, tlvid.type);
 }
diff --git a/src/csmpagent/csmp_firmwareMgmt.c b/src/csmpagent/csmp_firmwareMgmt.c
index 3e8ff61..c1e632f 100755
--- a/src/csmpagent/csmp_firmwareMgmt.c
+++ b/src/csmpagent/csmp_firmwareMgmt.c
@@ -336,6 +336,7 @@ int csmp_put_imageBlock(tlvid_t tlvid, const uint8_t *buf, size_t len,
                      &image_block__descriptor);
   if (rv == 0) {
     DPRINTF("csmpagent_firmwaremgmt: csmptlv %d read error!\n", tlvid.type);
+    csmptlv_free((ProtobufCMessage *)CurrentBlkMsg);
     return CSMP_OP_TLV_RD_ERROR;
   }
 
@@ -385,6 +386,7 @@ int csmp_put_imageBlock(tlvid_t tlvid, const uint8_t *buf, size_t len,
   pbuf += rv; used += rv;
   DPRINTF("g_downloadbusy(after)=%d\n", g_downloadbusy);
   DPRINTF("** csmpagent_firmwaremgmt: POST for TLV %d done.\n", tlvid.type);
+  csmptlv_free((ProtobufCMessage *)CurrentBlkMsg);
   return used;
 }