nrf52840 S140 Central DFU Bluetooth Zephyr 2.0.0 nrf52840 peripheral

[DavidKaplan]

We have nrf52840 S140 BLE Central device that currently connects to TI based sensors. I have implemented code that can upgrade those TI sensors' firmware using the TI BLE protocol.

Now we are developing our own BLE peripheral sensors also based on the nrf52840 but now using the newly supported Zephyr 2.0.0. RF connect stuff. I can update the sensors using the Nordic Android program (unauthenticated for now) and now need to implement upgrading BLE DFU using our central device which connects to a cellular gateway.

I do not want to reinvent the wheel, writing existing code, like I had to do with the TI sensors.

Could you point me to code that allows a Central device to update a peripheral device BLE DFU?

If not where can I look to reinvent another wheel?

Thanks David

[Sigurd Hellesvik]

Hi David,

Thanks for testing my suggestion.

To get more insight into the issue, I think that I need to set up Simons sample on my end.
I will do so and return with my status tomorrow.

DavidKaplan said:

I maybe could covert the central and the sensor to both run on SDKs and make a serial PC app that would stream the image to the central as requested.

I then could let Nordic support see the problems.

I would need both AMI and Nordic management OKs for this.

I do not know exactly how to advance.

I do not understand what you mean by this, could you explain a bit more?

Regards,
Sigurd Hellesvik

[DavidKaplan]

I could comment out any hardware specific code on our central and Sensor.

In order to simulate correctly image transfer which is done by the serial connection from our Central host, I could build a simple PC program that streams any specified image as the central requests it.

Simon's test code used the central's flash as storage for the sensor's image.

I stream it from our host's flash as needed.

David

[Sigurd Hellesvik]

DavidKaplan said:

I maybe could covert the central and the sensor to both run on SDKs and make a serial PC app that would stream the image to the central as requested.

I then could let Nordic support see the problems.

I would need both AMI and Nordic management OKs for this.

I do not know exactly how to advance.

Both SMP ZCBOR and my manual transfers were not reliable.

Thanks David

I think you are moving a bit too fast for me to follow.

1. Do you currently try to use Simons sample SMP Client without changes?
   1. Yes
   2. No
2. Have Simons sample SMP Client ever worked for you?
   1. Yes
   2. No
3. Are your current solution of the SMP Client doing a or b?
   1. Storing new image to flash
   2. Streaming data directly
4. Is your end goal to do either a, b or c
   1. Store new image in flash of SMP Client before sending it to the SMP Server
   2. Stream image via SMP Client to the SMP Server
   3. Does not matter as long as SMP from client works.

In addition to this, if you are able: Can you draw a simple block diagram of your DFU design so that I can understand better?

Regards,
Sigurd Hellesik

[DavidKaplan]

I tried to answer as well as I can. I could not find my ruler but his is my block diagram:

I could run my sensor code on SDKs with the special hardware additions commented out.

I could run our Central code on any SDK since there are no hardware additions.

Any PC could be used as the host as I could write a program that streamed the image file to the Central when requested.

-----------------------------------------------------------------------------------------------------

1) Do you currently try to use Simons sample SMP Client without changes?

 NO, I can't store sensor images in the Central's internal flash using any local serial interface since the units are in the field and only have modem access and in any case, images must be streamed from the board's host micro-controller which has cellular modem access to the Central. The Host is responsible for updating both the Central and sensors by way of the Central. 

 There are very few changes in the Simon's code such as using error numbers instead of local printouts in his callback functions and not waiting for the image upload semaphore forever. In the

smp_list_rsp_proc(0 function, I had to increase two array sizes, but I could also just not parse anything but the 2 slot's hash values.

2) Have Simons sample SMP Client ever worked for you?

 No. Mostly the upload smp_upload_rsp_proc() would return all OK, but then there would be no response from the sensor to the first packet sent by bt_dfu_smp_command() which returns zero. I have compared roughly that the first packet's streamed bytes are identical as the file image bytes.

3) Are your current solution of the SMP Client doing a or b?

  a) Storing new image to flash - No

  b) Streaming data directly - Yes

4) Is your end goal to do either a, b or c

  a) Store new image in flash of SMP Client before sending it to the SMP Server

      I could stream the image from the host to the SMP Client's internal flash, but I thought it would just add another step and flash wear as updates can be quite frequent. It does have an advantage that I would not have to stream the image from the host multiple times when I DFU each sensor but this only adds a little more time. Time vs flash wear. The Central must also be able to be updated by the serial interface from the host. I do not use SMP for this and it seems to work OK.

  b) Stream image via SMP Client to the SMP Server

     This is the way I went. I keep a ring buffer filled and instead of reading from the flash, flash_read(), I read from the ring buffer. I could even retry if I kept the last data read from the ring buffer is a local array.

  c) Does not matter as long as SMP from client works.

    I just want sensor DFU to work, you are correct. 

I set in the child_image/mcuboot.conf the boot move define.

### ?!? 18-Dec-2022 Without this swap is used which limits upgrades 10000 / (150 / 4) ~ 267

CONFIG_BOOT_SWAP_USING_MOVE=y

--------------------------------------------------------------------------

Below is my Central SMP code which you can see is very similar to Simon's original code.

I am some dyslectic so I could messed up a line.

d_printf is my function that prints with code file name and line number.

The AMI_SMP_PRINT defined is not set and I let the messages in the callbacks for reference purposes.

#ifdef AMI_SMP_PRINT        

          d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected operation code (%u)!",smp_rsp_buff.header.op);

#endif

Fullscreen ami_smp.c Download

/* main.c - Application main entry point */

