/***********************************************************************************************************************
 * File Name    : hal_entry.c
 * Description  : Entry function.
 **********************************************************************************************************************/
/***********************************************************************************************************************
 * DISCLAIMER
 * This software is supplied by Renesas Electronics Corporation and is only intended for use with Renesas products. No
 * other uses are authorized. This software is owned by Renesas Electronics Corporation and is protected under all
 * applicable laws, including copyright laws.
 * THIS SOFTWARE IS PROVIDED "AS IS" AND RENESAS MAKES NO WARRANTIES REGARDING
 * THIS SOFTWARE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. ALL SUCH WARRANTIES ARE EXPRESSLY DISCLAIMED. TO THE MAXIMUM
 * EXTENT PERMITTED NOT PROHIBITED BY LAW, NEITHER RENESAS ELECTRONICS CORPORATION NOR ANY OF ITS AFFILIATED COMPANIES
 * SHALL BE LIABLE FOR ANY DIRECT, INDIRECT, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES FOR ANY REASON RELATED TO THIS
 * SOFTWARE, EVEN IF RENESAS OR ITS AFFILIATES HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
 * Renesas reserves the right, without notice, to make changes to this software and to discontinue the availability of
 * this software. By using this software, you agree to the additional terms and conditions found by accessing the
 * following link:
 * http://www.renesas.com/disclaimer
 *
 * Copyright (C) 2020 Renesas Electronics Corporation. All rights reserved.
 ***********************************************************************************************************************/

#include "hal_data.h"

#include <stdbool.h>
#include <stdint.h>
#include "udp_cdc.h"
#include "udp_printf.h"

#define APP_ERR_TRAP(a)           if(a) {__asm("BKPT #0\n");} /* trap the error location */

#define USER_FLASH_END_ADDRESS    (0x0007FFFF)
#define IAP_HEAD_ADDRESS          (0x00010000)
#define IAP_FOOT_ADDRESS          (0x0007FFFF)

#define USER_FLASH_START_BLOCK    (8)
#define FLASH_BLOCK_SIZE_BYTE     (32768)
#define USER_FLASH_START_ADDRESS  ((USER_FLASH_START_BLOCK - 8) * FLASH_BLOCK_SIZE_BYTE + IAP_HEAD_ADDRESS)

//#define ENABLE_RTC
#define RTC_TIME_H_YEAR_OFFSET    (1900)

// uart tx/rx buffer
uint8_t txb[100], rxb[2000];

static bool wait (int32_t ms, bool rx) {
    volatile int i;
    for (; ms; --ms) {
        for (i = 0; i < 100; i++) {
            if (rx && udp_rx_buff ()) return true;
            R_BSP_SoftwareDelay(10, BSP_DELAY_UNITS_MICROSECONDS);
        }
    }
    return false;
}

static int64_t H2I (char *s) {
    int64_t i, n = 0;

    if (!*s) return -1;
    for (i = 0; *s; i++, s++) {
        if ('0' <= *s && *s <= '9') {
            n <<= 4;
            n |= *s - '0';
        } else if ('a' <= *s && *s <= 'f') {
            n <<= 4;
            n |= *s - ('a' - 10);
        } else if ('A' <= *s && *s <= 'F') {
            n <<= 4;
            n |= *s - ('A' - 10);
        }
    }
    return n;
}

static int D2I (char *s) {
    int result = 0, n = 0, sig = 1;
    if (*s=='-') {
        sig = -1;
        result++;
        s++;
    } else if (*s=='+') {
        sig = -1;
        result++;
        s++;
    }
    while (*s != 0 && (*s >= '0' && *s <= '9')) {
        n = (n * 10) + *s - '0';
        result++;
        s++;
    }
    return n * sig;
}

/*****************************************************************************
 * Flash functions
 ****************************************************************************/
static bool FLASH_Write (uint32_t addr, uint8_t *buf, uint32_t size) {
    bool result = false;
    __disable_irq();
    result = R_FLASH_HP_Write(&g_flash0_ctrl, (uint32_t)buf, addr, size) == FSP_SUCCESS;
    __enable_irq();
    return result;
}

static bool FLASH_EraseAll (void) {
    bool result = false;
    __disable_irq();
    result = R_FLASH_HP_Erase(&g_flash0_ctrl, IAP_HEAD_ADDRESS, 10) == FSP_SUCCESS;
    __enable_irq();
    return result;
}

