// Copyright (c) 1998-2009 Nokia Corporation and/or its subsidiary(-ies).

// All rights reserved.

// This component and the accompanying materials are made available

// under the terms of the License "Eclipse Public License v1.0"

// which accompanies this distribution, and is available

// at the URL "http://www.eclipse.org/legal/epl-v10.html".

//

// Initial Contributors:

// Nokia Corporation - initial contribution.

//

// Contributors:

//

// Description:

// e32\klib\arm\cbma.cia

// Machine coded bitmap allocator for ARM

// This file is directly included in the test harness t_tbma

// 

//

#include <kernel/kbma.h>

#include <cpudefs.h>

#include <e32cia.h>

#ifdef TBMA_TEST_CODE

#include <e32atomics.h>

#ifdef __MARM__

#define __TBMA_MACHINE_CODED__

#endif

#else

#include <kernel/kern_priv.h>

#endif

#ifdef __TBMA_MACHINE_CODED__

extern void TBmaFault(TInt aLine);

#define	ASM_FAULT_LINE(x)	asm("ldr r0, [pc] "); asm("b " CSM_Z9TBmaFaulti ); asm(".word %a0" : : "i" (x));

#define	ASM_FAULT()		ASM_FAULT_LINE(__LINE__)

#ifndef __EABI_CTORS__

/**	Construct a new TBitMapAllocator object

	@param	aSize The number of bit positions required

	@param	aState	TRUE if all bit positions initially free

					FALSE if all bit positions initially allocated

 */

EXPORT_C __NAKED__ TBitMapAllocator::TBitMapAllocator(TInt /*aSize*/, TBool /*aState*/)

	{

	asm("cmp r1, #0 ");

	asm("ble 0f ");

	asm("cmp r2, #0 ");

	asm("movne r2, r1 ");				// if aState r2=aSize else r2=0

	asm("str r2, [r0, #0] ");			// iAvail=aState?aSize:0

	asm("add r12, r0, #12 ");			// r12=&iMap[0]

	asm("str r1, [r0, #8] ");			// iSize=r1

	asm("add r3, r1, #31 ");

	asm("bic r3, r3, #31 ");			// r3=aSize rounded up to multiple of 32

	asm("sub r3, r3, #32 ");			// r3=32*(number of map words-1)

	asm("addeq r12, r12, r3, lsr #3 ");	// if !aState r12=&iMap[nmapw-1]

	asm("str r12, [r0, #4] ");			// iCheckFirst=aState?&iMap[0]:&iMap[nmapw-1]

	asm("mvnne r2, #0 ");				// if aState r2=0xffffffff else r2=0

	asm("add r12, r0, #12 ");			// r12=&iMap[0]

	asm("1: ");

	asm("str r2, [r12], #4 ");			// fill map

	asm("subs r1, r1, #32 ");

	asm("bhi 1b ");

	asm("rsbne r1, r1, #0 ");			// if aSize not a multiple of 32, r1=number of tail bits to clear

	asm("movne r2, r2, lsl r1 ");		// clear unused bits

	asm("strne r2, [r12, #-4] ");

	__JUMP(,lr);

	asm("0: ");

	ASM_FAULT();

	}

#endif

/**	Allocate the next available bit position

	@return	Number of position allocated, -1 if all positions occupied

 */

