/**
 * crash_handler.c
 *
 * Performs crash reporting services. Functions or interrupt
 * handlers that would result in infinite loops or resets
 * will instead call this.
 * Adapted from CrashCatcher (https://github.com/adamgreen/CrashCatcher)
 * 
 * 
 * Copyright (c) 2023 Bigscreen, Inc.
 */

#include <asf.h>
#include <string.h>
#include <malloc.h>
#include "crash_handler.h"

// Linker script defined regions
// Note: all linker symbols have no value, just
// an address. It's only valid to use them like "&_srelocate", 
// not as "_srelocate". 
extern uint32_t _srelocate; // start of "relocate" or .data section
extern uint32_t _erelocate; // end of .data
extern uint32_t _sbss; // start of zero initialized data .bss section
extern uint32_t _ebss; // end of .bss
extern uint32_t _sstack; // start of .stack
extern uint32_t _estack; // end of .stack
extern uint32_t _end; // end of assigned ram, which is the start of heap
// end of heap has to be retrieved from mallinfo


// Defined already in udc_desc.h
//extern UDC_DESC_STORAGE udc_config_t udc_config; // Used directly in udc.c, can modify its pointers to change USB modes

// Settings for crashdump USB mode
extern UDC_DESC_STORAGE usb_dev_desc_t crashdump_udc_device_desc;
extern UDC_DESC_STORAGE usb_dev_qual_desc_t udc_device_qual;
extern UDC_DESC_STORAGE udc_config_speed_t udc_config_fshs[1];

// Settings for normal USB mode
extern UDC_DESC_STORAGE usb_dev_desc_t udc_device_desc;
extern UDC_DESC_STORAGE udc_config_speed_t udc_config_fs[1];

#define LOOPS_PER_MS	(9830) // Clock is 98.304MHz, loop is 8 nops, 
// 1 cycle per subtract, 1 per conditional branch, 1 per nop = 10 cycles per loop

__attribute__((aligned(8)))
uint32_t crashdump_stack[CRASHDUMP_STACK_SIZE]; // statically allocated stack space for the crashdump application

xTaskHandle* crashed_task;
signed char* crashed_task_name;
Crashdump_Crash_Reason crash_reason = 0; // If not set, it came from a natural hard fault. Not an elevated one.
uint16_t crash_isr;
uint32_t crash_hfsr;
uint32_t crash_cfsr;

Crashdump_AutoStackedRegisters* cd_autoregs;
Crashdump_ManuallyStackedRegisters* cd_manualregs;
Crashdump_FPURegisters cd_floats;
Crashdump_Info cd_info;

__attribute__((aligned(4)))
uint8_t _crashdump_report_data[UDI_HID_REPORT_FEATURE_SIZE];


bool auto_repeat_on;
uint32_t get_memory_address;
uint8_t get_memory_length;
uint8_t auto_repeat_count;

static void crashdump_main(void);
static void exit_to_crashdump_app(void);
static void crashdump_fill_regs_part1(uint32_t* data);
static void crashdump_fill_regs_part2(uint32_t* data);
static inline void disconnect_usb(void);
static inline void reset_all_power(void);
static uint32_t get_next_system_exception(void) ;
static uint32_t get_next_peripheral_exception(void);
static void crashdump_reset(void);

// Can be called if a task is known to have crashed. Usually due to stack overflow.
// The crashdump IRQ should be triggered right after calling this function.
void crashdump_set_crashed_task(void *pxTask, signed char *pcTaskName) {
	crashed_task = pxTask;
	crashed_task_name = pcTaskName;
}

void crashdump_set_crash_reason(Crashdump_Crash_Reason rsn) {
	crash_reason = rsn;
}

void crashdump_set_irqn(uint32_t ipsr) {
	// Save the currently executing ISR from the IPSR
	crash_isr = (uint16_t)(ipsr & (xPSR_ISR_Msk));
}

void crashdump_change_usb_mode(bool cd_mode) {
	
	if(cd_mode) {
		// Change the UDI_config to point to crashdump settings instead of normal ones
		udc_config.confdev_lsfs = &crashdump_udc_device_desc;
		udc_config.conf_lsfs = udc_config_fshs;
		#ifdef USB_DEVICE_HS_SUPPORT
		udc_config.confdev_hs = &crashdump_udc_device_desc;
		udc_config.qualifier = &udc_device_qual;
		udc_config.conf_hs = udc_config_fshs;
		#endif
		
		
		
	} else {
		// Back to normal
		udc_config.confdev_lsfs = &udc_device_desc;
		udc_config.conf_lsfs = udc_config_fs;
		udc_config.conf_bos = NULL;
	}
	
	// And set the feature report received callback
	udi_hid_generic_set_crashdump_mode(cd_mode);
}

