/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file 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.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "stm32_util.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <stm32_dma.h>
#include <hrt.h>

static int64_t utc_time_offset;

/*
  setup the timer capture digital filter for a channel
 */
void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t filter_mode)
{
    switch (channel) {
    case 0:
        tim->CCMR1 |= STM32_TIM_CCMR1_IC1F(filter_mode);
        break;
    case 1:
        tim->CCMR1 |= STM32_TIM_CCMR1_IC2F(filter_mode);
        break;
    case 2:
        tim->CCMR2 |= STM32_TIM_CCMR2_IC3F(filter_mode);
        break;
    case 3:
        tim->CCMR2 |= STM32_TIM_CCMR2_IC4F(filter_mode);
        break;
    }
}

/*
  set the input source of a timer channel
 */    
void stm32_timer_set_channel_input(stm32_tim_t *tim, uint8_t channel, uint8_t input_source)
{
    switch (channel) {
        case 0:
            tim->CCER &= ~STM32_TIM_CCER_CC1E;
            tim->CCMR1 &= ~STM32_TIM_CCMR1_CC1S_MASK;
            tim->CCMR1 |= STM32_TIM_CCMR1_CC1S(input_source);
            tim->CCER |= STM32_TIM_CCER_CC1E;
            break;
        case 1:
            tim->CCER &= ~STM32_TIM_CCER_CC2E;
            tim->CCMR1 &= ~STM32_TIM_CCMR1_CC2S_MASK;
            tim->CCMR1 |= STM32_TIM_CCMR1_CC2S(input_source);
            tim->CCER |= STM32_TIM_CCER_CC2E;
            break;
        case 2:
            tim->CCER &= ~STM32_TIM_CCER_CC3E;
            tim->CCMR2 &= ~STM32_TIM_CCMR2_CC3S_MASK;
            tim->CCMR2 |= STM32_TIM_CCMR2_CC3S(input_source);
            tim->CCER |= STM32_TIM_CCER_CC3E;
            break;
        case 3:
            tim->CCER &= ~STM32_TIM_CCER_CC4E;
            tim->CCMR2 &= ~STM32_TIM_CCMR2_CC4S_MASK;
            tim->CCMR2 |= STM32_TIM_CCMR2_CC4S(input_source);
            tim->CCER |= STM32_TIM_CCER_CC4E;
            break;
    }
}

#if CH_DBG_ENABLE_STACK_CHECK == TRUE && !defined(HAL_BOOTLOADER_BUILD)
void show_stack_usage(void)
{
  thread_t *tp;

  tp = chRegFirstThread();
  do {
      uint32_t stklimit = (uint32_t)tp->wabase;
      uint8_t *p = (uint8_t *)tp->wabase;
      while (*p == CH_DBG_STACK_FILL_VALUE) {
          p++;
      }
      uint32_t stack_left = ((uint32_t)p) - stklimit;
      printf("%s %u\n", tp->name, (unsigned)stack_left);
      tp = chRegNextThread(tp);
  } while (tp != NULL);
}
#endif

/*
  set the utc time
 */
void stm32_set_utc_usec(uint64_t time_utc_usec)
{
    uint64_t now = hrt_micros64();
    if (now <= time_utc_usec) {
        utc_time_offset = time_utc_usec - now;
    }
}

/*
  get system clock in UTC microseconds
*/
uint64_t stm32_get_utc_usec()
{
    return hrt_micros64() + utc_time_offset;
}

struct utc_tm {
    uint8_t tm_year; // since 1900
    uint8_t tm_mon;  // zero based
    uint8_t tm_mday; // zero based
    uint8_t tm_hour;
    uint8_t tm_min;
    uint8_t tm_sec;
};


/*
  return true if a year is a leap year
 */
static bool is_leap(uint32_t y)
{
    y += 1900;
    return (y % 4) == 0 && ((y % 100) != 0 || (y % 400) == 0);
}

static const uint8_t ndays[2][12] ={
    {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31},
    {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}};

/*
  parse a seconds since 1970 into a utc_tm structure
  code based on _der_gmtime from samba
 */