EXPORT_C __NAKED__ TInt TBitMapAllocator::Alloc()

	{

	asm("ldmia r0, {r1,r2} ");			// r1=available, r2=check first address

	asm("subs r1, r1, #1 ");			// decrement free count

	asm("mvnmi r0, #0 ");				// if none free, return with r0=-1

	__JUMP(mi,lr);

	asm("str r1, [r0] ");				// store decremented free count

	asm("alloc_1: ");

	asm("ldr r3, [r2], #4 ");			// check word

	asm("cmp r3, #0 ");					// any free entries?

	asm("beq alloc_1 ");				// if not, check next word

#ifdef __CPU_ARM_HAS_CLZ

	CLZ(12, 3);

#else

	asm("mov ip, #0 ");

	asm("cmp r3, #0x00010000 ");		// ip=number of leading zeros in r3

	asm("movlo r3, r3, lsl #16 ");

	asm("addlo ip, ip, #16 ");

	asm("cmp r3, #0x01000000 ");

	asm("movlo r3, r3, lsl #8 ");

	asm("addlo ip, ip, #8 ");

	asm("cmp r3, #0x10000000 ");

	asm("movlo r3, r3, lsl #4 ");

	asm("addlo ip, ip, #4 ");

	asm("cmp r3, #0x40000000 ");

	asm("movlo r3, r3, lsl #2 ");

	asm("addlo ip, ip, #2 ");

	asm("cmp r3, #0x80000000 ");

	asm("addlo ip, ip, #1 ");

#endif

	asm("ldr r3, [r2, #-4]! ");

	asm("mov r1, #0x80000000 ");

	asm("bic r3, r3, r1, lsr ip ");		// clear bit in allocator word

	asm("str r3, [r2] ");

	asm("str r2, [r0, #4] ");			// update check first address

	asm("sub r0, r2, r0 ");

	asm("sub r0, r0, #12 ");			// r0=offset of word from iMap[0]

	asm("adds r0, ip, r0, lsl #3 ");	// multiply by 8 and add bit position

	__JUMP(,lr);

	}

/**	Free the specified bit position

	@param	aPos Number of bit position to be freed; must be currently allocated.

 */

EXPORT_C __NAKED__ void TBitMapAllocator::Free(TInt /*aPos*/)

	{

	asm("ldr r3, [r0, #8] ");			// r3=iSize

	asm("mov r2, r1, lsr #5 ");			// r2=word index

	asm("add r2, r0, r2, lsl #2 ");		// r2=address of word-12

	asm("cmp r1, r3 ");

	asm("bhs free_error ");

	asm("and ip, r1, #0x1f ");			// ip=bit number in word

	asm("ldr r3, [r2, #12]! ");			// r3=allocator word

	asm("mov r1, #0x80000000 ");

	asm("tst r3, r1, lsr ip ");			// test bit

	asm("bne free_error ");				// if already free, error

	asm("orr r3, r3, r1, lsr ip ");		// set free bit

	asm("str r3, [r2] ");

	asm("ldmia r0, {r1,r3} ");			// r1=available count, r3=first free address

	asm("cmp r1, #1 ");					// check original free count

	asm("add r1, r1, #1 ");				// increment available count

	asm("str r1, [r0, #0] ");

	asm("cmpcs r2, r3 ");				// compare word address with first free

	asm("strcc r2, [r0, #4] ");			// if lower, update first free

	__JUMP(,lr);

	asm("free_error: ");

	ASM_FAULT();

	}

/**	Allocate a specific range of bit positions

	Specified range must lie within the total range for this allocator and all

	the positions must currently be free.

	@param	aStart	First position to allocate

	@param	aLength	Number of consecutive positions to allocate, must be >0

 */

