1 files changed, 47 insertions, 21 deletions

diff --git a/board/scalys/grapeboard/board_configuration_data.c b/board/scalys/grapeboard/board_configuration_data.c
index 4daff4f..a730c11 100644
--- a/board/scalys/grapeboard/board_configuration_data.c
+++ b/board/scalys/grapeboard/board_configuration_data.c
@@ -83,35 +83,61 @@ int add_mac_addressess_to_env(const void* blob)
 	return 0;
 }
 
-const void* get_boardinfo_rescue_flash(void)
-{
+struct udevice* sel_rescue_qspi_flash(bool sel_rescue) {
 	struct ccsr_gpio *pgpio = (void *)(CONFIG_SYS_GPIO2);
-
-	uint32_t bcd_data_length;
-	uint8_t *bcd_data = NULL;
-	uint32_t calculated_crc, received_crc;
-	int dtb_length;
-	int ret = 0;
-
 	struct udevice *rescue_flash_dev,*bus_dev;
+	int ret = 0;
 	unsigned int bus = 0;
 	unsigned int cs = 0;
 	unsigned int speed = 0;
 	unsigned int mode = 0;
 
-	/* Change chip select to rescue QSPI NOR flash */
-	setbits_be32(&pgpio->gpdir, QSPI_MUX_N_MASK);
-	setbits_be32(&pgpio->gpdat, QSPI_MUX_N_MASK);
-
+	/* Remove previous DM device */
 	ret = spi_find_bus_and_cs(bus, cs, &bus_dev, &rescue_flash_dev);
 	if (!ret) {
 		device_remove(rescue_flash_dev, DM_REMOVE_NORMAL);
 	}
+
+	setbits_be32(&pgpio->gpdir, QSPI_MUX_N_MASK);
+	if (sel_rescue == true) {
+		/* Change chip select to rescue QSPI NOR flash */
+		setbits_be32(&pgpio->gpdat, QSPI_MUX_N_MASK);
+	} else {
+		/* Revert chip select muxing to standard QSPI flash */
+		clrbits_be32(&pgpio->gpdat, QSPI_MUX_N_MASK);
+		/* Delay required (to meet RC time for button debouncing) before probing flash again.
+		 * May be removed but the primary flash is only available after delay */
+		udelay(75000);
+	}
+
+	/* Probe new flash */
 	ret = spi_flash_probe_bus_cs(bus, cs, speed, mode, &rescue_flash_dev);
 	if (ret != 0) {
 		printf("probe failed\n");
+		return NULL;
 	}
 
+	return rescue_flash_dev;
+
+}
+
+
+
+const void* get_boardinfo_rescue_flash(void)
+{
+	struct udevice *rescue_flash_dev;
+	uint32_t bcd_data_length;
+	uint8_t *bcd_data = NULL;
+	uint32_t calculated_crc, received_crc;
+	int dtb_length;
+	int ret = 0;
+
+	/* Select and probe rescue flash */
+	rescue_flash_dev = sel_rescue_qspi_flash(true);
+
+	if (rescue_flash_dev == NULL)
+		goto err_no_free;
+
 	/* Read the last 4 bytes to determine the length of the DTB data */
 	ret = spi_flash_read_dm(rescue_flash_dev, (BCD_FLASH_SIZE-4), 4, (uint8_t*) &bcd_data_length);
 	if (ret != 0) {
@@ -122,10 +148,10 @@ const void* get_boardinfo_rescue_flash(void)
 	
 	/* Convert length from big endianess to architecture endianess */
 	bcd_data_length = ntohl(bcd_data_length);
-	printf("bcd_data_lenght = %i\n", bcd_data_length );
+	printf("bcd_data_length = %i\n", bcd_data_length );
 	
 	if (bcd_data_length > BCD_FLASH_SIZE ) {
-		printf("BCD data length error %02x %02x %02x %02x\n", 
+		debug("BCD data length error %02x %02x %02x %02x\n",
 		      ( (uint8_t*) &bcd_data_length)[0],
 		      ( (uint8_t*) &bcd_data_length)[1],
 		      ( (uint8_t*) &bcd_data_length)[2],
@@ -140,11 +166,11 @@ const void* get_boardinfo_rescue_flash(void)
 		printf("Error locating memory for BCD data\n");
 		goto err_no_free;
 	}
-	printf("Allocated memory for BCD data\n");
+	debug("Allocated memory for BCD data\n");
 
 	/* Read the DTB BCD data to memory */
 	ret = spi_flash_read_dm(rescue_flash_dev, (BCD_FLASH_SIZE-bcd_data_length), bcd_data_length, (uint8_t*) bcd_data);
-	printf("Read data from QSPI bus\n");
+	debug("Read data from QSPI bus\n");
 
 	if (ret != 0) {
 		printf("Error reading complete BCD data from EEPROM\n");
@@ -169,8 +195,8 @@ const void* get_boardinfo_rescue_flash(void)
 		goto err_free;
 	}
 
-	/* Revert chip select muxing to standard QSPI flash */
-	clrbits_be32(&pgpio->gpdat, QSPI_MUX_N_MASK);
+   	/* Select and probe normal flash */
+   	rescue_flash_dev = sel_rescue_qspi_flash(false);
 
 	/* Everything checked out, return the BCD data.
 	 * The caller is expected to free this data */
@@ -181,9 +207,9 @@ err_free:
 	free(bcd_data);
 	
 err_no_free:	
-	/* Revert chip select muxing to standard QSPI flash */
-	clrbits_be32(&pgpio->gpdat, QSPI_MUX_N_MASK);
 
+	/* Select and probe normal flash */
+	rescue_flash_dev = sel_rescue_qspi_flash(false);
 	return NULL;
 }