/**
 * Placed at the end of all monitor functions, this leaves a monitor.
 */
void monitor_leave(monitor_t *monitor) {
  if (monitor->next_count > 0) {
    semaphore_signal(monitor->sem_id, NEXT_SEM);
  } else {
    semaphore_signal(monitor->sem_id, MUTEX_SEM);
  }
}

static int vsync_isr(void *data)
{

  stm_display_output_handle_interrupts(pOutput);

  topfields++;

  if((topfields % framerate) == 0)
  {
    semaphore_signal(framerate_sem);
  }

  semaphore_signal(frameupdate_sem);

  if(pHDMI)
  {
    stm_display_status_t oldDisplayStatus = currentDisplayStatus;

    stm_display_output_get_status(pHDMI, &currentDisplayStatus);

    if(hotplug_poll_pio)
    {
      unsigned char hotplugstate = *hotplug_poll_pio & hotplug_pio_pin;
      if(currentDisplayStatus == STM_DISPLAY_DISCONNECTED)
      {
        /*
         * If the device has just been plugged in, flag that we now need to
         * start the output.
         */
        if(hotplugstate != 0)
        {
          currentDisplayStatus = STM_DISPLAY_NEEDS_RESTART;
          stm_display_output_set_status(pHDMI, currentDisplayStatus);
        }
      }
      else
      {
        /*
         * We may either be waiting for the output to be started, or already
         * started, so only change the state if the device has now been
         * disconnected.
         */
        if(hotplugstate == 0)
        {
          currentDisplayStatus = STM_DISPLAY_DISCONNECTED;
          stm_display_output_set_status(pHDMI, currentDisplayStatus);
        }
      }
    }

    if(oldDisplayStatus != currentDisplayStatus)
    {
      semaphore_signal(hotplug_sem);
    }
  }

  return OS21_SUCCESS;
}

void queue_push(Queue *queue, int item) {
  semaphore_wait(queue->spaces);
  semaphore_wait(queue->mutex);

  queue->array[queue->next_in] = item;
  queue->next_in = queue_incr(queue, queue->next_in);

  semaphore_signal(queue->mutex);
  semaphore_signal(queue->items);
}

void *
start_routine(void *id)
{
    semaphore_signal(g_sem[1]);
    fprintf(stderr, "thread: %lx about to decrement semaphore count\n", id);
    semaphore_wait(g_sem[0]);
    fprintf(stderr, "thread: %lx succeeded in decrementing semaphore count\n", id);
    semaphore_signal(g_sem[1]);
    return NULL;
}

/**
 * Waits on the specified condition variable in the monitor.
 * cond is the zero based index of the condition variable which
 * must be less than the number of condition variables the monitor
 * was created with.
 */
void monitor_cond_wait(monitor_t *monitor, int cond) {
  monitor->x_count[cond]++;

  if (monitor->next_count > 0) {
    semaphore_signal(monitor->sem_id, NEXT_SEM);
  } else {
    semaphore_signal(monitor->sem_id, MUTEX_SEM);
  }

  semaphore_wait(monitor->sem_id, MONITOR_SEM_COUNT + cond);
  monitor->x_count[cond]--;
}

int queue_pop(Queue *queue) {
  semaphore_wait(queue->items);
  semaphore_wait(queue->mutex);
  
  int item = queue->array[queue->next_out];
  queue->next_out = queue_incr(queue, queue->next_out);

  semaphore_signal(queue->mutex);
  semaphore_signal(queue->spaces);

  return item;
}

/*
 * Add an integer to the buffer.  Yield control to another
 * thread if the buffer is full.
 */
void bounded_buffer_add(BoundedBuffer *bufp, const BufferElem *elem) {
    /* Not adding until the queue is known to not be full */
    semaphore_wait(bufp->full_sema);
    /* Accessing the mutex semaphore, to ensure only one thread is running */
    semaphore_wait(bufp->single_sema);
    /* Now the buffer has space */
    bufp->buffer[(bufp->first + bufp->count) % bufp->length] = *elem;
    bufp->count++;
    /* Signaling that this thread is done with the buffer */
    semaphore_signal(bufp->single_sema);
    /* Incrementing the empty sema */
    semaphore_signal(bufp->empty_sema);
}