EXPORT_C __NAKED__ void TBitMapAllocator::Alloc(TInt /*aStart*/, TInt /*aLength*/)

	{

	asm("ldr ip, [r0, #8] ");

	asm("str lr, [sp, #-4]! ");

	asm("adds lr, r1, r2 ");

	asm("bcs 0f ");

	asm("cmp lr, ip ");

	asm("bhi 0f ");

	asm("mov r3, r1, lsr #5 ");

	asm("ldr ip, [r0] ");

	asm("and r1, r1, #0x1f ");

	asm("add r3, r0, r3, lsl #2 ");

	asm("sub ip, ip, r2 ");				// reduce free count

	asm("str ip, [r0] ");

	asm("add ip, r2, r1 ");

	asm("cmp ip, #32 ");

	asm("bhi 1f ");

	asm("mvn ip, #0 ");

	asm("ldr r0, [r3, #12]! ");

	asm("mvn ip, ip, lsr r2 ");

	asm("mov ip, ip, lsr r1 ");

	asm("orr lr, r0, ip ");

	asm("cmp lr, r0 ");

	asm("bne 0f ");

	asm("bic r0, r0, ip ");

	asm("str r0, [r3] ");

	asm("ldr pc, [sp], #4 ");

	asm("1: ");

	asm("add r3, r3, #12 ");

	asm("mvn r2, #0 ");

	asm("mov r2, r2, lsr r1 ");

	asm("2: ");

	asm("ldr r1, [r3] ");

	asm("orr lr, r1, r2 ");

	asm("cmp lr, r1 ");

	asm("bne 0f ");

	asm("bic r1, r1, r2 ");

	asm("str r1, [r3], #4 ");

	asm("mvn r2, #0 ");

	asm("subs ip, ip, #32 ");

	asm("ldrls pc, [sp], #4 ");

	asm("cmp ip, #32 ");

	asm("mvncc r2, r2, lsr ip ");

	asm("b 2b ");

	asm("0: ");

	ASM_FAULT();

	}

/**	Free a specific range of bit positions

	Specified range must lie within the total range for this allocator and all

	the positions must currently be allocated.

	@param	aStart	First position to free

	@param	aLength	Number of consecutive positions to free, must be >0

 */

EXPORT_C __NAKED__ void TBitMapAllocator::Free(TInt /*aStart*/, TInt /*aLength*/)

	{

	asm("ldr ip, [r0, #8] ");

	asm("str lr, [sp, #-4]! ");

	asm("adds lr, r1, r2 ");

	asm("bcs 0f ");

	asm("cmp lr, ip ");

	asm("bhi 0f ");

	asm("mov r3, r1, lsr #5 ");

	asm("and r1, r1, #0x1f ");

	asm("add r3, r0, r3, lsl #2 ");

	asm("ldmia r0, {ip,lr} ");			// ip=free count, lr=first check addr

	asm("add r3, r3, #12 ");

	asm("cmp ip, #1 ");					// check original free count

	asm("add ip, ip, r2 ");				// increase free count

	asm("cmpcs r3, lr ");				// if none free originally, always update address

	asm("str ip, [r0] ");

	asm("strcc r3, [r0, #4] ");			// update first check addr if necessary

	asm("add lr, r2, r1 ");

	asm("cmp lr, #32 ");

	asm("bhi 1f ");

	asm("mvn lr, #0 ");

	asm("ldr r0, [r3] ");

	asm("mvn lr, lr, lsr r2 ");

	asm("mov lr, lr, lsr r1 ");

	asm("tst r0, lr ");

	asm("bne 0f ");

	asm("orr r0, r0, lr ");

	asm("str r0, [r3] ");

	asm("ldr pc, [sp], #4 ");

	asm("1: ");

	asm("mvn r2, #0 ");

	asm("mov r2, r2, lsr r1 ");

	asm("2: ");

	asm("ldr r1, [r3] ");

	asm("tst r1, r2 ");

	asm("bne 0f ");

	asm("orr r1, r1, r2 ");

	asm("str r1, [r3], #4 ");

	asm("mvn r2, #0 ");

	asm("subs lr, lr, #32 ");

	asm("ldrls pc, [sp], #4 ");

	asm("cmp lr, #32 ");

	asm("mvncc r2, r2, lsr lr ");

	asm("b 2b ");

	asm("0: ");

	ASM_FAULT();

	}

/**	Free a specific range of bit positions

	Specified range must lie within the total range for this allocator but it is

	not necessary that all the positions are currently allocated.

	@param	aStart	First position to free

	@param	aLength	Number of consecutive positions to free, must be >0

 */

