/**
 * Atomically wait for the semaphore to become non-zero (if needed),
 * then decrements it.
 */
void vlc_sem_wait (vlc_sem_t *sem)
{
    int val;

#if defined(__APPLE__)
    if (likely(semaphore_wait(*sem) == KERN_SUCCESS))
        return;

    val = EINVAL;
#else
    do
        if (likely(sem_wait (sem) == 0))
            return;
    while ((val = errno) == EINTR);
#endif

    VLC_THREAD_ASSERT ("locking semaphore");
}

void
_dispatch_thread_semaphore_wait(_dispatch_thread_semaphore_t sema)
{
#if USE_MACH_SEM
    semaphore_t s4 = (semaphore_t)sema;
    kern_return_t kr;
    do {
        kr = semaphore_wait(s4);
    } while (slowpath(kr == KERN_ABORTED));
    DISPATCH_SEMAPHORE_VERIFY_KR(kr);
#elif USE_POSIX_SEM
    sem_t s4 = (sem_t)sema;
    int ret;
    do {
        ret = sem_wait(&s4);
    } while (slowpath(ret != 0));
    DISPATCH_SEMAPHORE_VERIFY_RET(ret);
#endif
}

thread_t *thread_new_sized(const char *name, size_t work_queue_capacity) {
  assert(name != NULL);
  assert(work_queue_capacity != 0);

  thread_t *ret = osi_calloc(sizeof(thread_t));
  if (!ret)
    goto error;

  ret->reactor = reactor_new();
  if (!ret->reactor)
    goto error;

  ret->work_queue = fixed_queue_new(work_queue_capacity);
  if (!ret->work_queue)
    goto error;

  // Start is on the stack, but we use a semaphore, so it's safe
  struct start_arg start;
  start.start_sem = semaphore_new(0);
  if (!start.start_sem)
    goto error;

  strncpy(ret->name, name, THREAD_NAME_MAX);
  start.thread = ret;
  start.error = 0;
  pthread_create(&ret->pthread, NULL, run_thread, &start);
  semaphore_wait(start.start_sem);
  semaphore_free(start.start_sem);

  if (start.error)
    goto error;

  return ret;

error:;
  if (ret) {
    fixed_queue_free(ret->work_queue, osi_free);
    reactor_free(ret->reactor);
  }
  osi_free(ret);
  return NULL;
}

inline int
fast_sendrecv_m (task_id dst_id, u32 arg1, u32 arg2, u32 *r_arg1, u32 *r_arg2)
{
  quest_tss *src = lookup_TSS (str ());
  quest_tss *dst = lookup_TSS (dst_id);

  semaphore_wait (&dst->Msem, 1, -1);
  dst->M[0] = arg1;
  dst->M[1] = arg2;
  semaphore_signal (&src->Msem, 1);
  ltr (dst_id);
  asm volatile ("call _sw_ipc"
                :"+S" (src), "+D" (dst)
                :
                :"eax", "ebx", "ecx", "edx", "cc", "memory");
  /* after _sw_ipc dst and src are swapped */
  *r_arg1 = dst->M[0];
  *r_arg2 = dst->M[1];
  return 0;
}

int
fprintf(FILE *fp, const char *fmt, ...)
{
	kern_return_t kern_res;
	va_list ap;
	if (!_init)
	{
		_init++;
		MACH_CALL(semaphore_create(mach_task_self(), 
					   &lock, 
					   SYNC_POLICY_FIFO,
					   1),
			  kern_res);
	}
	MACH_CALL(semaphore_wait(lock), kern_res);
	va_start(ap, fmt);
	vfprintf(fp, fmt, ap);
	va_end(ap);
	MACH_CALL(semaphore_signal(lock), kern_res);
}

message_t *msg_receive(void)
{
    message_t *msg = NULL;
    const uint32_t pid = proc_ctrl_get_curr_pid();


    if (0 < msg_list[pid].nbr_in_que) {
        // Critical region starts...
        semaphore_wait(&g_mutex_msg_list);
        
        msg = msg_list[pid].next_msg;
        msg_list[pid].next_msg = msg_list[pid].next_msg->next_msg;
        msg_list[pid].nbr_in_que -= 1;
        
        // Critical regions ends...
        semaphore_signal(&g_mutex_msg_list);    
    }
    
    return msg;
}

