Refactoring

9 files changed, 624 insertions, 589 deletions

diff --git a/STM32/Core/Inc/dumper.h b/STM32/Core/Inc/dumper.h
index f604cda..28fe384 100644
--- a/STM32/Core/Inc/dumper.h
+++ b/STM32/Core/Inc/dumper.h
@@ -42,20 +42,14 @@
 #define CHR(address) (*(volatile uint8_t*) ((address) + 0x64000000))

 #define IRQ_FIRED (!HAL_GPIO_ReadPin(IRQ_GPIO_Port, IRQ_Pin))

 

+void delay_clock(uint32_t cycles);

 void reset(void);

-void set_flash_buffer_size(uint16_t value);

 void read_prg_send(uint16_t address, uint16_t len);

 void read_prg_crc_send(uint16_t address, uint16_t len);

 void write_prg(uint16_t address, uint16_t len, uint8_t *data);

 void read_chr_send(uint16_t address, uint16_t len);

 void read_chr_crc_send(uint16_t address, uint16_t len);

 void write_chr(uint16_t address, uint16_t len, uint8_t *data);

-void erase_flash_sector();

-void write_flash(uint16_t address, uint16_t len, uint8_t *data);

-void erase_unrom512();

-void write_unrom512(uint32_t address, uint16_t len, uint8_t *data);

-void fds_transfer(uint8_t block_read_start, uint8_t block_read_count, uint8_t block_write_count, uint8_t *block_write_ids, uint16_t *write_lengths, uint8_t *write_data);

 void get_mirroring();

-void set_coolboy_gpio_mode(uint8_t coolboy_gpio_mode);

 

 #endif

diff --git a/STM32/Core/Inc/fds.h b/STM32/Core/Inc/fds.h
new file mode 100644
index 0000000..43b61d7
--- /dev/null
+++ b/STM32/Core/Inc/fds.h
@@ -0,0 +1,6 @@
+#ifndef INC_FDS_H_
+#define INC_FDS_H_
+
+void fds_transfer(uint8_t block_read_start, uint8_t block_read_count, uint8_t block_write_count, uint8_t *block_write_ids, uint16_t *write_lengths, uint8_t *write_data);
+
+#endif /* INC_FDS_H_ */
diff --git a/STM32/Core/Inc/flash.h b/STM32/Core/Inc/flash.h
new file mode 100644
index 0000000..fb58a51
--- /dev/null
+++ b/STM32/Core/Inc/flash.h
@@ -0,0 +1,9 @@
+#ifndef INC_FLASH_H_
+#define INC_FLASH_H_
+
+void set_flash_buffer_size(uint16_t value);
+void erase_flash_sector();
+void write_flash(uint16_t address, uint16_t len, uint8_t *data);
+void set_coolboy_gpio_mode(uint8_t coolboy_gpio_mode);
+
+#endif /* INC_FLASH_H_ */
diff --git a/STM32/Core/Inc/unrom512.h b/STM32/Core/Inc/unrom512.h
new file mode 100644
index 0000000..7ebbe05
--- /dev/null
+++ b/STM32/Core/Inc/unrom512.h
@@ -0,0 +1,7 @@
+#ifndef INC_UNROM512_H_
+#define INC_UNROM512_H_
+
+void erase_unrom512();
+void write_unrom512(uint32_t address, uint16_t len, uint8_t *data);
+
+#endif /* INC_UNROM512_H_ */
diff --git a/STM32/Core/Src/dumper.c b/STM32/Core/Src/dumper.c
index 276df9f..c4f0c4e 100644
--- a/STM32/Core/Src/dumper.c
+++ b/STM32/Core/Src/dumper.c
@@ -5,10 +5,9 @@
 #include "crc.h"

 #include "led.h"

 

-volatile uint8_t dummy;

-uint16_t flash_buffer_mask = 0xFFC0;

+static volatile uint8_t dummy;

 

-static void delay_clock(uint32_t cycles)

+void delay_clock(uint32_t cycles)

 {

   if (cycles < 30000)

   {

@@ -36,19 +35,6 @@ void reset(void)
   HAL_GPIO_WritePin(SHIFTERS_OE_GPIO_Port, SHIFTERS_OE_Pin, GPIO_PIN_RESET);

 }

 

-void set_flash_buffer_size(uint16_t value)

-{

-  // Set maximum number of bytes in multi-byte flash program

-  // 0 = disable multi-byte program

-  uint8_t bit_value = 0;

-  while (value > 1)

-  {

-    value >>= 1;

-    bit_value++;

-  }

-  flash_buffer_mask = 0xFFFF << bit_value;

-}

-

 void read_prg_send(uint16_t address, uint16_t length)

 {

   led_green();

@@ -117,564 +103,6 @@ void write_chr(uint16_t address, uint16_t length, uint8_t *data)
   }

 }

 

-void erase_flash_sector()

-{

-  led_yellow();

-  PRG(0x8000 | 0x0000) = 0xF0;

-  PRG(0x8000 | 0x0AAA) = 0xAA;

-  PRG(0x8000 | 0x0555) = 0x55;

-  PRG(0x8000 | 0x0AAA) = 0x80;

-  PRG(0x8000 | 0x0AAA) = 0xAA;

-  PRG(0x8000 | 0x0555) = 0x55;

-  PRG(0x8000 | 0x0000) = 0x30;

-

-  uint32_t start_time = HAL_GetTick();

-  // waiting for result

-  uint8_t ff_count = 0;

-  while (1)

-  {

-    if (HAL_GetTick() >= start_time + 5000) // 5 seconds timeout

-    {

-      // timeout

-      comm_start(COMMAND_FLASH_ERASE_TIMEOUT, 0);

-      break;

-    }

-    if (PRG(0x8000) != 0xFF)

-      ff_count = 0;

-    else

-      ff_count++;

-    if (ff_count >= 2)

-    {

-      // OK

-      comm_start(COMMAND_PRG_WRITE_DONE, 0);

-      break;

-    }

-  }

-}

