Fix for Frauchekey
[gnuk/neug.git] / src / main.c
index c02a678..06f6b32 100644 (file)
@@ -2,10 +2,11 @@
  * main.c - main routine of neug
  *
  * Main routine:
- * Copyright (C) 2011, 2012 Free Software Initiative of Japan
+ * Copyright (C) 2011, 2012, 2013, 2015, 2016
+ *              Free Software Initiative of Japan
  * Author: NIIBE Yutaka <gniibe@fsij.org>
  *
- * This file is a part of NeuG, a Random Number Generator
+ * This file is a part of NeuG, a True Random Number Generator
  * implementation.
  *
  * NeuG is free software: you can redistribute it and/or modify it
@@ -13,7 +14,7 @@
  * the Free Software Foundation, either version 3 of the License, or
  * (at your option) any later version.
  *
- * Gnuk is distributed in the hope that it will be useful, but WITHOUT
+ * NeuG is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
  * License for more details.
  *
  */
 
+
+#include <stdint.h>
+#include <string.h>
+#include <chopstx.h>
+
 #include "config.h"
-#include "ch.h"
-#include "hal.h"
-#include "board.h"
 #include "neug.h"
 #include "usb_lld.h"
 #include "sys.h"
+#include "mcu/stm32f103.h"
+#include "adc.h"
+
+enum {
+  FSIJ_DEVICE_RUNNING = 0,
+  FSIJ_DEVICE_EXITED,
+  FSIJ_DEVICE_EXEC_REQUESTED,
+  /**/
+  FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED = 254,
+  FSIJ_DEVICE_NEUG_EXIT_REQUESTED = 255
+};
+
+#ifdef FRAUCHEKY_SUPPORT
+static uint8_t running_neug;
+extern void EP6_IN_Callback (uint32_t len);
+extern void EP6_OUT_Callback (void);
+#endif
+
+static chopstx_mutex_t usb_mtx;
+static chopstx_cond_t usb_cnd;
+static uint32_t bDeviceState = UNCONNECTED; /* USB device status */
+static uint8_t fsij_device_state = FSIJ_DEVICE_RUNNING;
+static uint8_t connected;
 
-/*
- * We are trying to avoid dependency to C library. 
- * GCC built-in function(s) are declared here.
- */
-extern void *memcpy(void *dest, const void *src, size_t n);
-extern void *memset (void *s, int c, size_t n);
 
-\f
 #define ENDP0_RXADDR        (0x40)
 #define ENDP0_TXADDR        (0x80)
 #define ENDP1_TXADDR        (0xc0)
