tests/subsys/mgmt/mcumgr/mcumgr_client/src/os_gr_stub.c

/*
 * Copyright (c) 2023 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/net/buf.h>
#include <string.h>
#include <stdio.h>
#include <zephyr/mgmt/mcumgr/smp/smp_client.h>
#include <zephyr/mgmt/mcumgr/mgmt/mgmt.h>
#include <zephyr/mgmt/mcumgr/grp/os_mgmt/os_mgmt.h>
#include <zephyr/mgmt/mcumgr/grp/os_mgmt/os_mgmt_client.h>
#include <zephyr/sys/byteorder.h>

#include <zcbor_common.h>
#include <zcbor_encode.h>
#include <zcbor_decode.h>
#include <mgmt/mcumgr/util/zcbor_bulk.h>
#include <mgmt/mcumgr/transport/smp_internal.h>
#include "smp_stub.h"

static const char *echo_ptr;

void os_stub_init(const char *echo_str)
{
	echo_ptr = echo_str;
}

void os_reset_response(void)
{
	struct net_buf *nb;

	nb = smp_response_buf_allocation();
	if (nb) {
		nb->len = 0;
	}
}

static void os_echo_response(int status, struct zcbor_string *echo_data)
{
	struct net_buf *nb;
	zcbor_state_t zse[3 + 2];
	bool ok;

	nb = smp_response_buf_allocation();

	if (!nb) {
		return;
	}

	zcbor_new_encode_state(zse, ARRAY_SIZE(zse), nb->data, net_buf_tailroom(nb), 0);

	if (status) {
		/* Init map start and write image info and data */
		ok = zcbor_map_start_encode(zse, 2) && zcbor_tstr_put_lit(zse, "rc") &&
		     zcbor_int32_put(zse, status) && zcbor_map_end_encode(zse, 2);
	} else {
		/* Init map start and write image info and data */
		ok = zcbor_map_start_encode(zse, 2) && zcbor_tstr_put_lit(zse, "r") &&
		     zcbor_tstr_encode_ptr(zse, echo_data->value, echo_data->len) &&
		     zcbor_map_end_encode(zse, 2);
	}

	if (!ok) {
		smp_client_response_buf_clean();
	} else {
		nb->len = zse->payload - nb->data;
	}
}

void os_echo_verify(struct net_buf *nb)
{
	/* Parse CBOR data: hash and confirm */
	zcbor_state_t zsd[3 + 2];
	int rc;
	int response_status;
	struct zcbor_string echo_data;
	size_t decoded;
	struct zcbor_map_decode_key_val list_res_decode[] = {
		ZCBOR_MAP_DECODE_KEY_DECODER("d", zcbor_tstr_decode, &echo_data)
		};

	zcbor_new_decode_state(zsd, ARRAY_SIZE(zsd), nb->data + sizeof(struct smp_hdr), nb->len, 1,
				NULL, 0);
	echo_data.len = 0;

	rc = zcbor_map_decode_bulk(zsd, list_res_decode, ARRAY_SIZE(list_res_decode), &decoded);
	if (rc || !echo_data.len) {
		printf("Corrupted data %d or no echo data %d\r\n", rc, echo_data.len);
		response_status = MGMT_ERR_EINVAL;
	} else if (memcmp(echo_data.value, echo_ptr, echo_data.len)) {
		response_status = MGMT_ERR_EINVAL;
	} else {
		response_status = MGMT_ERR_EOK;
	}

	os_echo_response(response_status, &echo_data);
}