/*
 * Copyright (c) 2019 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

#include <zephyr/types.h>
#include <stddef.h>
#include <errno.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

#include <zcbor_encode.h>
#include <zcbor_decode.h>
#include <zcbor_common.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <bluetooth/gatt_dm.h>
#include <zephyr/sys/byteorder.h>
#include <bluetooth/scan.h>
#include <bluetooth/services/dfu_smp.h>
#include <dk_buttons_and_leds.h>
#include <zephyr/sys/ring_buffer.h>

#include <zephyr/device.h>
#include <drivers/flash.h>
#include "aqua_def.h"
#include "d_printf.h"
#include "ami_uart_host.h"
#include "ami_smp.h"
#include "ami_ble.h"
#include "ami_timer.h"
#include "ami_watchdog.h"

static struct bt_dfu_smp dfu_smp;

static struct k_work dfu_work_item;
K_SEM_DEFINE(upload_sem, 1, 1);

/* Buffer for response */
struct smp_buffer {
	struct bt_dfu_smp_header header;
	uint8_t payload[512]; //300
};

#ifdef AMI_OAD_TRANSFER

#else
static struct smp_buffer smp_rsp_buff;
static char hash_value_secondary_slot[33];
static char hash_value_primary_slot[33];
static uint8_t which_work;

static void dfu_smp_on_error(struct bt_dfu_smp *dfu_smp, int err)
{
	d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"DFU SMP generic error: %d", err);
} // dfu_smp_on_error

static const struct bt_dfu_smp_init_params init_params = {
	.error_cb = dfu_smp_on_error
};

static void smp_reset_rsp_proc(struct bt_dfu_smp *dfu_smp)
{
	atomic_set(&smp_finished,true);
#ifdef AMI_SMP_PRINT
	d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"RESET RESPONSE CB. Doing nothing");
#endif	
} // smp_reset_rsp_proc

static void smp_upload_rsp_proc(struct bt_dfu_smp *dfu_smp)
{
	uint8_t *p_outdata = (uint8_t *)(&smp_rsp_buff);
	const struct bt_dfu_smp_rsp_state *rsp_state;

	rsp_state = bt_dfu_smp_rsp_state(dfu_smp);

	if (rsp_state->offset + rsp_state->chunk_size > sizeof(smp_rsp_buff)) {
#ifdef AMI_SMP_PRINT		
	  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Response size buffer overflow");
#endif
	}else{
		  p_outdata += rsp_state->offset;
		  memcpy(p_outdata,rsp_state->data,rsp_state->chunk_size);
	}
	if (bt_dfu_smp_rsp_total_check(dfu_smp)) {
		if (smp_rsp_buff.header.op != 3 /* WRITE RSP*/) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected operation code (%u)!",smp_rsp_buff.header.op);
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+1;
		  return;
		}
		uint16_t group = ((uint16_t)smp_rsp_buff.header.group_h8) << 8 |
		       		                smp_rsp_buff.header.group_l8;
		if (group != 1 /* Application/software image management group */) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected command group (%u)!", group);
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+2;
		  return;
		}
		if (smp_rsp_buff.header.id != 1 /* UPLOAD */) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected command (%u)",smp_rsp_buff.header.id);
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+3;
		  return;
		}
		size_t payload_len = ((uint16_t)smp_rsp_buff.header.len_h8) << 8 |
 				                        smp_rsp_buff.header.len_l8;
		zcbor_state_t zsd[CBOR_DECODER_STATE_NUM];
		struct zcbor_string value = {0};
		bool ok;
		zcbor_new_decode_state(zsd, ARRAY_SIZE(zsd), smp_rsp_buff.payload, payload_len, 1);
        //---------------------------------------------------------------------
		// Stop decoding on the error.
        //---------------------------------------------------------------------
		zsd->constant_state->stop_on_error = true;
		ok = zcbor_map_start_decode(zsd);
		if (!ok) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, start_decode (err: %d)", zcbor_pop_error(zsd));
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+4;
		  return;
		}
        //---------------------------------------------------------------------
		// Decoding rc (status error code)
        //---------------------------------------------------------------------
		char rc_key[5];
		ok = zcbor_tstr_decode(zsd, &value);
		if (!ok) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, rc key (err: %d)", zcbor_pop_error(zsd));
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+5;
		  return;
		}else
		if (value.len != 2) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Invalid data received (rc key). Length %d is not equal 2", value.len);
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+6;
		  return;
		}else
		if (!strncmp(value.value,'rc', 2)){
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Invalid data received (rc key). String '%.2s' is not equal to 'rc'", value.value);
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+7;
		  return;
		}
		memcpy(rc_key, value.value, value.len);
		rc_key[value.len] = '\0';
        //---------------------------------------------------------------------
		// Decoding rc value
        //---------------------------------------------------------------------
		int32_t rc_value;
		ok = zcbor_int32_decode(zsd, &rc_value);
		if (!ok) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, rc value (err: %d)", zcbor_pop_error(zsd));
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+8;
		  return;
		}
		if (rc_value){
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Error in image upload response: %d", rc_value);
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+9;
		  return;
		}
        //---------------------------------------------------------------------
		// Decoding offset key
        //---------------------------------------------------------------------
		char off_key[5];
		ok = zcbor_tstr_decode(zsd, &value);
		if (!ok) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, offset key (err: %d)", zcbor_pop_error(zsd));
#endif
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+10;
		  return;
		}else
		if ((value.len!=3)) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Invalid data received (rc key). Length %d is not equal 3", value.len);
 		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+11;