IOReturn com_ximeta_driver_NDASDiskArrayController::receiveMsg ( void * newRequest, void *command, void *, void * )
{
	DbgIOLog(DEBUG_MASK_NDAS_TRACE, ("receiveMsg: Entered\n"));
	
	switch((int)newRequest) {
		case kNDASCommandQueueAppend:
		{
			DbgIOLog(DEBUG_MASK_NDAS_INFO, ("Append Command!!!\n"));

			if(!command) {
				DbgIOLog(DEBUG_MASK_NDAS_INFO, ("No Command!!!\n"));
				
				return kIOReturnBadArgument;
			}
			
			com_ximeta_driver_NDASCommand	*tempCommand = (com_ximeta_driver_NDASCommand *)command;	

			fCommandArray->setObject(tempCommand);
			
			if( 1 == fCommandArray->getCount() ) {
				// Notify to the worker thread.
				semaphore_signal(fCommandSema);
			}			
		}
			break;
		case kNDASCommandQueueCompleteCommand:
		{
			DbgIOLog(DEBUG_MASK_NDAS_INFO, ("Complete Command!!!\n"));
			
//			com_ximeta_driver_NDASCommand	*tempCommand = (com_ximeta_driver_NDASCommand *)fCommandArray->getObject(0);
						
			fCommandArray->removeObject(0);

			if( 0 < fCommandArray->getCount() ) {
			
				// Notify to the worker thread.
				semaphore_signal(fCommandSema);
			}			
		}
			break;
		default:
			DbgIOLog(DEBUG_MASK_NDAS_ERROR, ("Invalid Command!!! %d\n", (int)newRequest));
			
			return kIOReturnInvalid;
	}
			
	return kIOReturnSuccess;
}

/*
 * Get an integer from the buffer.  Yield control to another
 * thread if the buffer is empty.
 */
void bounded_buffer_take(BoundedBuffer *bufp, BufferElem *elem) {
    /* Not taking until the queue is known to not be empty */
    semaphore_wait(bufp->empty_sema);
    /* Accessing the mutex semaphore, to ensure only one thread is running */
    semaphore_wait(bufp->single_sema);
    /* Copy the element from the buffer, and clear the record */
    *elem = bufp->buffer[bufp->first];
    bufp->buffer[bufp->first] = empty;
    bufp->count--;
    bufp->first = (bufp->first + 1) % bufp->length;

    /* Signaling that this thread is done with the buffer */
    semaphore_signal(bufp->single_sema);
    /* Incrementing the full sema */
    semaphore_signal(bufp->full_sema);
}

void Threading::Semaphore::Post(int multiple)
{
	for (int i = 0; i < multiple; ++i) {
		MACH_CHECK(semaphore_signal(m_sema));
	}
	__atomic_add_fetch(&m_counter, multiple, __ATOMIC_SEQ_CST);
}

void
os_sem_post(os_sem_t *sem, char *what)
{
    if (KERN_SUCCESS!=semaphore_signal(*sem))
        lose("%s: os_sem_post(%p): %s", what, sem, strerror(errno));
    FSHOW((stderr, "%s: os_sem_post(%p) ok\n", what, sem));
}

static void ReceiveEvtVSync (STEVT_CallReason_t Reason,
                                const ST_DeviceName_t RegistrantName,
                                STEVT_EventConstant_t Event,
                                const void *EventData,
                                const void *SubscriberData_p )
{
    STVTG_VSYNC_t EventType;

    EventType = *((STVTG_VSYNC_t*)EventData);
    switch (EventType)
    {
        case STVTG_TOP:
            stlayer_osd_context.stosd_ActiveTopNotBottom = TRUE;
            break;

        case STVTG_BOTTOM:
            stlayer_osd_context.stosd_ActiveTopNotBottom = FALSE;
            break;

        default :
            break;
    }
    if ((stlayer_osd_context.UpdateTop)
            || (stlayer_osd_context.UpdateBottom))
    {
        semaphore_signal(stlayer_osd_context.stosd_VSync_p);
    }
}

void RBSemaphore::Signal() throw(RBException)
{
	mValue.Decrement();
	
	int errorCode = semaphore_signal(mMachSemaphore);
	RBAssertNoErr(errorCode, CFSTR("semaphore_signal failed with error code %d"), errorCode);
}

void HighPrecisionClock::runLoop() {
    if (!(MachThreadSelf("MWorks High Precision Clock").setRealtime(period, computation, period))) {
        merror(M_SCHEDULER_MESSAGE_DOMAIN, "HighPrecisionClock failed to achieve real time scheduling");
    }
    
    uint64_t startTime = mach_absolute_time();
    
    while (true) {
        {
            lock_guard lock(waitsMutex);
            while (!waits.empty() && waits.top().getExpirationTime() <= startTime) {
                logMachError("semaphore_signal", semaphore_signal(waits.top().getSemaphore()));
                waits.pop();
            }
        }
        
        // Give another thread a chance to terminate this one
        boost::this_thread::interruption_point();
        
        // Sleep until the next work cycle
        startTime += period;
        if (mach_absolute_time() < startTime) {
            logMachError("mach_wait_until", mach_wait_until(startTime));
        }
    }
}

