use uint for state and context variables

/*  Copyright 2014 Aaron Boxer

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>. */

#include "ocl_platform.cl"

/*

Bit Plane Coder
===============

A. Scan Pattern

Xx4 stripes

The stripes are scanned from top to bottom, and the columns within a
stripe are scanned from left to right.

*/

#define CODEBLOCKY_QUARTER 8

#define BOUNDARY 1
#define BOUNDARY_X2 2

#define CODEBLOCKY_X4 128

//////////////////////////////////////////////////////
// State Buffer

#define STATE_BUFFER_SIZE 1156
#define STATE_BOTTOM_BOUNDARY_OFFSET 1124

#define STATE_BUFFER_STRIDE 34
#define STATE_BUFFER_STRIDE_X2 68
#define STATE_BUFFER_STRIDE_X3 102
#define STATE_BUFFER_STRIDE_X4 136

#define LEFT_TOP -35
#define TOP  -34
#define RIGHT_TOP -33

#define LEFT   -1
#define RIGHT   1

#define LEFT_BOTTOM  33
#define BOTTOM  34
#define RIGHT_BOTTOM  35

////////////////////////////////////////////////////
// State Variables

// bit positions (0 based indices)
#define INPUT_SIGN_BITPOS  15

#define SIGMA_NEW_BITPOS   0x1
#define SIGMA_OLD_BITPOS   0x1
#define NBH_BITPOS         0x2
#define SIGMA_OLD_TO_NBH_SHIFT 0x1
#define RLC_BITPOS         0x4
#define PIXEL_START_BITPOS 0xA
#define PIXEL_END_BITPOS   0x18
#define SIGN_BITPOS        0x19

// bit flags
#define SIGMA_NEW_F			 0x1		  //position 0
#define SIGMA_OLD_F			 0x10		  //position  1
#define NBH_F				 0x20		  //position  2
#define RLC_F				 0x80		  //position  4
#define RLC_D_POSITION_F	 0x380        //positions 6-9
#define PIXEL_F				 0x7FFF0000   //positions 10-24
#define SIGN_F				 0x2000000    //position  25

#define NOT_SIGMA_NEW_F      0xFFFFFFFE   // ~SIGMA_NEW
#define SIGMA_OLD_AND_NEW_F  0x11

#define INPUT_TO_SIGN_SHIFT 10

#define BIT(pix) (((pix)>>(bp))&1)
#define NBH(pix) ((pix) & NBH_F)
#define SIGMA_OLD(pix) ((pix) & SIGMA_OLD_F)
#define SIGMA_NEW(pix) ((pix) & SIGMA_NEW_F)
#define SIGMA_OLD_AND_NEW(pix) ((pix) & SIGMA_OLD_AND_NEW_F)
#define SIGN(pix) ((pix) & SIGN_F)

#define SET_SIGMA_NEW(pix) ( (pix) |= SIGMA_NEW_F )
#define CLEAR_SIGMA_NEW(pix) ( (pix) &= NOT_SIGMA_NEW_F )

/////////////////////////////////////
// Context Variables

/*
0-2 (CX,D) pairs counter
3		SPP flag
4		MRP flag
5		SPP flag
8		D4
9-13	CX4
14		D3
15-19	CX3
20		D2
21-25	CX2
26		D1
27-31	CX1

*/

/////////////////////////////////////////////////////

// TABLES
/*

// ZERO CODING /////////////////////////////////////

// LL and LH sub-bands
values					Context
sumH	sumV	sumD	CX
2		x		x		8
1 		1		x		7
1		0 		1		6
1		0		0		5
0		2		x		4
0		1		x		3
0		0 		2		2
0		0		1		1
0		0		0		0

//HH sub-band

values					Context
sum(H + V )		sumD	CX
2 				3		8
1				2		7
1				2		6
1				1		5
0				1		4
0				1		3
0				0		2
0				0		1
0				0		0

// HL sub-band

 values Context
sumH	sumV	sumD	CX
x		2		x		8
1		1		x		7
0		1 		1		6
0		1		0		5
2		0		x		4
1		0		x		3
0		0 		2		2
0		0		1		1
0		0		0		0

// SIGN CODING ///////////////////////

H	V	X^	 CX
1	1	0	 13
1	0	0	 12
1	-1	0	 11
0	1	0	 10
0	0	0	 9
0	-1	1	 10
-1	1	1	 11
-1	0	1	 12
-1 -1	1	 13

// Magnitude Refinement //////////////////

sigm_prime [y; x]	(H + V + D)	CX
1					x			16
0 					1			15
0					0			14

*/

CONSTANT sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE  | CLK_FILTER_NEAREST;

void KERNEL run(read_only image2d_t channel) {

	// state buffer
	LOCAL uint state[STATE_BUFFER_SIZE];
	LOCAL uint cxd[STATE_BUFFER_SIZE];

	///////////////////////////////////////////////////////////////////////////////////
	//1. Calculate MSB

	LOCAL int msbScratch[CODEBLOCKX]; // between -1 and 31 - negative one value indicates that this code block is identically zero

	int maxSigBit;
	if (getLocalId(1) == 0) {

		uint maxVal = 0;
		state[getLocalId(0)] = 0;   //top boundary
		LOCAL uint* statePtr = state + (BOUNDARY + getLocalId(0));
		int2 posIn = (int2)(getGlobalId(0),  (getGlobalId(1) >> 3)*CODEBLOCKY);

		for (int i = 0; i < CODEBLOCKY; ++i) {
			int pixel = read_imagei(channel, sampler, posIn).x;
			uint absPixel = abs(pixel);
			maxVal = max(maxVal, absPixel);
			pixel = (absPixel << PIXEL_START_BITPOS) | SIGN((pixel << INPUT_TO_SIGN_SHIFT));
			statePtr[0] = pixel;

			posIn.y++;
			statePtr += STATE_BUFFER_STRIDE;
		}
		state[ getLocalId(0) + STATE_BOTTOM_BOUNDARY_OFFSET] = 0;		//bottom boundary

		//initialize full left and right boundary columns
		if (getLocalId(0) == 0 || getLocalId(0) == CODEBLOCKX-1) {
			int delta = -1 + ((getLocalId(0)/(CODEBLOCKX-1)) << 1); // -1 or +1
			statePtr = state + BOUNDARY + getLocalId(0) + delta;
			for (int i = 0; i < CODEBLOCKY+ BOUNDARY_X2; ++i) {
				 *statePtr = 0;
				 statePtr += STATE_BUFFER_STRIDE;
			}
		}

		// calculate column-msb
		maxSigBit = 31 - clz(maxVal);
		msbScratch[getLocalId(0)] =maxSigBit;
	}
	localMemoryFence();

	//calculate global msb
	if (!getLocalId(0) && !getLocalId(1) ) {
		int4 mx = (int4)(maxSigBit);

		LOCAL int* scratchPtr = msbScratch;
		for(int i=0; i < CODEBLOCKX; i+=4) {
			mx = max(mx,(int4)(msbScratch[i],msbScratch[i+1],msbScratch[i+2],msbScratch[i+3]));
		}
		maxSigBit = mx.x;
		maxSigBit = max(maxSigBit, mx.y);
		maxSigBit = max(maxSigBit, mx.z);
		maxSigBit = max(maxSigBit, mx.w);
		msbScratch[0] = maxSigBit;
	}
	localMemoryFence();

	if (msbScratch[0] == -1)
		return;

	int startIndex = (BOUNDARY + getLocalId(0)) + (BOUNDARY + getLocalId(1))*STATE_BUFFER_STRIDE_X4;

	////////////////////////////////////////////////////////////////////////////////////////////////
	// 2. CUP on MSB

	uint bp = msbScratch[0] + PIXEL_START_BITPOS;

	// i) set sigma_new for stripe column (sigma_old is zero)
	LOCAL uint* statePtr = state + startIndex;
	for (int i = 0; i < 4; ++i) {
		int current = statePtr[0];
		current |= BIT(current);
		*statePtr = current;
		statePtr += STATE_BUFFER_STRIDE;

	}
	localMemoryFence();

	// ii) set nbh for stripe column, and do CUP
	statePtr = state + startIndex;
	uint current			= statePtr[0];
	uint top				= statePtr[TOP];
	uint leftTop			= statePtr[LEFT_TOP];
	uint left			= statePtr[LEFT];
	uint leftBottom		= statePtr[LEFT_BOTTOM];

	int rlcCount = 0;

	// first pixel in stripe column
	uint nbh =  SIGMA_NEW(top) | SIGMA_NEW(leftTop) | SIGMA_NEW(left) | SIGMA_NEW(leftBottom);
	bool doRLC = !nbh && !BIT(current);
	if (doRLC) {
		rlcCount++;
	} else {
		//ZC

		if (BIT(current)) {
		    //SC
		}
	}

	// next two pixels in stripe column
	for (int i = 0; i < 2; ++i) {

		statePtr += STATE_BUFFER_STRIDE;
		top = current;
		current		= statePtr[0];
		leftTop		= left;
		left		= leftBottom;
		leftBottom	= statePtr[LEFT_BOTTOM];

		nbh =  SIGMA_NEW(top) | SIGMA_NEW(leftTop) | SIGMA_NEW(left) | SIGMA_NEW(leftBottom);
		doRLC = doRLC && !nbh && !BIT(current);
		if (doRLC) {

			rlcCount++;
		} else {
		   //ZC

		   if (BIT(current)) {
		       //SC
		   }
		}
	}

	// last pixel in stripe column -
	// ignore leftBottom pixel, because it is in the next
	// stripe and it hasn't been processed yet
	statePtr += STATE_BUFFER_STRIDE;
	top			= current;
	current		= statePtr[0];
	leftTop		= left;
	left		= leftBottom;

	nbh =  SIGMA_NEW(top) | SIGMA_NEW(leftTop) | SIGMA_NEW(left);

	doRLC = doRLC && !nbh && !BIT(current);
	if (doRLC) {

		rlcCount++;
	} else {
		//ZC

		if (BIT(current)) {
		    //SC
		}
	}

	localMemoryFence();

	/////////////////////////////////////////////////////////////////////////////////////////////////
	// 3. Processs rest of bit planes
	bp--;
	LOCAL char blockVote;

	while (bp >= PIXEL_START_BITPOS) {
		blockVote = 0;

		/////////////////////////////
		// 4. pre-process bit plane

		// i) migrate sigma_new to sigma_old, and clear sigma new bit
		statePtr = state + startIndex;
		for (int i = 0; i < 4; ++i) {
		    int current = *statePtr;
			current |=  SIGMA_NEW(current) << SIGMA_OLD_BITPOS;
			CLEAR_SIGMA_NEW(current);

			*statePtr = current;
			statePtr += STATE_BUFFER_STRIDE;

		}
		localMemoryFence();

		// ii) update nbh
		statePtr = state + startIndex;

		uint top			= statePtr[TOP];
		uint leftTop		= statePtr[LEFT_TOP];
		uint rightTop	= statePtr[RIGHT_TOP];
		uint left		= statePtr[LEFT];
		uint current		= statePtr[0];
		uint right		= statePtr[RIGHT];
		uint leftBottom  = statePtr[LEFT_BOTTOM];
		uint bottom		= statePtr[BOTTOM];
		uint rightBottom = statePtr[RIGHT_BOTTOM];

		current |=  (   SIGMA_OLD(leftTop) |
						SIGMA_OLD( top) |
						SIGMA_OLD( rightTop) |
						SIGMA_OLD( left) |
						SIGMA_OLD( right) |
						SIGMA_OLD( leftBottom) |
						SIGMA_OLD( bottom) |
						SIGMA_OLD( rightBottom)  ) << SIGMA_OLD_TO_NBH_SHIFT;

		if ( BIT(current) &&
				NBH(current) &&
					!SIGMA_OLD(current) ) {
			SET_SIGMA_NEW(current);
			blockVote = 1;

		}
		*statePtr = current;

		for (int i = 0; i < 3; ++i) {
			statePtr += STATE_BUFFER_STRIDE;

			top			 = current;
			leftTop		 = left;
			rightTop	 = right;
			left		 = leftBottom;
			current		 = statePtr[0];
			right		 = rightBottom;
			leftBottom   = statePtr[LEFT_BOTTOM];
			bottom		 = statePtr[BOTTOM];
			rightBottom  = statePtr[RIGHT_BOTTOM];

			current |=	   ( SIGMA_OLD(leftTop) |
							 SIGMA_OLD( top) |
							 SIGMA_OLD( rightTop) |
							 SIGMA_OLD( left) |
							 SIGMA_OLD( right) |
							 SIGMA_OLD( leftBottom) |
							 SIGMA_OLD( bottom) |
							 SIGMA_OLD( rightBottom)  ) << SIGMA_OLD_TO_NBH_SHIFT;

			if ( BIT(current) &&
					 NBH(current) &&
						 !SIGMA_OLD(current) ) {
			    SET_SIGMA_NEW(current);
				blockVote = 1;
			}
			*statePtr = current;

		}
		localMemoryFence();

		// iii) block vote on sigma new
		while (blockVote)
		{
		    blockVote = 0;
			localMemoryFence();

			statePtr = state + startIndex;

			//first pixel in stripe column
			uint top			= statePtr[TOP];
			uint leftTop		= statePtr[LEFT_TOP];
			uint left			= statePtr[LEFT];
			uint current		= statePtr[0];
			uint leftBottom		= statePtr[LEFT_BOTTOM];

			current  |= (   SIGMA_NEW(leftTop) |
							SIGMA_NEW( top) |
							SIGMA_NEW( left) |
							SIGMA_NEW( leftBottom) ) << NBH_BITPOS;

			if (  BIT(current) &&
				        !SIGMA_OLD(current) &&
						    NBH(current)  &&
							    !SIGMA_NEW(current)) {
				SET_SIGMA_NEW(current);
				blockVote = 1;

			}
			statePtr[0] = current;

			// next two pixels in stripe column
			for (int i = 0; i < 2; ++i) {
				statePtr += STATE_BUFFER_STRIDE;

				top			= current;
				current		= statePtr[0];
				leftTop		= left;
				left		= leftBottom;
				leftBottom  = statePtr[LEFT_BOTTOM];

				current  |= (   SIGMA_NEW(leftTop) |
								SIGMA_NEW( top) |
								SIGMA_NEW( left) |
								SIGMA_NEW( leftBottom) ) << NBH_BITPOS;

				if (  BIT(current) &&
				          !SIGMA_OLD(current) &&
						      NBH(current)  &&
							     !SIGMA_NEW(current)) {
					SET_SIGMA_NEW(current);
					blockVote = 1;

				}
				statePtr[0] = current;
			}

			// last pixel in stripe column -
			// ignore leftBottom pixel, because it is in the next
			// stripe and it hasn't been processed yet
			statePtr += STATE_BUFFER_STRIDE;

			top			= current;
			current		= statePtr[0];
			leftTop		= left;
			left		= leftBottom;

			current |= ( SIGMA_NEW(leftTop) |
						 SIGMA_NEW( top) |
						 SIGMA_NEW( left) ) << NBH_BITPOS;
			if (  BIT(current) &&
			        !SIGMA_OLD(current) &&
					    NBH(current)  &&
						    !SIGMA_NEW(current)) {
				SET_SIGMA_NEW(current);
				blockVote = 1;
			}
			statePtr[0] = current;

			localMemoryFence();

		}

		/////////////////////////
		// 5. bit plane processing
		statePtr = state + startIndex;

		top = statePtr[TOP];
		leftTop			= statePtr[LEFT_TOP];
		rightTop		= statePtr[RIGHT_TOP];
		left			= statePtr[LEFT];
		current			= statePtr[0];
		right			= statePtr[RIGHT];
		leftBottom		= statePtr[LEFT_BOTTOM];
		bottom			= statePtr[BOTTOM];
		rightBottom		= statePtr[RIGHT_BOTTOM];

		doRLC = false;
		rlcCount = 0;
		if (SIGMA_OLD(current)) {
			// MRC
		} else {
			int nbh = ( SIGMA_OLD_AND_NEW(leftTop) |
						SIGMA_OLD_AND_NEW( top) |
						SIGMA_OLD_AND_NEW( rightTop) |
						SIGMA_OLD_AND_NEW( left) |
						SIGMA_OLD_AND_NEW( right) |
						SIGMA_OLD_AND_NEW( leftBottom) |
						SIGMA_OLD_AND_NEW( bottom) |
						SIGMA_OLD_AND_NEW( rightBottom)  ) ;

			if (!nbh) {
				if ( !SIGMA_NEW(current) ) {
					doRLC = !BIT(current);
					if (doRLC) {
					   rlcCount++;
					}
				}
			}
			else  {
				// ZC
				if (BIT(current)) {
				    //SC
				}
			}
		}

		for (int i = 0; i < 3; ++i) {
			statePtr += STATE_BUFFER_STRIDE;

			top			= current;
			leftTop		= left;
			rightTop	= right;
			left		= leftBottom;
			current		= statePtr[0];
			right		= rightBottom;
			leftBottom  = statePtr[LEFT_BOTTOM];
			bottom		= statePtr[BOTTOM];
			rightBottom = statePtr[RIGHT_BOTTOM];

			if (SIGMA_OLD(current)) {
				// MRC
			} else {
				int nbh = ( SIGMA_OLD_AND_NEW(leftTop) |
							SIGMA_OLD_AND_NEW( top) |
							SIGMA_OLD_AND_NEW( rightTop) |
							SIGMA_OLD_AND_NEW( left) |
							SIGMA_OLD_AND_NEW( right) |
							SIGMA_OLD_AND_NEW( leftBottom) |
							SIGMA_OLD_AND_NEW( bottom) |
							SIGMA_OLD_AND_NEW( rightBottom)  ) ;

				if (!nbh) {
					if ( !SIGMA_NEW(current) ) {
						doRLC = doRLC && !BIT(current);
						if (!doRLC) {

							rlcCount++;

						}
					}
				}
				else  {
					// ZC
					if (BIT(current)) {
						//SC
					}
				}
			}
		}

		localMemoryFence();
		bp--;
	}
}