DECLARE_TEST( semaphore, initialize )
{
	semaphore_t sem;

	semaphore_initialize( &sem, 0 );
	EXPECT_FALSE( semaphore_try_wait( &sem, 100 ) );
	semaphore_destroy( &sem );

	semaphore_initialize( &sem, 1 );
	EXPECT_TRUE( semaphore_try_wait( &sem, 100 ) );
	semaphore_destroy( &sem );

	semaphore_initialize( &sem, 2 );
	EXPECT_TRUE( semaphore_wait( &sem ) );
	EXPECT_TRUE( semaphore_try_wait( &sem, 100 ) );
	EXPECT_FALSE( semaphore_try_wait( &sem, 100 ) );
	semaphore_destroy( &sem );
	
	return 0;
}

enum imx233_i2c_error_t imx233_i2c_end(unsigned timeout)
{
    if(i2c_nr_stages == 0)
        return I2C_ERROR;
    i2c_stage[i2c_nr_stages - 1].dma.cmd |= BM_APB_CHx_CMD_SEMAPHORE | BM_APB_CHx_CMD_IRQONCMPLT;

    BF_CLR(I2C_CTRL1, ALL_IRQ);
    imx233_dma_reset_channel(APB_I2C);
    imx233_icoll_enable_interrupt(INT_SRC_I2C_DMA, true);
    imx233_icoll_enable_interrupt(INT_SRC_I2C_ERROR, true);
    imx233_dma_enable_channel_interrupt(APB_I2C, true);
    imx233_dma_start_command(APB_I2C, &i2c_stage[0].dma);

    enum imx233_i2c_error_t ret;
    if(semaphore_wait(&i2c_sema, timeout) == OBJ_WAIT_TIMEDOUT)
    {
        imx233_dma_reset_channel(APB_I2C);
        imx233_i2c_reset();
        ret = I2C_TIMEOUT;
    }
    else if(BF_RD(I2C_CTRL1, MASTER_LOSS_IRQ))
        ret = I2C_MASTER_LOSS;
    else if(BF_RD(I2C_CTRL1, NO_SLAVE_ACK_IRQ))
    {
        /* the core doesn't like this error, this is a workaround to prevent lock up */
#if IMX233_SUBTARGET >= 3780
        BF_SET(I2C_CTRL1, CLR_GOT_A_NAK);
#endif
        imx233_dma_reset_channel(APB_I2C);
        imx233_i2c_reset();
        ret = I2C_NO_SLAVE_ACK;
    }
    else if(BF_RD(I2C_CTRL1, EARLY_TERM_IRQ))
        ret = I2C_SLAVE_NAK;
    else
        ret = imx233_i2c_finalize();
    /* sleep */
    BF_SET(I2C_CTRL0, CLKGATE);
    mutex_unlock(&i2c_mutex);
    return ret;
}

int usb_drv_send(int ep, void *ptr, int len)
{
    logf("usb_drv_send(%d,%x,%d): ", ep, (int)ptr, len);

    ep &= 0x7f;

    if (ep == 0 && got_set_configuration) {
        got_set_configuration = 0;
        if (len != 0)
            panicf("usb_drv_send: GSC, but len!=0");
        /* Tell the HW we handled the request */
        USB_DEV_CTRL |= USB_DEV_CTRL_APCSR_DONE;
        return 0;
    }

    ep_send(ep, ptr, len);
    if (semaphore_wait(&endpoints[ep][0].complete, HZ) == OBJ_WAIT_TIMEDOUT)
        logf("send timed out!\n");

    return endpoints[ep][0].rc;
}

static void *timer_func(void *arg)
{
	timer_thread = mach_thread_self();
	timer_thread_active = true;
	
	while (timer_thread_active) {
		clock_sleep(system_clock, TIME_ABSOLUTE, wakeup_time, NULL);
		semaphore_wait(wakeup_time_sem);
	   
		tm_time_t system_time;
		
		timer_current_time(system_time);
		if (timer_cmp_time(wakeup_time, system_time) < 0) {
			wakeup_time = wakeup_time_max;
			SetInterruptFlag(INTFLAG_TIMER);
			TriggerInterrupt();
		}
		semaphore_signal(wakeup_time_sem);
	}
    return NULL;
}

