[PATCH 5/8] ichspi.c: refactor filling and reading the fdata/spid registers

[Stefan Tauner]

- add ich_fill_data to fill the chipset registers from an array
- add ich_read_data to copy the data from the chipset register into an array
- replace the existing code with calls to those functions

NB: this changes the semantic of the *run_opcode functions a bit:
previously they could trash the chipset registers following the data registers because there
was no (explicit) length check (it was and is checked by the dispatching run_opcode method).

Signed-off-by: Stefan Tauner 
---
 ichspi.c |  116 ++++++++++++++++++++++++++++++--------------------------------
 1 files changed, 56 insertions(+), 60 deletions(-)

diff --git a/ichspi.c b/ichspi.c
index 19e52d2..26522a5 100644
--- a/ichspi.c
+++ b/ichspi.c
@@ -592,6 +592,55 @@ static void ich_set_bbar(uint32_t min_addr)
    	msg_perr("Setting BBAR failed!\n");
 }

+/* Reads up to len byte from the fdata/spid register into the data array.
+ * The amount actually read is limited by the maximum read size of the
+ * chipset. */
+ static void ich_read_data(uint8_t *data, uint8_t len, int reg0_off)
+ {
+	int a;
+	uint32_t temp32 = 0;
+
+	if (len > spi_programmer->max_data_read)
+		len = spi_programmer->max_data_read;
+
+	for (a = 0; a < len; a++) {
+		if ((a % 4) == 0)
+			temp32 = REGREAD32(reg0_off + (a));
+
+		data[a] = (temp32 & (((uint32_t) 0xff) << ((a % 4) * 8)))
+			  >> ((a % 4) * 8);
+	}
+}
+
+/* Fills up to len bytes from the data array into the fdata/spid registers.
+ * The amount actually written is limited by the maximum write size of the
+ * chipset and is returned by the function on success. Negative values indicate
+ * an error. */
+static uint8_t ich_fill_data(const uint8_t *data, uint8_t len, int reg0_off)
+{
+	uint32_t temp32 = 0;
+	int a;
+
+	if (len > spi_programmer->max_data_write)
+		len = spi_programmer->max_data_write;
+
+	for (a = 0; a < len; a++) {
+		if ((a % 4) == 0) {
+			temp32 = 0;
+		}
+
+		temp32 |= ((uint32_t) data[a]) << ((a % 4) * 8);
+
+		if ((a % 4) == 3) {
+			REGWRITE32(reg0_off + (a - (a % 4)), temp32);
+		}
+	}
+	if (((a - 1) % 4) != 3) {
+		REGWRITE32(reg0_off + ((a - 1) - ((a - 1) % 4)), temp32);
+	}
+	return len;
+}
+
 /* This function generates OPCODES from or programs OPCODES to ICH according to
  * the chipset's SPI configuration lock.
  *
@@ -638,9 +687,8 @@ static int ich7_run_opcode(OPCODE op, uint32_t offset,
 {
    int write_cmd = 0;
    int timeout;
-	uint32_t temp32 = 0;
+	uint32_t temp32;
    uint16_t temp16;
-	uint32_t a;
    uint64_t opmenu;
    int opcode_index;

@@ -664,26 +712,8 @@ static int ich7_run_opcode(OPCODE op, uint32_t offset,
    REGWRITE32(ICH7_REG_SPIA, (offset & 0x00FFFFFF) | temp32);

/* Program data into SPID0 to N */
-	if (write_cmd && (datalength != 0)) {
-		temp32 = 0;
-		for (a = 0; a < datalength; a++) {
-			if ((a % 4) == 0) {
-				temp32 = 0;
-			}
-
-			temp32 |= ((uint32_t) data[a]) << ((a % 4) * 8);
-
-			if ((a % 4) == 3) {
-				REGWRITE32(ICH7_REG_SPID0 + (a - (a % 4)),
-					   temp32);
-			}
-		}
-		if (((a - 1) % 4) != 3) {
-			REGWRITE32(ICH7_REG_SPID0 +
-				   ((a - 1) - ((a - 1) % 4)), temp32);
-		}
-
-	}
+	if (write_cmd && (datalength != 0))
+		ich_fill_data(data, datalength, ICH7_REG_SPID0);

/* Assemble SPIS */
    temp16 = REGREAD16(ICH7_REG_SPIS);
@@ -765,15 +795,7 @@ static int ich7_run_opcode(OPCODE op, uint32_t offset,
    }

if ((!write_cmd) && (datalength != 0)) {
-		for (a = 0; a < datalength; a++) {
-			if ((a % 4) == 0) {
-				temp32 = REGREAD32(ICH7_REG_SPID0 + (a));
-			}
-
-			data[a] =
-			    (temp32 & (((uint32_t) 0xff) << ((a % 4) * 8)))
-			    >> ((a % 4) * 8);
-		}
+		ich_read_data(data, datalength, ICH7_REG_SPID0);
    }

return 0;
@@ -785,7 +807,6 @@ static int ich9_run_opcode(OPCODE op, uint32_t offset,
    int write_cmd = 0;
    int timeout;
    uint32_t temp32;
-	uint32_t a;
    uint64_t opmenu;
    int opcode_index;

@@ -810,25 +831,8 @@ static int ich9_run_opcode(OPCODE op, uint32_t offset,
    REGWRITE32(ICH9_REG_FADDR, (offset & 0x00FFFFFF) | temp32);

/* Program data into FDATA0 to N */
-	if (write_cmd && (datalength != 0)) {
-		temp32 = 0;
-		for (a = 0; a < datalength; a++) {
-			if ((a % 4) == 0) {
-				temp32 = 0;
-			}
-
-			temp32 |= ((uint32_t) data[a]) << ((a % 4) * 8);
-
-			if ((a % 4) == 3) {
-				REGWRITE32(ICH9_REG_FDATA0 + (a - (a % 4)),
-					   temp32);
-			}
-		}
-		if (((a - 1) % 4) != 3) {
-			REGWRITE32(ICH9_REG_FDATA0 +
-				   ((a - 1) - ((a - 1) % 4)), temp32);
-		}
-	}
+	if (write_cmd && (datalength != 0))
+		ich_fill_data(data, datalength, ICH9_REG_FDATA0);

/* Assemble SSFS + SSFC */
    temp32 = REGREAD32(ICH9_REG_SSFS);
@@ -917,15 +921,7 @@ static int ich9_run_opcode(OPCODE op, uint32_t offset,
    }

if ((!write_cmd) && (datalength != 0)) {
-		for (a = 0; a < datalength; a++) {
-			if ((a % 4) == 0) {
-				temp32 = REGREAD32(ICH9_REG_FDATA0 + (a));
-			}
-
-			data[a] =
-			    (temp32 & (((uint32_t) 0xff) << ((a % 4) * 8)))
-			    >> ((a % 4) * 8);
-		}
+		ich_read_data(data, datalength, ICH9_REG_FDATA0);
    }

return 0;