/*  Copyright 2014 Aaron Boxer

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>. */

#include "ocl_platform.cl"

/*

Bit Plane Coder
===============

A. Scan Pattern

Xx4 stripes

The stripes are scanned from top to bottom, and the columns within a
stripe are scanned from left to right.

*/

#define CODEBLOCKY_QUARTER 8

#define BOUNDARY 1
#define BOUNDARY_X2 2

#define CODEBLOCKY_X4 128

#define STATE_BUFFER_SIZE 1156
#define STATE_BOTTOM_BOUNDARY_OFFSET 1124

#define STATE_BUFFER_STRIDE 34
#define STATE_BUFFER_STRIDE_X2 68
#define STATE_BUFFER_STRIDE_X3 102
#define STATE_BUFFER_STRIDE_X4 136

#define LEFT_TOP -35
#define TOP  -34
#define RIGHT_TOP -33

#define LEFT   -1
#define RIGHT   1

#define LEFT_BOTTOM  33
#define BOTTOM  34
#define RIGHT_BOTTOM  35

// bit positions (0 based indices)
#define INPUT_SIGN_BITPOS  15

#define SIGMA_NEW_BITPOS   0x1
#define SIGMA_OLD_BITPOS   0x1
#define NBH_BITPOS         0x2
#define SIGMA_OLD_TO_NBH_SHIFT 0x1
#define RLC_BITPOS         0x4
#define PIXEL_START_BITPOS 0xA
#define PIXEL_END_BITPOS   0x18
#define SIGN_BITPOS        0x19

// bit flags
#define SIGMA_NEW_F			 0x1		  //position 0
#define SIGMA_OLD_F			 0x10		  //position  1
#define NBH_F				 0x20		  //position  2
#define RLC_F				 0x80		  //position  4
#define RLC_D_POSITION_F	 0x380        //positions 6-9
#define PIXEL_F				 0x7FFF0000   //positions 10-24
#define SIGN_F				 0x2000000    //position  25

#define NOT_SIGMA_NEW_F      0xFFFFFFFE   // ~SIGMA_NEW
#define SIGMA_OLD_AND_NEW_F  0x11

#define INPUT_TO_SIGN_SHIFT 10

CONSTANT sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE  | CLK_FILTER_NEAREST;

#define BIT(pix) (((pix)>>(bp))&1)
#define NBH(pix) ((pix) & NBH_F)
#define SIGMA_OLD(pix) ((pix) & SIGMA_OLD_F)
#define SIGMA_NEW(pix) ((pix) & SIGMA_NEW_F)
#define SIGMA_OLD_AND_NEW(pix) ((pix) & SIGMA_OLD_AND_NEW_F)
#define SIGN(pix) ((pix) & SIGN_F)

#define SET_SIGMA_NEW(pix) ( (pix) |= SIGMA_NEW_F )
#define CLEAR_SIGMA_NEW(pix) ( (pix) &= NOT_SIGMA_NEW_F )

/**

**/