/*****************************************************************************
 * XMODEM Protocol functions
 ****************************************************************************/
#define _SOH  (0x01)
#define _STX  (0x02)
#define _EOT  (0x04)
#define _ACK  (0x06)
#define _NAK  (0x15)
#define _CAN  (0x18)
#define _XYN  (_NAK)
#define _XYC  ('C')
#define _XYG  ('G')

#if 0
#define _XMODEM_PACKET_SIZE (128)
#else
#define _XMODEM_PACKET_SIZE (1024)
#endif
/*-----XMODEMのパケット処理-------*
  受信側              送信側
         --- NAK -->
         <-- SOH --- (128byte時のブロック開始)
         <--  01 --- (ブロック番号)
         <--  FE --- (ブロック番号を反転させたもの)
         <--  .. --- (実際のデータ 128 bytes)
              ..
         <-- sum --- (checksum)

         --- ACK -->
 *--------------------------------*/
/*-----XMODEM1kのパケット処理-------*
  受信側              送信側
         --- 'C' -->
         <-- STX --- (1024byte時のブロック開始)
         <--  01 --- (ブロック番号)
         <--  FE --- (ブロック番号を反転させたもの)
         <--  .. --- (実際のデータ 1024 bytes)
              ..
         <-- sumH --- (CRC)
         <-- sumL --- (CRC)

         --- ACK -->
 *--------------------------------*/
#define _CNTOVER  (5)

static bool XmodemRX (void) {

    bool my_put_ack (void) {
        int8_t i = 0;
        for (i = 0; i < _CNTOVER; i++) {
            udp_putc (_ACK);  // OK
            if (wait (100, true)) return true;
        }
        return false;
    }

    bool my_get_char (uint8_t *ch, bool f) {
        int8_t cntover = 0;

        if (wait (50, true)) {
            *ch = (uint8_t)udp_getc ();
            return true;
        } else {
            if (f) {
                wait (100, false);
                udp_rx_purge ();
                do {
                    udp_putc (_NAK);
                    wait (100, true);
                    if (cntover++ > _CNTOVER) break;
                } while (!udp_rx_buff ());
            }
            return false;
        }
    }

    static uint8_t xbuff[_XMODEM_PACKET_SIZE];
    uint8_t head, sum0 = 0, sum1, h0 = 1, h1, h2;
    uint16_t sum2;
    int  i = 0;
    uint32_t size = 0, adr = 0, fladdr = USER_FLASH_START_ADDRESS;
    bool f;

    while (!udp_rx_buff ()) {
#if _XMODEM_PACKET_SIZE == 128
        udp_putc (_NAK);
#else
        udp_putc (_XYC);
#endif
        if (wait (100, true)) break;
        if (i++ > 240) return false;
    }
    for (;;) {
        if (my_get_char (&head, true)) {
            switch (head) {
#if _XMODEM_PACKET_SIZE == 128
                case _SOH:
#else
                case _STX:
#endif
                    if (my_get_char (&h1, false)) {
                        if (my_get_char (&h2, false)) {
                            if (h1 != ((~h2) & 0xff)) {
                                wait (50, false);
                                udp_rx_purge ();
                                udp_putc (_NAK);
                            } else {
                                if (h1 == h0) {
                                    sum2 = 0;
                                    f = false;
                                    for (i = 0; i < _XMODEM_PACKET_SIZE; i++) {
                                        if ((f = my_get_char (&xbuff[adr + i], false))) {
#if _XMODEM_PACKET_SIZE == 128
                                            sum2 = (sum2 + xbuff[adr + i]) & 0xff;
#else
                                            sum2 = (uint16_t)(sum2 ^ xbuff[adr + i] << 8);
                                            int  j;
                                            for (j = 0; j < 8; ++j) {
                                                if (sum2 & 0x8000) sum2 = (uint16_t)(sum2 << 1 ^ 0x1021);
                                                else               sum2 = sum2 << 1;
                                            }
#endif
                                        } else break;
                                    }
                                    if (f) {
#if _XMODEM_PACKET_SIZE == 128
                                        if (my_get_char (&sum1, false))
#else
                                            if (my_get_char (&sum0, false) && my_get_char (&sum1, false))
#endif
                                            {
                                                if (((sum0 << 8) | sum1) == sum2) {
                                                    adr += _XMODEM_PACKET_SIZE;
                                                    h0++;
                                                    if ((adr % sizeof (xbuff)) == 0) {
                                                        if (FLASH_Write (fladdr, (void *)xbuff, sizeof (xbuff))) {
                                                            fladdr += sizeof (xbuff);
                                                            adr = 0;
                                                        } else {
                                                            udp_putc (_CAN);
                                                            wait (200, false);
                                                            udp_rx_purge ();
                                                            return false;
                                                        }
                                                    }
                                                    size += _XMODEM_PACKET_SIZE;
                                                    if (!my_put_ack ()) { // OK
                                                        udp_putc (_CAN);
                                                        wait (200, false);
                                                        udp_rx_purge ();
                                                        return false;
                                                        break;
                                                    }
                                                } else {
                                                    wait (50, false);
                                                    udp_rx_purge ();
                                                    udp_putc (_NAK);
                                                }
                                            }
                                    } else {
                                        wait (50, false);
                                        udp_rx_purge ();
                                        udp_putc (_NAK);
                                    }
                                }
                            }
                        }
                    }
                    break;
                case _EOT:
                    if (adr) {
                        if (!(FLASH_Write (fladdr, (void *)xbuff, adr) == 0)) {
                            udp_putc (_CAN);
                            wait (200, false);
                            udp_rx_purge ();
                            return false;
                        }
                    }
                    udp_rx_purge ();
                    udp_putc (_ACK);
                    wait (200, false);
                    return true;
                    break;
                case _CAN:
                    wait (200, false);
                    udp_rx_purge ();
                    return false;
                    break;
                default:
                    wait (50, false);
                    udp_rx_purge ();
                    udp_putc (_NAK);
                    break;
            }
        }
    }
    return false;
}