static void parse_utc_seconds(uint64_t utc_sec, struct utc_tm *tm)
{
    uint32_t secday = utc_sec % (3600U * 24U);
    uint32_t days = utc_sec / (3600U * 24U);

    memset(tm, 0, sizeof(*tm));

    tm->tm_sec = secday % 60U;
    tm->tm_min = (secday % 3600U) / 60U;
    tm->tm_hour = secday / 3600U;
    tm->tm_year = 70;

    if (days > (2000 * 365)) {
        // don't look for dates too far into the future
        return;
    }

    while (true) {
        unsigned dayinyear = (is_leap(tm->tm_year) ? 366 : 365);
        if (days < dayinyear) {
            break;
        }
        tm->tm_year += 1;
        days -= dayinyear;
    }
    tm->tm_mon = 0;

    while (true) {
        unsigned daysinmonth = ndays[is_leap(tm->tm_year)?1:0][tm->tm_mon];
        if (days < daysinmonth) {
            break;
        }
        days -= daysinmonth;
        tm->tm_mon++;
    }
    tm->tm_mday = days + 1;
}


/*
  get time for fat filesystem. This is based on
  rtcConvertDateTimeToFAT from the ChibiOS RTC driver. We don't use
  the hw RTC clock as it is very inaccurate
 */
uint32_t get_fattime()
{
    if (utc_time_offset == 0) {
        // return a fixed time
        return ((uint32_t)0 | (1 << 16)) | (1 << 21);
    }
    uint64_t utc_usec = stm32_get_utc_usec();
    uint64_t utc_sec = utc_usec / 1000000UL;
    struct utc_tm tm;

    parse_utc_seconds(utc_sec, &tm);
    
    uint32_t fattime;

    fattime  = tm.tm_sec  >> 1U;
    fattime |= tm.tm_min  << 5U;
    fattime |= tm.tm_hour << 11U;
    fattime |= tm.tm_mday << 16U;
    fattime |= (tm.tm_mon+1)  << 21U;
    fattime |= (uint32_t)((tm.tm_year-80) << 25U);
    
    return fattime;
}

#if AP_FASTBOOT_ENABLED

// get RTC backup registers starting at given idx
void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n)
{
    while (n--) {
#if defined(STM32F1)
        (void)idx;
        __IO uint32_t *dr = (__IO uint32_t *)&BKP->DR1;
        *v++ = (dr[n/2]&0xFFFF) | (dr[n/2+1]<<16);
#elif defined(STM32G4)
        *v++ = ((__IO uint32_t *)&TAMP->BKP0R)[idx++];
#else
        *v++ = ((__IO uint32_t *)&RTC->BKP0R)[idx++];
#endif
    }
}

// set n RTC backup registers starting at given idx
void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n)
{
#if !defined(STM32F1)
    if ((RCC->BDCR & RCC_BDCR_RTCEN) == 0) {
        RCC->BDCR |= STM32_RTCSEL;
        RCC->BDCR |= RCC_BDCR_RTCEN;
    }
#ifdef PWR_CR_DBP
    PWR->CR |= PWR_CR_DBP;
#else
    PWR->CR1 |= PWR_CR1_DBP;
#endif
#endif
    while (n--) {
#if defined(STM32F1)
        (void)idx;
        __IO uint32_t *dr = (__IO uint32_t *)&BKP->DR1;
        dr[n/2] =   (*v) & 0xFFFF;
        dr[n/2+1] = (*v) >> 16;
#elif defined(STM32G4)
        ((__IO uint32_t *)&TAMP->BKP0R)[idx++] = *v++;
#else
        ((__IO uint32_t *)&RTC->BKP0R)[idx++] = *v++;
#endif
    }
}

// see if RTC registers is setup for a fast reboot
enum rtc_boot_magic check_fast_reboot(void)
{
    uint32_t v;
    get_rtc_backup(0, &v, 1);
    return (enum rtc_boot_magic)v;
}

// set RTC register for a fast reboot
void set_fast_reboot(enum rtc_boot_magic v)
{
    if (check_fast_reboot() != v) {
        uint32_t vv = (uint32_t)v;
        set_rtc_backup(0, &vv, 1);
    }
}

#else // AP_FASTBOOT_ENABLED is not set

// set n RTC backup registers starting at given idx
void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n)
{
    (void)idx;
    (void)v;
    (void)n;
}