#endif
		  return;
		}
		memcpy(off_key, value.value, value.len);
		off_key[value.len] = '\0';
        //---------------------------------------------------------------------
		// Decoding offset value
        //---------------------------------------------------------------------
		int32_t off_val;
		ok = zcbor_int32_decode(zsd, &off_val);
		if (!ok) {
#ifdef AMI_SMP_PRINT		
		  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, offset value (err: %d)", zcbor_pop_error(zsd));
#endif
		  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+12;
		  return;
		}
		zcbor_map_end_decode(zsd);
		if (zcbor_check_error(zsd)) {
		  //d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"%s: %d", off_key, off_val);
		  //do nothing
		}else{
#ifdef AMI_SMP_PRINT		
			  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Cannot print received image upload CBOR stream (err: %d)",
					zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD+13;
		}
		k_sem_give(&upload_sem);
		return;
	}
} // smp_upload_rsp_proc

static void smp_list_rsp_proc(struct bt_dfu_smp *dfu_smp)
{
	uint8_t *p_outdata = (uint8_t *)(&smp_rsp_buff);
	const struct bt_dfu_smp_rsp_state *rsp_state;
#ifdef AMI_DEBUG_BLE
  struct ami_timer wait_Timer;
  ami_state->bDebugBLE = true;
  //---------------------------------------------------------------------------
  // Disconnect before sending reply
  //---------------------------------------------------------------------------
  bt_conn_disconnect(dfu_smp->conn,BT_HCI_ERR_REMOTE_USER_TERM_CONN);
  ami_timer_set(&wait_Timer,AMI_TIME_2S);
  //---------------------------------------------------------------------------
  // Wait until VibeBT disconnects
  //---------------------------------------------------------------------------
  while (atomic_get(&ble_connected) && !ami_timer_expired(&wait_Timer)){
	k_sleep(K_MSEC(25));
  }
#endif
	rsp_state = bt_dfu_smp_rsp_state(dfu_smp);
	if (rsp_state->offset + rsp_state->chunk_size > sizeof(smp_rsp_buff)) {
#ifdef AMI_SMP_PRINT		
	  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Response size buffer overflow");
#endif
	}else{
		  p_outdata += rsp_state->offset;
		  memcpy(p_outdata,rsp_state->data,rsp_state->chunk_size);
	}
	if (bt_dfu_smp_rsp_total_check(dfu_smp)) {
		if (smp_rsp_buff.header.op != 1 && smp_rsp_buff.header.op != 3 ) {
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected operation code (%u)!",smp_rsp_buff.header.op);
#endif
			ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_BAD_OPER;
			return;
		}
		uint16_t group = ((uint16_t)smp_rsp_buff.header.group_h8) << 8 |
				      smp_rsp_buff.header.group_l8;
		if (group != 1 /* Application/software image management group */) {
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected command group (%u)!", group);
#endif
			ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_BAD_GROUP;
			return;
		}
		if (smp_rsp_buff.header.id != 0 /* STATE */) {
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected command (%u)",smp_rsp_buff.header.id);
#endif
			ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_BAD_CMD;
			return;
		}
		size_t payload_len = ((uint16_t)smp_rsp_buff.header.len_h8) << 8 |
				      smp_rsp_buff.header.len_l8;
		zcbor_state_t zsd[10];
		struct zcbor_string value = {0};
		bool ok;
		zcbor_new_decode_state(zsd, ARRAY_SIZE(zsd), smp_rsp_buff.payload, payload_len, 5);
		/* Stop decoding on the error. */
		zsd->constant_state->stop_on_error = true;
		ok = zcbor_map_start_decode(zsd);
		if (!ok) {
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error 1, start_decode (err: %d)", zcbor_pop_error(zsd));
#endif
			ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_START_DECODE;
			return;
		}
		//Decoding images key
		char images_key[10];
		ok = zcbor_tstr_decode(zsd, &value);
		if (!ok) {
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error 2, images key (err: %d)", zcbor_pop_error(zsd));
#endif
			ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_IMAGE_DECODE;
			return;
		}
		memcpy(images_key, value.value, value.len);
		images_key[value.len] = '\0';
		//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Images key: %s",images_key);
		ok = zcbor_list_start_decode(zsd);
		if (!ok) {
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, start_decode images->list  (err: %d)", zcbor_pop_error(zsd));
#endif
			ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_IMAGE_LIST;
			return;
		}
		for(int slot=0; slot<2;slot++){
			ok = zcbor_map_start_decode(zsd);
			if (!ok) {
				if(slot == 0){
#ifdef AMI_SMP_PRINT
					d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Error decoding slot 0. Err: %d", zcbor_pop_error(zsd));
#endif
				}else if(slot == 1){
#ifdef AMI_SMP_PRINT
					d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"No secondary image present");
#endif			
                    //---------------------------------------------------------
					// No secondary image exists!
                    //---------------------------------------------------------
					memset(hash_value_secondary_slot,0, sizeof(hash_value_secondary_slot));
					break;
				}
			}
#ifdef AMI_SMP_PRINT
			if(slot==0){
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"-----------PRIMARY IMAGE-----------");
			}else if(slot == 1){
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"-----------SECONDARY IMAGE-----------");
			}
