#include <kernel.h>
#include <context-switch.h>
extern int GetAvailability(TD* Task);

void kernelInit(KernelStruct* Colonel) {

	int i;
	TD* temp;
	TDPQ* PQ;
	int FreeBitMask = 1<<31;
	for (i = 0; i < MAX_NUM_TD; i++) {
		temp = &(Colonel->Tasks[i]);
		temp->TaskID = i | FreeBitMask;
		temp->ParentID = 0;
		temp->TaskState = Init;
		temp->TaskPriority = Prio0;

		if (i == MAX_NUM_TD-1) temp->NextFree = 0;
		else temp->NextFree = &(Colonel->Tasks[i+1]);
		temp->NextInPQ = 0;

		temp->sp = (int *)(USER_STACK_TOP - (STACK_SIZE*i));
		temp->RetVal = 22;
		temp->lr = 0;
	}

	for(i = 0; i<MAX_NUM_PRIORITY ;i++){
		PQ = &(Colonel->ArrayPQ[i]);
		PQ->Head = NULL;
		PQ->Tail = NULL;
		PQ->Length = 0;
	}
	Colonel->FirstOfFreeList = &(Colonel->Tasks[0]);
	Colonel->LastOfFreeList = &(Colonel->Tasks[MAX_NUM_TD-1]);
	Colonel->NumTDsAvail = MAX_NUM_TD;
	Colonel->Active = 0;
}

void InitSwi(){
	asm volatile(
			"mov r4, #0x28\n\t"
			"ldr r5, =kerent\n\t"
			"add r5, r5, #0x218000\n\t"
			"str r5, [r4]\n\t"
	);
}


TD* GetFreeTD(KernelStruct* Colonel){
	TD* temp;
	int i = 0;
	for(;i<MAX_NUM_TD;i++){
		temp = (Colonel->Tasks + i);
		if(GetAvailability(temp)){
			return temp;
		}
	}
	return 0;
}

/*
 * Reference: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0363g/Chdhgibd.html
 */
void EnableCaches() {

	asm volatile (
			"stmfd sp!, {r0, r1}\n\t" // push r0 to save its value
			"mrc p15, 0, r1, c1, c0, 0\n\t" // read SCTLR (system control register) configuration data
			"orr r1, r1, #0x1 <<12\n\t" // instruction cache enable
			"orr r1, r1, #0x1 <<2\n\t"// data cache enable
			"mcr p15, 0, r0, c7, c5, 0\n\t" // invalidate entire instruction cache, r0 main core register
			"mcr p15, 0, r1, c1, c0, 0\n\t"// enabled cache RAMs
			"ldmfd sp!, {r0, r1}\n\t" // pop r0 to restore its value
		     );

}


/*
 * Reference: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0363g/Chdhgibd.html
 */
void DisableCaches() {

	asm volatile (
			"stmfd sp!, {r1}\n\t"         // push r0 to save its value
			"mrc p15, 0, r1, c1, c0, 0\n\t" //Read SCTLR configuration data
			"bic r1, r1, #0x1 <<12\n\t"   // instruction cache disable
			"bic r1, r1, #0x1 <<2\n\t"   // data cache disable
			"mcr p15, 0, r1, c1, c0, 0\n\t"  // disabled cache RAMs
			"ldmfd sp!, {r1}\n\t" // pop r0 to restore its value
		     );
}