-

-void write_flash(uint16_t address, uint16_t len, uint8_t *data)

-{

-  led_red();

-  while (len > 0)

-  {

-    uint16_t count = 0;

-    uint8_t *d = data;

-    uint16_t a = address;

-    uint16_t last_address = 0;

-    uint8_t last_data = 0;

-    if (flash_buffer_mask != 0xFFFF)

-    {

-      // multi-byte program

-      uint16_t address_base = a & flash_buffer_mask;

-      while ((len > 0) && ((a & flash_buffer_mask) == address_base))

-      {

-        if (*d != 0xFF)

-          count++;

-        a++;

-        len--;

-        d++;

-      }

-    } else {

-      // single-byte program

-      if (*d != 0xFF)

-        count = 1;

-      a++;

-      len--;

-      d++;

-    }

-

-    if (count)

-    {

-      if (count > 0)

-      {

-        // multi-byte

-        PRG(0x8000 | 0x0000) = 0xF0;

-        PRG(0x8000 | 0x0AAA) = 0xAA;

-        PRG(0x8000 | 0x0555) = 0x55;

-        PRG(0x8000 | 0x0000) = 0x25;

-        PRG(0x8000 | 0x0000) = count - 1;

-        while (count > 0)

-        {

-          if (*data != 0xFF)

-          {

-            PRG(0x8000 | address) = *data;

-            last_address = address;

-            last_data = *data;

-            count--;

-          }

-          address++;

-          data++;

-        }

-        PRG(0x8000 + 0x0000) = 0x29;

-      } else {

-        // single-byte

-        PRG(0x8000 | 0x0000) = 0xF0;

-        PRG(0x8000 | 0x0AAA) = 0xAA;

-        PRG(0x8000 | 0x0555) = 0x55;

-        PRG(0x8000 | 0x0AAA) = 0xA0;

-        PRG(0x8000 | address) = *data;

-        last_address = address;

-        last_data = *data;

-        count--;

-        address++;

-        data++;

-      }

-

-      uint32_t start_time = HAL_GetTick();

-      // waiting for result

-      while (1)

-      {

-        if (HAL_GetTick() >= start_time + 50) // 50 ms timeout

-        {

-          // timeout

-          comm_start(COMMAND_FLASH_WRITE_TIMEOUT, 0);

-          return;

-        }

-        uint8_t read_1 = PRG(0x8000 | last_address);

-        uint8_t read_2 = PRG(0x8000 | last_address);

-        uint8_t read_3 = PRG(0x8000 | last_address);

-        if (((read_1 ^ read_2) & (1 << 6)) && ((read_2 ^ read_3) & (1 << 6)))

-        {

-          if (read_1 & (1 << 1))

-          {

-            comm_start(COMMAND_FLASH_WRITE_ERROR, 3);

-            comm_send_byte(read_1);

-            comm_send_byte(read_2);

-            comm_send_byte(read_3);

-            return;

-          } else if (read_1 & (1 << 5))

-          {

-            comm_start(COMMAND_FLASH_WRITE_TIMEOUT, 3);

-            comm_send_byte(read_1);

-            comm_send_byte(read_2);

-            comm_send_byte(read_3);

-            return;

-          }

-        } else

-        {

-          read_1 = PRG(0x8000 | last_address);

-          read_2 = PRG(0x8000 | last_address);

-          if ((read_1 == read_2) && (read_2 == last_data)) break; // OK

-        }

-      }

-    }

-

-    address = a;

-    data = d;

-  }

-  comm_start(COMMAND_PRG_WRITE_DONE, 0);

-}

-

-void unrom512_cmd_write(uint32_t address, uint8_t data)

-{

-  PRG(0xC000) = address >> 14;

-  PRG(0x8000 | (address & 0x3FFF)) = data;

-}

-

-uint8_t unrom512_cmd_read(uint32_t address)

-{

-  PRG(0xC000) = address >> 14;

-  return PRG(0x8000 | (address & 0x3FFF));

-}

-

-void erase_unrom512()

-{

-  led_yellow();

-  unrom512_cmd_write(0x0000, 0xF0);

-  unrom512_cmd_write(0x5555, 0xAA);

-  unrom512_cmd_write(0x2AAA, 0x55);

-  unrom512_cmd_write(0x5555, 0x80);

-  unrom512_cmd_write(0x5555, 0xAA);

-  unrom512_cmd_write(0x2AAA, 0x55);

-  unrom512_cmd_write(0x5555, 0x10);

-

-  uint32_t start_time = HAL_GetTick();

-  // waiting for result

-  uint8_t ff_count = 0;

-  while (1)

-  {

-    if (HAL_GetTick() >= start_time + 5000) // 5 seconds timeout

-    {

-      // timeout

-      comm_start(COMMAND_FLASH_ERASE_TIMEOUT, 0);

-      break;

-    }

-    if (PRG(0x8000) != 0xFF)

-      ff_count = 0;

-    else

-      ff_count++;

-    if (ff_count >= 2)

-    {

-      // OK

-      comm_start(COMMAND_PRG_WRITE_DONE, 0);

-      break;

-    }

-  }

-}

-

-void write_unrom512(uint32_t address, uint16_t len, uint8_t *data)