void KERNEL run(read_only image2d_t channel) {

	// state buffer
	LOCAL int state[STATE_BUFFER_SIZE];
	LOCAL int cxd[STATE_BUFFER_SIZE];

	///////////////////////////////////////////////////////////////////////////////////
	//1. Calculate MSB

	LOCAL int msbScratch[CODEBLOCKX]; // between -1 and 31 - negative one value indicates that this code block is identically zero

	int maxSigBit;
	if (getLocalId(1) == 0) {

		int maxVal = 0;
		state[getLocalId(0)] = 0;   //top boundary
		LOCAL int* statePtr = state + (BOUNDARY + getLocalId(0));
		int2 posIn = (int2)(getGlobalId(0),  (getGlobalId(1) >> 3)*CODEBLOCKY);

		for (int i = 0; i < CODEBLOCKY; ++i) {
			int pixel = read_imagei(channel, sampler, posIn).x;
			int absPixel = abs(pixel);
			maxVal = max(maxVal, absPixel);
			pixel = (absPixel << PIXEL_START_BITPOS) | SIGN((pixel << INPUT_TO_SIGN_SHIFT));
			statePtr[0] = pixel;

			posIn.y++;
			statePtr += STATE_BUFFER_STRIDE;
		}
		state[ getLocalId(0) + STATE_BOTTOM_BOUNDARY_OFFSET] = 0;		//bottom boundary

		//initialize full left and right boundary columns
		if (getLocalId(0) == 0 || getLocalId(0) == CODEBLOCKX-1) {
			int delta = -1 + (getLocalId(0)/(CODEBLOCKX-1))*2; // -1 or +1
			statePtr = state + BOUNDARY + getLocalId(0) + delta;
			for (int i = 0; i < CODEBLOCKY+ BOUNDARY_X2; ++i) {
				 *statePtr = 0;
				 statePtr += STATE_BUFFER_STRIDE;
			}
		}

		// calculate column-msb
		maxSigBit = 31 - clz(maxVal);
		msbScratch[getLocalId(0)] =maxSigBit;
	}
	localMemoryFence();

	//calculate global msb
	if (!getLocalId(0) && !getLocalId(1) ) {
		int4 mx = (int4)(maxSigBit);

		LOCAL int* scratchPtr = msbScratch;
		for(int i=0; i < CODEBLOCKX; i+=4) {
			mx = max(mx,(int4)(msbScratch[i],msbScratch[i+1],msbScratch[i+2],msbScratch[i+3]));
		}
		maxSigBit = mx.x;
		maxSigBit = max(maxSigBit, mx.y);
		maxSigBit = max(maxSigBit, mx.z);
		maxSigBit = max(maxSigBit, mx.w);
		msbScratch[0] = maxSigBit;
	}
	localMemoryFence();

	if (msbScratch[0] == -1)
		return;

	int startIndex = (BOUNDARY + getLocalId(0)) + (BOUNDARY + getLocalId(1))*STATE_BUFFER_STRIDE_X4;

	////////////////////////////////////////////////////////////////////////////////////////////////
	// 2. CUP on MSB

	int bp = msbScratch[0] + PIXEL_START_BITPOS;

	// i) set sigma_new for strip column (sigma_old is zero)
	LOCAL int* statePtr = state + startIndex;
	for (int i = 0; i < 4; ++i) {
		int current = statePtr[0];
		current |= BIT(current);
		*statePtr = current;
		statePtr += STATE_BUFFER_STRIDE;

	}
	localMemoryFence();

	// ii) set nbh for strip column, and do CUP
	statePtr = state + startIndex;
	int current			= statePtr[0];
	int top				= statePtr[TOP];
	int leftTop			= statePtr[LEFT_TOP];
	int left			= statePtr[LEFT];
	int leftBottom		= statePtr[LEFT_BOTTOM];

	int rlcCount = 0;

	// first pixel in strip column
	int nbh =  SIGMA_NEW(top) | SIGMA_NEW(leftTop) | SIGMA_NEW(left) | SIGMA_NEW(leftBottom);
	bool doRLC = !nbh && !BIT(current);
	if (doRLC) {
		rlcCount++;
	} else {
		//ZC

		if (BIT(current)) {
		    //SC
		}
	}

	// next two pixels in strip column
	for (int i = 0; i < 2; ++i) {

		statePtr += STATE_BUFFER_STRIDE;
		top = current;
		current		= statePtr[0];
		leftTop		= left;
		left		= leftBottom;
		leftBottom	= statePtr[LEFT_BOTTOM];

		nbh =  SIGMA_NEW(top) | SIGMA_NEW(leftTop) | SIGMA_NEW(left) | SIGMA_NEW(leftBottom);
		doRLC = doRLC && !nbh && !BIT(current);
		if (doRLC) {

			rlcCount++;
		} else {
		   //ZC

		   if (BIT(current)) {
		       //SC
		   }
		}
	}

	// last pixel in strip column -
	// ignore leftBottom pixel, because it is in the next
	// stripe and it hasn't been processed yet
	statePtr += STATE_BUFFER_STRIDE;
	top			= current;
	current		= statePtr[0];
	leftTop		= left;
	left		= leftBottom;

	nbh =  SIGMA_NEW(top) | SIGMA_NEW(leftTop) | SIGMA_NEW(left);

	doRLC = doRLC && !nbh && !BIT(current);
	if (doRLC) {

		rlcCount++;
	} else {
		//ZC

		if (BIT(current)) {
		    //SC
		}
	}

	localMemoryFence();

	/////////////////////////////////////////////////////////////////////////////////////////////////
	// 3. Processs rest of bit planes
	bp--;
	LOCAL char blockVote;

	while (bp >= PIXEL_START_BITPOS) {
		blockVote = 0;

		/////////////////////////////
		// 4. pre-process bit plane

		// i) migrate sigma_new to sigma_old, and clear sigma new bit
		statePtr = state + startIndex;
		for (int i = 0; i < 4; ++i) {
		    int current = *statePtr;
			current |=  SIGMA_NEW(current) << SIGMA_OLD_BITPOS;
			CLEAR_SIGMA_NEW(current);

			*statePtr = current;
			statePtr += STATE_BUFFER_STRIDE;

		}
		localMemoryFence();

		// ii) update nbh
		statePtr = state + startIndex;

		int top			= statePtr[TOP];
		int leftTop		= statePtr[LEFT_TOP];
		int rightTop	= statePtr[RIGHT_TOP];
		int left		= statePtr[LEFT];
		int current		= statePtr[0];
		int right		= statePtr[RIGHT];
		int leftBottom  = statePtr[LEFT_BOTTOM];
		int bottom		= statePtr[BOTTOM];
		int rightBottom = statePtr[RIGHT_BOTTOM];

		current |=  (   SIGMA_OLD(leftTop) |
						SIGMA_OLD( top) |
						SIGMA_OLD( rightTop) |
						SIGMA_OLD( left) |
						SIGMA_OLD( right) |
						SIGMA_OLD( leftBottom) |
						SIGMA_OLD( bottom) |
						SIGMA_OLD( rightBottom)  ) << SIGMA_OLD_TO_NBH_SHIFT;

		if ( BIT(current) &&
				NBH(current) &&
					!SIGMA_OLD(current) ) {
			SET_SIGMA_NEW(current);
			blockVote = 1;

		}
		*statePtr = current;

		for (int i = 0; i < 3; ++i) {
			statePtr += STATE_BUFFER_STRIDE;

			top			 = current;
			leftTop		 = left;
			rightTop	 = right;
			left		 = leftBottom;
			current		 = statePtr[0];
			right		 = rightBottom;
			leftBottom   = statePtr[LEFT_BOTTOM];
			bottom		 = statePtr[BOTTOM];
			rightBottom  = statePtr[RIGHT_BOTTOM];

			current |=	   ( SIGMA_OLD(leftTop) |
							 SIGMA_OLD( top) |
							 SIGMA_OLD( rightTop) |
							 SIGMA_OLD( left) |
							 SIGMA_OLD( right) |
							 SIGMA_OLD( leftBottom) |
							 SIGMA_OLD( bottom) |
							 SIGMA_OLD( rightBottom)  ) << SIGMA_OLD_TO_NBH_SHIFT;

			if ( BIT(current) &&
					 NBH(current) &&
						 !SIGMA_OLD(current) ) {
			    SET_SIGMA_NEW(current);
				blockVote = 1;
			}
			*statePtr = current;

		}
		localMemoryFence();

		// iii) block vote on sigma new
		while (blockVote)
		{
		    blockVote = 0;
			localMemoryFence();

			statePtr = state + startIndex;

			//first pixel in strip column
			int top			= statePtr[TOP];
			int leftTop		= statePtr[LEFT_TOP];
			int left		= statePtr[LEFT];
			int current		= statePtr[0];
			int leftBottom  = statePtr[LEFT_BOTTOM];

			current  |= (   SIGMA_NEW(leftTop) |
							SIGMA_NEW( top) |
							SIGMA_NEW( left) |
							SIGMA_NEW( leftBottom) ) << NBH_BITPOS;

			if (  BIT(current) &&
				        !SIGMA_OLD(current) &&
						    NBH(current)  &&
							    !SIGMA_NEW(current)) {
				SET_SIGMA_NEW(current);
				blockVote = 1;

			}
			statePtr[0] = current;

			// next two pixels in strip column
			for (int i = 0; i < 2; ++i) {
				statePtr += STATE_BUFFER_STRIDE;

				top			= current;
				current		= statePtr[0];
				leftTop		= left;
				left		= leftBottom;
				leftBottom  = statePtr[LEFT_BOTTOM];

				current  |= (   SIGMA_NEW(leftTop) |
								SIGMA_NEW( top) |
								SIGMA_NEW( left) |
								SIGMA_NEW( leftBottom) ) << NBH_BITPOS;

				if (  BIT(current) &&
				          !SIGMA_OLD(current) &&
						      NBH(current)  &&
							     !SIGMA_NEW(current)) {
					SET_SIGMA_NEW(current);
					blockVote = 1;

				}
				statePtr[0] = current;
			}

			// last pixel in strip column -
			// ignore leftBottom pixel, because it is in the next
			// stripe and it hasn't been processed yet
			statePtr += STATE_BUFFER_STRIDE;

			top			= current;
			current		= statePtr[0];
			leftTop		= left;
			left		= leftBottom;

			current |= ( SIGMA_NEW(leftTop) |
						 SIGMA_NEW( top) |
						 SIGMA_NEW( left) ) << NBH_BITPOS;
			if (  BIT(current) &&
			        !SIGMA_OLD(current) &&
					    NBH(current)  &&
						    !SIGMA_NEW(current)) {
				SET_SIGMA_NEW(current);
				blockVote = 1;
			}
			statePtr[0] = current;

			localMemoryFence();

		}

		/////////////////////////
		// 5. bit plane processing
		statePtr = state + startIndex;

		top = statePtr[TOP];
		leftTop			= statePtr[LEFT_TOP];
		rightTop		= statePtr[RIGHT_TOP];
		left			= statePtr[LEFT];
		current			= statePtr[0];
		right			= statePtr[RIGHT];
		leftBottom		= statePtr[LEFT_BOTTOM];
		bottom			= statePtr[BOTTOM];
		rightBottom		= statePtr[RIGHT_BOTTOM];

		doRLC = false;
		rlcCount = 0;
		if (SIGMA_OLD(current)) {
			// MRC
		} else {
			int nbh = ( SIGMA_OLD_AND_NEW(leftTop) |
						SIGMA_OLD_AND_NEW( top) |
						SIGMA_OLD_AND_NEW( rightTop) |
						SIGMA_OLD_AND_NEW( left) |
						SIGMA_OLD_AND_NEW( right) |
						SIGMA_OLD_AND_NEW( leftBottom) |
						SIGMA_OLD_AND_NEW( bottom) |
						SIGMA_OLD_AND_NEW( rightBottom)  ) ;

			if (!nbh) {
				if ( !SIGMA_NEW(current) ) {
					doRLC = !BIT(current);
					if (doRLC) {
					   rlcCount++;
					}
				}
			}
			else  {
				// ZC
				if (BIT(current)) {
				    //SC
				}
			}
		}

		for (int i = 0; i < 3; ++i) {
			statePtr += STATE_BUFFER_STRIDE;

			top			= current;
			leftTop		= left;
			rightTop	= right;
			left		= leftBottom;
			current		= statePtr[0];
			right		= rightBottom;
			leftBottom  = statePtr[LEFT_BOTTOM];
			bottom		= statePtr[BOTTOM];
			rightBottom = statePtr[RIGHT_BOTTOM];

			if (SIGMA_OLD(current)) {
				// MRC
			} else {
				int nbh = ( SIGMA_OLD_AND_NEW(leftTop) |
							SIGMA_OLD_AND_NEW( top) |
							SIGMA_OLD_AND_NEW( rightTop) |
							SIGMA_OLD_AND_NEW( left) |
							SIGMA_OLD_AND_NEW( right) |
							SIGMA_OLD_AND_NEW( leftBottom) |
							SIGMA_OLD_AND_NEW( bottom) |
							SIGMA_OLD_AND_NEW( rightBottom)  ) ;

				if (!nbh) {
					if ( !SIGMA_NEW(current) ) {
						doRLC = doRLC && !BIT(current);
						if (!doRLC) {

							rlcCount++;

						}
					}
				}
				else  {
					// ZC
					if (BIT(current)) {
						//SC
					}
				}
			}
		}

		localMemoryFence();
		bp--;
	}
}


add utility method for verification

add utility method for verification

#define PREFER_MCL_WORKGROUP_FUNCTIONS
#if __OPENCL_VERSION__ >= 200 && !defined(PREFER_MCL_WORKGROUP_FUNCTIONS)
#define USE_CL2_WORKGROUP_FUNCTIONS
#endif

#include "clcommons/common.h"
#include "clcommons/image.h"
#include "clcommons/work_group.h"

#ifndef PIXELT
#define PIXELT uint
#endif
#ifndef LABELT
#define LABELT uint
#endif
#ifndef CONNECTIVITYPIXELT
#define CONNECTIVITYPIXELT uint
#endif
#ifndef LDSPIXELT
#define LDSPIXELT uint
#endif
#ifndef LDSLABELT
#define LDSLABELT uint
#endif
#ifndef LDSCONNECTIVITYPIXELT
#define LDSCONNECTIVITYPIXELT uint
#endif

typedef PIXELT PixelT;
typedef LDSPIXELT LDSPixelT;
typedef LABELT LabelT;
typedef LDSLABELT LDSLabelT;
typedef CONNECTIVITYPIXELT ConnectivityPixelT;
typedef LDSCONNECTIVITYPIXELT LDSConnectivityPixelT;

#ifndef BG_VALUE
#define BG_VALUE 0
#endif

#ifndef CONNECTIVITY  //4 or 8
#define CONNECTIVITY 8
#endif

#ifndef WORKGROUP_TILE_SIZE_X
#define WORKGROUP_TILE_SIZE_X 32
#endif
#ifndef WORKGROUP_TILE_SIZE_Y
#define WORKGROUP_TILE_SIZE_Y 2
#endif

#ifndef WORKITEM_REPEAT_X
#define WORKITEM_REPEAT_X 1
#endif
#ifndef WORKITEM_REPEAT_Y
#define WORKITEM_REPEAT_Y 16
#endif

#define TILE_COLS (WORKGROUP_TILE_SIZE_X * WORKITEM_REPEAT_X)
#define TILE_ROWS (WORKGROUP_TILE_SIZE_Y * WORKITEM_REPEAT_Y)

__constant const uint UP = (1<<0);
__constant const uint LEFT = (1<<1);
__constant const uint DOWN = (1<<2);
__constant const uint RIGHT = (1<<3);
__constant const uint LEFT_UP = (1<<4);
__constant const uint LEFT_DOWN = (1<<5);
__constant const uint RIGHT_UP = (1<<6);
__constant const uint RIGHT_DOWN = (1<<7);

#define isConnected(p1, p2) ((p1) == (p2))

#define pixel_at(type, basename, r, c) image_pixel_at(type, PASTE2(basename, _p), im_rows, im_cols, PASTE2(basename, _pitch), (r), (c))

//global dimensions: divUp(image.cols, block.x), divUp(image.rows, block.y);
//__attribute__((reqd_work_group_size(WORKGROUP_TILE_SIZE_X, WORKGROUP_TILE_SIZE_Y, 1)))
//__kernel void
//make_connectivity_image(uint im_rows, uint im_cols, __global PixelT *image_p, uint image_pitch, __global ConnectivityPixelT *connectivityim_p, uint connectivityim_pitch){
//    const uint c = get_global_id(1);
//    const uint r = get_global_id(0);
//    const bool valid_pixel_task = (c < im_cols) & (r < im_rows);
//
//    if(valid_pixel_task){
//        PixelT pixel = pixel_at(PixelT, image, r, c);
//        ConnectivityPixelT connectivity = 0;
//
//#if CONNECTIVITY == 8
//        connectivity |= c > 0 && r > 0                     && isConnected(pixel, pixel_at(PixelT, image, r-1, c - 1)) ? LEFT_UP : 0
//        connectivity |= c > 0                              && isConnected(pixel, pixel_at(PixelT, image, r, c - 1)) ? LEFT : 0
//        connectivity |= c > 0 && r < im_rows - 1           && isConnected(pixel, pixel_at(PixelT, image, r+1, c - 1)) ? LEFT_DOWN : 0
//        connectivity |=          r < im_rows - 1           && isConnected(pixel, pixel_at(PixelT, image, r+1, c)) ? DOWN : 0
//        connectivity |= c < im_cols - 1 && r < im_rows - 1 && isConnected(pixel, pixel_at(PixelT, image, r+1, c + 1)) ? RIGHT_DOWN : 0
//        connectivity |= c < im_cols - 1                    && isConnected(pixel, pixel_at(PixelT, image, r, c + 1)) ? RIGHT : 0
//        connectivity |= c < im_cols - 1 && r > 0           && isConnected(pixel, pixel_at(PixelT, image, r-1, c + 1)) ? RIGHT_UP : 0
//        connectivity |=          r > 0                     && isConnected(pixel, pixel_at(PixelT, image, r-1, c)) ? UP : 0
//#else
//        connectivity |= c > 0                              && isConnected(pixel, pixel_at(PixelT, image, r, c - 1)) ? LEFT : 0
//        connectivity |=          r < im_rows - 1           && isConnected(pixel, pixel_at(PixelT, image, r+1, c)) ? DOWN : 0
//        connectivity |= c < im_cols - 1                    && isConnected(pixel, pixel_at(PixelT, image, r, c + 1)) ? RIGHT : 0
//        connectivity |=          r > 0                     && isConnected(pixel, pixel_at(PixelT, image, r-1, c)) ? UP : 0
//#endif
//
//        pixel_at(ConnectivityPixelT, connectivity, r, c) = connectivity;
//    }
//}

