/*
 * This is a simple program to test multi-core microblaze SoC
 */
#include "u_kernel.h"

#include "AVC/DecTest.h"


#define APP_MUTEX_NUM 10
#define STACK_SIZE 4096

volatile kernelTable *KTBL = (kernelTable *) (KERNEL_TABLE_ADDR);
volatile int *main_stack[NUM_CORES];
volatile void *AppThreadPtr[NUM_CORES];


void startApplicationThreads(void);
void stack_init(void);
void core_1_thread(void *parameter);
void core_2_thread(void *parameter);
void core_3_thread(void *parameter);


volatile int* control_reg = (int*) (0xFFF8);
volatile int* read_reg = (int*) (0xFFF8);

int main()
{
    unsigned int core_id = getCurrentCoreID();

    enableCaches();
	microblaze_flush_dcache();


    xil_printf("Core %d: Hello World!\r\n", core_id);

    AppThreadPtr[1] = core_1_thread;
    AppThreadPtr[2] = core_2_thread;
    AppThreadPtr[3] = core_3_thread;

    /* lock other core */
    mutex_lock(INITIAL_MUTEX);

    startApplicationThreads();

    /* application part */

    int initState = initAVCDecoder();
    if(initState){
    	mutex_lock(APP_MUTEX_NUM);
    	printf("Init error\n");
    	mutex_unlock(APP_MUTEX_NUM);
    }
	int prodState = producer();
	if(prodState){
		mutex_lock(APP_MUTEX_NUM);
		printf("Decode error\n");
		mutex_unlock(APP_MUTEX_NUM);
	}
	mutex_lock(APP_MUTEX_NUM);
		printf("app over\n");
	mutex_unlock(APP_MUTEX_NUM);


    /* end */

    disableCaches();


    return 0;
}


void core_1_thread(void *parameter)
{

    unsigned int core_id = getCurrentCoreID();

    while(1){


    	mutex_lock( INITIAL_MUTEX );
    	mutex_unlock(INITIAL_MUTEX);

    	int state = consumer();
    	if(state){
    		mutex_lock(APP_MUTEX_NUM);
    		printf("core : %d error\n",core_id);
    		mutex_unlock(APP_MUTEX_NUM);

    	}
    	break;

    }

    while (1) { /* thread never returns */ };
}


void core_2_thread(void *parameter)
{
    unsigned int core_id = getCurrentCoreID();

    while(1){

    	/* initialize local memory address */
    	int i;
    	for(i=0;i<BUF_DEPTH;i++){
    		mutex_lock(KENEL_MEM_MUTEX_NUM);
    		localMemAddr_fir[i] = (int*)lm_alloc(0x960);
    		mutex_unlock(KENEL_MEM_MUTEX_NUM);
    	}
    	volatile int *pbr_bufferInit1;
    	pbr_bufferInit1 = (void*)0x84410C10L;
    	*pbr_bufferInit1 = 0;

    	mutex_lock( INITIAL_MUTEX );
    	mutex_unlock(INITIAL_MUTEX);

	    int state = consumer_mc();
	    if(state){
	    	mutex_lock(APP_MUTEX_NUM);
	    	printf("core : %d error\n",core_id);
	    	mutex_unlock(APP_MUTEX_NUM);
	    }
	    break;

    }


    while (1) { /* thread never returns */ };
}


void core_3_thread(void *parameter)
{
    unsigned int core_id = getCurrentCoreID();

    while(1){

    	/* initialize local memory address */
    	int i;
    	for(i=0;i<BUF_DEPTH;i++){
    		mutex_lock(KENEL_MEM_MUTEX_NUM);
    		localMemAddr_sec[i] = (int*)lm_alloc(0x960);
    		mutex_unlock(KENEL_MEM_MUTEX_NUM);
    	}

    	mutex_lock( INITIAL_MUTEX );
    	mutex_unlock(INITIAL_MUTEX);

    	volatile int *pbr_bufferInit2;
    	pbr_bufferInit2 = 0x84410C14L;
    	*pbr_bufferInit2 = 0;

		int state = consumer_deblock();
		if(state){
			mutex_lock(APP_MUTEX_NUM);
			printf("core : %d error\n",core_id);
			mutex_unlock(APP_MUTEX_NUM);
		}
		break;
    }



    while (1) { /* thread never returns */ };
}

void startApplicationThreads(void)
{
    /* Tell the slave cores to start running its thread */
    KTBL->applicationThreadPtr[1] = core_1_thread;
    KTBL->applicationThreadPtr[2] = core_2_thread;
    KTBL->applicationThreadPtr[3] = core_3_thread;
    KTBL->bootSync = MASTER_CPU_SYNC_MAGIC_CODE;
}
