
#include "reg.h"
#include <math.h>
#include "BUS.h"

unsigned reg::buildMask(unsigned s)
{
	unsigned msk = 0;
	for(unsigned i=0; i<s; i++)
	{
		msk = (msk << 1) | 1;
	}
	return msk;
}

unsigned reg::readField(unsigned msb, unsigned lsb)
{
	return (slaveVal >> (lsb-1)) & buildMask((msb-lsb)+1);
}

void reg::writeField(unsigned msb, unsigned lsb, unsigned v)
{
	load = true;
	unsigned fmask = buildMask((msb-lsb)+1) << (lsb-1);
	v = (v << (lsb-1)) & fmask;
	masterVal = (masterVal &(~fmask)) | v;
}

void reg::writeShift(unsigned in, unsigned* ib)
{
	load = true;
	unsigned out = masterVal;

		// iterate through each bit of the output word, copying in bits from the input
		// word and transposing bit position according to the specification (ib)
	for(unsigned i=0; i<16; i++)
	{
		if(ib[i] == BX) continue; // BX is 'don't care', so leave it alone

			// zero the output bit at 'ob', where ob specifies a bit
			// position (numbered 16-1, where 1 is lsb)
		unsigned ob = 16-i;
		unsigned obmask = 1 << (ob - 1); // create mask for output bit
		out &= ~obmask;

		if(ib[i] == D0) continue; // D0 is 'force the bit to zero'
		
			// copy input bit ib[i] to output bit 'ob', where ib and ob
			// specify bit positions (numbered 16-1, where 1 is lsb)
		unsigned ibmask = 1 << (ib[i] - 1); // create mask for input bit
		unsigned inbit = in & ibmask;
		
		int shift = ib[i]-ob;
		if(shift<0)
			inbit = inbit << abs(shift);
		else if(shift > 0)
			inbit = inbit >> shift;
		out |= inbit;
	}
	masterVal = out;
}

unsigned reg::shiftData(unsigned out, unsigned in, unsigned* ib)
{
		// iterate through each bit of the output word, copying in bits from the input
		// word and transposing bit position according to the specification (ib)
	for(unsigned i=0; i<16; i++)
	{
		if(ib[i] == BX) continue; // BX is 'don't care', so leave it alone

			// zero the output bit at 'ob', where ob specifies a bit
			// position (numbered 16-1, where 1 is lsb)
		unsigned ob = 16-i;
		unsigned obmask = 1 << (ob - 1); // create mask for output bit
		out &= ~obmask;

		if(ib[i] == D0) continue; // D0 is 'force the bit to zero'

			// copy input bit ib[i] to output bit 'ob', where ib and ob
			// specify bit positions (numbered 16-1, where 1 is lsb)
		unsigned ibmask = 1 << (ib[i] - 1); // create mask for input bit
		unsigned inbit = in & ibmask;
		
		int shift = ib[i]-ob;
		if(shift<0)
			inbit = inbit << abs(shift);
		else if(shift > 0)
			inbit = inbit >> shift;
		out |= inbit;
	}
	return out;
}
