diff --git a/lib/sata/bist.py b/lib/sata/bist.py
index 34f0951f9..e55c0b8c6 100644
--- a/lib/sata/bist.py
+++ b/lib/sata/bist.py
@@ -27,9 +27,9 @@ class SATABIST(Module):
 		fsm.act("IDLE",
 			self.done.eq(1),
 			counter.reset.eq(1),
-			self.ctrl_error_counter.reset.eq(1),
-			self.data_error_counter.reset.eq(1),
 			If(self.start,
+				self.ctrl_error_counter.reset.eq(1),
+				self.data_error_counter.reset.eq(1),
 				NextState("SEND_WRITE_CMD_AND_DATA")
 			)
 		)
diff --git a/targets/test.py b/targets/test.py
index 7d3c98e6b..05cba7f13 100644
--- a/targets/test.py
+++ b/targets/test.py
@@ -268,29 +268,63 @@ class CommandGenerator(Module, AutoCSR):
 class BIST(Module, AutoCSR):
 	def __init__(self, sata_con, sector_size):
 		self._start = CSR()
-		self._sector = CSRStorage(48)
-		self._count = CSRStorage(4)
-		self._done = CSRStatus()
+		self._stop = CSR()
 
+		self._sector = CSRStatus(48)
 		self._ctrl_errors = CSRStatus(32)
 		self._data_errors = CSRStatus(32)
 
+		check_prepare = Signal()
+		sector = self._sector.status
+		ctrl_errors = self._ctrl_errors.status
+		data_errors = self._data_errors.status
+
 		###
 
 		self.sata_bist = SATABIST(sector_size)
 		self.comb += [
-			Record.connect(self.sata_bist.source, sata_con.sink),
 			Record.connect(sata_con.source, self.sata_bist.sink),
-
-			self.sata_bist.start.eq(self._start.r & self._start.re),
-			self.sata_bist.sector.eq(self._sector.storage),
-			self.sata_bist.count.eq(self._count.storage),
-			self._done.status.eq(self.sata_bist.done),
-
-			self._ctrl_errors.status.eq(self.sata_bist.ctrl_errors),
-			self._data_errors.status.eq(self.sata_bist.data_errors),
+			Record.connect(self.sata_bist.source, sata_con.sink)
 		]
 
+		self.fsm = fsm = FSM(reset_state="IDLE")
+
+		self.comb += [
+			self.sata_bist.sector.eq(sector),
+			self.sata_bist.count.eq(4)
+		]
+
+		# FSM
+		fsm.act("IDLE",
+			If(self._start.r & self._start.re,
+				NextState("START")
+			)
+		)
+		fsm.act("START",
+			self.sata_bist.start.eq(1),
+			NextState("WAIT_DONE")
+		)
+		fsm.act("WAIT_DONE",
+			If(self.sata_bist.done,
+				NextState("CHECK_PREPARE")
+			).Elif(self._stop.r & self._stop.re,
+				NextState("IDLE")
+			)
+		)
+		fsm.act("CHECK_PREPARE",
+			check_prepare.eq(1),
+			NextState("START")
+		)
+
+		self.sync += [
+			If(check_prepare,
+				ctrl_errors.eq(ctrl_errors + self.sata_bist.ctrl_errors),
+				data_errors.eq(data_errors + self.sata_bist.data_errors),
+				sector.eq(sector+4)
+			)
+		]
+
+
 class TestDesign(UART2WB, AutoCSR):
 	default_platform = "kc705"
 	csr_map = {
diff --git a/test/test_bist.py b/test/test_bist.py
index d8bf46fc2..915099d7e 100644
--- a/test/test_bist.py
+++ b/test/test_bist.py
@@ -2,24 +2,21 @@ import time
 from config import *
 from tools import *
 
+sector_size = 512
+
 wb.open()
 regs = wb.regs
 ###
-i = 0
-data_errors = 0
-ctrl_errors = 0
+regs.bist_start.write(1)
+last_sector = 0
 while True:
-	regs.bist_sector.write(i)
-	regs.bist_count.write(4)
-	regs.bist_start.write(1)
-	while (regs.bist_done.read() != 1):
-		time.sleep(0.01)
-	data_errors += regs.bist_data_errors.read()
-	ctrl_errors += regs.bist_ctrl_errors.read()
-	if i%10 == 0:
-		print("sector %08d / data_errors %0d / ctrl_errors %d " %(i, data_errors, ctrl_errors))
-		data_errors = 0
-		ctrl_errors = 0
-	i += 1
+	time.sleep(1)
+	sector = regs.bist_sector.read()
+	n_sectors = sector - last_sector
+	last_sector = sector
+	n_bytes = n_sectors*sector_size*4*2
+	ctrl_errors = regs.bist_ctrl_errors.read()
+	data_errors = regs.bist_data_errors.read()
+	print("%04d MB/s / data_errors %08d / ctrl_errors %08d " %(n_bytes/(1024*1024), data_errors, ctrl_errors))
 ###
 wb.close()