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

#include <zephyr.h>
#include <string.h>
#include <errno.h>
#include <cache.h>
#include <ipc/ipc_icmsg_buf.h>


/* Helpers */
static uint32_t idx_occupied(uint32_t len, uint32_t a, uint32_t b)
{
	/* It is implicitly assumed a and b cannot differ by more then len. */
	return (b > a) ? (len - (b - a)) : (a - b);
}

static uint32_t idx_cut(uint32_t len, uint32_t idx)
{
	/* It is implicitly assumed a and b cannot differ by more then len. */
	return (idx >= len) ? (idx - len) : (idx);
}

struct icmsg_buf *icmsg_buf_init(void *buf, size_t blen)
{
	/* blen must be big enough to contain icmsg_buf struct, byte of data
	 * and message len (2 bytes).
	 */
	struct icmsg_buf *ib = buf;

	__ASSERT_NO_MSG(blen > (sizeof(*ib) + sizeof(uint16_t)));

	ib->len = blen - sizeof(*ib);
	ib->wr_idx = 0;
	ib->rd_idx = 0;

	__sync_synchronize();
	sys_cache_data_range(ib, sizeof(*ib), K_CACHE_WB);

	return ib;
}

int icmsg_buf_write(struct icmsg_buf *ib, const char *buf, uint16_t len)
{
	/* The length of buffer is immutable - avoid reloading that may happen
	 * due to memory bariers.
	 */
	const uint32_t iblen = ib->len;

	/* rx_idx == wr_idx means the buffer is empty.
	 * Max bytes that can be stored is len - 1.
	 */
	const uint32_t max_len = iblen - 1;

	sys_cache_data_range(ib, sizeof(*ib), K_CACHE_INVD);
	__sync_synchronize();

	uint32_t wr_idx = ib->wr_idx;
	uint32_t rd_idx = ib->rd_idx;

	if (len == 0) {
		/* Incorrect call. */
		return -EINVAL;
	}

	uint32_t avail = max_len - idx_occupied(iblen, wr_idx, rd_idx);

	if ((len + sizeof(len) > avail) ||
	    (len + sizeof(len) > max_len)) {
		/* No free space. */
		return -ENOMEM;
	}

	/* Store info about the message length. */
	ib->data[wr_idx] = (uint8_t)len;
	sys_cache_data_range(&ib->data[wr_idx], sizeof(ib->data[wr_idx]), K_CACHE_WB);
	wr_idx = idx_cut(iblen, wr_idx + sizeof(ib->data[wr_idx]));

	ib->data[wr_idx] = (uint8_t)(len >> 8);
	sys_cache_data_range(&ib->data[wr_idx], sizeof(ib->data[wr_idx]), K_CACHE_WB);
	wr_idx = idx_cut(iblen, wr_idx + sizeof(ib->data[wr_idx]));

	/* Write until the end of the buffer. */
	uint32_t sz = MIN(len, iblen - wr_idx);

	memcpy(&ib->data[wr_idx], buf, sz);
	sys_cache_data_range(&ib->data[wr_idx], sz, K_CACHE_WB);

	if (len > sz) {
		/* Write remaining data at the buffer head. */
		memcpy(&ib->data[0], buf + sz, len - sz);
		sys_cache_data_range(&ib->data[0], len - sz, K_CACHE_WB);
	}

	/* Update write index - make other side aware data was written. */
	__sync_synchronize();
	wr_idx = idx_cut(iblen, wr_idx + len);
	ib->wr_idx = wr_idx;

	sys_cache_data_range(ib, sizeof(*ib), K_CACHE_WB);

	return len;
}

int icmsg_buf_read(struct icmsg_buf *ib, char *buf, uint16_t len)
{
	/* The length of buffer is immutable - avoid reloading. */
	const uint32_t iblen = ib->len;

	sys_cache_data_range(ib, sizeof(*ib), K_CACHE_INVD);
	__sync_synchronize();

	uint32_t rd_idx = ib->rd_idx;
	uint32_t wr_idx = ib->wr_idx;

	if (rd_idx == wr_idx) {
		/* The buffer is empty. */
		return 0;
	}

	uint32_t bytes_stored = idx_occupied(iblen, wr_idx, rd_idx);

	/* Read message len. */
	sys_cache_data_range(&ib->data[rd_idx], sizeof(ib->data[rd_idx]), K_CACHE_INVD);
	uint16_t mlen = ib->data[rd_idx];

	rd_idx = idx_cut(iblen, rd_idx + sizeof(ib->data[rd_idx]));

	sys_cache_data_range(&ib->data[rd_idx], sizeof(ib->data[rd_idx]), K_CACHE_INVD);
	mlen |= (ib->data[rd_idx] << 8);
	rd_idx = idx_cut(iblen, rd_idx + sizeof(ib->data[rd_idx]));

	if (!buf) {
		return mlen;
	}

	if (len < mlen) {
		return -ENOMEM;
	}

	if (bytes_stored < mlen + sizeof(mlen)) {
		/* Part of message not available. Should not happen. */
		__ASSERT_NO_MSG(false);
		return -EAGAIN;
	}

	len = MIN(len, mlen);

	/* Read up to the end of the buffer. */
	uint32_t sz = MIN(len, iblen - rd_idx);

	sys_cache_data_range(&ib->data[rd_idx], sz, K_CACHE_INVD);
	memcpy(buf, &ib->data[rd_idx], sz);
	if (len > sz) {
		/* Read remaining bytes starting from the buffer head. */
		sys_cache_data_range(&ib->data[0], len - sz, K_CACHE_INVD);
		memcpy(&buf[sz], &ib->data[0], len - sz);
	}

	/* Update read index - make other side aware data was read. */
	__sync_synchronize();
	rd_idx = idx_cut(iblen, rd_idx + len);
	ib->rd_idx = rd_idx;

	sys_cache_data_range(ib, sizeof(*ib), K_CACHE_WB);

	return len;
}