/*
 * CORTEXM GDB support
 * arch-specific portion of GDB stub
 * 
 * File      : cortexm_stub.c
 * This file is part of RT-Thread RTOS
 * COPYRIGHT (C) 2006, RT-Thread Develop Team
 *
 * The license and distribution terms for this file may be
 * found in the file LICENSE in this distribution or at
 * http://www.rt-thread.org/license/LICENSE
 *
 * Change Logs:
 * Date           Author       Notes
 * 2014-07-04     Wzyy2      first version
 */
#include <rtthread.h>
#include <rthw.h>
#include <gdb_stub.h>

static Gdb_SavedRegisters *regs;
unsigned long single_step_basepri = 0;

void gdb_remove_all_hw_break();
void gdb_enable_hw_debug();
void gdb_disable_hw_debug();
int gdb_set_hw_break(unsigned long, int, enum gdb_bptype);
int gdb_remove_hw_break(unsigned long, int, enum gdb_bptype);

/*struct gdb_arch - Describe architecture specific values.*/
struct gdb_arch arch_gdb_ops = {
	/* Breakpoint instruction: */
    .gdb_bpt_instr		=  {0xfe, 0xde, 0xff, 0xe7},  //Little-Endian
    .flags			= RT_GDB_HAVE_HWBP,
    .set_hw_breakpoint	= gdb_set_hw_break,
    .remove_hw_breakpoint	= gdb_remove_hw_break,
    .remove_all_hw_break	= gdb_remove_all_hw_break
};

static struct hw_breakpoint {
	int		enabled;
	unsigned long		addr;
} breakinfo[HBP_NUM];

static struct hw_watchpoint {
	int		enabled;
	unsigned long		addr;
    int			len;
	enum gdb_bptype		type;
} watchinfo[HWP_NUM];

//The following table defines the memory areas that GDB is allow to touch
static  const struct {
    unsigned long   start;
    unsigned long   end;
} data_access[] =
{
   { 0x20000000,         0x40000000-1}, // On-chip ram
   { 0x60000000,         0xa0000000-1}, // External ram
   { 0x00000000,         0x20000000-1}, // On-chip flash
   { 0x60000000,         0xa0000000-1}, // External flash
   { 0xE0000000,         0x00000000-1}, // cortex-M peripheral
   { 0x40000000,         0x60000000-1}, // mcu peripheral
};


int gdb_permit_data_access(unsigned long addr, unsigned long count)
{
     unsigned char i;

     for (i = 0; i < sizeof(data_access)/sizeof(data_access[0]); i++) { 
         if ((addr >= data_access[i].start) && (addr + count) <= data_access[i].end) {
             return 0;
         }
     }

     return -1;
}

/*we need to block all pending interrupts by swtting basepri
 * before doing the steo
 */
void gdb_single_step()
{
    volatile unsigned long *base;

    //mask all interrupts
    single_step_basepri = regs->basepri;
    regs->basepri = GDB_CORTEXM_PRIORITY_MAX;

    //When MON_EN = 1, this steps the core     
    base = (unsigned long*)(GDB_DEBUG_REG_BASE + GDB_DEBUG_REG_DEMCR);
    *base |= GDB_DEBUG_REG_DEMCR_MON_STEP;

    /* Clear any bits set in DFSR*/
    base = (unsigned long*)(GDB_NVIC_REG_BASE + GDB_NVIC_REG_DFSR);
    *base = 0xffffffff;

}

void gdb_clear_single_step()
{
    volatile unsigned long *base;

    regs->basepri = single_step_basepri;

    /*clear single step*/
    base = (unsigned long*)(GDB_DEBUG_REG_BASE + GDB_DEBUG_REG_DEMCR);
    *base &= ~GDB_DEBUG_REG_DEMCR_MON_STEP;

    // Clear any bits set in DFSR
    base = (unsigned long*)(GDB_NVIC_REG_BASE + GDB_NVIC_REG_DFSR);
    *base = 0xffffffff;

}

/**
 * gdb_breakpoint - generate a breadk
 * It is used to sync up with a debugger and stop progarm
 */
void gdb_breakpoint()
{
    volatile unsigned long *base;

    // Enable the FPB-FLASH PATCH BREAKPOINT 
    base = (unsigned long*)(GDB_FPB_REG_BASE + GDB_FPB_REG_CTRL);
    *base |= GDB_FPB_REG_CTRL_KEY | GDB_FPB_REG_CTRL_ENABLE ;

    base = (unsigned long*)(GDB_DEBUG_REG_BASE + GDB_DEBUG_REG_DEMCR);
     /* 
      * Enable the debug monitor. When enabled, the System handler priority
      * register controls its priority level.
      * If disabled, then all debug events go
      * to Hard fault
      */
    *base |= GDB_DEBUG_REG_DEMCR_MON_EN;

    // Enable DWT
    *base |= GDB_DEBUG_REG_DEMCR_TRCENA ;

    //Fall into debug monitor
    *base |= GDB_DEBUG_REG_DEMCR_MON_PEND;

}