-{

-  led_red();

-  PRG(0x8000 | 0x0000) = 0xF0;

-  while (len > 0)

-  {

-    unrom512_cmd_write(0x5555, 0xAA);

-    unrom512_cmd_write(0x2AAA, 0x55);

-    unrom512_cmd_write(0x5555, 0xA0);

-    unrom512_cmd_write(address, *data);

-

-    uint32_t start_time = HAL_GetTick();

-    // waiting for result

-    while (1)

-    {

-      if (HAL_GetTick() >= start_time + 50) // 50 ms timeout

-      {

-        // timeout

-        comm_start(COMMAND_FLASH_WRITE_TIMEOUT, 0);

-        return;

-      }

-      uint8_t read_1 = unrom512_cmd_read(address);

-      uint8_t read_2 = unrom512_cmd_read(address);

-      if ((read_1 == read_2) && (read_2 == *data)) break; // OK

-    }

-    address++;

-    data++;

-    len--;

-  }

-  comm_start(COMMAND_PRG_WRITE_DONE, 0);

-}

-

-static uint8_t transfer_fds_byte(uint8_t *output, uint8_t input, uint8_t *end_of_head)

-{

-  uint32_t start_time;

-  start_time = HAL_GetTick();

-  while (!IRQ_FIRED)

-  {

-    // waiting for interrupt

-    // timeout 5 secs

-    if (HAL_GetTick() - start_time >= 5000)

-    {

-      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-      comm_start(COMMAND_FDS_TIMEOUT, 0);

-      return 0;

-    }

-  }

-  if (output)

-    *output = PRG(FDS_DATA_READ);

-  else

-    dummy = PRG(FDS_DATA_READ);

-  PRG(FDS_DATA_WRITE) = input; // clear interrupt

-  uint8_t disk_status = PRG(FDS_DISK_STATUS);

-  if (end_of_head)

-    *end_of_head |= (disk_status >> 6) & 1;

-  start_time = HAL_GetTick();

-  while (IRQ_FIRED)

-  {

-    // is interrupt flag cleared?

-    // timeout 5 secs

-    if (HAL_GetTick() - start_time >= 5000)

-    {

-      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-      comm_start(COMMAND_FDS_TIMEOUT, 0);

-      return 0;

-    }

-  }

-  return 1;

-}

-

-static uint8_t read_fds_block_send(uint16_t length, uint8_t send, uint16_t *file_size, uint32_t gap_delay)

-{

-  uint8_t data;

-  uint8_t disk_status;

-  uint32_t b;

-  uint8_t crc_ok = 1;

-  uint8_t end_of_head = 0;

-

-  delay_clock(gap_delay);

-  if (send)

-  {

-    led_green();

-    comm_start(COMMAND_FDS_READ_RESULT_BLOCK, length + 2);

-  }

-  // start transfer, enable IRQ

-  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;

-  for (b = 0; b < length; b++)

-  {

-    if (!transfer_fds_byte(&data, 0, &end_of_head))

-      return 0;

-    // parse file size if need

-    if (file_size)

-    {

-      if (b == 13)

-        *file_size = data;

-      else if (b == 14)

-        *file_size |= data << 8;

-    }

-    if (send)

-    {

-      if (!comm_send_byte(data))

-        return 0;

-    }

-  }

-  if (!transfer_fds_byte(0, 0, &end_of_head))

-    return 0;

-  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON | FDS_CONTROL_CRC; // enable CRC control

-  if (!transfer_fds_byte(0, 0, &end_of_head))

-    return 0;

-  disk_status = PRG(FDS_DISK_STATUS);

-  crc_ok &= ((disk_status >> 4) & 1) ^ 1;

-  end_of_head |= (disk_status >> 6) & 1;

-  if (send)

-  {

-    if (!comm_send_byte(crc_ok)) // CRC check result

-      return 0;

-    if (!comm_send_byte(end_of_head)) // end of head meet?

-      return 0;

-  }

-  if (!crc_ok || end_of_head)

-  {

-    // invalid data or end of disk, abort transfer

-    PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-    HAL_Delay(50);

-    if (send)

-      comm_start(COMMAND_FDS_READ_RESULT_END, 0);

-    else {

-      if (!crc_ok)

-        comm_start(COMMAND_FDS_BLOCK_CRC_ERROR, 0);

-      else

-        comm_start(COMMAND_FDS_END_OF_HEAD, 0);

-    }

-    return 0;

-  }

-  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON; // motor on without transfer

-

-  led_cyan();

-  return 1; // success

-}

-

-static uint8_t write_fds_block(uint8_t *data, uint16_t length, uint32_t gap_delay)

-{

-  uint8_t end_of_head = 0;

-  uint32_t start_time;

-  uint16_t pos = 0;

-  led_red();

-  PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON; // enable writing without transfer

-  delay_clock(gap_delay);

-  PRG(FDS_DATA_WRITE) = 0x00; // write $00

-  // start transfer, enable IRQ

-  PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;

-  transfer_fds_byte(0, 0x80, &end_of_head); // write $80

-  while (length)

-  {

-    if (end_of_head)

-    {

-      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-      comm_start(COMMAND_FDS_END_OF_HEAD, 0);

-      return 0;

-    }

-    if (!transfer_fds_byte(0, *data, &end_of_head))

-      return 0;

-    data++;

-    length--;

-    pos++;

-    // avoid copy protection, lol

-    if ((pos % FDS_COPY_PROTECTION_RESET_INTERVAL) == 0)

-    {

-      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;

-      PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;

-    }

-  }

-  if (!transfer_fds_byte(0, 0xFF, &end_of_head))

-    return 0;

-  if (end_of_head)

-  {

-    PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-    comm_start(COMMAND_FDS_END_OF_HEAD, 0);

-    return 0;

-  }

-  PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON | FDS_CONTROL_CRC; // enable CRC control

-  delay_clock(FDS_WRITE_CRC_DELAY);

-  start_time = HAL_GetTick();

-  while (1)

-  {

-    uint8_t drive_status = PRG(FDS_DRIVE_STATUS);

-    if (!(drive_status & FDS_DRIVE_STATUS_DISK_NOT_READY))

-      break; // ready

-    // timeout 1 sec

-    if (HAL_GetTick() - start_time >= 1000)

-    {

-      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-      comm_start(COMMAND_FDS_TIMEOUT, 0);

-      return 0;

-    }

-  }

-  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON; // motor on without transfer

-

-  led_cyan();

-  return 1; // success

-}