static int getc2 (int w) {
    wait (w, true);
    if (udp_rx_buff ()) return udp_getc ();
    return -1;
}

static int XmodemTX (uint8_t *src, int srcsz) {
    uint16_t sum2;
    static uint8_t xbuff[_XMODEM_PACKET_SIZE + 30], packetno = 1;
    int packetsize = 0, i, j, c, len = 0, retry = 0;
    bool crc = false;

    while (!udp_rx_buff ()) {
        if (wait (100, true)) {
            switch (c = udp_getc ()) {
                case _XYC:  // 1kbyte
                    crc = true;
                    packetsize = 1024;
                    break;
                case _NAK:  // 128byte
                    crc = false;
                    packetsize = 128;
                    break;
                case _CAN:  // canceled by remote
                    wait (50, false);
                    udp_rx_purge ();
                    udp_putc (_ACK);
                    return -1;
                    break;
                default:    // cancel
                    wait (50, false);
                    udp_rx_purge ();
                    udp_putc (_CAN);
                    return -1;
                    break;
            }
            break;
        }
        if (retry++ > _CNTOVER * 10) return -1;
    }

    for (;;) {
        xbuff[0] = crc ? _STX : _SOH;
        xbuff[1] = packetno;
        xbuff[2] = ~packetno;
        if ((c = srcsz - len) > packetsize) c = packetsize;
        if (c >= 0) {
            memset (&xbuff[3], 0xff, packetsize);
            sum2 = 0;
            for (i = 0; i < packetsize; i++) {
                xbuff[3 + i] = src[len + i];
                if (crc) {
                    sum2 = (uint16_t)(sum2 ^ xbuff[3 + i] << 8);
                    for (j = 0; j < 8; ++j) {
                        if (sum2 & 0x8000) sum2 = (uint16_t)(sum2 << 1 ^ 0x1021);
                        else               sum2 = sum2 << 1;
                    }
                } else sum2 = (sum2 + xbuff[3 + i]) & 0xff;
            }
            if (crc) {
                xbuff[packetsize + 3] = (uint8_t)((sum2 >> 8) & 0xFF);
                xbuff[packetsize + 4] = (uint8_t)(sum2 & 0xFF);
            } else xbuff[packetsize + 3] = (uint8_t)(sum2 & 0xff);
            for (retry = 0; retry < _CNTOVER; ++retry) {
                udp_putsb ((uint8_t *)xbuff, packetsize + 4 + (crc ? 1 : 0));
                if ((c = getc2 (200)) >= 0) {
                    switch (c) {
                        case _ACK:  // ok
                            packetno++;
                            len += packetsize;
                            retry = _CNTOVER;
                            break;
                        case _CAN:  // canceled by remote
                            wait (50, false);
                            udp_rx_purge ();
                            udp_putc (_ACK);
                            return -1;
                            break;
                        case _NAK:  // retry
                        default:
                            wait (50, false);
                            udp_rx_purge ();
                            break;
                    }
                }
            }
            if (c != _ACK) {
                wait (50, false);
                udp_rx_purge ();
                udp_putc (_CAN);
                return -1;
            }
        } else {
            for (retry = 0; retry < _CNTOVER; retry++) {
                udp_putc (_EOT);
                if ((c = getc2 (500)) == _ACK) break;
            }
            udp_rx_purge ();
            return (c == _ACK) ? len : -5;
        }
    }
    return -1;
}