void *Consumer(void *arg)
{
	int fd, hit=0;

	for (;;) {
#if ApplePositive
		semaphore_wait(shared.full);
#else
		sem_wait(&shared.full);
#endif
		pthread_mutex_lock(&shared.mutex);
		fd = shared.buff[shared.out];
		shared.buff[shared.out] = 0;
		shared.out = (shared.out+1)%shared.buffSize;
		/* Release the buffer */
		pthread_mutex_unlock(&shared.mutex);
		ftp(fd, hit);
		/* Increment the number of full slots */
#if ApplePositive
		semaphore_signal(shared.empty);
#else
		sem_post(&shared.empty);
#endif
		hit++;
	}
	return NULL;
}

DISPATCH_NOINLINE
long
_dispatch_semaphore_signal_slow(dispatch_semaphore_t dsema)
{
	// Before dsema_sent_ksignals is incremented we can rely on the reference
	// held by the waiter. However, once this value is incremented the waiter
	// may return between the atomic increment and the semaphore_signal(),
	// therefore an explicit reference must be held in order to safely access
	// dsema after the atomic increment.
	_dispatch_retain(dsema);

	(void)dispatch_atomic_inc2o(dsema, dsema_sent_ksignals);

#if USE_MACH_SEM
	_dispatch_semaphore_create_port(&dsema->dsema_port);
	kern_return_t kr = semaphore_signal(dsema->dsema_port);
	DISPATCH_SEMAPHORE_VERIFY_KR(kr);
#elif USE_POSIX_SEM
	int ret = sem_post(&dsema->dsema_sem);
	DISPATCH_SEMAPHORE_VERIFY_RET(ret);
#endif

	_dispatch_release(dsema);
	return 1;
}

/*
 * Signal a condition variable, waking up all threads waiting for it.
 */
int       
pthread_cond_broadcast(pthread_cond_t *cond)
{
	kern_return_t kern_res;
	if (cond->sig == _PTHREAD_COND_SIG_init)
	{
		int res;
		if (res = pthread_cond_init(cond, NULL))
			return (res);
	}
	if (cond->sig == _PTHREAD_COND_SIG)
	{ 
		LOCK(cond->lock);
		if (cond->waiters == 0)
		{ /* Avoid kernel call since there are no waiters... */
			UNLOCK(cond->lock);	
			return (ESUCCESS);
		}
		UNLOCK(cond->lock);
		MACH_CALL(semaphore_signal(cond->sem), kern_res);
		MACH_CALL(semaphore_signal_all(cond->sem), kern_res);
		if (kern_res == KERN_SUCCESS)
		{
			return (ESUCCESS);
		} else
		{
			return (EINVAL);
		}
	} else
		return (EINVAL); /* Not a condition variable */
}

int XLinuxTask_preemptive::SemWrapper::Post()
{
	kern_return_t kres=semaphore_signal(fSem);
	xbox_assert(kres==KERN_SUCCESS);

	return static_cast<int>(kres);
}

void handler ()
{
  printf("%s:%d - %s, name [%s]\n", __FILE__, __LINE__, __FUNCTION__, handlerName);
    int i;
    for(i=0; i < 5; i++)
    {
        /* If you remove this protection, you should be able to see different
         * out of every time you run this program.  With this protection, you
         * should always be able to see result to be 151402.656521 */
        semaphore_wait(counter_mutex);       /* down semaphore */

        /* START CRITICAL REGION */
	/*
        int j;
        for (j = 0; j < 1000; j++) {
            result = result + sin(counter) * tan(counter);
        }
	*/
	result += counter;
        counter++;
	// sleep(1);

        /* END CRITICAL REGION */    

        semaphore_signal(counter_mutex);       /* up semaphore */
    }
    
    exit_my_thread(); /* exit thread */
}

/**
 * Signals a monitor condition variable.
 * cond is the zero based index of the condition variable which
 * must be less than the number of condition variables the monitor
 * was created with.
 */
void monitor_cond_signal(monitor_t *monitor, int cond) {
  if (monitor->x_count[cond] > 0) {
    monitor->next_count++;
    semaphore_signal(monitor->sem_id, MONITOR_SEM_COUNT + cond);
    semaphore_wait(monitor->sem_id, NEXT_SEM);
    monitor->next_count--;
  }
}