-

-uint8_t fds_transfer_real(uint8_t block_read_start, uint8_t block_read_count, uint8_t block_write_count, uint8_t *block_write_ids, uint16_t *write_lengths, uint8_t *write_data)

-{

-  uint8_t current_block = 0;

-  uint32_t start_time;

-  uint8_t drive_status;

-  uint8_t blocks_writed = 0;

-

-  led_magenta();

-  PRG(FDS_IRQ_CONTROL) = 0x00; // disable timer IRQ

-  PRG(FDS_MASTER_IO) = 0x01; // enable disk registers

-  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset

-  drive_status = PRG(FDS_DRIVE_STATUS);

-  // is disk inserted?

-  if (drive_status & FDS_DRIVE_STATUS_DISK_NOT_INSERTED)

-  {

-    comm_start(COMMAND_FDS_DISK_NOT_INSERTED, 0);

-    return 0;

-  }

-  // is it write protected while we are writing?

-  if (block_write_count && (drive_status & FDS_DRIVE_STATUS_DISK_WRITE_PROTECTED))

-  {

-    comm_start(COMMAND_FDS_DISK_WRITE_PROTECTED, 0);

-    return 0;

-  }

-  // battery test

-  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON; // monor on, unreset

-  PRG(FDS_EXT_WRITE) = 0xFF;

-  HAL_Delay(100);

-  if (!(PRG(FDS_EXT_READ) & 0x80))

-  {

-    // battery low

-    PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-    comm_start(COMMAND_FDS_BATTERY_LOW, 0);

-    return 0;

-  }

-  // waiting until drive is rewinded

-  start_time = HAL_GetTick();

-  do

-  {

-    // timeout 15 secs

-    if (HAL_GetTick() - start_time >= 15000)

-    {

-      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-      comm_start(COMMAND_FDS_TIMEOUT, 0);

-      return 0;

-    }

-  } while (!(PRG(FDS_DRIVE_STATUS) & FDS_DRIVE_STATUS_DISK_NOT_READY));

-  start_time = HAL_GetTick();

-  do

-  {

-    // timeout 15 secs

-    if (HAL_GetTick() - start_time >= 15000)

-    {

-      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-      comm_start(COMMAND_FDS_TIMEOUT, 0);

-      return 0;

-    }

-  } while (PRG(FDS_DRIVE_STATUS) & FDS_DRIVE_STATUS_DISK_NOT_READY);

-

-  led_cyan();

-

-  // disk info block

-  if (block_write_count && (current_block == block_write_ids[blocks_writed]))

-  {

-    // gap delay while writing = ~28300 bits = (~28300 / 8)bits * ~165cycles = ~583687.5

-    uint16_t write_length = write_lengths[blocks_writed];

-    if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BEFORE_FIRST_BLOCK))

-      return 0;

-    write_data += write_length;

-    blocks_writed++;

-    block_write_count--;

-  } else

-  {

-    // gap delay while reading = ~486974 cycles

-    if (!read_fds_block_send(56, (current_block >= block_read_start) && block_read_count, 0, FDS_READ_GAP_BEFORE_FIRST_BLOCK))

-      return 0;

-  }

-  if ((current_block >= block_read_start) && block_read_count)

-    block_read_count--;

-  current_block++;

-

-  if (block_read_count || block_write_count)

-  {

-    // file amount block

-    if (block_write_count && (current_block == block_write_ids[blocks_writed]))

-    {

-      uint16_t write_length = write_lengths[blocks_writed];

-      if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BETWEEN_BLOCKS))

-        return 0;

-      write_data += write_length;

-      blocks_writed++;

-      block_write_count--;

-    } else

-    {

-      if (!read_fds_block_send(2, (current_block >= block_read_start) && block_read_count, 0, FDS_READ_GAP_BETWEEN_BLOCKS))

-        return 0;

-    }

-    if ((current_block >= block_read_start) && block_read_count)

-      block_read_count--;

-    current_block++;

-  }

-

-  while (block_read_count || block_write_count)

-  {

-    // file header block

-    uint16_t file_size = 0; // size of the next file

-    if (block_write_count && (current_block == block_write_ids[blocks_writed]))

-    {

-      uint16_t write_length = write_lengths[blocks_writed];

-      if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BETWEEN_BLOCKS))

-        return 0;

-      write_data += write_length;

-      blocks_writed++;

-      block_write_count--;

-    } else

-    {

-      if (!read_fds_block_send(16, (current_block >= block_read_start) && block_read_count, &file_size, FDS_READ_GAP_BETWEEN_BLOCKS))

-        return 0;

-    }

-    if ((current_block >= block_read_start) && block_read_count)

-      block_read_count--;

-    current_block++;

-

-    if (block_read_count || block_write_count)

-    {

-      // file data block

-      if (block_write_count && (current_block == block_write_ids[blocks_writed]))

-      {

-        uint16_t write_length = write_lengths[blocks_writed];

-        if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BETWEEN_BLOCKS))

-          return 0;

-        write_data += write_length;

-        blocks_writed++;

-        block_write_count--;

-      } else

-      {

-        if (!read_fds_block_send(file_size + 1, (current_block >= block_read_start) && block_read_count, 0, FDS_READ_GAP_BETWEEN_BLOCKS))

-          return 0;

-      }

-      if ((current_block >= block_read_start) && block_read_count)

-        block_read_count--;

-      current_block++;

-    }

-  }

-

-  return 1;

-}

-