/*****************************************************************************
 * Init/Deinit
 ****************************************************************************/
#ifdef ENABLE_RTC
// Referring to https://community.renesas.com/mcu-mpu/ra/f/forum/28558/ra6m2-rtc-issue
static uint32_t weekday_get(rtc_time_t * p_time) {
    const uint8_t month_table[13] = { 0, 0, 3, 3, 6, 1, 4, 6, 2, 5, 0, 3, 5 };
    const uint8_t ly_month_table[13] = { 0, 6, 2, 3, 6, 1, 4, 6, 2, 5, 0, 3, 5 };
    uint32_t day = (uint32_t)p_time->tm_mday;
    bool leap = false;

    if (0 == p_time->tm_year % 4) {
        leap = true;
        if (0 == p_time->tm_year % 100) {
            leap = false;
            if (0 != p_time->tm_year % 400) {
                leap = true;
            }
        }
    }

    uint32_t month_offset;
    if (leap) {
        month_offset = ly_month_table[p_time->tm_mon];
    } else {
        month_offset = month_table[p_time->tm_mon];
    }

    uint32_t year_offset    = (uint32_t)(p_time->tm_year % 100);
    uint32_t century_offset = (uint32_t)((3 - ((p_time->tm_year / 100) % 4)) * 2);
    uint32_t offset         = day + month_offset + year_offset + (year_offset / 4) + century_offset;
    uint32_t index          = offset % 7;

    return index;   // Sun = 0, Mon = 1, Tue = 2, Wed = 3, Thu = 4, Fri = 5, Sat = 6
}

static uint32_t yearday_get(rtc_time_t * p_time) {
    const uint8_t days_in_month[13]  = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
    uint8_t month_count;
    uint32_t year_day = 0;

    /* Add the number of elapsed months days together */
    /* For example, if the month is June, add Jan(31) + Feb(28) + Mar(31) + April(30) + May(31) */
    for( month_count=1; month_count<p_time->tm_mon; month_count++ ) {
        year_day += (uint32_t)days_in_month[month_count];
    }

    /* For example, if the day is June 26
     * Add 26 to this total */
    year_day += (uint32_t) p_time->tm_mday;

    /* If we are in a leap year, and the month has passed Feb, add an extra day to the total */
    bool leap = false;

    if (0 == p_time->tm_year % 4) {
        leap = true;
        if (0 == p_time->tm_year % 100) {
            leap = false;
            if (0 != p_time->tm_year % 400) {
                leap = true;
            }
        }
    }

    if ((leap) && (p_time->tm_mon > 1)) {
        year_day++;
    }

    return (year_day);
}