#endif
			//Decoding slot key
			char slot_key[5];
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, slot key (err: %d)", zcbor_pop_error(zsd));
#endif				
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_SLOT_KEY;
   			    return;
			}
			memcpy(slot_key, value.value, value.len);
			slot_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Slot key: %s",slot_key);

			//Decoding slot value
			int32_t slot_value;
			ok = zcbor_int32_decode(zsd, &slot_value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, slot value (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_SLOT_VAL;
        		return;
			}
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: %d",slot_key,slot_value);
#endif
			//Decoding version key
			char version_key[16]; // ?!? len =7
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, version key (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_DECODE_VER;
     			return;
			}
			memcpy(version_key, value.value, value.len);
			version_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"version key: %s",version_key);

			//decoding version value
			char version_value[16]; // ?!? len=5 was [5]
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, version value (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_VER_VAL;
        		return;
			}
			memcpy(version_value, value.value, value.len);
			version_value[value.len] = '\0';
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: %s",version_key,version_value);
#endif
			//Decoding hash key
			char hash_key[5];
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, hash key (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_DECODE_HASH;
    			return;
			} 
			memcpy(hash_key, value.value, value.len);
			hash_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"hash key: %s",hash_key);

			//decoding hash value
			char hash_value[40];
			ok = zcbor_bstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, hash value (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_HASH_VAL;
    			return;
			}
			memcpy(hash_value, value.value, value.len);
			if(slot == 0){
				memcpy(hash_value_primary_slot, value.value, value.len);
			}
			else if(slot == 1){
				memcpy(hash_value_secondary_slot, value.value, value.len);
			}
			hash_value[value.len] = '\0';
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: 0x", hash_key);
			for(int x = 0; x< value.len;x++){
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"%x", hash_value[x]);
			}
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"");
#endif

			//Decoding bootable key
			char bootable_key[10];
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, hash key (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_DECODE_BOOT;
     			return;
			}
			memcpy(bootable_key, value.value, value.len);
			bootable_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"bootable key: %s",bootable_key);

			//Decoding bootable value
			bool bootable_value;
			bootable_value = zcbor_bool_expect(zsd, true);
			if (!zcbor_check_error(zsd)) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, bootable value (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_BOOT_VAL;
    			return;
			}
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: %s",bootable_key, bootable_value?"true":"false");
#endif

			//Decoding pending key
			char pending_key[10];
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, pending key (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_DECODE_PEND;
    			return;
			}
			memcpy(pending_key, value.value, value.len);
			pending_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"pending key: %s",pending_key);

			//Decoding pending value
			bool pending_value;
			pending_value = zcbor_bool_expect(zsd, true);
			if (!zcbor_check_error(zsd)) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, pending value (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_PEND_VAL;
      			return;
			}
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: %s",pending_key, pending_value?"true":"false");
#endif

			//Decoding confirmed key
			char confirmed_key[10];
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, confirmed key (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_DECODE_CONFIRM;
      			return;
			}
			memcpy(confirmed_key, value.value, value.len);
			confirmed_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Confirmed key: %s",confirmed_key);

			//Decoding confirmed value
			bool confirmed_value;
			confirmed_value = zcbor_bool_expect(zsd, true);
			if (!zcbor_check_error(zsd)) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, confirmed value (err: %d)", zcbor_pop_error(zsd));
#endif				
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_CONFIRM_VAL;
    			return;
			}
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: %s",confirmed_key, confirmed_value?"true":"false");
#endif
			//Decoding active key
			char active_key[10];
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, active key (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_DECODE_ACT;
     			return;
			}
			memcpy(active_key, value.value, value.len);
			active_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Active key: %s",active_key);

			//Decoding active value
			bool active_value;
			active_value = zcbor_bool_expect(zsd, true);
			if (!zcbor_check_error(zsd)) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, active value (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_ACT_VAL;
      			return;
			}
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: %s",active_key, active_value?"true":"false");
#endif
			//Decoding permanent key
			char permanent_key[10];
			ok = zcbor_tstr_decode(zsd, &value);
			if (!ok) {
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, permanent key (err: %d)", zcbor_pop_error(zsd));
#endif
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_DECODE_PERM;
      			return;
			}
			memcpy(permanent_key, value.value, value.len);
			permanent_key[value.len] = '\0';
			//d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Permanent key: %s",permanent_key);

			//Decoding permanent value
			bool permanent_value;
			permanent_value = zcbor_bool_expect(zsd, true);
			if (!zcbor_check_error(zsd)) {
				ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_PERM_VAL;
#ifdef AMI_SMP_PRINT
				d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error, permanent value (err: %d)", zcbor_pop_error(zsd));
#endif
        		return;
			}
#ifdef AMI_SMP_PRINT
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"      %s: %s",permanent_key, permanent_value?"true":"false");
#endif
			zcbor_map_end_decode(zsd);

		}
		zcbor_list_end_decode(zsd);
		zcbor_map_end_decode(zsd);
	}
	atomic_set(&smp_finished,true);
} // smp_list_rsp_proc