static void delay_busy_loop(uint32_t delay_ms) {
	uint32_t total_delay = delay_ms * LOOPS_PER_MS;
	for(uint32_t i = total_delay; i>0; i--){
		__asm volatile(
				"nop\n"
				"nop\n"
				"nop\n"
				"nop\n"
				"nop\n"
				"nop\n"
				"nop\n"
				"nop\n"
				);
	}
}

// We can only send a total of 64 bytes per HID message
// and the response message contains two ID bytes (message
// type and the part number), two blanks for aligning
// to 4 bytes, and then we only have 60 bytes left
// that's 60/4 = 15 registers at a time
// Need to send 16 + 4 + 33 (FPU) registers in total!
// That's 53 regs to dump. 
// We split that into 4 parts

/*************************************************************
//	Data Byte		Part0		Part1		Part2		Part4
//	0-3				header		header		header		header
//	4-7				R0			R15	(PC)	S10			S25
//	8-11			R1			PSR			S11			S26
//	12-15			R2			MSP			S12			S27
//	16-19			R3			PSP			S13			S28
//	20-23			R4			Exptn PSR	S14			S29
//	24-27			R5			S0			S15			S30
//	28-31			R6			S1			S16			S31
//	32-35			R7			S2			S17			FPSCR
//	36-39			R8			S3			S18
//	40-43			R9			S4			S19
//	44-47			R10			S5			S20
//	48-51			R11			S6			S21
//	52-55			R12			S7			S22
//	56-69			R13 (SP)	S8			S23
//	60-63			R14 (LR)	S9			S24
**************************************************************/


static void crashdump_fill_regs_part1(uint32_t* data) {
	data[0] = cd_autoregs->r0;
	data[1] = cd_autoregs->r1;
	data[2] = cd_autoregs->r2;
	data[3] = cd_autoregs->r3;
	data[4] = cd_manualregs->r4;
	data[5] = cd_manualregs->r5;
	data[6] = cd_manualregs->r6;
	data[7] = cd_manualregs->r7;
	data[8] = cd_manualregs->r8;
	data[9] = cd_manualregs->r9;
	data[10] = cd_manualregs->r10;
	data[11] = cd_manualregs->r11;
	data[12] = cd_autoregs->r12;
	data[13] = cd_info.sp;
	data[14] = cd_autoregs->r14_lr;
}

static void crashdump_fill_regs_part2(uint32_t* data) {

	data[0] = cd_autoregs->r15_pc;
	data[1] = cd_autoregs->xPSR;
	data[2] = cd_manualregs->msp;
	data[3] = cd_manualregs->psp;
	data[4] = cd_manualregs->exception_xPSR;
	// Load floats S0 through S9
	// into data[5] through data[14]
	for(uint32_t i = 0; i <= 9; i++) {
		data[i+5] = cd_floats.floats[i];
	}
}

static void crashdump_fill_regs_part3(uint32_t* data) {
	// Load floats S10 through S24
	// into data[0] through data[14]
	for(uint32_t i = 10; i <= 24; i++) {
		data[i-10] = cd_floats.floats[i];
	}
}

static void crashdump_fill_regs_part4(uint32_t* data) {
	// Load floats S25 through S31
	// into data[0] through data[6]
	for(uint32_t i = 25; i <= 31; i++) {
		data[i-25] = cd_floats.floats[i];
	}
	data[7] = cd_floats.fpscr;
}

static inline void disconnect_usb(void) {
	UDP->UDP_TXVC |= UDP_TXVC_TXVDIS; // Disable USB transceiver
	UDP->UDP_TXVC |= UDP_TXVC_PUON; // Detach from bus by removing pullup resistor
	// Disconnect USB Device from I/O
	MATRIX->CCFG_SYSIO |= (CCFG_SYSIO_SYSIO10 | CCFG_SYSIO_SYSIO11);
}