static void init_rtc (void) {
    rtc_time_t t;

    t.tm_hour = ((__TIME__[0] - '0') * 10 + __TIME__[1] - '0');
    t.tm_min =  ((__TIME__[3] - '0') * 10 + __TIME__[4] - '0');
    t.tm_sec =  ((__TIME__[6] - '0') * 10 + __TIME__[7] - '0');
    t.tm_year = (__DATE__[ 7] - '0') * 1000 +
                (__DATE__[ 8] - '0') * 100 +
                (__DATE__[ 9] - '0') * 10 +
                (__DATE__[10] - '0');
    t.tm_mday = ((__DATE__[4] >= '0') ? (__DATE__[4] - '0') * 10 : 0) +
                 (__DATE__[5] - '0');
    t.tm_mon =  ((__DATE__[0] == 'J' && __DATE__[1] == 'a' && __DATE__[2] == 'n')) ?  0 :
                ((__DATE__[0] == 'F'))                                             ?  1 :
                ((__DATE__[0] == 'M' && __DATE__[1] == 'a' && __DATE__[2] == 'r')) ?  2 :
                ((__DATE__[0] == 'A' && __DATE__[1] == 'p'))                       ?  3 :
                ((__DATE__[0] == 'M' && __DATE__[1] == 'a' && __DATE__[2] == 'y')) ?  4 :
                ((__DATE__[0] == 'J' && __DATE__[1] == 'u' && __DATE__[2] == 'n')) ?  5 :
                ((__DATE__[0] == 'J' && __DATE__[1] == 'u' && __DATE__[2] == 'l')) ?  6 :
                ((__DATE__[0] == 'A' && __DATE__[1] == 'u'))                       ?  7 :
                ((__DATE__[0] == 'S'))                                             ?  8 :
                ((__DATE__[0] == 'O'))                                             ?  9 :
                ((__DATE__[0] == 'N'))                                             ? 10 :
                ((__DATE__[0] == 'D'))                                             ? 11 : 255; // 255 == error
    t.tm_wday = (int)weekday_get (&t);
    t.tm_yday = (int)yearday_get (&t);
    t.tm_year = t.tm_year - 1900;
    t.tm_isdst = 0;

    // Power On Reset has been detected
    if (R_SYSTEM->RSTSR0_b.PORF == 1) {
        R_SYSTEM->RSTSR0 &= (uint8_t)~0x01; // Clear the PORF

        R_SYSTEM->RSTSR2_b.CWSF = 1;

/////////////////////////////////

        R_BSP_RegisterProtectDisable(BSP_REG_PROTECT_OM_LPC_BATT);

        R_SYSTEM->VBTBER_b.VBAE = 1;    //enable backup register access

        R_BSP_SoftwareDelay(1, BSP_DELAY_UNITS_MICROSECONDS);

        /* Is the test string in memory? */
        if( memcmp((void*)&R_SYSTEM->VBTBKR[0], "RTC Config", 10) != 0 ) {
            /* **************************************************************************************************** */
            /* Make "hard" settings to the "undefined on reset" RTC bits, to simulate what could be a valid setting */
            /* **************************************************************************************************** */
            do{
                R_RTC->RCR2_b.START = 0;
            } while (R_RTC->RCR2_b.START != 0);     // Stop the RTC,

            do{
                R_RTC->RCR2_b.CNTMD = 1;            // force the mode to Binary mode
            } while (R_RTC->RCR2_b.CNTMD != 1);

            do{
                R_RTC->RCR2_b.START = 1;            // Start the RTC, so that most of Open is skipped
            } while (R_RTC->RCR2_b.START != 1);     // Start the RTC,
        }


        /* This is a POR.
         * On detecting this, read the VBATT registers
         *
         * If VBatt registers contain {R,T,C, ,C,o,n,f,i,g} assume that
         * the VBatt was previously active and the RTC does not
         * need to be set
         */
        R_BSP_RegisterProtectDisable(BSP_REG_PROTECT_OM_LPC_BATT);

        /* Enable access to the VBatt circuit */
        R_SYSTEM->VBTBER_b.VBAE = 1;

        R_BSP_SoftwareDelay(1, BSP_DELAY_UNITS_MICROSECONDS);

        if (memcmp((void*)&R_SYSTEM->VBTBKR[0], "RTC Config", 10) == 0) {
        } else {
            /* Force RTC Start to 0
             * This will ensure that the RTC open will configure the RTC
             */
            do{
                R_RTC->RCR2_b.START = 0;
            } while (R_RTC->RCR2_b.START != 0);

            /* Force RTC Count Mode to 0 - Calendar count mode */
            do{
                R_RTC->RCR2_b.CNTMD = 0;
            } while (R_RTC->RCR2_b.CNTMD != 0);
        }
    }

    R_RTC_Open(&g_rtc0_ctrl, &g_rtc0_cfg);

    // Check if the RTC is already running
    if (R_RTC->RCR2_b.START == 0) {
        R_RTC_CalendarTimeSet(&g_rtc0_ctrl, &t);

        /* Set the VBatt registers to indicate that the RTC is configured and running
         * Therefore, another Power On Reset (with VBatt active) will not reset the RTC
         * and time will be maintained
         * */
        R_BSP_RegisterProtectDisable(BSP_REG_PROTECT_OM_LPC_BATT);

        /* Enable access to the VBatt circuit */
        R_SYSTEM->VBTBER_b.VBAE = 1;

        R_BSP_SoftwareDelay (1, BSP_DELAY_UNITS_MICROSECONDS);
        R_SYSTEM->VBTBKR[0] = 'R';
        R_SYSTEM->VBTBKR[1] = 'T';
        R_SYSTEM->VBTBKR[2] = 'C';
        R_SYSTEM->VBTBKR[3] = ' ';
        R_SYSTEM->VBTBKR[4] = 'C';
        R_SYSTEM->VBTBKR[5] = 'o';
        R_SYSTEM->VBTBKR[6] = 'n';
        R_SYSTEM->VBTBKR[7] = 'f';
        R_SYSTEM->VBTBKR[8] = 'i';
        R_SYSTEM->VBTBKR[9] = 'g';
        R_SYSTEM->VBTBKR[10] = '\0';

        R_BSP_SoftwareDelay(1, BSP_DELAY_UNITS_MICROSECONDS);

        R_SYSTEM->VBTBER_b.VBAE = 0;
    }
}
#endif

