text: /*
   * Copyright (c) 2021 Intel Corporation
   * SPDX-License-Identifier: Apache-2.0
   */
  #include <zephyr/sys/util.h>
  #include <zephyr/sys/winstream.h>
  
  /* This code may be used (e.g. for trace/logging) in very early
   * environments where the standard library isn't available yet.
   * Implement a simple memcpy() as an option.
   */
  #ifndef CONFIG_WINSTREAM_STDLIB_MEMCOPY
  # define MEMCPY(dst, src, n) \
  	do { for (int i = 0; i < (n); i++) { (dst)[i] = (src)[i]; } } while (0)
  #else
  # include <string.h>
  # define MEMCPY memcpy
  #endif
  
  /* These are just compiler barriers now.  Zephyr doesn't currently
   * have a framework for hardware memory ordering, and all our targets
   * (arm64 excepted) either promise firm ordering (x86) or are in-order
   * cores (everything else).  But these are marked for future
   * enhancement.
   */
  #define READ_BARRIER()  __asm__ volatile("" ::: "memory")
  #define WRITE_BARRIER() __asm__ volatile("")
  
  static uint32_t idx_mod(struct sys_winstream *ws, uint32_t idx)
  {
  	return idx >= ws->len ? idx - ws->len : idx;
  }
  
  /* Computes modular a - b, assuming a and b are in [0:len) */
  static uint32_t idx_sub(struct sys_winstream *ws, uint32_t a, uint32_t b)
  {
  	return idx_mod(ws, a + (ws->len - b));
  }
  
  void sys_winstream_write(struct sys_winstream *ws,
  			 const char *data, uint32_t len)
  {
  	uint32_t len0 = len, suffix;
  	uint32_t start = ws->start, end = ws->end, seq = ws->seq;
  
  	/* Overflow: if we're truncating then just reset the buffer.
  	 * (Max bytes buffered is actually len-1 because start==end is
  	 * reserved to mean "empty")
  	 */
  	if (len > ws->len - 1) {
  		start = end;
  		len = ws->len - 1;
  	}
  
  	/* Make room in the buffer by advancing start first (note same
  	 * len-1 from above)
  	 */
  	len = MIN(len, ws->len);
  	if (seq != 0) {
  		uint32_t avail = (ws->len - 1) - idx_sub(ws, end, start);
  
  		if (len > avail) {
  			ws->start = idx_mod(ws, start + (len - avail));
  			WRITE_BARRIER();
  		}
  	}
  
  	/* Had to truncate? */
  	if (len < len0) {
  		ws->start = end;
  		data += len0 - len;
  	}
  
  	suffix = MIN(len, ws->len - end);
  	MEMCPY(&ws->data[end], data, suffix);
  	if (len > suffix) {
  		MEMCPY(&ws->data[0], data + suffix, len - suffix);
  	}
  
  	ws->end = idx_mod(ws, end + len);
  	ws->seq += len0; /* seq represents dropped bytes too! */
  	WRITE_BARRIER();
  }
  
  uint32_t sys_winstream_read(struct sys_winstream *ws,
  			    uint32_t *seq, char *buf, uint32_t buflen)
  {
  	uint32_t seq0 = *seq, start, end, wseq, len, behind, copy, suffix;
  
  	do {
  		start = ws->start; end = ws->end; wseq = ws->seq;
  		READ_BARRIER();
  
  		/* No change in buffer state or empty initial stream are easy */
  		if (*seq == wseq || start == end) {
  			*seq = wseq;
  			return 0;
  		}
  
  		/* Underflow: we're trying to read from a spot farther
  		 * back than start.  We dropped some bytes, so cheat
  		 * and just drop them all to catch up.
  		 */
  		behind = wseq - *seq;
  		if (behind > idx_sub(ws, ws->end, ws->start)) {
  			*seq = wseq;
  			return 0;
  		}
  
  		/* Copy data */
  		copy = idx_sub(ws, ws->end, behind);
  		len = MIN(buflen, behind);
  		suffix = MIN(len, ws->len - copy);
  		MEMCPY(buf, &ws->data[copy], suffix);
  		if (len > suffix) {
  			MEMCPY(buf + suffix, &ws->data[0], len - suffix);
  		}
  		*seq = seq0 + len;
  
  		/* Check vs. the state we initially read and repeat if
  		 * needed.  This can't loop forever even if the other
  		 * side is stuck spamming writes: we'll run out of
  		 * buffer space and exit via the underflow condition.
  		 */
  		READ_BARRIER();
  	} while (start != ws->start || wseq != ws->seq);
  
  	return len;
  }
  