static inline void reset_all_power(void) {
	// Stop the fan from running
	TC0->TC_CHANNEL[1].TC_RB;
	// Change Set on RC match to Clear on RC match
	// Otherwise the output is 100% high, and we don't want that
	TC0->TC_CHANNEL[1].TC_CMR &= ~(TC_CMR_BCPC_Msk);
	TC0->TC_CHANNEL[1].TC_CMR |= TC_CMR_BCPC_CLEAR;
	
	// Disable power supplies
	ioport_set_pin_level(PIN_DDIC_RST, false); // Reset the VXR
	ioport_set_pin_level(PIN_USBCC_EN, HPD_DISABLED); // Disconnect video
	
	// *** Reset OLED panels ***
	// Apply reset first - will keep it asserted as long as power is disabled
	ioport_set_pin_level(PIN_OLED_RESX, false); 
	delay_busy_loop(2); // delay after reset asserted to AVEE off
	ioport_set_pin_level(PIN_EN_VOLEDn, false);
	delay_busy_loop(4); // delay between AVEE off and AVDD off
	ioport_set_pin_level(PIN_EN_VOLEDp, false);
	// *************************
	
	delay_busy_loop(4);
	// ==== BS2 notes ==== 
	// There is no 1v8 enable on BS2, but this is okay to keep. It will just force the FPGA to reset.
	ioport_set_pin_level(PIN_EN_1V8, false);
	ioport_set_pin_level(PIN_EN_1V, false);

}

static inline void disable_free_rtos(void) {
	// Stopping the rtos is as simple as disabling 
	// the systick interrupt
	SysTick->CTRL &= (~SysTick_CTRL_ENABLE_Msk);
}

static void crashdump_failed_return(void) {
	while(1);
}

// The assembly interrupt handler calls this function
// Here, we assign structs to the stacked registers so
// they can be retrieved easily.
// The stacks being pointed to by these structs should not
// change unless we return from this function.
// At the end, we call "exit_to_crashdump_app" which
// artificially creates a return stack so a different function
// (crashdump_main) can run instead of returning.
void crashdump_start(Crashdump_ManuallyStackedRegisters* manualregs) {
	
	// Disable all peripheral IRQs
	NVIC->ICER[0] = 0xFFFFFFFFul;
	NVIC->ICER[1] = 0xFFFFFFFFul;
	// Un-pend anything that we can
	NVIC->ICPR[0] = 0xFFFFFFFFul;
	NVIC->ICPR[1] = 0xFFFFFFFFul;
	
	// Un-pend any of the RTOS interrupts, PendSV and systick
	SCB->ICSR |= (SCB_ICSR_PENDSVCLR_Msk + SCB_ICSR_PENDSTCLR_Msk);
	// Also try to un-pend SVCall
	SCB->SHCSR &= ~(SCB_SHCSR_SVCALLPENDED_Msk);
	
	reset_all_power();
	disable_free_rtos();
	disconnect_usb();
	
	__set_BASEPRI(0); // In case it was set high by FreeRTOS
	
	// Now we can start pulling data off these stacks.
	cd_manualregs = manualregs;
	// Get the calling stack. Depends on the Link Register of the
	// exception.
	// bit 2 of EXC_RETURN tells us if the thread was using PSP
	if((cd_manualregs->exception_r14_lr & (1<<2)) != 0) {
		cd_autoregs = cd_manualregs->psp;
		} else {
		cd_autoregs = cd_manualregs->msp;
	}
	cd_info.sp = (uint32_t)cd_autoregs;
	
	// Go back in the stack to the original calling function's
	// stack pointer at the time of the fault.
	cd_info.sp += 8*4; // 8 registers always pushed to the stack
	if(0 == (cd_manualregs->exception_r14_lr & (1<<4))) {
		// bit 4 of the exception link register is the FPU status
		cd_info.sp += 18*4; // Adds S0-S15, FPSCR, and a single padding word
		
		// Now we can also retrieve the floating point registers.
		// Since FPU was in use, we can copy higher floats first.
		// This will automatically push the lower floats and FPSCR into the 
		// stack if lazy stacking was being used.
		// And if lazy stacking wasn't used, they would have already been
		// pushed into the stack so either way, it's fine!
		Copy_Upper_FPU_Regs(&(cd_floats.floats[16]));
		
		// And we can copy the other ones from the stack
		memcpy(&cd_floats, &(cd_autoregs->float_regs_lo), 16*4); // Copy 16 floating point registers
		cd_floats.fpscr = cd_autoregs->fpscr;
	} else {
		// We will need to copy all of the FPU registers manually
		Copy_All_FPU_Regs(&cd_floats);
	}

	// Finally, check if stack alignment was necessary.
	// Bit 9 of the stacked xPSR contains the alignment status of the active SP
	// when the exception processing begins.
	// In other words, if bit 9 is true then an additional 4-byte space was skipped.
	// So the stack from the previous process begins an additional word back.
	if(cd_manualregs->exception_xPSR & (1<<9)){
		cd_info.sp += 4;
	}
	
	// If crash reason is hard fault, isr number will always be 3. That's set in
	// the processor.
	// Also grab the hard fault reason
	if(crash_reason == CRASH_HARD_FAULT) {
		crash_isr = 3;
	}
	crash_hfsr = SCB->HFSR;
	crash_cfsr = SCB->CFSR;
	
	//unwind_exception_nesting();
	exit_to_crashdump_app();
}

