/****************************************************************************/
/* Copyright 2016 MBARI														*/
/****************************************************************************/
/* Summary  : Exception and Assertion Routines for OASIS5 on PIC32MX470F512L*/
/* Filename : exceptn.c														*/
/* Author   : Robert Herlien (rah)                                          */
/* Project  : OASIS Mooring Replacement (OASIS5)                            */
/* Revision : 1.0                                                           */
/* Created  : 07/27/2016													*/
/*                                                                          */
/* MBARI provides this documentation and code "as is", with no warranty,    */
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium  */
/* Research Institute to assist in its use, correction, modification, or    */
/* enhancement. This information should not be published or distributed to  */
/* third parties without specific written permission from MBARI.            */
/*                                                                          */
/****************************************************************************/
/* Modification History:                                                    */
/* 27jul2016 rah - created													*/
/****************************************************************************/

/* Kernel includes. */
#include "FreeRTOS.h"
#include "FreeRTOSConfig.h"
#include "task.h"

#include <xc.h>
#include <mbariTypes.h>
#include <oasis.h>
#include <exceptn.h>
#include <picConfig.h>
#include <otask.h>
#include <syslog.h>
#include <o5debug.h>


/************************************************************************/
/* Function    : vApplicationMallocFailedHook							*/
/* Purpose     : Called when malloc() fails and configUSE_MALLOC_FAILED_HOOK is set*/
/* Input       : None                                                   */
/* Outputs     : None                                                   */
/* Comment     : We don't set configuUSE_MALLOC_FAILED_HOOK, so this is unneeded*/
/************************************************************************/
void vApplicationMallocFailedHook(void)
{
}


/************************************************************************/
/* Function    : vApplicationStackOverflowHook							*/
/* Purpose     : Called when task stack overflows						*/
/* Input       : None                                                   */
/* Outputs     : None                                                   */
/* Run time task stack overflow checking is performed if				*/
/*	configCHECK_FOR_STACK_OVERFLOW is defined to >= 1.  This hook		*/
/* function is called if a task stack overflow is detected.				*/
/************************************************************************/
void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName)
{
#ifdef __DEBUG	
	__builtin_software_breakpoint();
#else
	Driver	*dp;

	if (!rtosIsRunning())				/* If still init, just go to diagnostic*/
		o5Diagnostic(0xffff);
	else if ((dp = (Driver *)pvTaskGetThreadLocalStoragePointer(NULL, DrvrP)) != NULL)
		taskKill(NULL);					/* If Oasis driver, just kill ourself*/
	else
		picSoftReset();					/* System task - reset system		*/
#endif
}


// declared static in case exception condition would prevent
// auto variable being created
static enum {
	EXCEP_IRQ = 0,			// interrupt
	EXCEP_AdEL = 4,			// address error exception (load or ifetch)
	EXCEP_AdES,				// address error exception (store)
	EXCEP_IBE,				// bus error (ifetch)
	EXCEP_DBE,				// bus error (load/store)
	EXCEP_Sys,				// syscall
	EXCEP_Bp,				// breakpoint
	EXCEP_RI,				// reserved instruction
	EXCEP_CpU,				// coprocessor unusable
	EXCEP_Overflow,			// arithmetic overflow
	EXCEP_Trap,				// trap (possible divide by zero)
	EXCEP_IS1 = 16,			// implementation specfic 1
	EXCEP_CEU,				// CorExtend Unuseable
	EXCEP_C2E				// coprocessor 2
} _excep_code;

static unsigned int _excep_code;
static unsigned int _excep_addr;


/************************************************************************/
/* Function    : exceptionHandlerCallback								*/
/* Purpose     : Function to run in Timer task to syslog the exception	*/
/* Input       : None                                                   */
/* Outputs     : None                                                   */
/* Comment     : this function overrides the normal _weak_ generic handler*/
/************************************************************************/
static void exceptionCallback(TaskHandle_t td)
{
	Driver	*dp;

	sysLogPrintf("Exception: Task %s addr 0x%x code %u",
				 pcTaskGetName(td), _excep_addr, _excep_code);

	if ((dp = (Driver *)pvTaskGetThreadLocalStoragePointer(td, DrvrP)) != NULL)
		taskKill(td);					/* If Oasis driver, just kill it*/
	else
		picSoftReset();					/* System task - reset system	*/
}


/************************************************************************/
/* Function    : _general_exception_handler								*/
/* Purpose     : General Exception Handler								*/
/* Input       : None                                                   */
/* Outputs     : None                                                   */
/* Comment     : this function overrides the normal _weak_ generic handler*/
/************************************************************************/
void _general_exception_handler(void)
{
	asm volatile("mfc0 %0,$13" : "=r" (_excep_code));
	asm volatile("mfc0 %0,$14" : "=r" (_excep_addr));

	_excep_code = (_excep_code & 0x0000007C) >> 2;

	sysLogPrintf("Exception at addr 0x%x code %u",
				 _excep_addr, _excep_code);

#ifdef __DEBUG	
	__builtin_software_breakpoint();
#else
//	xTimerPendFunctionCallFromISR(exceptionCallback,
//								  xTaskGetCurrentTaskHandle(), 0, NULL);

//	vTaskSuspend(NULL);

//	if (!rtosIsRunning())				/* If still init, just go to diagnostic*/
//		o5Diagnostic(0xffff);
//	else if (pvTaskGetThreadLocalStoragePointer(NULL, DrvrP) == NULL)
		picSoftReset();					/* System task - reset system	*/
#endif
}

/************************************************************************/
/* Function    : vAssertCalled											*/
/* Purpose     : Called on Assertion Failure							*/
/* Input       : None                                                   */
/* Outputs     : None                                                   */
/* Note - when OASIS application is fully debugged, #undef configASSERT()*/
/* in FreeRTOSConfig.h  See http://www.freertos.org/a00110.html#configASSERT*/
/************************************************************************/
void vAssertCalled(const char * pcFile, unsigned long ulLine)
{
    volatile unsigned long ul = 0;

    ( void ) pcFile;
    ( void ) ulLine;

#ifdef __DEBUG	

	__builtin_software_breakpoint();
#else
	Driver	*dp;

	if (!rtosIsRunning())				/* If still init, just go to diagnostic*/
		o5Diagnostic(0xffff);
	else
	{
		sysLogPrintf("Assertion failure: File %s line %u", pcFile, ulLine);

		if ((dp = (Driver *)pvTaskGetThreadLocalStoragePointer(NULL, DrvrP)) != NULL)
			taskKill(NULL);				/* If Oasis driver, just kill ourself*/
		else
			picSoftReset();				/* System task - reset system		*/
	}
#endif	
}