void gdb_set_register(void *hw_regs)
{
    regs = hw_regs;
}

void gdb_get_register(unsigned long *gdb_regs)
{   
    int regno;
    /* Initialize all to zero. */
    for (regno = 0; regno < GDB_MAX_REGS; regno++)
        gdb_regs[regno] = 0;

    gdb_regs[GDB_R0]		= regs->r0;
    gdb_regs[GDB_R1]		= regs->r1;
    gdb_regs[GDB_R2]		= regs->r2;
    gdb_regs[GDB_R3]		= regs->r3;
    gdb_regs[GDB_R4]		= regs->r4;
    gdb_regs[GDB_R5]		= regs->r5;
    gdb_regs[GDB_R6]		= regs->r6;
    gdb_regs[GDB_R7]		= regs->r7;
    gdb_regs[GDB_R8]		= regs->r8;
    gdb_regs[GDB_R9]		= regs->r9;
    gdb_regs[GDB_R10]		= regs->r10;
    gdb_regs[GDB_FP]		= regs->r11;
    gdb_regs[GDB_IP]	    = regs->r12;
    gdb_regs[GDB_SPT]		= regs->sp;
    gdb_regs[GDB_LR]		= regs->lr;
    gdb_regs[GDB_PC]		= regs->pc;
    gdb_regs[GDB_CPSR]  	= regs->psr;

};


void gdb_put_register(unsigned long *gdb_regs)
{
    regs->r0    	= gdb_regs[GDB_R0];
    regs->r1    	= gdb_regs[GDB_R1];
    regs->r2    	= gdb_regs[GDB_R2];
    regs->r3    	= gdb_regs[GDB_R3];

    regs->r4       	= gdb_regs[GDB_R4];
    regs->r5    	= gdb_regs[GDB_R5];
    regs->r6    	= gdb_regs[GDB_R6];
    regs->r7    	= gdb_regs[GDB_R7];
    regs->r8    	= gdb_regs[GDB_R8];
    regs->r9    	= gdb_regs[GDB_R9];
    regs->r10    	= gdb_regs[GDB_R10];
    regs->r11    	= gdb_regs[GDB_FP];
    regs->r12    	= gdb_regs[GDB_IP];
    regs->sp    	= gdb_regs[GDB_SPT];
    regs->lr	    = gdb_regs[GDB_LR];
    regs->pc        = gdb_regs[GDB_PC];
    regs->psr       = gdb_regs[GDB_CPSR];

}


/* It will be called during process_packet */
int gdb_arch_handle_exception(char *remcom_in_buffer,
                              char *remcom_out_buffer)
{
    unsigned long addr;
    char *ptr;
    static int step = 0;

    if (step){
        gdb_clear_single_step();
        step = 0;
    }

    switch (remcom_in_buffer[0]) {
        case 'D':
        case 'k':
        case 'c':
            /*
             * If this was a compiled breakpoint, we need to move
             * to the next instruction or we will breakpoint
             * over and over again
             */
            ptr = &remcom_in_buffer[1];
            if (gdb_hex2long(&ptr, &addr))
                regs->pc = addr;

            return 0;
        case 's':
            ptr = &remcom_in_buffer[1];
            if (gdb_hex2long(&ptr, &addr))
                regs->pc = addr;

            gdb_single_step();
            step = 1;

            return 0;
    }

	return -1;

}

int gdb_set_hw_break(unsigned long addr, int len, enum gdb_bptype bptype)
{
    int i;

    if (bptype == BP_HARDWARE_BREAKPOINT) {
        for (i = 0; i < HBP_NUM; i++)
            if (!breakinfo[i].enabled)
                break;
        if (i == HBP_NUM)
            return -1;
        breakinfo[i].addr = addr;
        breakinfo[i].enabled = 1;
    }
    else if (bptype == BP_WRITE_WATCHPOINT) {
                for (i = 0; i < HWP_NUM; i++)
            if (!watchinfo[i].enabled)
                break;
        if (i == HWP_NUM)
            return -1;
        watchinfo[i].addr = addr;
        watchinfo[i].len = len;
        watchinfo[i].type = BP_WRITE_WATCHPOINT;	
        watchinfo[i].enabled = 1;
    }
    else if (bptype == BP_READ_WATCHPOINT) {
        for (i = 0; i < HWP_NUM; i++)
            if (!watchinfo[i].enabled)
                break;
        if (i == HWP_NUM)
            return -1;
        watchinfo[i].addr = addr;
        watchinfo[i].len = len;
        watchinfo[i].type = BP_READ_WATCHPOINT;	
        watchinfo[i].enabled = 1;
    }
    else if (bptype == BP_ACCESS_WATCHPOINT) {
                for (i = 0; i < HWP_NUM; i++)
            if (!watchinfo[i].enabled)
                break;
        if (i == HWP_NUM)
            return -1;
        watchinfo[i].addr = addr;
        watchinfo[i].len = len;
        watchinfo[i].type = BP_ACCESS_WATCHPOINT;	
        watchinfo[i].enabled = 1;
    }

    return 0;
}

