zephyr/subsys/ipc/ipc_service/lib/pbuf.c

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

#include <zephyr/kernel.h>
#include <string.h>
#include <errno.h>
#include <zephyr/cache.h>
#include <zephyr/ipc/pbuf.h>
#include <zephyr/sys/byteorder.h>

/* Helper funciton for getting numer of bytes being written to the bufer. */
static uint32_t idx_occupied(uint32_t len, uint32_t wr_idx, uint32_t rd_idx)
{
	/* It is implicitly assumed wr_idx and rd_idx cannot differ by more then len. */
	return (rd_idx > wr_idx) ? (len - (rd_idx - wr_idx)) : (wr_idx - rd_idx);
}

/* Helper function for wrapping the index from the begging if above buffer len. */
static uint32_t idx_wrap(uint32_t len, uint32_t idx)
{
	return (idx >= len) ? (idx % len) : (idx);
}

static int validate_cfg(const struct pbuf_cfg *cfg)
{
	/* Validate pointers. */
	if (!cfg || !cfg->rd_idx_loc || !cfg->wr_idx_loc || !cfg->data_loc) {
		return -EINVAL;
	}

	/* Validate pointer alignment. */
	if (!IS_PTR_ALIGNED_BYTES(cfg->rd_idx_loc, MAX(cfg->dcache_alignment, _PBUF_IDX_SIZE)) ||
	    !IS_PTR_ALIGNED_BYTES(cfg->wr_idx_loc, MAX(cfg->dcache_alignment, _PBUF_IDX_SIZE)) ||
	    !IS_PTR_ALIGNED_BYTES(cfg->data_loc, _PBUF_IDX_SIZE)) {
		return -EINVAL;
	}

	/* Validate len. */
	if (cfg->len < _PBUF_MIN_DATA_LEN || !IS_PTR_ALIGNED_BYTES(cfg->len, _PBUF_IDX_SIZE)) {
		return -EINVAL;
	}

	/* Validate pointer values. */
	if (!(cfg->rd_idx_loc < cfg->wr_idx_loc) ||
	    !((uint8_t *)cfg->wr_idx_loc < cfg->data_loc) ||
	    !(((uint8_t *)cfg->rd_idx_loc + MAX(_PBUF_IDX_SIZE, cfg->dcache_alignment)) ==
	    (uint8_t *)cfg->wr_idx_loc)) {
		return -EINVAL;
	}

	return 0;
}

int pbuf_init(struct pbuf *pb)
{
	if (validate_cfg(pb->cfg) != 0) {
		return -EINVAL;
	}

	/* Initialize local copy of indexes. */
	pb->data.wr_idx = 0;
	pb->data.rd_idx = 0;

	/* Clear shared memory. */
	*(pb->cfg->wr_idx_loc) = pb->data.wr_idx;
	*(pb->cfg->rd_idx_loc) = pb->data.rd_idx;

	__sync_synchronize();

	/* Take care cache. */
	sys_cache_data_flush_range((void *)(pb->cfg->wr_idx_loc), sizeof(*(pb->cfg->wr_idx_loc)));
	sys_cache_data_flush_range((void *)(pb->cfg->rd_idx_loc), sizeof(*(pb->cfg->rd_idx_loc)));

	return 0;
}

