#include "spinlock.h"



void init_spinlock(spinlock_t* sp){

	#ifdef LEON4_IN_PROGRESS
		sp->mutex = 0;
	#endif

	#ifdef RTEMS

		rtems_status_code status;

		/* Create/verify synchronisation semaphore */
		status = rtems_semaphore_create(
			rtems_build_name ('S', 'E', 'M', '2'),
			1,
			RTEMS_GLOBAL 
			| RTEMS_BINARY_SEMAPHORE 
			| RTEMS_FIFO,
			1,
			&(sp->mutex)
		);

		if(status!=RTEMS_SUCCESSFUL){
			locked_printf("\n**rtems_semaphore_create not sucessfull**\n");
			exit(1);
		}

	#endif
	
}


int compareAndSwap(uint32_t* mutex){

	#ifdef LEON4_IN_PROGRESS

		uint32_t* mem = mutex;

		register __typeof (*(mem)) __acev_newval __asm__ ("%g6");
		register __typeof (mem) __acev_mem __asm__ ("%g1") = (mem);
		register __typeof (*(mem)) __acev_oldval __asm__ ("%g5");

		__acev_oldval = 0;
		__acev_newval = 1;
		
		/**
		 *  From https://cr.yp.to/2005-590/sparcv9.pdf
		 *
		 * Compare-and-swap is an atomic operation which compares a value in a processor register to a
		 * value in memory, and, if and only if they are equal, swaps the value in memory with the value in a
		 * second processor register. Both 32-bit (CASA) and 64-bit (CASXA) operations are provided. The
		 * compare-and-swap operation is atomic in the sense that once begun, no other processor can access
		 * the memory location specified until the compare has completed and the swap (if any) has also
		 * completed and is potentially visible to all other processors in the system.
		 *
		 * Note: .word 0xcde05005 is cas [%g1], %g5, %g6.  Can't use cas here though,
		 * because as will then mark the object file as V8+ arch.  */
		__asm__ __volatile__ (  ".word 0xcde05005"
							: "+r"(__acev_newval), "=m" (*__acev_mem)
							: "r" (__acev_oldval), "m" (*__acev_mem),
							  "r" (__acev_mem) : "memory");

		return __acev_newval;
	#endif

	#ifdef RTEMS
		return 0;
	#endif
}




#ifdef LEON4_IN_PROGRESS

	int lockMutex(volatile  uint32_t* mutexRegister){

		// spin until the spinlock is released
		while(compareAndSwap((uint32_t*)&mutexRegister) == 1) {
		}

		return 0;

	}


	int unlockMutex(volatile  uint32_t* mutexRegister){

		// unlock mutex
		(*mutexRegister) = 0U;

		return 0;

	}

#endif

#ifdef RTEMS

	int lockMutex(spinlock_t sp){

		rtems_status_code  status;

		status = rtems_semaphore_obtain( sp.mutex, RTEMS_NO_WAIT, 0 );

		

		while (status != RTEMS_SUCCESSFUL ){
			status = rtems_semaphore_obtain( sp.mutex, RTEMS_NO_WAIT, 0 );
		}

		return 0;
		
	}


	int unlockMutex(spinlock_t sp){

		return rtems_semaphore_release( sp.mutex );

	}

#endif


int lock_spinlock(spinlock_t sp){

	#ifdef LEON4_IN_PROGRESS

		lockMutex(&(sp.mutex));

	#endif

	#ifdef RTEMS

		lockMutex(sp);

	#endif

	return 0;
}


int unlock_spinlock(spinlock_t sp){

	#ifdef LEON4_IN_PROGRESS

		unlockMutex(&(sp.mutex));

	#endif

	#ifdef RTEMS

		unlockMutex(sp);

	#endif

	return 0;
}