// get RTC backup registers starting at given idx
void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n)
{
    (void)idx;
    (void)v;
    (void)n;
}
#endif // AP_FASTBOOT_ENABLED

/*
  enable peripheral power if needed This is done late to prevent
  problems with CTS causing SiK radios to stay in the bootloader. A
  SiK radio will stay in the bootloader if CTS is held to GND on boot
*/
void peripheral_power_enable(void)
{
#if defined(HAL_GPIO_PIN_nVDD_5V_PERIPH_EN) || defined(HAL_GPIO_PIN_VDD_5V_PERIPH_EN) || defined(HAL_GPIO_PIN_nVDD_5V_HIPOWER_EN) || defined(HAL_GPIO_PIN_VDD_3V3_SENSORS_EN)|| defined(HAL_GPIO_PIN_VDD_3V3_SENSORS2_EN) || defined(HAL_GPIO_PIN_VDD_3V3_SENSORS3_EN) || defined(HAL_GPIO_PIN_VDD_3V3_SENSORS4_EN) || defined(HAL_GPIO_PIN_nVDD_3V3_SD_CARD_EN) || defined(HAL_GPIO_PIN_VDD_3V3_SD_CARD_EN) || defined(HAL_GPIO_PIN_VDD_3V5_LTE_EN)
    // we don't know what state the bootloader had the CTS pin in, so
    // wait here with it pulled up from the PAL table for enough time
    // for the radio to be definately powered down
    uint8_t i;
    for (i=0; i<100; i++) {
        // use a loop as this may be a 16 bit timer
        chThdSleep(chTimeMS2I(1));
    }
#ifdef HAL_GPIO_PIN_nVDD_5V_PERIPH_EN
    palWriteLine(HAL_GPIO_PIN_nVDD_5V_PERIPH_EN, 0);
#endif
#ifdef HAL_GPIO_PIN_VDD_5V_PERIPH_EN
    palWriteLine(HAL_GPIO_PIN_VDD_5V_PERIPH_EN, 1);
#endif
#ifdef HAL_GPIO_PIN_nVDD_5V_HIPOWER_EN
    palWriteLine(HAL_GPIO_PIN_nVDD_5V_HIPOWER_EN, 0);
#endif
#ifdef HAL_GPIO_PIN_VDD_5V_HIPOWER_EN
    palWriteLine(HAL_GPIO_PIN_VDD_5V_HIPOWER_EN, 1);
#endif
#ifdef HAL_GPIO_PIN_VDD_3V3_SENSORS_EN
    // the TBS-Colibri-F7 needs PE3 low at power on
    palWriteLine(HAL_GPIO_PIN_VDD_3V3_SENSORS_EN, 1);
#endif
#ifdef HAL_GPIO_PIN_VDD_3V3_SENSORS2_EN
    palWriteLine(HAL_GPIO_PIN_VDD_3V3_SENSORS2_EN, 1);
#endif
#ifdef HAL_GPIO_PIN_VDD_3V3_SENSORS3_EN
    palWriteLine(HAL_GPIO_PIN_VDD_3V3_SENSORS3_EN, 1);
#endif
#ifdef HAL_GPIO_PIN_VDD_3V3_SENSORS4_EN
    palWriteLine(HAL_GPIO_PIN_VDD_3V3_SENSORS4_EN, 1);
#endif
#ifdef HAL_GPIO_PIN_nVDD_3V3_SD_CARD_EN
    // the TBS-Colibri-F7 needs PG7 low for SD card
    palWriteLine(HAL_GPIO_PIN_nVDD_3V3_SD_CARD_EN, 0);
#endif
#ifdef HAL_GPIO_PIN_VDD_3V3_SD_CARD_EN
    // others need it active high
    palWriteLine(HAL_GPIO_PIN_VDD_3V3_SD_CARD_EN, 1);
#endif
#ifdef HAL_GPIO_PIN_VDD_3V5_LTE_EN
    palWriteLine(HAL_GPIO_PIN_VDD_3V5_LTE_EN, 1);
#endif
    for (i=0; i<20; i++) {
        // give 20ms for sensors to settle
        chThdSleep(chTimeMS2I(1));
    }
#endif
}