int gdb_remove_hw_break(unsigned long addr, int len, enum gdb_bptype bptype)
{
    int i;

    if (bptype == BP_HARDWARE_BREAKPOINT) {
        for (i = 0; i < HBP_NUM; i++)
            if (breakinfo[i].addr == addr && breakinfo[i].enabled)
                break;
        if (i == HBP_NUM)
            return -1;
        breakinfo[i].enabled = 0;

    }
    else if (bptype == BP_WRITE_WATCHPOINT) {
        for (i = 0; i < HWP_NUM; i++)
            if (watchinfo[i].addr == addr && watchinfo[i].enabled && watchinfo[i].type == bptype)
                break;
        if (i == HWP_NUM)
            return -1;
        watchinfo[i].enabled = 0;

    }
    else if (bptype == BP_READ_WATCHPOINT) {
        for (i = 0; i < HWP_NUM; i++)
            if (watchinfo[i].addr == addr && watchinfo[i].enabled && watchinfo[i].type == bptype)
                break;
        if (i == HWP_NUM)
            return -1;
        watchinfo[i].enabled = 0;

    }
    else if (bptype == BP_ACCESS_WATCHPOINT) {
        for (i = 0; i < HWP_NUM; i++)
            if (watchinfo[i].addr == addr && watchinfo[i].enabled && watchinfo[i].type == bptype)
                break;
        if (i == HWP_NUM)
            return -1;
        watchinfo[i].enabled = 0;

    }

    return 0;
}

void gdb_remove_all_hw_break()
{
    int i;
    volatile unsigned long *base;

    // Disable hardware break
    for (i = 0; i < HBP_NUM; i++) {
        if (!breakinfo[i].enabled)
            break;
        base = (unsigned long*)(GDB_FPB_REG_BASE + GDB_FPB_REG_COMP + i * 4);
        *base &= ~GDB_FPB_REG_COMP_ENABLE ;  
    }
 
    // Disable watchpoint
    for (i = 0; i < HWP_NUM; i++) {
        if (!watchinfo[i].enabled)
            break;
        base = (unsigned long*)(GDB_DWT_REG_BASE + GDB_DWT_REG_FUNCTION + i * 12);
        *base &= ~GDB_DWT_REG_FUNCTION_FUC ;  
    }

}

void gdb_arch_late()
{
    gdb_remove_all_hw_break();
}

void gdb_arch_exit()
{  
    volatile unsigned long *base;
    char num = 1;
    int i;

    // Install the hardware break
    for (i = 0; i < HBP_NUM; i++) {
        if (breakinfo[i].enabled) {
            base = (unsigned long*)(GDB_FPB_REG_BASE + GDB_FPB_REG_COMP + i * 4);
            
            *base = GDB_FPB_REG_COMP_ADDR & ((unsigned long)(breakinfo[i].addr));

            if (breakinfo[i].addr & 2)
                *base |= (1UL << 31); //set BKPT on upper halfword
            else 
                *base |= (1UL << 30); //set BKPT on lower halfword,

            *base |= GDB_FPB_REG_COMP_ENABLE ;  
        }   
    }

    // Install the watchpoint
    for (i = 0; i < HWP_NUM; i++) {
        if (watchinfo[i].enabled) {
            base = (unsigned long*)(GDB_DWT_REG_BASE + GDB_DWT_REG_COMP + i * 12);
            *base = watchinfo[i].addr;

            base = (unsigned long*)(GDB_DWT_REG_BASE + GDB_DWT_REG_MASK + i * 12);
            while (watchinfo[i].len >> num) {
                num++;
            }
            *base = num - 1; //DWT matching is performed as:(ADDR & (~0 << MASK)) == COMP

            base = (unsigned long*)(GDB_DWT_REG_BASE + GDB_DWT_REG_FUNCTION + i * 12);

            if (watchinfo[i].type == BP_WRITE_WATCHPOINT) 
                *base =  (*base & ~GDB_DWT_REG_FUNCTION_FUC) + 0x05;            
            else if (watchinfo[i].type == BP_READ_WATCHPOINT) 
                *base =  (*base & ~GDB_DWT_REG_FUNCTION_FUC) + 0x06;          
            else if (watchinfo[i].type == BP_ACCESS_WATCHPOINT) 
                *base =  (*base & ~GDB_DWT_REG_FUNCTION_FUC) + 0x07;

        }   
    }

}