static void smp_echo_rsp_proc(struct bt_dfu_smp *dfu_smp)
{
	uint8_t *p_outdata = (uint8_t *)(&smp_rsp_buff);
	const struct bt_dfu_smp_rsp_state *rsp_state;

	rsp_state = bt_dfu_smp_rsp_state(dfu_smp);
	d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Echo response part received, size: %zu.",
	       rsp_state->chunk_size);

	if (rsp_state->offset + rsp_state->chunk_size > sizeof(smp_rsp_buff)) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Response size buffer overflow");
	} else {
		p_outdata += rsp_state->offset;
		memcpy(p_outdata,
		       rsp_state->data,
		       rsp_state->chunk_size);
	}

	if (bt_dfu_smp_rsp_total_check(dfu_smp)) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Total response received - decoding");
		if (smp_rsp_buff.header.op != 3 /* WRITE RSP*/) {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected operation code (%u)!",
			       smp_rsp_buff.header.op);
			goto smp_echo_rsp_proc_end;
		}
		uint16_t group = ((uint16_t)smp_rsp_buff.header.group_h8) << 8 |
				      smp_rsp_buff.header.group_l8;
		if (group != 0 /* OS */) {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected command group (%u)!", group);
			goto smp_echo_rsp_proc_end;
		}
		if (smp_rsp_buff.header.id != 0 /* ECHO */) {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Unexpected command (%u)",
			       smp_rsp_buff.header.id);
			goto smp_echo_rsp_proc_end;
		}
		size_t payload_len = ((uint16_t)smp_rsp_buff.header.len_h8) << 8 |
				      smp_rsp_buff.header.len_l8;

		zcbor_state_t zsd[CBOR_DECODER_STATE_NUM];
		struct zcbor_string value = {0};
		char map_key[SMP_ECHO_MAP_KEY_MAX_LEN];
		char map_value[SMP_ECHO_MAP_VALUE_MAX_LEN];
		bool ok;

		zcbor_new_decode_state(zsd, ARRAY_SIZE(zsd), smp_rsp_buff.payload, payload_len, 1);

		/* Stop decoding on the error. */
		zsd->constant_state->stop_on_error = true;

		zcbor_map_start_decode(zsd);

		ok = zcbor_tstr_decode(zsd, &value);

		if (!ok) {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error (err: %d)", zcbor_pop_error(zsd));
			goto smp_echo_rsp_proc_end;
		} else if ((value.len != 1) || (*value.value != 'r')) {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Invalid data received.");
			goto smp_echo_rsp_proc_end;
		} else {
			/* Do nothing */
		}

		map_key[0] = value.value[0];

		/* Add string NULL terminator */
		map_key[1] = '\0';

		ok = zcbor_tstr_decode(zsd, &value);

		if (!ok) {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Decoding error (err: %d)", zcbor_pop_error(zsd));
			goto smp_echo_rsp_proc_end;
		} else if (value.len > (sizeof(map_value) - 1)) {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"To small buffer for received data.");
			goto smp_echo_rsp_proc_end;
		} else {
			/* Do nothing */
		}

		memcpy(map_value, value.value, value.len);

		/* Add string NULL terminator */
		map_value[value.len] = '\0';

		zcbor_map_end_decode(zsd);

		if (zcbor_check_error(zsd)) {
			/* Print textual representation of the received CBOR map. */
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"{_\"%s\": \"%s\"}", map_key, map_value);
		} else {
			d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Cannot print received CBOR stream (err: %d)",
			       zcbor_pop_error(zsd));
		}
	}
smp_echo_rsp_proc_end:
	atomic_set(&smp_finished,true);
} // smp_echo_rsp_proc

#define PROGRESS_WIDTH 50
static void progress_print(size_t downloaded, size_t file_size)
{
#ifdef TBD
	const int percent = (downloaded * 100) / file_size;
	size_t lpad = (percent * PROGRESS_WIDTH) / 100;
	size_t rpad = PROGRESS_WIDTH - lpad;

	d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"\r[ %3d%% ] |", percent);
	for (size_t i = 0; i < lpad; i++) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"=");
	}
	for (size_t i = 0; i < rpad; i++) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host," ");
	}
	d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"| (%d/%d bytes)", downloaded, file_size);
#endif
	d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"DFU:%d/%d", downloaded, file_size);
} // progress_print

//-----------------------------------------------------------------------------
// send_upload2
//  Description
//   WorkItem that attempts to transfer a VibeBT OAD image
//   Performed once for each DFU attempt
//  Parameters
//    item
//  Returns
//-----------------------------------------------------------------------------
#define UPLOAD_CHUNK		400// 50 //This has to be at least 32 bytes, since first it has to send the whole header (which is 32 bytes)
void send_upload2(void)
{
	    zcbor_state_t zse[2];
  	    size_t        payload_len;
	    uint8_t       data[UPLOAD_CHUNK+1]; // One more byte, to store '/0'
		int           start_addr   = 0;
		int           curr_addr    = 0;
		int           upload_chunk = UPLOAD_CHUNK;
		int           last_addr;
		int           err;
	    bool          update_complete = false;
        uint8_t       wait_timeouts = 0;

		ami_state->DFUErr = AMI_SMP_DFU_ERR_OK;
		while (!update_complete){
			struct smp_buffer smp_cmd;
			zcbor_new_encode_state(zse, ARRAY_SIZE(zse), smp_cmd.payload,sizeof(smp_cmd.payload), 0);
			//-----------------------------------------------------------------
			// Wait until response is received until sending the next chunk
			//-----------------------------------------------------------------
			err = k_sem_take(&upload_sem, K_MSEC(AMI_DFU_MAX_TOTAL_MS/10));
			last_addr    = ami_state->dfu.ImageSize.ulong;
			if (ami_state->DFUErr){
			  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"SMPErr:%u",ami_state->DFUErr);
			  update_complete = true;
			  break;
			}
			//-------------------------------------------------------
			// Disconnected?
			//-------------------------------------------------------
			if (!atomic_get(&ble_connected)){
			  ami_state->DFUErr = AMI_DFU_ERR_DISCONNECTED;
			  update_complete = true;
			  break;
			}
			//-----------------------------------------------------------------
			// No Error but semiphore times out!
			//-----------------------------------------------------------------
			if (err){
			  wait_timeouts++;
			  if (wait_timeouts<10){
//				goto upload_retry;
			    continue;
			  }
			  ami_state->DFUErr = AMI_SMP_DFU_ERR_LOOP_TIMEOUT;
			  update_complete = true;
			  break;
			}
//            uint16_t mtu = bt_gatt_get_mtu(dfu_smp.conn);
// ?!?			(zse->payload - smp_cmd.payload);
            wait_timeouts = 0;
			if ((curr_addr+UPLOAD_CHUNK) > last_addr){
				upload_chunk = last_addr - curr_addr;
				update_complete = true;
			}
			//-----------------------------------------------------------------
			// Try to get block from ringbuffer
			//-----------------------------------------------------------------
			uint16_t bytes_rd = 0;
			uint16_t pos      = 0;
			uint8_t  secs     = 0;
			do{
			   uint16_t len = ring_buf_get(&dfu_ring_buf,&data[pos],upload_chunk-bytes_rd);
			   //--------------------------------------------------------------
			   // Wait for ringbuffer to refill
			   //--------------------------------------------------------------
			   if (len<=0){
				 //------------------------------------------------------------
				 // Cause a chain reaction to fill the image buffer
				 //------------------------------------------------------------
				 if (ami_state->dfu.ImageRingWrOffset.ulong<ami_state->dfu.ImageSize.ulong){
					uint32_t free_space = ring_buf_space_get(&dfu_ring_buf);
					if (free_space>=(VIBEBT_DFU_BLOCK_SIZE*3)){
					//---------------------------------------------------------
					// Request to send another Image block to fill up our buffer
					//---------------------------------------------------------
					ami_state->rqt_alert_bitmap  |= AMI_NORDIC_ALERT_VIBEBT_GET_IMAGE_BLOCK_MASK;
					atomic_or(&host_msg_action,AMI_HOST_COM_ACT_SEND_MSG_MASK);
					//---------------------------------------------------------
					// Raise semiphore to trigger message handling
					//---------------------------------------------------------
					k_sem_give(&host_com_sem);
					}
				 }
				 k_sleep(K_MSEC(1000));
				 secs++;
				 if (secs>60){
					break;
				 }
			   }else{
                     pos      += len;
                     bytes_rd += len;
			   }
			}while(bytes_rd<upload_chunk);
			//-----------------------------------------------------------------
			// RingUnderrun?
			//-----------------------------------------------------------------
			if (bytes_rd<upload_chunk){
			  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"RingUnderrun");
			  ami_state->DFUErr = AMI_SMP_DFU_ERR_RING_UNDERRUN;
			  update_complete = true;
			  break;
			}