/*******************************************************************************
Name        : EnterCriticalSection
Description : Used to protect critical sections of Init/Open/Close/Term
Parameters  : None
Assumptions :
Limitations :
Returns     : Nothing
*******************************************************************************/
static void EnterCriticalSection(void)
{
    static BOOL InstancesAccessControlInitialized = FALSE;

#if !defined(ST_OSLINUX)
    task_lock();
#endif

    if (!InstancesAccessControlInitialized)
    {

        InstancesAccessControlInitialized = TRUE;
        InstancesAccessControl_p = semaphore_create_fifo(1);

    }
#if !defined(ST_OSLINUX)
    task_unlock();
#endif


    semaphore_wait(InstancesAccessControl_p);
}

void handler()
{
    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);
        }
        counter++;
        /* END CRITICAL REGION */    

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

uint32_t msg_send(message_t *msg)
{
    uint32_t ret = 0;


    ASSERT(NULL != msg);
    // ASSERT if the msg not pointing on the heap

    ASSERT(PID_IDLE != msg->receiver);
    ASSERT(HIGH_PID >= msg->receiver);
    ASSERT(msg_none_1 < msg->id && msg->id < msg_none_2);
    ret = msg->receiver;

    // Critical region starts...
    semaphore_wait(&g_mutex_msg_list);
    if (0 == msg_list[msg->receiver].nbr_in_que) {
        msg_list[msg->receiver].next_msg = msg;
        msg_list[msg->receiver].last_msg = msg;
    }
    else {
        msg_list[msg->receiver].last_msg->next_msg = msg;
        msg_list[msg->receiver].last_msg = msg;
    }
    
    msg_list[msg->receiver].nbr_in_que += 1;
    msg->next_msg = NULL;

    // semaphore_signal will generate a context switch. Make sure that the
    // receiving process is in state ready.
    if (msg->receiver != proc_ctrl_get_curr_pid()) {
        proc_ctrl_change_state(msg->receiver, ready);
    }
    
    // Critical regions ends...
    semaphore_signal(&g_mutex_msg_list);
    
    msg = NULL;
    return ret;
}

thread_t *thread_new(const char *name) {
  assert(name != NULL);

  // Start is on the stack, but we use a semaphore, so it's safe
  thread_t *ret = calloc(1, sizeof(thread_t));
  if (!ret)
    goto error;

  ret->reactor = reactor_new();
  if (!ret->reactor)
    goto error;

  ret->work_queue = fixed_queue_new(WORK_QUEUE_CAPACITY);
  if (!ret->work_queue)
    goto error;

  struct start_arg start;
  start.start_sem = semaphore_new(0);
  if (!start.start_sem)
    goto error;

  strncpy(ret->name, name, THREAD_NAME_MAX);
  start.thread = ret;
  start.error = 0;
  pthread_create(&ret->pthread, NULL, run_thread, &start);
  semaphore_wait(start.start_sem);
  semaphore_free(start.start_sem);
  if (start.error)
    goto error;
  return ret;

error:;
  if (ret) {
    fixed_queue_free(ret->work_queue, free);
    reactor_free(ret->reactor);
  }
  free(ret);
  return NULL;
}

char* file_queue_get()
{
  char* result;

  semaphore_wait(&used_slots);
  mutex_lock(&queue_mutex);

  if (queue_head == queue_tail) // queue is empty
  {
    result = NULL;
  }
  else
  {
    result = file_queue[queue_head].path;
    queue_head = (queue_head + 1) % (MAX_QUEUED_FILES + 1);
  }

  mutex_unlock(&queue_mutex);
  semaphore_release(&unused_slots);

  return result;
}

static int _usb_drv_send(int endpoint, void *ptr, int length, bool block)
{
    struct endpoint_t *ep;
    int ep_num = EP_NUM(endpoint);
    
    if (ep_num == 0)
        ep = &ctrlep[DIR_IN];
    else
        ep = &endpoints[ep_num];

    ep->buf = ptr;
    ep->len = ep->cnt = length;
    
    if (block)
        ep->block = true;
    else
        ep->block = false;
    
    switch (ep->type)
    {
        case USB_ENDPOINT_XFER_CONTROL:                     
            ctr_write();
            break;
        
        case USB_ENDPOINT_XFER_BULK:
            blk_write(ep_num);
            break;
        
        case USB_ENDPOINT_XFER_INT:
            int_write(ep_num);
            break;
    }
    
    if (block)
        /* wait for transfer to end */
        semaphore_wait(&ep->complete, TIMEOUT_BLOCK);

    return 0;
}

