--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/loadtools/flutil.c	Sat Jun 11 00:13:35 2016 +0000
@@ -0,0 +1,350 @@
+/*
+ * Miscellaneous utility functions for flash support
+ */
+
+#include <sys/types.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include "flash.h"
+
+extern struct flash_bank_info flash_bank_info[2];
+extern struct flash_cmdset flash_cmdset_amd;
+extern struct flash_cmdset flash_cmdset_intel;
+
+static int
+cfi_read_byte(bi, off, ret16p)
+	struct flash_bank_info *bi;
+	int off;
+	uint16_t *ret16p;
+{
+	return do_r16(bi->base_addr + (off << 1), ret16p);
+}
+
+static int
+cfi_read_twobyte(bi, off, retptr)
+	struct flash_bank_info *bi;
+	int off;
+	uint16_t *retptr;
+{
+	uint16_t lo, hi;
+
+	if (cfi_read_byte(bi, off, &lo) < 0)
+		return(-1);
+	lo &= 0xFF;
+	if (cfi_read_byte(bi, off + 1, &hi) < 0)
+		return(-1);
+	hi &= 0xFF;
+	*retptr = (hi << 8) | lo;
+	return(0);
+}
+
+flash_get_cfi(bank)
+{
+	struct flash_bank_info *bi;
+	struct flash_geom *geom;
+	struct flash_region_desc *reg;
+	int nr;
+	uint16_t rdval, cmdset_id;
+	uint32_t size_check;
+
+	bi = flash_bank_info + bank;
+	if (bi->geom)
+		return(0);
+	printf("Performing CFI query\n");
+	if (do_w16(bi->base_addr + 0xAA, 0x98)) {
+		fprintf(stderr, "unexpected response to w16 - aborting\n");
+		return(-1);
+	}
+	/* if do_r16() returns -1, error msg has already been printed */
+	if (cfi_read_byte(bi, 0x10, &rdval) < 0)
+		return(-1);
+	if (rdval != 'Q') {
+noqry:		fprintf(stderr, "error: no QRY response from flash\n");
+		amd_reset_cmd(bi);
+		return(-1);
+	}
+	if (cfi_read_byte(bi, 0x11, &rdval) < 0)
+		return(-1);
+	if (rdval != 'R')
+		goto noqry;
+	if (cfi_read_byte(bi, 0x12, &rdval) < 0)
+		return(-1);
+	if (rdval != 'Y')
+		goto noqry;
+	if (cfi_read_twobyte(bi, 0x13, &cmdset_id) < 0)
+		return(-1);
+	if (!bi->ops) {
+		switch (cmdset_id) {
+		case 2:
+			bi->ops = &flash_cmdset_amd;
+			break;
+		case 3:
+			bi->ops = &flash_cmdset_intel;
+			break;
+		default:
+			fprintf(stderr, "error: command set %04X unsupported\n",
+				cmdset_id);
+			amd_reset_cmd(bi);
+			return(-1);
+		}
+	}
+	geom = malloc(sizeof(struct flash_geom));
+	if (!geom) {
+		fprintf(stderr,
+	"unable to malloc buffer for flash bank %d CFI geometry structure\n",
+			bank);
+		bi->ops->reset_cmd(bi);
+		return(-1);
+	}
+	/* total device size */
+	if (cfi_read_byte(bi, 0x27, &rdval) < 0) {
+free_and_immed_out:
+		free(geom);
+		return(-1);
+	}
+	if (rdval < 20 || rdval > 24) {
+		fprintf(stderr,
+			"error: CFI reports unreasonable device size\n");
+free_and_clean_out:
+		free(geom);
+		bi->ops->reset_cmd(bi);
+		return(-1);
+	}
+	geom->total_size = 1 << rdval;
+	if (geom->total_size > bi->bank_desc->align_size) {
+		fprintf(stderr,
+	"error: CFI device size 0x%lx exceeds configured maximum 0x%lx\n",
+			(u_long) geom->total_size, bi->bank_desc->align_size);
+		goto free_and_clean_out;
+	}
+	if (cfi_read_byte(bi, 0x2C, &rdval) < 0)
+		goto free_and_immed_out;
+	if (rdval < 1 || rdval > CFI_MAX_REGIONS) {
+		fprintf(stderr,
+			"error: CFI reports unreasonable # of erase regions\n");
+		goto free_and_clean_out;
+	}
+	geom->nregions = rdval;
+	geom->total_sectors = 0;
+	size_check = 0;
+	for (nr = 0; nr < geom->nregions; nr++) {
+		reg = geom->regions + nr;
+		if (cfi_read_twobyte(bi, 0x2D + nr*4, &rdval) < 0)
+			goto free_and_immed_out;
+		if (rdval > 255) {
+			fprintf(stderr,
+		"error: CFI reports unreasonable # of sectors in region %d\n",
+				nr);
+			goto free_and_clean_out;
+		}
+		reg->nsectors = rdval + 1;
+		geom->total_sectors += reg->nsectors;
+		if (cfi_read_twobyte(bi, 0x2F + nr*4, &rdval) < 0)
+			goto free_and_immed_out;
+		if (rdval < 0x20 || rdval > 0x400) {
+			fprintf(stderr,
+		"error: CFI reports unreasonable sector size in region %d\n",
+				nr);
+			goto free_and_clean_out;
+		}
+		reg->sector_size = rdval << 8;
+		size_check += reg->sector_size * reg->nsectors;
+	}
+	if (bi->ops->reset_cmd(bi) < 0) {
+		/* error msg already printed */
+		free(geom);
+		return(-1);
+	}
+	if (size_check != geom->total_size) {
+		fprintf(stderr,
+"CFI error: added size of erase regions (%lx) != reported devive size (%lx)\n",
+			(u_long) size_check, (u_long) geom->total_size);
+		free(geom);
+		return(-1);
+	}
+	/* all checks passed */
+	bi->geom = geom;
+	printf(
+"CFI query successful: total size %lx, %u sectors, command set style %04X\n",
+		(u_long) geom->total_size, geom->total_sectors, cmdset_id);
+	return(1);
+}
+
+get_flash_sector_table(bank)
+{
+	struct flash_bank_info *bi;
+	struct flash_geom *geom;
+	struct flash_region_desc *reg;
+	struct sector_info *sp;
+	uint32_t offset;
+	int nr, i;
+
+	bi = flash_bank_info + bank;
+	if (bi->sectors)
+		return(0);
+	i = flash_get_cfi(bank);
+	if (i < 0)
+		return(i);
+	geom = bi->geom;
+	sp = (struct sector_info *) malloc(sizeof(struct sector_info)
+						* (geom->total_sectors + 1));
+	if (!sp) {
+		fprintf(stderr,
+		"unable to malloc buffer for flash bank %d sector table\n",
+			bank);
+		return(-1);
+	}
+	bi->sectors = sp;
+	/* now fill it */
+	offset = 0;
+	for (nr = 0; nr < geom->nregions; nr++) {
+		reg = geom->regions + nr;
+		for (i = 0; i < reg->nsectors; i++) {
+			sp->start = offset;
+			sp->size = reg->sector_size;
+			sp++;
+			offset += reg->sector_size;
+		}
+	}
+	/* sanity checks */
+	if (sp - bi->sectors != geom->total_sectors) {
+		fprintf(stderr,
+	"BUG in get_flash_sector_table(): wrong # of sectors at the end\n");
+		abort();
+	}
+	if (offset != geom->total_size) {
+		fprintf(stderr,
+		"BUG in get_flash_sector_table(): wrong offset at the end\n");
+		abort();
+	}
+	/* finish */
+	sp->start = 0;
+	sp->size = 0;
+	return(0);
+}
+
+flashcmd_sectors(argc, argv, bank)
+	char **argv;
+{
+	struct flash_bank_info *bi;
+	struct sector_info *sp;
+
+	if (argc > 2) {
+		fprintf(stderr, "error: too many arguments\n");
+		return(-1);
+	}
+	if (get_flash_sector_table(bank) < 0)
+		return(-1);
+	bi = flash_bank_info + bank;
+	printf("%u sectors in flash bank %d:\n", bi->geom->total_sectors, bank);
+	printf("Offset    Size\n");
+	for (sp = bi->sectors; sp->size; sp++)
+		printf("%08lX  %lx\n", (u_long) sp->start, (u_long) sp->size);
+	return(0);
+}
+
+get_flash_sector_range(bi, useroff, userlen, startp, endp)
+	struct flash_bank_info *bi;
+	u_long useroff, userlen;
+	struct sector_info **startp, **endp;
+{
+	struct sector_info *sp;
+	uint32_t remlen;
+
+	for (sp = bi->sectors; sp->size; sp++)
+		if (sp->start == useroff)
+			break;
+	if (!sp->size) {
+		fprintf(stderr,
+	"error: specified offset not aligned to a flash sector boundary\n");
+		return(-1);
+	}
+	*startp = sp;
+	for (remlen = userlen; remlen; ) {
+		if (remlen < sp->size) {
+			fprintf(stderr,
+	"error: specified length not aligned to a flash sector boundary\n");
+			return(-1);
+		}
+		remlen -= sp->size;
+		sp++;
+	}
+	*endp = sp;
+	return(0);
+}
+
+build_flashw_hex_string(bin, strbuf, nwords, m0src)
+	u_char *bin;
+	char *strbuf;
+	int nwords, m0src;
+{
+	int i;
+	u_char *dp;
+	char *s;
+
+	for (dp = bin, s = strbuf, i = 0; i < nwords; dp += 2, s += 4, i++) {
+		if (m0src)
+			sprintf(s, "%02X%02X", dp[0], dp[1]);
+		else
+			sprintf(s, "%02X%02X", dp[1], dp[0]);
+	}
+	*s = '\0';
+}
+
+flash_id_check(bank, repeat)
+{
+	struct flash_bank_info *bi;
+	struct flash_bank_desc *bd;
+	struct flash_idcheck *id;
+	int stat, fail;
+	uint16_t rdval;
+	unsigned cnt;
+
+	bi = flash_bank_info + bank;
+	if (bi->idcheck_done && !repeat)
+		return(0);
+	bd = bi->bank_desc;
+	if (!bd->idcheck_table || !bd->idcheck_num)
+		return(0);
+	printf("Performing flash ID check\n");
+	stat = do_w16(bi->base_addr + 0xAAA, 0xAA);
+	if (stat) {
+bad_w16:	fprintf(stderr,
+	"unexpected response to w16 in read ID cmd sequence - aborting\n");
+		return(-1);
+	}
+	stat = do_w16(bi->base_addr + 0x554, 0x55);
+	if (stat)
+		goto bad_w16;
+	stat = do_w16(bi->base_addr + 0xAAA, 0x90);
+	if (stat)
+		goto bad_w16;
+	id = bd->idcheck_table;
+	fail = 0;
+	for (cnt = 0; cnt < bd->idcheck_num; cnt++) {
+		stat = do_r16(bi->base_addr + id->offset, &rdval);
+		if (stat)
+			return(stat);	/* error msg already printed */
+		printf("offset %02X: %04X -- ", (int)id->offset, (int)rdval);
+		if (rdval == id->expect_val)
+			printf("PASS\n");
+		else {
+			printf("FAIL: expected %04X\n", (int)id->expect_val);
+			fail = 1;
+			break;
+		}
+		id++;
+	}
+	/* reset flash to read mode */
+	stat = do_w16(bi->base_addr + 0xAAA, 0xF0);
+	if (stat) {
+		fprintf(stderr,
+	"unexpected response to w16 when resetting flash to read mode!\n");
+		return(-1);
+	}
+	if (fail)
+		return(-1);
+	bi->idcheck_done = 1;
+	return(0);
+}