EXPORT_C __NAKED__ void TBitMapAllocator::SelectiveFree(TInt /*aStart*/, TInt /*aLength*/)

	{

	asm("ldr r3, [r0, #8] ");

	asm("stmfd sp!, {r4-r8,lr} ");

	asm("adds lr, r1, r2 ");

	asm("bcs 0f ");

	asm("cmp lr, r3 ");

	asm("bhi 0f ");

	asm("mov r7, r0 ");					// r7 -> this

	asm("mov r4, r1, lsr #5 ");

	asm("and r1, r1, #0x1f ");

	asm("ldmia r7, {r6,r8} ");			// r6=free count, r8=first check addr

	asm("add r4, r7, r4, lsl #2 ");

	asm("add r4, r4, #12 ");

	asm("cmp r6, #1 ");					// check original free count

	asm("add r6, r6, r2 ");				// r6=new free count assuming no positions already free

	asm("cmpcs r4, r8 ");				// if none free originally, always update address

	asm("strcc r4, [r7, #4] ");			// update first check addr if necessary

	asm("add r8, r2, r1 ");

	asm("cmp r8, #32 ");

	asm("bhi sfree_cross_bdry ");

	asm("mvn r8, #0 ");

	asm("mvn r8, r8, lsr r2 ");

	asm("mov r8, r8, lsr r1 ");

	asm("ldr r1, [r4] ");

	asm("ands r0, r1, r8 ");			// r0 has 1's in positions which are already free

	asm("orr r1, r1, r8 ");

	asm("str r1, [r4] ");				// store new bit mask

	asm("beq sfree_0 ");				// if none were already free, finished

	asm("bl " CSM_CFUNC(__e32_bit_count_32));

	asm("sub r6, r6, r0 ");

	asm("sfree_0: ");

	asm("str r6, [r7] ");				// store free count

	asm("ldmfd sp!, {r4-r8,pc} ");			// return

	asm("sfree_cross_bdry: ");

	asm("mvn r5, #0 ");

	asm("mov r5, r5, lsr r1 ");

	asm("sfree_cross_bdry_1: ");

	asm("ldr r1, [r4] ");				// original bit mask

	asm("ands r0, r1, r5 ");			// r0 has 1's in bit positions which are already free

	asm("orr r1, r1, r5 ");

	asm("str r1, [r4], #4 ");			// store new bit mask

	asm("beq sfree_2 ");				// skip if none were already free

	asm("bl " CSM_CFUNC(__e32_bit_count_32));

	asm("sub r6, r6, r0 ");

	asm("sfree_2: ");

	asm("mvn r5, #0 ");

	asm("subs r8, r8, #32 ");

	asm("bls sfree_0 ");

	asm("cmp r8, #32 ");

	asm("mvncc r5, r5, lsr r8 ");

	asm("b sfree_cross_bdry_1 ");

	asm("0: ");

	ASM_FAULT();

	}

/**	Allocates a specific range of bit positions.

	The specified range must lie within the total range for this allocator but it is

	not necessary that all the positions are currently free.

	@param	aStart	First position to allocate.

	@param	aLength	Number of consecutive positions to allocate, must be >0.

	@return The number of previously free positions that were allocated.

 */

