diff --git a/fpga/zsbl/boot.c b/fpga/zsbl/boot.c index 16f7c0430..96bde4205 100644 --- a/fpga/zsbl/boot.c +++ b/fpga/zsbl/boot.c @@ -52,10 +52,40 @@ int disk_read(BYTE * buf, LBA_t sector, UINT count) { return -1; } - // Begin reading + // Begin reading blocks for (i = 0; i < count; i++) { + uint16_t crc, crc_exp; + + // Read the data token + r = spi_readbyte(); + if (r != SD_DATA_TOKEN) { + print_uart("Didn't receive data token first thing. Shoot: "); + print_byte(r & 0xff); + return -1; + } + + // Read block into memory. + for (int j = 0; j < 8; j++) { + *buf = sd_read64(&crc); + buf = buf + 64; + } + + // Read CRC16 and check + crc_exp = ((uint16_t)spi_txrx(0xff) << 8); + crc_exp |= spi_txrx(0xff); + + if (crc != crc_exp) { + print_uart("Stinking CRC16 didn't match on block "); + print_int(i); + print_uart("\r\n"); + return -1; + } } + + sd_cmd(SD_CMD_STOP_TRANSMISSION, 0, 0x01); + spi_txrx(0xff); + return 0; } // copyFlash: -------------------------------------------------------- diff --git a/fpga/zsbl/sd.h b/fpga/zsbl/sd.h index a70d203e7..37e4a2c94 100644 --- a/fpga/zsbl/sd.h +++ b/fpga/zsbl/sd.h @@ -15,4 +15,5 @@ uint8_t crc7(uint8_t prev, uint8_t in); uint16_t crc16(uint16_t crc, uint8_t data); uint64_t sd_cmd(uint8_t cmd, uint32_t arg, uint8_t crc); +uint64_t sd_read64(uint16_t * crc); void init_sd(); diff --git a/fpga/zsbl/spi.c b/fpga/zsbl/spi.c index 687a98ceb..76985a350 100644 --- a/fpga/zsbl/spi.c +++ b/fpga/zsbl/spi.c @@ -58,6 +58,12 @@ inline void waitrx() { while(read_reg(SPI_IP) & 2)) {} } +uint8_t spi_txrx(uint8_t byte) { + spi_sendbyte(0xFF); + waittx(); + return spi_readbyte(); +} + uint64_t spi_read64() { uint64_t r; uint8_t rbyte;