1 files changed, 134 insertions, 41 deletions
diff --git a/example-fs-bb48/sys.c b/example-fs-bb48/sys.c
index a587098..d700ec8 100644
--- a/example-fs-bb48/sys.c
+++ b/example-fs-bb48/sys.c
@@ -1,5 +1,5 @@
 /*
- * first-pages.c - First pages for MKL27Z256.
+ * sys.c - First pages for MKL27Z256.
  *
  * Copyright (C) 2016 Flying Stone Technology
  * Author: NIIBE Yutaka <gniibe@fsij.org>
@@ -18,22 +18,37 @@
  * data for predefined purposes.
  */
 
-
 #include <stdint.h>
+#include <stdlib.h>
+#include "board.h"
+
+#include "mcu/clk_gpio_init-kl.c"
 
-extern uint8_t __main_stack_end__;
+static void
+set_led (int on)
+{
+  if (on)
+    GPIOB->PCOR = (1 << 0); /* PTB0: Clear: Light on  */
+  else
+    GPIOB->PSOR = (1 << 0); /* PTB0: Set  : Light off */
+}
+
+#define ADDR_VECTORS (0x00000900)
+#define ADDR_SCR_VTOR 0xe000ed08
 
-static void __attribute__ ((naked,section(".flash_config_page")))
+static void __attribute__ ((naked,section(".reset.entry")))
 reset (void)
 {
-  uint32_t r3 = 0xe000ed08;
+  uint32_t r3 = ADDR_SCR_VTOR;
 
   asm volatile ("str	%2, [%0]\n\t"	  /* Set SCR->VTOR     */
-		"ldr	%0, [%2, #4]\n\t" /* Jump to the entry */
-		"bx	%0\n\t"
+		"ldr	%0, [%2]\n\t"     /* Stack address     */
+		"msr	MSP, %0\n\t"	  /* Exception handler stack. */
+		"ldr	%0, [%2, #4]\n\t" /* The entry address */
+		"bx	%0\n\t"           /* Jump to the entry */
 		".align	2\n"
 		: "=r" (r3)
-		: "0" (r3), "r" (0x00000800)
+		: "0" (r3), "r" (ADDR_VECTORS)
 		: "memory");
 
   /* Never reach here. */
@@ -42,16 +57,18 @@ reset (void)
 
 static uint32_t
 stack_entry[] __attribute__ ((section(".first_page.first_words"),used)) = {
-  (uint32_t)(&__main_stack_end__ - 32),
+  /* Since MSP are soon modified in RESET, we put 0 here.  */
+  0,
   (uint32_t)reset,
 };
 
 /*
- * NOTE: We don't use backdoor comparison key.  The area is used by
- * CRC32 table.
+ * Here comes SYS routines and data.
  */
+
 static uint32_t
 flash_config[] __attribute__ ((section(".flash_config"),used)) = {
+  0xffffffff, 0xffffffff, /* Comparison Key */
   0xffffffff, /* Protection bytes */
   0xffff3ffe, /* FSEC=0xfe, FOPT=0x3f */
   /* FOPT=0x3f:
@@ -66,12 +83,96 @@ flash_config[] __attribute__ ((section(".flash_config"),used)) = {
 /*
  * CRC32 calculation routines.
  */
+void __attribute__ ((naked,section(".fixed_function.crc32_init")))
+crc32_init  (unsigned int *p)
+{
+#ifdef ORIGINAL_IN_C
+  *p = 0xffffffff;
+#else
+  register unsigned int r3 asm ("r3");
+
+  asm volatile ("mov	%0, #1\n\t"
+		"neg	%0, %0\n\t"
+		"str	%0, [%1]\n\t"
+		"bx	lr\n"
+		: "=r" (r3)
+		: "r" (p)
+		: "memory");
+#endif
+}
+
+#ifdef ORIGINAL_IN_C
+const unsigned int *const crc32_table= (const unsigned int *const)0x00000500;
+#endif
+
+void __attribute__ ((naked,section(".fixed_function.crc32_u8")))
+crc32_u8 (unsigned int *p, unsigned char v)
+{
+#ifdef ORIGINAL_IN_C
+  *p = crc32_table[(*p & 0xff) ^ v] ^ (*p >> 8);
+#else
+  register unsigned int r2 asm ("r2");
+  register unsigned int r3 asm ("r3");
+
+  asm volatile ("ldrb	%2, [%4]\n\t"
+		"eor	%0, %2\n\t"
+		"mov	%2, #0xa0\n\t" /* (0x0500 >> 3) */
+		"lsl	%0, %0, #2\n\t"
+		"lsl	%2, %2, #3\n\t"
+		"add	%0, %0, %2\n\t"
+		"ldr	%2, [%4]\n\t"
+		"ldr	%1, [%0]\n\t"
+		"lsr	%2, %2, #8\n\t"
+		"eor	%2, %1\n\t"
+		"str	%2, [%4]\n\t"
+		"bx	lr\n"
+		: "=r" (v), "=r" (r2), "=r" (r3)
+		: "0" (v), "r" (p)
+		: "memory");
+#endif
+}
+
+void __attribute__ ((naked,section(".fixed_function.crc32_u32")))
+crc32_u32 (unsigned int *p, unsigned int u)
+{
+#ifdef ORIGINAL_IN_C
+  crc32_u8 (p, u & 0xff);
+  crc32_u8 (p, (u >> 8)& 0xff);
+  crc32_u8 (p, (u >> 16)& 0xff);
+  crc32_u8 (p, (u >> 24)& 0xff);
+#else
+  register unsigned int r3 asm ("r3");
+  register unsigned int r4 asm ("r4");
+  register unsigned int r5 asm ("r5");
+
+  asm volatile ("push	{%1, %2, %3, lr}\n\t"
+		"mov	%2, %0\n\t"
+		"mov	%3, %5\n\t"
+		"uxtb	%0, %0\n\t"
+		"bl	crc32_u8\n\t"
+		"lsr	%0, %2, #8\n\t"
+		"mov	%5, %3\n\t"
+		"uxtb	%0, %0\n\t"
+		"bl	crc32_u8\n\t"
+		"lsr	%0, %2, #16\n\t"
+		"mov	%5, %3\n\t"
+		"uxtb	%0, %0\n\t"
+		"bl	crc32_u8\n\t"
+		"mov	%5, %3\n\t"
+		"lsr	%0, %2, #24\n\t"
+		"bl	crc32_u8\n\t"
+		"pop	{%1, %2, %3, pc}\n\t"
+		: "=r" (u), "=r" (r3), "=r" (r4), "=r" (r5)
+		: "0" (u), "r" (p)
+		: "memory");
+#endif
+}
 
 /*
  * Table of CRC32, generated by gen_crc_table.py
  */
-static unsigned int
-crc32_table[256] __attribute__ ((section(".first_page"))) = {
+const unsigned int
+crc32_table[256] __attribute__ ((section(".crc32_table"))) = {
   0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 
   0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 
   0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2, 
@@ -117,35 +218,27 @@ crc32_table[256] __attribute__ ((section(".first_page"))) = {
   0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d,
 };
 
-static unsigned int crc_reg;
 
-__attribute__ ((section(".flash_config_page")))
-void
-crc32_init (void)
-{
-  crc_reg = 0xffffffff;
-}
+const uint8_t sys_version[8] __attribute__((section(".sys.version"))) = {
+  3*2+2,	     /* bLength */
+  0x03,		     /* bDescriptorType = STRING_DESCRIPTOR */
+  /* sys version: "3.0" */
+  '3', 0, '.', 0, '0', 0,
+};
 
-__attribute__ ((section(".flash_config_page")))
-unsigned int
-crc32_value (void)
-{
-  return crc_reg;
-}
+static const uint8_t board_name_string[] = BOARD_NAME;
 
-__attribute__ ((section(".flash_config_page")))
-void
-crc32_u8 (unsigned char bits_eight)
-{
-  crc_reg = crc32_table[(crc_reg & 0xff) ^ bits_eight] ^ (crc_reg >> 8);
-}
+const uint8_t __attribute__((section(".sys.board_info")))
+*const sys_board_name = board_name_string;
 
-__attribute__ ((section(".flash_config_page")))
-void
-crc32_u32 (unsigned int u)
-{
-  crc32_u8 (u & 0xff);
-  crc32_u8 ((u >> 8)& 0xff);
-  crc32_u8 ((u >> 16)& 0xff);
-  crc32_u8 ((u >> 24)& 0xff);
-}
+const uint32_t __attribute__((section(".sys.board_info")))
+sys_board_id = BOARD_ID;
+
+typedef void (*handler)(void);
+
+handler sys_vector[] __attribute__ ((section(".sys.vectors"))) = {
+  clock_init,
+  gpio_init,
+  (handler)set_led,
+  NULL,
+};