/*
 * sysctl.c
 *
 *  Created on: 2014-4-26
 *      Author: Tuowen
 */

#include<msp430g2553.h>
#include"sysctl.h"

syshdlr sysarr[8];

volatile unsigned char syscall=0;

void sysctl_void()
{
	// This is the default routine
	// I don't want some monkey just come and call some random places in the text!!!!!
}

void sysroutine()
{
	if (syscall>0)
	switch ((syscall & (-syscall)))
	{
	case SYS_BIT7:
		syscall=syscall&(~SYS_BIT7);
		sysarr[7]();
		break;
	case SYS_BIT6:
		syscall=syscall&(~SYS_BIT6);
		sysarr[6]();
		break;
	case SYS_BIT5:
		syscall=syscall&(~SYS_BIT5);
		sysarr[5]();
		break;
	case SYS_BIT4:
		syscall=syscall&(~SYS_BIT4);
		sysarr[4]();
		break;
	case SYS_BIT3:
		syscall=syscall&(~SYS_BIT3);
		sysarr[3]();
		break;
	case SYS_BIT2:
		syscall=syscall&(~SYS_BIT2);
		sysarr[2]();
		break;
	case SYS_BIT1:
		syscall=syscall&(~SYS_BIT1);
		sysarr[1]();
		break;
	case SYS_BIT0:
		syscall=syscall&(~SYS_BIT0);
		sysarr[0]();
		break;
	}
	else
		_BIS_SR(CPUOFF);
}

inline int sys_b2i(int bit)
{
	int i=0;
	while (bit>1){bit>>=1;++i;}
	return i;
}

void sysctl_reghdlr(unsigned int bit,syshdlr rot)
{
	bit=sys_b2i(bit);
	sysarr[bit]=rot;
}

void sysctl_rmhdlr(unsigned int bit)
{
	bit=sys_b2i(bit);
	sysarr[bit]=&sysctl_void;
}

void sysctl_init()
{
	unsigned int i;
	for (i=0;i<8;++i)
		sysarr[i]=&sysctl_void;
}