@@ -53,7 +72,7 @@ extern void *memset (void *s, int c, size_t n);
 /* USB Device Descriptor */
 static const uint8_t vcom_device_desc[18] = {
   18,   /* bLength */
-  USB_DEVICE_DESCRIPTOR_TYPE,  /* bDescriptorType */
+  DEVICE_DESCRIPTOR,           /* bDescriptorType */
   0x10, 0x01,                  /* bcdUSB = 1.1 */
   0x02,                                /* bDeviceClass (CDC).              */
   0x00,                                /* bDeviceSubClass.                 */
@@ -66,20 +85,22 @@ static const uint8_t vcom_device_desc[18] = {
   1                            /* bNumConfigurations.              */
 };
 
+#define VCOM_FEATURE_BUS_POWERED       0x80
+
 /* Configuration Descriptor tree for a CDC.*/
-static const uint8_t vcom_configuration_desc[67] = {
+static const uint8_t vcom_config_desc[67] = {
   9,
-  USB_CONFIGURATION_DESCRIPTOR_TYPE, /* bDescriptorType: Configuration */
+  CONFIG_DESCRIPTOR,            /* bDescriptorType: Configuration   */
   /* Configuration Descriptor.*/
   67, 0x00,                    /* wTotalLength.                    */
   0x02,                                /* bNumInterfaces.                  */
   0x01,                                /* bConfigurationValue.             */
   0,                           /* iConfiguration.                  */
-  0x80,                                /* bmAttributes (bus powered).      */
+  VCOM_FEATURE_BUS_POWERED,    /* bmAttributes.                    */
   50,                          /* bMaxPower (100mA).               */
   /* Interface Descriptor.*/
   9,
-  USB_INTERFACE_DESCRIPTOR_TYPE,
+  INTERFACE_DESCRIPTOR,
   0x00,                   /* bInterfaceNumber.                */
   0x00,                   /* bAlternateSetting.               */
   0x01,                   /* bNumEndpoints.                   */
@@ -118,14 +139,14 @@ static const uint8_t vcom_configuration_desc[67] = {
   0x01,         /* bSlaveInterface0 (Data Class Interface).  */
   /* Endpoint 2 Descriptor.*/
   7,
-  USB_ENDPOINT_DESCRIPTOR_TYPE,
+  ENDPOINT_DESCRIPTOR,
   ENDP2|0x80,    /* bEndpointAddress.    */
   0x03,          /* bmAttributes (Interrupt).        */
   0x08, 0x00,   /* wMaxPacketSize.                  */
   0xFF,                 /* bInterval.                       */
   /* Interface Descriptor.*/
   9,
-  USB_INTERFACE_DESCRIPTOR_TYPE, /* bDescriptorType: */
+  INTERFACE_DESCRIPTOR,          /* bDescriptorType: */
   0x01,          /* bInterfaceNumber.                */
   0x00,          /* bAlternateSetting.               */
   0x02,          /* bNumEndpoints.                   */
@@ -135,14 +156,14 @@ static const uint8_t vcom_configuration_desc[67] = {
   0x00,                 /* iInterface.                      */
   /* Endpoint 3 Descriptor.*/
   7,
-  USB_ENDPOINT_DESCRIPTOR_TYPE,        /* bDescriptorType: Endpoint */
+  ENDPOINT_DESCRIPTOR,         /* bDescriptorType: Endpoint */
   ENDP3,    /* bEndpointAddress. */
   0x02,                                /* bmAttributes (Bulk).             */
   0x40, 0x00,                  /* wMaxPacketSize.                  */
   0x00,                                /* bInterval.                       */
   /* Endpoint 1 Descriptor.*/
   7,
-  USB_ENDPOINT_DESCRIPTOR_TYPE,        /* bDescriptorType: Endpoint */
+  ENDPOINT_DESCRIPTOR,         /* bDescriptorType: Endpoint */
   ENDP1|0x80,                  /* bEndpointAddress. */
   0x02,                                /* bmAttributes (Bulk).             */
   0x40, 0x00,                  /* wMaxPacketSize.                  */
@@ -155,82 +176,82 @@ static const uint8_t vcom_configuration_desc[67] = {
  */
 static const uint8_t vcom_string0[4] = {
   4,                           /* bLength */
-  USB_STRING_DESCRIPTOR_TYPE,
+  STRING_DESCRIPTOR,
   0x09, 0x04                   /* LangID = 0x0409: US-English */
 };
 
-#define USB_STRINGS_FOR_NEUG 1
 #include "usb-strings.c.inc"
 
-/*
- * Serial Number string.  NOTE: This does not have CONST qualifier.
- */
-static uint8_t vcom_string3[28] = {
-  28,                    /* bLength.                         */
-  USB_STRING_DESCRIPTOR_TYPE, /* bDescriptorType.                 */
-  '0', 0,  '.', 0,  '0', 0, '2', 0,    /* Version number of NeuG.          */
-  '-', 0,
-  0, 0, 0, 0,  /* Filled by Unique device ID.      */
-  0, 0, 0, 0,
-  0, 0, 0, 0,
-  0, 0, 0, 0,
-};
+static void neug_setup_endpoints_for_interface (uint16_t interface, int stop);
+#ifdef FRAUCHEKY_SUPPORT
+extern int fraucheky_enabled (void);
+extern void fraucheky_main (void);
 
-static const struct Descriptor device_desc = {
-  vcom_device_desc,
-  sizeof (vcom_device_desc)
-};
-
-static const struct Descriptor config_desc = {
-  vcom_configuration_desc,
-  sizeof (vcom_configuration_desc)
-};
-
-static const struct Descriptor string_descs[] = {
-  {vcom_string0, sizeof vcom_string0},
-  {neug_string_vendor, sizeof (neug_string_vendor)},
-  {neug_string_product, sizeof (neug_string_product)},
-  {vcom_string3, sizeof (vcom_string3)},
-  {neug_revision_detail, sizeof (neug_revision_detail)},
-  {neug_config_options, sizeof (neug_config_options)},
-  {sys_version, sizeof (sys_version)},
-};
-
-#define NUM_STRING_DESC (sizeof (string_descs)/sizeof (struct Descriptor))
+extern void fraucheky_setup_endpoints_for_interface (int stop);
+extern int fraucheky_setup (uint8_t req, uint8_t req_no,
+                           struct req_args *detail);
+extern int fraucheky_get_descriptor (uint8_t rcp, uint8_t desc_type,
+                                    uint8_t desc_index,
+                                    struct req_args *detail);
+#endif
 
 #define NUM_INTERFACES 2
 
-uint32_t bDeviceState = UNCONNECTED; /* USB device status */
-
-
-static void
-neug_device_reset (void)
+void
+usb_cb_device_reset (void)
 {
-  /* Set DEVICE as not configured */
-  usb_lld_set_configuration (0);
-
-  /* Current Feature initialization */
-  usb_lld_set_feature (config_desc.Descriptor[7]);
+  int i;
 
-  usb_lld_reset ();
+  usb_lld_reset (VCOM_FEATURE_BUS_POWERED);
 
-  /* Initialize Endpoint 0 */
+  /* Initialize Endpoint 0 */
   usb_lld_setup_endpoint (ENDP0, EP_CONTROL, 0, ENDP0_RXADDR, ENDP0_TXADDR, 64);
+
+  /* Stop the interface */
+  for (i = 0; i < NUM_INTERFACES; i++)
+    neug_setup_endpoints_for_interface (i, 1);
+
+  /* Notify upper layer.  */
+  chopstx_mutex_lock (&usb_mtx);
+  bDeviceState = ATTACHED;
+  connected = 0;
+  chopstx_cond_signal (&usb_cnd);
+  chopstx_mutex_unlock (&usb_mtx);
 }
 
-#define USB_REGNUAL_MEMINFO    0
-#define USB_REGNUAL_SEND       1
-#define USB_REGNUAL_RESULT     2
-#define USB_REGNUAL_FLASH      3
-#define USB_REGNUAL_PROTECT    4
-#define USB_REGNUAL_FINISH     5
+extern uint8_t _regnual_start, __heap_end__;
 
-static uint8_t mem[256];
-static uint32_t result;
+static const uint8_t *const mem_info[] = { &_regnual_start,  &__heap_end__, };
 
-extern uint8_t __flash_start__,  __flash_end__;
-static const uint8_t *const mem_info[] = { &__flash_start__,  &__flash_end__ };
+/* USB vendor requests to control pipe */
+#define USB_FSIJ_MEMINFO         0
+#define USB_FSIJ_DOWNLOAD        1
+#define USB_FSIJ_EXEC            2
+#define USB_NEUG_SET_PASSWD    253
+#define USB_NEUG_GET_INFO      254
+#define USB_NEUG_EXIT          255 /* Ask to exit and to receive reGNUal */
 
+uint8_t neug_passwd[33] __attribute__ ((section(".passwd"))) = {
+  0xff,
+  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+};
+static uint8_t usbbuf[64];
+
+#define DEFAULT_PASSWD "12345678"
+#define DEFAULT_PASSWD_LEN 8
+
+static void set_passwd (void)
+{
+  flash_unlock ();
+  if (neug_passwd[0] != 0xff)
+    flash_erase_page ((uint32_t)neug_passwd);
+  if (usbbuf[0] == DEFAULT_PASSWD_LEN
+      && !memcmp (usbbuf + 1, DEFAULT_PASSWD, DEFAULT_PASSWD_LEN))
+    return;
+  flash_write ((uint32_t)neug_passwd, usbbuf, usbbuf[0] + 1);
+}
 
 static uint32_t rbit (uint32_t v)
 {
@@ -240,51 +261,26 @@ static uint32_t rbit (uint32_t v)
   return r;
 }
 
-static uint32_t fetch (int i)
+/* After calling this function, CRC module remain enabled.  */
+static int download_check_crc32 (const uint32_t *end_p)
 {
-  uint32_t v;
-
-  v = *(uint32_t *)(&mem[i*4]);
-  return rbit (v);
-}
-
-static uint32_t calc_crc32 (void)
-{
-  int i;
+  uint32_t crc32 = *end_p;
+  const uint32_t *p;
 
+  RCC->AHBENR |= RCC_AHBENR_CRCEN;
   CRC->CR = CRC_CR_RESET;
 
-  for (i = 0; i < 256/4; i++)
-    CRC->DR = fetch (i);
-
-  return rbit (CRC->DR);
-}
+  for (p = (const uint32_t *)&_regnual_start; p < end_p; p++)
+    CRC->DR = rbit (*p);
 
+  if ((rbit (CRC->DR) ^ crc32) == 0xffffffff)
+    return USB_SUCCESS;
 
-static void neug_ctrl_write_finish (uint8_t req, uint8_t req_no,
-                                   uint16_t value, uint16_t index,
-                                   uint16_t len)
-{
-  uint8_t type_rcp = req & (REQUEST_TYPE|RECIPIENT);
+  return USB_UNSUPPORT;
+}
 
-  if (type_rcp == (VENDOR_REQUEST | DEVICE_RECIPIENT) && USB_SETUP_SET (req))
-    {
-      if (req_no == USB_REGNUAL_SEND && value == 0)
-       result = calc_crc32 ();
-      else if (req_no == USB_REGNUAL_FLASH && len == 0 && index == 0)
-       {
-         uint32_t dst_addr = (0x08000000 + value * 0x100);
 
-         result = flash_write (dst_addr, mem, 256);
-       }
-      else if (req_no == USB_REGNUAL_PROTECT && len == 0
-              && value == 0 && index == 0)
-       result = flash_protect ();
-      else if (req_no == USB_REGNUAL_FINISH && len == 0
-              && value == 0 && index == 0)
-       nvic_system_reset ();
-    }
-}
+#define NEUG_SPECIAL_BITRATE 110
 
 struct line_coding
 {
@@ -292,7 +288,7 @@ struct line_coding
   uint8_t format;
   uint8_t paritytype;
   uint8_t datatype;
-};
+} __attribute__((packed));
 
 static struct line_coding line_coding = {
   115200, /* baud rate: 115200    */
@@ -301,48 +297,95 @@ static struct line_coding line_coding = {
   0x08    /* bits:      8         */
 };
 
-static uint8_t connected;
+#define CDC_CTRL_DTR            0x0001
 
-static int
-vcom_port_data_setup (uint8_t req, uint8_t req_no, uint16_t value)
+void
+usb_cb_ctrl_write_finish (uint8_t req, uint8_t req_no, struct req_args *arg)
 {
-  if (USB_SETUP_GET (req))
+  uint8_t type_rcp = req & (REQUEST_TYPE|RECIPIENT);
+
+  if (type_rcp == (VENDOR_REQUEST | DEVICE_RECIPIENT) && USB_SETUP_SET (req))
     {
-      if (req_no == USB_CDC_REQ_GET_LINE_CODING)
+      if (req_no == USB_FSIJ_EXEC)
        {
-         usb_lld_set_data_to_send (&line_coding, sizeof(line_coding));
-         return USB_SUCCESS;
+         chopstx_mutex_lock (&usb_mtx);
+         if (fsij_device_state == FSIJ_DEVICE_EXITED)
+           {
+             usb_lld_prepare_shutdown (); /* No further USB communication */
+             fsij_device_state = FSIJ_DEVICE_EXEC_REQUESTED;
+           }
+         chopstx_mutex_unlock (&usb_mtx);
+       }
+      else if (req_no == USB_NEUG_SET_PASSWD)
+       set_passwd ();
+      else if (req_no == USB_NEUG_EXIT)
+       {
+         if ((neug_passwd[0] == 0xff && usbbuf[0] == DEFAULT_PASSWD_LEN
+              && !memcmp (usbbuf + 1, DEFAULT_PASSWD, DEFAULT_PASSWD_LEN))
+             || (neug_passwd[0] == usbbuf[0]
+                 && !memcmp (neug_passwd+1, usbbuf+1, neug_passwd[0])))
+           {
+             chopstx_mutex_lock (&usb_mtx);
+             fsij_device_state = FSIJ_DEVICE_NEUG_EXIT_REQUESTED;
+             chopstx_cond_signal (&usb_cnd);
+             chopstx_mutex_unlock (&usb_mtx);
+           }
        }
     }
-  else  /* USB_SETUP_SET (req) */
+  else if (type_rcp == (CLASS_REQUEST | INTERFACE_RECIPIENT) && arg->index == 0
+          && USB_SETUP_SET (req))
     {
-      if (req_no == USB_CDC_REQ_SET_LINE_CODING)
+      if (req_no == USB_CDC_REQ_SET_CONTROL_LINE_STATE)
        {
-         usb_lld_set_data_to_recv (&line_coding, sizeof(line_coding));
-         return USB_SUCCESS;
+         /* Open/close the connection.  */
+         chopstx_mutex_lock (&usb_mtx);
+         connected = (arg->value & CDC_CTRL_DTR)? 1 : 0;
+         chopstx_cond_signal (&usb_cnd);
+         chopstx_mutex_unlock (&usb_mtx);
        }
-      else if (req_no == USB_CDC_REQ_SET_CONTROL_LINE_STATE)
+#ifdef FRAUCHEKY_SUPPORT
+      else if (req_no == USB_CDC_REQ_SET_LINE_CODING)
        {
-         if (value != 0)
+         chopstx_mutex_lock (&usb_mtx);
+         if (line_coding.bitrate == NEUG_SPECIAL_BITRATE)
            {
-             if ((connected & 1) == 0)
-               /* It's Open call */
-               connected++;
+             fsij_device_state = FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED;
+             chopstx_cond_signal (&usb_cnd);
            }
-         else
-           /* Close call */
-           connected++;
+         else if (fsij_device_state == FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED)
+           fsij_device_state = FSIJ_DEVICE_RUNNING;
+         chopstx_mutex_unlock (&usb_mtx);
+       }
+#endif
+    }
+}
+
 
+static int
+vcom_port_data_setup (uint8_t req, uint8_t req_no, struct req_args *arg)
+{
+  if (USB_SETUP_GET (req))
+    {
+      if (req_no == USB_CDC_REQ_GET_LINE_CODING)
+       return usb_lld_reply_request (&line_coding, sizeof(line_coding), arg);
+    }
+  else  /* USB_SETUP_SET (req) */
+    {
+      if (req_no == USB_CDC_REQ_SET_LINE_CODING
+         && arg->len == sizeof (line_coding))
+       {
+         usb_lld_set_data_to_recv (&line_coding, sizeof (line_coding));
          return USB_SUCCESS;
        }
+      else if (req_no == USB_CDC_REQ_SET_CONTROL_LINE_STATE)
+       return USB_SUCCESS;
     }
 
   return USB_UNSUPPORT;
 }
 
-static int
-neug_setup (uint8_t req, uint8_t req_no,
-              uint16_t value, uint16_t index, uint16_t len)
+int
+usb_cb_setup (uint8_t req, uint8_t req_no, struct req_args *arg)
 {
   uint8_t type_rcp = req & (REQUEST_TYPE|RECIPIENT);
 
@@ -350,78 +393,195 @@ neug_setup (uint8_t req, uint8_t req_no,
     {
       if (USB_SETUP_GET (req))
        {
-         if (req_no == USB_REGNUAL_MEMINFO)
+         if (req_no == USB_FSIJ_MEMINFO)
            {
-             usb_lld_set_data_to_send (mem_info, sizeof (mem_info));
+             chopstx_mutex_lock (&usb_mtx);
+             if (fsij_device_state != FSIJ_DEVICE_EXITED)
+               {
+                 chopstx_mutex_unlock (&usb_mtx);
+                 return USB_UNSUPPORT;
+               }
+             chopstx_mutex_unlock (&usb_mtx);
+             usb_lld_reply_request (mem_info, sizeof (mem_info), arg);
              return USB_SUCCESS;
            }
-         else if (req_no == USB_REGNUAL_RESULT)
+         else if (req_no == USB_NEUG_GET_INFO)
            {
-             usb_lld_set_data_to_send (&result, sizeof (uint32_t));
+             if (arg->index == 0)
+               usb_lld_reply_request (&neug_mode, sizeof (uint8_t), arg);
+             else if (arg->index == 1)
+               usb_lld_reply_request (&neug_err_cnt, sizeof (uint16_t), arg);
+             else if (arg->index == 2)
+               usb_lld_reply_request (&neug_err_cnt_rc, sizeof (uint16_t), arg);
+             else if (arg->index == 3)
+               usb_lld_reply_request (&neug_err_cnt_p64, sizeof (uint16_t), arg);
+             else if (arg->index == 4)
+               usb_lld_reply_request (&neug_err_cnt_p4k, sizeof (uint16_t), arg);
+             else if (arg->index == 5)
+               usb_lld_reply_request (&neug_rc_max, sizeof (uint16_t), arg);
+             else if (arg->index == 6)
+               usb_lld_reply_request (&neug_p64_max, sizeof (uint16_t), arg);
+             else if (arg->index == 7)
+               usb_lld_reply_request (&neug_p4k_max, sizeof (uint16_t), arg);
+             else
+               return USB_UNSUPPORT;
+
              return USB_SUCCESS;
            }
        }
       else /* SETUP_SET */
        {
-         if (req_no == USB_REGNUAL_SEND)
+         uint8_t *addr = (uint8_t *)(0x20000000 + arg->value * 0x100 + arg->index);
+
+         if (req_no == USB_FSIJ_DOWNLOAD)
            {
-             if (value != 0 || index + len > 256)
+             chopstx_mutex_lock (&usb_mtx);
+             if (fsij_device_state != FSIJ_DEVICE_EXITED)
+               {
+                 chopstx_mutex_unlock (&usb_mtx);
+                 return USB_UNSUPPORT;
+               }
+             chopstx_mutex_unlock (&usb_mtx);
+
+             if (addr < &_regnual_start || addr + arg->len > &__heap_end__)
                return USB_UNSUPPORT;
 
-             if (index + len < 256)
-               memset (mem + index + len, 0xff, 256 - (index + len));
+             if (arg->index + arg->len < 256)
+               memset (addr + arg->index + arg->len, 0, 256 - (arg->index + arg->len));
 
-             usb_lld_set_data_to_recv (mem + index, len);
+             usb_lld_set_data_to_recv (addr, arg->len);
              return USB_SUCCESS;
            }
-         else if (req_no == USB_REGNUAL_FLASH && len == 0 && index == 0)
+         else if (req_no == USB_FSIJ_EXEC && arg->len == 0)
            {
-             uint32_t dst_addr = (0x08000000 + value * 0x100);
+             chopstx_mutex_lock (&usb_mtx);
+             if (fsij_device_state != FSIJ_DEVICE_EXITED)
+               {
+                 chopstx_mutex_unlock (&usb_mtx);
+                 return USB_UNSUPPORT;
+               }
+             chopstx_mutex_unlock (&usb_mtx);
+
+             if (((uint32_t)addr & 0x03))
+               return USB_UNSUPPORT;
 
-             if (dst_addr + 256 <= (uint32_t)&__flash_end__)
-               return USB_SUCCESS;
+             return download_check_crc32 ((uint32_t *)addr);
+           }
+         else if (req_no == USB_NEUG_SET_PASSWD && arg->len <= 32)
+           {
+             usbbuf[0] = arg->len;
+             usb_lld_set_data_to_recv (usbbuf + 1, arg->len);
+             return USB_SUCCESS;
+           }
+         else if (req_no == USB_NEUG_EXIT && arg->len <= 32)
+           {
+             chopstx_mutex_lock (&usb_mtx);
+             if (fsij_device_state != FSIJ_DEVICE_RUNNING)
+               {
+                 chopstx_mutex_unlock (&usb_mtx);
+                 return USB_UNSUPPORT;
+               }
+             chopstx_mutex_unlock (&usb_mtx);
+
+             usbbuf[0] = arg->len;
+             usb_lld_set_data_to_recv (usbbuf + 1, arg->len);
+             return USB_SUCCESS;
            }
-         else if (req_no == USB_REGNUAL_PROTECT && len == 0
-                  && value == 0 && index == 0)
-           return USB_SUCCESS;
-         else if (req_no == USB_REGNUAL_FINISH && len == 0
-                  && value == 0 && index == 0)
-           return USB_SUCCESS;
        }
     }
-  else if (type_rcp == (CLASS_REQUEST | INTERFACE_RECIPIENT))
-    if (index == 0)
-      return vcom_port_data_setup (req, req_no, value);
+  else if (type_rcp == (CLASS_REQUEST | INTERFACE_RECIPIENT)
+          && arg->index == 0)
+    {
+#ifdef FRAUCHEKY_SUPPORT
+      if (running_neug)
+       return vcom_port_data_setup (req, req_no, arg);
+      else
+       fraucheky_setup (req, req_no, arg);
+#else
+      return vcom_port_data_setup (req, req_no, arg);
+#endif
+    }
 
   return USB_UNSUPPORT;
 }
 
-static int
-neug_get_descriptor (uint8_t desc_type, uint16_t index, uint16_t value)
+int
+usb_cb_get_descriptor (uint8_t rcp, uint8_t desc_type, uint8_t desc_index,
+                      struct req_args *arg)
 {
-  (void)index;
+#ifdef FRAUCHEKY_SUPPORT
+  if (!running_neug)
+    return fraucheky_get_descriptor (rcp, desc_type, desc_index, arg);
+#endif
+
+  if (rcp != DEVICE_RECIPIENT)
+    return USB_UNSUPPORT;
+
   if (desc_type == DEVICE_DESCRIPTOR)
-    {
-      usb_lld_set_data_to_send (device_desc.Descriptor,
-                               device_desc.Descriptor_Size);
-      return USB_SUCCESS;
-    }
+    return usb_lld_reply_request (vcom_device_desc, sizeof (vcom_device_desc),
+                                 arg);
   else if (desc_type == CONFIG_DESCRIPTOR)
-    {
-      usb_lld_set_data_to_send (config_desc.Descriptor,
-                               config_desc.Descriptor_Size);
-      return USB_SUCCESS;
-    }
+    return usb_lld_reply_request (vcom_config_desc, sizeof (vcom_config_desc),
+                                 arg);
   else if (desc_type == STRING_DESCRIPTOR)
     {
-      uint8_t desc_index = value & 0xff;
+      const uint8_t *str;
+      int size;
 
-      if (desc_index < NUM_STRING_DESC)
+      switch (desc_index)
        {
-         usb_lld_set_data_to_send (string_descs[desc_index].Descriptor,
-                                   string_descs[desc_index].Descriptor_Size);
-         return USB_SUCCESS;
+       case 0:
+         str = vcom_string0;
+         size = sizeof (vcom_string0);
+         break;
+       case 1:
+         str = neug_string_vendor;
+         size = sizeof (neug_string_vendor);
+         break;
+       case 2:
+         str = neug_string_product;
+         size = sizeof (neug_string_product);
+         break;
+       case 3:
+         str = neug_string_serial;
+         size = sizeof (neug_string_serial);
+         break;
+       case 4:
+         str = neug_revision_detail;
+         size = sizeof (neug_revision_detail);
+         break;
+       case 5:
+         str = neug_config_options;
+         size = sizeof (neug_config_options);
+         break;
+       case 6:
+         str = sys_version;
+         size = sizeof (sys_version);
+         break;
+#ifdef USE_SYS3
+       case 7:
+         {
+           int i;
+           str = usbbuf;
+           for (i = 0; i < (int)sizeof (usbbuf)/2 - 2; i++)
+             {
+               if (sys_board_name[i] == 0)
+                 break;
+
+               usbbuf[i*2+2] = sys_board_name[i];
+               usbbuf[i*2+3] = 0;
+             }
+           usbbuf[0] = i*2 + 2;
+           usbbuf[1] = STRING_DESCRIPTOR;
+           size = i*2 + 2;
+         }
+         break;
+#endif
+       default:
+         return USB_UNSUPPORT;
        }
+
+      return usb_lld_reply_request (str, size, arg);
     }
 
   return USB_UNSUPPORT;
@@ -432,10 +592,22 @@ neug_setup_endpoints_for_interface (uint16_t interface, int stop)
 {
   if (interface == 0)
     {
+#ifdef FRAUCHEKY_SUPPORT
+      if (running_neug)
+       {
+         if (!stop)
+           usb_lld_setup_endpoint (ENDP2, EP_INTERRUPT, 0, 0, ENDP2_TXADDR, 0);
+         else
+           usb_lld_stall_tx (ENDP2);
+       }
+      else
+       fraucheky_setup_endpoints_for_interface (stop);
+#else
       if (!stop)
        usb_lld_setup_endpoint (ENDP2, EP_INTERRUPT, 0, 0, ENDP2_TXADDR, 0);
       else
        usb_lld_stall_tx (ENDP2);
+#endif
     }
   else if (interface == 1)
     {
@@ -443,6 +615,7 @@ neug_setup_endpoints_for_interface (uint16_t interface, int stop)
        {
          usb_lld_setup_endpoint (ENDP1, EP_BULK, 0, 0, ENDP1_TXADDR, 0);
          usb_lld_setup_endpoint (ENDP3, EP_BULK, 0, ENDP3_RXADDR, 0, 64);
+         /* Start with no data receiving (ENDP3 not enabled) */
        }
       else
        {
@@ -452,7 +625,7 @@ neug_setup_endpoints_for_interface (uint16_t interface, int stop)
     }
 }
 
-static int neug_usb_event (uint8_t event_type, uint16_t value)
+int usb_cb_handle_event (uint8_t event_type, uint16_t value)
 {
   int i;
   uint8_t current_conf;
@@ -460,7 +633,9 @@ static int neug_usb_event (uint8_t event_type, uint16_t value)
   switch (event_type)
     {
     case USB_EVENT_ADDRESS:
+      chopstx_mutex_lock (&usb_mtx);
       bDeviceState = ADDRESSED;
+      chopstx_mutex_unlock (&usb_mtx);
       return USB_SUCCESS;
     case USB_EVENT_CONFIG:
       current_conf = usb_lld_current_configuration ();
@@ -472,7 +647,9 @@ static int neug_usb_event (uint8_t event_type, uint16_t value)
          usb_lld_set_configuration (1);
          for (i = 0; i < NUM_INTERFACES; i++)
            neug_setup_endpoints_for_interface (i, 0);
+         chopstx_mutex_lock (&usb_mtx);
          bDeviceState = CONFIGURED;
+         chopstx_mutex_unlock (&usb_mtx);
        }
       else if (current_conf != value)
        {
@@ -482,12 +659,12 @@ static int neug_usb_event (uint8_t event_type, uint16_t value)
          usb_lld_set_configuration (0);
          for (i = 0; i < NUM_INTERFACES; i++)
            neug_setup_endpoints_for_interface (i, 1);
+         chopstx_mutex_lock (&usb_mtx);
          bDeviceState = ADDRESSED;
+         chopstx_mutex_unlock (&usb_mtx);
        }
       /* Do nothing when current_conf == value */
       return USB_SUCCESS;
-
-      return USB_SUCCESS;
     default:
       break;
     }
@@ -496,9 +673,11 @@ static int neug_usb_event (uint8_t event_type, uint16_t value)
 }
 
 
-static int neug_interface (uint8_t cmd, uint16_t interface, uint16_t alt)
+int usb_cb_interface (uint8_t cmd, struct req_args *arg)
 {
-  static uint8_t zero = 0;
+  const uint8_t zero = 0;
+  uint16_t interface = arg->index;
+  uint16_t alt = arg->value;
 
   if (interface >= NUM_INTERFACES)
     return USB_UNSUPPORT;
@@ -515,8 +694,7 @@ static int neug_interface (uint8_t cmd, uint16_t interface, uint16_t alt)
        }
 
     case USB_GET_INTERFACE:
-      usb_lld_set_data_to_send (&zero, 1);
-      return USB_SUCCESS;
+      return usb_lld_reply_request (&zero, 1, arg);
 
     default:
     case USB_QUERY_INTERFACE:
@@ -524,109 +702,213 @@ static int neug_interface (uint8_t cmd, uint16_t interface, uint16_t alt)
     }
 }
 
-const struct usb_device_method Device_Method = {
-  neug_device_reset,
-  neug_ctrl_write_finish,
-  neug_setup,
-  neug_get_descriptor,
-  neug_usb_event,
-  neug_interface,
-};
+#define INTR_REQ_USB 20
+#define PRIO_USB 3
 
+static void *
+usb_intr (void *arg)
+{
+  chopstx_intr_t interrupt;
+
+  (void)arg;
+  usb_lld_init (VCOM_FEATURE_BUS_POWERED);
+  chopstx_claim_irq (&interrupt, INTR_REQ_USB);
+  usb_interrupt_handler ();    /* For old SYS < 3.0 */
 
+  while (1)
+    {
+      chopstx_poll (NULL, 1, &interrupt);
+
+      /* Process interrupt. */
+      usb_interrupt_handler ();
+    }
 
+  return NULL;
+}
+
+#define ID_OFFSET (2+SERIALNO_STR_LEN*2)
 static void fill_serial_no_by_unique_id (void)
 {
   extern const uint8_t * unique_device_id (void);
-  uint8_t *p = &vcom_string3[12];
-  const uint8_t *u = unique_device_id ();
+  uint8_t *p = &neug_string_serial[ID_OFFSET];
+  const uint8_t *u = unique_device_id () + 8;
   int i;
 
   for (i = 0; i < 4; i++)
     {
-      uint8_t b = u[i];
-      uint8_t nibble; 
+      uint8_t b = u[3-i];
+      uint8_t nibble;
 
       nibble = (b >> 4);
       nibble += (nibble >= 10 ? ('A' - 10) : '0');
       p[i*4] = nibble;
+      p[i*4+1] = 0;
       nibble = (b & 0x0f);
       nibble += (nibble >= 10 ? ('A' - 10) : '0');
       p[i*4+2] = nibble;
+      p[i*4+3] = 0;
     }
 }
-\f
-static Thread *main_thread = NULL;
 
-CH_IRQ_HANDLER (Vector90)
-{
-  CH_IRQ_PROLOGUE();
-  chSysLockFromIsr();
-
-  usb_interrupt_handler ();
-
-  chSysUnlockFromIsr();
-  CH_IRQ_EPILOGUE();
-}
 
 void
-EP1_IN_Callback (void)
+usb_cb_tx_done (uint8_t ep_num, uint32_t len)
 {
-  if (main_thread != NULL)
+  (void)len;
+
+  if (ep_num == ENDP1)
+    {
+      chopstx_mutex_lock (&usb_mtx);
+      chopstx_cond_signal (&usb_cnd);
+      chopstx_mutex_unlock (&usb_mtx);
+    }
+  else if (ep_num == ENDP2)
     {
-      chSysLockFromIsr ();
-      main_thread->p_u.rdymsg = RDY_OK;
-      chSchReadyI (main_thread);
-      chSysUnlockFromIsr ();
+      /* INTERRUPT Transfer done */
     }
+#ifdef FRAUCHEKY_SUPPORT
+  else if (ep_num == ENDP6)
+    EP6_IN_Callback (len);
+#endif
 }
 
 void
-EP2_IN_Callback (void)
+usb_cb_rx_ready (uint8_t ep_num)
 {
+  if (ep_num == ENDP3)
+    usb_lld_rx_enable (ENDP3);
+#ifdef FRAUCHEKY_SUPPORT
+  else if (ep_num == ENDP6)
+    EP6_OUT_Callback ();
+#endif
 }
+\f
+typedef uint32_t eventmask_t;
+#define ALL_EVENTS (~0)
 
-void
-EP3_OUT_Callback (void)
+struct event_flag {
+  chopstx_mutex_t mutex;
+  chopstx_cond_t cond;
+  eventmask_t flag;
+};
+
+static void event_flag_init (struct event_flag *ev)
 {
-  usb_lld_rx_enable (ENDP3);
+  ev->flag = 0;
+  chopstx_mutex_init (&ev->mutex);
+  chopstx_cond_init (&ev->cond);
 }
-\f
-static WORKING_AREA(wa_led, 64);
 
-#define LED_ONESHOT_SHORT ((eventmask_t)1)
-#define LED_ONESHOT_LONG  ((eventmask_t)2)
-static Thread *led_thread;
+
+static eventmask_t event_flag_waitone (struct event_flag *ev, eventmask_t m)
+{
+  int n;
+
+  chopstx_mutex_lock (&ev->mutex);
+  while (!(ev->flag & m))
+    chopstx_cond_wait (&ev->cond, &ev->mutex);
+
+  n = __builtin_ffs ((ev->flag & m));
+  ev->flag &= ~(1 << (n - 1));
+  chopstx_mutex_unlock (&ev->mutex);
+
+  return (1 << (n - 1));
+}
+
+static void event_flag_signal (struct event_flag *ev, eventmask_t m)
+{
+  chopstx_mutex_lock (&ev->mutex);
+  ev->flag |= m;
+  chopstx_cond_signal (&ev->cond);
+  chopstx_mutex_unlock (&ev->mutex);
+}
+
+extern uint8_t __process1_stack_base__, __process1_stack_size__;
+extern uint8_t __process3_stack_base__, __process3_stack_size__;
+
+const uint32_t __stackaddr_led = (uint32_t)&__process1_stack_base__;
+const size_t __stacksize_led = (size_t)&__process1_stack_size__;
+const uint32_t __stackaddr_usb = (uint32_t)&__process3_stack_base__;
+const size_t __stacksize_usb = (size_t)&__process3_stack_size__;
+
+
+#define PRIO_LED 3
+static struct event_flag led_event;
+
+#define LED_ONESHOT_SHORT      ((eventmask_t)1)
+#define LED_TWOSHOTS           ((eventmask_t)2)
+#define LED_ONESHOT_LONG       ((eventmask_t)4)
 
 /*
  * LED blinker: When notified, let LED emit for 100ms.
  */
-static msg_t led_blinker (void *arg)
+static void *
+led_blinker (void *arg)
 {
   (void)arg;
 
-  led_thread = chThdSelf ();
   set_led (0);
 
   while (1)
     {
       eventmask_t m;
 
-      m = chEvtWaitOne (ALL_EVENTS);
+      m = event_flag_waitone (&led_event, ALL_EVENTS);
+
       set_led (1);
       if (m == LED_ONESHOT_SHORT)
-       chThdSleep (MS2ST (100));
+       chopstx_usec_wait (100*1000);
+      else if (m == LED_TWOSHOTS)
+       {
+         chopstx_usec_wait (50*1000);
+         set_led (0);
+         chopstx_usec_wait (50*1000);
+         set_led (1);
+         chopstx_usec_wait (50*1000);
+       }
       else
-       chThdSleep (MS2ST (250));
+       chopstx_usec_wait (250*1000);
       set_led (0);
     }
 
-  return 0;
+  return NULL;
 }
 \f
-#define RANDOM_BYTES_LENGTH 60
+#define RANDOM_BYTES_LENGTH 64
 static uint32_t random_word[RANDOM_BYTES_LENGTH/sizeof (uint32_t)];
 
+static void copy_to_tx (uint32_t v, int i)
+{
+  usb_lld_txcpy (&v, ENDP1, i * 4, 4);
+}
+
+/*
+ * In Gnuk 1.0.[12], reGNUal was not relocatable.
+ * Now, it's relocatable, but we need to calculate its entry address
+ * based on it's pre-defined address.
+ */
+#define REGNUAL_START_ADDRESS_COMPATIBLE 0x20001400
+static uint32_t
+calculate_regnual_entry_address (const uint8_t *addr)
+{
+  const uint8_t *p = addr + 4;
+  uint32_t v = p[0] + (p[1] << 8) + (p[2] << 16) + (p[3] << 24);
+
+  v -= REGNUAL_START_ADDRESS_COMPATIBLE;
+  v += (uint32_t)addr;
+  return v;
+}
+
+
+static int
+check_usb_status (void *arg)
+{
+  (void)arg;
+
+  return (connected || bDeviceState != CONFIGURED
+         || fsij_device_state != FSIJ_DEVICE_RUNNING);
+}
+
 /*
  * Entry point.
  *
@@ -635,71 +917,244 @@ static uint32_t random_word[RANDOM_BYTES_LENGTH/sizeof (uint32_t)];
 int
 main (int argc, char **argv)
 {
+  uint32_t entry;
+  chopstx_t led_thread, usb_thd;
+  unsigned int count;
+
   (void)argc;
   (void)argv;
 
   fill_serial_no_by_unique_id ();
 
-  halInit();
-  chSysInit();
+  adc_init ();
 
-  main_thread = chThdSelf ();
+  event_flag_init (&led_event);
 
-  usb_lld_init (config_desc.Descriptor[7]);
+  chopstx_mutex_init (&usb_mtx);
+  chopstx_cond_init (&usb_cnd);
 
-  chThdCreateStatic (wa_led, sizeof (wa_led), NORMALPRIO, led_blinker, NULL);
+#ifdef FRAUCHEKY_SUPPORT
+  if (fraucheky_enabled ())
+    {
+    go_fraucheky:
+      bDeviceState = UNCONNECTED;
+      running_neug = 0;
+      usb_thd = chopstx_create (PRIO_USB, __stackaddr_usb, __stacksize_usb,
+                               usb_intr, NULL);
+      while (bDeviceState != CONFIGURED)
+       chopstx_usec_wait (250*1000);
+      set_led (1);
+      fraucheky_main ();
+      chopstx_cancel (usb_thd);
+      chopstx_join (usb_thd, NULL);
+      usb_lld_shutdown ();
+      bDeviceState = UNCONNECTED;
+    }
+
+  running_neug = 1;
+#endif
+
+  led_thread = chopstx_create (PRIO_LED, __stackaddr_led, __stacksize_led,
+                              led_blinker, NULL);
+
+  usb_thd = chopstx_create (PRIO_USB, __stackaddr_usb, __stacksize_usb,
+                           usb_intr, NULL);
 
   neug_init (random_word, RANDOM_BYTES_LENGTH/sizeof (uint32_t));
 
+  chopstx_mutex_lock (&usb_mtx);
+
+ not_configured:
+  count = 0;
+  /* Initial run-up */
+  while (count < NEUG_PRE_LOOP || bDeviceState != CONFIGURED)
+    {
+      if (fsij_device_state != FSIJ_DEVICE_RUNNING)
+       break;
+
+      chopstx_mutex_unlock (&usb_mtx);
+      neug_wait_full ();
+      neug_flush ();
+
+      if ((count & 0x0007) == 0)
+       event_flag_signal (&led_event, LED_ONESHOT_SHORT);
+      chopstx_usec_wait (25*1000);
+      count++;
+      chopstx_mutex_lock (&usb_mtx);
+    }
+
+  /* Holding USB_MTX...  */
   while (1)
     {
-      unsigned int count = 0;
+      int last_was_fullsizepacket = 0;
 
-      connected = 0;
-      while (count < NEUG_PRE_LOOP || bDeviceState != CONFIGURED)
-       {
-         (void)neug_get (NEUG_KICK_FILLING);
-         if ((count & 0x000f) == 0)
-           chEvtSignalFlags (led_thread, LED_ONESHOT_SHORT);
-         chThdSleep (MS2ST (25));
-         count++;
-       }
+      if (fsij_device_state != FSIJ_DEVICE_RUNNING)
+       break;
 
-    waiting_connection:
-      while ((connected & 1) == 0)
+      chopstx_mutex_unlock (&usb_mtx);
+      while (1)
        {
+         chopstx_poll_cond_t poll_desc;
+         uint32_t usec = 5000*1000;
+
+         poll_desc.type = CHOPSTX_POLL_COND;
+         poll_desc.ready = 0;
+         poll_desc.cond = &usb_cnd;
+         poll_desc.mutex = &usb_mtx;
+         poll_desc.check = check_usb_status;
+         poll_desc.arg = NULL;
+
+         if (chopstx_poll (&usec, 1, &poll_desc))
+           break;
+
+         /* Timeout */
          neug_flush ();
-         chEvtSignalFlags (led_thread, LED_ONESHOT_LONG);
-         chThdSleep (MS2ST (2500));
+         neug_mode_select (line_coding.paritytype);
+         event_flag_signal (&led_event, LED_TWOSHOTS);
        }
 
+      chopstx_mutex_lock (&usb_mtx);
+      if (bDeviceState != CONFIGURED)
+       goto not_configured;
+
       /* The connection opened.  */
       count = 0;
 
       while (1)
        {
-         if ((connected & 1) == 0)
-           goto waiting_connection;
-
-         if (bDeviceState != CONFIGURED)
-           break;
-
-         neug_wait_full ();
+         int i;
+
+         chopstx_mutex_unlock (&usb_mtx);
+         /*
+          * No parity is standard.  It means providing conditioned output.
+          * When parity enabled, it means to provide raw output
+          * (CRC32 filtered when odd, direct sample of ADC when even).
+          *
+          * line_coding.paritytype:
+          *   0: None, 1: Odd, 2: Even
+          */
+         neug_mode_select (line_coding.paritytype);
+
+         if ((count & 0x03ff) == 0)
+           event_flag_signal (&led_event, LED_ONESHOT_SHORT);
+
+         i = neug_consume_random (copy_to_tx);
+
+         if (i == 0 && !last_was_fullsizepacket)
+           {    /* Only send ZLP when the last packet was fullsize.  */
+             neug_wait_full ();
+
+             chopstx_mutex_lock (&usb_mtx);
+             if (bDeviceState != CONFIGURED || !connected
+                 || fsij_device_state != FSIJ_DEVICE_RUNNING)
+               break;
+           }
+         else
+           {
+             if (i == 64/4)
+               last_was_fullsizepacket = 1;
+             else
+               last_was_fullsizepacket = 0;
+
+             chopstx_mutex_lock (&usb_mtx);
+             if (bDeviceState != CONFIGURED || !connected
+                 || fsij_device_state != FSIJ_DEVICE_RUNNING)
+               break;
+
+             /* Prepare sending random data.  */
+             usb_lld_tx_enable (ENDP1, i * 4);
+             chopstx_cond_wait (&usb_cnd, &usb_mtx);
+             count++;
+           }
+       }
+    }
 
-         if ((count & 0x00ff) == 0)
-           chEvtSignalFlags (led_thread, LED_ONESHOT_SHORT);
+  chopstx_mutex_unlock (&usb_mtx);
 
-         usb_lld_txcpy (random_word, ENDP1, 0, RANDOM_BYTES_LENGTH);
-         neug_flush ();
+  chopstx_cancel (led_thread);
+  chopstx_join (led_thread, NULL);
 
-         chSysLock ();
-         usb_lld_tx_enable (ENDP1, RANDOM_BYTES_LENGTH);
-         chSchGoSleepS (THD_STATE_SUSPENDED);
-         chSysUnlock();
+  /*
+   * We come here, because of FSIJ_DEVICE_NEUG_EXIT_REQUESTED
+   * or FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED.
+   */
+  neug_fini ();
 
-         count++;
+  chopstx_mutex_lock (&usb_mtx);
+#ifdef FRAUCHEKY_SUPPORT
+  if (line_coding.bitrate == NEUG_SPECIAL_BITRATE)
+    {
+      while (fsij_device_state == FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED)
+       {
+         chopstx_mutex_unlock (&usb_mtx);
+         set_led (1);
+         chopstx_usec_wait (500*1000);
+         set_led (0);
+         chopstx_usec_wait (500*1000);
+         chopstx_mutex_lock (&usb_mtx);
        }
+
+      usb_lld_prepare_shutdown ();
+      chopstx_mutex_unlock (&usb_mtx);
+      chopstx_cancel (usb_thd);
+      chopstx_join (usb_thd, NULL);
+      usb_lld_shutdown ();
+      goto go_fraucheky;
     }
+#endif
 
+  fsij_device_state = FSIJ_DEVICE_EXITED;
+
+  while (fsij_device_state == FSIJ_DEVICE_EXITED)
+    {
+      chopstx_mutex_unlock (&usb_mtx);
+      chopstx_usec_wait (500*1000);
+      chopstx_mutex_lock (&usb_mtx);
+    }
+  chopstx_mutex_unlock (&usb_mtx);
+
+  flash_unlock ();             /* Flash unlock should be done here */
+  set_led (1);
+  usb_lld_shutdown ();
+
+  /* Finish application.  */
+  chopstx_cancel (usb_thd);
+  chopstx_join (usb_thd, NULL);
+
+  /* Set vector */
+  SCB->VTOR = (uint32_t)&_regnual_start;
+  entry = calculate_regnual_entry_address (&_regnual_start);
+#ifdef DFU_SUPPORT
+#define FLASH_SYS_START_ADDR 0x08000000
+#define FLASH_SYS_END_ADDR (0x08000000+0x1000)
+#define CHIP_ID_REG ((uint32_t *)0xE0042000)
+  {
+    extern uint8_t _sys;
+    uint32_t addr;
+    handler *new_vector = (handler *)FLASH_SYS_START_ADDR;
+    void (*func) (void (*)(void)) = (void (*)(void (*)(void)))new_vector[9];
+    uint32_t flash_page_size = 1024; /* 1KiB default */
+
+   if ((*CHIP_ID_ADDR)&0x07 == 0x04) /* High dencity device.  */
+     flash_page_size += 0x0400; /* It's 2KiB. */
+
+    /* Kill DFU */
+    for (addr = FLASH_SYS_START_ADDR; addr < FLASH_SYS_END_ADDR;
+        addr += flash_page_size)
+      flash_erase_page (addr);
+
+    /* copy system service routines */
+    flash_write (FLASH_SYS_START_ADDR, &_sys, 0x1000);
+
+    /* Leave NeuG to exec reGNUal */
+    (*func) ((void (*)(void))entry);
+    for (;;);
+  }
+#else
+  /* Leave NeuG to exec reGNUal */
+  flash_erase_all_and_exec ((void (*)(void))entry);
+#endif
+
+  /* Never reached */
   return 0;
 }