#include "stm32f10x.h"
#include "FreeRTOS.h"
#include "task.h"
#include "utils.h"
#include "io/io_cli.h"
#include "io/io_gsm.h"
#include <stdlib.h>

extern void main(void)  __attribute__((noreturn));
extern void xPortPendSVHandler( void );
extern void xPortSysTickHandler( void );
extern void vPortSVCHandler( void );
extern void mmc_dma_isr( void );

/* provided by the linker script */
extern unsigned long _sidata;            /* start address of the static initialization data */
extern unsigned long _sdata;             /* start address of the data section */
extern unsigned long _edata;             /* end address of the data section */
extern unsigned long _sbss;              /* start address of the bss section */
extern unsigned long _ebss;              /* end address of the bss section */
extern unsigned long _eheap;              /* end address of the heap */

/* Static array for containing the handler-mode stack.
 * 64 items * 4 bytes = 512 bytes.
 */
static int handler_stack[64];
#define STACK_TOP ((void*)handler_stack + sizeof(handler_stack))

/* Simple static & bss data initializer. */
register void *stack_pointer asm("sp");
void __Init_Data() __attribute__((noreturn, naked));
void __Init_Data() {    
    /* hope that GCC actually places these in registers
     * instead of overwriting them halfway through */
    register unsigned long *src, *dst;
    
    /* copy the data segment into ram */
    src = &_sidata;
    dst = &_sdata;
    if (src != dst)
        while(dst < &_edata)
            *(dst++) = *(src++);
 
    /* zero the bss segment */
    dst = &_sbss;
    while(dst < &_ebss)
        *(dst++) = 0;
    
    // This stack is used for startup and interrupts
    // The register is automatically initialized from 0x00000000,
    // but if we are using the bootloader then this is wrong.
    stack_pointer = STACK_TOP;
    handler_stack[0] = handler_stack[1] = 0xDEADBEEF;
    
    /* initialize malloc space */
    unsigned long *block_start = &_ebss;
    *block_start++ = 0xDEADBEEF;
    *block_start++ = 0xDEADBEEF;
    size_t block_size = (void*)&_eheap - (void*)block_start;
    add_malloc_block(block_start, block_size);
    
    main();
}

/* Very fool-proof functions to report errors through serial console. */
 
static char hexnibble(int n)
{
    if (n < 10)
        return '0' + n;
    else
        return 'A' - 10 + n;
}

static void putstring(const char *p)
{
    while (*p)
    {
        while(!(USART1->SR & USART_SR_TXE));
        USART1->DR=*p;
        p++;
    }
}

static void puthex(const char *p, int count)
{
    while (count--)
    {
        while(!(USART1->SR & USART_SR_TXE));
        USART1->DR = hexnibble(*(p + count) >> 4);
        while(!(USART1->SR & USART_SR_TXE));
        USART1->DR = hexnibble(*(p + count) & 0x0F);
    }
}

/* Fault handlers for various events */

void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed char *pcTaskName )
{
	/* This function will get called if a task overflows its stack.   If the
	parameters are corrupt then inspect pxCurrentTCB to find which was the
	offending task. */
	
	putstring("STACK OVERFLOW: ");
	putstring((char*)pcTaskName);
	putstring("\n");
	
	for( ;; );
}

void assert_failed( unsigned char *pucFile, unsigned long ulLine )
{
	( void ) pucFile;
	( void ) ulLine;

	putstring("ASSERTFAIL: ");
	putstring((char*)pucFile);
	puthex((char*)&ulLine, sizeof(ulLine));
	putstring("\n");

	for( ;; );
}


void nmi_handler(void)
{
    putstring("NMI");
    
    for(;;);
}

void hardfault_handler(void)
{
    putstring("HARDFAULT\n");
    
    void *pc = __builtin_return_address(0);
    putstring("PC: ");
    puthex((char*) &pc, sizeof(pc));
    
    putstring("\nSCB_HFSR: ");
    puthex((char*) &(SCB->HFSR), sizeof(SCB->HFSR));
    
    putstring("\nSCB_CFSR: ");
    puthex((char*) &(SCB->CFSR), sizeof(SCB->CFSR));
    
    if (pc == (void*)0xFFFFFFFDU)
    {
        // Fault happened in thread mode, get actual info from thread stack
        void **psp;
        asm("mrs %0, psp" : "=r"(psp) : :);
        putstring("\nTHREAD PC: ");
        puthex((char*) (psp + 6), sizeof(void*));
        putstring("\nTHREAD LR: ");
        puthex((char*) (psp + 5), sizeof(void*));
        putstring("\nTHREAD REGS: r0 ");
        puthex((char*) (psp + 0), sizeof(void*));
        putstring(", r1 ");
        puthex((char*) (psp + 1), sizeof(void*));
        putstring(", r2 ");
        puthex((char*) (psp + 2), sizeof(void*));
        putstring(", r3 ");
        puthex((char*) (psp + 3), sizeof(void*));
        putstring(", r12 ");
        puthex((char*) (psp + 4), sizeof(void*));
    }
    
    putstring("\n");
    
    for(;;);
}

unsigned int * const myvectors[76] __attribute__ ((section("vectors")))= {
    (unsigned int *)    STACK_TOP,
    (unsigned int *)    __Init_Data,
    (unsigned int *)    nmi_handler, // NMI
    (unsigned int *)    hardfault_handler, // HardFault
    (unsigned int *)    0, // MemManage
    (unsigned int *)    0, // BusFault
    (unsigned int *)    0, // UsageFault
    (unsigned int *)    0, // Reserved
    (unsigned int *)    0, // Reserved
    (unsigned int *)    0, // Reserved
    (unsigned int *)    0, // Reserved
    (unsigned int *)    vPortSVCHandler, // System service SWI
    (unsigned int *)    0, // DebugMon
    (unsigned int *)    0, // Reserved
    (unsigned int *)    xPortPendSVHandler, // Pendable request for system service
    (unsigned int *)    xPortSysTickHandler, // System tick timer

	[16 + USART1_IRQn] = (unsigned int *)    io_cli_isr, // 38: USART1
        [16 + USART3_IRQn] = (unsigned int *)    io_gsm_isr, // USART3
//	[16 + USART2_IRQn] = (unsigned int *)    gps_isr, // USART2
	[16 + DMA1_Channel2_IRQn] = (unsigned int*) mmc_dma_isr,
	[16 + DMA1_Channel3_IRQn] = (unsigned int*) mmc_dma_isr,
};