static void Init (void) {
#ifdef LED
    SysTick->CTRL = 0;
    SysTick_Config (100000000 / 10000);
#endif

    // Init GPIO
    R_IOPORT_Open(&g_ioport_ctrl, &g_bsp_pin_cfg);

    // Init UART
    udp_init (txb, sizeof(txb), rxb, sizeof(rxb));

#ifdef ENABLE_RTC
    // Init RTC
    init_rtc ();
#endif

    R_FLASH_HP_Open(&g_flash0_ctrl, &g_flash0_cfg);
}

static void DeInit (void) {
#ifdef LED
    SysTick->CTRL = 0;
    SysTick->LOAD = 0;
    SysTick->VAL = 0;
#endif

    udp_deinit ();
    R_FLASH_HP_Close (&g_flash0_ctrl);
    R_IOPORT_Close(&g_ioport_ctrl);
}

/*****************************************************************************
 * Dump
 ****************************************************************************/
typedef enum {
    mode_str,
    mode_hex,
    mode_dec,
} TCharMode;

static char *getstr (char *s, int len, TCharMode m) {
    int i = 0;
    char c;

    while (1) {
        c = udp_getc ();
        if (c == '\n' || c == '\r') {
            s[i++] = '\0';
            break;
        } else if ((i > 0) && (c == '\b')) {
            udp_puts ("\b \b");
            i--;
        } else if ((i < len) && (i >= 0) && (c >= ' ') && (c <= '~')) {
            switch (m) {
                case mode_dec:
                    if ((c >= '0') && (c <= '9')) {
                        s[i++] = (char)c;
                        udp_putc (c);
                    }
                    break;
                case mode_hex:
                    if (((c >= '0') && (c <= '9')) || ((c >= 'a') && (c <= 'f')) || ((c >= 'A') && (c <= 'F'))) {
                        s[i++] = c;
                        udp_putc (c);
                    }
                    break;
                default:
                    s[i++] = (char)c;
                    udp_putc (c);
                    break;
            }
        } else udp_putc ('\a');
    }
    return s;
}

/*****************************************************************************
 * mem dump
 ****************************************************************************/
static void dump (uint32_t addr) {
    uint32_t i, j;
    uint8_t *dat = (uint8_t *)addr;
    char c;
    while (1) {
        for (i = 0; i < 16; i++) {
            udp_printf("%08X:", dat + (i << 4));
            for (j = 0; j < 16; j++) {
                udp_printf("%02X ", dat[(i << 4) + j]);
            }
            udp_putc (' ');
            for (j = 0; j < 16; j++) {
                c = (char)dat[(i << 4) + j];
                udp_putc (((0x20 <= c) && (c < 0x7f)) ? c : '.');
            }
            udp_puts("\r\n");
        }
        udp_puts ("---([u]up [d]down [ESC]exit)---\r\n");
        switch (udp_getc ()) {
            case 0x1b:
            case 'q':
            case 'Q':
                return;
                break;
            case ' ':
            case 'u':
            case 'U':
            case '\r':
                dat += 256;
                break;
            case 'd':
            case 'D':
                dat -= 256;
                break;
        }
#ifdef _LED
        duty = 100;
#endif
    }
}