// upload_retry:			
			progress_print(curr_addr-start_addr, last_addr-start_addr);
			data[upload_chunk] = '\0';
			zse->constant_state->stop_on_error = true;
			zcbor_map_start_encode(zse, 20);
			zcbor_tstr_put_lit(zse, "image");
			zcbor_int64_put(zse, 0);
			zcbor_tstr_put_lit(zse, "data");
			zcbor_bstr_put_lit(zse, data);
			zcbor_tstr_put_lit(zse, "len");
			zcbor_uint64_put(zse, (uint64_t)ami_state->dfu.ImageSize.ulong);
			zcbor_tstr_put_lit(zse, "off");
			zcbor_uint64_put(zse, curr_addr - start_addr);
			zcbor_tstr_put_lit(zse, "sha");
			zcbor_bstr_put_lit(zse, "12345");
			zcbor_tstr_put_lit(zse, "upgrade");
			zcbor_bool_put(zse, false);
			zcbor_map_end_encode(zse, 20);
			//-----------------------------------------------------------------
			// ZCBOR error?
			//-----------------------------------------------------------------
			if (!zcbor_check_error(zse)) {
			  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Failed to encode SMP test packet, err: %d", zcbor_pop_error(zse));
			  ami_state->DFUErr = AMI_SMP_DFU_ERR_UPLOAD_ENCODE;
			  break;
			}
			curr_addr+=upload_chunk;
			payload_len = (size_t)(zse->payload - smp_cmd.payload);		
			smp_cmd.header.op = 2; /* write request */
			smp_cmd.header.flags = 0;
			smp_cmd.header.len_h8 = (uint8_t)((payload_len >> 8) & 0xFF);
			smp_cmd.header.len_l8 = (uint8_t)((payload_len >> 0) & 0xFF);
			smp_cmd.header.group_h8 = 0;
			smp_cmd.header.group_l8 = 1; /* IMAGE */
			smp_cmd.header.seq = 0;
			smp_cmd.header.id  = 1; /* UPLOAD */
			//-----------------------------------------------------------------
			// ZCBOR error?
			//-----------------------------------------------------------------
			err = bt_dfu_smp_command(&dfu_smp,smp_upload_rsp_proc,sizeof(smp_cmd.header)+payload_len,&smp_cmd); // ?!?
			if (err){
			  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"bt_dfu_smp_command failed with %d", err);
			  ami_state->DFUErr = AMI_SMP_DFU_ERR_SEND;
			  break;
			}
		} // while
		if (!ami_state->DFUErr){
		  d_printf(LINE_INFO,kDbg_General|kDbg_Host,"SMP UploadOK");
		}
  	    atomic_set(&smp_finished,true);
} // send_upload2

static int send_smp_list(struct bt_dfu_smp *dfu_smp)
{
	static struct smp_buffer smp_cmd;
//	zcbor_state_t zse[CBOR_ENCODER_STATE_NUM];
//	size_t payload_len;

//	payload_len = (size_t)(zse->payload);
	smp_cmd.header.op = 0; /* read request */
	smp_cmd.header.flags = 0;
	smp_cmd.header.len_h8 = 0;
	smp_cmd.header.len_l8 = 0;
	smp_cmd.header.group_h8 = 0;
	smp_cmd.header.group_l8 = 1; /* IMAGE */
	smp_cmd.header.seq = 0;
	smp_cmd.header.id  = 0; /* LIST */
	return bt_dfu_smp_command(dfu_smp, smp_list_rsp_proc,
				  sizeof(smp_cmd.header),
				  &smp_cmd);
} // send_smp_list

