Fix USB suspend for Fraucheky.
[gnuk/neug.git] / src / main.c
1 /*
2  * main.c - main routine of neug
3  *
4  * Main routine:
5  * Copyright (C) 2011, 2012, 2013, 2015, 2016, 2017, 2018
6  *               Free Software Initiative of Japan
7  * Author: NIIBE Yutaka <gniibe@fsij.org>
8  *
9  * This file is a part of NeuG, a True Random Number Generator
10  * implementation.
11  *
12  * NeuG is free software: you can redistribute it and/or modify it
13  * under the terms of the GNU General Public License as published by
14  * the Free Software Foundation, either version 3 of the License, or
15  * (at your option) any later version.
16  *
17  * NeuG is distributed in the hope that it will be useful, but WITHOUT
18  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
19  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
20  * License for more details.
21  *
22  * You should have received a copy of the GNU General Public License
23  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
24  *
25  */
26
27
28 #include <stdint.h>
29 #include <string.h>
30 #include <chopstx.h>
31 #include <eventflag.h>
32
33 #include "config.h"
34 #include "neug.h"
35 #include "usb_lld.h"
36 #include "sys.h"
37 #ifdef GNU_LINUX_EMULATION
38 #include <stdio.h>
39 #include <stdlib.h>
40 #define main emulated_main
41 #else
42 #include "mcu/stm32f103.h"
43 #define FLASH_UPGRADE_SUPPORT 1
44 #endif
45 #include "adc.h"
46
47 #define RANDOM_BYTES_LENGTH 64
48 static uint32_t random_word[RANDOM_BYTES_LENGTH/sizeof (uint32_t)];
49
50 enum {
51   FSIJ_DEVICE_RUNNING = 0,
52   FSIJ_DEVICE_EXITED,
53   FSIJ_DEVICE_EXEC_REQUESTED,
54   /**/
55   FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED = 254,
56   FSIJ_DEVICE_NEUG_EXIT_REQUESTED = 255
57 };
58
59 #ifdef FRAUCHEKY_SUPPORT
60 static uint8_t running_neug;
61 extern void EP6_IN_Callback (uint16_t len);
62 extern void EP6_OUT_Callback (uint16_t len);
63 #endif
64
65 static chopstx_mutex_t usb_mtx;
66 static chopstx_cond_t usb_cnd;
67 static uint32_t bDeviceState = USB_DEVICE_STATE_UNCONNECTED;
68 static uint8_t fsij_device_state = FSIJ_DEVICE_RUNNING;
69 static uint8_t connected;
70
71
72 #define ENDP0_RXADDR        (0x40)
73 #define ENDP0_TXADDR        (0x80)
74 #define ENDP1_TXADDR        (0xc0)
75 #define ENDP2_TXADDR        (0x100)
76 #define ENDP3_RXADDR        (0x10A)
77 /* Next available space for USB is: 0x14A */
78
79 #define USB_CDC_REQ_SET_LINE_CODING             0x20
80 #define USB_CDC_REQ_GET_LINE_CODING             0x21
81 #define USB_CDC_REQ_SET_CONTROL_LINE_STATE      0x22
82 #define USB_CDC_REQ_SEND_BREAK                  0x23
83
84 /* USB Device Descriptor */
85 static
86 #if !defined(GNU_LINUX_EMULATION)
87 const
88 #endif
89 uint8_t vcom_device_desc[18] = {
90   18,   /* bLength */
91   DEVICE_DESCRIPTOR,            /* bDescriptorType */
92   0x10, 0x01,                   /* bcdUSB = 1.1 */
93   0x02,                         /* bDeviceClass (CDC).              */
94   0x00,                         /* bDeviceSubClass.                 */
95   0x00,                         /* bDeviceProtocol.                 */
96   0x40,                         /* bMaxPacketSize.                  */
97 #include "usb-vid-pid-ver.c.inc"
98   1,                            /* iManufacturer.                   */
99   2,                            /* iProduct.                        */
100   3,                            /* iSerialNumber.                   */
101   1                             /* bNumConfigurations.              */
102 };
103
104 #define VCOM_FEATURE_BUS_POWERED        0x80
105
106 /* Configuration Descriptor tree for a CDC.*/
107 static const uint8_t vcom_config_desc[67] = {
108   9,
109   CONFIG_DESCRIPTOR,            /* bDescriptorType: Configuration   */
110   /* Configuration Descriptor.*/
111   67, 0x00,                     /* wTotalLength.                    */
112   0x02,                         /* bNumInterfaces.                  */
113   0x01,                         /* bConfigurationValue.             */
114   0,                            /* iConfiguration.                  */
115   VCOM_FEATURE_BUS_POWERED,     /* bmAttributes.                    */
116   50,                           /* bMaxPower (100mA).               */
117   /* Interface Descriptor.*/
118   9,
119   INTERFACE_DESCRIPTOR,
120   0x00,            /* bInterfaceNumber.                */
121   0x00,            /* bAlternateSetting.               */
122   0x01,            /* bNumEndpoints.                   */
123   0x02,            /* bInterfaceClass (Communications Interface Class,
124                       CDC section 4.2).  */
125   0x02,            /* bInterfaceSubClass (Abstract Control Model, CDC
126                       section 4.3).  */
127   0x01,            /* bInterfaceProtocol (AT commands, CDC section
128                       4.4).  */
129   0,               /* iInterface.                      */
130   /* Header Functional Descriptor (CDC section 5.2.3).*/
131   5,          /* bLength.                         */
132   0x24,       /* bDescriptorType (CS_INTERFACE).  */
133   0x00,       /* bDescriptorSubtype (Header Functional Descriptor). */
134   0x10, 0x01, /* bcdCDC.                          */
135   /* Call Management Functional Descriptor. */
136   5,            /* bFunctionLength.                 */
137   0x24,         /* bDescriptorType (CS_INTERFACE).  */
138   0x01,         /* bDescriptorSubtype (Call Management Functional
139                    Descriptor). */
140   0x03,         /* bmCapabilities (D0+D1).          */
141   0x01,         /* bDataInterface.                  */
142   /* ACM Functional Descriptor.*/
143   4,            /* bFunctionLength.                 */
144   0x24,         /* bDescriptorType (CS_INTERFACE).  */
145   0x02,         /* bDescriptorSubtype (Abstract Control Management
146                    Descriptor).  */
147   0x02,         /* bmCapabilities.                  */
148   /* Union Functional Descriptor.*/
149   5,            /* bFunctionLength.                 */
150   0x24,         /* bDescriptorType (CS_INTERFACE).  */
151   0x06,         /* bDescriptorSubtype (Union Functional
152                    Descriptor).  */
153   0x00,         /* bMasterInterface (Communication Class
154                    Interface).  */
155   0x01,         /* bSlaveInterface0 (Data Class Interface).  */
156   /* Endpoint 2 Descriptor.*/
157   7,
158   ENDPOINT_DESCRIPTOR,
159   ENDP2|0x80,    /* bEndpointAddress.    */
160   0x03,          /* bmAttributes (Interrupt).        */
161   0x0A, 0x00,    /* wMaxPacketSize.                  */
162   0xFF,          /* bInterval.                       */
163   /* Interface Descriptor.*/
164   9,
165   INTERFACE_DESCRIPTOR,          /* bDescriptorType: */
166   0x01,          /* bInterfaceNumber.                */
167   0x00,          /* bAlternateSetting.               */
168   0x02,          /* bNumEndpoints.                   */
169   0x0A,          /* bInterfaceClass (Data Class Interface, CDC section 4.5). */
170   0x00,          /* bInterfaceSubClass (CDC section 4.6). */
171   0x00,          /* bInterfaceProtocol (CDC section 4.7). */
172   0x00,          /* iInterface.                      */
173   /* Endpoint 3 Descriptor.*/
174   7,
175   ENDPOINT_DESCRIPTOR,          /* bDescriptorType: Endpoint */
176   ENDP3,    /* bEndpointAddress. */
177   0x02,                         /* bmAttributes (Bulk).             */
178   0x40, 0x00,                   /* wMaxPacketSize.                  */
179   0x00,                         /* bInterval.                       */
180   /* Endpoint 1 Descriptor.*/
181   7,
182   ENDPOINT_DESCRIPTOR,          /* bDescriptorType: Endpoint */
183   ENDP1|0x80,                   /* bEndpointAddress. */
184   0x02,                         /* bmAttributes (Bulk).             */
185   0x40, 0x00,                   /* wMaxPacketSize.                  */
186   0x00                          /* bInterval.                       */
187 };
188
189
190 /*
191  * U.S. English language identifier.
192  */
193 static const uint8_t vcom_string0[4] = {
194   4,                            /* bLength */
195   STRING_DESCRIPTOR,
196   0x09, 0x04                    /* LangID = 0x0409: US-English */
197 };
198
199 #include "usb-strings.c.inc"
200
201 static void neug_setup_endpoints_for_interface (struct usb_dev *dev,
202                                                 uint16_t interface, int stop);
203 #ifdef FRAUCHEKY_SUPPORT
204 #define MSC_MASS_STORAGE_RESET_COMMAND 0xFF
205 extern int fraucheky_enabled (void);
206 extern void fraucheky_main (void);
207
208 extern void fraucheky_setup_endpoints_for_interface (int stop);
209 extern int fraucheky_setup (struct usb_dev *dev);
210 extern int fraucheky_get_descriptor (struct usb_dev *dev);
211 #endif
212
213 #define NUM_INTERFACES 2
214
215 static void
216 usb_device_reset (struct usb_dev *dev)
217 {
218   int i;
219
220   usb_lld_reset (dev, VCOM_FEATURE_BUS_POWERED);
221
222   /* Initialize Endpoint 0.  */
223 #ifdef GNU_LINUX_EMULATION
224   usb_lld_setup_endp (dev, ENDP0, 1, 1);
225 #else
226   usb_lld_setup_endpoint (ENDP0, EP_CONTROL, 0, ENDP0_RXADDR, ENDP0_TXADDR, 64);
227 #endif
228
229   /* Stop the interface */
230   for (i = 0; i < NUM_INTERFACES; i++)
231     neug_setup_endpoints_for_interface (dev, i, 1);
232
233   /* Notify upper layer.  */
234   chopstx_mutex_lock (&usb_mtx);
235   bDeviceState = USB_DEVICE_STATE_ATTACHED;
236   connected = 0;
237   chopstx_cond_signal (&usb_cnd);
238   chopstx_mutex_unlock (&usb_mtx);
239 }
240
241 #ifdef FLASH_UPGRADE_SUPPORT
242 extern uint8_t _regnual_start, __heap_end__;
243
244 static const uint8_t *const mem_info[] = { &_regnual_start,  &__heap_end__, };
245 #endif
246
247 /* USB vendor requests to control pipe */
248 #define USB_FSIJ_MEMINFO          0
249 #define USB_FSIJ_DOWNLOAD         1
250 #define USB_FSIJ_EXEC             2
251 #define USB_NEUG_SET_PASSWD     253
252 #define USB_NEUG_GET_INFO       254
253 #define USB_NEUG_EXIT           255 /* Ask to exit and to receive reGNUal */
254
255 #define DEFAULT_PASSWD "12345678"
256 #define DEFAULT_PASSWD_LEN 8
257
258 uint8_t neug_passwd[33] __attribute__ ((section(".passwd"))) = {
259   0xff,
260   0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
261   0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
262   0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
263 };
264 static uint8_t usbbuf[64];
265
266 #ifdef FLASH_UPGRADE_SUPPORT
267 static void set_passwd (void)
268 {
269   flash_unlock ();
270   if (neug_passwd[0] != 0xff)
271     flash_erase_page ((uintptr_t)neug_passwd);
272   if (usbbuf[0] == DEFAULT_PASSWD_LEN
273       && !memcmp (usbbuf + 1, DEFAULT_PASSWD, DEFAULT_PASSWD_LEN))
274     return;
275   flash_write ((uintptr_t)neug_passwd, usbbuf, usbbuf[0] + 1);
276 }
277
278
279 /* After calling this function, CRC module remain enabled.  */
280 static int download_check_crc32 (struct usb_dev *dev, const uint32_t *end_p)
281 {
282   uint32_t crc32 = *end_p;
283   const uint32_t *p;
284
285   crc32_rv_reset ();
286
287   for (p = (const uint32_t *)&_regnual_start; p < end_p; p++)
288     crc32_rv_step (rbit (*p));
289
290   if ((rbit (crc32_rv_get ()) ^ crc32) == 0xffffffff)
291     return usb_lld_ctrl_ack (dev);
292
293   return -1;
294 }
295 #endif
296
297
298 #define NEUG_SPECIAL_BITRATE 110
299
300 struct line_coding
301 {
302   uint32_t bitrate;
303   uint8_t format;
304   uint8_t paritytype;
305   uint8_t datatype;
306 } __attribute__((packed));
307
308 static struct line_coding line_coding = {
309   115200, /* baud rate: 115200    */
310   0x00,   /* stop bits: 1         */
311   0x00,   /* parity:    none      */
312   0x08    /* bits:      8         */
313 };
314
315 #define CDC_CTRL_DTR            0x0001
316
317 static void
318 usb_ctrl_write_finish (struct usb_dev *dev)
319 {
320   struct device_req *arg = &dev->dev_req;
321   uint8_t type_rcp = arg->type & (REQUEST_TYPE|RECIPIENT);
322
323   if (type_rcp == (VENDOR_REQUEST | DEVICE_RECIPIENT)
324       && USB_SETUP_SET (arg->type))
325     {
326       if (arg->request == USB_FSIJ_EXEC)
327         {
328           chopstx_mutex_lock (&usb_mtx);
329           if (fsij_device_state == FSIJ_DEVICE_EXITED)
330             {
331               usb_lld_prepare_shutdown (); /* No further USB communication */
332               fsij_device_state = FSIJ_DEVICE_EXEC_REQUESTED;
333             }
334           chopstx_mutex_unlock (&usb_mtx);
335         }
336 #ifdef FLASH_UPGRADE_SUPPORT
337       else if (arg->request == USB_NEUG_SET_PASSWD)
338         set_passwd ();
339 #endif
340       else if (arg->request == USB_NEUG_EXIT)
341         {
342           if ((neug_passwd[0] == 0xff && usbbuf[0] == DEFAULT_PASSWD_LEN
343                && !memcmp (usbbuf + 1, DEFAULT_PASSWD, DEFAULT_PASSWD_LEN))
344 #ifdef FLASH_UPGRADE_SUPPORT
345               || (neug_passwd[0] == usbbuf[0]
346                   && !memcmp (neug_passwd+1, usbbuf+1, neug_passwd[0]))
347 #endif
348               )
349             {
350               chopstx_mutex_lock (&usb_mtx);
351               fsij_device_state = FSIJ_DEVICE_NEUG_EXIT_REQUESTED;
352               chopstx_cond_signal (&usb_cnd);
353               chopstx_mutex_unlock (&usb_mtx);
354             }
355         }
356     }
357   else if (type_rcp == (CLASS_REQUEST | INTERFACE_RECIPIENT) && arg->index == 0
358            && USB_SETUP_SET (arg->type))
359     {
360       if (arg->request == USB_CDC_REQ_SET_CONTROL_LINE_STATE)
361         {
362           /* Open/close the connection.  */
363           chopstx_mutex_lock (&usb_mtx);
364           connected = (arg->value & CDC_CTRL_DTR)? 1 : 0;
365           chopstx_cond_signal (&usb_cnd);
366           chopstx_mutex_unlock (&usb_mtx);
367         }
368 #ifdef FRAUCHEKY_SUPPORT
369       else if (running_neug && arg->request == USB_CDC_REQ_SET_LINE_CODING)
370         {
371           chopstx_mutex_lock (&usb_mtx);
372           if (line_coding.bitrate == NEUG_SPECIAL_BITRATE)
373             {
374               fsij_device_state = FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED;
375               chopstx_cond_signal (&usb_cnd);
376             }
377           else if (fsij_device_state == FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED)
378             fsij_device_state = FSIJ_DEVICE_RUNNING;
379           chopstx_mutex_unlock (&usb_mtx);
380         }
381       else if (!running_neug && arg->request == MSC_MASS_STORAGE_RESET_COMMAND)
382         fraucheky_setup_endpoints_for_interface (0);
383 #endif
384     }
385 }
386
387
388 static int
389 vcom_port_data_setup (struct usb_dev *dev)
390 {
391   struct device_req *arg = &dev->dev_req;
392
393   if (USB_SETUP_GET (arg->type))
394     {
395       if (arg->request == USB_CDC_REQ_GET_LINE_CODING)
396         return usb_lld_ctrl_send (dev, &line_coding, sizeof(line_coding));
397     }
398   else  /* USB_SETUP_SET (req) */
399     {
400       if (arg->request == USB_CDC_REQ_SET_LINE_CODING
401           && arg->len == sizeof (line_coding))
402         return usb_lld_ctrl_recv (dev, &line_coding, sizeof (line_coding));
403       else if (arg->request == USB_CDC_REQ_SET_CONTROL_LINE_STATE)
404         return usb_lld_ctrl_ack (dev);
405     }
406
407   return -1;
408 }
409
410 static int
411 usb_setup (struct usb_dev *dev)
412 {
413   struct device_req *arg = &dev->dev_req;
414   uint8_t type_rcp = arg->type & (REQUEST_TYPE|RECIPIENT);
415
416   if (type_rcp == (VENDOR_REQUEST | DEVICE_RECIPIENT))
417     {
418       if (USB_SETUP_GET (arg->type))
419         {
420           if (arg->request == USB_FSIJ_MEMINFO)
421             {
422 #ifdef FLASH_UPGRADE_SUPPORT
423               chopstx_mutex_lock (&usb_mtx);
424               if (fsij_device_state != FSIJ_DEVICE_EXITED)
425                 {
426                   chopstx_mutex_unlock (&usb_mtx);
427                   return -1;
428                 }
429               chopstx_mutex_unlock (&usb_mtx);
430               return usb_lld_ctrl_send (dev, mem_info, sizeof (mem_info));
431 #else
432               return -1;
433 #endif
434             }
435           else if (arg->request == USB_NEUG_GET_INFO)
436             {
437               if (arg->index == 0)
438                 return usb_lld_ctrl_send (dev, &neug_mode, sizeof (uint8_t));
439               else if (arg->index == 1)
440                 return usb_lld_ctrl_send (dev, &neug_err_cnt, sizeof (uint16_t));
441               else if (arg->index == 2)
442                 return usb_lld_ctrl_send (dev, &neug_err_cnt_rc, sizeof (uint16_t));
443               else if (arg->index == 3)
444                 return usb_lld_ctrl_send (dev, &neug_err_cnt_p64, sizeof (uint16_t));
445               else if (arg->index == 4)
446                 return usb_lld_ctrl_send (dev, &neug_err_cnt_p4k, sizeof (uint16_t));
447               else if (arg->index == 5)
448                 return usb_lld_ctrl_send (dev, &neug_rc_max, sizeof (uint16_t));
449               else if (arg->index == 6)
450                 return usb_lld_ctrl_send (dev, &neug_p64_max, sizeof (uint16_t));
451               else if (arg->index == 7)
452                 return usb_lld_ctrl_send (dev, &neug_p4k_max, sizeof (uint16_t));
453               else
454                 return -1;
455             }
456         }
457       else /* SETUP_SET */
458         {
459 #ifdef FLASH_UPGRADE_SUPPORT
460           uint8_t *addr = (uint8_t *)(0x20000000UL + arg->value * 0x100 + arg->index);
461 #endif
462
463           if (arg->request == USB_FSIJ_DOWNLOAD)
464             {
465 #ifdef FLASH_UPGRADE_SUPPORT
466               chopstx_mutex_lock (&usb_mtx);
467               if (fsij_device_state != FSIJ_DEVICE_EXITED)
468                 {
469                   chopstx_mutex_unlock (&usb_mtx);
470                   return -1;
471                 }
472               chopstx_mutex_unlock (&usb_mtx);
473
474               if (addr < &_regnual_start || addr + arg->len > &__heap_end__)
475                 return -1;
476
477               if (arg->index + arg->len < 256)
478                 memset (addr + arg->index + arg->len, 0, 256 - (arg->index + arg->len));
479
480               return usb_lld_ctrl_recv (dev, addr, arg->len);
481 #else
482               return -1;
483 #endif
484             }
485           else if (arg->request == USB_FSIJ_EXEC && arg->len == 0)
486             {
487 #ifdef FLASH_UPGRADE_SUPPORT
488               chopstx_mutex_lock (&usb_mtx);
489               if (fsij_device_state != FSIJ_DEVICE_EXITED)
490                 {
491                   chopstx_mutex_unlock (&usb_mtx);
492                   return -1;
493                 }
494               chopstx_mutex_unlock (&usb_mtx);
495
496               if (((uintptr_t)addr & 0x03))
497                 return -1;
498
499               return download_check_crc32 (dev, (uint32_t *)addr);
500 #else
501               return -1;
502 #endif
503             }
504           else if (arg->request == USB_NEUG_SET_PASSWD && arg->len <= 32)
505             {
506               usbbuf[0] = arg->len;
507               return usb_lld_ctrl_recv (dev, usbbuf + 1, arg->len);
508             }
509           else if (arg->request == USB_NEUG_EXIT && arg->len <= 32)
510             {
511               chopstx_mutex_lock (&usb_mtx);
512               if (fsij_device_state != FSIJ_DEVICE_RUNNING)
513                 {
514                   chopstx_mutex_unlock (&usb_mtx);
515                   return -1;
516                 }
517               chopstx_mutex_unlock (&usb_mtx);
518
519               usbbuf[0] = arg->len;
520               return usb_lld_ctrl_recv (dev, usbbuf + 1, arg->len);
521             }
522         }
523     }
524   else if (type_rcp == (CLASS_REQUEST | INTERFACE_RECIPIENT)
525            && arg->index == 0)
526     {
527 #ifdef FRAUCHEKY_SUPPORT
528       if (running_neug)
529         return vcom_port_data_setup (dev);
530       else
531         return fraucheky_setup (dev);
532 #else
533       return vcom_port_data_setup (dev);
534 #endif
535     }
536
537   return -1;
538 }
539
540 static int
541 usb_get_descriptor (struct usb_dev *dev)
542 {
543   struct device_req *arg = &dev->dev_req;
544   uint8_t rcp = arg->type & RECIPIENT;
545   uint8_t desc_type = (arg->value >> 8);
546   uint8_t desc_index = (arg->value & 0xff);
547
548 #ifdef FRAUCHEKY_SUPPORT
549   if (!running_neug)
550     return fraucheky_get_descriptor (dev);
551 #endif
552
553   if (rcp != DEVICE_RECIPIENT)
554     return -1;
555
556   if (desc_type == DEVICE_DESCRIPTOR)
557     return usb_lld_ctrl_send (dev,
558                               vcom_device_desc, sizeof (vcom_device_desc));
559   else if (desc_type == CONFIG_DESCRIPTOR)
560     return usb_lld_ctrl_send (dev,
561                               vcom_config_desc, sizeof (vcom_config_desc));
562   else if (desc_type == STRING_DESCRIPTOR)
563     {
564       const uint8_t *str;
565       int size;
566
567       switch (desc_index)
568         {
569         case 0:
570           str = vcom_string0;
571           size = sizeof (vcom_string0);
572           break;
573         case 1:
574           str = neug_string_vendor;
575           size = sizeof (neug_string_vendor);
576           break;
577         case 2:
578           str = neug_string_product;
579           size = sizeof (neug_string_product);
580           break;
581         case 3:
582           str = neug_string_serial;
583           size = sizeof (neug_string_serial);
584           break;
585         case 4:
586           str = neug_revision_detail;
587           size = sizeof (neug_revision_detail);
588           break;
589         case 5:
590           str = neug_config_options;
591           size = sizeof (neug_config_options);
592           break;
593         case 6:
594           str = sys_version;
595           size = sizeof (sys_version);
596           break;
597 #ifdef USE_SYS3
598         case 7:
599           {
600             int i;
601             str = usbbuf;
602             for (i = 0; i < (int)sizeof (usbbuf)/2 - 2; i++)
603               {
604                 if (sys_board_name[i] == 0)
605                   break;
606
607                 usbbuf[i*2+2] = sys_board_name[i];
608                 usbbuf[i*2+3] = 0;
609               }
610             usbbuf[0] = i*2 + 2;
611             usbbuf[1] = STRING_DESCRIPTOR;
612             size = i*2 + 2;
613           }
614           break;
615 #endif
616         default:
617           return -1;
618         }
619
620       return usb_lld_ctrl_send (dev, str, size);
621     }
622
623   return -1;
624 }
625
626 static void
627 neug_setup_endpoints_for_interface (struct usb_dev *dev,
628                                     uint16_t interface, int stop)
629 {
630 #if !defined(GNU_LINUX_EMULATION)
631   (void)dev;
632 #endif
633   
634   if (interface == 0)
635     {
636 #ifdef FRAUCHEKY_SUPPORT
637       if (running_neug)
638         {
639           if (!stop)
640 #ifdef GNU_LINUX_EMULATION
641             usb_lld_setup_endp (dev, ENDP2, 0, 1);
642 #else
643             usb_lld_setup_endpoint (ENDP2, EP_INTERRUPT, 0, 0, ENDP2_TXADDR, 0);
644 #endif
645           else
646             usb_lld_stall_tx (ENDP2);
647         }
648       else
649         fraucheky_setup_endpoints_for_interface (stop);
650 #else
651       if (!stop)
652 #ifdef GNU_LINUX_EMULATION
653         usb_lld_setup_endp (dev, ENDP2, 0, 1);
654 #else
655         usb_lld_setup_endpoint (ENDP2, EP_INTERRUPT, 0, 0, ENDP2_TXADDR, 0);
656 #endif
657       else
658         usb_lld_stall_tx (ENDP2);
659 #endif
660     }
661   else if (interface == 1)
662     {
663       if (!stop)
664         {
665 #ifdef GNU_LINUX_EMULATION
666           usb_lld_setup_endp (dev, ENDP1, 0, 1);
667           usb_lld_setup_endp (dev, ENDP3, 1, 0);
668 #else
669           usb_lld_setup_endpoint (ENDP1, EP_BULK, 0, 0, ENDP1_TXADDR, 0);
670           usb_lld_setup_endpoint (ENDP3, EP_BULK, 0, ENDP3_RXADDR, 0, 64);
671 #endif
672           /* Start with no data receiving (ENDP3 not enabled) */
673         }
674       else
675         {
676           usb_lld_stall_tx (ENDP1);
677           usb_lld_stall_rx (ENDP3);
678         }
679     }
680 }
681
682 static int
683 usb_set_configuration (struct usb_dev *dev)
684 {
685   int i;
686   uint8_t current_conf;
687
688   current_conf = usb_lld_current_configuration (dev);
689   if (current_conf == 0)
690     {
691       if (dev->dev_req.value != 1)
692         return -1;
693
694       usb_lld_set_configuration (dev, 1);
695       for (i = 0; i < NUM_INTERFACES; i++)
696         neug_setup_endpoints_for_interface (dev, i, 0);
697       chopstx_mutex_lock (&usb_mtx);
698       bDeviceState = USB_DEVICE_STATE_CONFIGURED;
699       chopstx_mutex_unlock (&usb_mtx);
700     }
701   else if (current_conf != dev->dev_req.value)
702     {
703       if (dev->dev_req.value != 0)
704         return -1;
705
706       usb_lld_set_configuration (dev, 0);
707       for (i = 0; i < NUM_INTERFACES; i++)
708         neug_setup_endpoints_for_interface (dev, i, 1);
709       chopstx_mutex_lock (&usb_mtx);
710       bDeviceState = USB_DEVICE_STATE_ADDRESSED;
711       chopstx_cond_signal (&usb_cnd);
712       chopstx_mutex_unlock (&usb_mtx);
713     }
714
715   /* Do nothing when current_conf == value */
716   return usb_lld_ctrl_ack (dev);
717 }
718
719
720 static int
721 usb_set_interface (struct usb_dev *dev)
722 {
723   uint16_t interface = dev->dev_req.index;
724   uint16_t alt = dev->dev_req.value;
725
726   if (interface >= NUM_INTERFACES)
727     return -1;
728
729   if (alt != 0)
730     return -1;
731   else
732     {
733       neug_setup_endpoints_for_interface (dev, interface, 0);
734       return usb_lld_ctrl_ack (dev);
735     }
736 }
737
738 static int
739 usb_get_interface (struct usb_dev *dev)
740 {
741   const uint8_t zero = 0;
742   uint16_t interface = dev->dev_req.index;
743
744   if (interface >= NUM_INTERFACES)
745     return -1;
746
747   return usb_lld_ctrl_send (dev, &zero, 1);
748 }
749
750
751 static int
752 usb_get_status_interface (struct usb_dev *dev)
753 {
754   const uint16_t status_info = 0;
755   uint16_t interface = dev->dev_req.index;
756
757   if (interface >= NUM_INTERFACES)
758     return -1;
759
760   return usb_lld_ctrl_send (dev, &status_info, 2);
761 }
762
763 static void usb_tx_done (uint8_t ep_num, uint16_t len);
764 static void usb_rx_ready (uint8_t ep_num, uint16_t len);
765
766
767 #ifdef GNU_LINUX_EMULATION
768 #include <signal.h>
769 #define INTR_REQ_USB SIGUSR1
770 #else
771 #define INTR_REQ_USB 20
772 #endif
773
774 #define PRIO_USB 3
775
776 static void *
777 usb_main (void *arg)
778 {
779   chopstx_intr_t interrupt;
780   struct usb_dev dev;
781   int e;
782
783   (void)arg;
784   usb_lld_init (&dev, VCOM_FEATURE_BUS_POWERED);
785   chopstx_claim_irq (&interrupt, INTR_REQ_USB);
786   goto event_handle;    /* For old SYS < 3.0 */
787
788   while (1)
789     {
790       chopstx_intr_wait (&interrupt);
791
792       if (interrupt.ready)
793         {
794           uint8_t ep_num;
795
796         event_handle:
797           e = usb_lld_event_handler (&dev);
798           ep_num = USB_EVENT_ENDP (e);
799
800           if (ep_num != 0)
801             {
802               if (USB_EVENT_TXRX (e))
803                 usb_tx_done (ep_num, USB_EVENT_LEN (e));
804               else
805                 usb_rx_ready (ep_num, USB_EVENT_LEN (e));
806             }
807           else
808             switch (USB_EVENT_ID (e))
809               {
810               case USB_EVENT_DEVICE_RESET:
811                 usb_device_reset (&dev);
812                 break;
813
814               case USB_EVENT_DEVICE_ADDRESSED:
815                 chopstx_mutex_lock (&usb_mtx);
816                 bDeviceState = USB_DEVICE_STATE_ADDRESSED;
817                 chopstx_cond_signal (&usb_cnd);
818                 chopstx_mutex_unlock (&usb_mtx);
819                 break;
820
821               case USB_EVENT_GET_DESCRIPTOR:
822                 if (usb_get_descriptor (&dev) < 0)
823                   usb_lld_ctrl_error (&dev);
824                 break;
825
826               case USB_EVENT_SET_CONFIGURATION:
827                 if (usb_set_configuration (&dev) < 0)
828                   usb_lld_ctrl_error (&dev);
829                 break;
830
831               case USB_EVENT_SET_INTERFACE:
832                 if (usb_set_interface (&dev) < 0)
833                   usb_lld_ctrl_error (&dev);
834                 break;
835
836               case USB_EVENT_CTRL_REQUEST:
837                 /* Device specific device request.  */
838                 if (usb_setup (&dev) < 0)
839                   usb_lld_ctrl_error (&dev);
840                 break;
841
842               case USB_EVENT_GET_STATUS_INTERFACE:
843                 if (usb_get_status_interface (&dev) < 0)
844                   usb_lld_ctrl_error (&dev);
845                 break;
846
847               case USB_EVENT_GET_INTERFACE:
848                 if (usb_get_interface (&dev) < 0)
849                   usb_lld_ctrl_error (&dev);
850                 break;
851
852               case USB_EVENT_SET_FEATURE_DEVICE:
853               case USB_EVENT_SET_FEATURE_ENDPOINT:
854               case USB_EVENT_CLEAR_FEATURE_DEVICE:
855               case USB_EVENT_CLEAR_FEATURE_ENDPOINT:
856                 usb_lld_ctrl_ack (&dev);
857                 break;
858
859               case USB_EVENT_CTRL_WRITE_FINISH:
860                 /* Control WRITE transfer finished.  */
861                 usb_ctrl_write_finish (&dev);
862                 break;
863
864               case USB_EVENT_DEVICE_SUSPEND:
865 #ifdef FRAUCHEKY_SUPPORT
866                 if (running_neug)
867 #endif
868                   neug_fini ();
869                 chopstx_conf_idle (2);
870                 bDeviceState |= USB_DEVICE_STATE_SUSPEND;
871                 break;
872
873               case USB_EVENT_DEVICE_WAKEUP:
874                 chopstx_conf_idle (1);
875 #ifdef FRAUCHEKY_SUPPORT
876                 if (running_neug)
877 #endif
878                   neug_init (random_word, RANDOM_BYTES_LENGTH/sizeof (uint32_t));
879                 bDeviceState &= ~USB_DEVICE_STATE_SUSPEND;
880                 break;
881
882               case USB_EVENT_OK:
883               default:
884                 break;
885               }
886         }
887     }
888
889   return NULL;
890 }
891
892 #define ID_OFFSET (2+SERIALNO_STR_LEN*2)
893 static void fill_serial_no_by_unique_id (void)
894 {
895   extern const uint8_t * unique_device_id (void);
896   uint8_t *p = &neug_string_serial[ID_OFFSET];
897   const uint8_t *u = unique_device_id () + 8;
898   int i;
899
900   for (i = 0; i < 4; i++)
901     {
902       uint8_t b = u[3-i];
903       uint8_t nibble;
904
905       nibble = (b >> 4);
906       nibble += (nibble >= 10 ? ('A' - 10) : '0');
907       p[i*4] = nibble;
908       p[i*4+1] = 0;
909       nibble = (b & 0x0f);
910       nibble += (nibble >= 10 ? ('A' - 10) : '0');
911       p[i*4+2] = nibble;
912       p[i*4+3] = 0;
913     }
914 }
915
916
917 static void
918 usb_tx_done (uint8_t ep_num, uint16_t len)
919 {
920   if (ep_num == ENDP1)
921     {
922       chopstx_mutex_lock (&usb_mtx);
923       chopstx_cond_signal (&usb_cnd);
924       chopstx_mutex_unlock (&usb_mtx);
925     }
926   else if (ep_num == ENDP2)
927     {
928       /* INTERRUPT Transfer done */
929     }
930 #ifdef FRAUCHEKY_SUPPORT
931   else if (ep_num == ENDP6)
932     EP6_IN_Callback (len);
933 #else
934   (void)len;
935 #endif
936 }
937
938 #ifdef GNU_LINUX_EMULATION
939 static uint8_t endp3_buf[64];
940 #endif
941
942 static void
943 usb_rx_ready (uint8_t ep_num, uint16_t len)
944 {
945   if (ep_num == ENDP3)
946 #ifdef GNU_LINUX_EMULATION
947     usb_lld_rx_enable_buf (ENDP3, endp3_buf, 64);
948 #else
949     usb_lld_rx_enable (ENDP3);
950 #endif
951 #ifdef FRAUCHEKY_SUPPORT
952   else if (ep_num == ENDP6)
953     EP6_OUT_Callback (len);
954 #else
955   (void)len;
956 #endif
957 }
958 \f
959 #define STACK_MAIN
960 #define STACK_PROCESS_1
961 #define STACK_PROCESS_3
962 #include "stack-def.h"
963 #define STACK_ADDR_LED ((uintptr_t)process1_base)
964 #define STACK_SIZE_LED (sizeof process1_base)
965 #define STACK_ADDR_USB ((uintptr_t)process3_base)
966 #define STACK_SIZE_USB (sizeof process3_base)
967
968
969 #define PRIO_LED 3
970 static struct eventflag led_event;
971
972 #define LED_ONESHOT_SHORT       ((eventmask_t)1)
973 #define LED_TWOSHOTS            ((eventmask_t)2)
974 #define LED_ONESHOT_LONG        ((eventmask_t)4)
975
976 /*
977  * LED blinker: When notified, let LED emit for 100ms.
978  */
979 static void *
980 led_blinker (void *arg)
981 {
982   (void)arg;
983
984   set_led (0);
985
986   while (1)
987     {
988       eventmask_t m;
989
990       m = eventflag_wait (&led_event);
991
992       set_led (1);
993       if (m == LED_ONESHOT_SHORT)
994         chopstx_usec_wait (100*1000);
995       else if (m == LED_TWOSHOTS)
996         {
997           chopstx_usec_wait (50*1000);
998           set_led (0);
999           chopstx_usec_wait (50*1000);
1000           set_led (1);
1001           chopstx_usec_wait (50*1000);
1002         }
1003       else
1004         chopstx_usec_wait (250*1000);
1005       set_led (0);
1006     }
1007
1008   return NULL;
1009 }
1010 \f
1011 #ifdef GNU_LINUX_EMULATION
1012 static uint8_t endp1_buf[RANDOM_BYTES_LENGTH];
1013 #endif
1014
1015 static void copy_to_tx (uint32_t v, int i)
1016 {
1017 #ifdef GNU_LINUX_EMULATION
1018   memcpy (&endp1_buf[i*4], &v, 4);
1019 #else
1020   usb_lld_txcpy (&v, ENDP1, i * 4, 4);
1021 #endif
1022 }
1023
1024 #ifdef FLASH_UPGRADE_SUPPORT
1025 /*
1026  * In Gnuk 1.0.[12], reGNUal was not relocatable.
1027  * Now, it's relocatable, but we need to calculate its entry address
1028  * based on it's pre-defined address.
1029  */
1030 #define REGNUAL_START_ADDRESS_COMPATIBLE 0x20001400
1031 static uint32_t
1032 calculate_regnual_entry_address (const uint8_t *addr)
1033 {
1034   const uint8_t *p = addr + 4;
1035   uintptr_t v = p[0] + (p[1] << 8) + (p[2] << 16) + (p[3] << 24);
1036
1037   v -= REGNUAL_START_ADDRESS_COMPATIBLE;
1038   v += (uintptr_t)addr;
1039   return v;
1040 }
1041 #endif
1042
1043
1044 static int
1045 check_usb_status (void *arg)
1046 {
1047   (void)arg;
1048
1049   return (connected || bDeviceState != USB_DEVICE_STATE_CONFIGURED
1050           || fsij_device_state != FSIJ_DEVICE_RUNNING);
1051 }
1052
1053
1054
1055 /*
1056  * Entry point.
1057  *
1058  * NOTE: the main function is already a thread in the system on entry.
1059  */
1060 int
1061 main (int argc, char **argv)
1062 {
1063 #ifdef FLASH_UPGRADE_SUPPORT
1064   uintptr_t entry;
1065 #endif
1066   chopstx_t led_thread, usb_thd;
1067   unsigned int count;
1068
1069 #ifdef GNU_LINUX_EMULATION
1070   if (argc >= 4 || (argc == 2 && !strcmp (argv[1], "--help")))
1071     {
1072       fprintf (stdout, "Usage: %s [--vidpid=Vxxx:Pxxx]", argv[0]);
1073       exit (0);
1074     }
1075
1076   if (argc >= 2 && !strncmp (argv[1], "--debug=", 8))
1077     {
1078       debug = strtol (&argv[1][8], NULL, 10);
1079       argc--;
1080       argv++;
1081     }
1082
1083   if (argc >= 2 && !strncmp (argv[1], "--vidpid=", 9))
1084     {
1085       uint32_t id;
1086       char *p;
1087
1088       id = (uint32_t)strtol (&argv[1][9], &p, 16);
1089       vcom_device_desc[8] = (id & 0xff);
1090       vcom_device_desc[9] = (id >> 8);
1091
1092       if (p && p[0] == ':')
1093         {
1094           id = (uint32_t)strtol (&p[1], NULL, 16);
1095           vcom_device_desc[10] = (id & 0xff);
1096           vcom_device_desc[11] = (id >> 8);
1097         }
1098
1099       argc--;
1100       argv++;
1101     }
1102 #else
1103   (void)argc;
1104   (void)argv;
1105 #endif
1106
1107   fill_serial_no_by_unique_id ();
1108
1109   chopstx_conf_idle (1);
1110
1111   adc_init ();
1112
1113   eventflag_init (&led_event);
1114
1115   chopstx_mutex_init (&usb_mtx);
1116   chopstx_cond_init (&usb_cnd);
1117
1118 #ifdef FRAUCHEKY_SUPPORT
1119   if (fraucheky_enabled ())
1120     {
1121     go_fraucheky:
1122       bDeviceState = USB_DEVICE_STATE_UNCONNECTED;
1123       running_neug = 0;
1124       usb_thd = chopstx_create (PRIO_USB, STACK_ADDR_USB, STACK_SIZE_USB,
1125                                 usb_main, NULL);
1126       while (bDeviceState != USB_DEVICE_STATE_CONFIGURED)
1127         chopstx_usec_wait (250*1000);
1128       set_led (1);
1129       fraucheky_main ();
1130       chopstx_cancel (usb_thd);
1131       chopstx_join (usb_thd, NULL);
1132       usb_lld_shutdown ();
1133       bDeviceState = USB_DEVICE_STATE_UNCONNECTED;
1134     }
1135
1136   neug_init (random_word, RANDOM_BYTES_LENGTH/sizeof (uint32_t));
1137
1138   running_neug = 1;
1139 #else
1140   neug_init (random_word, RANDOM_BYTES_LENGTH/sizeof (uint32_t));
1141 #endif
1142
1143   led_thread = chopstx_create (PRIO_LED, STACK_ADDR_LED, STACK_SIZE_LED,
1144                                led_blinker, NULL);
1145
1146   usb_thd = chopstx_create (PRIO_USB, STACK_ADDR_USB, STACK_SIZE_USB,
1147                             usb_main, NULL);
1148
1149   chopstx_mutex_lock (&usb_mtx);
1150
1151  not_configured:
1152   count = 0;
1153   /* Initial run-up */
1154   while (count < NEUG_PRE_LOOP || bDeviceState != USB_DEVICE_STATE_CONFIGURED)
1155     {
1156       if (fsij_device_state != FSIJ_DEVICE_RUNNING)
1157         break;
1158
1159       chopstx_mutex_unlock (&usb_mtx);
1160       neug_wait_full ();
1161       neug_flush ();
1162
1163       if ((count & 0x0007) == 0)
1164         eventflag_signal (&led_event, LED_ONESHOT_SHORT);
1165       chopstx_usec_wait (25*1000);
1166       count++;
1167       chopstx_mutex_lock (&usb_mtx);
1168     }
1169
1170   /* Holding USB_MTX...  */
1171   while (1)
1172     {
1173       int last_was_fullsizepacket = 0;
1174
1175       if (fsij_device_state != FSIJ_DEVICE_RUNNING)
1176         break;
1177
1178       chopstx_mutex_unlock (&usb_mtx);
1179       while (1)
1180         {
1181           uint32_t usec = 5000*1000;
1182           chopstx_poll_cond_t poll_desc;
1183           struct chx_poll_head *pd_array[1] = {
1184             (struct chx_poll_head *)&poll_desc
1185           };
1186
1187           poll_desc.type = CHOPSTX_POLL_COND;
1188           poll_desc.ready = 0;
1189           poll_desc.cond = &usb_cnd;
1190           poll_desc.mutex = &usb_mtx;
1191           poll_desc.check = check_usb_status;
1192           poll_desc.arg = NULL;
1193
1194           if (chopstx_poll (&usec, 1, pd_array))
1195             break;
1196
1197           /* Timeout */
1198           neug_flush ();
1199           neug_mode_select (line_coding.paritytype);
1200           eventflag_signal (&led_event, LED_TWOSHOTS);
1201         }
1202
1203       chopstx_mutex_lock (&usb_mtx);
1204       if (bDeviceState != USB_DEVICE_STATE_CONFIGURED)
1205         goto not_configured;
1206
1207       /* The connection opened.  */
1208       count = 0;
1209
1210       while (1)
1211         {
1212           int i;
1213
1214           chopstx_mutex_unlock (&usb_mtx);
1215           /*
1216            * No parity is standard.  It means providing conditioned output.
1217            * When parity enabled, it means to provide raw output
1218            * (CRC32 filtered when odd, direct sample of ADC when even).
1219            *
1220            * line_coding.paritytype:
1221            *   0: None, 1: Odd, 2: Even
1222            */
1223           neug_mode_select (line_coding.paritytype);
1224
1225           if ((count & 0x03ff) == 0)
1226             eventflag_signal (&led_event, LED_ONESHOT_SHORT);
1227
1228           i = neug_consume_random (copy_to_tx);
1229
1230           if (i == 0 && !last_was_fullsizepacket)
1231             {    /* Only send ZLP when the last packet was fullsize.  */
1232               neug_wait_full ();
1233
1234               chopstx_mutex_lock (&usb_mtx);
1235               if (bDeviceState != USB_DEVICE_STATE_CONFIGURED || !connected
1236                   || fsij_device_state != FSIJ_DEVICE_RUNNING)
1237                 break;
1238             }
1239           else
1240             {
1241               if (i == 64/4)
1242                 last_was_fullsizepacket = 1;
1243               else
1244                 last_was_fullsizepacket = 0;
1245
1246               chopstx_mutex_lock (&usb_mtx);
1247               if (bDeviceState != USB_DEVICE_STATE_CONFIGURED || !connected
1248                   || fsij_device_state != FSIJ_DEVICE_RUNNING)
1249                 break;
1250
1251               /* Prepare sending random data.  */
1252 #ifdef GNU_LINUX_EMULATION
1253               usb_lld_tx_enable_buf (ENDP1, endp1_buf, i * 4);
1254 #else
1255               usb_lld_tx_enable (ENDP1, i * 4);
1256 #endif
1257               chopstx_cond_wait (&usb_cnd, &usb_mtx);
1258               count++;
1259             }
1260         }
1261     }
1262
1263   chopstx_mutex_unlock (&usb_mtx);
1264
1265   chopstx_cancel (led_thread);
1266   chopstx_join (led_thread, NULL);
1267
1268   /*
1269    * We come here, because of FSIJ_DEVICE_NEUG_EXIT_REQUESTED
1270    * or FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED.
1271    */
1272   neug_fini ();
1273
1274   chopstx_mutex_lock (&usb_mtx);
1275 #ifdef FRAUCHEKY_SUPPORT
1276   if (line_coding.bitrate == NEUG_SPECIAL_BITRATE)
1277     {
1278       while (fsij_device_state == FSIJ_DEVICE_NEUG_FRAUCHEKY_REQUESTED)
1279         {
1280           chopstx_mutex_unlock (&usb_mtx);
1281           set_led (1);
1282           chopstx_usec_wait (500*1000);
1283           set_led (0);
1284           chopstx_usec_wait (500*1000);
1285           chopstx_mutex_lock (&usb_mtx);
1286         }
1287
1288       usb_lld_prepare_shutdown ();
1289       chopstx_mutex_unlock (&usb_mtx);
1290       chopstx_cancel (usb_thd);
1291       chopstx_join (usb_thd, NULL);
1292       usb_lld_shutdown ();
1293       goto go_fraucheky;
1294     }
1295 #endif
1296
1297   fsij_device_state = FSIJ_DEVICE_EXITED;
1298
1299   while (fsij_device_state == FSIJ_DEVICE_EXITED)
1300     {
1301       chopstx_mutex_unlock (&usb_mtx);
1302       chopstx_usec_wait (500*1000);
1303       chopstx_mutex_lock (&usb_mtx);
1304     }
1305   chopstx_mutex_unlock (&usb_mtx);
1306
1307   flash_unlock ();              /* Flash unlock should be done here */
1308   set_led (1);
1309   usb_lld_shutdown ();
1310
1311   /* Finish application.  */
1312   chopstx_cancel (usb_thd);
1313   chopstx_join (usb_thd, NULL);
1314
1315 #ifdef FLASH_UPGRADE_SUPPORT
1316   /* Set vector */
1317   SCB->VTOR = (uintptr_t)&_regnual_start;
1318   entry = calculate_regnual_entry_address (&_regnual_start);
1319 #ifdef DFU_SUPPORT
1320 #define FLASH_SYS_START_ADDR 0x08000000
1321 #define FLASH_SYS_END_ADDR (0x08000000+0x1000)
1322 #define CHIP_ID_REG ((uint32_t *)0xE0042000)
1323   {
1324     extern uint8_t _sys;
1325     uintptr_t addr;
1326     handler *new_vector = (handler *)FLASH_SYS_START_ADDR;
1327     void (*func) (void (*)(void)) = (void (*)(void (*)(void)))new_vector[9];
1328     uint32_t flash_page_size = 1024; /* 1KiB default */
1329
1330    if ((*CHIP_ID_REG)&0x07 == 0x04) /* High dencity device.  */
1331      flash_page_size += 0x0400; /* It's 2KiB. */
1332
1333     /* Kill DFU */
1334     for (addr = FLASH_SYS_START_ADDR; addr < FLASH_SYS_END_ADDR;
1335          addr += flash_page_size)
1336       flash_erase_page (addr);
1337
1338     /* copy system service routines */
1339     flash_write (FLASH_SYS_START_ADDR, &_sys, 0x1000);
1340
1341     /* Leave NeuG to exec reGNUal */
1342     (*func) ((void (*)(void))entry);
1343     for (;;);
1344   }
1345 #else
1346   /* Leave NeuG to exec reGNUal */
1347   flash_erase_all_and_exec ((void (*)(void))entry);
1348 #endif
1349 #else
1350   exit (0);
1351 #endif
1352
1353   /* Never reached */
1354   return 0;
1355 }