typedef void (*fp)(void);

/*****************************************************************************
 * for user programs
 ****************************************************************************/
static uint32_t FlashIsNotEmpty (void) {
    uint32_t *FWBOOT = (uint32_t *)(USER_FLASH_START_ADDRESS + 4);
    if ((USER_FLASH_START_ADDRESS <= *FWBOOT) && (*FWBOOT <= USER_FLASH_END_ADDRESS)) return *FWBOOT;
    else return 0;
}


__attribute__((always_inline)) __STATIC_INLINE void __set_PC(uint32_t ProgramCounter) {
    __ASM volatile ("MOV pc, %0" : : "r" (ProgramCounter) : );
}

#define EnablePrivilegedMode() __asm("SVC #0")

static bool JumpToUserProgram (void) {
    uint32_t jump_address = FlashIsNotEmpty ();
    if (jump_address != 0) {
        DeInit ();
        R_BSP_SoftwareDelay(500, BSP_DELAY_UNITS_MILLISECONDS);

#if 0
        __disable_irq();

        if (CONTROL_nPRIV_Msk & __get_CONTROL()) EnablePrivilegedMode( ) ;

        NVIC->ICER[ 0 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 1 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 2 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 3 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 4 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 5 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 6 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 7 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 8 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 9 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 10 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 11 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 12 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 13 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 14 ] = 0xFFFFFFFF ;
        NVIC->ICER[ 15 ] = 0xFFFFFFFF ;


        NVIC->ICPR[ 0 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 1 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 2 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 3 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 4 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 5 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 6 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 7 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 8 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 9 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 10 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 11 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 12 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 13 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 14 ] = 0xFFFFFFFF ;
        NVIC->ICPR[ 15 ] = 0xFFFFFFFF ;


        SysTick->CTRL = 0 ;
        SCB->ICSR |= SCB_ICSR_PENDSTCLR_Msk;

        SCB->SHCSR &= ~(SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk | SCB_SHCSR_MEMFAULTENA_Msk);

        __set_PSPLIM(0);
        __set_MSPLIM(0);
        R_MPU_SPMON->SP[0].CTL = 0;

        if (CONTROL_SPSEL_Msk & __get_CONTROL( )) {
          __set_MSP (__get_PSP( )) ;
          __set_CONTROL (__get_CONTROL( ) & ~CONTROL_SPSEL_Msk) ;
        }
        SCB->VTOR = ( uint32_t )jump_address & 0x1FFFFF80;

        void (*UserProgram)(void) = (fp)*(uint32_t*)(jump_address + 4);
        __set_MSP (*(uint32_t*)jump_address);
        UserProgram ();

        __NOP ();
        __NOP ();
        __NOP ();
#else

        __disable_irq();

        //disable Main Stack Pointer monitoring
        R_MPU_SPMON->SP[0].CTL = 0;

        // Set the Vector Table Offset Register
        SCB->VTOR = USER_FLASH_START_ADDRESS & 0xffffff80;//jump_address & 0xffffff80;

        __DSB();

        // Ensure that when we jump we're using the Main Stack Pointer
        __set_CONTROL(0);

        __ISB();

        __set_PSPLIM(0);
        __set_MSPLIM(0);

//        __set_MSP(*(uint32_t *)jump_address);
        __set_MSP(*(uint32_t *)USER_FLASH_START_ADDRESS);

        __enable_irq();
        __ASM volatile ("bx %0"::"r"(jump_address));
//        __set_PC(*(uint32_t *)(jump_address + 4));

        __NOP();
        __NOP();
        __NOP();

        // Shouldn't get here
        while(1);
#endif
    }
    return false;
}

/*****************************************************************************
 * etc
 ****************************************************************************/
static bool AreYouSure (void) {
    char c;
    udp_puts ("(y/n)");
    udp_putc ((c = udp_getc ()));
    return (c == 'y');
}

#ifdef LED
void SysTick_Handler(void);