/**
 * This is the main system call function. It takes an interrupt stack frame
 * and decodes which interrupt number generated the system call. This number
 * is used to decode which system call was called.
 *
 * The stack frame is used to both pass and receive information from the
 * system call.
 * ECX: On completion of the system call ecx is populated with the return
 *      value
 * EAX: Contains the system call number
 * EBX: Contains the ptr to the first of the parameters
 *
 * @param frame Ptr to a handler_stack_frame struture
 * @return SUCCESS on success or FAILURE if something goes wrong
 */
int system_call( struct handler_stack_frame* frame )
{
    int system_call_number;
    char** system_call_params;
    int* system_call_result;

    system_call_result = (int*)frame->ecx;
    system_call_params = (char**)frame->ebx;
    system_call_number = frame->eax;

    switch( system_call_number )
    {
    case SYS_SEMA_CREATE:
        *system_call_result = semaphore_create( (sema_handle*)system_call_params[0], (int)system_call_params[1] );
        break;
    case SYS_SEMA_WAIT:
        *system_call_result = semaphore_wait( (int)system_call_params[0] );
        break;
    case SYS_SEMA_SIGNAL:
        *system_call_result = semaphore_signal( (int)system_call_params[0] );
        break;
    case SYS_SEMA_DESTROY:
        *system_call_result = semaphore_destroy( (int)system_call_params[0] );
        break;
    case SYS_THREAD_EXIT:
        *system_call_result = sys_thread_exit();
        break;
    case SYS_MSLEEP:
        *system_call_result = sys_msleep((uint)system_call_params[0] );
        break;
    default:
        *system_call_result = FAILURE;
        break;
    }

    return SUCCESS;
}

/*
 * Block forever waiting for EVENT to be signalled,
 * or timeout after timeout (ms) have expired
 * set timeout to EMBX_TIMEOUT_INFINITE to wait forever
 *
 * Returns EMBX_SUCCESS if signalled, EMBX_SYSTEM_TIMEOUT otherwise
 */
EMBX_ERROR EMBX_OS_EventWaitTimeout(EMBX_EVENT ev, unsigned long timeout)
{
    int res;
    osclock_t time;

    EMBX_Assert(ev);

    if (timeout != EMBX_TIMEOUT_INFINITE)
    {
	/* OS21 semaphore timeout takes an absolute time value
	 * so we calculate that here
	 */
	time = time_plus(time_now(), (timeout*time_ticks_per_sec())/1000);
	res = semaphore_wait_timeout(ev->event, &time);
    }
    else
	/* Infinite wait */
	res = semaphore_wait(ev->event);

    if (res == OS21_FAILURE)
	return EMBX_SYSTEM_TIMEOUT;

    return EMBX_SUCCESS;
}

int
printf(const char *fmt, ...)
{
	kern_return_t kern_res;
	va_list ap;
	if (!_init)
	{
		_init++;
		MACH_CALL(semaphore_create(mach_task_self(), 
					   &lock, 
					   SYNC_POLICY_FIFO,
					   1),
			  kern_res);
	}
	MACH_CALL(semaphore_wait(lock), kern_res);
	va_start(ap, fmt);
	vprintf(fmt, ap);
	va_end(ap);
	MACH_CALL(semaphore_signal(lock), kern_res);
	if (kern_res == KERN_SUCCESS)
		return 0;
	else
		return -1;
}

void hotplug_task_fn(void *val)
{
  while(1)
  {
    semaphore_wait(hotplug_sem);
    switch(currentDisplayStatus)
    {
      case STM_DISPLAY_NEEDS_RESTART:
        if(!silent_hotplug) printf("HDMI TV Starting Link\n");
        if(stm_display_output_start(pHDMI, pModeLine, STM_OUTPUT_STD_CEA861C)<0)
        {
          if(!silent_hotplug) printf("Unable to start HDMI\n");
        }
        break;
      case STM_DISPLAY_CONNECTED:
        if(!silent_hotplug) printf("HDMI TV Successfully Connected\n");
        break;
      case STM_DISPLAY_DISCONNECTED:
        if(!silent_hotplug) printf("HDMI TV Disconnected\n");
        stm_display_output_stop(pHDMI);
        break;
    }
  }
}