static uint32_t get_next_system_exception(void) {
	if((SCB->SHCSR & SCB_SHCSR_MEMFAULTACT_Msk) != 0) {
		if(__get_IPSR() != (MemoryManagement_IRQn + 16)) {
			return MemoryManagement_IRQn + 16;
		}
	}
	if((SCB->SHCSR & SCB_SHCSR_BUSFAULTACT_Msk) != 0) {
		if(__get_IPSR() != (BusFault_IRQn + 16)) {
			return BusFault_IRQn + 16;
		}
	}
	if((SCB->SHCSR & SCB_SHCSR_USGFAULTACT_Msk) != 0) {
		if(__get_IPSR() != (UsageFault_IRQn + 16)) {
			return UsageFault_IRQn + 16;
		}
	}

	if((SCB->SHCSR & SCB_SHCSR_SVCALLACT_Msk) != 0) {
		if(__get_IPSR() != (SVCall_IRQn + 16)) {
			return SVCall_IRQn + 16;
		}
	}

	if((SCB->SHCSR & SCB_SHCSR_MONITORACT_Msk) != 0) {
		if(__get_IPSR() != (DebugMonitor_IRQn + 16)) {
			return DebugMonitor_IRQn + 16;
		}
	}

	if((SCB->SHCSR & SCB_SHCSR_PENDSVACT_Msk) != 0) {
		if(__get_IPSR() != (PendSV_IRQn + 16)) {
			return PendSV_IRQn + 16;
		}
	}

	if((SCB->SHCSR & SCB_SHCSR_SYSTICKACT_Msk) != 0) {
		if(__get_IPSR() != (SysTick_IRQn + 16)) {
			return SysTick_IRQn + 16;
		}
	}
	
	return 0;
}

static uint32_t get_next_peripheral_exception(void) {
	uint32_t iabr_bit = 0;
	
	while(iabr_bit < PERIPH_COUNT_IRQn) {
		if(iabr_bit < 32) {
			if((NVIC->IABR[0] & (1 << iabr_bit)) != 0) {
				if(__get_IPSR() != (iabr_bit + 16)) {
					return iabr_bit + 16;
				}
			}
		} else {
			if((NVIC->IABR[1] & (1 << (iabr_bit - 32))) != 0) {
				if(__get_IPSR() != (iabr_bit + 16)) {
					return iabr_bit + 16;
				}
			}
		}
		iabr_bit++;
	}
	
	return 0;

}