int pbuf_write(struct pbuf *pb, const char *data, uint16_t len)
{
	if (pb == NULL || len == 0 || data == NULL) {
		/* Incorrect call. */
		return -EINVAL;
	}

	/* Invalidate rd_idx only, local wr_idx is used to increase buffer security. */
	sys_cache_data_invd_range((void *)(pb->cfg->rd_idx_loc), sizeof(*(pb->cfg->rd_idx_loc)));
	__sync_synchronize();

	uint8_t *const data_loc = pb->cfg->data_loc;
	const uint32_t blen = pb->cfg->len;
	uint32_t rd_idx = *(pb->cfg->rd_idx_loc);
	uint32_t wr_idx = pb->data.wr_idx;

	/* wr_idx must always be aligned. */
	__ASSERT_NO_MSG(IS_PTR_ALIGNED_BYTES(wr_idx, _PBUF_IDX_SIZE));
	/* rd_idx shall always be aligned, but its value is received from the reader.
	 * Can not assert.
	 */
	if (!IS_PTR_ALIGNED_BYTES(rd_idx, _PBUF_IDX_SIZE)) {
		return -EINVAL;
	}

	uint32_t free_space = blen - idx_occupied(blen, wr_idx, rd_idx) - _PBUF_IDX_SIZE;

	/* Packet length, data + packet length size. */
	uint32_t plen = len + PBUF_PACKET_LEN_SZ;

	/* Check if packet will fit into the buffer. */
	if (free_space < plen) {
		return -ENOMEM;
	}

	/* Clear packet len with zeros and update. Clearing is done for possible versioning in the
	 * future. Writing is allowed now, because shared wr_idx value is updated at the very end.
	 */
	*((uint32_t *)(&data_loc[wr_idx])) = 0;
	sys_put_be16(len, &data_loc[wr_idx]);
	__sync_synchronize();
	sys_cache_data_flush_range(&data_loc[wr_idx], PBUF_PACKET_LEN_SZ);

	wr_idx = idx_wrap(blen, wr_idx + PBUF_PACKET_LEN_SZ);

	/* Write until end of the buffer, if data will be wrapped. */
	uint32_t tail = MIN(len, blen - wr_idx);

	memcpy(&data_loc[wr_idx], data, tail);
	sys_cache_data_flush_range(&data_loc[wr_idx], tail);

	if (len > tail) {
		/* Copy remaining data to buffer front. */
		memcpy(&data_loc[0], data + tail, len - tail);
		sys_cache_data_flush_range(&data_loc[0], len - tail);
	}

	wr_idx = idx_wrap(blen, ROUND_UP(wr_idx + len, _PBUF_IDX_SIZE));
	/* Update wr_idx. */
	pb->data.wr_idx = wr_idx;
	*(pb->cfg->wr_idx_loc) = wr_idx;
	__sync_synchronize();
	sys_cache_data_flush_range((void *)pb->cfg->wr_idx_loc, sizeof(*(pb->cfg->wr_idx_loc)));

	return len;
}

int pbuf_read(struct pbuf *pb, char *buf, uint16_t len)
{
	if (pb == NULL) {
		/* Incorrect call. */
		return -EINVAL;
	}

	/* Invalidate wr_idx only, local rd_idx is used to increase buffer security. */
	sys_cache_data_invd_range((void *)(pb->cfg->wr_idx_loc), sizeof(*(pb->cfg->wr_idx_loc)));
	__sync_synchronize();

	uint8_t *const data_loc = pb->cfg->data_loc;
	const uint32_t blen = pb->cfg->len;
	uint32_t wr_idx = *(pb->cfg->wr_idx_loc);
	uint32_t rd_idx = pb->data.rd_idx;

	/* rd_idx must always be aligned. */
	__ASSERT_NO_MSG(IS_PTR_ALIGNED_BYTES(rd_idx, _PBUF_IDX_SIZE));
	/* wr_idx shall always be aligned, but its value is received from the
	 * writer. Can not assert.
	 */
	if (!IS_PTR_ALIGNED_BYTES(wr_idx, _PBUF_IDX_SIZE)) {
		return -EINVAL;
	}

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

	/* Get packet len.*/
	sys_cache_data_invd_range(&data_loc[rd_idx], PBUF_PACKET_LEN_SZ);
	uint16_t plen = sys_get_be16(&data_loc[rd_idx]);

	if (!buf) {
		return (int)plen;
	}

	if (plen > len) {
		return -ENOMEM;
	}

	uint32_t occupied_space = idx_occupied(blen, wr_idx, rd_idx);

	if (occupied_space < plen + PBUF_PACKET_LEN_SZ) {
		/* This should never happen. */
		return -EAGAIN;
	}

	rd_idx = idx_wrap(blen, rd_idx + PBUF_PACKET_LEN_SZ);

	/* Packet will fit into provided buffer, truncate len if provided len
	 * is bigger than necessary.
	 */
	len = MIN(plen, len);

	/* Read until end of the buffer, if data are wrapped. */
	uint32_t tail = MIN(blen - rd_idx, len);

	sys_cache_data_invd_range(&data_loc[rd_idx], tail);
	memcpy(buf, &data_loc[rd_idx], tail);

	if (len > tail) {
		sys_cache_data_invd_range(&data_loc[0], len - tail);
		memcpy(&buf[tail], &pb->cfg->data_loc[0], len - tail);
	}

	/* Update rd_idx. */
	rd_idx = idx_wrap(blen, ROUND_UP(rd_idx + len, _PBUF_IDX_SIZE));

	pb->data.rd_idx = rd_idx;
	*(pb->cfg->rd_idx_loc) = rd_idx;
	__sync_synchronize();
	sys_cache_data_flush_range((void *)pb->cfg->rd_idx_loc, sizeof(*(pb->cfg->rd_idx_loc)));

	return len;
}