static int send_smp_reset(struct bt_dfu_smp *dfu_smp,const char *string)
{
	static struct smp_buffer smp_cmd;
	zcbor_state_t zse[CBOR_ENCODER_STATE_NUM];
	size_t payload_len;

	zcbor_new_encode_state(zse, ARRAY_SIZE(zse), smp_cmd.payload,
			       sizeof(smp_cmd.payload), 0);

	/* Stop encoding on the error. */
	zse->constant_state->stop_on_error = true;

	zcbor_map_start_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);
	zcbor_tstr_put_lit(zse, "d");
	zcbor_tstr_put_term(zse, string);
	zcbor_map_end_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);

	if (!zcbor_check_error(zse)) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Failed to encode SMP reset packet, err: %d", zcbor_pop_error(zse));
		return -EFAULT;
	}

	payload_len = (size_t)(zse->payload - smp_cmd.payload);

	smp_cmd.header.op = 2; /* Write */
	smp_cmd.header.flags = 0;
	smp_cmd.header.len_h8 = 0;
	smp_cmd.header.len_l8 = 0;
	smp_cmd.header.group_h8 = 0;
	smp_cmd.header.group_l8 = 0; /* OS */
	smp_cmd.header.seq = 0;
	smp_cmd.header.id  = 5; /* RESET */

	return bt_dfu_smp_command(dfu_smp, smp_reset_rsp_proc,
				  sizeof(smp_cmd.header) + payload_len,
				  &smp_cmd);
} // send_smp_reset

static int send_smp_confirm(struct bt_dfu_smp *dfu_smp)
{
	static struct smp_buffer smp_cmd;
	zcbor_state_t zse[CBOR_ENCODER_STATE_NUM];
	size_t payload_len;

	zcbor_new_encode_state(zse, ARRAY_SIZE(zse), smp_cmd.payload,
			       sizeof(smp_cmd.payload), 0);

	/* Stop encoding on the error. */
	zse->constant_state->stop_on_error = true;

	zcbor_map_start_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);
	zcbor_tstr_put_lit(zse, "hash");
	zcbor_bstr_put_lit(zse, hash_value_secondary_slot);
	zcbor_tstr_put_lit(zse, "confirm");
	zcbor_bool_put(zse, true);

	zcbor_map_end_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);

	if (!zcbor_check_error(zse)) {
#ifdef AMI_SMP_PRINT
	  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Failed to encode SMP confirm packet, err: %d", zcbor_pop_error(zse));
#endif	  
	  ami_state->DFUErr = AMI_SMP_DFU_ERR_CONFIRM_ENCODE;
	  return -EFAULT;
	}

	payload_len = (size_t)(zse->payload - smp_cmd.payload);

	smp_cmd.header.op = 2; /* Write */
	smp_cmd.header.flags = 0;
	smp_cmd.header.len_h8 = (uint8_t)((payload_len >> 8) & 0xFF);
	smp_cmd.header.len_l8 = (uint8_t)((payload_len >> 0) & 0xFF);
	smp_cmd.header.group_h8 = 0;
	smp_cmd.header.group_l8 = 1; /* app/image */
	smp_cmd.header.seq = 0;
	smp_cmd.header.id  = 0; /* ECHO */

	// confirm has same response as list command
	return bt_dfu_smp_command(dfu_smp, smp_list_rsp_proc,
				  sizeof(smp_cmd.header) + payload_len,
				  &smp_cmd);
} // send_smp_confirm

static int send_smp_test(struct bt_dfu_smp *dfu_smp)
{
	static struct smp_buffer smp_cmd;
	zcbor_state_t zse[CBOR_ENCODER_STATE_NUM];
	size_t payload_len;

	zcbor_new_encode_state(zse, ARRAY_SIZE(zse), smp_cmd.payload,
			       sizeof(smp_cmd.payload), 0);

	/* Stop encoding on the error. */
	zse->constant_state->stop_on_error = true;

	zcbor_map_start_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);
	zcbor_tstr_put_lit(zse, "hash");
	zcbor_bstr_put_lit(zse, hash_value_secondary_slot);
	zcbor_tstr_put_lit(zse, "confirm");
	zcbor_bool_put(zse, false);

	zcbor_map_end_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);

	if (!zcbor_check_error(zse)) {
#ifdef AMI_SMP_PRINT		
	  d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Failed to encode SMP test packet, err: %d", zcbor_pop_error(zse));
#endif	  
	  ami_state->DFUErr = AMI_SMP_DFU_ERR_TEST_ENCODE;
	  return -EFAULT;
	}

	payload_len = (size_t)(zse->payload - smp_cmd.payload);

	smp_cmd.header.op = 2; /* Write */
	smp_cmd.header.flags = 0;
	smp_cmd.header.len_h8 = (uint8_t)((payload_len >> 8) & 0xFF);
	smp_cmd.header.len_l8 = (uint8_t)((payload_len >> 0) & 0xFF);
	smp_cmd.header.group_h8 = 0;
	smp_cmd.header.group_l8 = 1; /* app/image */
	smp_cmd.header.seq = 0;
	smp_cmd.header.id  = 0; /* ECHO */

	return bt_dfu_smp_command(dfu_smp, smp_list_rsp_proc,
				  sizeof(smp_cmd.header) + payload_len,
				  &smp_cmd);
} // send_smp_test