-void fds_transfer(uint8_t block_read_start, uint8_t block_read_count, uint8_t block_write_count, uint8_t *block_write_ids, uint16_t *write_lengths, uint8_t *write_data)

-{

-  uint8_t ok = fds_transfer_real(block_read_start, block_read_count, block_write_count, block_write_ids, write_lengths, write_data);

-  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop

-  if (ok)

-  {

-    HAL_Delay(50);

-    if (block_write_count && !block_read_count)

-      comm_start(COMMAND_FDS_WRITE_DONE, 0);

-    else

-      comm_start(COMMAND_FDS_READ_RESULT_END, 0);

-  }

-}

 

 void get_mirroring()

 {

@@ -689,11 +117,3 @@ void get_mirroring()
   dummy = CHR((1 << 10) | (1 << 11));

   comm_send_byte(HAL_GPIO_ReadPin(CIRAM_A10_GPIO_Port, CIRAM_A10_Pin));

 }

-

-void set_coolboy_gpio_mode(uint8_t coolboy_gpio_mode)

-{

-	if (coolboy_gpio_mode)

-		HAL_GPIO_WritePin(COOLBOY_MODE_GPIO_Port, COOLBOY_MODE_Pin, GPIO_PIN_SET);

-	else

-		HAL_GPIO_WritePin(COOLBOY_MODE_GPIO_Port, COOLBOY_MODE_Pin, GPIO_PIN_RESET);

-}