#define apron_pixel(apron, _t_r, _t_c) apron[(_t_r+ 1)][(_t_c + 1)]
//global dimensions: divUp(im_cols, tile_cols), divUp(im_rows, tile_rows);
__attribute__((reqd_work_group_size(WORKGROUP_TILE_SIZE_X, WORKGROUP_TILE_SIZE_Y, 1)))
__kernel void make_connectivity_image(
    uint im_rows, uint im_cols, __global const PixelT *image_p, uint image_pitch, __global ConnectivityPixelT *connectivityim_p, uint connectivityim_pitch
){
    const uint tile_col_blocksize = TILE_COLS;
    const uint tile_row_blocksize = TILE_ROWS;
    const uint tile_col_block = get_group_id(0) + get_global_offset(0) / get_local_size(0);
    const uint tile_row_block = get_group_id(1) + get_global_offset(1) / get_local_size(1);
    const uint tile_col = get_local_id(0);
    const uint tile_row = get_local_id(1);

    uint tile_rows = tile_row_blocksize;
    uint tile_cols = tile_col_blocksize;

    const uint tile_row_start = tile_row_block * tile_rows;
    const uint tile_col_start = tile_col_block * tile_cols;
    const uint tile_row_end = min(tile_row_start + tile_rows, (uint) im_rows);
    const uint tile_col_end = min(tile_col_start + tile_cols, (uint) im_cols);
    //adjust to true tile dimensions
    tile_rows = tile_row_end - tile_row_start;
    tile_cols = tile_col_end - tile_col_start;
    const uint apron_tile_cols = tile_cols + 2;;
    const uint n_tile_pixels = tile_rows * tile_cols;
    const uint n_work_items = get_local_size(0) * get_local_size(1);
    const uint n_apron_tile_pixels = (tile_rows + 2) * (apron_tile_cols);
    __local LDSPixelT im_tile[TILE_ROWS + 2][TILE_COLS + 2];
    __local LDSConnectivityPixelT connectivity_tile[TILE_ROWS][TILE_COLS];

    const uint tid = get_local_linear_id();
    for(uint im_tile_fill_task_id = tid; im_tile_fill_task_id < n_apron_tile_pixels; im_tile_fill_task_id += n_work_items){
        const uint im_apron_tile_row = im_tile_fill_task_id / apron_tile_cols;
        const uint im_apron_tile_col = im_tile_fill_task_id % apron_tile_cols;
        const int g_c = ((int)(im_apron_tile_col + tile_col_start)) - 1;
        const int g_r = ((int)(im_apron_tile_row + tile_row_start)) - 1;

        im_tile[im_apron_tile_row][im_apron_tile_col] = image_tex2D(PixelT, image_p, (int) im_rows, (int) im_cols, image_pitch, g_r, g_c, ADDRESS_ZERO);
    }
    lds_barrier();

    #pragma unroll
    for (int i = 0; i < WORKITEM_REPEAT_Y; ++i){
        #pragma unroll
        for (int j = 0; j < WORKITEM_REPEAT_X; ++j){
            const uint t_c = get_local_id(0) + WORKGROUP_TILE_SIZE_X * j;
            const uint t_r = get_local_id(1) + WORKGROUP_TILE_SIZE_Y * i;
            const uint c = t_c + tile_col_start;
            const uint r = t_r + tile_row_start;
            PixelT pixel = apron_pixel(im_tile, t_r, t_c);
            ConnectivityPixelT connectivity = 0;

#if CONNECTIVITY == 8
            connectivity |= c > 0 && r > 0                         && isConnected(pixel, apron_pixel(im_tile, t_r-1, t_c - 1)) ? LEFT_UP : 0;
            connectivity |= c > 0                                  && isConnected(pixel, apron_pixel(im_tile, t_r  , t_c - 1)) ? LEFT : 0;
            connectivity |= c > 0 && r < im_rows - 1               && isConnected(pixel, apron_pixel(im_tile, t_r+1, t_c - 1)) ? LEFT_DOWN : 0;
            connectivity |=          r < im_rows - 1               && isConnected(pixel, apron_pixel(im_tile, t_r+1, t_c    )) ? DOWN : 0;
            connectivity |= c < im_cols - 1 && r < im_rows - 1     && isConnected(pixel, apron_pixel(im_tile, t_r+1, t_c + 1)) ? RIGHT_DOWN : 0;
            connectivity |= c < im_cols - 1                        && isConnected(pixel, apron_pixel(im_tile, t_r  , t_c + 1)) ? RIGHT : 0;
            connectivity |= c < im_cols - 1 && r > 0               && isConnected(pixel, apron_pixel(im_tile, t_r-1, t_c + 1)) ? RIGHT_UP : 0;
            connectivity |=          r > 0                         && isConnected(pixel, apron_pixel(im_tile, t_r-1, t_c    )) ? UP : 0;
#else
            connectivity |= c > 0                                  && isConnected(pixel, apron_pixel(im_tile, t_r  , t_c - 1)) ? LEFT : 0;
            connectivity |=          r < im_rows - 1               && isConnected(pixel, apron_pixel(im_tile, t_r+1, t_c    )) ? DOWN : 0;
            connectivity |= c < im_cols - 1                        && isConnected(pixel, apron_pixel(im_tile, t_r  , t_c + 1)) ? RIGHT : 0;
            connectivity |=          r > 0                         && isConnected(pixel, apron_pixel(im_tile, t_r-1, t_c    )) ? UP : 0;
#endif
            connectivity = (c < im_cols) & (r < im_rows) ? connectivity : 0;
            connectivity_tile[t_r][t_c] = connectivity;
        }
    }
    lds_barrier();

    for(uint im_tile_fill_task_id = tid; im_tile_fill_task_id < n_tile_pixels; im_tile_fill_task_id += n_work_items){
        const uint im_tile_row = im_tile_fill_task_id / tile_cols;
        const uint im_tile_col = im_tile_fill_task_id % tile_cols;
        const uint g_c = im_tile_col + tile_col_start;
        const uint g_r = im_tile_row + tile_row_start;

        pixel_at(ConnectivityPixelT, connectivityim, g_r, g_c) = connectivity_tile[im_tile_row][im_tile_col];
    }
}

__attribute__((reqd_work_group_size(WORKGROUP_TILE_SIZE_X, WORKGROUP_TILE_SIZE_Y, 1)))
__kernel void label_tiles(
    uint im_rows, uint im_cols, __global LabelT *labelim_p, uint labelim_pitch, __global const ConnectivityPixelT *connectivityim_p, uint connectivityim_pitch
){
    const uint tile_col_blocksize = TILE_COLS;
    const uint tile_row_blocksize = TILE_ROWS;
    const uint tile_col_block = get_group_id(0) + get_global_offset(0) / get_local_size(0);
    const uint tile_row_block = get_group_id(1) + get_global_offset(1) / get_local_size(1);

    uint tile_rows = tile_row_blocksize;
    uint tile_cols = tile_col_blocksize;

    const uint tile_row_start = tile_row_block * tile_rows;
    const uint tile_col_start = tile_col_block * tile_cols;
    const uint tile_row_end = min(tile_row_start + tile_rows, (uint) im_rows);
    const uint tile_col_end = min(tile_col_start + tile_cols, (uint) im_cols);
    //adjust to true tile dimensions
    tile_rows = tile_row_end - tile_row_start;
    tile_cols = tile_col_end - tile_col_start;

    //if (x >= im_rows || y >= im_rows) return;

    __local LDSLabelT label_tile_im[TILE_ROWS][TILE_COLS];
    __local LDSConnectivityPixelT  edge_tile_im[TILE_ROWS][TILE_COLS];

    LDSLabelT new_labels[WORKITEM_REPEAT_Y][WORKITEM_REPEAT_X];
    LDSLabelT old_labels[WORKITEM_REPEAT_Y][WORKITEM_REPEAT_X];

    #pragma unroll
    for (int i = 0; i < WORKITEM_REPEAT_Y; ++i){
        #pragma unroll
        for (int j = 0; j < WORKITEM_REPEAT_X; ++j){
            const uint tile_row = get_local_id(1) + WORKGROUP_TILE_SIZE_Y * i;
            const uint tile_col = get_local_id(0) + WORKGROUP_TILE_SIZE_X * j;
            const bool valid_pixel_task = (tile_col < tile_cols) & (tile_row < tile_rows);
            ConnectivityPixelT c = valid_pixel_task ? pixel_at(ConnectivityPixelT, connectivityim, tile_row_start + tile_row, tile_col_start + tile_col) : 0;

            c = tile_col == 0 ? c & ~(LEFT|LEFT_DOWN|LEFT_UP) : c;
            c = tile_row == 0 ? c & ~(UP|LEFT_UP|RIGHT_UP) : c;

            c = tile_col >= tile_cols - 1 ? c & ~(RIGHT|RIGHT_DOWN|RIGHT_UP) : c;
            c = tile_row >= tile_rows - 1 ? c & ~(DOWN|LEFT_DOWN|RIGHT_DOWN) : c;

            new_labels[i][j] = valid_pixel_task ? tile_row * tile_cols + tile_col : ((LDSLabelT) -1);
            edge_tile_im[tile_row][tile_col] = c;
        }
    }

    for (uint k = 0; ;++k){
        //make copies
        #pragma unroll
        for (int i = 0; i < WORKITEM_REPEAT_Y; ++i){
            #pragma unroll
            for (int j = 0; j < WORKITEM_REPEAT_X; ++j){
                const uint tile_row = get_local_id(1) + WORKGROUP_TILE_SIZE_Y * i;
                const uint tile_col = get_local_id(0) + WORKGROUP_TILE_SIZE_X * j;
                old_labels[i][j] = new_labels[i][j];
                label_tile_im[tile_row][tile_col] = new_labels[i][j];
            }
        }
        lds_barrier();

        //take minimum label of local neighboorhood - single writer, multi reader version
        #pragma unroll
        for (int i = 0; i < WORKITEM_REPEAT_Y; ++i){
            #pragma unroll
            for (int j = 0; j < WORKITEM_REPEAT_X; ++j){
                const uint tile_row = get_local_id(1) + WORKGROUP_TILE_SIZE_Y * i;
                const uint tile_col = get_local_id(0) + WORKGROUP_TILE_SIZE_X * j;

                const ConnectivityPixelT connectivity = edge_tile_im[tile_row][tile_col];
                LDSLabelT label = new_labels[i][j];

#if CONNECTIVITY == 8
                label = connectivity & UP            ? min(label, label_tile_im[tile_row - 1][tile_col - 0]) : label;
                label = connectivity & LEFT_UP       ? min(label, label_tile_im[tile_row - 1][tile_col - 1]) : label;
                label = connectivity & LEFT          ? min(label, label_tile_im[tile_row - 0][tile_col - 1]) : label;
                label = connectivity & LEFT_DOWN     ? min(label, label_tile_im[tile_row + 1][tile_col - 1]) : label;
                label = connectivity & DOWN          ? min(label, label_tile_im[tile_row + 1][tile_col - 0]) : label;
                label = connectivity & RIGHT_DOWN    ? min(label, label_tile_im[tile_row + 1][tile_col + 1]) : label;
                label = connectivity & RIGHT         ? min(label, label_tile_im[tile_row + 0][tile_col + 1]) : label;
                label = connectivity & RIGHT_UP      ? min(label, label_tile_im[tile_row - 1][tile_col + 1]) : label;
#else
                label = connectivity & UP            ? min(label, label_tile_im[tile_row - 1][tile_col - 0]) : label;
                label = connectivity & LEFT          ? min(label, label_tile_im[tile_row - 0][tile_col - 1]) : label;
                label = connectivity & DOWN          ? min(label, label_tile_im[tile_row + 1][tile_col - 0]) : label;
                label = connectivity & RIGHT         ? min(label, label_tile_im[tile_row + 0][tile_col + 1]) : label;
#endif

                new_labels[i][j] = label;
            }
        }
        lds_barrier();

        __local uint changed;
        if((get_local_id(1) == 0) & (get_local_id(0) == 0)){
            changed = 0;
        }
        lds_barrier();

        uint pchanged = 0;
        #pragma unroll
        for(int i = 0; i < WORKITEM_REPEAT_Y; ++i){
            #pragma unroll
            for(int j = 0; j < WORKITEM_REPEAT_X; ++j){
                const uint tile_row = get_local_id(1) + WORKGROUP_TILE_SIZE_Y * i;
                const uint tile_col = get_local_id(0) + WORKGROUP_TILE_SIZE_X * j;
                const bool valid_pixel_task = (tile_col < tile_cols) & (tile_row < tile_rows);
                const uint old_label = old_labels[i][j];
                if(valid_pixel_task & (new_labels[i][j] < old_label)){
                    pchanged++;
                    atomic_min(&label_tile_im[old_label / tile_cols][old_label % tile_cols], new_labels[i][j]);
                }
            }
        }
        atomic_add(&changed, pchanged);
        lds_barrier();

        //if there are no updates, we are finished
        if(!changed){
            break;
        }

        //Compact paths
        #pragma unroll
        for(int i = 0; i < WORKITEM_REPEAT_Y; ++i){
            #pragma unroll
            for(int j = 0; j < WORKITEM_REPEAT_X; ++j){
                const uint tile_row = get_local_id(1) + WORKGROUP_TILE_SIZE_Y * i;
                const uint tile_col = get_local_id(0) + WORKGROUP_TILE_SIZE_X * j;
                const bool valid_pixel_task = (tile_col < tile_cols) & (tile_row < tile_rows);
                if(valid_pixel_task){
                    LDSLabelT label = new_labels[i][j];
                    //find root label
                    while(label_tile_im[label / tile_cols][label % tile_cols] < label){
                        label = label_tile_im[label / tile_cols][label % tile_cols];
                    }

                    new_labels[i][j] = label;
                }
            }
        }
        lds_barrier();
    }

    //save with adjusted global (untiled) labels
    #pragma unroll
    for(int i = 0; i < WORKITEM_REPEAT_Y; ++i){
        #pragma unroll
        for(int j = 0; j < WORKITEM_REPEAT_X; ++j){
            const LabelT tile_label = new_labels[i][j];
            //convert the tile label into it's 2-D equivilent
            const uint l_g_r = (tile_label / tile_cols) + tile_row_start;
            const uint l_g_c = (tile_label % tile_cols) + tile_col_start;

            const uint tile_row = get_local_id(1) + WORKGROUP_TILE_SIZE_Y * i;
            const uint tile_col = get_local_id(0) + WORKGROUP_TILE_SIZE_X * j;
            const uint g_r = tile_row_start + tile_row;
            const uint g_c = tile_col_start + tile_col;

            //adjust to global offset and convert to scanline order again - this is globally unique
            const LabelT glabel = l_g_r * im_cols + l_g_c;
            const bool valid_pixel_task = (g_c < im_cols) & (g_r < im_rows);
            if(valid_pixel_task){
                assert_val(tile_label < tile_rows * tile_cols, tile_label);
                pixel_at(LabelT, labelim, g_r, g_c) = glabel;
            }
        }
    }
}