static int send_smp_echo(struct bt_dfu_smp *dfu_smp,
			 const char *string)
{
	static struct smp_buffer smp_cmd;
	zcbor_state_t zse[CBOR_ENCODER_STATE_NUM];
	size_t payload_len;

	zcbor_new_encode_state(zse, ARRAY_SIZE(zse), smp_cmd.payload,
			       sizeof(smp_cmd.payload), 0);

	/* Stop encoding on the error. */
	zse->constant_state->stop_on_error = true;

	zcbor_map_start_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);
	zcbor_tstr_put_lit(zse, "d");
	zcbor_tstr_put_term(zse, string);
	zcbor_map_end_encode(zse, CBOR_MAP_MAX_ELEMENT_CNT);

	if (!zcbor_check_error(zse)) {
#ifdef AMI_SMP_PRINT
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Failed to encode SMP echo packet, err: %d", zcbor_pop_error(zse));
#endif		
		return -EFAULT;
	}

	payload_len = (size_t)(zse->payload - smp_cmd.payload);

	smp_cmd.header.op = 2; /* Write */
	smp_cmd.header.flags = 0;
	smp_cmd.header.len_h8 = (uint8_t)((payload_len >> 8) & 0xFF);
	smp_cmd.header.len_l8 = (uint8_t)((payload_len >> 0) & 0xFF);
	smp_cmd.header.group_h8 = 0;
	smp_cmd.header.group_l8 = 0; /* OS */
	smp_cmd.header.seq = 0;
	smp_cmd.header.id  = 0; /* ECHO */

	return bt_dfu_smp_command(dfu_smp, smp_echo_rsp_proc,
				  sizeof(smp_cmd.header) + payload_len,
				  &smp_cmd);
} // send_smp_echo

int ami_smp_confirm(void)
{
	int ret;
	ret = send_smp_confirm(&dfu_smp);
	if (ret) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Confirm command send error (err: %d)", ret);
	}
	return ret;
} // ami_smp_confirm


int ami_smp_echo(void)
{
	static unsigned int echo_cnt;
	char buffer[32];
	int ret;

	++echo_cnt;
	d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Echo test: %d", echo_cnt);
	snprintk(buffer, sizeof(buffer), "Echo message: %u", echo_cnt);
	ret = send_smp_echo(&dfu_smp, buffer);
	if (ret) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Echo command send error (err: %d)", ret);
	}
	return ret;
} // ami_smp_echo

int ami_smp_reset(void)
{
	int ret;

	ret = send_smp_reset(&dfu_smp,"RESET");
	if (ret) {
		d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Reset command send error (err: %d)", ret);
	}
	return ret;
} // ami_smp_reset
#endif

void submit_work(struct k_work *item)
{
   int ret = 0;
   switch (which_work){
     case AMI_SMP_WORK_UPLOAD:
       {
	      k_sem_give(&upload_sem); 
	      send_upload2();
	   }
	   break;
     case AMI_SMP_WORK_LIST:
	   {
		  ret = send_smp_list(&dfu_smp);
		  if (ret) {
#ifdef AMI_SMP_PRINT		
		    d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Image list command send error (err: %d)", ret);
#endif		
            ami_state->DFUErr = AMI_SMP_DFU_ERR_LIST_SEND;	
		  }
	   }
	   break;
     case AMI_SMP_WORK_TEST:
	   {
	      ret = send_smp_test(&dfu_smp);
	      if (ret) {
#ifdef AMI_SMP_PRINT			
		    d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"Test command send error (err: %d)", ret);
#endif			
            ami_state->DFUErr = AMI_SMP_DFU_ERR_TEST_SEND;	
	      }
	   }
	   break;
     case AMI_SMP_WORK_RESET:
	   {
		 ret = ami_smp_reset();
	   }
	   break;
     case AMI_SMP_WORK_CONFIRM:
	   {
		 ret = ami_smp_confirm();
	   }
	   break;
   } // switch
} // submit_work

int ami_smp_submit_work(uint8_t which)
{
   which_work = which;	
   switch (which_work){
     case AMI_SMP_WORK_UPLOAD:
       d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"ImageUpload");
	   break;
     case AMI_SMP_WORK_LIST:
       d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"ImageList");
	   break;
     case AMI_SMP_WORK_TEST:
       d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"ImageTest");
	   break;
     case AMI_SMP_WORK_RESET:
       d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"ImageReset");
	   break;
     case AMI_SMP_WORK_CONFIRM:
       d_printf(LINE_INFO,kDbg_Error|kDbg_Host,"ImageConfirm");
	   break;
   } // switch
#if CONFIG_WATCHDOG
   //--------------------------------------------------------------------------
   // Kick watchdog
   //--------------------------------------------------------------------------
   ami_watchdog_feed();
#endif  
   k_work_submit(&dfu_work_item);
   return 0;
} // ami_smp_submit_work

int  ami_smp_assign_handles(struct bt_gatt_dm *dm)
{
  //---------------------------------------------------------------------------
  // DFU for OTAP support
  //---------------------------------------------------------------------------
  bt_dfu_smp_init(&dfu_smp, &init_params);
  //---------------------------------------------------------------------------
  // Assign SMP characteristic handles
  //---------------------------------------------------------------------------
  int err = bt_dfu_smp_handles_assign(dm, &dfu_smp);
  return err;
} // ami_smp_assign_handles


void ami_smp_init(void)
{
#ifdef AMI_OAD_TRANSFER
#else
  //---------------------------------------------------------------------------
  // DFU for OTAP support
  //---------------------------------------------------------------------------
  bt_dfu_smp_init(&dfu_smp, &init_params);
  k_work_init(&dfu_work_item,submit_work);
#endif
} // ami_smp_init

ami_smp.h

Thank you very much.

 David

[Sigurd Hellesvik]

Hi David,

Sorry for the late answer.

I am trying to run Simons sample as well, but I am also getting some issues.

I will continue trying to make this work, and update you on my progress.

Regards,
Sigurd Hellesvik