EXPORT_C __NAKED__ TUint TBitMapAllocator::SelectiveAlloc(TInt /*aStart*/, TInt /*aLength*/)

	{

	asm("ldr r3, [r0, #%a0] " : : "i" _FOFF(TBitMapAllocator, iSize));	// r3->iSize

	asm("stmfd sp!, {r4-r8,lr} ");

	asm("adds r4, r1, r2 ");			// r4 = aStart + aLength

	asm("bcs 0f ");						// if (aStart + aLength < aStart)

	asm("cmp r4, r3 ");					// if (aStart + aLength > iSize)

	asm("bhi 0f ");

	asm("mov r7, r0 ");					// r7 = this

	asm("mov r4, r1, lsr #5 ");			// r4 = wix = aStart >> 5

	asm("and r1, r1, #0x1f ");			// r1 = sbit = aStart & 31

	asm("ldr r6, [r7, #%a0] " : : "i" _FOFF(TBitMapAllocator, iAvail));	// r6 = iAvail

	asm("add r4, r7, r4, lsl #2 ");	

	asm("add r4, r4, #%a0 " : : "i" _FOFF(TBitMapAllocator, iMap));		// r4 = iMap + wix

	asm("sub r6, r6, r2 ");				// r6 = iAvail -= aLength

	asm("add r5, r2, r1 ");				// r5 = ebit =  sbit + aLength

	asm("mvn r0, #0 ");

	asm("mvn r0, r0, lsr r1 ");			// r0 = b = ~(0xffffffff >> sbit)

	asm("cmp r5, #32 ");

	asm("mov r8, r2");					// r8 = aLength

	asm("bhi salloc_cross_bdry ");		// branch if (ebit >=32)

	asm("mvn r5, #0 ");					// r5 = 0xffffffff

	asm("mov r5, r5, lsr r8 ");			// r5 >> aLength

	asm("mov r5, r5, lsr r1 ");			// r5 >> sbit

	asm("orr r5, r5, r0 ");				// r5 = b = r0 | r5

	asm("ldr r0, [r4] ");				// r0 = w = *pW

	asm("and r1, r0, r5 ");				// r1 = w & b, clear the positions to be allocated

	asm("str r1, [r4] ");				// *pW = r1, store new bit map word.

	asm("mvn r0, r0");					// r0 = ~w

	asm("mvn r5, r5");					// r5 = ~b

	asm("and r0, r0, r5");				// r0 = ~w & ~b

	asm("bl " CSM_CFUNC(__e32_bit_count_32));

	asm("add r6, r6, r0 ");				// r6 = iAvail + allocated

	asm("sub r0, r8, r0 ");				// return aLength - allocated

	asm("str r6, [r7] ");				// iAvail += allocated, store free count

	asm("ldmfd sp!, {r4-r8,pc} ");		// return

	asm("salloc_cross_bdry: ");			// r0 = b, r8 = aLength, r7 = this, r5 = ebit

	asm("ldr r2, [r4] ");				// r2 = w = *pW

	asm("and r1, r2, r0 ");				// r1 = w & b

	asm("str r1, [r4], #4 ");			// *pW++ = r1, store new bit mask

	asm("mvn r2, r2");					// r2 = ~w

	asm("mvn r0, r0");					// r0 = ~b

	asm("and r0, r0, r2");				// r0 = ~w & ~b

	asm("bl " CSM_CFUNC(__e32_bit_count_32));

	asm("add r6, r6, r0 ");				// r6 = iAvail += allocated

	asm("sub r8, r8, r0 ");				// r8 = aLength -= allocated

	asm("subs r5, r5, #32 ");			// r5 = ebit -= 32

	asm("bls salloc_return ");			// ebit < 0 so return.

	asm("cmp r5, #32 ");				// if (ebit < 32) {

	asm("mvnlt r0, #0 ");

	asm("movlt r0, r0, lsr r5 ");		// r0 = 0xffffffff >> ebit }

	asm("movge r0, #0 ");				// if (ebit >= 32) r0 = b = 0

	asm("b salloc_cross_bdry ");

	asm("salloc_return: ");

	asm("str r6, [r7] ");				// iAvail += allocated, store free count

	asm("mov r0, r8 ");					// return aLength

	asm("ldmfd sp!, {r4-r8,pc} ");		// return

	asm("0: ");

	ASM_FAULT();

	}

/**	Tests if a specific range of bit positions are all free

	Specified range must lie within the total range for this allocator.

	@param	aStart	First position to check

	@param	aLength	Number of consecutive positions to check, must be >0

	@return	FALSE if all positions free, TRUE if at least one is occupied.

 */