//__attribute__((optimize("-O3")))
static void exit_to_crashdump_app(void) {
	uint32_t retaddr;
	// On leaving the ISR, will jump to a function address of our choice
	// Set our own stack (just below the already used portion of our 
	// special static stack)
	
	uint32_t* sptr = __get_MSP(); // This stack location will have been filled
	// since Arm Cortex M (armv7m) uses a full descending stack. The position
	// of the stack pointer is filled.
	// Check if we need to align to 8 bytes.
	if(((uint32_t)(sptr)) % 8 != 0) {
		sptr--;
	}
	sptr--; // get to the next empty position
	
	if((FPU->FPCCR & FPU_FPCCR_LSPACT_Msk) != 0) {
		// Lazy stacking is active. Execute an FPU
		// instruction to stack the registers. 
		// This will allow us to skip using EXC_RETURN_HANDLER_FPU
		// or EXC_RETURN_THREAD_MSP_FPU values
		__asm volatile("vmov.32 s0, #1.00");
	}
	
	
	// When returning from a handler to a lower priority handler, we need
	// to have the exception number being returned to in the stacked xPSR.
	// We normally would get that from the stack, but we can't trust our stack.
	// So we can just check what exceptions are active and pick the next one.
	// Keep going until all active handlers have been returned.
	uint32_t next_exception = 0;
	
	// Make sure not to set the same IRQ that we are currently in (check current PSR)
	// as the one you want to return to! It will still show as active if we are in it!
		
	if((SCB->ICSR & SCB_ICSR_RETTOBASE_Msk) == 0) {
		// We need to unwind the active exceptions before jumping to our handler.
		
		next_exception = get_next_system_exception();
		if(next_exception == 0) {
			next_exception = get_next_peripheral_exception();
		}
		retaddr = (uint32_t)(exit_to_crashdump_app)  & 0xFFFFFFFEUL; // we stay in this function
		__asm volatile ("ldr LR, =0xFFFFFFF1UL\n"); // Value of EXC_RETURN_HANDLER
	}
	else {
		retaddr =  (uint32_t)(crashdump_main) & 0xFFFFFFFEUL;
		__asm volatile ("ldr LR, =0xFFFFFFF9UL\n"); // Value of EXC_RETURN_THREAD_MSP 
	}
	
/* Arm Cortex M stack contents after interrupt pre-emption
 * OFFSET	* REG
 ********************
 * 32		* <optional spacing to align bottom of stack at 8 bytes> << -- old stack pointer here
 * 28		* xPSR << -- or old stack pointer could be here. depends on what was going on in the calling function
 * 24		* PC
 * 20		* LR
 * 16		* R12
 * 12		* R3
 * 8		* R2
 * 4		* R1
 * 0		* R0 << -- Stack pointer set here
 */


	//sptr--; // Alignment
	*sptr = (0x01000000ul + next_exception); // initial xPSR, the Thumb bit set and the next exception down (if we are staying in handler mode)
	sptr--;
	*sptr = retaddr; // Address to return to. Thumb bit must not be set in the stacked PC.
	sptr--;
	*sptr = (uint32_t) crashdump_failed_return; // Initial link register. If the crashdump app tries to return, it will go here
	sptr -= 5;	// R12, R3, R2, R1 and R0. 
	
	// We're going to use Main Stack Pointer instead of Process, but why not push both
	// so that nothing happens to the old process stack.
	__set_MSP((uint32_t)sptr);
	__set_PSP((uint32_t)sptr);
	// Set link register
	// Check if we are returning to handler mode or thread mode
	// We can look at the stacked 
	//__asm volatile ("ldr LR, =0xFFFFFFF9UL\n"); // Value of EXC_RETURN_THREAD_MSP 
	
	// Return from interrupt
	__asm volatile("bx lr\n"::);
	

	// jump to our entry point.
	//crashdump_main();
}

void NMI_Handler() {
	// Make sure LR is correct and simply return
	__asm volatile (
		"ldr LR, =0xFFFFFFF9ul"
	);
}

static void crashdump_main(void) {	
	
	delay_busy_loop(100);
	auto_repeat_on = false;
	
	__enable_irq();	
	__enable_fault_irq();
	crashdump_change_usb_mode(true);
	udc_start(); 
	vbus_monitor_start();
	while(1) {
		// only need to check if auto-repeat
		// memory dump is ongoing and service it
		
		if(auto_repeat_on) {
			
			if(auto_repeat_count == 0) {
				auto_repeat_on = false;
			} 
			
			bool send_success = false;
			_crashdump_report_data[0] = CRASH_CMD_GET_MEMORY;
			
			_crashdump_report_data[1] = get_memory_length; // echo the length
			_crashdump_report_data[2] = auto_repeat_count; // number of reports remaining to send
			memcpy(&(_crashdump_report_data[4]), get_memory_address, get_memory_length); // copy memory
			get_memory_address += get_memory_length;
			
			while(!send_success) {
				send_success = udi_hid_generic_send_report_in(_crashdump_report_data);
			}
			if(auto_repeat_count != 0) {
				auto_repeat_count--;
			}
			
		}
	}
	
}