#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) || defined(STM32G4) || defined(STM32L4) || defined(STM32L4PLUS)
/*
  read mode of a pin. This allows a pin config to be read, changed and
  then written back
 */
iomode_t palReadLineMode(ioline_t line)
{
    ioportid_t port = PAL_PORT(line);
    uint8_t pad = PAL_PAD(line);
    iomode_t ret = 0;
    ret |= (port->MODER >> (pad*2)) & 0x3;
    ret |= ((port->OTYPER >> pad)&1) << 2;
    ret |= ((port->OSPEEDR >> (pad*2))&3) << 3;
    ret |= ((port->PUPDR >> (pad*2))&3) << 5;
    if (pad < 8) {
        ret |= ((port->AFRL >> (pad*4))&0xF) << 7;
    } else {
        ret |= ((port->AFRH >> ((pad-8)*4))&0xF) << 7;
    }
    return ret;
}

/*
  set pin as pullup, pulldown or floating
 */
void palLineSetPushPull(ioline_t line, enum PalPushPull pp)
{
    ioportid_t port = PAL_PORT(line);
    uint8_t pad = PAL_PAD(line);
    port->PUPDR = (port->PUPDR & ~(3<<(pad*2))) | (pp<<(pad*2));
}

#endif // F7, H7, F4

void stm32_cacheBufferInvalidate(const void *p, size_t size)
{
    cacheBufferInvalidate(p, size);
}

void stm32_cacheBufferFlush(const void *p, size_t size)
{
    cacheBufferFlush(p, size);
}


#ifdef HAL_GPIO_PIN_FAULT
/*
  optional support for hard-fault debugging using soft-serial output to a pin
  To use this setup a pin like this:

    Pxx FAULT OUTPUT HIGH

  for some pin Pxx

  On a STM32F405 the baudrate will be around 42kBaud. Use the
  auto-baud function on your logic analyser to decode
*/
/*
  send one bit out a debug line
 */
static void fault_send_bit(ioline_t line, uint8_t b)
{
    palWriteLine(line, b);
    for (uint32_t i=0; i<1000; i++) {
        palWriteLine(line, b);
    }
}

/*
  send a byte out a debug line
 */
static void fault_send_byte(ioline_t line, uint8_t b)
{
    fault_send_bit(line, 0); // start bit
    for (uint8_t i=0; i<8; i++) {
        uint8_t bit = (b & (1U<<i))?1:0;
        fault_send_bit(line, bit);
    }
    fault_send_bit(line, 1); // stop bit
}

/*
  send a string out a debug line
 */
static void fault_send_string(const char *str)
{
    while (*str) {
        fault_send_byte(HAL_GPIO_PIN_FAULT, (uint8_t)*str++);
    }
    fault_send_byte(HAL_GPIO_PIN_FAULT, (uint8_t)'\n');
}

void fault_printf(const char *fmt, ...)
{
    static char buffer[100];
    va_list ap;
    va_start(ap, fmt);
    vsnprintf(buffer, sizeof(buffer), fmt, ap);
    va_end(ap);
    fault_send_string(buffer);
}
#endif // HAL_GPIO_PIN_HARDFAULT

void system_halt_hook(void)
{
#ifdef HAL_GPIO_PIN_FAULT
    // optionally print the message on a fault pin
    while (true) {
        fault_printf("PANIC:%s\n", currcore->dbg.panic_msg);
        fault_printf("RA0:0x%08x\n", __builtin_return_address(0));
    }
#endif
}

// hook for stack overflow
void stack_overflow(thread_t *tp)
{
#if !defined(HAL_BOOTLOADER_BUILD) && !defined(IOMCU_FW)
    extern void AP_stack_overflow(const char *thread_name);
    AP_stack_overflow(tp->name);
    // if we get here then we are armed and got a stack overflow. We
    // will report an internal error and keep trying to fly. We are
    // quite likely to crash anyway due to memory corruption. The
    // watchdog data should record the thread name and fault type
#else
    (void)tp;
#endif
}

#if CH_DBG_ENABLE_STACK_CHECK == TRUE
/*
  check how much stack is free given a stack base. Assumes the fill
  byte is 0x55
 */
