/*
 * Some routines for multi-core kernel operations.
 */

#include "u_kernel.h"

extern volatile kernelTable *KTBL;

unsigned int getCurrentCoreID()
{
    /* Query the USER1 bits in the PVR to determine the current core's ID. */
    pvr_t pvr_data;
    microblaze_get_pvr(&pvr_data);
    unsigned int core_id = (unsigned int) (pvr_data.pvr[0] & 0xFF);
    return core_id;
}

void enableCaches()
{
#if XPAR_MICROBLAZE_USE_ICACHE
	microblaze_enable_icache();
#endif
#if XPAR_MICROBLAZE_USE_DCACHE
	microblaze_enable_dcache();
#endif
}

void disableCaches()
{
#if XPAR_MICROBLAZE_USE_ICACHE
	microblaze_disable_icache();
#endif
#if XPAR_MICROBLAZE_USE_DCACHE
	microblaze_disable_dcache();
#endif
}

int mutex_lock(int mutex_num)
{
	unsigned int core_id = getCurrentCoreID();
    int LockPattern = ((core_id() << 1) | 1);

    volatile int value=1;

    while(1)
    {
    	if(core_id == 0 || core_id == 3){
    		putfsl(0, 2);//function = unlock
			putfsl(mutex_num, 2);//mutex number
			putfsl(LockPattern, 2);//pattern
			getfsl(value, 0);
		}
    	else{
			putfsl(0, 4);//function = unlock
			putfsl(mutex_num, 4);//mutex number
			putfsl(LockPattern, 4);//pattern
			getfsl(value, 0);
    	}
        if (value == LockPattern)
        {
            break;
        }
    }
    return value;
}


int mutex_unlock(int mutex_num)
{
	unsigned int core_id = getCurrentCoreID();
    int LockPattern = ((core_id() << 1) | 1);
    volatile int value = 1;

    while(1)
    {
    	if(core_id == 0 || core_id == 3){
    		putfsl(1, 2);//function = unlock
			putfsl(mutex_num, 2);//mutex number
			putfsl(LockPattern, 2);//pattern
			getfsl(value, 0);
		}
    	else{
			putfsl(1, 4);//function = unlock
			putfsl(mutex_num, 4);//mutex number
			putfsl(LockPattern, 4);//pattern
			getfsl(value, 0);
    	}
        if (value == 0)
        {
            break;
        }
    }
    return (value == 0);
}

int mutex_lock_id(int mutex_num, int task_id)
{
	unsigned int core_id = getCurrentCoreID();
    int LockPattern = ((task_id << 1) | 1);
    volatile int value = 1;

    while(1)
    {
    	if(core_id == 0 || core_id == 3){
    		putfsl(0, 2);//function = unlock
			putfsl(mutex_num, 2);//mutex number
			putfsl(LockPattern, 2);//pattern
			getfsl(value, 0);
		}
    	else{
			putfsl(0, 4);//function = unlock
			putfsl(mutex_num, 4);//mutex number
			putfsl(LockPattern, 4);//pattern
			getfsl(value, 0);
    	}
        if (value == LockPattern)
        {
            break;
        }
    }
    return value;
}

int mutex_unlock_id(int mutex_num, int task_id)
{
	unsigned int core_id = getCurrentCoreID();
    int LockPattern = ((task_id << 1) | 1);
    volatile int value = 1;

    while(1)
    {
    	if(core_id == 0 || core_id == 3){
    		putfsl(1, 2);//function = unlock
			putfsl(mutex_num, 2);//mutex number
			putfsl(LockPattern, 2);//pattern
			getfsl(value, 0);
		}
    	else{
			putfsl(1, 4);//function = unlock
			putfsl(mutex_num, 4);//mutex number
			putfsl(LockPattern, 4);//pattern
			getfsl(value, 0);
    	}
        if (value == 0)
        {
            break;
        }
    }
    return (value == 0);
}



void producer_write_0(int n)
{
	int value;
	putfsl(1, 3);//write
	putfsl(n, 3);
	getfsl(value, 1);//no use
	return;
}

void producer_write_1(int n)
{
	int value;
	putfsl(1, 6);//write
	putfsl(n, 6);
	getfsl(value, 2);//no use
	return;
}

void producer_write_2(int n)
{
	int value;
	putfsl(1, 6);//write
	putfsl(n, 6);
	getfsl(value, 2);//no use
	return;
}

int producer_read_0(void)
{
	int value;
	putfsl(0, 3);//read
	putfsl(0, 3);//no use
	getfsl(value, 1);
	return value;
}

int producer_read_1(void)
{
	int value;
	putfsl(0, 6);//read
	putfsl(0, 6);//no use
	getfsl(value, 2);
	return value;
}

int producer_read_2(void)
{
	int value;
	putfsl(0, 6);//read
	putfsl(0, 6);//no use
	getfsl(value, 2);
	return value;
}



void consumer_write_1(int n)
{
	int value;
	putfsl(1, 5);//write
	putfsl(n, 5);
	getfsl(value, 1);//no use
	return;
}

void consumer_write_2(int n)
{
	int value;
	putfsl(1, 5);//write
	putfsl(n, 5);
	getfsl(value, 1);//no use
	return;
}

void consumer_write_3(int n)
{
	int value;
	putfsl(1, 3);//write
	putfsl(n, 3);
	getfsl(value, 1);//no use
	return;
}

int consumer_read_1(void)
{
	int value;
	putfsl(0, 5);//read
	putfsl(0, 5);//no use
	getfsl(value, 1);
	return value;
}

int consumer_read_2(void)
{
	int value;
	putfsl(0, 5);//read
	putfsl(0, 5);//no use
	getfsl(value, 1);
	return value;
}

int consumer_read_3(void)
{
	int value;
	putfsl(0, 3);//read
	putfsl(0, 3);//no use
	getfsl(value, 1);
	return value;
}



void *smp_malloc(size_t xWantedSize)
{
    void *pvReturn;

    mutex_lock(KENEL_MEM_MUTEX_NUM);
    pvReturn = malloc(xWantedSize);
    mutex_unlock(KENEL_MEM_MUTEX_NUM);

    return pvReturn;
}

void smp_free(void *pv)
{
    if (pv)
    {
        mutex_lock(KENEL_MEM_MUTEX_NUM);
        free(pv);
        mutex_unlock(KENEL_MEM_MUTEX_NUM);
    }
}

void smp_sleep(volatile long count)
{
	int *local_count;
	local_count = 0x0000EE50L;
	*local_count = 0;
	while((*local_count) < count)
		(*local_count)++;

}