inline
LabelT find_root_global(const __global LabelT *labelim_p, uint labelim_pitch, LabelT label, const uint im_rows, const uint im_cols){
    for(;;){
        const uint y = label / im_cols;
        const uint x = label % im_cols;
        assert_val(y < im_rows, y);
        assert_val(x < im_cols, x);
        const LabelT parent = pixel_at(LabelT, labelim, y, x);
        //printf((__constant char *)"parent of %d (%d, %d) is %d (%d, %d)\n", label, y, x, parent, parent / im_cols, parent % im_cols);

        if(label == parent){
            break;
        }

        label = parent;
        //if(k == kmax-1){
        //    printf("kmax was reached in %d, %d\n", get_global_id(0), get_global_id(1));
        //}
    }
    return label;
}

__kernel void compact_paths_global(uint im_rows, uint im_cols, __global LabelT *labelim_p, uint labelim_pitch){
    const uint x = get_global_id(0);
    const uint y = get_global_id(1);

    if((x < im_cols) & (y < im_rows)){
        const LabelT label = pixel_at(LabelT, labelim, y, x);
        pixel_at(LabelT, labelim, y, x) = find_root_global(labelim_p, labelim_pitch, label, im_rows, im_cols);
    }
}

inline
uint merge_edge_labels(const uint im_rows, const uint im_cols, __global LabelT *labelim_p, const uint labelim_pitch, const LabelT l1, const LabelT l2){
    const LabelT r1 = find_root_global(labelim_p, labelim_pitch, l1, im_rows, im_cols);
    const LabelT r2 = find_root_global(labelim_p, labelim_pitch, l2, im_rows, im_cols);

    if(r1 == r2){
        return 0;
    }

    const LabelT mi = min(r1, r2);
    const LabelT ma = max(r1, r2);

    const uint ma_y = ma / im_cols;
    const uint ma_x = ma % im_cols;

    atomic_min(&pixel_at(LabelT, labelim, ma_y, ma_x), mi);
    return 1;
}

//merge along the adjacent pixels in the grid formed on the image with the following tiles
__kernel void merge_tiles(
    const uint im_rows, const uint im_cols,
    const uint n_vert_tiles, const uint n_horz_tiles,
    uint tile_rows, uint tile_cols,
    const __global ConnectivityPixelT *connectivityim_p, const uint connectivityim_pitch,
    __global LabelT *labelim_p, const uint labelim_pitch,
    const uint yIncomplete, uint xIncomplete
){
    const uint ngroups_x = get_num_groups(0);
    const uint ngroups_y = get_num_groups(1);

    const uint tid = get_local_linear_id();
    const uint nwork_elements = get_local_size(1) * get_local_size(0);

    const uint ybegin = get_group_id(1) * (n_vert_tiles * tile_rows);
    uint yend = ybegin + n_vert_tiles * tile_rows;

    if(get_group_id(1) == ngroups_y - 1){
        yend -= yIncomplete * tile_rows;
        yend -= tile_rows;
        tile_rows = (im_rows % tile_rows);

        yend += tile_rows;
    }

    const uint xbegin = get_group_id(0) * n_horz_tiles * tile_cols;
    uint xend = xbegin + n_horz_tiles * tile_cols;

    if(get_group_id(0) == ngroups_x - 1){
        if (xIncomplete) yend = ybegin;
        xend -= xIncomplete * tile_cols;
        xend -= tile_cols;
        tile_cols = (im_cols % tile_cols);

        xend += tile_cols;
    }

    if(get_group_id(1) == (ngroups_y - 1) && yIncomplete){
        xend = xbegin;
    }

    const uint tasksV = (n_horz_tiles - 1) * (yend - ybegin);
    const uint tasksH = (n_vert_tiles - 1) * (xend - xbegin);

    const uint total = tasksH + tasksV;

    uint pchanged;
    do{
        __local uint changed;
        if(tid == 0){
            changed = 0;
        }
        lds_barrier();

        pchanged = 0;
        for(uint taskIdx = tid; taskIdx < total; taskIdx += nwork_elements){
            if(taskIdx < tasksH){
                const uint indexH = taskIdx;

                const uint row = indexH / (xend - xbegin);
                const uint col = indexH % (xend - xbegin);

                const uint y = ybegin + (row + 1) * tile_rows;
                const uint x = xbegin + col;
                const LabelT lc = pixel_at(LabelT, labelim, y, x);

                const ConnectivityPixelT e = pixel_at(ConnectivityPixelT, connectivityim, y, x);
                if(e & UP){
                    const LabelT lu = pixel_at(LabelT, labelim, y - 1, x);
                    pchanged += merge_edge_labels(im_rows, im_cols, labelim_p, labelim_pitch, lc, lu);
                }
#if CONNECTIVITY == 8
                if(e & LEFT_UP){
                    const LabelT lu = pixel_at(LabelT, labelim, y - 1, x - 1);
                    pchanged += merge_edge_labels(im_rows, im_cols, labelim_p, labelim_pitch, lc, lu);
                }
                if(e & RIGHT_UP){
                    const LabelT lu = pixel_at(LabelT, labelim, y - 1, x + 1);
                    pchanged += merge_edge_labels(im_rows, im_cols, labelim_p, labelim_pitch, lc, lu);
                }
#endif
            }else{
                const uint indexV = taskIdx - tasksH;

                const uint col = indexV / (yend - ybegin);
                const uint row = indexV % (yend - ybegin);

                const uint x = xbegin + (col + 1) * tile_cols;
                const uint y = ybegin + row;
                const LabelT lc = pixel_at(LabelT, labelim, y, x);

                const ConnectivityPixelT e = pixel_at(ConnectivityPixelT, connectivityim, y, x);
                if(e & LEFT){
                    const LabelT lu = pixel_at(LabelT, labelim, y, x - 1);
                    pchanged += merge_edge_labels(im_rows, im_cols, labelim_p, labelim_pitch, lc, lu);
                }
#if CONNECTIVITY == 8
                if(e & LEFT_UP){
                    const LabelT lu = pixel_at(LabelT, labelim, y - 1, x - 1);
                    pchanged += merge_edge_labels(im_rows, im_cols, labelim_p, labelim_pitch, lc, lu);
                }
                if(e & LEFT_DOWN){
                    const LabelT lu = pixel_at(LabelT, labelim, y + 1, x - 1);
                    pchanged += merge_edge_labels(im_rows, im_cols, labelim_p, labelim_pitch, lc, lu);
                }
#endif
            }
        }
        atomic_add(&changed, pchanged);
        lds_barrier();
        pchanged = changed;
    }while(pchanged > 0);
}

#if 0
__kernel void mark_root_classes(
    uint im_rows, uint im_cols,
    __global PixelT *image_p, uint image_pitch,
    __global const LabelT* labelim_p, const uint labelim_pitch,
    __global uint* is_root_class_image_p, const uint is_root_class_image_pitch
){
    const uint c = get_global_id(0);
    const uint r = get_global_id(1);
    const bool valid_pixel_task = (c < im_cols) & (r < im_rows);

    const uint linear_index = c + r * im_cols;
    if(valid_pixel_task){
        const PixelT pixel = pixel_at(PixelT, image, r, c);
        const LabelT label = pixel_at(LabelT, labelim, r, c);
        pixel_at(uint, is_root_class_image, r, c) = (pixel != BG_VALUE) & (label == linear_index);
    }
}
#endif

#ifndef USE_CL2_WORKGROUP_FUNCTIONS
MAKE_WORK_GROUP_FUNCTIONS(uint, uint, 0U, UINT_MAX)
#endif

//root class inclusive prefix sums belonging to each compute unit given to each tile - note if narray_workers == 1, no merge step is necessary
//computes local prefix sums to get intra-wg blocksums, prefix sum that to get intra-wg offsets - this is needed to merge the final blocksums
//global dims: <wgs_per_histogram, n_tiles>, work_dims: <wg_size, 1>
//global blocksums[divUp(nblocks, blocks_per_wg)]
#ifdef PROMISE_WG_IS_WAVEFRONT
__attribute__((reqd_work_group_size(DEVICE_WAVEFRONT_SIZE, 1, 1)))
#endif
__kernel void mark_roots_and_make_intra_wg_block_local_prefix_sums(uint im_rows, uint im_cols,
    __global PixelT *image_p, uint image_pitch,
    __global const LabelT* labelim_p, const uint labelim_pitch,
    __global uint * restrict array_intra_wg_block_sums_p,
    __global uint * restrict array_prefix_sum_p, const uint array_prefix_sum_pitch
){
    const uint array_length = im_rows * im_cols;
    const uint array_wg_id = get_group_id(0);
    const uint narray_workers = get_num_groups(0);
    const uint wg_size = get_local_size(0);
    const uint block_size = wg_size;//efficient block size
    const uint nblocks = divUp(array_length, block_size);//number of efficiently processible blocks
    const uint nblocks_per_wg = nblocks / narray_workers;
    //const uint nblocks_to_merge = nblocks / nblocks_per_wg;
    const uint nblocks_remainder = nblocks - (narray_workers * nblocks_per_wg);
    const uint nblocks_to_left = nblocks_per_wg * array_wg_id + (array_wg_id < nblocks_remainder ? array_wg_id : nblocks_remainder);
    const uint n_wg_blocks = nblocks_per_wg + (array_wg_id < nblocks_remainder ? 1 : 0);

    const uint start_bin = nblocks_to_left * block_size;
    const uint end_bin_ = start_bin + n_wg_blocks * block_size;//block aligned end

    uint inter_block_sum = 0;

    for(uint linear_index = get_local_id(0) + start_bin; linear_index < end_bin_; linear_index += wg_size){
        const uint r = linear_index / im_cols;
        const uint c = linear_index % im_cols;

        uint count = 0;
        if(linear_index < array_length){
            const PixelT pixel = pixel_at(PixelT, image, r, c);
            const LabelT label = pixel_at(LabelT, labelim, r, c);
            count = (pixel != BG_VALUE) & (label == linear_index) ? 1 : 0;
        }

#ifdef USE_CL2_WORKGROUP_FUNCTIONS
        uint block_prefix_sum_inclusive = work_group_scan_inclusive_add(count);
#else
        __local uint lmem[WORK_GROUP_FUNCTION_MAX_MEMORY_SIZE];
        uint block_prefix_sum_inclusive = clc_work_group_scan_inclusive_add_uint(count, lmem);
#endif

        block_prefix_sum_inclusive += inter_block_sum;
        if(linear_index < array_length){
            pixel_at(uint, array_prefix_sum, r, c) = block_prefix_sum_inclusive;
        }
#ifdef USE_CL2_WORKGROUP_FUNCTIONS
        inter_block_sum = work_group_broadcast(block_prefix_sum_inclusive, wg_size - 1);
#else
        __local uint value;
        inter_block_sum = clc_work_group_broadcast1_uint(block_prefix_sum_inclusive, wg_size - 1, &value);
#endif
    }
    if((array_intra_wg_block_sums_p != 0) & (get_local_id(0) == wg_size - 1)){
        array_intra_wg_block_sums_p[array_wg_id] = inter_block_sum;
    }
}