diff --git a/STM32/Core/Src/fds.c b/STM32/Core/Src/fds.c
new file mode 100644
index 0000000..556d1ac
--- /dev/null
+++ b/STM32/Core/Src/fds.c
@@ -0,0 +1,338 @@
+#include "main.h"
+#include "dumper.h"
+#include "comm.h"
+#include "led.h"
+
+static volatile uint8_t dummy;
+
+static uint8_t transfer_fds_byte(uint8_t *output, uint8_t input, uint8_t *end_of_head)
+{
+  uint32_t start_time;
+  start_time = HAL_GetTick();
+  while (!IRQ_FIRED)
+  {
+    // waiting for interrupt
+    // timeout 5 secs
+    if (HAL_GetTick() - start_time >= 5000)
+    {
+      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+      comm_start(COMMAND_FDS_TIMEOUT, 0);
+      return 0;
+    }
+  }
+  if (output)
+    *output = PRG(FDS_DATA_READ);
+  else
+    dummy = PRG(FDS_DATA_READ);
+  PRG(FDS_DATA_WRITE) = input; // clear interrupt
+  uint8_t disk_status = PRG(FDS_DISK_STATUS);
+  if (end_of_head)
+    *end_of_head |= (disk_status >> 6) & 1;
+  start_time = HAL_GetTick();
+  while (IRQ_FIRED)
+  {
+    // is interrupt flag cleared?
+    // timeout 5 secs
+    if (HAL_GetTick() - start_time >= 5000)
+    {
+      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+      comm_start(COMMAND_FDS_TIMEOUT, 0);
+      return 0;
+    }
+  }
+  return 1;
+}
+
+static uint8_t read_fds_block_send(uint16_t length, uint8_t send, uint16_t *file_size, uint32_t gap_delay)
+{
+  uint8_t data;
+  uint8_t disk_status;
+  uint32_t b;
+  uint8_t crc_ok = 1;
+  uint8_t end_of_head = 0;
+
+  delay_clock(gap_delay);
+  if (send)
+  {
+    led_green();
+    comm_start(COMMAND_FDS_READ_RESULT_BLOCK, length + 2);
+  }
+  // start transfer, enable IRQ
+  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;
+  for (b = 0; b < length; b++)
+  {
+    if (!transfer_fds_byte(&data, 0, &end_of_head))
+      return 0;
+    // parse file size if need
+    if (file_size)
+    {
+      if (b == 13)
+        *file_size = data;
+      else if (b == 14)
+        *file_size |= data << 8;
+    }
+    if (send)
+    {
+      if (!comm_send_byte(data))
+        return 0;
+    }
+  }
+  if (!transfer_fds_byte(0, 0, &end_of_head))
+    return 0;
+  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON | FDS_CONTROL_CRC; // enable CRC control
+  if (!transfer_fds_byte(0, 0, &end_of_head))
+    return 0;
+  disk_status = PRG(FDS_DISK_STATUS);
+  crc_ok &= ((disk_status >> 4) & 1) ^ 1;
+  end_of_head |= (disk_status >> 6) & 1;
+  if (send)
+  {
+    if (!comm_send_byte(crc_ok)) // CRC check result
+      return 0;
+    if (!comm_send_byte(end_of_head)) // end of head meet?
+      return 0;
+  }
+  if (!crc_ok || end_of_head)
+  {
+    // invalid data or end of disk, abort transfer
+    PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+    HAL_Delay(50);
+    if (send)
+      comm_start(COMMAND_FDS_READ_RESULT_END, 0);
+    else {
+      if (!crc_ok)
+        comm_start(COMMAND_FDS_BLOCK_CRC_ERROR, 0);
+      else
+        comm_start(COMMAND_FDS_END_OF_HEAD, 0);
+    }
+    return 0;
+  }
+  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON; // motor on without transfer
+
+  led_cyan();
+  return 1; // success
+}
+
+static uint8_t write_fds_block(uint8_t *data, uint16_t length, uint32_t gap_delay)
+{
+  uint8_t end_of_head = 0;
+  uint32_t start_time;
+  uint16_t pos = 0;
+  led_red();
+  PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON; // enable writing without transfer
+  delay_clock(gap_delay);
+  PRG(FDS_DATA_WRITE) = 0x00; // write $00
+  // start transfer, enable IRQ
+  PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;
+  transfer_fds_byte(0, 0x80, &end_of_head); // write $80
+  while (length)
+  {
+    if (end_of_head)
+    {
+      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+      comm_start(COMMAND_FDS_END_OF_HEAD, 0);
+      return 0;
+    }
+    if (!transfer_fds_byte(0, *data, &end_of_head))
+      return 0;
+    data++;
+    length--;
+    pos++;
+    // avoid copy protection, lol
+    if ((pos % FDS_COPY_PROTECTION_RESET_INTERVAL) == 0)
+    {
+      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;
+      PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON;
+    }
+  }
+  if (!transfer_fds_byte(0, 0xFF, &end_of_head))
+    return 0;
+  if (end_of_head)
+  {
+    PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+    comm_start(COMMAND_FDS_END_OF_HEAD, 0);
+    return 0;
+  }
+  PRG(FDS_CONTROL) = FDS_CONTROL_WRITE | FDS_CONTROL_MOTOR_ON | FDS_CONTROL_TRANSFER_ON | FDS_CONTROL_IRQ_ON | FDS_CONTROL_CRC; // enable CRC control
+  delay_clock(FDS_WRITE_CRC_DELAY);
+  start_time = HAL_GetTick();
+  while (1)
+  {
+    uint8_t drive_status = PRG(FDS_DRIVE_STATUS);
+    if (!(drive_status & FDS_DRIVE_STATUS_DISK_NOT_READY))
+      break; // ready
+    // timeout 1 sec
+    if (HAL_GetTick() - start_time >= 1000)
+    {
+      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+      comm_start(COMMAND_FDS_TIMEOUT, 0);
+      return 0;
+    }
+  }
+  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON; // motor on without transfer
+
+  led_cyan();
+  return 1; // success
+}
+
+static uint8_t fds_transfer_real(uint8_t block_read_start, uint8_t block_read_count, uint8_t block_write_count, uint8_t *block_write_ids, uint16_t *write_lengths, uint8_t *write_data)
+{
+  uint8_t current_block = 0;
+  uint32_t start_time;
+  uint8_t drive_status;
+  uint8_t blocks_writed = 0;
+
+  led_magenta();
+  PRG(FDS_IRQ_CONTROL) = 0x00; // disable timer IRQ
+  PRG(FDS_MASTER_IO) = 0x01; // enable disk registers
+  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset
+  drive_status = PRG(FDS_DRIVE_STATUS);
+  // is disk inserted?
+  if (drive_status & FDS_DRIVE_STATUS_DISK_NOT_INSERTED)
+  {
+    comm_start(COMMAND_FDS_DISK_NOT_INSERTED, 0);
+    return 0;
+  }
+  // is it write protected while we are writing?
+  if (block_write_count && (drive_status & FDS_DRIVE_STATUS_DISK_WRITE_PROTECTED))
+  {
+    comm_start(COMMAND_FDS_DISK_WRITE_PROTECTED, 0);
+    return 0;
+  }
+  // battery test
+  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_ON; // monor on, unreset
+  PRG(FDS_EXT_WRITE) = 0xFF;
+  HAL_Delay(100);
+  if (!(PRG(FDS_EXT_READ) & 0x80))
+  {
+    // battery low
+    PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+    comm_start(COMMAND_FDS_BATTERY_LOW, 0);
+    return 0;
+  }
+  // waiting until drive is rewinded
+  start_time = HAL_GetTick();
+  do
+  {
+    // timeout 15 secs
+    if (HAL_GetTick() - start_time >= 15000)
+    {
+      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+      comm_start(COMMAND_FDS_TIMEOUT, 0);
+      return 0;
+    }
+  } while (!(PRG(FDS_DRIVE_STATUS) & FDS_DRIVE_STATUS_DISK_NOT_READY));
+  start_time = HAL_GetTick();
+  do
+  {
+    // timeout 15 secs
+    if (HAL_GetTick() - start_time >= 15000)
+    {
+      PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+      comm_start(COMMAND_FDS_TIMEOUT, 0);
+      return 0;
+    }
+  } while (PRG(FDS_DRIVE_STATUS) & FDS_DRIVE_STATUS_DISK_NOT_READY);
+
+  led_cyan();
+
+  // disk info block
+  if (block_write_count && (current_block == block_write_ids[blocks_writed]))
+  {
+    // gap delay while writing = ~28300 bits = (~28300 / 8)bits * ~165cycles = ~583687.5
+    uint16_t write_length = write_lengths[blocks_writed];
+    if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BEFORE_FIRST_BLOCK))
+      return 0;
+    write_data += write_length;
+    blocks_writed++;
+    block_write_count--;
+  } else
+  {
+    // gap delay while reading = ~486974 cycles
+    if (!read_fds_block_send(56, (current_block >= block_read_start) && block_read_count, 0, FDS_READ_GAP_BEFORE_FIRST_BLOCK))
+      return 0;
+  }
+  if ((current_block >= block_read_start) && block_read_count)
+    block_read_count--;
+  current_block++;
+
+  if (block_read_count || block_write_count)
+  {
+    // file amount block
+    if (block_write_count && (current_block == block_write_ids[blocks_writed]))
+    {
+      uint16_t write_length = write_lengths[blocks_writed];
+      if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BETWEEN_BLOCKS))
+        return 0;
+      write_data += write_length;
+      blocks_writed++;
+      block_write_count--;
+    } else
+    {
+      if (!read_fds_block_send(2, (current_block >= block_read_start) && block_read_count, 0, FDS_READ_GAP_BETWEEN_BLOCKS))
+        return 0;
+    }
+    if ((current_block >= block_read_start) && block_read_count)
+      block_read_count--;
+    current_block++;
+  }
+
+  while (block_read_count || block_write_count)
+  {
+    // file header block
+    uint16_t file_size = 0; // size of the next file
+    if (block_write_count && (current_block == block_write_ids[blocks_writed]))
+    {
+      uint16_t write_length = write_lengths[blocks_writed];
+      if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BETWEEN_BLOCKS))
+        return 0;
+      write_data += write_length;
+      blocks_writed++;
+      block_write_count--;
+    } else
+    {
+      if (!read_fds_block_send(16, (current_block >= block_read_start) && block_read_count, &file_size, FDS_READ_GAP_BETWEEN_BLOCKS))
+        return 0;
+    }
+    if ((current_block >= block_read_start) && block_read_count)
+      block_read_count--;
+    current_block++;
+
+    if (block_read_count || block_write_count)
+    {
+      // file data block
+      if (block_write_count && (current_block == block_write_ids[blocks_writed]))
+      {
+        uint16_t write_length = write_lengths[blocks_writed];
+        if (!write_fds_block(write_data, write_length, FDS_WRITE_GAP_BETWEEN_BLOCKS))
+          return 0;
+        write_data += write_length;
+        blocks_writed++;
+        block_write_count--;
+      } else
+      {
+        if (!read_fds_block_send(file_size + 1, (current_block >= block_read_start) && block_read_count, 0, FDS_READ_GAP_BETWEEN_BLOCKS))
+          return 0;
+      }
+      if ((current_block >= block_read_start) && block_read_count)
+        block_read_count--;
+      current_block++;
+    }
+  }
+
+  return 1;
+}
+
+void fds_transfer(uint8_t block_read_start, uint8_t block_read_count, uint8_t block_write_count, uint8_t *block_write_ids, uint16_t *write_lengths, uint8_t *write_data)
+{
+  uint8_t ok = fds_transfer_real(block_read_start, block_read_count, block_write_count, block_write_ids, write_lengths, write_data);
+  PRG(FDS_CONTROL) = FDS_CONTROL_READ | FDS_CONTROL_MOTOR_OFF; // reset, stop
+  if (ok)
+  {
+    HAL_Delay(50);
+    if (block_write_count && !block_read_count)
+      comm_start(COMMAND_FDS_WRITE_DONE, 0);
+    else
+      comm_start(COMMAND_FDS_READ_RESULT_END, 0);
+  }
+}
diff --git a/STM32/Core/Src/flash.c b/STM32/Core/Src/flash.c
new file mode 100644
index 0000000..f0e8805
--- /dev/null
+++ b/STM32/Core/Src/flash.c
@@ -0,0 +1,175 @@
+#include "main.h"
+#include "dumper.h"
+#include "comm.h"
+#include "led.h"
+
+static uint16_t flash_buffer_mask = 0xFFC0;
+
+void set_flash_buffer_size(uint16_t value)
+{
+  // Set maximum number of bytes in multi-byte flash program
+  // 0 = disable multi-byte program
+  uint8_t bit_value = 0;
+  while (value > 1)
+  {
+    value >>= 1;
+    bit_value++;
+  }
+  flash_buffer_mask = 0xFFFF << bit_value;
+}
+
+void erase_flash_sector()
+{
+  led_yellow();
+  PRG(0x8000 | 0x0000) = 0xF0;
+  PRG(0x8000 | 0x0AAA) = 0xAA;
+  PRG(0x8000 | 0x0555) = 0x55;
+  PRG(0x8000 | 0x0AAA) = 0x80;
+  PRG(0x8000 | 0x0AAA) = 0xAA;
+  PRG(0x8000 | 0x0555) = 0x55;
+  PRG(0x8000 | 0x0000) = 0x30;
+
+  uint32_t start_time = HAL_GetTick();
+  // waiting for result
+  uint8_t ff_count = 0;
+  while (1)
+  {
+    if (HAL_GetTick() >= start_time + 5000) // 5 seconds timeout
+    {
+      // timeout
+      comm_start(COMMAND_FLASH_ERASE_TIMEOUT, 0);
+      break;
+    }
+    if (PRG(0x8000) != 0xFF)
+      ff_count = 0;
+    else
+      ff_count++;
+    if (ff_count >= 2)
+    {
+      // OK
+      comm_start(COMMAND_PRG_WRITE_DONE, 0);
+      break;
+    }
+  }
+}
+
+void write_flash(uint16_t address, uint16_t len, uint8_t *data)
+{
+  led_red();
+  while (len > 0)
+  {
+    uint16_t count = 0;
+    uint8_t *d = data;
+    uint16_t a = address;
+    uint16_t last_address = 0;
+    uint8_t last_data = 0;
+    if (flash_buffer_mask != 0xFFFF)
+    {
+      // multi-byte program
+      uint16_t address_base = a & flash_buffer_mask;
+      while ((len > 0) && ((a & flash_buffer_mask) == address_base))
+      {
+        if (*d != 0xFF)
+          count++;
+        a++;
+        len--;
+        d++;
+      }
+    } else {
+      // single-byte program
+      if (*d != 0xFF)
+        count = 1;
+      a++;
+      len--;
+      d++;
+    }
+
+    if (count)
+    {
+      if (count > 0)
+      {
+        // multi-byte
+        PRG(0x8000 | 0x0000) = 0xF0;
+        PRG(0x8000 | 0x0AAA) = 0xAA;
+        PRG(0x8000 | 0x0555) = 0x55;
+        PRG(0x8000 | 0x0000) = 0x25;
+        PRG(0x8000 | 0x0000) = count - 1;
+        while (count > 0)
+        {
+          if (*data != 0xFF)
+          {
+            PRG(0x8000 | address) = *data;
+            last_address = address;
+            last_data = *data;
+            count--;
+          }
+          address++;
+          data++;
+        }
+        PRG(0x8000 + 0x0000) = 0x29;
+      } else {
+        // single-byte
+        PRG(0x8000 | 0x0000) = 0xF0;
+        PRG(0x8000 | 0x0AAA) = 0xAA;
+        PRG(0x8000 | 0x0555) = 0x55;
+        PRG(0x8000 | 0x0AAA) = 0xA0;
+        PRG(0x8000 | address) = *data;
+        last_address = address;
+        last_data = *data;
+        count--;
+        address++;
+        data++;
+      }
+
+      uint32_t start_time = HAL_GetTick();
+      // waiting for result
+      while (1)
+      {
+        if (HAL_GetTick() >= start_time + 50) // 50 ms timeout
+        {
+          // timeout
+          comm_start(COMMAND_FLASH_WRITE_TIMEOUT, 0);
+          return;
+        }
+        uint8_t read_1 = PRG(0x8000 | last_address);
+        uint8_t read_2 = PRG(0x8000 | last_address);
+        uint8_t read_3 = PRG(0x8000 | last_address);
+        if (((read_1 ^ read_2) & (1 << 6)) && ((read_2 ^ read_3) & (1 << 6)))
+        {
+          if (read_1 & (1 << 1))
+          {
+            comm_start(COMMAND_FLASH_WRITE_ERROR, 3);
+            comm_send_byte(read_1);
+            comm_send_byte(read_2);
+            comm_send_byte(read_3);
+            return;
+          } else if (read_1 & (1 << 5))
+          {
+            comm_start(COMMAND_FLASH_WRITE_TIMEOUT, 3);
+            comm_send_byte(read_1);
+            comm_send_byte(read_2);
+            comm_send_byte(read_3);
+            return;
+          }
+        } else
+        {
+          read_1 = PRG(0x8000 | last_address);
+          read_2 = PRG(0x8000 | last_address);
+          if ((read_1 == read_2) && (read_2 == last_data)) break; // OK
+        }
+      }
+    }
+
+    address = a;
+    data = d;
+  }
+  comm_start(COMMAND_PRG_WRITE_DONE, 0);
+}
+
+void set_coolboy_gpio_mode(uint8_t coolboy_gpio_mode)
+{
+  if (coolboy_gpio_mode)
+    HAL_GPIO_WritePin(COOLBOY_MODE_GPIO_Port, COOLBOY_MODE_Pin, GPIO_PIN_SET);
+  else
+    HAL_GPIO_WritePin(COOLBOY_MODE_GPIO_Port, COOLBOY_MODE_Pin, GPIO_PIN_RESET);
+}
diff --git a/STM32/Core/Src/main.c b/STM32/Core/Src/main.c
index 8ca37b4..4036077 100644
--- a/STM32/Core/Src/main.c
+++ b/STM32/Core/Src/main.c
@@ -26,6 +26,9 @@
 #include <stdio.h>

 #include "dumper.h"

 #include "comm.h"

