1 files changed, 10 insertions, 7 deletions

diff --git a/src/drive.c b/src/drive.c
index 9193608..8c06e69 100644
--- a/src/drive.c
+++ b/src/drive.c
@@ -1,7 +1,8 @@
-#include "drive.h"
-
+#include <hardware/flash.h>
 #include <stdlib.h>
 
+#include "drive.h"
+
 const uint8_t SECTORS_PER_TRACK[35] = {
 	21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21,
 	19, 19, 19, 19, 19, 19, 19,
@@ -46,7 +47,7 @@ void drive_init(drive_t *drive) {
 	drive_cycle(drive);
 }
 
-void drive_step_up(drive_t *drive) {
+void __not_in_flash_func(drive_step_up)(drive_t *drive) {
 	drive->halftrack = (drive->halftrack + 1) & 1;
 	if (drive->track < 34 && drive->halftrack == 0) {
 		drive->image_ptr += SECTORS_PER_TRACK[drive->track] << 8;
@@ -55,7 +56,7 @@ void drive_step_up(drive_t *drive) {
 	}
 }
 
-void drive_step_down(drive_t *drive) {
+void __not_in_flash_func(drive_step_down)(drive_t *drive) {
 	drive->halftrack = (drive->halftrack + 1) & 1;
 	if (drive->track > 0 && drive->halftrack == 0) {
 		drive->image_ptr -= SECTORS_PER_TRACK[drive->track - 1] << 8;
@@ -64,7 +65,7 @@ void drive_step_down(drive_t *drive) {
 	}
 }
 
-bool drive_advance(drive_t *drive, uint8_t cycles) {
+bool __not_in_flash_func(drive_advance)(drive_t *drive, uint8_t cycles) {
 	bool byte_ready = false;
 
 	for (uint8_t i = 0; i < cycles; i++) {
@@ -76,7 +77,7 @@ bool drive_advance(drive_t *drive, uint8_t cycles) {
 	return byte_ready;
 }
 
-void drive_cycle(drive_t *drive) {
+void __not_in_flash_func(drive_cycle)(drive_t *drive) {
 	while (drive->lookahead_idx != ((drive->current_idx - 1) & (DRIVE_LOOKAHEAD - 1))) {
 		uint8_t j = (drive->lookahead_idx + 1) & (DRIVE_LOOKAHEAD - 1);
 		uint16_t timer_next = drive->timer + BITRATES[drive->bitrate];
@@ -170,7 +171,9 @@ void drive_cycle(drive_t *drive) {
 
 			drive->unlatched[j] = ((prev << 1) | ((drive->gcr_word & 0x200) >> 9)) & 0x3ff;
 			drive->gcr_word <<= 1;
-			drive->gcr_bits--;
+			if (drive->gcr_bits > 0) {
+				drive->gcr_bits--;
+			}
 			drive->bit_counter += 1;
 		} else {
 			drive->unlatched[j] = prev;