uint32_t stack_free(void *stack_base)
{
    const uint32_t *p = (uint32_t *)stack_base;
    const uint32_t canary_word = 0x55555555;
    while (*p == canary_word) {
        p++;
    }
    return ((uint32_t)p) - (uint32_t)stack_base;
}
#endif

#if HAL_USE_HW_RNG && defined(RNG)
static bool stm32_rand_generate(uint32_t *val)
{
    uint32_t error_bits = 0;
    error_bits = RNG_SR_SEIS | RNG_SR_CEIS;
    /* Check for error flags and if data is ready. */
    if (((RNG->SR & error_bits) == 0) && ((RNG->SR & RNG_SR_DRDY) == RNG_SR_DRDY)) {
        *val = RNG->DR;
    } else {
        return false;
    }
    return true;
}

bool stm32_rand_generate_blocking(unsigned char* output, unsigned int sz, uint32_t timeout_us)
{
    unsigned int i = 0;
    uint32_t run_until = hrt_micros32() + timeout_us;
    uint32_t val;
    while ((i < sz) && (hrt_micros32() < run_until)) {
        /* If not aligned or there is odd/remainder */
        if( (i + sizeof(uint32_t)) > sz ||
            ((uint32_t)&output[i] % sizeof(uint32_t)) != 0) {
            /* Single byte at a time */
            if (stm32_rand_generate(&val)) {
                output[i] = val;
                i++;
            }
        } else {
            /* Use native 32 bit copy instruction */
            if (stm32_rand_generate((uint32_t*)&output[i])) {
                i += sizeof(uint32_t);
            }
        }
    }
    return i >= sz;
}

unsigned int stm32_rand_generate_nonblocking(unsigned char* output, unsigned int sz)
{
    if ((RNG->SR & RNG_SR_DRDY) != RNG_SR_DRDY) {
        return false;
    }
    unsigned int i = 0;
    uint32_t val;
    while (i < sz) {
        /* If not aligned or there is odd/remainder */
        if( (i + sizeof(uint32_t)) > sz ||
            ((uint32_t)&output[i] % sizeof(uint32_t)) != 0) {
            /* Single byte at a time */
            if (stm32_rand_generate(&val)) {
                output[i] = val;
                i++;
            } else {
                break;
            }
        } else {
            /* Use native 32 bit copy instruction */
            if (stm32_rand_generate((uint32_t*)&output[i])) {
                i += sizeof(uint32_t);
            } else {
                break;
            }
        }
    }
    return i;
}

#endif // #if HAL_USE_HW_RNG && defined(RNG)

/*
  see if we should limit flash to 1M on devices with older revisions of STM32F427
 */
#ifdef STM32F427xx
bool check_limit_flash_1M(void)
{
    const uint16_t revid = (*(uint32_t *)DBGMCU_BASE) >> 16;
    static const uint16_t badrevs[4] = { 0x1000, 0x1001, 0x1003, 0x1007 };
    for (uint8_t i=0; i<4; i++) {
        if (revid == badrevs[i]) {
            return true;
        }
    }
    return false;
}
#endif


#if defined(DUAL_CORE)
void stm32_disable_cm4_core() {
    // Turn off second core for now
    if ((FLASH->OPTSR_CUR & FLASH_OPTSR_BCM4)) {
        //unlock flash
        if (FLASH->OPTCR & FLASH_OPTCR_OPTLOCK) {
            /* Unlock sequence */
            FLASH->OPTKEYR = 0x08192A3B;
            FLASH->OPTKEYR = 0x4C5D6E7F;
        }
        while (FLASH->OPTSR_CUR & FLASH_OPTSR_OPT_BUSY) {
        }
        // disable core boot
        FLASH->OPTSR_PRG &= ~FLASH_OPTSR_BCM4;
        // start programming
        FLASH->OPTCR |= FLASH_OPTCR_OPTSTART;
        // wait for completion by checking busy bit
        while (FLASH->OPTSR_CUR & FLASH_OPTSR_OPT_BUSY) {
        }
        // lock flash
        FLASH->OPTCR |= FLASH_OPTCR_OPTLOCK;
        while (FLASH->OPTSR_CUR & FLASH_OPTSR_OPT_BUSY) {
        }
    }
}
#endif // DUAL_CORE