+#include "flash.h"

+#include "unrom512.h"

+#include "fds.h"

 #include "led.h"

 /* USER CODE END Includes */

 

diff --git a/STM32/Core/Src/unrom512.c b/STM32/Core/Src/unrom512.c
new file mode 100644
index 0000000..161ac0e
--- /dev/null
+++ b/STM32/Core/Src/unrom512.c
@@ -0,0 +1,83 @@
+#include "main.h"
+#include "dumper.h"
+#include "comm.h"
+#include "led.h"
+
+static void unrom512_cmd_write(uint32_t address, uint8_t data)
+{
+  PRG(0xC000) = address >> 14;
+  PRG(0x8000 | (address & 0x3FFF)) = data;
+}
+
+static uint8_t unrom512_cmd_read(uint32_t address)
+{
+  PRG(0xC000) = address >> 14;
+  return PRG(0x8000 | (address & 0x3FFF));
+}
+
+void erase_unrom512()
+{
+  led_yellow();
+  unrom512_cmd_write(0x0000, 0xF0);
+  unrom512_cmd_write(0x5555, 0xAA);
+  unrom512_cmd_write(0x2AAA, 0x55);
+  unrom512_cmd_write(0x5555, 0x80);
+  unrom512_cmd_write(0x5555, 0xAA);
+  unrom512_cmd_write(0x2AAA, 0x55);
+  unrom512_cmd_write(0x5555, 0x10);
+
+  uint32_t start_time = HAL_GetTick();
+  // waiting for result
+  uint8_t ff_count = 0;
+  while (1)
+  {
+    if (HAL_GetTick() >= start_time + 5000) // 5 seconds timeout
+    {
+      // timeout
+      comm_start(COMMAND_FLASH_ERASE_TIMEOUT, 0);
+      break;
+    }
+    if (PRG(0x8000) != 0xFF)
+      ff_count = 0;
+    else
+      ff_count++;
+    if (ff_count >= 2)
+    {
+      // OK
+      comm_start(COMMAND_PRG_WRITE_DONE, 0);
+      break;
+    }
+  }
+}
+
+void write_unrom512(uint32_t address, uint16_t len, uint8_t *data)
+{
+  led_red();
+  PRG(0x8000 | 0x0000) = 0xF0;
+  while (len > 0)
+  {
+    unrom512_cmd_write(0x5555, 0xAA);
+    unrom512_cmd_write(0x2AAA, 0x55);
+    unrom512_cmd_write(0x5555, 0xA0);
+    unrom512_cmd_write(address, *data);
+
+    uint32_t start_time = HAL_GetTick();
+    // waiting for result
+    while (1)
+    {
+      if (HAL_GetTick() >= start_time + 50) // 50 ms timeout
+      {
+        // timeout
+        comm_start(COMMAND_FLASH_WRITE_TIMEOUT, 0);
+        return;
+      }
+      uint8_t read_1 = unrom512_cmd_read(address);
+      uint8_t read_2 = unrom512_cmd_read(address);
+      if ((read_1 == read_2) && (read_2 == *data)) break; // OK
+    }
+    address++;
+    data++;
+    len--;
+  }
+  comm_start(COMMAND_PRG_WRITE_DONE, 0);
+}