16 files changed, 3669 insertions, 0 deletions
diff --git a/FamicomDumperBootloader/Core/Src/cpldwriter.c b/FamicomDumperBootloader/Core/Src/cpldwriter.c
new file mode 100644
index 0000000..3613931
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/cpldwriter.c
@@ -0,0 +1,192 @@
+#include "bootloader.h"
+#include "main.h"
+#include "cpldwriter.h"
+#include "libxsvf.h"
+#include "fatfs.h"
+#include <inttypes.h>
+#include <string.h>
+#include <stdlib.h>
+
+static int svf_setup(struct libxsvf_host *h)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+  /* Configure GPIO pin : TDO_Pin */
+  GPIO_InitStruct.Pin = TDO_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(TDO_GPIO_Port, &GPIO_InitStruct);
+
+  /* Configure GPIO pins : TCK_Pin TDI_Pin TMS_Pin */
+  GPIO_InitStruct.Pin = TCK_Pin|TDI_Pin|TMS_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  HAL_GPIO_WritePin(TCK_GPIO_Port, TCK_Pin, GPIO_PIN_SET);
+  HAL_GPIO_WritePin(TDI_GPIO_Port, TDI_Pin, GPIO_PIN_SET);
+  HAL_GPIO_WritePin(TMS_GPIO_Port, TMS_Pin, GPIO_PIN_SET);
+
+  return 0;
+}
+
+static int svf_shutdown(struct libxsvf_host *h)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+  /* Configure GPIO pin : TDO_Pin TCK_Pin TDI_Pin TMS_Pin */
+  GPIO_InitStruct.Pin = TDO_Pin|TCK_Pin|TDI_Pin|TMS_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  return 0;
+}
+
+static void svf_udelay(struct libxsvf_host *h, long usecs, int tms, long num_tck)
+{
+  if (usecs > 50000) usecs = 50000; // limit
+  TIM1->CNT = 0;
+  if (num_tck)
+  {
+    HAL_GPIO_WritePin(TMS_GPIO_Port, TMS_Pin, tms ? GPIO_PIN_SET : GPIO_PIN_RESET);
+    while (num_tck-- > 0)
+    {
+      HAL_GPIO_WritePin(TCK_GPIO_Port, TCK_Pin, GPIO_PIN_RESET);
+      HAL_GPIO_WritePin(TCK_GPIO_Port, TCK_Pin, GPIO_PIN_SET);
+    }
+    while (TIM1->CNT < usecs) ;
+  }
+}
+
+static int svf_getbyte(struct libxsvf_host *h)
+{
+  FRESULT fr;
+  struct udata_s *u = (struct udata_s *)h->user_data;
+
+  if (u->pos >= u->size)
+  {
+    // need to read data
+    fr = f_read(u->fil, &u->buff, SVF_BUFFER_SIZE, &u->size);
+    if (fr) return -1;
+    if (!u->size) return -1;
+    u->pos = 0;
+  }
+  return u->buff[u->pos++];
+}
+
+static int svf_pulse_tck(struct libxsvf_host *h, int tms, int tdi, int tdo, int rmask, int sync)
+{
+  HAL_GPIO_WritePin(TMS_GPIO_Port, TMS_Pin, tms ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  if (tdi >= 0) HAL_GPIO_WritePin(TDI_GPIO_Port, TDI_Pin, tdi ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(TCK_GPIO_Port, TCK_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(TCK_GPIO_Port, TCK_Pin, GPIO_PIN_SET);
+  GPIO_PinState line_tdo = HAL_GPIO_ReadPin(TDO_GPIO_Port, TDO_Pin);
+  return tdo < 0 || line_tdo == tdo ? line_tdo : -1;
+}
+
+int svf_set_frequency(struct libxsvf_host *h, int v)
+{
+  struct udata_s *u = (struct udata_s *)h->user_data;
+  u->freq = v;
+  return 0;
+}
+
+static void svf_report_error(struct libxsvf_host *h, const char *file, int line, const char *message)
+{
+  error();
+}
+
+static void *svf_realloc(struct libxsvf_host *h, void *ptr, int size, enum libxsvf_mem which)
+{
+  return realloc(ptr, size);
+}
+
+void write_cpld(FILINFO *svf_file)
+{
+  FATFS FatFs;
+  FIL fil;
+  FRESULT fr;
+  struct udata_s u;
+  u.fil = &fil;
+  u.pos = 0;
+  u.size = 0;
+  struct libxsvf_host h = {
+    .setup = svf_setup,
+    .shutdown = svf_shutdown,
+    .udelay = svf_udelay,
+    .getbyte = svf_getbyte,
+    .sync = 0,
+    .pulse_tck = svf_pulse_tck,
+    .pulse_sck = 0,
+    .set_trst = 0,
+    .set_frequency = svf_set_frequency,
+    .report_tapstate = 0,
+    .report_device = 0,
+    .report_status = 0,
+    .report_error = svf_report_error,
+    .realloc = svf_realloc,
+    .user_data = &u
+  };
+
+ // Mount FAT file system
+  fr = f_mount(&FatFs, "", 1);
+  if (fr)
+    error();
+  // Open firmware file
+  fr = f_open(u.fil, svf_file->fname, FA_READ);
+  if (fr)
+    error();
+
+  if (libxsvf_play(&h, LIBXSVF_MODE_SVF) < 0) {
+    error();
+  }
+
+  // Close file
+  fr = f_close(u.fil);
+  if (fr)
+    error();
+  // Delete file
+  fr = f_unlink(svf_file->fname);
+  // Unmount
+  fr = f_mount(NULL, "", 1);
+  if (fr)
+    error();
+}
+
+int find_svf_file(FILINFO *svf_file)
+{
+  // Find first .svf file if any
+  int len, r = 0;
+  FATFS fs;
+  FRESULT res;
+  DIR dir;
+  FILINFO fno;
+
+  f_mount(&fs, "", 1);
+  res = f_opendir(&dir, "/");
+  if (res == FR_OK)
+  {
+    while (1)
+    {
+      res = f_readdir(&dir, &fno);
+      if (res != FR_OK || fno.fname[0] == 0)
+        break;
+      if (!(fno.fattrib & AM_DIR) && (fno.fsize != 0))
+      {
+        len = strlen(fno.fname);
+        if ((len > 4) && (fno.fname[len - 4] == '.') && (fno.fname[len - 3] == 'S') && (fno.fname[len - 2] == 'V') && (fno.fname[len - 1] == 'F'))
+        {
+          memcpy(svf_file, &fno, sizeof(FILINFO));
+          r = 1;
+          break;
+        }
+      }
+    }
+  }
+  f_closedir(&dir);
+  f_mount(NULL, "", 1);
+
+  return r;
+}
diff --git a/FamicomDumperBootloader/Core/Src/firmwriter.c b/FamicomDumperBootloader/Core/Src/firmwriter.c
new file mode 100644
index 0000000..48f30fc
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/firmwriter.c
@@ -0,0 +1,148 @@
+#include "bootloader.h"
+#include "main.h"
+#include "firmwriter.h"
+#include "fatfs.h"
+#include <inttypes.h>
+#include <string.h>
+
+void write_hardware_version(void)
+{
+  uint32_t value;
+  uint32_t current_value;
+  HAL_StatusTypeDef hres;
+  FLASH_EraseInitTypeDef erase_init_struct;
+  uint32_t sector_error = 0;
+  uint8_t HARDWARE_VERSION[] = { HARDWARE_VERSION_MAJOR & 0xFF, (HARDWARE_VERSION_MAJOR >> 8) && 0xFF, HARDWARE_VERSION_MINOR, HARDWARE_VERSION_SUFFIX };
+
+  // Write hardware version
+  value = *(uint32_t*)HARDWARE_VERSION;
+  current_value = *(volatile uint32_t*)(HARDWARE_VERSION_ADDRESS);
+  if (value != current_value)
+  {
+    // Flash can be locked by FATFS
+    hres = HAL_FLASH_Unlock();
+    if (hres != HAL_OK)
+      error();
+    erase_init_struct.TypeErase = FLASH_TYPEERASE_PAGES;
+    erase_init_struct.PageAddress = HARDWARE_VERSION_ADDRESS;
+    erase_init_struct.NbPages = 1;
+    hres = HAL_FLASHEx_Erase(&erase_init_struct, &sector_error);
+    if (hres != HAL_OK)
+      error();
+    current_value = *(volatile uint32_t*)(HARDWARE_VERSION_ADDRESS);
+    hres = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, HARDWARE_VERSION_ADDRESS, value);
+    if (hres != HAL_OK)
+      error();
+    current_value = *(volatile uint32_t*)(HARDWARE_VERSION_ADDRESS);
+    if (value != current_value)
+      error();
+    HAL_FLASH_Lock();
+  }
+}
+
+void write_firmware(FILINFO *bin_file)
+{
+  int i;
+  uint8_t buff[MSD_BLOCK_SIZE];
+  FATFS FatFs;
+  FIL fil;
+  FRESULT fr;
+  UINT br = 0;
+  uint32_t address = APP_ADDRESS;
+  FLASH_EraseInitTypeDef erase_init_struct;
+  uint32_t sector_error = 0;
+  HAL_StatusTypeDef hres;
+  uint32_t value;
+
+  // Unlock flash
+  hres = HAL_FLASH_Unlock();
+  if (hres != HAL_OK)
+    error();
+
+  // Mount FAT file system
+  fr = f_mount(&FatFs, "", 1);
+  if (fr)
+    error();
+  // Open firmware file
+  fr = f_open(&fil, bin_file->fname, FA_READ);
+  if (fr)
+    error();
+  while (1)
+  {
+    // Read data
+    fr = f_read(&fil, buff, MSD_BLOCK_SIZE, &br);
+    if (fr)
+      error();
+    // Break if end of file
+    if (!br)
+      break;
+    if ((address % MSD_BLOCK_SIZE) != 0)
+      error();
+    // Erase flash page
+    erase_init_struct.TypeErase = FLASH_TYPEERASE_PAGES;
+    erase_init_struct.PageAddress = address;
+    erase_init_struct.NbPages = 1;
+    hres = HAL_FLASHEx_Erase(&erase_init_struct, &sector_error);
+    if (hres != HAL_OK)
+      error();
+    // Write data
+    for (i = 0; i < br; i += 4)
+    {
+      value = *(volatile uint32_t*) &buff[i];
+      hres = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, value);
+      if (hres != HAL_OK)
+        error();
+      address += 4;
+    }
+  }
+  // Close file
+  fr = f_close(&fil);
+  if (fr)
+    error();
+  // Delete file
+  fr = f_unlink(bin_file->fname);
+  // Unmount
+  fr = f_mount(NULL, "", 1);
+  if (fr)
+    error();
+
+  // Lock flash
+  HAL_FLASH_Lock();
+}
+
+int find_bin_file(FILINFO *bin_file)
+{
+  // Find first .bin file if any
+  int len, r = 0;
+  FATFS fs;
+  FRESULT res;
+  DIR dir;
+  FILINFO fno;
+
+  f_mount(&fs, "", 1);
+  res = f_opendir(&dir, "/");
+  if (res == FR_OK)
+  {
+    while (1)
+    {
+      res = f_readdir(&dir, &fno);
+      if (res != FR_OK || fno.fname[0] == 0)
+        break;
+      if (!(fno.fattrib & AM_DIR) && (fno.fsize != 0))
+      {
+        len = strlen(fno.fname);
+        if ((len > 4) && (fno.fname[len - 4] == '.') && (fno.fname[len - 3] == 'B') && (fno.fname[len - 2] == 'I') && (fno.fname[len - 1] == 'N'))
+        {
+          memcpy(bin_file, &fno, sizeof(FILINFO));
+          r = 1;
+          break;
+        }
+      }
+    }
+  }
+  f_closedir(&dir);
+  f_mount(NULL, "", 1);
+
+  return r;
+}
+
diff --git a/FamicomDumperBootloader/Core/Src/led.c b/FamicomDumperBootloader/Core/Src/led.c
new file mode 100644
index 0000000..a753cd5
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/led.c
@@ -0,0 +1,72 @@
+#include "main.h"
+#include <inttypes.h>
+
+extern TIM_HandleTypeDef htim5;
+static uint8_t pwm_values[8 * 3 + 1];
+static uint32_t last_led_update_time = 0;
+
+static void update_led()
+{
+  while (last_led_update_time + 2 > HAL_GetTick())
+  {
+  }
+  HAL_TIMEx_PWMN_Stop_DMA(&htim5, TIM_CHANNEL_3);
+  htim5.Instance->CCR3 = 1;
+  htim5.Instance->CNT = 0;
+  HAL_TIM_PWM_Start_DMA(&htim5, TIM_CHANNEL_3, (void*) pwm_values, sizeof(pwm_values));
+  last_led_update_time = HAL_GetTick();
+}
+
+void set_led_color(uint8_t r, uint8_t g, uint8_t b)
+{
+  uint8_t i;
+  const uint8_t led = 0;
+  for (i = 0; i < 8; i++)
+    pwm_values[led * 8 * 3 + i] = ((r >> (7 - i)) & 1) ? 65 : 24;
+  for (i = 0; i < 8; i++)
+    pwm_values[led * 8 * 3 + 8 + i] = ((g >> (7 - i)) & 1) ? 65 : 24;
+  for (i = 0; i < 8; i++)
+    pwm_values[led * 8 * 3 + 16 + i] = ((b >> (7 - i)) & 1) ? 65 : 24;
+  pwm_values[sizeof(pwm_values) - 1] = 0;
+  update_led();
+}
+
+void led_off()
+{
+  set_led_color(0x00, 0x00, 0x00);
+}
+
+void led_white()
+{
+  set_led_color(0xFF, 0xFF, 0xFF);
+}
+
+void led_green()
+{
+  set_led_color(0x00, 0xFF, 0x00);
+}
+
+void led_red()
+{
+  set_led_color(0xFF, 0x00, 0x00);
+}
+
+void led_yellow()
+{
+  set_led_color(0xFF, 0xFF, 0x00);
+}
+
+void led_blue()
+{
+  set_led_color(0x00, 0x00, 0xFF);
+}
+
+void led_magenta()
+{
+  set_led_color(0xFF, 0x00, 0xFF);
+}
+
+void led_cyan()
+{
+  set_led_color(0x00, 0xFF, 0xFF);
+}
diff --git a/FamicomDumperBootloader/Core/Src/libxsvf/memname.c b/FamicomDumperBootloader/Core/Src/libxsvf/memname.c
new file mode 100644
index 0000000..0cc3f31
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/libxsvf/memname.c
@@ -0,0 +1,65 @@
+/*
+ *  Lib(X)SVF  -  A library for implementing SVF and XSVF JTAG players
+ *
+ *  Copyright (C) 2009  RIEGL Research ForschungsGmbH
+ *  Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
+ *  
+ *  Permission to use, copy, modify, and/or distribute this software for any
+ *  purpose with or without fee is hereby granted, provided that the above
+ *  copyright notice and this permission notice appear in all copies.
+ *  
+ *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#include "libxsvf.h"
+
+const char *libxsvf_mem2str(enum libxsvf_mem which)
+{
+#define X(_w, _t) if (which == LIBXSVF_MEM_ ## _w) return #_t;
+	X(XSVF_TDI_DATA, xsvf_tdi_data)
+	X(XSVF_TDO_DATA, xsvf_tdo_data)
+	X(XSVF_TDO_MASK, xsvf_tdo_mask)
+	X(XSVF_ADDR_MASK, xsvf_addr_mask)
+	X(XSVF_DATA_MASK, xsvf_data_mask)
+	X(SVF_COMMANDBUF, svf_commandbuf)
+	X(SVF_HDR_TDI_DATA, svf_hdr_tdi_data)
+	X(SVF_HDR_TDI_MASK, svf_hdr_tdi_mask)
+	X(SVF_HDR_TDO_DATA, svf_hdr_tdo_data)
+	X(SVF_HDR_TDO_MASK, svf_hdr_tdo_mask)
+	X(SVF_HDR_RET_MASK, svf_hdr_ret_mask)
+	X(SVF_HIR_TDI_DATA, svf_hir_tdi_data)
+	X(SVF_HIR_TDI_MASK, svf_hir_tdi_mask)
+	X(SVF_HIR_TDO_DATA, svf_hir_tdo_data)
+	X(SVF_HIR_TDO_MASK, svf_hir_tdo_mask)
+	X(SVF_HIR_RET_MASK, svf_hir_ret_mask)
+	X(SVF_TDR_TDI_DATA, svf_tdr_tdi_data)
+	X(SVF_TDR_TDI_MASK, svf_tdr_tdi_mask)
+	X(SVF_TDR_TDO_DATA, svf_tdr_tdo_data)
+	X(SVF_TDR_TDO_MASK, svf_tdr_tdo_mask)
+	X(SVF_TDR_RET_MASK, svf_tdr_ret_mask)
+	X(SVF_TIR_TDI_DATA, svf_tir_tdi_data)
+	X(SVF_TIR_TDI_MASK, svf_tir_tdi_mask)
+	X(SVF_TIR_TDO_DATA, svf_tir_tdo_data)
+	X(SVF_TIR_TDO_MASK, svf_tir_tdo_mask)
+	X(SVF_TIR_RET_MASK, svf_tir_ret_mask)
+	X(SVF_SDR_TDI_DATA, svf_sdr_tdi_data)
+	X(SVF_SDR_TDI_MASK, svf_sdr_tdi_mask)
+	X(SVF_SDR_TDO_DATA, svf_sdr_tdo_data)
+	X(SVF_SDR_TDO_MASK, svf_sdr_tdo_mask)
+	X(SVF_SDR_RET_MASK, svf_sdr_ret_mask)
+	X(SVF_SIR_TDI_DATA, svf_sir_tdi_data)
+	X(SVF_SIR_TDI_MASK, svf_sir_tdi_mask)
+	X(SVF_SIR_TDO_DATA, svf_sir_tdo_data)
+	X(SVF_SIR_TDO_MASK, svf_sir_tdo_mask)
+	X(SVF_SIR_RET_MASK, svf_sir_ret_mask)
+#undef X
+	return (void*)0;
+}
+
diff --git a/FamicomDumperBootloader/Core/Src/libxsvf/play.c b/FamicomDumperBootloader/Core/Src/libxsvf/play.c
new file mode 100644
index 0000000..e57c01e
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/libxsvf/play.c
@@ -0,0 +1,72 @@
+/*
+ *  Lib(X)SVF  -  A library for implementing SVF and XSVF JTAG players
+ *
+ *  Copyright (C) 2009  RIEGL Research ForschungsGmbH
+ *  Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
+ *  
+ *  Permission to use, copy, modify, and/or distribute this software for any
+ *  purpose with or without fee is hereby granted, provided that the above
+ *  copyright notice and this permission notice appear in all copies.
+ *  
+ *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#include "libxsvf.h"
+
+int libxsvf_play(struct libxsvf_host *h, enum libxsvf_mode mode)
+{
+	int rc = -1;
+
+	h->tap_state = LIBXSVF_TAP_INIT;
+	if (LIBXSVF_HOST_SETUP() < 0) {
+		LIBXSVF_HOST_REPORT_ERROR("Setup of JTAG interface failed.");
+		return -1;
+	}
+
+	if (mode == LIBXSVF_MODE_SVF) {
+#ifdef LIBXSVF_WITHOUT_SVF
+		LIBXSVF_HOST_REPORT_ERROR("SVF support in libxsvf is disabled.");
+#else
+		rc = libxsvf_svf(h);
+#endif
+	}
+
+	if (mode == LIBXSVF_MODE_XSVF) {
+#ifdef LIBXSVF_WITHOUT_XSVF
+		LIBXSVF_HOST_REPORT_ERROR("XSVF support in libxsvf is disabled.");
+#else
+		rc = libxsvf_xsvf(h);
+#endif
+	}
+
+	if (mode == LIBXSVF_MODE_SCAN) {
+#ifdef LIBXSVF_WITHOUT_SCAN
+		LIBXSVF_HOST_REPORT_ERROR("SCAN support in libxsvf is disabled.");
+#else
+		rc = libxsvf_scan(h);
+#endif
+	}
+
+	libxsvf_tap_walk(h, LIBXSVF_TAP_RESET);
+	if (LIBXSVF_HOST_SYNC() != 0 && rc >= 0 ) {
+		LIBXSVF_HOST_REPORT_ERROR("TDO mismatch in TAP reset. (this is not possible!)");
+		rc = -1;
+	}
+
+	int shutdown_rc = LIBXSVF_HOST_SHUTDOWN();
+
+	if (shutdown_rc < 0) {
+		LIBXSVF_HOST_REPORT_ERROR("Shutdown of JTAG interface failed.");
+		rc = rc < 0 ? rc : shutdown_rc;
+	}
+
+	return rc;
+}
+
diff --git a/FamicomDumperBootloader/Core/Src/libxsvf/scan.c b/FamicomDumperBootloader/Core/Src/libxsvf/scan.c
new file mode 100644
index 0000000..cb24e56
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/libxsvf/scan.c
@@ -0,0 +1,58 @@
+/*
+ *  Lib(X)SVF  -  A library for implementing SVF and XSVF JTAG players
+ *
+ *  Copyright (C) 2009  RIEGL Research ForschungsGmbH
+ *  Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
+ *  
+ *  Permission to use, copy, modify, and/or distribute this software for any
+ *  purpose with or without fee is hereby granted, provided that the above
+ *  copyright notice and this permission notice appear in all copies.
+ *  
+ *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#include "libxsvf.h"
+
+int libxsvf_scan(struct libxsvf_host *h)
+{
+	int i, j;
+
+	if (libxsvf_tap_walk(h, LIBXSVF_TAP_RESET) < 0)
+		return -1;
+
+	if (libxsvf_tap_walk(h, LIBXSVF_TAP_DRSHIFT) < 0)
+		return -1;
+
+	for (i=0; i<256; i++)
+	{
+		int bit = LIBXSVF_HOST_PULSE_TCK(0, 1, -1, 0, 1);
+
+		if (bit < 0)
+			return -1;
+
+		if (bit == 0) {
+			LIBXSVF_HOST_REPORT_DEVICE(0);
+		} else {
+			unsigned long idcode = 1;
+			for (j=1; j<32; j++) {
+				int bit = LIBXSVF_HOST_PULSE_TCK(0, 1, -1, 0, 1);
+				if (bit < 0)
+					return -1;
+				idcode |= ((unsigned long)bit) << j;
+			}
+			if (idcode == 0xffffffff)
+				break;
+			LIBXSVF_HOST_REPORT_DEVICE(idcode);
+		}
+	}
+
+	return 0;
+}
+
diff --git a/FamicomDumperBootloader/Core/Src/libxsvf/statename.c b/FamicomDumperBootloader/Core/Src/libxsvf/statename.c
new file mode 100644
index 0000000..128fb7f
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/libxsvf/statename.c
@@ -0,0 +1,46 @@
+/*
+ *  Lib(X)SVF  -  A library for implementing SVF and XSVF JTAG players
+ *
+ *  Copyright (C) 2009  RIEGL Research ForschungsGmbH
+ *  Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
+ *  
+ *  Permission to use, copy, modify, and/or distribute this software for any
+ *  purpose with or without fee is hereby granted, provided that the above
+ *  copyright notice and this permission notice appear in all copies.
+ *  
+ *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#include "libxsvf.h"
+
+const char *libxsvf_state2str(enum libxsvf_tap_state tap_state)
+{
+#define X(_s) if (tap_state == _s) return #_s;
+	X(LIBXSVF_TAP_INIT)
+	X(LIBXSVF_TAP_RESET)
+	X(LIBXSVF_TAP_IDLE)
+	X(LIBXSVF_TAP_DRSELECT)
+	X(LIBXSVF_TAP_DRCAPTURE)
+	X(LIBXSVF_TAP_DRSHIFT)
+	X(LIBXSVF_TAP_DREXIT1)
+	X(LIBXSVF_TAP_DRPAUSE)
+	X(LIBXSVF_TAP_DREXIT2)
+	X(LIBXSVF_TAP_DRUPDATE)
+	X(LIBXSVF_TAP_IRSELECT)
+	X(LIBXSVF_TAP_IRCAPTURE)
+	X(LIBXSVF_TAP_IRSHIFT)
+	X(LIBXSVF_TAP_IREXIT1)
+	X(LIBXSVF_TAP_IRPAUSE)
+	X(LIBXSVF_TAP_IREXIT2)
+	X(LIBXSVF_TAP_IRUPDATE)
+#undef X
+	return "UNKOWN_STATE";
+}
+
diff --git a/FamicomDumperBootloader/Core/Src/libxsvf/svf.c b/FamicomDumperBootloader/Core/Src/libxsvf/svf.c
new file mode 100644
index 0000000..4e2f4b0
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/libxsvf/svf.c
@@ -0,0 +1,668 @@
+/*
+ *  Lib(X)SVF  -  A library for implementing SVF and XSVF JTAG players
+ *
+ *  Copyright (C) 2009  RIEGL Research ForschungsGmbH
+ *  Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
+ *  
+ *  Permission to use, copy, modify, and/or distribute this software for any
+ *  purpose with or without fee is hereby granted, provided that the above
+ *  copyright notice and this permission notice appear in all copies.
+ *  
+ *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#include "libxsvf.h"
+
+static int read_command(struct libxsvf_host *h, char **buffer_p, int *len_p)
+{
+	char *buffer = *buffer_p;
+	int braket_mode = 0;
+	int len = *len_p;
+	int p = 0;
+
+	while (1)
+	{
+		if (len < p+10) {
+			len = len < 64 ? 96 : len*2;
+			buffer = LIBXSVF_HOST_REALLOC(buffer, len, LIBXSVF_MEM_SVF_COMMANDBUF);
+			*buffer_p = buffer;
+			*len_p = len;
+			if (!buffer) {
+				LIBXSVF_HOST_REPORT_ERROR("Allocating memory failed.");
+				return -1;
+			}
+		}
+		buffer[p] = 0;
+
+		int ch = LIBXSVF_HOST_GETBYTE();
+		if (ch < 0) {
+handle_eof:
+			if (p == 0)
+				return 0;
+			LIBXSVF_HOST_REPORT_ERROR("Unexpected EOF.");
+			return -1;
+		}
+		if (ch <= ' ') {
+insert_eol:
+			if (!braket_mode && p > 0 && buffer[p-1] != ' ')
+				buffer[p++] = ' ';
+			continue;
+		}
+		if (ch == '!') {
+skip_to_eol:
+			while (1) {
+				ch = LIBXSVF_HOST_GETBYTE();
+				if (ch < 0)
+					goto handle_eof;
+				if (ch < ' ' && ch != '\t')
+					goto insert_eol;
+			}
+		}
+		if (ch == '/' && p > 0 && buffer[p-1] == '/') {
+			p--;
+			goto skip_to_eol;
+		}
+		if (ch == ';')
+			break;
+		if (ch == '(') {
+			if (!braket_mode && p > 0 && buffer[p-1] != ' ')
+				buffer[p++] = ' ';
+			braket_mode++;
+		}
+		if (ch >= 'a' && ch <= 'z')
+			buffer[p++] = ch - ('a' - 'A');
+		else
+			buffer[p++] = ch;
+		if (ch == ')') {
+			braket_mode--;
+			if (!braket_mode)
+				buffer[p++] = ' ';
+		}
+	}
+	return 1;
+}
+
+static int strtokencmp(const char *str1, const char *str2)
+{
+	int i = 0;
+	while (1) {
+		if ((str1[i] == ' ' || str1[i] == 0) && (str2[i] == ' ' || str2[i] == 0))
+			return 0;
+		if (str1[i] < str2[i])
+			return -1;
+		if (str1[i] > str2[i])
+			return +1;
+		i++;
+	}
+}
+
+static int strtokenskip(const char *str1)
+{
+	int i = 0;
+	while (str1[i] != 0 && str1[i] != ' ') i++;
+	while (str1[i] == ' ') i++;
+	return i;
+}
+
+static int token2tapstate(const char *str1)
+{
+#define X(_t) if (!strtokencmp(str1, #_t)) return LIBXSVF_TAP_ ## _t;
+	X(RESET)
+	X(IDLE)
+	X(DRSELECT)
+	X(DRCAPTURE)
+	X(DRSHIFT)
+	X(DREXIT1)
+	X(DRPAUSE)
+	X(DREXIT2)
+	X(DRUPDATE)
+	X(IRSELECT)
+	X(IRCAPTURE)
+	X(IRSHIFT)
+	X(IREXIT1)
+	X(IRPAUSE)
+	X(IREXIT2)
+	X(IRUPDATE)
+#undef X
+	return -1;
+}
+
+struct bitdata_s {
+	int len, alloced_len;
+	int alloced_bytes;
+	unsigned char *tdi_data;
+	unsigned char *tdi_mask;
+	unsigned char *tdo_data;
+	unsigned char *tdo_mask;
+	unsigned char *ret_mask;
+	int has_tdo_data;
+};
+
+static void bitdata_free(struct libxsvf_host *h, struct bitdata_s *bd, int offset)
+{
+	LIBXSVF_HOST_REALLOC(bd->tdi_data, 0, offset+0);
+	LIBXSVF_HOST_REALLOC(bd->tdi_mask, 0, offset+1);
+	LIBXSVF_HOST_REALLOC(bd->tdo_data, 0, offset+2);
+	LIBXSVF_HOST_REALLOC(bd->tdo_mask, 0, offset+3);
+	LIBXSVF_HOST_REALLOC(bd->ret_mask, 0, offset+4);
+
+	bd->tdi_data = (void*)0;
+	bd->tdi_mask = (void*)0;
+	bd->tdo_data = (void*)0;
+	bd->tdo_mask = (void*)0;
+	bd->ret_mask = (void*)0;
+}
+
+static int hex(char ch)
+{
+	if (ch >= 'A' && ch <= 'Z')
+		return (ch - 'A') + 10;
+	if (ch >= '0' && ch <= '9')
+		return ch - '0';
+	return 0;
+}
+
+static const char *bitdata_parse(struct libxsvf_host *h, const char *p, struct bitdata_s *bd, int offset)
+{
+	int i, j;
+	bd->len = 0;
+	bd->has_tdo_data = 0;
+	while (*p >= '0' && *p <= '9') {
+		bd->len = bd->len * 10 + (*p - '0');
+		p++;
+	}
+	while (*p == ' ') {
+		p++;
+	}
+	if (bd->len != bd->alloced_len) {
+		bitdata_free(h, bd, offset);
+		bd->alloced_len = bd->len;
+		bd->alloced_bytes = (bd->len+7) / 8;
+	}
+	while (*p)
+	{
+		int memnum = 0;
+		unsigned char **dp = (void*)0;
+		if (!strtokencmp(p, "TDI")) {
+			p += strtokenskip(p);
+			dp = &bd->tdi_data;
+			memnum = 0;
+		}
+		if (!strtokencmp(p, "TDO")) {
+			p += strtokenskip(p);
+			dp = &bd->tdo_data;
+			bd->has_tdo_data = 1;
+			memnum = 1;
+		}
+		if (!strtokencmp(p, "SMASK")) {
+			p += strtokenskip(p);
+			dp = &bd->tdi_mask;
+			memnum = 2;
+		}
+		if (!strtokencmp(p, "MASK")) {
+			p += strtokenskip(p);
+			dp = &bd->tdo_mask;
+			memnum = 3;
+		}
+		if (!strtokencmp(p, "RMASK")) {
+			p += strtokenskip(p);
+			dp = &bd->ret_mask;
+			memnum = 4;
+		}
+		if (!dp)
+			return (void*)0;
+		if (*dp == (void*)0) {
+			*dp = LIBXSVF_HOST_REALLOC(*dp, bd->alloced_bytes, offset+memnum);
+		}
+		if (*dp == (void*)0) {
+			LIBXSVF_HOST_REPORT_ERROR("Allocating memory failed.");
+			return (void*)0;
+		}
+
+		unsigned char *d = *dp;
+		for (i=0; i<bd->alloced_bytes; i++)
+			d[i] = 0;
+
+		if (*p != '(')
+			return (void*)0;
+		p++;
+
+		int hexdigits = 0;
+		for (i=0; (p[i] >= 'A' && p[i] <= 'F') || (p[i] >= '0' && p[i] <= '9'); i++)
+			hexdigits++;
+
+		i = bd->alloced_bytes*2 - hexdigits;
+		for (j=0; j<hexdigits; j++, i++, p++) {
+			if (i%2 == 0) {
+				d[i/2] |= hex(*p) << 4;
+			} else {
+				d[i/2] |= hex(*p);
+			}
+		}
+
+		if (*p != ')')
+			return (void*)0;
+		p++;
+		while (*p == ' ') {
+			p++;
+		}
+	}
+#if 0
+	/* Debugging Output, needs <stdio.h> */
+	printf("--- Parsed bitdata [%d] ---\n", bd->len);
+	if (bd->tdi_data) {
+		printf("TDI DATA:");
+		for (i=0; i<bd->alloced_bytes; i++)
+			printf(" %02x", bd->tdi_data[i]);
+		printf("\n");
+	}
+	if (bd->tdo_data && has_tdo_data) {
+		printf("TDO DATA:");
+		for (i=0; i<bd->alloced_bytes; i++)
+			printf(" %02x", bd->tdo_data[i]);
+		printf("\n");
+	}
+	if (bd->tdi_mask) {
+		printf("TDI MASK:");
+		for (i=0; i<bd->alloced_bytes; i++)
+			printf(" %02x", bd->tdi_mask[i]);
+		printf("\n");
+	}
+	if (bd->tdo_mask) {
+		printf("TDO MASK:");
+		for (i=0; i<bd->alloced_bytes; i++)
+			printf(" %02x", bd->tdo_mask[i]);
+		printf("\n");
+	}
+#endif
+	return p;
+}
+
+static int getbit(unsigned char *data, int n)
+{
+	return (data[n/8] & (1 << (7 - n%8))) ? 1 : 0;
+}
+
+static int bitdata_play(struct libxsvf_host *h, struct bitdata_s *bd, enum libxsvf_tap_state estate)
+{
+	int left_padding = (8 - bd->len % 8) % 8;
+	int tdo_error = 0;
+	int tms = 0;
+	int i;
+
+	for (i=bd->len+left_padding-1; i >= left_padding; i--) {
+		if (i == left_padding && h->tap_state != estate) {
+			h->tap_state++;
+			tms = 1;
+		}
+		int tdi = -1;
+		if (bd->tdi_data) {
+			if (!bd->tdi_mask || getbit(bd->tdi_mask, i))
+				tdi = getbit(bd->tdi_data, i);
+		}
+		int tdo = -1;
+		if (bd->tdo_data && bd->has_tdo_data && (!bd->tdo_mask || getbit(bd->tdo_mask, i)))
+			tdo = getbit(bd->tdo_data, i);
+		int rmask = bd->ret_mask && getbit(bd->ret_mask, i);
+		if (LIBXSVF_HOST_PULSE_TCK(tms, tdi, tdo, rmask, 0) < 0)
+			tdo_error = 1;
+	}
+
+	if (tms)
+		LIBXSVF_HOST_REPORT_TAPSTATE();
+
+	if (!tdo_error)
+		return 0;
+
+	LIBXSVF_HOST_REPORT_ERROR("TDO mismatch.");
+	return -1;
+}
+
+int libxsvf_svf(struct libxsvf_host *h)
+{
+	char *command_buffer = (void*)0;
+	int command_buffer_len = 0;
+	int rc, i;
+
+	struct bitdata_s bd_hdr = { 0, 0, 0, (void*)0, (void*)0, (void*)0, (void*)0, (void*)0 };
+	struct bitdata_s bd_hir = { 0, 0, 0, (void*)0, (void*)0, (void*)0, (void*)0, (void*)0 };
+	struct bitdata_s bd_tdr = { 0, 0, 0, (void*)0, (void*)0, (void*)0, (void*)0, (void*)0 };
+	struct bitdata_s bd_tir = { 0, 0, 0, (void*)0, (void*)0, (void*)0, (void*)0, (void*)0 };
+	struct bitdata_s bd_sdr = { 0, 0, 0, (void*)0, (void*)0, (void*)0, (void*)0, (void*)0 };
+	struct bitdata_s bd_sir = { 0, 0, 0, (void*)0, (void*)0, (void*)0, (void*)0, (void*)0 };
+
+	int state_endir = LIBXSVF_TAP_IDLE;
+	int state_enddr = LIBXSVF_TAP_IDLE;
+	int state_run = LIBXSVF_TAP_IDLE;
+	int state_endrun = LIBXSVF_TAP_IDLE;
+
+	while (1)
+	{
+		rc = read_command(h, &command_buffer, &command_buffer_len);
+
+		if (rc <= 0)
+			break;
+
+		const char *p = command_buffer;
+
+		LIBXSVF_HOST_REPORT_STATUS(command_buffer);
+
+		if (!strtokencmp(p, "ENDIR")) {
+			p += strtokenskip(p);
+			state_endir = token2tapstate(p);
+			if (state_endir < 0)
+				goto syntax_error;
+			p += strtokenskip(p);
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "ENDDR")) {
+			p += strtokenskip(p);
+			state_enddr = token2tapstate(p);
+			if (state_endir < 0)
+				goto syntax_error;
+			p += strtokenskip(p);
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "FREQUENCY")) {
+			unsigned long number = 0;
+			int exp = 0;
+			int nexp = 0;
+			p += strtokenskip(p);
+			if (*p < '0' || *p > '9')
+				goto syntax_error;
+			while (*p >= '0' && *p <= '9') {
+				number = number*10 + (*p - '0');
+				p++;
+			}
+			if(*p == '.') {
+			  p++;
+	      while (*p >= '0' && *p <= '9') {
+	        number = number*10 + (*p - '0');
+	        nexp++;
+	        p++;
+	      }
+      }
+			if(*p == 'E' || *p == 'e') {
+				p++;
+	      if(*p == '+') p++;
+				while (*p >= '0' && *p <= '9') {
+					exp = exp*10 + (*p - '0');
+					p++;
+				}
+				exp -= nexp;
+				for(i=0; i<exp; i++)
+					number *= 10;
+			}
+			while (*p == ' ') {
+				p++;
+			}
+			p += strtokenskip(p);
+			if (LIBXSVF_HOST_SET_FREQUENCY(number) < 0) {
+				LIBXSVF_HOST_REPORT_ERROR("FREQUENCY command failed!");
+				goto error;
+			}
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "HDR")) {
+			p += strtokenskip(p);
+			p = bitdata_parse(h, p, &bd_hdr, LIBXSVF_MEM_SVF_HDR_TDI_DATA);
+			if (!p)
+				goto syntax_error;
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "HIR")) {
+			p += strtokenskip(p);
+			p = bitdata_parse(h, p, &bd_hir, LIBXSVF_MEM_SVF_HIR_TDI_DATA);
+			if (!p)
+				goto syntax_error;
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "PIO") || !strtokencmp(p, "PIOMAP")) {
+			goto unsupported_error;
+		}
+
+		if (!strtokencmp(p, "RUNTEST")) {
+			p += strtokenskip(p);
+			int tck_count = -1;
+			int sck_count = -1;
+			int min_time = -1;
+			int max_time = -1;
+			while (*p) {
+				int got_maximum = 0;
+				if (!strtokencmp(p, "MAXIMUM")) {
+					p += strtokenskip(p);
+					got_maximum = 1;
+				}
+				int got_endstate = 0;
+				if (!strtokencmp(p, "ENDSTATE")) {
+					p += strtokenskip(p);
+					got_endstate = 1;
+				}
+				int st = token2tapstate(p);
+				if (st >= 0) {
+					p += strtokenskip(p);
+					if (got_endstate)
+						state_endrun = st;
+					else
+						state_run = st;
+					continue;
+				}
+				if (*p < '0' || *p > '9')
+					goto syntax_error;
+				int number = 0;
+				int exp = 0, expsign = 1;
+				int number_e6, exp_e6;
+				while (*p >= '0' && *p <= '9') {
+					number = number*10 + (*p - '0');
+					p++;
+				}
+				if(*p == 'E' || *p == 'e') {
+					p++;
+					if(*p == '-') {
+						expsign = -1;
+						p++;
+					}
+					while (*p >= '0' && *p <= '9') {
+						exp = exp*10 + (*p - '0');
+						p++;
+					}
+					exp = exp * expsign;
+					number_e6 = number;
+					exp_e6 = exp + 6;
+					while (exp < 0) {
+						number /= 10;
+						exp++;
+					}
+					while (exp > 0) {
+						number *= 10;
+						exp--;
+					}
+					while (exp_e6 < 0) {
+						number_e6 /= 10;
+						exp_e6++;
+					}
+					while (exp_e6 > 0) {
+						number_e6 *= 10;
+						exp_e6--;
+					}
+				} else {
+					number_e6 = number * 1000000;
+				}
+				while (*p == ' ') {
+					p++;
+				}
+				if (!strtokencmp(p, "SEC")) {
+					p += strtokenskip(p);
+					if (got_maximum)
+						max_time = number_e6;
+					else
+						min_time = number_e6;
+					continue;
+				}
+				if (!strtokencmp(p, "TCK")) {
+					p += strtokenskip(p);
+					tck_count = number;
+					continue;
+				}
+				if (!strtokencmp(p, "SCK")) {
+					p += strtokenskip(p);
+					sck_count = number;
+					continue;
+				}
+				goto syntax_error;
+			}
+			if (libxsvf_tap_walk(h, state_run) < 0)
+				goto error;
+			if (max_time >= 0) {
+				LIBXSVF_HOST_REPORT_ERROR("WARNING: Maximum time in SVF RUNTEST command is ignored.");
+			}
+			if (sck_count >= 0) {
+				for (i=0; i < sck_count; i++) {
+					LIBXSVF_HOST_PULSE_SCK();
+				}
+			}
+			if (min_time >= 0 || tck_count >= 0) {
+				LIBXSVF_HOST_UDELAY(min_time >= 0 ? min_time : 0, 0, tck_count >= 0 ? tck_count : 0);
+			}
+			if (libxsvf_tap_walk(h, state_endrun) < 0)
+				goto error;
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "SDR")) {
+			p += strtokenskip(p);
+			p = bitdata_parse(h, p, &bd_sdr, LIBXSVF_MEM_SVF_SDR_TDI_DATA);
+			if (!p)
+				goto syntax_error;
+			if (libxsvf_tap_walk(h, LIBXSVF_TAP_DRSHIFT) < 0)
+				goto error;
+			if (bitdata_play(h, &bd_hdr, bd_sdr.len+bd_tdr.len > 0 ? LIBXSVF_TAP_DRSHIFT : state_enddr) < 0)
+				goto error;
+			if (bitdata_play(h, &bd_sdr, bd_tdr.len > 0 ? LIBXSVF_TAP_DRSHIFT : state_enddr) < 0)
+				goto error;
+			if (bitdata_play(h, &bd_tdr, state_enddr) < 0)
+				goto error;
+			if (libxsvf_tap_walk(h, state_enddr) < 0)
+				goto error;
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "SIR")) {
+			p += strtokenskip(p);
+			p = bitdata_parse(h, p, &bd_sir, LIBXSVF_MEM_SVF_SIR_TDI_DATA);
+			if (!p)
+				goto syntax_error;
+			if (libxsvf_tap_walk(h, LIBXSVF_TAP_IRSHIFT) < 0)
+				goto error;
+			if (bitdata_play(h, &bd_hir, bd_sir.len+bd_tir.len > 0 ? LIBXSVF_TAP_IRSHIFT : state_endir) < 0)
+				goto error;
+			if (bitdata_play(h, &bd_sir, bd_tir.len > 0 ? LIBXSVF_TAP_IRSHIFT : state_endir) < 0)
+				goto error;
+			if (bitdata_play(h, &bd_tir, state_endir) < 0)
+				goto error;
+			if (libxsvf_tap_walk(h, state_endir) < 0)
+				goto error;
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "STATE")) {
+			p += strtokenskip(p);
+			while (*p) {
+				int st = token2tapstate(p);
+				if (st < 0)
+					goto syntax_error;
+				if (libxsvf_tap_walk(h, st) < 0)
+					goto error;
+				p += strtokenskip(p);
+			}
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "TDR")) {
+			p += strtokenskip(p);
+			p = bitdata_parse(h, p, &bd_tdr, LIBXSVF_MEM_SVF_TDR_TDI_DATA);
+			if (!p)
+				goto syntax_error;
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "TIR")) {
+			p += strtokenskip(p);
+			p = bitdata_parse(h, p, &bd_tir, LIBXSVF_MEM_SVF_TIR_TDI_DATA);
+			if (!p)
+				goto syntax_error;
+			goto eol_check;
+		}
+
+		if (!strtokencmp(p, "TRST")) {
+			p += strtokenskip(p);
+			if (!strtokencmp(p, "ON")) {
+				p += strtokenskip(p);
+				LIBXSVF_HOST_SET_TRST(1);
+				goto eol_check;
+			}
+			if (!strtokencmp(p, "OFF")) {
+				p += strtokenskip(p);
+				LIBXSVF_HOST_SET_TRST(0);
+				goto eol_check;
+			}
+			if (!strtokencmp(p, "Z")) {
+				p += strtokenskip(p);
+				LIBXSVF_HOST_SET_TRST(-1);
+				goto eol_check;
+			}
+			if (!strtokencmp(p, "ABSENT")) {
+				p += strtokenskip(p);
+				LIBXSVF_HOST_SET_TRST(-2);
+				goto eol_check;
+			}
+			goto syntax_error;
+		}
+
+eol_check:
+		while (*p == ' ')
+			p++;
+		if (*p == 0)
+			continue;
+
+syntax_error:
+		LIBXSVF_HOST_REPORT_ERROR("SVF Syntax Error:");
+		if (0) {
+unsupported_error:
+			LIBXSVF_HOST_REPORT_ERROR("Error in SVF input: unsupported command:");
+		}
+		LIBXSVF_HOST_REPORT_ERROR(command_buffer);
+error:
+		rc = -1;
+		break;
+	}
+
+	if (LIBXSVF_HOST_SYNC() != 0 && rc >= 0 ) {
+		LIBXSVF_HOST_REPORT_ERROR("TDO mismatch.");
+		rc = -1;
+	}
+
+	bitdata_free(h, &bd_hdr, LIBXSVF_MEM_SVF_HDR_TDI_DATA);
+	bitdata_free(h, &bd_hir, LIBXSVF_MEM_SVF_HIR_TDI_DATA);
+	bitdata_free(h, &bd_tdr, LIBXSVF_MEM_SVF_TDR_TDI_DATA);
+	bitdata_free(h, &bd_tir, LIBXSVF_MEM_SVF_TIR_TDI_DATA);
+	bitdata_free(h, &bd_sdr, LIBXSVF_MEM_SVF_SDR_TDI_DATA);
+	bitdata_free(h, &bd_sir, LIBXSVF_MEM_SVF_SIR_TDI_DATA);
+
+	LIBXSVF_HOST_REALLOC(command_buffer, 0, LIBXSVF_MEM_SVF_COMMANDBUF);
+
+	return rc;
+}
+
diff --git a/FamicomDumperBootloader/Core/Src/libxsvf/tap.c b/FamicomDumperBootloader/Core/Src/libxsvf/tap.c
new file mode 100644
index 0000000..c258fde
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/libxsvf/tap.c
@@ -0,0 +1,173 @@
+/*
+ *  Lib(X)SVF  -  A library for implementing SVF and XSVF JTAG players
+ *
+ *  Copyright (C) 2009  RIEGL Research ForschungsGmbH
+ *  Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
+ *  
+ *  Permission to use, copy, modify, and/or distribute this software for any
+ *  purpose with or without fee is hereby granted, provided that the above
+ *  copyright notice and this permission notice appear in all copies.
+ *  
+ *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#include "libxsvf.h"
+
+static void tap_transition(struct libxsvf_host *h, int v)
+{
+	LIBXSVF_HOST_PULSE_TCK(v, -1, -1, 0, 0);
+}
+
+int libxsvf_tap_walk(struct libxsvf_host *h, enum libxsvf_tap_state s)
+{
+	int i, j;
+	for (i=0; s != h->tap_state; i++)
+	{
+		switch (h->tap_state)
+		{
+		/* Special States */
+		case LIBXSVF_TAP_INIT:
+			for (j = 0; j < 6; j++)
+				tap_transition(h, 1);
+			h->tap_state = LIBXSVF_TAP_RESET;
+			break;
+		case LIBXSVF_TAP_RESET:
+			tap_transition(h, 0);
+			h->tap_state = LIBXSVF_TAP_IDLE;
+			break;
+		case LIBXSVF_TAP_IDLE:
+			tap_transition(h, 1);
+			h->tap_state = LIBXSVF_TAP_DRSELECT;
+			break;
+
+		/* DR States */
+		case LIBXSVF_TAP_DRSELECT:
+			if (s >= LIBXSVF_TAP_IRSELECT || s == LIBXSVF_TAP_RESET) {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_IRSELECT;
+			} else {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_DRCAPTURE;
+			}
+			break;
+		case LIBXSVF_TAP_DRCAPTURE:
+			if (s == LIBXSVF_TAP_DRSHIFT) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_DRSHIFT;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_DREXIT1;
+			}
+			break;
+		case LIBXSVF_TAP_DRSHIFT:
+			tap_transition(h, 1);
+			h->tap_state = LIBXSVF_TAP_DREXIT1;
+			break;
+		case LIBXSVF_TAP_DREXIT1:
+			if (s == LIBXSVF_TAP_DRPAUSE) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_DRPAUSE;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_DRUPDATE;
+			}
+			break;
+		case LIBXSVF_TAP_DRPAUSE:
+			tap_transition(h, 1);
+			h->tap_state = LIBXSVF_TAP_DREXIT2;
+			break;
+		case LIBXSVF_TAP_DREXIT2:
+			if (s == LIBXSVF_TAP_DRSHIFT) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_DRSHIFT;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_DRUPDATE;
+			}
+			break;
+		case LIBXSVF_TAP_DRUPDATE:
+			if (s == LIBXSVF_TAP_IDLE) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_IDLE;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_DRSELECT;
+			}
+			break;
+
+		/* IR States */
+		case LIBXSVF_TAP_IRSELECT:
+			if (s == LIBXSVF_TAP_RESET) {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_RESET;
+			} else {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_IRCAPTURE;
+			}
+			break;
+		case LIBXSVF_TAP_IRCAPTURE:
+			if (s == LIBXSVF_TAP_IRSHIFT) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_IRSHIFT;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_IREXIT1;
+			}
+			break;
+		case LIBXSVF_TAP_IRSHIFT:
+			tap_transition(h, 1);
+			h->tap_state = LIBXSVF_TAP_IREXIT1;
+			break;
+		case LIBXSVF_TAP_IREXIT1:
+			if (s == LIBXSVF_TAP_IRPAUSE) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_IRPAUSE;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_IRUPDATE;
+			}
+			break;
+		case LIBXSVF_TAP_IRPAUSE:
+			tap_transition(h, 1);
+			h->tap_state = LIBXSVF_TAP_IREXIT2;
+			break;
+		case LIBXSVF_TAP_IREXIT2:
+			if (s == LIBXSVF_TAP_IRSHIFT) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_IRSHIFT;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_IRUPDATE;
+			}
+			break;
+		case LIBXSVF_TAP_IRUPDATE:
+			if (s == LIBXSVF_TAP_IDLE) {
+				tap_transition(h, 0);
+				h->tap_state = LIBXSVF_TAP_IDLE;
+			} else {
+				tap_transition(h, 1);
+				h->tap_state = LIBXSVF_TAP_DRSELECT;
+			}
+			break;
+
+		default:
+			LIBXSVF_HOST_REPORT_ERROR("Illegal tap state.");
+			return -1;
+		}
+		if (h->report_tapstate)
+			LIBXSVF_HOST_REPORT_TAPSTATE();
+		if (i>10) {
+			LIBXSVF_HOST_REPORT_ERROR("Loop in tap walker.");
+			return -1;
+		}
+	}
+
+	return 0;
+}
diff --git a/FamicomDumperBootloader/Core/Src/libxsvf/xsvf.c b/FamicomDumperBootloader/Core/Src/libxsvf/xsvf.c
new file mode 100644
index 0000000..e6823f6
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/libxsvf/xsvf.c
@@ -0,0 +1,486 @@
+/*
+ *  Lib(X)SVF  -  A library for implementing SVF and XSVF JTAG players
+ *
+ *  Copyright (C) 2009  RIEGL Research ForschungsGmbH
+ *  Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
+ *  
+ *  Permission to use, copy, modify, and/or distribute this software for any
+ *  purpose with or without fee is hereby granted, provided that the above
+ *  copyright notice and this permission notice appear in all copies.
+ *  
+ *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#include "libxsvf.h"
+
+/* command codes as defined in xilinx xapp503 */
+enum xsvf_cmd {
+	XCOMPLETE       = 0x00,
+	XTDOMASK        = 0x01,
+	XSIR            = 0x02,
+	XSDR            = 0x03,
+	XRUNTEST        = 0x04,
+	XREPEAT         = 0x07,
+	XSDRSIZE        = 0x08,
+	XSDRTDO         = 0x09,
+	XSETSDRMASKS    = 0x0A,
+	XSDRINC         = 0x0B,
+	XSDRB           = 0x0C,
+	XSDRC           = 0x0D,
+	XSDRE           = 0x0E,
+	XSDRTDOB        = 0x0F,
+	XSDRTDOC        = 0x10,
+	XSDRTDOE        = 0x11,
+	XSTATE          = 0x12,
+	XENDIR          = 0x13,
+	XENDDR          = 0x14,
+	XSIR2           = 0x15,
+	XCOMMENT        = 0x16,
+	XWAIT           = 0x17,
+	/* Extensions used in svf2xsvf.py */
+	XWAITSTATE      = 0x18,
+	XTRST           = 0x1c
+};
+
+// This is to not confuse the VIM syntax highlighting
+#define VAL_OPEN (
+#define VAL_CLOSE )
+
+#define READ_BITS(_buf, _len) do {                                          \
+	unsigned char *_p = _buf; int _i;                                   \
+	for (_i=0; _i<(_len); _i+=8) {                                      \
+		int tmp = LIBXSVF_HOST_GETBYTE();                           \
+		if (tmp < 0) {                                              \
+			LIBXSVF_HOST_REPORT_ERROR("Unexpected EOF.");       \
+			goto error;                                         \
+		}                                                           \
+		*(_p++) = tmp;                                              \
+	}                                                                   \
+} while (0)
+
+#define READ_LONG() VAL_OPEN{                                               \
+	long _buf = 0; int _i;                                              \
+	for (_i=0; _i<4; _i++) {                                            \
+		int tmp = LIBXSVF_HOST_GETBYTE();                           \
+		if (tmp < 0) {                                              \
+			LIBXSVF_HOST_REPORT_ERROR("Unexpected EOF.");       \
+			goto error;                                         \
+		}                                                           \
+		_buf = _buf << 8 | tmp;                                     \
+	}                                                                   \
+	_buf;                                                               \
+}VAL_CLOSE
+
+#define READ_BYTE() VAL_OPEN{                                               \
+	int _tmp = LIBXSVF_HOST_GETBYTE();                                  \
+	if (_tmp < 0) {                                                     \
+		LIBXSVF_HOST_REPORT_ERROR("Unexpected EOF.");               \
+		goto error;                                                 \
+	}                                                                   \
+	_tmp;                                                               \
+}VAL_CLOSE
+
+#define SHIFT_DATA(_inp, _outp, _maskp, _len, _state, _estate, _edelay, _ret) do { \
+	if (shift_data(h, _inp, _outp, _maskp, _len, _state, _estate, _edelay, _ret) < 0) { \
+		goto error;                                                 \
+	}                                                                   \
+} while (0)
+
+#define TAP(_state) do {                                                    \
+	if (libxsvf_tap_walk(h, _state) < 0)                                \
+		goto error;                                                 \
+} while (0)
+
+static int bits2bytes(int bits)
+{
+	return (bits+7) / 8;
+}
+
+static int getbit(unsigned char *data, int n)
+{
+	return (data[n/8] & (1 << (7 - n%8))) ? 1 : 0;
+}
+
+static void setbit(unsigned char *data, int n, int v)
+{
+	unsigned char mask = 1 << (7 - n%8);
+	if (v)
+		data[n/8] |= mask;
+	else
+		data[n/8] &= ~mask;
+}
+
+static int xilinx_tap(int state)
+{
+	/* state codes as defined in xilinx xapp503 */
+	switch (state)
+	{
+	case 0x00:
+		return LIBXSVF_TAP_RESET;
+		break;
+	case 0x01:
+		return LIBXSVF_TAP_IDLE;
+		break;
+	case 0x02:
+		return LIBXSVF_TAP_DRSELECT;
+		break;
+	case 0x03:
+		return LIBXSVF_TAP_DRCAPTURE;
+		break;
+	case 0x04:
+		return LIBXSVF_TAP_DRSHIFT;
+		break;
+	case 0x05:
+		return LIBXSVF_TAP_DREXIT1;
+		break;
+	case 0x06:
+		return LIBXSVF_TAP_DRPAUSE;
+		break;
+	case 0x07:
+		return LIBXSVF_TAP_DREXIT2;
+		break;
+	case 0x08:
+		return LIBXSVF_TAP_DRUPDATE;
+		break;
+	case 0x09:
+		return LIBXSVF_TAP_IRSELECT;
+		break;
+	case 0x0A:
+		return LIBXSVF_TAP_IRCAPTURE;
+		break;
+	case 0x0B:
+		return LIBXSVF_TAP_IRSHIFT;
+		break;
+	case 0x0C:
+		return LIBXSVF_TAP_IREXIT1;
+		break;
+	case 0x0D:
+		return LIBXSVF_TAP_IRPAUSE;
+		break;
+	case 0x0E:
+		return LIBXSVF_TAP_IREXIT2;
+		break;
+	case 0x0F:
+		return LIBXSVF_TAP_IRUPDATE;
+		break;
+	}
+	return -1;
+}
+
+static int shift_data(struct libxsvf_host *h, unsigned char *inp, unsigned char *outp, unsigned char *maskp, int len, enum libxsvf_tap_state state, enum libxsvf_tap_state estate, int edelay, int retries)
+{
+	int left_padding = (8 - len % 8) % 8;
+	int with_retries = retries > 0;
+	int i;
+
+	if (with_retries && LIBXSVF_HOST_SYNC() < 0) {
+		LIBXSVF_HOST_REPORT_ERROR("TDO mismatch.");
+		return -1;
+	}
+
+	while (1)
+	{
+		int tdo_error = 0;
+		int tms = 0;
+
+		TAP(state);
+		tms = 0;
+
+		for (i=len+left_padding-1; i>=left_padding; i--) {
+			if (i == left_padding && h->tap_state != estate) {
+				h->tap_state++;
+				tms = 1;
+			}
+			int tdi = getbit(inp, i);
+			int tdo = -1;
+			if (maskp && getbit(maskp, i))
+				tdo = outp && getbit(outp, i);
+			int sync = with_retries && i == left_padding;
+			if (LIBXSVF_HOST_PULSE_TCK(tms, tdi, tdo, 0, sync) < 0)
+				tdo_error = 1;
+		}
+
+		if (tms)
+			LIBXSVF_HOST_REPORT_TAPSTATE();
+	
+		if (edelay) {
+			TAP(LIBXSVF_TAP_IDLE);
+			LIBXSVF_HOST_UDELAY(edelay, 0, edelay);
+		} else {
+			TAP(estate);
+		}
+
+		if (!tdo_error)
+			return 0;
+
+		if (retries <= 0) {
+			LIBXSVF_HOST_REPORT_ERROR("TDO mismatch.");
+			return -1;
+		}
+
+		retries--;
+	}
+
+error:
+	return -1;
+}
+
+int libxsvf_xsvf(struct libxsvf_host *h)
+{
+	int rc = 0;
+	int i, j;
+
+	unsigned char *buf_tdi_data = (void*)0;
+	unsigned char *buf_tdo_data = (void*)0;
+	unsigned char *buf_tdo_mask = (void*)0;
+	unsigned char *buf_addr_mask = (void*)0;
+	unsigned char *buf_data_mask = (void*)0;
+
+	long state_dr_size = 0;
+	long state_data_size = 0;
+	long state_runtest = 0;
+	unsigned char state_xendir = 0;
+	unsigned char state_xenddr = 0;
+	unsigned char state_retries = 0;
+	unsigned char cmd = 0;
+
+	while (1)
+	{
+		unsigned char last_cmd = cmd;
+		cmd = LIBXSVF_HOST_GETBYTE();
+
+#define STATUS(_c) LIBXSVF_HOST_REPORT_STATUS("XSVF Command " #_c);
+
+		switch (cmd)
+		{
+		case XCOMPLETE: {
+			STATUS(XCOMPLETE);
+			goto got_complete_command;
+		  }
+		case XTDOMASK: {
+			STATUS(XTDOMASK);
+			READ_BITS(buf_tdo_mask, state_dr_size);
+			break;
+		  }
+		case XSIR: {
+			STATUS(XSIR);
+			int length = READ_BYTE();
+			unsigned char buf[bits2bytes(length)];
+			READ_BITS(buf, length);
+			SHIFT_DATA(buf, (void*)0, (void*)0, length, LIBXSVF_TAP_IRSHIFT,
+					state_xendir ? LIBXSVF_TAP_IRPAUSE : LIBXSVF_TAP_IDLE,
+					state_runtest, state_retries);
+			break;
+		  }
+		case XSDR: {
+			STATUS(XSDR);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, buf_tdo_data, buf_tdo_mask, state_dr_size, LIBXSVF_TAP_DRSHIFT,
+					state_xenddr ? LIBXSVF_TAP_DRPAUSE : LIBXSVF_TAP_IDLE,
+					state_runtest, state_retries);
+			break;
+		  }
+		case XRUNTEST: {
+			STATUS(XRUNTEST);
+			state_runtest = READ_LONG();
+			break;
+		  }
+		case XREPEAT: {
+			STATUS(XREPEAT);
+			state_retries = READ_BYTE();
+			break;
+		  }
+		case XSDRSIZE: {
+			STATUS(XSDRSIZE);
+			state_dr_size = READ_LONG();
+			buf_tdi_data = LIBXSVF_HOST_REALLOC(buf_tdi_data, bits2bytes(state_dr_size), LIBXSVF_MEM_XSVF_TDI_DATA);
+			buf_tdo_data = LIBXSVF_HOST_REALLOC(buf_tdo_data, bits2bytes(state_dr_size), LIBXSVF_MEM_XSVF_TDO_DATA);
+			buf_tdo_mask = LIBXSVF_HOST_REALLOC(buf_tdo_mask, bits2bytes(state_dr_size), LIBXSVF_MEM_XSVF_TDO_MASK);
+			buf_addr_mask = LIBXSVF_HOST_REALLOC(buf_addr_mask, bits2bytes(state_dr_size), LIBXSVF_MEM_XSVF_ADDR_MASK);
+			buf_data_mask = LIBXSVF_HOST_REALLOC(buf_data_mask, bits2bytes(state_dr_size), LIBXSVF_MEM_XSVF_DATA_MASK);
+			if (!buf_tdi_data || !buf_tdo_data || !buf_tdo_mask || !buf_addr_mask || !buf_data_mask) {
+				LIBXSVF_HOST_REPORT_ERROR("Allocating memory failed.");
+				goto error;
+			}
+			break;
+		  }
+		case XSDRTDO: {
+			STATUS(XSDRTDO);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			READ_BITS(buf_tdo_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, buf_tdo_data, buf_tdo_mask, state_dr_size, LIBXSVF_TAP_DRSHIFT,
+					state_xenddr ? LIBXSVF_TAP_DRPAUSE : LIBXSVF_TAP_IDLE,
+					state_runtest, state_retries);
+			break;
+		  }
+		case XSETSDRMASKS: {
+			STATUS(XSETSDRMASKS);
+			READ_BITS(buf_addr_mask, state_dr_size);
+			READ_BITS(buf_data_mask, state_dr_size);
+			state_data_size = 0;
+			for (i=0; i<state_dr_size; i++)
+				state_data_size += getbit(buf_data_mask, i);
+			break;
+		  }
+		case XSDRINC: {
+			STATUS(XSDRINC);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			int num = READ_BYTE();
+			while (1) {
+				SHIFT_DATA(buf_tdi_data, buf_tdo_data, buf_tdo_mask, state_dr_size, LIBXSVF_TAP_DRSHIFT,
+						state_xenddr ? LIBXSVF_TAP_DRPAUSE : LIBXSVF_TAP_IDLE,
+						state_runtest, state_retries);
+				if (num-- <= 0)
+					break;
+				int carry = 1;
+				for (i=state_dr_size-1; i>=0; i--) {
+					if (getbit(buf_addr_mask, i) == 0)
+						continue;
+					if (getbit(buf_tdi_data, i)) {
+						setbit(buf_tdi_data, i, !carry);
+					} else {
+						setbit(buf_tdi_data, i, carry);
+						carry = 0;
+					}
+				}
+				unsigned char this_byte = 0;
+				for (i=0, j=0; i<state_data_size; i++) {
+					if (i%8 == 0)
+						this_byte = READ_BYTE();
+					while (getbit(buf_data_mask, j) == 0)
+						j++;
+					setbit(buf_tdi_data, j++, getbit(&this_byte, i%8));
+				}
+			}
+			break;
+		  }
+		case XSDRB: {
+			STATUS(XSDRB);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, (void*)0, (void*)0, state_dr_size, LIBXSVF_TAP_DRSHIFT, LIBXSVF_TAP_DRSHIFT, 0, 0);
+			break;
+		  }
+		case XSDRC: {
+			STATUS(XSDRC);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, (void*)0, (void*)0, state_dr_size, LIBXSVF_TAP_DRSHIFT, LIBXSVF_TAP_DRSHIFT, 0, 0);
+			break;
+		  }
+		case XSDRE: {
+			STATUS(XSDRE);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, (void*)0, (void*)0, state_dr_size, LIBXSVF_TAP_DRSHIFT,
+					state_xenddr ? LIBXSVF_TAP_DRPAUSE : LIBXSVF_TAP_IDLE, 0, 0);
+			break;
+		  }
+		case XSDRTDOB: {
+			STATUS(XSDRTDOB);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			READ_BITS(buf_tdo_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, buf_tdo_data, (void*)0, state_dr_size, LIBXSVF_TAP_DRSHIFT, LIBXSVF_TAP_DRSHIFT, 0, 0);
+			break;
+		  }
+		case XSDRTDOC: {
+			STATUS(XSDRTDOC);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			READ_BITS(buf_tdo_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, buf_tdo_data, (void*)0, state_dr_size, LIBXSVF_TAP_DRSHIFT, LIBXSVF_TAP_DRSHIFT, 0, 0);
+			break;
+		  }
+		case XSDRTDOE: {
+			STATUS(XSDRTDOE);
+			READ_BITS(buf_tdi_data, state_dr_size);
+			READ_BITS(buf_tdo_data, state_dr_size);
+			SHIFT_DATA(buf_tdi_data, buf_tdo_data, (void*)0, state_dr_size, LIBXSVF_TAP_DRSHIFT,
+					state_xenddr ? LIBXSVF_TAP_DRPAUSE : LIBXSVF_TAP_IDLE, 0, 0);
+			break;
+		  }
+		case XSTATE: {
+			STATUS(XSTATE);
+			if (state_runtest && last_cmd == XRUNTEST) {
+				TAP(LIBXSVF_TAP_IDLE);
+				LIBXSVF_HOST_UDELAY(state_runtest, 0, state_runtest);
+			}
+			unsigned char state = READ_BYTE();
+			TAP(xilinx_tap(state));
+			break;
+		  }
+		case XENDIR: {
+			STATUS(XENDIR);
+			state_xendir = READ_BYTE();
+			break;
+		  }
+		case XENDDR: {
+			STATUS(XENDDR);
+			state_xenddr = READ_BYTE();
+			break;
+		  }
+		case XSIR2: {
+			STATUS(XSIR2);
+			int length = READ_BYTE();
+			length = length << 8 | READ_BYTE();
+			unsigned char buf[bits2bytes(length)];
+			READ_BITS(buf, length);
+			SHIFT_DATA(buf, (void*)0, (void*)0, length, LIBXSVF_TAP_IRSHIFT,
+					state_xendir ? LIBXSVF_TAP_IRPAUSE : LIBXSVF_TAP_IDLE,
+					state_runtest, state_retries);
+			break;
+		  }
+		case XCOMMENT: {
+			STATUS(XCOMMENT);
+			unsigned char this_byte;
+			do {
+				this_byte = READ_BYTE();
+			} while (this_byte);
+			break;
+		  }
+		case XWAIT:
+		case XWAITSTATE: {
+			STATUS(XWAIT);
+			unsigned char state1 = READ_BYTE();
+			unsigned char state2 = READ_BYTE();
+			long usecs = READ_LONG();
+			TAP(xilinx_tap(state1));
+			LIBXSVF_HOST_UDELAY(usecs, 0, 0);
+			TAP(xilinx_tap(state2));
+			if (cmd==XWAITSTATE) {
+				READ_LONG();   /* XWAITSTATE has count, time arguments */
+			}
+			break;
+		  }
+		case XTRST: {
+			STATUS(XTRST);
+			READ_BYTE();  /* enum: ON, OFF, Z, ABSENT */
+			break;
+		}
+		default:
+			LIBXSVF_HOST_REPORT_ERROR("Unknown XSVF command.");
+			goto error;
+		}
+	}
+
+error:
+	rc = -1;
+
+got_complete_command:
+	if (LIBXSVF_HOST_SYNC() != 0 && rc >= 0 ) {
+		LIBXSVF_HOST_REPORT_ERROR("TDO mismatch.");
+		rc = -1;
+	}
+
+	LIBXSVF_HOST_REALLOC(buf_tdi_data, 0, LIBXSVF_MEM_XSVF_TDI_DATA);
+	LIBXSVF_HOST_REALLOC(buf_tdo_data, 0, LIBXSVF_MEM_XSVF_TDO_DATA);
+	LIBXSVF_HOST_REALLOC(buf_tdo_mask, 0, LIBXSVF_MEM_XSVF_TDO_MASK);
+	LIBXSVF_HOST_REALLOC(buf_addr_mask, 0, LIBXSVF_MEM_XSVF_ADDR_MASK);
+	LIBXSVF_HOST_REALLOC(buf_data_mask, 0, LIBXSVF_MEM_XSVF_DATA_MASK);
+
+	return rc;
+}
+
diff --git a/FamicomDumperBootloader/Core/Src/main.c b/FamicomDumperBootloader/Core/Src/main.c
new file mode 100644
index 0000000..adac80e
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/main.c
@@ -0,0 +1,544 @@
+/* USER CODE BEGIN Header */
+/**
+ ******************************************************************************
+ * @file           : main.c
+ * @brief          : Main program body
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
+ * All rights reserved.</center></h2>
+ *
+ * This software component is licensed by ST under BSD 3-Clause license,
+ * the "License"; You may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at:
+ *                        opensource.org/licenses/BSD-3-Clause
+ *
+ ******************************************************************************
+ */
+/* USER CODE END Header */
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+#include "fatfs.h"
+#include "usb_device.h"
+
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+#include "bootloader.h"
+#include "usbd_core.h"
+#include "firmwriter.h"
+#include "cpldwriter.h"
+#include "led.h"
+#include <string.h>
+/* USER CODE END Includes */
+
+/* Private typedef -----------------------------------------------------------*/
+/* USER CODE BEGIN PTD */
+
+/* USER CODE END PTD */
+
+/* Private define ------------------------------------------------------------*/
+/* USER CODE BEGIN PD */
+/* USER CODE END PD */
+
+/* Private macro -------------------------------------------------------------*/
+/* USER CODE BEGIN PM */
+
+/* USER CODE END PM */
+
+/* Private variables ---------------------------------------------------------*/
+TIM_HandleTypeDef htim1;
+TIM_HandleTypeDef htim2;
+TIM_HandleTypeDef htim5;
+DMA_HandleTypeDef hdma_tim5_ch3_up;
+
+/* USER CODE BEGIN PV */
+extern USBD_HandleTypeDef hUsbDeviceFS;
+uint32_t last_write_ts = 0;
+/* USER CODE END PV */
+
+/* Private function prototypes -----------------------------------------------*/
+void SystemClock_Config(void);
+static void MX_GPIO_Init(void);
+static void MX_DMA_Init(void);
+static void MX_TIM5_Init(void);
+static void MX_TIM2_Init(void);
+static void MX_TIM1_Init(void);
+/* USER CODE BEGIN PFP */
+
+/* USER CODE END PFP */
+
+/* Private user code ---------------------------------------------------------*/
+/* USER CODE BEGIN 0 */
+
+static void start_app(void)
+{
+  // App start address
+  void (*jmp_to_app)(void) = (void (*)(void))(*((volatile uint32_t*)(APP_ADDRESS + 4)));
+  // Vector table
+  SCB->VTOR = APP_ADDRESS;
+  // Stack pointer (to RAM) for USER app in this address
+  __set_MSP(*((volatile uint32_t*) APP_ADDRESS));
+  jmp_to_app();
+
+  // Should not execute
+  while (1)
+  {}
+}
+
+void error(void)
+{
+  // Dispose resources, show red LED and stop
+  f_mount(NULL, "", 1);
+  HAL_FLASH_Lock();
+  led_red();
+  HAL_Delay(50);
+  __disable_irq();
+  while (1)
+  {
+  }
+}
+
+/* USER CODE END 0 */
+
+/**
+  * @brief  The application entry point.
+  * @retval int
+  */
+int main(void)
+{
+  /* USER CODE BEGIN 1 */
+
+  /* USER CODE END 1 */
+
+  /* MCU Configuration--------------------------------------------------------*/
+
+  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+  HAL_Init();
+
+  /* USER CODE BEGIN Init */
+
+  /* USER CODE END Init */
+
+  /* Configure the system clock */
+  SystemClock_Config();
+
+  /* USER CODE BEGIN SysInit */
+  // Pre-init
+  MX_GPIO_Init();
+  MX_DMA_Init();
+  MX_TIM5_Init();
+  MX_TIM2_Init();
+  write_hardware_version();
+  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
+  HAL_Delay(10);
+  // If IRQ pin not grounded - start app
+#ifndef DEBUG
+  if (HAL_GPIO_ReadPin(IRQ_GPIO_Port, IRQ_Pin)) start_app();
+#endif
+  // Green LED
+  led_green();
+  // Need to release pin during 1/2 second
+  HAL_Delay(500);
+  // If IRQ pin not released...
+  if (!HAL_GPIO_ReadPin(IRQ_GPIO_Port, IRQ_Pin))
+  {
+    // Turn off the LED
+    led_off();
+    HAL_Delay(50);
+    // Start app
+    start_app();
+  }
+  /* USER CODE END SysInit */
+
+  /* Initialize all configured peripherals */
+  MX_GPIO_Init();
+  MX_DMA_Init();
+  MX_USB_DEVICE_Init();
+  MX_TIM5_Init();
+  MX_FATFS_Init();
+  MX_TIM2_Init();
+  MX_TIM1_Init();
+  /* USER CODE BEGIN 2 */
+  // Microsecond counter
+  HAL_TIM_Base_Start(&htim1);
+  // M2
+  HAL_TIM_Base_Start(&htim2);
+  // Yellow LED
+  led_yellow();
+  /* USER CODE END 2 */
+
+  /* Infinite loop */
+  /* USER CODE BEGIN WHILE */
+  while (1)
+  {
+    // Check if last write operation was long time ago
+    if (last_write_ts && (HAL_GetTick() - last_write_ts >= WRITE_TO_FLASH_TIME))
+    {
+      FILINFO fno;
+      int usb_stopped = 0; // to prevent double deinit
+      if (find_svf_file(&fno))
+      {
+        // White LED
+        led_white();
+        // Disable USB MSD
+        USBD_DeInit(&hUsbDeviceFS);
+        usb_stopped = 1;
+        // Write firmware
+        write_cpld(&fno);
+        // Turn off the LED
+        led_green();
+        HAL_Delay(50);
+        // Stop
+      }
+      if (find_bin_file(&fno))
+      {
+        // White LED
+        led_white();
+        // Disable USB MSD
+        if (!usb_stopped) USBD_DeInit(&hUsbDeviceFS);
+        // Write firmware
+        write_firmware(&fno);
+        // Turn off the LED
+        led_green();
+        HAL_Delay(50);
+        // Stop
+        __disable_irq();
+        while (1)
+        {
+        }
+      }
+
+      last_write_ts = 0;
+    }
+    HAL_Delay(100);
+    /* USER CODE END WHILE */
+
+    /* USER CODE BEGIN 3 */
+  }
+  /* USER CODE END 3 */
+}
+
+/**
+  * @brief System Clock Configuration
+  * @retval None
+  */
+void SystemClock_Config(void)
+{
+  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
+
+  /** Initializes the RCC Oscillators according to the specified parameters
+  * in the RCC_OscInitTypeDef structure.
+  */
+  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
+  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
+  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV2;
+  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
+  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
+  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  /** Initializes the CPU, AHB and APB buses clocks
+  */
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
+  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
+  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
+
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
+  PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5;
+  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  HAL_RCC_MCOConfig(RCC_MCO, RCC_MCO1SOURCE_PLLCLK, RCC_MCODIV_1);
+}
+
+/**
+  * @brief TIM1 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM1_Init(void)
+{
+
+  /* USER CODE BEGIN TIM1_Init 0 */
+
+  /* USER CODE END TIM1_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM1_Init 1 */
+
+  /* USER CODE END TIM1_Init 1 */
+  htim1.Instance = TIM1;
+  htim1.Init.Prescaler = 71;
+  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim1.Init.Period = 65535;
+  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim1.Init.RepetitionCounter = 0;
+  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM1_Init 2 */
+
+  /* USER CODE END TIM1_Init 2 */
+
+}
+
+/**
+  * @brief TIM2 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM2_Init(void)
+{
+
+  /* USER CODE BEGIN TIM2_Init 0 */
+
+  /* USER CODE END TIM2_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
+
+  /* USER CODE BEGIN TIM2_Init 1 */
+
+  /* USER CODE END TIM2_Init 1 */
+  htim2.Instance = TIM2;
+  htim2.Init.Prescaler = 0;
+  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim2.Init.Period = 39;
+  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 19;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM2_Init 2 */
+
+  /* USER CODE END TIM2_Init 2 */
+  HAL_TIM_MspPostInit(&htim2);
+
+}
+
+/**
+  * @brief TIM5 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM5_Init(void)
+{
+
+  /* USER CODE BEGIN TIM5_Init 0 */
+
+  /* USER CODE END TIM5_Init 0 */
+
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
+
+  /* USER CODE BEGIN TIM5_Init 1 */
+
+  /* USER CODE END TIM5_Init 1 */
+  htim5.Instance = TIM5;
+  htim5.Init.Prescaler = 0;
+  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim5.Init.Period = 107;
+  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_PWM_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 0;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM5_Init 2 */
+
+  /* USER CODE END TIM5_Init 2 */
+  HAL_TIM_MspPostInit(&htim5);
+
+}
+
+/**
+  * Enable DMA controller clock
+  */
+static void MX_DMA_Init(void)
+{
+
+  /* DMA controller clock enable */
+  __HAL_RCC_DMA2_CLK_ENABLE();
+
+  /* DMA interrupt init */
+  /* DMA2_Channel2_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA2_Channel2_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA2_Channel2_IRQn);
+
+}
+
+/**
+  * @brief GPIO Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_GPIO_Init(void)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+/* USER CODE BEGIN MX_GPIO_Init_1 */
+/* USER CODE END MX_GPIO_Init_1 */
+
+  /* GPIO Ports Clock Enable */
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOD_CLK_ENABLE();
+  __HAL_RCC_GPIOG_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOE_CLK_ENABLE();
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOD, FSMC_NOE_Pin|FSMC_NWE_Pin|FSMC_NWAIT_Pin|FSMC_NE1_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(FSMC_NE2_GPIO_Port, FSMC_NE2_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(SHIFTERS_OE_GPIO_Port, SHIFTERS_OE_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pins : TDO_Pin TCK_Pin TDI_Pin TMS_Pin */
+  GPIO_InitStruct.Pin = TDO_Pin|TCK_Pin|TDI_Pin|TMS_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : PA8 */
+  GPIO_InitStruct.Pin = GPIO_PIN_8;
+  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : FSMC_NOE_Pin FSMC_NWE_Pin FSMC_NWAIT_Pin FSMC_NE1_Pin */
+  GPIO_InitStruct.Pin = FSMC_NOE_Pin|FSMC_NWE_Pin|FSMC_NWAIT_Pin|FSMC_NE1_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : FSMC_NE2_Pin */
+  GPIO_InitStruct.Pin = FSMC_NE2_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(FSMC_NE2_GPIO_Port, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : IRQ_Pin */
+  GPIO_InitStruct.Pin = IRQ_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+  GPIO_InitStruct.Pull = GPIO_PULLUP;
+  HAL_GPIO_Init(IRQ_GPIO_Port, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : SHIFTERS_OE_Pin */
+  GPIO_InitStruct.Pin = SHIFTERS_OE_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(SHIFTERS_OE_GPIO_Port, &GPIO_InitStruct);
+
+/* USER CODE BEGIN MX_GPIO_Init_2 */
+/* USER CODE END MX_GPIO_Init_2 */
+}
+
+/* USER CODE BEGIN 4 */
+
+/* USER CODE END 4 */
+
+/**
+  * @brief  This function is executed in case of error occurrence.
+  * @retval None
+  */
+void Error_Handler(void)
+{
+  /* USER CODE BEGIN Error_Handler_Debug */
+  /* User can add his own implementation to report the HAL error return state */
+  __disable_irq();
+  while (1)
+  {
+  }
+  /* USER CODE END Error_Handler_Debug */
+}
+
+#ifdef  USE_FULL_ASSERT
+/**
+  * @brief  Reports the name of the source file and the source line number
+  *         where the assert_param error has occurred.
+  * @param  file: pointer to the source file name
+  * @param  line: assert_param error line source number
+  * @retval None
+  */
+void assert_failed(uint8_t *file, uint32_t line)
+{
+  /* USER CODE BEGIN 6 */
+  /* User can add his own implementation to report the file name and line number,
+     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
+  /* USER CODE END 6 */
+}
+#endif /* USE_FULL_ASSERT */
diff --git a/FamicomDumperBootloader/Core/Src/stm32f1xx_hal_msp.c b/FamicomDumperBootloader/Core/Src/stm32f1xx_hal_msp.c
new file mode 100644
index 0000000..d32a39d
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/stm32f1xx_hal_msp.c
@@ -0,0 +1,267 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file         stm32f1xx_hal_msp.c
+  * @brief        This file provides code for the MSP Initialization
+  *               and de-Initialization codes.
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
+  * All rights reserved.</center></h2>
+  *
+  * This software component is licensed by ST under BSD 3-Clause license,
+  * the "License"; You may not use this file except in compliance with the
+  * License. You may obtain a copy of the License at:
+  *                        opensource.org/licenses/BSD-3-Clause
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+extern DMA_HandleTypeDef hdma_tim5_ch3_up;
+
+/* Private typedef -----------------------------------------------------------*/
+/* USER CODE BEGIN TD */
+
+/* USER CODE END TD */
+
+/* Private define ------------------------------------------------------------*/
+/* USER CODE BEGIN Define */
+
+/* USER CODE END Define */
+
+/* Private macro -------------------------------------------------------------*/
+/* USER CODE BEGIN Macro */
+
+/* USER CODE END Macro */
+
+/* Private variables ---------------------------------------------------------*/
+/* USER CODE BEGIN PV */
+
+/* USER CODE END PV */
+
+/* Private function prototypes -----------------------------------------------*/
+/* USER CODE BEGIN PFP */
+
+/* USER CODE END PFP */
+
+/* External functions --------------------------------------------------------*/
+/* USER CODE BEGIN ExternalFunctions */
+
+/* USER CODE END ExternalFunctions */
+
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
+                                        /**
+  * Initializes the Global MSP.
+  */
+void HAL_MspInit(void)
+{
+  /* USER CODE BEGIN MspInit 0 */
+
+  /* USER CODE END MspInit 0 */
+
+  __HAL_RCC_AFIO_CLK_ENABLE();
+  __HAL_RCC_PWR_CLK_ENABLE();
+
+  /* System interrupt init*/
+
+  /** NOJTAG: JTAG-DP Disabled and SW-DP Enabled
+  */
+  __HAL_AFIO_REMAP_SWJ_NOJTAG();
+
+  /* USER CODE BEGIN MspInit 1 */
+
+  /* USER CODE END MspInit 1 */
+}
+
+/**
+* @brief TIM_Base MSP Initialization
+* This function configures the hardware resources used in this example
+* @param htim_base: TIM_Base handle pointer
+* @retval None
+*/
+void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
+{
+  if(htim_base->Instance==TIM1)
+  {
+  /* USER CODE BEGIN TIM1_MspInit 0 */
+
+  /* USER CODE END TIM1_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM1_CLK_ENABLE();
+  /* USER CODE BEGIN TIM1_MspInit 1 */
+
+  /* USER CODE END TIM1_MspInit 1 */
+  }
+  else if(htim_base->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspInit 0 */
+
+  /* USER CODE END TIM2_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM2_CLK_ENABLE();
+  /* USER CODE BEGIN TIM2_MspInit 1 */
+
+  /* USER CODE END TIM2_MspInit 1 */
+  }
+
+}
+
+/**
+* @brief TIM_PWM MSP Initialization
+* This function configures the hardware resources used in this example
+* @param htim_pwm: TIM_PWM handle pointer
+* @retval None
+*/
+void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* htim_pwm)
+{
+  if(htim_pwm->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspInit 0 */
+
+  /* USER CODE END TIM5_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM5_CLK_ENABLE();
+
+    /* TIM5 DMA Init */
+    /* TIM5_CH3_UP Init */
+    hdma_tim5_ch3_up.Instance = DMA2_Channel2;
+    hdma_tim5_ch3_up.Init.Direction = DMA_MEMORY_TO_PERIPH;
+    hdma_tim5_ch3_up.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_tim5_ch3_up.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_tim5_ch3_up.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+    hdma_tim5_ch3_up.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_tim5_ch3_up.Init.Mode = DMA_NORMAL;
+    hdma_tim5_ch3_up.Init.Priority = DMA_PRIORITY_VERY_HIGH;
+    if (HAL_DMA_Init(&hdma_tim5_ch3_up) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    /* Several peripheral DMA handle pointers point to the same DMA handle.
+     Be aware that there is only one channel to perform all the requested DMAs. */
+    __HAL_LINKDMA(htim_pwm,hdma[TIM_DMA_ID_CC3],hdma_tim5_ch3_up);
+    __HAL_LINKDMA(htim_pwm,hdma[TIM_DMA_ID_UPDATE],hdma_tim5_ch3_up);
+
+  /* USER CODE BEGIN TIM5_MspInit 1 */
+
+  /* USER CODE END TIM5_MspInit 1 */
+  }
+
+}
+
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(htim->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspPostInit 0 */
+
+  /* USER CODE END TIM2_MspPostInit 0 */
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**TIM2 GPIO Configuration
+    PA0-WKUP     ------> TIM2_CH1
+    */
+    GPIO_InitStruct.Pin = M2_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(M2_GPIO_Port, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM2_MspPostInit 1 */
+
+  /* USER CODE END TIM2_MspPostInit 1 */
+  }
+  else if(htim->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspPostInit 0 */
+
+  /* USER CODE END TIM5_MspPostInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**TIM5 GPIO Configuration
+    PA2     ------> TIM5_CH3
+    */
+    GPIO_InitStruct.Pin = WS2812_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(WS2812_GPIO_Port, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM5_MspPostInit 1 */
+
+  /* USER CODE END TIM5_MspPostInit 1 */
+  }
+
+}
+/**
+* @brief TIM_Base MSP De-Initialization
+* This function freeze the hardware resources used in this example
+* @param htim_base: TIM_Base handle pointer
+* @retval None
+*/
+void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base)
+{
+  if(htim_base->Instance==TIM1)
+  {
+  /* USER CODE BEGIN TIM1_MspDeInit 0 */
+
+  /* USER CODE END TIM1_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM1_CLK_DISABLE();
+  /* USER CODE BEGIN TIM1_MspDeInit 1 */
+
+  /* USER CODE END TIM1_MspDeInit 1 */
+  }
+  else if(htim_base->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspDeInit 0 */
+
+  /* USER CODE END TIM2_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM2_CLK_DISABLE();
+  /* USER CODE BEGIN TIM2_MspDeInit 1 */
+
+  /* USER CODE END TIM2_MspDeInit 1 */
+  }
+
+}
+
+/**
+* @brief TIM_PWM MSP De-Initialization
+* This function freeze the hardware resources used in this example
+* @param htim_pwm: TIM_PWM handle pointer
+* @retval None
+*/
+void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* htim_pwm)
+{
+  if(htim_pwm->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspDeInit 0 */
+
+  /* USER CODE END TIM5_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM5_CLK_DISABLE();
+
+    /* TIM5 DMA DeInit */
+    HAL_DMA_DeInit(htim_pwm->hdma[TIM_DMA_ID_CC3]);
+    HAL_DMA_DeInit(htim_pwm->hdma[TIM_DMA_ID_UPDATE]);
+  /* USER CODE BEGIN TIM5_MspDeInit 1 */
+
+  /* USER CODE END TIM5_MspDeInit 1 */
+  }
+
+}
+
+/* USER CODE BEGIN 1 */
+
+/* USER CODE END 1 */
diff --git a/FamicomDumperBootloader/Core/Src/stm32f1xx_it.c b/FamicomDumperBootloader/Core/Src/stm32f1xx_it.c
new file mode 100644
index 0000000..2d89f32
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/stm32f1xx_it.c
@@ -0,0 +1,233 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    stm32f1xx_it.c
+  * @brief   Interrupt Service Routines.
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
+  * All rights reserved.</center></h2>
+  *
+  * This software component is licensed by ST under BSD 3-Clause license,
+  * the "License"; You may not use this file except in compliance with the
+  * License. You may obtain a copy of the License at:
+  *                        opensource.org/licenses/BSD-3-Clause
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+#include "stm32f1xx_it.h"
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+/* USER CODE END Includes */
+
+/* Private typedef -----------------------------------------------------------*/
+/* USER CODE BEGIN TD */
+
+/* USER CODE END TD */
+
+/* Private define ------------------------------------------------------------*/
+/* USER CODE BEGIN PD */
+
+/* USER CODE END PD */
+
+/* Private macro -------------------------------------------------------------*/
+/* USER CODE BEGIN PM */
+
+/* USER CODE END PM */
+
+/* Private variables ---------------------------------------------------------*/
+/* USER CODE BEGIN PV */
+
+/* USER CODE END PV */
+
+/* Private function prototypes -----------------------------------------------*/
+/* USER CODE BEGIN PFP */
+
+/* USER CODE END PFP */
+
+/* Private user code ---------------------------------------------------------*/
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+
+/* External variables --------------------------------------------------------*/
+extern PCD_HandleTypeDef hpcd_USB_FS;
+extern DMA_HandleTypeDef hdma_tim5_ch3_up;
+/* USER CODE BEGIN EV */
+
+/* USER CODE END EV */
+
+/******************************************************************************/
+/*           Cortex-M3 Processor Interruption and Exception Handlers          */
+/******************************************************************************/
+/**
+  * @brief This function handles Non maskable interrupt.
+  */
+void NMI_Handler(void)
+{
+  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */
+
+  /* USER CODE END NonMaskableInt_IRQn 0 */
+  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
+  while (1)
+  {
+  }
+  /* USER CODE END NonMaskableInt_IRQn 1 */
+}
+
+/**
+  * @brief This function handles Hard fault interrupt.
+  */
+void HardFault_Handler(void)
+{
+  /* USER CODE BEGIN HardFault_IRQn 0 */
+
+  /* USER CODE END HardFault_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
+    /* USER CODE END W1_HardFault_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles Memory management fault.
+  */
+void MemManage_Handler(void)
+{
+  /* USER CODE BEGIN MemoryManagement_IRQn 0 */
+
+  /* USER CODE END MemoryManagement_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
+    /* USER CODE END W1_MemoryManagement_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles Prefetch fault, memory access fault.
+  */
+void BusFault_Handler(void)
+{
+  /* USER CODE BEGIN BusFault_IRQn 0 */
+
+  /* USER CODE END BusFault_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
+    /* USER CODE END W1_BusFault_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles Undefined instruction or illegal state.
+  */
+void UsageFault_Handler(void)
+{
+  /* USER CODE BEGIN UsageFault_IRQn 0 */
+
+  /* USER CODE END UsageFault_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
+    /* USER CODE END W1_UsageFault_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles System service call via SWI instruction.
+  */
+void SVC_Handler(void)
+{
+  /* USER CODE BEGIN SVCall_IRQn 0 */
+
+  /* USER CODE END SVCall_IRQn 0 */
+  /* USER CODE BEGIN SVCall_IRQn 1 */
+
+  /* USER CODE END SVCall_IRQn 1 */
+}
+
+/**
+  * @brief This function handles Debug monitor.
+  */
+void DebugMon_Handler(void)
+{
+  /* USER CODE BEGIN DebugMonitor_IRQn 0 */
+
+  /* USER CODE END DebugMonitor_IRQn 0 */
+  /* USER CODE BEGIN DebugMonitor_IRQn 1 */
+
+  /* USER CODE END DebugMonitor_IRQn 1 */
+}
+
+/**
+  * @brief This function handles Pendable request for system service.
+  */
+void PendSV_Handler(void)
+{
+  /* USER CODE BEGIN PendSV_IRQn 0 */
+
+  /* USER CODE END PendSV_IRQn 0 */
+  /* USER CODE BEGIN PendSV_IRQn 1 */
+
+  /* USER CODE END PendSV_IRQn 1 */
+}
+
+/**
+  * @brief This function handles System tick timer.
+  */
+void SysTick_Handler(void)
+{
+  /* USER CODE BEGIN SysTick_IRQn 0 */
+
+  /* USER CODE END SysTick_IRQn 0 */
+  HAL_IncTick();
+  /* USER CODE BEGIN SysTick_IRQn 1 */
+
+  /* USER CODE END SysTick_IRQn 1 */
+}
+
+/******************************************************************************/
+/* STM32F1xx Peripheral Interrupt Handlers                                    */
+/* Add here the Interrupt Handlers for the used peripherals.                  */
+/* For the available peripheral interrupt handler names,                      */
+/* please refer to the startup file (startup_stm32f1xx.s).                    */
+/******************************************************************************/
+
+/**
+  * @brief This function handles USB low priority or CAN RX0 interrupts.
+  */
+void USB_LP_CAN1_RX0_IRQHandler(void)
+{
+  /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 0 */
+
+  /* USER CODE END USB_LP_CAN1_RX0_IRQn 0 */
+  HAL_PCD_IRQHandler(&hpcd_USB_FS);
+  /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 1 */
+
+  /* USER CODE END USB_LP_CAN1_RX0_IRQn 1 */
+}
+
+/**
+  * @brief This function handles DMA2 channel2 global interrupt.
+  */
+void DMA2_Channel2_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA2_Channel2_IRQn 0 */
+
+  /* USER CODE END DMA2_Channel2_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_tim5_ch3_up);
+  /* USER CODE BEGIN DMA2_Channel2_IRQn 1 */
+
+  /* USER CODE END DMA2_Channel2_IRQn 1 */
+}
+
+/* USER CODE BEGIN 1 */
+
+/* USER CODE END 1 */
diff --git a/FamicomDumperBootloader/Core/Src/syscalls.c b/FamicomDumperBootloader/Core/Src/syscalls.c
new file mode 100644
index 0000000..bc0dd6c
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/syscalls.c
@@ -0,0 +1,156 @@
+/**
+ ******************************************************************************
+ * @file      syscalls.c
+ * @author    Auto-generated by STM32CubeIDE
+ * @brief     STM32CubeIDE Minimal System calls file
+ *
+ *            For more information about which c-functions
+ *            need which of these lowlevel functions
+ *            please consult the Newlib libc-manual
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
+ * All rights reserved.</center></h2>
+ *
+ * This software component is licensed by ST under BSD 3-Clause license,
+ * the "License"; You may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at:
+ *                        opensource.org/licenses/BSD-3-Clause
+ *
+ ******************************************************************************
+ */
+
+/* Includes */
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <stdio.h>
+#include <signal.h>
+#include <time.h>
+#include <sys/time.h>
+#include <sys/times.h>
+
+
+/* Variables */
+extern int __io_putchar(int ch) __attribute__((weak));
+extern int __io_getchar(void) __attribute__((weak));
+
+
+char *__env[1] = { 0 };
+char **environ = __env;
+
+
+/* Functions */
+void initialise_monitor_handles()
+{
+}
+
+int _getpid(void)
+{
+	return 1;
+}
+
+int _kill(int pid, int sig)
+{
+	errno = EINVAL;
+	return -1;
+}
+
+void _exit (int status)
+{
+	_kill(status, -1);
+	while (1) {}		/* Make sure we hang here */
+}
+
+__attribute__((weak)) int _read(int file, char *ptr, int len)
+{
+	int DataIdx;
+
+	for (DataIdx = 0; DataIdx < len; DataIdx++)
+	{
+		*ptr++ = __io_getchar();
+	}
+
+return len;
+}
+
+__attribute__((weak)) int _write(int file, char *ptr, int len)
+{
+	int DataIdx;
+
+	for (DataIdx = 0; DataIdx < len; DataIdx++)
+	{
+		__io_putchar(*ptr++);
+	}
+	return len;
+}
+
+int _close(int file)
+{
+	return -1;
+}
+
+
+int _fstat(int file, struct stat *st)
+{
+	st->st_mode = S_IFCHR;
+	return 0;
+}
+
+int _isatty(int file)
+{
+	return 1;
+}
+
+int _lseek(int file, int ptr, int dir)
+{
+	return 0;
+}
+
+int _open(char *path, int flags, ...)
+{
+	/* Pretend like we always fail */
+	return -1;
+}
+
+int _wait(int *status)
+{
+	errno = ECHILD;
+	return -1;
+}
+
+int _unlink(char *name)
+{
+	errno = ENOENT;
+	return -1;
+}
+
+int _times(struct tms *buf)
+{
+	return -1;
+}
+
+int _stat(char *file, struct stat *st)
+{
+	st->st_mode = S_IFCHR;
+	return 0;
+}
+
+int _link(char *old, char *new)
+{
+	errno = EMLINK;
+	return -1;
+}
+
+int _fork(void)
+{
+	errno = EAGAIN;
+	return -1;
+}
+
+int _execve(char *name, char **argv, char **env)
+{
+	errno = ENOMEM;
+	return -1;
+}
diff --git a/FamicomDumperBootloader/Core/Src/sysmem.c b/FamicomDumperBootloader/Core/Src/sysmem.c
new file mode 100644
index 0000000..305263f
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/sysmem.c
@@ -0,0 +1,81 @@
+/**
+ ******************************************************************************
+ * @file      sysmem.c
+ * @author    Generated by STM32CubeIDE
+ * @brief     STM32CubeIDE System Memory calls file
+ *
+ *            For more information about which C functions
+ *            need which of these lowlevel functions
+ *            please consult the newlib libc manual
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
+ * All rights reserved.</center></h2>
+ *
+ * This software component is licensed by ST under BSD 3-Clause license,
+ * the "License"; You may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at:
+ *                        opensource.org/licenses/BSD-3-Clause
+ *
+ ******************************************************************************
+ */
+
+/* Includes */
+#include <errno.h>
+#include <stdint.h>
+
+/**
+ * Pointer to the current high watermark of the heap usage
+ */
+static uint8_t *__sbrk_heap_end = NULL;
+
+/**
+ * @brief _sbrk() allocates memory to the newlib heap and is used by malloc
+ *        and others from the C library
+ *
+ * @verbatim
+ * ############################################################################
+ * #  .data  #  .bss  #       newlib heap       #          MSP stack          #
+ * #         #        #                         # Reserved by _Min_Stack_Size #
+ * ############################################################################
+ * ^-- RAM start      ^-- _end                             _estack, RAM end --^
+ * @endverbatim
+ *
+ * This implementation starts allocating at the '_end' linker symbol
+ * The '_Min_Stack_Size' linker symbol reserves a memory for the MSP stack
+ * The implementation considers '_estack' linker symbol to be RAM end
+ * NOTE: If the MSP stack, at any point during execution, grows larger than the
+ * reserved size, please increase the '_Min_Stack_Size'.
+ *
+ * @param incr Memory size
+ * @return Pointer to allocated memory
+ */
+void *_sbrk(ptrdiff_t incr)
+{
+  extern uint8_t _end; /* Symbol defined in the linker script */
+  extern uint8_t _estack; /* Symbol defined in the linker script */
+  extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */
+  const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size;
+  const uint8_t *max_heap = (uint8_t *)stack_limit;
+  uint8_t *prev_heap_end;
+
+  /* Initialize heap end at first call */
+  if (NULL == __sbrk_heap_end)
+  {
+    __sbrk_heap_end = &_end;
+  }
+
+  /* Protect heap from growing into the reserved MSP stack */
+  if (__sbrk_heap_end + incr > max_heap)
+  {
+    errno = ENOMEM;
+    return (void *)-1;
+  }
+
+  prev_heap_end = __sbrk_heap_end;
+  __sbrk_heap_end += incr;
+
+  return (void *)prev_heap_end;
+
+}
diff --git a/FamicomDumperBootloader/Core/Src/system_stm32f1xx.c b/FamicomDumperBootloader/Core/Src/system_stm32f1xx.c
new file mode 100644
index 0000000..bc96aae
--- /dev/null
+++ b/FamicomDumperBootloader/Core/Src/system_stm32f1xx.c
@@ -0,0 +1,408 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32f1xx.c
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M3 Device Peripheral Access Layer System Source File.
+  * 
+  * 1.  This file provides two functions and one global variable to be called from 
+  *     user application:
+  *      - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
+  *                      factors, AHB/APBx prescalers and Flash settings). 
+  *                      This function is called at startup just after reset and 
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32f1xx_xx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick 
+  *                                  timer or configure other parameters.
+  *                                     
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  * 2. After each device reset the HSI (8 MHz) is used as system clock source.
+  *    Then SystemInit() function is called, in "startup_stm32f1xx_xx.s" file, to
+  *    configure the system clock before to branch to main program.
+  *
+  * 4. The default value of HSE crystal is set to 8 MHz (or 25 MHz, depending on
+  *    the product used), refer to "HSE_VALUE". 
+  *    When HSE is used as system clock source, directly or through PLL, and you
+  *    are using different crystal you have to adapt the HSE value to your own
+  *    configuration.
+  *        
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; Copyright (c) 2017 STMicroelectronics.
+  * All rights reserved.</center></h2>
+  *
+  * This software component is licensed by ST under BSD 3-Clause license,
+  * the "License"; You may not use this file except in compliance with the
+  * License. You may obtain a copy of the License at:
+  *                        opensource.org/licenses/BSD-3-Clause
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f1xx_system
+  * @{
+  */  
+  
+/** @addtogroup STM32F1xx_System_Private_Includes
+  * @{
+  */
+
+#include "stm32f1xx.h"
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Defines
+  * @{
+  */
+
+#if !defined  (HSE_VALUE) 
+  #define HSE_VALUE               8000000U /*!< Default value of the External oscillator in Hz.
+                                                This value can be provided and adapted by the user application. */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE               8000000U /*!< Default value of the Internal oscillator in Hz.
+                                                This value can be provided and adapted by the user application. */
+#endif /* HSI_VALUE */
+
+/*!< Uncomment the following line if you need to use external SRAM  */ 
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+/* #define DATA_IN_ExtSRAM */
+#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */
+
+/* Note: Following vector table addresses must be defined in line with linker
+         configuration. */
+/*!< Uncomment the following line if you need to relocate the vector table
+     anywhere in Flash or Sram, else the vector table is kept at the automatic
+     remap of boot address selected */
+/* #define USER_VECT_TAB_ADDRESS */
+
+#if defined(USER_VECT_TAB_ADDRESS)
+/*!< Uncomment the following line if you need to relocate your vector Table
+     in Sram else user remap will be done in Flash. */
+/* #define VECT_TAB_SRAM */
+#if defined(VECT_TAB_SRAM)
+#define VECT_TAB_BASE_ADDRESS   SRAM_BASE       /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table base offset field.
+                                                     This value must be a multiple of 0x200. */
+#else
+#define VECT_TAB_BASE_ADDRESS   FLASH_BASE      /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table base offset field.
+                                                     This value must be a multiple of 0x200. */
+#endif /* VECT_TAB_SRAM */
+#endif /* USER_VECT_TAB_ADDRESS */
+
+/******************************************************************************/
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Variables
+  * @{
+  */
+
+  /* This variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+uint32_t SystemCoreClock = 16000000;
+const uint8_t AHBPrescTable[16U] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+const uint8_t APBPrescTable[8U] =  {0, 0, 0, 0, 1, 2, 3, 4};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+#ifdef DATA_IN_ExtSRAM
+  static void SystemInit_ExtMemCtl(void); 
+#endif /* DATA_IN_ExtSRAM */
+#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system
+  *         Initialize the Embedded Flash Interface, the PLL and update the 
+  *         SystemCoreClock variable.
+  * @note   This function should be used only after reset.
+  * @param  None
+  * @retval None
+  */
+void SystemInit (void)
+{
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+  #ifdef DATA_IN_ExtSRAM
+    SystemInit_ExtMemCtl(); 
+  #endif /* DATA_IN_ExtSRAM */
+#endif 
+
+  /* Configure the Vector Table location -------------------------------------*/
+#if defined(USER_VECT_TAB_ADDRESS)
+  SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */
+#endif /* USER_VECT_TAB_ADDRESS */
+}
+
+/**
+  * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *           
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.         
+  *     
+  * @note   - The system frequency computed by this function is not the real 
+  *           frequency in the chip. It is calculated based on the predefined 
+  *           constant and the selected clock source:
+  *             
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+  *                                              
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+  *                          
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 
+  *             or HSI_VALUE(*) multiplied by the PLL factors.
+  *         
+  *         (*) HSI_VALUE is a constant defined in stm32f1xx.h file (default value
+  *             8 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.   
+  *    
+  *         (**) HSE_VALUE is a constant defined in stm32f1xx.h file (default value
+  *              8 MHz or 25 MHz, depending on the product used), user has to ensure
+  *              that HSE_VALUE is same as the real frequency of the crystal used.
+  *              Otherwise, this function may have wrong result.
+  *                
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate (void)
+{
+  uint32_t tmp = 0U, pllmull = 0U, pllsource = 0U;
+
+#if defined(STM32F105xC) || defined(STM32F107xC)
+  uint32_t prediv1source = 0U, prediv1factor = 0U, prediv2factor = 0U, pll2mull = 0U;
+#endif /* STM32F105xC */
+
+#if defined(STM32F100xB) || defined(STM32F100xE)
+  uint32_t prediv1factor = 0U;
+#endif /* STM32F100xB or STM32F100xE */
+    
+  /* Get SYSCLK source -------------------------------------------------------*/
+  tmp = RCC->CFGR & RCC_CFGR_SWS;
+  
+  switch (tmp)
+  {
+    case 0x00U:  /* HSI used as system clock */
+      SystemCoreClock = HSI_VALUE;
+      break;
+    case 0x04U:  /* HSE used as system clock */
+      SystemCoreClock = HSE_VALUE;
+      break;
+    case 0x08U:  /* PLL used as system clock */
+
+      /* Get PLL clock source and multiplication factor ----------------------*/
+      pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
+      pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
+      
+#if !defined(STM32F105xC) && !defined(STM32F107xC)      
+      pllmull = ( pllmull >> 18U) + 2U;
+      
+      if (pllsource == 0x00U)
+      {
+        /* HSI oscillator clock divided by 2 selected as PLL clock entry */
+        SystemCoreClock = (HSI_VALUE >> 1U) * pllmull;
+      }
+      else
+      {
+ #if defined(STM32F100xB) || defined(STM32F100xE)
+       prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1U;
+       /* HSE oscillator clock selected as PREDIV1 clock entry */
+       SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; 
+ #else
+        /* HSE selected as PLL clock entry */
+        if ((RCC->CFGR & RCC_CFGR_PLLXTPRE) != (uint32_t)RESET)
+        {/* HSE oscillator clock divided by 2 */
+          SystemCoreClock = (HSE_VALUE >> 1U) * pllmull;
+        }
+        else
+        {
+          SystemCoreClock = HSE_VALUE * pllmull;
+        }
+ #endif
+      }
+#else
+      pllmull = pllmull >> 18U;
+      
+      if (pllmull != 0x0DU)
+      {
+         pllmull += 2U;
+      }
+      else
+      { /* PLL multiplication factor = PLL input clock * 6.5 */
+        pllmull = 13U / 2U; 
+      }
+            
+      if (pllsource == 0x00U)
+      {
+        /* HSI oscillator clock divided by 2 selected as PLL clock entry */
+        SystemCoreClock = (HSI_VALUE >> 1U) * pllmull;
+      }
+      else
+      {/* PREDIV1 selected as PLL clock entry */
+        
+        /* Get PREDIV1 clock source and division factor */
+        prediv1source = RCC->CFGR2 & RCC_CFGR2_PREDIV1SRC;
+        prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1U;
+        
+        if (prediv1source == 0U)
+        { 
+          /* HSE oscillator clock selected as PREDIV1 clock entry */
+          SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull;          
+        }
+        else
+        {/* PLL2 clock selected as PREDIV1 clock entry */
+          
+          /* Get PREDIV2 division factor and PLL2 multiplication factor */
+          prediv2factor = ((RCC->CFGR2 & RCC_CFGR2_PREDIV2) >> 4U) + 1U;
+          pll2mull = ((RCC->CFGR2 & RCC_CFGR2_PLL2MUL) >> 8U) + 2U; 
+          SystemCoreClock = (((HSE_VALUE / prediv2factor) * pll2mull) / prediv1factor) * pllmull;                         
+        }
+      }
+#endif /* STM32F105xC */ 
+      break;
+
+    default:
+      SystemCoreClock = HSI_VALUE;
+      break;
+  }
+  
+  /* Compute HCLK clock frequency ----------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4U)];
+  /* HCLK clock frequency */
+  SystemCoreClock >>= tmp;  
+}
+
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+/**
+  * @brief  Setup the external memory controller. Called in startup_stm32f1xx.s 
+  *          before jump to __main
+  * @param  None
+  * @retval None
+  */ 
+#ifdef DATA_IN_ExtSRAM
+/**
+  * @brief  Setup the external memory controller. 
+  *         Called in startup_stm32f1xx_xx.s/.c before jump to main.
+  *         This function configures the external SRAM mounted on STM3210E-EVAL
+  *         board (STM32 High density devices). This SRAM will be used as program
+  *         data memory (including heap and stack).
+  * @param  None
+  * @retval None
+  */ 
+void SystemInit_ExtMemCtl(void) 
+{
+  __IO uint32_t tmpreg;
+  /*!< FSMC Bank1 NOR/SRAM3 is used for the STM3210E-EVAL, if another Bank is 
+    required, then adjust the Register Addresses */
+
+  /* Enable FSMC clock */
+  RCC->AHBENR = 0x00000114U;
+
+  /* Delay after an RCC peripheral clock enabling */
+  tmpreg = READ_BIT(RCC->AHBENR, RCC_AHBENR_FSMCEN);
+  
+  /* Enable GPIOD, GPIOE, GPIOF and GPIOG clocks */
+  RCC->APB2ENR = 0x000001E0U;
+  
+  /* Delay after an RCC peripheral clock enabling */
+  tmpreg = READ_BIT(RCC->APB2ENR, RCC_APB2ENR_IOPDEN);
+
+  (void)(tmpreg);
+  
+/* ---------------  SRAM Data lines, NOE and NWE configuration ---------------*/
+/*----------------  SRAM Address lines configuration -------------------------*/
+/*----------------  NOE and NWE configuration --------------------------------*/  
+/*----------------  NE3 configuration ----------------------------------------*/
+/*----------------  NBL0, NBL1 configuration ---------------------------------*/
+  
+  GPIOD->CRL = 0x44BB44BBU;  
+  GPIOD->CRH = 0xBBBBBBBBU;
+
+  GPIOE->CRL = 0xB44444BBU;  
+  GPIOE->CRH = 0xBBBBBBBBU;
+
+  GPIOF->CRL = 0x44BBBBBBU;  
+  GPIOF->CRH = 0xBBBB4444U;
+
+  GPIOG->CRL = 0x44BBBBBBU;  
+  GPIOG->CRH = 0x444B4B44U;
+   
+/*----------------  FSMC Configuration ---------------------------------------*/  
+/*----------------  Enable FSMC Bank1_SRAM Bank ------------------------------*/
+  
+  FSMC_Bank1->BTCR[4U] = 0x00001091U;
+  FSMC_Bank1->BTCR[5U] = 0x00110212U;
+}
+#endif /* DATA_IN_ExtSRAM */
+#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+  
+/**
+  * @}
+  */    
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/