EXPORT_C __NAKED__ TBool TBitMapAllocator::NotFree(TInt /*aStart*/, TInt /*aLength*/) const

	{

	// Inverse logic - returns 0 if all positions free, nonzero otherwise

	asm("ldr r3, [r0, #8] ");

	asm("adds ip, r1, r2 ");

	asm("bcs 0f ");

	asm("cmp ip, r3 ");

	asm("bhi 0f ");

	asm("mov r3, r1, lsr #5 ");

	asm("and r1, r1, #0x1f ");

	asm("add r3, r0, r3, lsl #2 ");

	asm("add ip, r2, r1 ");

	asm("add r3, r3, #12 ");

	asm("cmp ip, #32 ");

	asm("bhi 1f ");

	asm("mvn ip, #0 ");

	asm("ldr r0, [r3] ");

	asm("mvn ip, ip, lsr r2 ");

	asm("mov ip, ip, lsr r1 ");

	asm("eor r0, r0, ip ");

	asm("ands r0, r0, ip ");

	__JUMP(,lr);

	asm("1: ");

	asm("mvn r2, #0 ");

	asm("mov r2, r2, lsr r1 ");

	asm("2: ");

	asm("ldr r1, [r3], #4 ");

	asm("eor r0, r1, r2 ");

	asm("ands r0, r0, r2 ");

	__JUMP(ne,lr);

	asm("mvn r2, #0 ");

	asm("subs ip, ip, #32 ");

	__JUMP(ls,lr);

	asm("cmp ip, #32 ");

	asm("mvncc r2, r2, lsr ip ");

	asm("b 2b ");

	asm("0: ");

	ASM_FAULT();

	}

/**	Tests if a specific range of bit positions are all occupied

	Specified range must lie within the total range for this allocator.

	@param	aStart	First position to check

	@param	aLength	Number of consecutive positions to check, must be >0

	@return	FALSE if all positions occupied, TRUE if at least one is free.

 */

EXPORT_C __NAKED__ TBool TBitMapAllocator::NotAllocated(TInt /*aStart*/, TInt /*aLength*/) const

	{

	// Inverse logic - returns 0 if all positions allocated, nonzero otherwise

	asm("ldr r3, [r0, #8] ");

	asm("adds ip, r1, r2 ");

	asm("bcs 0f ");

	asm("cmp ip, r3 ");

	asm("bhi 0f ");

	asm("mov r3, r1, lsr #5 ");

	asm("and r1, r1, #0x1f ");

	asm("add r3, r0, r3, lsl #2 ");

	asm("add ip, r2, r1 ");

	asm("add r3, r3, #12 ");

	asm("cmp ip, #32 ");

	asm("bhi 1f ");

	asm("mvn ip, #0 ");

	asm("ldr r0, [r3] ");

	asm("mvn ip, ip, lsr r2 ");

	asm("ands r0, r0, ip, lsr r1 ");

	__JUMP(,lr);

	asm("1: ");

	asm("mvn r2, #0 ");

	asm("mov r2, r2, lsr r1 ");

	asm("2: ");

	asm("ldr r1, [r3], #4 ");

	asm("ands r0, r1, r2 ");

	__JUMP(ne,lr);

	asm("mvn r2, #0 ");

	asm("subs ip, ip, #32 ");

	__JUMP(ls,lr);

	asm("cmp ip, #32 ");

	asm("mvncc r2, r2, lsr ip ");

	asm("b 2b ");

	asm("0: ");

	ASM_FAULT();

	}

/**	Allocate up to a specified number of available bit positions

	The allocated positions are not required to bear any relationship to

	each other.

	If the number of free positions is less than the number requested,

	allocate all currently free positions.

	@param	aLength	Maximum number of positions to allocate.

	@param	aList	Pointer to memory area where allocated bit numbers should be stored.

	@return	The number of positions allocated

 */

EXPORT_C __NAKED__ TInt TBitMapAllocator::AllocList(TInt /*aLength*/, TInt* /*aList*/)

	{

	asm("ldmia r0, {r3,ip} ");				// r3=iAvail, ip=first check word

	asm("stmfd sp!, {r4-r5,lr} ");

	asm("cmp r1, r3 ");