//exclusive prefix sums intra-wg blocksums to get intra-wg offsets - needed to merge together all the wg-local prefix sums
//global dims: <1, n_tiles>, work_dims: <wg_size, 1>
//global tile_intra_wg_block_sums[n_tiles][nblocks_to_merge]
#ifdef PROMISE_WG_IS_WAVEFRONT
__attribute__((reqd_work_group_size(DEVICE_WAVEFRONT_SIZE, 1, 1)))
#endif
__kernel void make_intra_wg_block_global_sums(
    __global uint * restrict intra_wg_block_sums_p, uint nblocks_to_merge
){
    const uint wg_size = get_local_size(0);

    __local uint intra_wg_block_sums[WG_SIZE_MAX + 1];
    if(get_local_id(0) == 0){
        intra_wg_block_sums[0] = 0;
    }
    lds_barrier();
    uint intra_wg_block_offset = 0;

    for(uint intra_wg_block_id = get_local_id(0); intra_wg_block_id < divUp(nblocks_to_merge, wg_size) * wg_size; intra_wg_block_id += wg_size){
        //get the unsumed blocksums
        const uint intra_wg_block_sum = intra_wg_block_id < nblocks_to_merge ? intra_wg_block_sums_p[intra_wg_block_id] : 0;
        intra_wg_block_sums[get_local_id(0) + 1] = intra_wg_block_sum;
        lds_barrier();

        const uint intra_wg_block_sum_delayed = intra_wg_block_sums[get_local_id(0)];
#ifdef USE_CL2_WORKGROUP_FUNCTIONS
        intra_wg_block_offset += work_group_scan_inclusive_add(intra_wg_block_sum_delayed);
#else
        __local uint lmem[WORK_GROUP_FUNCTION_MAX_MEMORY_SIZE];
        intra_wg_block_offset += clc_work_group_scan_inclusive_add_uint(intra_wg_block_sum_delayed, lmem);
#endif
        if(intra_wg_block_id < nblocks_to_merge){
            intra_wg_block_sums_p[intra_wg_block_id] = intra_wg_block_offset;
        }
#ifdef USE_CL2_WORKGROUP_FUNCTIONS
        intra_wg_block_offset = work_group_broadcast(intra_wg_block_offset, wg_size - 1);
#else
        __local uint value;
        intra_wg_block_offset = clc_work_group_broadcast1_uint(intra_wg_block_offset, wg_size - 1, &value);
#endif

        if(get_local_id(0) == 0){
            intra_wg_block_sums[0] = intra_wg_block_sums[wg_size];
        }
        lds_barrier();
    }
}

//merges global offsets of intra-wg-block offsets of prefix sums
//global dims: <wgs_per_sum>, work_dims: <wg_size> : wg_size >= nblocks_to_merge
//global array_of_prefix_sums[im_rows*im_cols] : as input partial sums, as output full prefix sum
#ifdef PROMISE_WG_IS_WAVEFRONT
__attribute__((reqd_work_group_size(DEVICE_WAVEFRONT_SIZE, 1, 1)))
#endif
__kernel void make_prefix_sums_with_intra_wg_block_global_sums(
    const uint im_rows, const uint im_cols,
    __global const uint * restrict intra_wg_block_sums_p,
    __global uint * restrict array_of_prefix_sums_p, uint array_of_prefix_sums_pitch,
    __global LabelT* label_count_p
){
    const uint array_length = im_rows * im_cols;
    const uint n_arrays = get_num_groups(1);
    const uint array_id = get_group_id(1);
    const uint array_wg_id = get_group_id(0);
    const uint narray_workers = get_num_groups(0);
    const uint wg_size = get_local_size(0);
    const uint block_size = wg_size;//efficient block size
    const uint nblocks = divUp(array_length, block_size);//number of efficiently processible blocks
    const uint nblocks_per_wg = nblocks / narray_workers;
    const uint nblocks_to_merge = nblocks / nblocks_per_wg;
    const uint nblocks_remainder = nblocks - (narray_workers * nblocks_per_wg);
    const uint nblocks_to_left = nblocks_per_wg * array_wg_id + (array_wg_id < nblocks_remainder ? array_wg_id : nblocks_remainder);
    const uint n_wg_blocks = nblocks_per_wg + (array_wg_id < nblocks_remainder ? 1 : 0);

    const uint start_index = nblocks_to_left * block_size;
    const uint end_index_ = start_index + n_wg_blocks * block_size;//block aligned end

    const uint inter_block_sum = intra_wg_block_sums_p[array_wg_id];
    for(uint linear_index = get_local_id(0) + start_index; linear_index < end_index_; linear_index += wg_size){
        if(linear_index < array_length){
            const uint r = linear_index / im_cols;
            const uint c = linear_index % im_cols;
            const uint count = image_pixel_at(uint, array_of_prefix_sums_p, im_rows, im_cols, array_of_prefix_sums_pitch, r, c) + inter_block_sum;
            if(linear_index == array_length - 1){
                *label_count_p = count + 2;//include BG and count shift
            }
            image_pixel_at(uint, array_of_prefix_sums_p, im_rows, im_cols, array_of_prefix_sums_pitch, r, c) = count;
        }
    }
}

__kernel void relabel_with_scanline_order(
    uint im_rows, uint im_cols,
    __global LabelT* labelim_out_p, const uint labelim_out_pitch,
    __global const PixelT* image_p, const uint image_pitch,
    __global const LabelT* labelim_p, const uint labelim_pitch,
    __global uint* scanline_prefix_sum_of_root_classes_p, const uint scanline_prefix_sum_of_root_classes_pitch
){
    const uint c = get_global_id(0);
    const uint r = get_global_id(1);
    const bool valid_pixel_task = (c < im_cols) & (r < im_rows);

    if(valid_pixel_task){
        const PixelT pixel = pixel_at(PixelT, image, r, c);
        LabelT final_label = 0;
        if(pixel != BG_VALUE){
            const LabelT label = pixel_at(LabelT, labelim, r, c);
            const uint label_r = label / im_cols;
            const uint label_c = label % im_cols;
            final_label = pixel_at(uint, scanline_prefix_sum_of_root_classes, label_r, label_c) + 1;
        }
        pixel_at(LabelT, labelim_out, r, c) = final_label;
    }
}

__kernel void count_invalid_labels(
    uint im_rows, uint im_cols,
    __global const LabelT* labelim_p, const uint labelim_pitch,
    __global const ConnectivityPixelT *connectivityim_p, const uint connectivityim_pitch,
    __global const uint *dcountim_p, const uint dcountim_pitch
){
    const uint c = get_global_id(0);
    const uint r = get_global_id(1);
    const bool valid_pixel_task = (c < im_cols) & (r < im_rows);

    if(valid_pixel_task){
        const ConnectivityPixelT connectivity = pixel_at(ConnectivityPixelT, connectivityim, r, c);
        const LabelT label = pixel_at(LabelT, labelim, r, c);
        uint dcount = 0;

        dcount += connectivity & UP         ? label != pixel_at(LabelT, labelim, r - 1, c - 0) : 0;
        dcount += connectivity & LEFT_UP    ? label != pixel_at(LabelT, labelim, r - 1, c - 1) : 0;
        dcount += connectivity & LEFT       ? label != pixel_at(LabelT, labelim, r - 0, c - 1) : 0;
        dcount += connectivity & LEFT_DOWN  ? label != pixel_at(LabelT, labelim, r + 1, c - 1) : 0;
        dcount += connectivity & DOWN       ? label != pixel_at(LabelT, labelim, r + 1, c - 0) : 0;
        dcount += connectivity & RIGHT_DOWN ? label != pixel_at(LabelT, labelim, r + 1, c + 1) : 0;
        dcount += connectivity & RIGHT      ? label != pixel_at(LabelT, labelim, r + 0, c + 1) : 0;
        dcount += connectivity & RIGHT_UP   ? label != pixel_at(LabelT, labelim, r - 1, c + 1) : 0;

        pixel_at(uint, dcountim, r, c) = dcount;
    }
}


add main and step functions

add main and step functions

;emulator for the MSP430, written to help solve the final level in the Matasano/Square CTF at https://microcorruption.com/

;data structure plans:
;memory to be represented as a large array of bytes
;registers as a much smaller array of words