void enable_crashdump(void) {
	//NVIC_EnableIRQ(TC1_IRQn);
	//NVIC_SetPriority(TC1_IRQn, 15); // must be lowest priority
	// so it can return to thread mode when it has finished
}
/**	
 * Crash_ISRHandler
 * This function is used to replace an existing interrupt service
 * routine (ISR) for any unused interrupt vector.
 * The function saves the crash state to be retrieved later, 
 * and initializes the crash USB device by jumping to crash_main
 * after interrupt service is completed.
 */


/*

__attribute__((naked)) void Crash_ISRHandler(void)  {
	
	__disable_irq(); // Now make sure we are not interrupted in this function.
	NVIC->ICER[0] = 0xFFFFFFFFul;
	NVIC->ICER[1] = 0xFFFFFFFFul; // Disable all peripheral interrupts
		
	save_crash_status();
	
	reset_all_power();
	disable_free_rtos();
	disconnect_usb();
	
	exit_to_crashdump_app();
	
	// Reset our stack and re-startup just about everything.

}
*/

static void crashdump_reset(void) {
	
	uint32_t rstc_mode_reg;
	rstc_mode_reg = RSTC->RSTC_MR;
	rstc_mode_reg &= ~(RSTC_MR_ERSTL_Msk | RSTC_MR_KEY_Msk);
	// rstc_mode_reg |= RSTC_MR_ERSTL(0xFF); // longest possible reset time (about 2 seconds)
	rstc_mode_reg |= RSTC_MR_ERSTL(0x07); // About 2^8 / 32768 seconds (7.8ms)
	rstc_mode_reg |= RSTC_MR_KEY_PASSWD;
	RSTC->RSTC_MR = rstc_mode_reg;
	RSTC->RSTC_CR = RSTC_CR_KEY_PASSWD | RSTC_CR_EXTRST; // assert reset pin
}


