From 836168be81da9d05aa87128516b06d556aa84c5d Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Thu, 12 Apr 2018 15:07:20 -0700
Subject: [PATCH] Fix FESPI assert when guessing few algorithm steps

Instead of trying to predict exactly how many steps will be required
(doable but error-prone), just allocate more memory when we need it.

Tested against HiFive1, and Arty board image.

Change-Id: I3cd9798432e65176616c700ba122daf7a5ed6209
---
 src/flash/nor/fespi.c | 64 +++++++++++++++++++++++--------------------
 1 file changed, 35 insertions(+), 29 deletions(-)

diff --git a/src/flash/nor/fespi.c b/src/flash/nor/fespi.c
index cedcb3b4a..f6827e385 100644
--- a/src/flash/nor/fespi.c
+++ b/src/flash/nor/fespi.c
@@ -607,11 +607,11 @@ struct algorithm_steps {
 	uint8_t **steps;
 };
 
-static struct algorithm_steps *as_new(unsigned size)
+static struct algorithm_steps *as_new(void)
 {
 	struct algorithm_steps *as = calloc(1, sizeof(struct algorithm_steps));
-	as->size = size;
-	as->steps = calloc(size, sizeof(as->steps[0]));
+	as->size = 8;
+	as->steps = malloc(as->size * sizeof(as->steps[0]));
 	return as;
 }
 
@@ -701,17 +701,27 @@ static unsigned as_compile(struct algorithm_steps *as, uint8_t *target,
 	return offset;
 }
 
+static void as_add_step(struct algorithm_steps *as, uint8_t *step)
+{
+	if (as->used == as->size) {
+		as->size *= 2;
+		as->steps = realloc(as->steps, sizeof(as->steps[0]) * as->size);
+		LOG_DEBUG("Increased size to 0x%x", as->size);
+	}
+	as->steps[as->used] = step;
+	as->used++;
+}
+
 static void as_add_tx(struct algorithm_steps *as, unsigned count, const uint8_t *data)
 {
 	LOG_DEBUG("count=%d", count);
 	while (count > 0) {
 		unsigned step_count = MIN(count, 255);
-		assert(as->used < as->size);
-		as->steps[as->used] = malloc(step_count + 2);
-		as->steps[as->used][0] = STEP_TX;
-		as->steps[as->used][1] = step_count;
-		memcpy(as->steps[as->used] + 2, data, step_count);
-		as->used++;
+		uint8_t *step = malloc(step_count + 2);
+		step[0] = STEP_TX;
+		step[1] = step_count;
+		memcpy(step + 2, data, step_count);
+		as_add_step(as, step);
 		data += step_count;
 		count -= step_count;
 	}
@@ -726,37 +736,33 @@ static void as_add_tx1(struct algorithm_steps *as, uint8_t byte)
 
 static void as_add_write_reg(struct algorithm_steps *as, uint8_t offset, uint8_t data)
 {
-	assert(as->used < as->size);
-	as->steps[as->used] = malloc(3);
-	as->steps[as->used][0] = STEP_WRITE_REG;
-	as->steps[as->used][1] = offset;
-	as->steps[as->used][2] = data;
-	as->used++;
+	uint8_t *step = malloc(3);
+	step[0] = STEP_WRITE_REG;
+	step[1] = offset;
+	step[2] = data;
+	as_add_step(as, step);
 }
 
 static void as_add_txwm_wait(struct algorithm_steps *as)
 {
-	assert(as->used < as->size);
-	as->steps[as->used] = malloc(1);
-	as->steps[as->used][0] = STEP_TXWM_WAIT;
-	as->used++;
+	uint8_t *step = malloc(1);
+	step[0] = STEP_TXWM_WAIT;
+	as_add_step(as, step);
 }
 
 static void as_add_wip_wait(struct algorithm_steps *as)
 {
-	assert(as->used < as->size);
-	as->steps[as->used] = malloc(1);
-	as->steps[as->used][0] = STEP_WIP_WAIT;
-	as->used++;
+	uint8_t *step = malloc(1);
+	step[0] = STEP_WIP_WAIT;
+	as_add_step(as, step);
 }
 
 static void as_add_set_dir(struct algorithm_steps *as, bool dir)
 {
-	assert(as->used < as->size);
-	as->steps[as->used] = malloc(2);
-	as->steps[as->used][0] = STEP_SET_DIR;
-	as->steps[as->used][1] = FESPI_FMT_DIR(dir);
-	as->used++;
+	uint8_t *step = malloc(2);
+	step[0] = STEP_SET_DIR;
+	step[1] = FESPI_FMT_DIR(dir);
+	as_add_step(as, step);
 }
 
 /* This should write something less than or equal to a page.*/
@@ -910,7 +916,7 @@ static int fespi_write(struct flash_bank *bank, const uint8_t *buffer,
 	if (retval != ERROR_OK)
 		return retval;
 
-	struct algorithm_steps *as = as_new(count / 4);
+	struct algorithm_steps *as = as_new();
 
 	/* unaligned buffer head */
 	if (count > 0 && (offset & 3) != 0) {