(defvar *reg* (make-array 16 :element-type '(UNSIGNED-BYTE 16)))

;(setf *reg* (make-array 16 :element-type '(unsigned-byte 16)))

(defvar *mem* (make-array 65536 :element-type '(UNSIGNED-BYTE 8)))

(defvar *err*)

(defun mem (idx)
  (elt *mem* idx))

(defun reg (idx)
  (elt *reg* idx))

(defun flag (idx)
  (ldb (byte 1 idx) (reg 2)))

(defun get-c ()
  (ldb (byte 1 0) (reg 2)))

(defun set-c (int)
  (setf (elt *reg* 2) (dpb int (byte 1 0) (reg 2))))

(defun get-z ()
  (ldb (byte 1 1) (reg 2)))

(defun set-z (int)
  (setf (elt *reg* 2) (dpb int (byte 1 1) (reg 2))))

(defun get-n ()
  (ldb (byte 1 2) (reg 2)))

(defun set-n (int)
  (setf (elt *reg* 2) (dpb int (byte 1 2) (reg 2))))

(defun get-v ()
  (ldb (byte 1 8) (reg 2)))

(defun set-v (int)
  (setf (elt *reg* 2) (dpb int (byte 1 8) (reg 2))))

(defun bytes-to-word (low high)
  (+ (* 256 high) low))

(defun mod+ (&rest args)
  (mod (reduce #'+ args) 65536))

(defun mod* (&rest args)
  (mod (reduce #'* args) 65536))

(defun get-mem (idx)
  (bytes-to-word (mem idx) (mem (+ idx 1))))

(defun set-mem (idx int)
  (let* ((low (mod int 256))
	 (high (/ (- int low) 256)))
    (setf (elt *mem* idx) low
	  (elt *mem* (+ idx 1)) high)))

(defun mem-fun (idx)
  (function (lambda (int) (set-mem idx int))))

(defun reg-fun (idx)
  (function (lambda (int) (setf (elt *reg* idx) int))))

(defun nullp (val)
  (if (= val 0)
      1
      0))

(defun negp (val b-w)
  (ldb (byte 1 (+ b-w 7)) val))

(defun trim-bw (val b-w)
  (ldb (byte (+ b-w 8) 0) val))

(defun main ()
  (loop while (emu-step)))

(defun emu-step ()
  (let ((pc (reg 0)))
    (cond ((oddp pc) (progn (setf *err* "PC unalligned")
			    (return-from emu-step nil)))
	  ((= pc 10) (int-handler))
	  (t (instruction-decode (subseq *mem* pc (+ pc 6)))))))

(defun instruction-decode (bytes)
  ;bytes to be an arry of 6 bytes, to allow for the longest possible instruction
  ;this is the entry point into the instruction decoding, will eventually call the
  ;appropriate function
  (let ((instn (bytes-to-word (elt bytes 0) (elt bytes 1)))
	(data1 (bytes-to-word (elt bytes 2) (elt bytes 3)))
	(data2 (bytes-to-word (elt bytes 4) (elt bytes 5))))
    (cond ((= (ldb (byte 4 12) instn)) (single-decode instn data1))
	  ((= (ldb (byte 3 13) instn)) (jump-decode instn))
	  ((>= (ldb (byte 4 12) instn)) (double-decode instn data1 data2)))))

(defun address-decode (ad dest data)
  ;decodes an address for a single operand instruction
  ;returns an integer representing the current value and a function that sets the new value
  (if (or (= dest 2) (= dest 3))
      (if (= dest 2)
	  (cond ((= ad 0) (values (reg dest)
				  (reg-fun dest))) ;register mode
		((= ad 1) (values (get-mem data)
				  (mem-fun data))) ;absolute mode
		((= ad 2) (values 4 4)) ;represents a constant, can't set things
		((= ad 3) (values 8 8)) ;as above
		(t 'nil))
	  (cond ((= ad 0) (values 0 0))
		((= ad 1) (values 1 1))
		((= ad 2) (values 2 2))
		((= ad 3) (values 65535 65535))
		(t 'nil)))
      (cond ((= ad 0) (values (reg dest)
			      (reg-fun dest))) ;register mode
	    ((= ad 1) (let ((idx (mod+ (reg dest) data)))
			(values (get-mem idx)
				(mem-fun idx)))) ;index mode
	    ((= ad 2) (values (get-mem (reg dest))
			      (mem-fun (reg dest)))) ;indirect mode
	    ((= ad 3) (progn (incf (elt *reg* dest) 2)
			     (values (get-mem (- (reg dest) 2))
				     (mem-fun (- (reg dest) 2))))) ;indirect mode with increment
	    (t 'nil))))

(defun single-decode (instn data)
  ;decodes and calls a single operand instruction
  (let ((opcode (ldb (byte 3 7) instn))
	(ad (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (* (lbd (byte 1 6) instn))))
    (multiple-value-bind (value set-fun)
	(address-decode ad dest data)
      (cond ((= opcode 0) (rrc value set-fun b-w))
	    ((= opcode 1) (swpb value set-fun))
	    ((= opcode 2) (rra value set-fun b-w))
	    ((= opcode 3) (sxt value set-fun b-w))
	    ((= opcode 4) (psh value set-fun b-w))
	    ((= opcode 5) (call value set-fun b-w))
	    (t 'nil)))))
	      ;RETI isn't implemented because it's not in the CTF

(defun rrc (value set-fun b-w)
  (let ((carry (ldb (byte 1 0) value))
	(tmp (ash (trim-bw value b-w) -1)))
    (let ((res (dpb (get-c) (byte 1 (+ b-w 7)) tmp)))
      (funcall set-fun (trim-bw res b-w))
      (set-v 0)
      (set-c carry)
      (if (= (negp res) 1)
	  (set-n 1))
      (if (= (nullp res) 0)
	  (set-z 0)))))

(defun swpb (value set-fun)
  (let ((low (ldb (byte 8 8) value))
	(high (ldb (byte 8 0) value)))
    (funcall set-fun (+ (* 256 high) low))))

(defun rra (value set-fun b-w)
  (let ((res (ash (trim-bw value b-w) -1)))
    (funcall set-fun (trim-bw res b-w))
    (if (= (negp res) 1)
	(set-n 1))
    (set-v 0)
    (set-z 0)))

(defun sxt (value set-fun)
  (let ((res (dpb (* 255 (ldb (byte 1 7) value)) (byte 8 8) value)))
    (funcall set-fun res)
    (set-c (if (= res 0)
	       0
	       1))
    (set-v 0)
    (set-n (negp res))
    (set-z (nullp res))))

(defun psh (value set-fun b-w)
  (setf (elt *mem* (reg 1)) (trim-bw value b-w))
  (incf (elt *reg* 1) -2))

(defun call (value set-fun)
  (psh (reg 0) 'nil)
  (setf (elt *reg* 0) value))

(defun jump-decode (instn)
  (let ((type (ldb (byte 3 10) instn))
	(offset (ldb (byte 10 0) instn))
	(z (flag 1))
	(c (flag 0))
	(n (flag 2))
	(v (flag 8)))
    (cond ((= type 0) (if (= z 0) (jmp offset)))
	  ((= type 1) (if (= z 1) (jmp offset)))
	  ((= type 2) (if (= c 0) (jmp offset)))
	  ((= type 3) (if (= c 1) (jmp offset)))
	  ((= type 4) (if (= n 1) (jmp offset)))
	  ((= type 5) (if (= n v) (jmp offset)))
	  ((= type 6) (if (/= n v) (jmp offset)))
	  ((= type 7) (jmp offset)))))

(defun jmp (offset)
  (setf (elt *reg* 0) (+ (reg 0) 2
		   (- (ldb (byte 9 0) offset)
		      (* (ldb (byte 1 9) offset) 1024))))
  t)

(defun double-decode (instn data1 data2)
  (let ((opcode (ldb (byte 4 12) instn))
	(source (ldb (byte 4 8) instn))
	(ad (ldb (byte 1 7) instn))
	(as (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (* (ldb (byte 1 6) instn) 8)))
    (multiple-value-bind (dest-val dest-fun)
	(address-decode ad dest data2)
      (multiple-value-bind (source-val source-fun)
	  (address-decode as source data1)
	(cond ((= opcode 4) (mov source-val dest-fun b-w))
	      ((= opcode 5) (add source-val dest-val dest-fun b-w))
	      ((= opcode 6) (addc source-val dest-val dest-fun b-w))
	      ((= opcode 7) (subc source-val dest-val dest-fun b-w))
	      ((= opcode 8) (sub source-val dest-val dest-fun b-w))
	      ((= opcode 9) (cmp source-val dest-val b-w))
	      ((= opcode 10) (dadd source-val dest-val dest-fun b-w))
	      ((= opcode 11) (emu-bit source-val dest-val b-w))
	      ((= opcode 12) (bic source-val dest-val dest-fun b-w))
	      ((= opcode 13) (bis source-val dest-val dest-fun b-w))
	      ((= opcode 14) (xor source-val dest-val dest-fun b-w))
	      ((= opcode 15) (emu-and source-val dest-val dest-fun b-w))
	      (t 'nil))))))

(defun mov (val dest-fun)
  (funcall dest-fun val))

(defun add-flags (res b-w)
  (set-c (ldb (byte 1 (+ b-w 8)) res))
  (set-v 0)
  (set-n (negp res b-w))
  (set-z (nullp res)))

(defun add (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (add-flags res b-w)))

(defun addc (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w) (get-c))))
    (funcall dest-fun (trim-bw res))
    (add-flags res b-w)))

(defun subc (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) (get-c) 1))) ;random +1 for bug compat
    (funcall dest-fun (trim-bw res))
    (add-flags res)))

(defun sub (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) 1)))
    (funcall dest-fun (trim-bw res b-w))
    (add-flags res b-w)))

(defun cmp (source-val dest-val b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) 1)))
    (add-flags res b-w)))

(defun dadd (source-val dest-val dest-fun b-w)
  (let ((trim-s (trim-bw source-val b-w))
	(trim-d (trim-bw dest-val b-w)))
    (let ((res (loop for i in (list 0 4 8 12)
		  for c = 0 then (if (>= tmp 10)
				     1
				     0)
		  for tmp = (+ (ldb (byte 4 i) trim-s)
			       (ldb (byte 4 i) trim-d)
			       c)
		  for tmp2 = (if (>= tmp 10)
				 (- tmp 10)
				 tmp)
		  for val = tmp2 then (dpb tmp2 (byte 4 i) val)
		  finally (progn (set-c c)
				 (if (ldb (byte 1 3) tmp)
				     (set-n 1))
				 (return val)))))
      (funcall dest-fun (trim-bw res b-w)))))

(defun emu-bit (source-val dest-val b-w)
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

(defun bic (source-val dest-val dest-fun b-w)
  (let ((res (logand (lognot (trim-bw source-val b-w)) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))))

(defun bis (source-val dest-val dest-fun b-w)
  (funcall dest-fun (trim-bw (logior (trim-bw source-val b-w) (trim-bw dest-val b-w)) b-w)))

(defun xor (source-val dest-val dest-fun b-w)
  (let ((res (logxor (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

(defun emu-and (source-val dest-val dest-fun b-w)
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

;emulator for the MSP430, written to help solve the final level in the Matasano/Square CTF at https://microcorruption.com/

;data structure plans:
;memory to be represented as a large array of bytes
;registers as a much smaller array of words

(defvar *reg* (make-array 16 :element-type '(UNSIGNED-BYTE 16)))

;(setf *reg* (make-array 16 :element-type '(unsigned-byte 16)))

(defvar *mem* (make-array 65536 :element-type '(UNSIGNED-BYTE 8)))

(defun mem (idx)
  (elt *mem* idx))

(defun reg (idx)
  (elt *reg* idx))

(defun flag (idx)
  (ldb (byte 1 idx) (reg 2)))

(defun get-c ()
  (ldb (byte 1 0) (reg 2)))

(defun set-c (int)
  (setf (elt *reg* 2) (dpb int (byte 1 0) (reg 2))))

(defun get-z ()
  (ldb (byte 1 1) (reg 2)))

(defun set-z (int)
  (setf (elt *reg* 2) (dpb int (byte 1 1) (reg 2))))

(defun get-n ()
  (ldb (byte 1 2) (reg 2)))

(defun set-n (int)
  (setf (elt *reg* 2) (dpb int (byte 1 2) (reg 2))))

(defun get-v ()
  (ldb (byte 1 8) (reg 2)))

(defun set-v (int)
  (setf (elt *reg* 2) (dpb int (byte 1 8) (reg 2))))

(defun bytes-to-word (low high)
  (+ (* 256 high) low))

(defun mod+ (&rest args)
  (mod (reduce #'+ args) 65536))

(defun mod* (&rest args)
  (mod (reduce #'* args) 65536))

(defun get-mem (idx)
  (bytes-to-word (mem idx) (mem (+ idx 1))))

(defun set-mem (idx int)
  (let* ((low (mod int 256))
	 (high (/ (- int low) 256)))
    (setf (elt *mem* idx) low
	  (elt *mem* (+ idx 1)) high)))

(defun mem-fun (idx)
  (function (lambda (int) (set-mem idx int))))

(defun reg-fun (idx)
  (function (lambda (int) (setf (elt *reg* idx) int))))

(defun nullp (val)
  (if (= val 0)
      1
      0))

(defun negp (val b-w)
  (ldb (byte 1 (+ b-w 7)) val))

(defun trim-bw (val b-w)
  (ldb (byte (+ b-w 8) 0) val))

(defun instruction-decode (bytes)
  ;bytes to be an arry of 6 bytes, to allow for the longest possible instruction
  ;this is the entry point into the instruction decoding, will eventually call the
  ;appropriate function
  (let ((instn (bytes-to-word (elt bytes 0) (elt bytes 1)))
	(data1 (bytes-to-word (elt bytes 2) (elt bytes 3)))
	(data2 (bytes-to-word (elt bytes 4) (elt bytes 5))))
    (cond ((= (ldb (byte 4 12) instn)) (single-decode instn data1))
	  ((= (ldb (byte 3 13) instn)) (jump-decode instn))
	  ((>= (ldb (byte 4 12) instn)) (double-decode instn data1 data2)))))

(defun address-decode (ad dest data)
  ;decodes an address for a single operand instruction
  ;returns an integer representing the current value and a function that sets the new value
  (if (or (= dest 2) (= dest 3))
      (if (= dest 2)
	  (cond ((= ad 0) (values (reg dest)
				  (reg-fun dest))) ;register mode
		((= ad 1) (values (get-mem data)
				  (mem-fun data))) ;absolute mode
		((= ad 2) (values 4 4)) ;represents a constant, can't set things
		((= ad 3) (values 8 8)) ;as above
		(t 'nil))
	  (cond ((= ad 0) (values 0 0))
		((= ad 1) (values 1 1))
		((= ad 2) (values 2 2))
		((= ad 3) (values 65535 65535))
		(t 'nil)))
      (cond ((= ad 0) (values (reg dest)
			      (reg-fun dest))) ;register mode
	    ((= ad 1) (let ((idx (mod+ (reg dest) data)))
			(values (get-mem idx)
				(mem-fun idx)))) ;index mode
	    ((= ad 2) (values (get-mem (reg dest))
			      (mem-fun (reg dest)))) ;indirect mode
	    ((= ad 3) (progn (incf (elt *reg* dest) 2)
			     (values (get-mem (- (reg dest) 2))
				     (mem-fun (- (reg dest) 2))))) ;indirect mode with increment
	    (t 'nil))))

(defun single-decode (instn data)
  ;decodes and calls a single operand instruction
  (let ((opcode (ldb (byte 3 7) instn))
	(ad (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (* (lbd (byte 1 6) instn))))
    (multiple-value-bind (value set-fun)
	(address-decode ad dest data)
      (cond ((= opcode 0) (rrc value set-fun b-w))
	    ((= opcode 1) (swpb value set-fun))
	    ((= opcode 2) (rra value set-fun b-w))
	    ((= opcode 3) (sxt value set-fun b-w))
	    ((= opcode 4) (psh value set-fun b-w))
	    ((= opcode 5) (call value set-fun b-w))
	    (t 'nil)))))
	      ;RETI isn't implemented because it's not in the CTF

(defun rrc (value set-fun b-w)
  (let ((carry (ldb (byte 1 0) value))
	(tmp (ash (trim-bw value b-w) -1)))
    (let ((res (dpb (get-c) (byte 1 (+ b-w 7)) tmp)))
      (funcall set-fun (trim-bw res b-w))
      (set-v 0)
      (set-c carry)
      (if (= (negp res) 1)
	  (set-n 1))
      (if (= (nullp res) 0)
	  (set-z 0)))))

(defun swpb (value set-fun)
  (let ((low (ldb (byte 8 8) value))
	(high (ldb (byte 8 0) value)))
    (funcall set-fun (+ (* 256 high) low))))

(defun rra (value set-fun b-w)
  (let ((res (ash (trim-bw value b-w) -1)))
    (funcall set-fun (trim-bw res b-w))
    (if (= (negp res) 1)
	(set-n 1))
    (set-v 0)
    (set-z 0)))

(defun sxt (value set-fun)
  (let ((res (dpb (* 255 (ldb (byte 1 7) value)) (byte 8 8) value)))
    (funcall set-fun res)
    (set-c (if (= res 0)
	       0
	       1))
    (set-v 0)
    (set-n (negp res))
    (set-z (nullp res))))

(defun psh (value set-fun b-w)
  (setf (elt *mem* (reg 1)) (trim-bw value b-w))
  (incf (elt *reg* 1) -2))

(defun call (value set-fun)
  (psh (reg 0) 'nil)
  (setf (elt *reg* 0) value))

(defun jump-decode (instn)
  (let ((type (ldb (byte 3 10) instn))
	(offset (ldb (byte 10 0) instn))
	(z (flag 1))
	(c (flag 0))
	(n (flag 2))
	(v (flag 8)))
    (cond ((= type 0) (if (= z 0) (jmp offset)))
	  ((= type 1) (if (= z 1) (jmp offset)))
	  ((= type 2) (if (= c 0) (jmp offset)))
	  ((= type 3) (if (= c 1) (jmp offset)))
	  ((= type 4) (if (= n 1) (jmp offset)))
	  ((= type 5) (if (= n v) (jmp offset)))
	  ((= type 6) (if (/= n v) (jmp offset)))
	  ((= type 7) (jmp offset)))))

(defun jmp (offset)
  (setf (elt *reg* 0) (+ (reg 0) 2
		   (- (ldb (byte 9 0) offset)
		      (* (ldb (byte 1 9) offset) 1024))))
  t)

(defun double-decode (instn data1 data2)
  (let ((opcode (ldb (byte 4 12) instn))
	(source (ldb (byte 4 8) instn))
	(ad (ldb (byte 1 7) instn))
	(as (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (* (ldb (byte 1 6) instn) 8)))
    (multiple-value-bind (dest-val dest-fun)
	(address-decode ad dest data2)
      (multiple-value-bind (source-val source-fun)
	  (address-decode as source data1)
	(cond ((= opcode 4) (mov source-val dest-fun b-w))
	      ((= opcode 5) (add source-val dest-val dest-fun b-w))
	      ((= opcode 6) (addc source-val dest-val dest-fun b-w))
	      ((= opcode 7) (subc source-val dest-val dest-fun b-w))
	      ((= opcode 8) (sub source-val dest-val dest-fun b-w))
	      ((= opcode 9) (cmp source-val dest-val b-w))
	      ((= opcode 10) (dadd source-val dest-val dest-fun b-w))
	      ((= opcode 11) (emu-bit source-val dest-val b-w))
	      ((= opcode 12) (bic source-val dest-val dest-fun b-w))
	      ((= opcode 13) (bis source-val dest-val dest-fun b-w))
	      ((= opcode 14) (xor source-val dest-val dest-fun b-w))
	      ((= opcode 15) (emu-and source-val dest-val dest-fun b-w))
	      (t 'nil))))))

(defun mov (val dest-fun)
  (funcall dest-fun val))

(defun add-flags (res b-w)
  (set-c (ldb (byte 1 (+ b-w 8)) res))
  (set-v 0)
  (set-n (negp res b-w))
  (set-z (nullp res)))

(defun add (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (add-flags res b-w)))

(defun addc (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w) (get-c))))
    (funcall dest-fun (trim-bw res))
    (add-flags res b-w)))

(defun subc (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) (get-c) 1))) ;random +1 for bug compat
    (funcall dest-fun (trim-bw res))
    (add-flags res)))

(defun sub (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) 1)))
    (funcall dest-fun (trim-bw res b-w))
    (add-flags res b-w)))

(defun cmp (source-val dest-val b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) 1)))
    (add-flags res b-w)))

(defun dadd (source-val dest-val dest-fun b-w)
  (let ((trim-s (trim-bw source-val b-w))
	(trim-d (trim-bw dest-val b-w)))
    (let ((res (loop for i in (list 0 4 8 12)
		  for c = 0 then (if (>= tmp 10)
				     1
				     0)
		  for tmp = (+ (ldb (byte 4 i) trim-s)
			       (ldb (byte 4 i) trim-d)
			       c)
		  for tmp2 = (if (>= tmp 10)
				 (- tmp 10)
				 tmp)
		  for val = tmp2 then (dpb tmp2 (byte 4 i) val)
		  finally (progn (set-c c)
				 (if (ldb (byte 1 3) tmp)
				     (set-n 1))
				 (return val)))))
      (funcall dest-fun (trim-bw res b-w)))))

(defun emu-bit (source-val dest-val b-w)
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

(defun bic (source-val dest-val dest-fun b-w)
  (let ((res (logand (lognot (trim-bw source-val b-w)) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))))

(defun bis (source-val dest-val dest-fun b-w)
  (funcall dest-fun (trim-bw (logior (trim-bw source-val b-w) (trim-bw dest-val b-w)) b-w)))

(defun xor (source-val dest-val dest-fun b-w)
  (let ((res (logxor (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

(defun emu-and (source-val dest-val dest-fun b-w)
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

(let ((test2 19))
  (print (lognot test2)));emulator for the MSP430, written to help solve the final level in the Matasano/Square CTF at https://microcorruption.com/

;data structure plans:
;memory to be represented as a large array of bytes
;registers as a much smaller array of words

(defvar *reg* (make-array 16 :element-type '(UNSIGNED-BYTE 16)))

;(setf *reg* (make-array 16 :element-type '(unsigned-byte 16)))

(defvar *mem* (make-array 65536 :element-type '(UNSIGNED-BYTE 8)))

(defun mem (idx)
  (elt *mem* idx))

(defun reg (idx)
  (elt *reg* idx))

(defun flag (idx)
  (ldb (byte 1 idx) (reg 2)))

(defun get-c ()
  (ldb (byte 1 0) (reg 2)))

(defun set-c (int)
  (setf (elt *reg* 2) (dpb int (byte 1 0) (reg 2))))

(defun get-z ()
  (ldb (byte 1 1) (reg 2)))

(defun set-z (int)
  (setf (elt *reg* 2) (dpb int (byte 1 1) (reg 2))))

(defun get-n ()
  (ldb (byte 1 2) (reg 2)))

(defun set-n (int)
  (setf (elt *reg* 2) (dpb int (byte 1 2) (reg 2))))

(defun get-v ()
  (ldb (byte 1 8) (reg 2)))

(defun set-v (int)
  (setf (elt *reg* 2) (dpb int (byte 1 8) (reg 2))))

(defun bytes-to-word (low high)
  (+ (* 256 high) low))

(defun mod+ (&rest args)
  (mod (reduce #'+ args) 65536))

(defun mod* (&rest args)
  (mod (reduce #'* args) 65536))

(defun get-mem (idx)
  (bytes-to-word (mem idx) (mem (+ idx 1))))

(defun set-mem (idx int)
  (let* ((low (mod int 256))
	 (high (/ (- int low) 256)))
    (setf (elt *mem* idx) low
	  (elt *mem* (+ idx 1)) high)))

(defun mem-fun (idx)
  (function (lambda (int) (set-mem idx int))))

(defun reg-fun (idx)
  (function (lambda (int) (setf (elt *reg* idx) int))))

(defun nullp (val)
  (if (= val 0)
      1
      0))

(defun negp (val b-w)
  (ldb (byte 1 (+ b-w 7)) val))

(defun trim-bw (val b-w)
  (ldb (byte (+ b-w 8) 0) val))

(defun instruction-decode (bytes)
  ;bytes to be an arry of 6 bytes, to allow for the longest possible instruction
  ;this is the entry point into the instruction decoding, will eventually call the
  ;appropriate function
  (let ((instn (bytes-to-word (elt bytes 0) (elt bytes 1)))
	(data1 (bytes-to-word (elt bytes 2) (elt bytes 3)))
	(data2 (bytes-to-word (elt bytes 4) (elt bytes 5))))
    (cond ((= (ldb (byte 4 12) instn)) (single-decode instn data1))
	  ((= (ldb (byte 3 13) instn)) (jump-decode instn))
	  ((>= (ldb (byte 4 12) instn)) (double-decode instn data1 data2)))))

(defun address-decode (ad dest data)
  ;decodes an address for a single operand instruction
  ;returns an integer representing the current value and a function that sets the new value
  (if (or (= dest 2) (= dest 3))
      (if (= dest 2)
	  (cond ((= ad 0) (values (reg dest)
				  (reg-fun dest))) ;register mode
		((= ad 1) (values (get-mem data)
				  (mem-fun data))) ;absolute mode
		((= ad 2) (values 4 4)) ;represents a constant, can't set things
		((= ad 3) (values 8 8)) ;as above
		(t 'nil))
	  (cond ((= ad 0) (values 0 0))
		((= ad 1) (values 1 1))
		((= ad 2) (values 2 2))
		((= ad 3) (values 65535 65535))
		(t 'nil)))
      (cond ((= ad 0) (values (reg dest)
			      (reg-fun dest))) ;register mode
	    ((= ad 1) (let ((idx (mod+ (reg dest) data)))
			(values (get-mem idx)
				(mem-fun idx)))) ;index mode
	    ((= ad 2) (values (get-mem (reg dest))
			      (mem-fun (reg dest)))) ;indirect mode
	    ((= ad 3) (progn (incf (elt *reg* dest) 2)
			     (values (get-mem (- (reg dest) 2))
				     (mem-fun (- (reg dest) 2))))) ;indirect mode with increment
	    (t 'nil))))

(defun single-decode (instn data)
  ;decodes and calls a single operand instruction
  (let ((opcode (ldb (byte 3 7) instn))
	(ad (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (* (lbd (byte 1 6) instn))))
    (multiple-value-bind (value set-fun)
	(address-decode ad dest data)
      (cond ((= opcode 0) (rrc value set-fun b-w))
	    ((= opcode 1) (swpb value set-fun))
	    ((= opcode 2) (rra value set-fun b-w))
	    ((= opcode 3) (sxt value set-fun b-w))
	    ((= opcode 4) (psh value set-fun b-w))
	    ((= opcode 5) (call value set-fun b-w))
	    (t 'nil)))))
	      ;RETI isn't implemented because it's not in the CTF

(defun rrc (value set-fun b-w)
  (let ((carry (ldb (byte 1 0) value))
	(tmp (ash (trim-bw value b-w) -1)))
    (let ((res (dpb (get-c) (byte 1 (+ b-w 7)) tmp)))
      (funcall set-fun (trim-bw res b-w))
      (set-v 0)
      (set-c carry)
      (if (= (negp res) 1)
	  (set-n 1))
      (if (= (nullp res) 0)
	  (set-z 0)))))

(defun swpb (value set-fun)
  (let ((low (ldb (byte 8 8) value))
	(high (ldb (byte 8 0) value)))
    (funcall set-fun (+ (* 256 high) low))))

(defun rra (value set-fun b-w)
  (let ((res (ash (trim-bw value b-w) -1)))
    (funcall set-fun (trim-bw res b-w))
    (if (= (negp res) 1)
	(set-n 1))
    (set-v 0)
    (set-z 0)))

(defun sxt (value set-fun)
  (let ((res (dpb (* 255 (ldb (byte 1 7) value)) (byte 8 8) value)))
    (funcall set-fun res)
    (set-c (if (= res 0)
	       0
	       1))
    (set-v 0)
    (set-n (negp res))
    (set-z (nullp res))))

(defun psh (value set-fun b-w)
  (setf (elt *mem* (reg 1)) (trim-bw value b-w))
  (incf (elt *reg* 1) -2))

(defun call (value set-fun)
  (psh (reg 0) 'nil)
  (setf (elt *reg* 0) value))

(defun jump-decode (instn)
  (let ((type (ldb (byte 3 10) instn))
	(offset (ldb (byte 10 0) instn))
	(z (flag 1))
	(c (flag 0))
	(n (flag 2))
	(v (flag 8)))
    (cond ((= type 0) (if (= z 0) (jmp offset)))
	  ((= type 1) (if (= z 1) (jmp offset)))
	  ((= type 2) (if (= c 0) (jmp offset)))
	  ((= type 3) (if (= c 1) (jmp offset)))
	  ((= type 4) (if (= n 1) (jmp offset)))
	  ((= type 5) (if (= n v) (jmp offset)))
	  ((= type 6) (if (/= n v) (jmp offset)))
	  ((= type 7) (jmp offset)))))

(defun jmp (offset)
  (setf (elt *reg* 0) (+ (reg 0) 2
		   (- (ldb (byte 9 0) offset)
		      (* (ldb (byte 1 9) offset) 1024))))
  t)

(defun double-decode (instn data1 data2)
  (let ((opcode (ldb (byte 4 12) instn))
	(source (ldb (byte 4 8) instn))
	(ad (ldb (byte 1 7) instn))
	(as (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (* (ldb (byte 1 6) instn) 8)))
    (multiple-value-bind (dest-val dest-fun)
	(address-decode ad dest data2)
      (multiple-value-bind (source-val source-fun)
	  (address-decode as source data1)
	(cond ((= opcode 4) (mov source-val dest-fun b-w))
	      ((= opcode 5) (add source-val dest-val dest-fun b-w))
	      ((= opcode 6) (addc source-val dest-val dest-fun b-w))
	      ((= opcode 7) (subc source-val dest-val dest-fun b-w))
	      ((= opcode 8) (sub source-val dest-val dest-fun b-w))
	      ((= opcode 9) (cmp source-val dest-val b-w))
	      ((= opcode 10) (dadd source-val dest-val dest-fun b-w))
	      ((= opcode 11) (emu-bit source-val dest-val b-w))
	      ((= opcode 12) (bic source-val dest-val dest-fun b-w))
	      ((= opcode 13) (bis source-val dest-val dest-fun b-w))
	      ((= opcode 14) (xor source-val dest-val dest-fun b-w))
	      ((= opcode 15) (emu-and source-val dest-val dest-fun b-w))
	      (t 'nil))))))

(defun mov (val dest-fun)
  (funcall dest-fun val))

(defun add-flags (res b-w)
  (set-c (ldb (byte 1 (+ b-w 8)) res))
  (set-v 0)
  (set-n (negp res b-w))
  (set-z (nullp res)))

(defun add (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (add-flags res b-w)))

(defun addc (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w) (get-c))))
    (funcall dest-fun (trim-bw res))
    (add-flags res b-w)))

(defun subc (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) (get-c) 1))) ;random +1 for bug compat
    (funcall dest-fun (trim-bw res))
    (add-flags res)))

(defun sub (source-val dest-val dest-fun b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) 1)))
    (funcall dest-fun (trim-bw res b-w))
    (add-flags res b-w)))

(defun cmp (source-val dest-val b-w)
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) 1)))
    (add-flags res b-w)))

(defun dadd (source-val dest-val dest-fun b-w)
  (let ((trim-s (trim-bw source-val b-w))
	(trim-d (trim-bw dest-val b-w)))
    (let ((res (loop for i in (list 0 4 8 12)
		  for c = 0 then (if (>= tmp 10)
				     1
				     0)
		  for tmp = (+ (ldb (byte 4 i) trim-s)
			       (ldb (byte 4 i) trim-d)
			       c)
		  for tmp2 = (if (>= tmp 10)
				 (- tmp 10)
				 tmp)
		  for val = tmp2 then (dpb tmp2 (byte 4 i) val)
		  finally (progn (set-c c)
				 (if (ldb (byte 1 3) tmp)
				     (set-n 1))
				 (return val)))))
      (funcall dest-fun (trim-bw res b-w)))))

(defun emu-bit (source-val dest-val b-w)
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

(defun bic (source-val dest-val dest-fun b-w)
  (let ((res (logand (lognot (trim-bw source-val b-w)) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))))

(defun bis (source-val dest-val dest-fun b-w)
  (funcall dest-fun (trim-bw (logior (trim-bw source-val b-w) (trim-bw dest-val b-w)) b-w)))

(defun xor (source-val dest-val dest-fun b-w)
  (let ((res (logxor (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))

(defun emu-and (source-val dest-val dest-fun b-w)
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun (trim-bw res b-w))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res))
    (set-c (nullp res))))