/*
the implementation of these semaphores is a trade-off between
latency and bus-hogging. If a cycle on the XP takes 4 ticks, and
3 processors are spinning on a semaphore, then we will use up 12 ticks
per semaphore interval, so setting the interval to 120 ticks uses up just
10% of the bus, and puts semaphore latency at around 2.4uS
*/

#include "stdio.h"
#include "stdlib.h"
#include "i860sem.h"

extern void *newBytes(int);

void SemWait ( Semaphore *sem, int index )
{
  extern void bla(int);

  int wait, i;

  while (1) {
    wait=0;
    sem->lock[index]=1;

    #if (LOCKS_PER_SEM==4)
    wait+=sem->lock[0];
    wait+=sem->lock[1];
    wait+=sem->lock[2];
    wait+=sem->lock[3];
    #else
    for (i=0; i<LOCKS_PER_SEM; i++ )
      wait+=sem->lock[i];
    #endif

    if (wait == 1) return;

    sem->lock[index]=0;
    bla(80+(index<<4));
  }
}

void SemSignal ( Semaphore *sem, int index )
{
  sem->lock[index]=0;
}

void InitSem ( Semaphore *sem )
{
  int i;

  for (i=0; i<LOCKS_PER_SEM; i++ )
    sem->lock[i]=0;
}

static Semaphore *__freeSem=NULL;
static int __sems_free=0;


Semaphore *nextSem (void)
{
  Semaphore *ret=__freeSem;

  if (__sems_free==0) {
    __freeSem=(Semaphore *) newBytes(4096);
    if (__freeSem==NULL) {
      printf ("Failed to allocate semaphore\n" );
      return NULL;
    }
    __sems_free=4096/sizeof(Semaphore);

    ret=__freeSem;
  }

  __sems_free--;
  __freeSem++;

  return ret;
}