somehow it worked

hydrausb3 · Feb 24, 2025 · 0da6a9f · 0da6a9f
1 parent 3294f3f
commit 0da6a9f
Showing 1 changed file with 11 additions and 30 deletions.
diff --git a/src/wch-ch56x-lib/memory/fifo.h b/src/wch-ch56x-lib/memory/fifo.h
@@ -145,23 +145,23 @@ _fifo_read_n(hydra_fifo_t* fifo, void* buffer, uint16_t n, uint16_t rd_idx,
 	{
 		uint16_t linear_count = MIN(n, wr_idx - rd_idx);
 		uint16_t linear_bytes = linear_count * fifo->type_size;
-		memcpy(buffer, fifo->buffer + rd_idx * fifo->type_size, linear_bytes);
+		memcpy(buffer, fifo->buffer + (rd_idx + 1) * fifo->type_size, linear_bytes);
 		LOG_IF_LEVEL(LOG_LEVEL_TRACE,
 					 "read rd_idx=%d wr_idx=%d linear_count=%d\r\n", rd_idx, wr_idx,
 					 linear_count);
 		return linear_count;
 	}
 
-	uint16_t linear_count = MIN(n, fifo->size - rd_idx);
+	uint16_t linear_count = MIN(n, fifo->size - 1 - rd_idx);
 	uint16_t linear_bytes = linear_count * fifo->type_size;
-	memcpy(buffer, fifo->buffer + rd_idx * fifo->type_size, linear_bytes);
+	memcpy(buffer, fifo->buffer + (rd_idx + 1) * fifo->type_size, linear_bytes);
 	LOG_IF_LEVEL(LOG_LEVEL_TRACE, "read rd_idx=%d wr_idx=%d linear_count=%d\r\n",
 				 rd_idx, wr_idx, linear_count);
 
 	if (n > linear_count)
 	{
-		uint16_t wrap_count = MIN(wr_idx, n - linear_count);
-		uint16_t wrap_bytes = wr_idx * fifo->type_size;
+		uint16_t wrap_count = MIN(wr_idx + 1, n - linear_count);
+		uint16_t wrap_bytes = wrap_count * fifo->type_size;
 		memcpy((uint8_t*)buffer + linear_bytes, fifo->buffer, wrap_bytes);
 		LOG_IF_LEVEL(LOG_LEVEL_TRACE,
 					 "read rd_idx=%d wr_idx=%d linear_count=%d, wrap_count=%d\r\n",
@@ -191,23 +191,22 @@ _fifo_write_n(hydra_fifo_t* fifo, void* buffer, uint16_t n, uint16_t rd_idx,
 	{
 		uint16_t linear_count = MIN(free_space, n);
 		uint16_t linear_bytes = linear_count * fifo->type_size;
-		memcpy(fifo->buffer + wr_idx * fifo->type_size, buffer, linear_bytes);
+		memcpy(fifo->buffer + (wr_idx + 1) * fifo->type_size, buffer, linear_bytes);
 		LOG_IF_LEVEL(LOG_LEVEL_TRACE,
 					 "write rd_idx=%d wr_idx=%d linear_count=%d\r\n", rd_idx,
 					 wr_idx, linear_count);
 		return linear_count;
 	}
 
-	uint16_t linear_count = MIN(fifo->size - wr_idx, n);
+	uint16_t linear_count = MIN(fifo->size - 1 - wr_idx, n);
 	uint16_t linear_bytes = linear_count * fifo->type_size;
-	memcpy(fifo->buffer + wr_idx * fifo->type_size, buffer, linear_bytes);
+	memcpy(fifo->buffer + (wr_idx + 1) * fifo->type_size, buffer, linear_bytes);
 
 	if (n > linear_count)
 	{
 		uint16_t wrap_count = MIN(rd_idx, n - linear_count);
 		uint16_t wrap_bytes = wrap_count * fifo->type_size;
-		memcpy(fifo->buffer + wr_idx * fifo->type_size + linear_bytes,
-			   (uint8_t*)buffer + linear_bytes, wrap_bytes);
+		memcpy(fifo->buffer, (uint8_t*)buffer + linear_bytes, wrap_bytes);
 		LOG_IF_LEVEL(LOG_LEVEL_TRACE,
 					 "write rd_idx=%d wr_idx=%d linear_count=%d, wrap_count=%d\r\n",
 					 rd_idx, wr_idx, linear_count, wrap_count);
@@ -227,33 +226,15 @@ _fifo_advance_read(hydra_fifo_t* fifo, uint16_t rd_idx, uint16_t offset);
 __attribute__((always_inline)) static inline void
 _fifo_advance_read(hydra_fifo_t* fifo, uint16_t rd_idx, uint16_t offset)
 {
-	uint16_t remaining = fifo->size - rd_idx;
-	LOG_IF_LEVEL(LOG_LEVEL_TRACE, "advance_read rd_idx=%d offset=%d\r\n", rd_idx,
-				 offset);
-
-	if (offset < remaining)
-	{
-		fifo->rd_idx += offset;
-		return;
-	}
-	fifo->rd_idx = offset - remaining;
+	fifo->rd_idx = (fifo->rd_idx + offset) % fifo->size;
 }
 
 __attribute__((always_inline)) static inline void
 _fifo_advance_write(hydra_fifo_t* fifo, uint16_t wr_idx, uint16_t offset);
 __attribute__((always_inline)) static inline void
 _fifo_advance_write(hydra_fifo_t* fifo, uint16_t wr_idx, uint16_t offset)
 {
-	uint16_t remaining = fifo->size - wr_idx;
-	LOG_IF_LEVEL(LOG_LEVEL_TRACE, "advance_write wr_idx=%d offset=%d\r\n", wr_idx,
-				 offset);
-
-	if (offset < remaining)
-	{
-		fifo->wr_idx += offset;
-		return;
-	}
-	fifo->wr_idx = offset - remaining;
+	fifo->wr_idx = (fifo->wr_idx + offset) % fifo->size;
 }
 
 /**