void usbhid_crashdump_set_feature(uint8_t* report){
	uint32_t start_addr, end_addr;
	
	memset(_crashdump_report_data, 0, UDI_HID_REPORT_FEATURE_SIZE);
	// First byte of the report is our command
	switch(report[0]) {
		case CRASH_CMD_GET_SW_VER:
			// Reply with software version number string
			_crashdump_report_data[0] = CRASH_CMD_GET_SW_VER;
			strcpy(&(_crashdump_report_data[1]), VERSION_STRING);
			break;
		case CRASH_CMD_GET_REASON:
			_crashdump_report_data[0] = CRASH_CMD_GET_REASON;
			_crashdump_report_data[1] = crash_reason;
			//_crashdump_report_data[2] = crash_isr & 0xFF;
			//_crashdump_report_data[3] = (crash_isr & 0xFF00) >> 8;
			
			memcpy(&(_crashdump_report_data[2]), &crash_isr, 2); // also show what interrupt was active during the crash
			memcpy(&(_crashdump_report_data[4]), &crash_hfsr, 4); // Hard fault status register
			memcpy(&(_crashdump_report_data[8]), &crash_cfsr, 4); // Configurable fault status register (bus + MemManage + usage fault registers)
			
			break;
		case CRASH_CMD_GET_PROCESSOR_STATE:
			// Gets the processor's registers and special registers
			// Requires one input parameter, a "part" number from 0 to 3
			// The register dump is too big to fit in a single HID message, so
			// it's split into 4 parts
			// Check up above at the definitions of "crashdump_fill_regs_partN"
			// to see where each register is returned
			_crashdump_report_data[0] = CRASH_CMD_GET_PROCESSOR_STATE;
			_crashdump_report_data[1] = report[1]; // echo the part selection
			_crashdump_report_data[2] = 0; // Memory region number (there's only one region, so this is fixed to zero)
			switch(report[1]) {
				case 0:
					crashdump_fill_regs_part1(&(_crashdump_report_data[4]));
					_crashdump_report_data[3] = 4*15; // This message's length
					break;
				case 1:
					crashdump_fill_regs_part2(&(_crashdump_report_data[4]));
					_crashdump_report_data[3] = 4*15;
					break;
				case 2:
					crashdump_fill_regs_part3(&(_crashdump_report_data[4]));
					_crashdump_report_data[3] = 4*15;
					break;
				case 3:
					crashdump_fill_regs_part4(&(_crashdump_report_data[4]));
					_crashdump_report_data[3] = 4*8; // last part has fewer registers to send
					break;
			}
			break;
			
		case CRASH_CMD_GET_TASK_NAME:
			_crashdump_report_data[0] = CRASH_CMD_GET_TASK_NAME;
			_crashdump_report_data[1] = strnlen(crashed_task_name, 16);
			memcpy(&(_crashdump_report_data[2]), crashed_task_name, _crashdump_report_data[1]);
			break;
		case CRASH_CMD_GET_NUM_MEMORY_REGIONS:
			_crashdump_report_data[0] = CRASH_CMD_GET_NUM_MEMORY_REGIONS;
			_crashdump_report_data[1] = 4; // always fixed to 4 regions: .data, .bss, .stack, and .heap
			break;
		case CRASH_CMD_GET_MEMORY_REGION_INFO:
			// get data region info
			// Input data: just the region's number in byte 1
			
			// Output data:
			// Byte 0: repeat the command (CRASH_CMD_GET_MEMORY_REGION_INFO)
			// Byte 1: repeat the selected region number
			// Byte 2-3: set to zero
			// Byte 4-7: start address of region
			// Byte 8-11: end address of region (non-inclusive, it's the next byte AFTER the region end)
			// Byte 12-n: text name of region. Null terminated string 
			_crashdump_report_data[0] = CRASH_CMD_GET_MEMORY_REGION_INFO;
			_crashdump_report_data[1] = report[1];
			
			if(report[1] == 0) {
				// get .data region
				start_addr = &_srelocate; 
				end_addr = &_erelocate;
				strcpy(&(_crashdump_report_data[12]), "data");
			} else if(report[1] == 1) {
				// get .bss region
				start_addr = &_sbss;
				end_addr = &_ebss;
				strcpy(&(_crashdump_report_data[12]), "bss");
			} else if(report[1] == 2) {
				// get .stack region
				start_addr = &_sstack;
				end_addr = &_estack;
				strcpy(&(_crashdump_report_data[12]), "stack");
			} else if(report[1] == 3) {
				// get heap. 
				// It's a little trickier than the others since the end isn't defined
				// by the linker script
				start_addr = &_end;
				struct mallinfo m = mallinfo();
				// allocated space is in "arena"
				end_addr = start_addr + m.arena;
				strcpy(&(_crashdump_report_data[12]), "heap");
			}
			memcpy(&(_crashdump_report_data[4]), &start_addr, 4);
			memcpy(&(_crashdump_report_data[8]), &end_addr, 4);
			break;
		case CRASH_CMD_GET_MEMORY:
			// Input: 
			// Byte 1 - length requested (from 1 to 60 bytes)
			// Byte 2 - auto-repeat number. 0 to 255. 
			//		if set to anything other than 0, then this 
			//		command will send additional HID reports
			//		as quickly as it can, incrementing the 
			//		memory address by the length (byte 1) value
			//		each time.
			//		For example, if you request len=24, auto-repeat=6,
			//		then it will send:
			//			1st report: bytes 0-23
			//			2nd report: bytes 24-47
			//			3rd report: bytes 48-71
			//			4th report: bytes 72-95
			//			5th report: bytes 96-119
			//			6th report: bytes 120-143
			// Byte 4-7: starting address of request
			get_memory_address = (uint32_t)report[4] + ((uint32_t)report[5] << 8) + ((uint32_t)report[6] << 16) + ((uint32_t)report[7] << 24);
			//
			// returns all data in bytes 4-63 (or up to as much was requested)
			// 
			_crashdump_report_data[0] = CRASH_CMD_GET_MEMORY;
			_crashdump_report_data[1] = report[1]; // echo the length
			_crashdump_report_data[2] = report[2]; // echo the auto-repeat count (this is number of packets remaining)
			memcpy(&(_crashdump_report_data[4]), get_memory_address, report[1]);
			
			if(report[2] != 0) {
				get_memory_length = report[1];
				auto_repeat_count = report[2] - 1;
				get_memory_address += get_memory_length;
				auto_repeat_on = true;
			}
			
			break;
		case CRASH_CMD_RESTART:
			crashdump_reset();
			break;
		case CRASH_CMD_RESTART_IN_BOOTLOADER:
			GPBR->SYS_GPBR[0] = 0xBB;
			GPBR->SYS_GPBR[1] = 0x1A;
			crashdump_reset();
			break;
	}

	udi_hid_generic_send_report_in(_crashdump_report_data);
	
}