void SysTick_Handler (void) {
    static int freq = 0;

    if (duty > 0) {
        if (++freq > 100) {
            freq = 0;
            duty -= 3;
        }
        if (duty > 0) {
            R_IOPORT_PinWrite(&g_ioport_ctrl, _LED, (duty <= freq) ? BSP_IO_LEVEL_LOW: BSP_IO_LEVEL_HIGH);
        } else {
            R_IOPORT_PinWrite(&g_ioport_ctrl, _LED, BSP_IO_LEVEL_LOW);
            freq = 0;
        }
    }
}
#endif

void hal_entry(void) {
    const char *mess[] = {
        "\nRA4M2 USB BL0.6",
        "\n>","\nOK","\nNG",
        "\nw:write r:read e:erase d:dump g:go",
    };

    int64_t i, j;
    char c = 3, s[20];

    // For cold start or software reset
    if (R_SYSTEM->RSTSR2_b.CWSF == 0 || R_SYSTEM->RSTSR1_b.SWRF == 1) {
        R_SYSTEM->RSTSR2_b.CWSF = 1;
        JumpToUserProgram ();
    }

    Init ();
    for (;;) {
        if (udp_rx_buff ()) {
#ifdef LED
            duty = 100;
#endif
            udp_putc ((c = udp_getc ()));
            switch (c) {
#ifdef ENABLE_RTC
                case 's':
                    if (AreYouSure ()) {
                        char *dlm="/ :\n";
                        rtc_time_t time_t;
                        udp_puts("\nDateTime:");
                        udp_gets (s, 19);
                        udp_puts("\n");
                        time_t.tm_year = D2I(strtok(s ,dlm))- RTC_TIME_H_YEAR_OFFSET;
                        time_t.tm_mon  = D2I(strtok(NULL, dlm)) - 1;
                        time_t.tm_mday = D2I(strtok(NULL, dlm));
                        time_t.tm_hour = D2I(strtok(NULL, dlm));
                        time_t.tm_min  = D2I(strtok(NULL, dlm));
                        time_t.tm_sec  = D2I(strtok(NULL, dlm));
                        udp_puts ((R_RTC_CalendarTimeSet(&g_rtc0_ctrl, &time_t) == FSP_SUCCESS) ? mess[2] : mess[3]);
                    }
                    break;
#endif
                case 'w':
                    if (AreYouSure ()) udp_puts (XmodemRX () ? mess[2] : mess[3]);
                    break;
                case 'g':
                    if (FlashIsNotEmpty () != 0) NVIC_SystemReset();
                    else udp_puts (mess[3]);
                    break;
                case 'r':
                    udp_puts ("\n START ADDRESS:$");
                    if ((i = H2I (getstr (s, 8, mode_hex))) < 0) i = USER_FLASH_START_ADDRESS;
                    udp_printf ("\33[%cD%08X",strlen(s)+'0',i);
                    udp_puts ("\n SiZE:$");
                    if ((j = H2I (getstr (s, 8, mode_hex))) < 0) j = USER_FLASH_END_ADDRESS - IAP_HEAD_ADDRESS + 1;
                    udp_printf ("\33[%cD%08X\n START DOWNLOAD...",strlen(s)+'0',j);
                    if (AreYouSure ()) udp_puts (XmodemTX ((uint8_t *)(uint32_t)i, (int)j) ? "\n <SUCCESS>" : "\n <ERROR>");
                    break;

                case 'e':
                    if (AreYouSure ()) udp_puts (FLASH_EraseAll () ? mess[2] : mess[3]);
                    break;
                case 'd':
                    udp_puts ("\n START ADDRESS:$");
                    if ((i = H2I (getstr (s, 8, mode_hex))) < 0) i = USER_FLASH_START_ADDRESS;
                    udp_printf ("\33[%cD%08X\r\n",strlen(s)+'0',i);
                    dump ((uint32_t)i);
                    break;
                default:
                    udp_puts ((char *)mess[0]);
                    udp_puts ((char *)mess[4]);
                    break;
            }
#ifdef ENABLE_RTC
            rtc_time_t time_t;
            R_RTC_CalendarTimeGet(&g_rtc0_ctrl, &time_t);
            udp_printf ("\n%02d/%02d/%02d %02d:%02d:%02d>",time_t.tm_year+RTC_TIME_H_YEAR_OFFSET, time_t.tm_mon + 1, time_t.tm_mday, time_t.tm_hour, time_t.tm_min, time_t.tm_sec);
#else
            udp_puts ((char *)mess[1]);
#endif
            __WFI ();
        }
    }
}
