Пример #1
0
int rwlock_wrwait(rwlock_t* rwlock)
{
  COND_WAIT(rwlock->reader_cond, rwlock->lock);
  while(rwlock->readers > 0) {
    COND_WAIT(rwlock->writer_cond, rwlock->lock);
  }
  return 0;
}
Пример #2
0
/*
 * Validate the PQI mode of adapter.
 */
int pqisrc_check_pqimode(pqisrc_softstate_t *softs)
{
	int ret = PQI_STATUS_FAILURE;
	int tmo = 0;
	uint64_t signature = 0;

	DBG_FUNC("IN\n");

	/* Check the PQI device signature */
	tmo = PQISRC_PQIMODE_READY_TIMEOUT;
	do {
		signature = LE_64(PCI_MEM_GET64(softs, &softs->pqi_reg->signature, PQI_SIGNATURE));
        
		if (memcmp(&signature, PQISRC_PQI_DEVICE_SIGNATURE,
				sizeof(uint64_t)) == 0) {
			ret = PQI_STATUS_SUCCESS;
			break;
		}
		OS_SLEEP(PQISRC_MODE_READY_POLL_INTERVAL);
	} while (tmo--);

	PRINT_PQI_SIGNATURE(signature);

	if (tmo <= 0) {
		DBG_ERR("PQI Signature is invalid\n");
		ret = PQI_STATUS_TIMEOUT;
		goto err_out;
	}

	tmo = PQISRC_PQIMODE_READY_TIMEOUT;
	/* Check function and status code for the device */
	COND_WAIT((PCI_MEM_GET64(softs, &softs->pqi_reg->admin_q_config,
		PQI_ADMINQ_CONFIG) == PQI_ADMIN_QUEUE_CONF_FUNC_STATUS_IDLE), tmo);
	if (!tmo) {
		DBG_ERR("PQI device is not in IDLE state\n");
		ret = PQI_STATUS_TIMEOUT;
		goto err_out;
	}


	tmo = PQISRC_PQIMODE_READY_TIMEOUT;
	/* Check the PQI device status register */
	COND_WAIT(LE_32(PCI_MEM_GET32(softs, &softs->pqi_reg->pqi_dev_status, PQI_DEV_STATUS)) &
				PQI_DEV_STATE_AT_INIT, tmo);
	if (!tmo) {
		DBG_ERR("PQI Registers are not ready\n");
		ret = PQI_STATUS_TIMEOUT;
		goto err_out;
	}

	DBG_FUNC("OUT\n");
	return ret;
err_out:
	DBG_FUNC("OUT failed\n");
	return ret;
}
Пример #3
0
int rwlock_wrlock(rwlock_t* rwlock)
{
  MUTEX_LOCK(rwlock->lock);
  while(rwlock->readers) {
    COND_WAIT(rwlock->writer_cond, rwlock->lock);
  }
  return 0;
}
Пример #4
0
void *sw_intr_thread(void *arg)
{
     MU_LOCK(&kern_lock);
     while (1) {
	  COND_WAIT(&sw_intr_cond,&kern_lock);
	  do_sw_intr();
     }
}
Пример #5
0
static inline
void API_CALL(wait_contrib_comp_2d)(Sopalin_Data_t *sopalin_data,
                                    PASTIX_INT me, PASTIX_INT i){

  SolverMatrix  *datacode    = sopalin_data->datacode;
#ifdef TRACE_SOPALIN
  Thread_Data_t *thread_data = sopalin_data->thread_data[me];
#endif
#ifdef SMP_SOPALIN
  PASTIX_INT            firsttask   = TASK_MASTER(i);
#endif

  /* Attente contribution locale et MPI */
#if (defined FORCE_CONSO)
  if (THREAD_FUNNELED_OFF)
    {
      while ((!(TASK_BTAGPTR(i)))
             || (!(RTASK_COEFTAB(i))))
        {
          API_CALL(rcsd_testall_fab)(sopalin_data, me);
        }
    }
  else
#endif
    {
      if (THREAD_COMM_OFF)
        {
          while ((TASK_BTAGPTR(i) == NULL) && (sopalin_data->taskmark[i] > 0))
            {
              ASSERTDBG(i == firsttask, MOD_SOPALIN);
              RECV_ONE_BLOCK;
            }
        }
    }

  trace_begin_task(thread_data->tracefile,
                   SOPALIN_CLOCK_TRACE, SOLV_PROCNUM, me, 1,
                   STATE_WAITLOC, i);

#if (DBG_PASTIX_DYNSCHED > 0)
  MUTEX_LOCK(&(sopalin_data->mutex_task[i]));
  ASSERTDBG(((TASK_TASKID(i) == E2) && (sopalin_data->taskmark[i] == 0))
            || ((TASK_TASKID(i) == E1) &&
                (sopalin_data->taskmark[i] == 1)), MOD_SOPALIN);
  ASSERTDBG(TASK_BTAGPTR(i)  != NULL, MOD_SOPALIN);
  ASSERTDBG(RTASK_COEFTAB(i) != NULL, MOD_SOPALIN);
  sopalin_data->taskmark[i]++;
  MUTEX_UNLOCK(&(sopalin_data->mutex_task[i]));
#endif

  MUTEX_LOCK(&(sopalin_data->mutex_task[firsttask]));
  while ((!(TASK_BTAGPTR(i)))
         || (!(RTASK_COEFTAB(i))))
  COND_WAIT(&(sopalin_data->cond_task[firsttask]),
            &(sopalin_data->mutex_task[firsttask]));
  MUTEX_UNLOCK(&(sopalin_data->mutex_task[firsttask]));
}
Пример #6
0
int rwlock_rdwait(rwlock_t* rwlock)
{
  MUTEX_LOCK(rwlock->lock);
  rwlock->readers--;
  COND_SIGNAL(rwlock->writer_cond);
  COND_WAIT(rwlock->reader_cond, rwlock->lock);
  rwlock->readers++;
  MUTEX_UNLOCK(rwlock->lock);
  return 0;
}
Пример #7
0
void *fast_mtq_thread(void *arg)
{
	fast_mtq f_mtq = (fast_mtq) arg;
	register fast_mtq_elem e;
	void *cb_arg;

	cb_arg = f_mtq->arg;

	if (f_mtq->start_cb) {
		cb_arg = (f_mtq->start_cb) (cb_arg);
	}

	log_debug("fast mtq thread start");

	while (1) {

		/* check if something to resolv */
		SEM_LOCK(f_mtq->sem);

		e = NULL;

		if (f_mtq->shutdown != 2) {
			if ((!(f_mtq->first)) && (!(f_mtq->shutdown))) {
				COND_WAIT(f_mtq->cond, f_mtq->sem);
			}

			/* get element */
			e = f_mtq->first;
			if (e) {
				f_mtq->first = e->mtq_next;
				f_mtq->queue--;
				f_mtq->queue2--;
			}
		}

		SEM_UNLOCK(f_mtq->sem);

		if (e) {
			(f_mtq->cb) (cb_arg, (void *) e);
		} else {
			if (f_mtq->shutdown)
				break;
		}
	}

	log_alert("mtq", "fast mtq thread stop %d [%d][%d]",
		  f_mtq->shutdown, f_mtq->queue, f_mtq->queue2);

	if (f_mtq->stop_cb) {
		(f_mtq->stop_cb) (cb_arg);
	}

	return NULL;
}
Пример #8
0
int pqisrc_wait_for_cmnd_complete(pqisrc_softstate_t *softs)
{
	int ret = PQI_STATUS_SUCCESS;
	int tmo = PQI_CMND_COMPLETE_TMO;
	
	COND_WAIT((softs->taglist.num_elem == softs->max_outstanding_io), tmo);
	if (!tmo) {
		DBG_ERR("Pending commands %x!!!",softs->taglist.num_elem);
		ret = PQI_STATUS_TIMEOUT;
	}
	return ret;
}
Пример #9
0
const message* S_queue_dequeue(pTHX_ message_queue* queue) {
	const message* ret;
	MUTEX_LOCK(&queue->mutex);

	while (!queue->front)
		COND_WAIT(&queue->condvar, &queue->mutex);

	ret = queue_shift(queue);
	MUTEX_UNLOCK(&queue->mutex);

	return ret;
}
Пример #10
0
/* main slave thread */
void *mtq_main(void *arg)
{
    mth t = (mth)arg;
    mtqqueue mq; /* temp call structure */
    _mtqqueue mqcall; /* temp call structure */
    log_debug("mtq","%X starting mq=%d",t->thread,t->mtq);

    while(1) {

	  /* check if something to resolv */
	  SEM_LOCK(t->mtq->sem);
	  if (t->mtq->last == NULL) {
		COND_WAIT(t->mtq->cond,t->mtq->sem);
	  }

	  /* get element */
	  mq = t->mtq->last;
	  if (mq != NULL) {
		mqcall.f = mq->f;
		mqcall.arg = mq->arg;
		
		/* remove call from list */		    
		t->mtq->last = t->mtq->last->prev;		    
		t->mtq->dl--;
		
		/* add mq to list */
		mq->prev = NULL;
		
		if (t->mtq->free_last == NULL)
		  t->mtq->free_last = mq;
		else
		  t->mtq->free_first->prev = mq;
		
		t->mtq->free_first = mq;		
	  }

	  SEM_UNLOCK(t->mtq->sem);

	  if (mq != NULL) {
		(*(mqcall.f))(mqcall.arg);
	  }
	  else {	  
		/* mq is NULL here, so no more packets in queue */
		if (mtq__shutdown == 1)
		  break;		
	  }
	} /* loop end */
    
    log_debug("mtq","%X ending mq=%d",t->thread,t->mtq);
    return NULL;
}
void* findDongleThread(void* arg)
{
  static int args[MAX_COMPORT];
  int i;
  THREAD_T threads[MAX_COMPORT];
  for(i = 0; i < MAX_COMPORT; i++) {
    args[i] = i;
  }
  g_dongleSearchStatus = DONGLE_SEARCHING;
  /* Spawn worker threads to find the dongle on a com port */
  for(i = 0; i < MAX_COMPORT; i++) { 
    /* First, make sure there are less than MAX_THREADS running */
    MUTEX_LOCK(&g_giant_lock);
    while(
        (g_numThreads >= MAX_THREADS) &&
        (g_dongleSearchStatus == DONGLE_SEARCHING)
        ) 
    {
      COND_WAIT(&g_giant_cond, &g_giant_lock);
    }
    if(g_dongleSearchStatus != DONGLE_SEARCHING) {
      i++;
      MUTEX_UNLOCK(&g_giant_lock);
      break;
    }
    /* Spawn a thread */
    THREAD_CREATE(&threads[i], findDongleWorkerThread, &args[i]);
    g_numThreads++;
    MUTEX_UNLOCK(&g_giant_lock);
  }
  /* Join all threads */
  int j;
  for(j = 0; j < i; j++) {
    MUTEX_LOCK(&g_giant_lock);
    if(g_dongleSearchStatus != DONGLE_SEARCHING) {
      MUTEX_UNLOCK(&g_giant_lock);
      break;
    }
    MUTEX_UNLOCK(&g_giant_lock);
    THREAD_JOIN(threads[j]);
  }
  MUTEX_LOCK(&g_giant_lock);
  if(g_dongleSearchStatus == DONGLE_SEARCHING) {
    g_dongleSearchStatus = DONGLE_NOTFOUND;
  }
  MUTEX_UNLOCK(&g_giant_lock);
  return NULL;
}
Пример #12
0
/* Validate the FW status PQI_CTRL_KERNEL_UP_AND_RUNNING */
int pqisrc_check_fw_status(pqisrc_softstate_t *softs)
{
	int ret = PQI_STATUS_SUCCESS;
	uint32_t timeout = SIS_STATUS_OK_TIMEOUT;

	DBG_FUNC("IN\n");

	OS_SLEEP(1000000);
	COND_WAIT((GET_FW_STATUS(softs) &
		PQI_CTRL_KERNEL_UP_AND_RUNNING), timeout);
	if (!timeout) {
		DBG_ERR("FW check status timedout\n");
		ret = PQI_STATUS_TIMEOUT;
	}

	DBG_FUNC("OUT\n");
	return ret;
}
Пример #13
0
static inline void API_CALL(wait_contrib_comp_1d)(Sopalin_Data_t *sopalin_data, PASTIX_INT me, PASTIX_INT i){

  SolverMatrix  *datacode    = sopalin_data->datacode;
#ifdef TRACE_SOPALIN
  Thread_Data_t *thread_data = sopalin_data->thread_data[me];
#endif

#if (defined FORCE_CONSO)
  if (THREAD_FUNNELED_OFF)
    {
      /* Attente en Multiple / force_conso */
      while(TASK_CTRBCNT(i))
        {
          API_CALL(rcsd_testall_fab)(sopalin_data, me);
        }
    }
  else
#endif
    {
      if (THREAD_COMM_OFF)
        {
          /* Attente en multiple sans force conso */
          while(TASK_FTGTCNT(i))
            {
              RECV_ONE_FANIN;
            }
        }
    }
  trace_begin_task(thread_data->tracefile,
                   SOPALIN_CLOCK_TRACE, SOLV_PROCNUM, me, 1,
                   STATE_WAITLOC, i);

  MUTEX_LOCK(&(sopalin_data->mutex_task[i]));
#if (DBG_PASTIX_DYNSCHED > 0)
  ASSERTDBG(sopalin_data->taskmark[i] == 0, MOD_SOPALIN);
  sopalin_data->taskmark[i]++;
#endif
  while (TASK_CTRBCNT(i))
  {
    COND_WAIT(&(sopalin_data->cond_task[i]), &(sopalin_data->mutex_task[i]));
  }
  MUTEX_UNLOCK(&(sopalin_data->mutex_task[i]));

}
Пример #14
0
void buffer_wait_for_empty (buf_t *buf)
{
  int empty = 0;

  DEBUG("Enter buffer_wait_for_empty");

  pthread_cleanup_push(buffer_mutex_unlock, buf);

  LOCK_MUTEX(buf->mutex);
  while (!empty && !buf->abort_write) {

    if (buf->curfill > 0) {
      DEBUG1("Buffer curfill = %ld, going back to sleep.", buf->curfill);
      COND_WAIT(buf->write_cond, buf->mutex);
    } else 
      empty = 1;
  }
  UNLOCK_MUTEX(buf->mutex);

  pthread_cleanup_pop(0);

  DEBUG("Exit buffer_wait_for_empty");
}
Пример #15
0
size_t buffer_get_data (buf_t *buf, char *data, long nbytes)
{
  int write_amount;
  int orig_size;

  orig_size = nbytes;

  DEBUG("Enter buffer_get_data");

  pthread_cleanup_push(buffer_mutex_unlock, buf);

  LOCK_MUTEX(buf->mutex);

  /* Put the data into the buffer as space is made available */
  while (nbytes > 0) {

    if (buf->abort_write)
      break;

    DEBUG("Obtaining lock on buffer");
    /* Block until we can read something */
    if (buf->curfill == 0 && buf->eos)
      break; /* No more data to read */

    if (buf->curfill == 0 || (buf->prebuffering && !buf->eos)) {
      DEBUG("Waiting for more data to copy.");
      COND_WAIT(buf->playback_cond, buf->mutex);
    }

    if (buf->abort_write)
      break;

    /* Note: Even if curfill is still 0, nothing bad will happen here */

    /* For simplicity, the number of bytes played must satisfy
       the following three requirements:

       1. Do not copy more bytes than are stored in the buffer.
       2. Do not copy more bytes than the reqested data size.
       3. Do not run off the end of the buffer. */
    write_amount = compute_dequeue_size(buf, nbytes);

    UNLOCK_MUTEX(buf->mutex);
    execute_actions(buf, &buf->actions, buf->position);

    /* No need to lock mutex here because the other thread will
       NEVER reduce the number of bytes stored in the buffer */
    DEBUG1("Copying %d bytes from the buffer", write_amount);
    memcpy(data, buf->buffer + buf->start, write_amount);
    LOCK_MUTEX(buf->mutex);

    buf->curfill -= write_amount;
    data += write_amount;
    nbytes -= write_amount;
    buf->start = (buf->start + write_amount) % buf->size;
    DEBUG1("Updated buffer fill, curfill = %ld", buf->curfill);

    /* Signal a waiting decoder thread that they can put more
       audio into the buffer */
    DEBUG("Signal decoder thread that buffer space is available");
    COND_SIGNAL(buf->write_cond);
  }

  UNLOCK_MUTEX(buf->mutex);

  pthread_cleanup_pop(0);

  pthread_testcancel();

  DEBUG("Exit buffer_get_data");

  return orig_size - nbytes;
}
Пример #16
0
int submit_data_chunk (buf_t *buf, char *data, size_t size)
{
  long   buf_write_pos; /* offset of first available write location */
  size_t write_size;

  DEBUG1("Enter submit_data_chunk, size %d", size);

  pthread_cleanup_push(buffer_mutex_unlock, buf);

  /* Put the data into the buffer as space is made available */
  while (size > 0 && !buf->abort_write) {

    /* Section 1: Write a chunk of data */
    DEBUG("Obtaining lock on buffer");
    LOCK_MUTEX(buf->mutex);
    if (buf->size - buf->curfill > 0) {

      /* Figure how much we can write into the buffer.  Requirements:
	 1. Don't write more data than we have.
	 2. Don't write more data than we have room for.
	 3. Don't write past the end of the buffer. */
      buf_write_pos = (buf->start + buf->curfill) % buf->size;
      write_size = MIN3(size, buf->size - buf->curfill,
			buf->size - buf_write_pos);

      memcpy(buf->buffer + buf_write_pos, data, write_size);
      buf->curfill += write_size;
      data += write_size;
      size -= write_size;
      buf->position_end += write_size;
      DEBUG1("writing chunk into buffer, curfill = %ld", buf->curfill);
    }
    else {

      if (buf->cancel_flag || sig_request.cancel) {
        UNLOCK_MUTEX(buf->mutex);
        break;
      }
      /* No room for more data, wait until there is */
      DEBUG("No room for data in buffer.  Waiting.");
      COND_WAIT(buf->write_cond, buf->mutex);
    }
    /* Section 2: signal if we are not prebuffering, done
       prebuffering, or paused */
    if (buf->prebuffering && (buf->prebuffer_size <= buf->curfill)) {

      DEBUG("prebuffering done")
	buf->prebuffering = 0; /* done prebuffering */
    }

    if (!buf->prebuffering && !buf->paused) {

      DEBUG("Signalling playback thread that more data is available.");
      COND_SIGNAL(buf->playback_cond);
    } else
      DEBUG("Not signalling playback thread since prebuffering or paused.");

    UNLOCK_MUTEX(buf->mutex);
  }

  pthread_cleanup_pop(0);

  DEBUG("Exit submit_data_chunk");
  return !buf->abort_write;
}
Пример #17
0
void *buffer_thread_func (void *arg)
{
  buf_t *buf = (buf_t*) arg;
  size_t write_amount;

  DEBUG("Enter buffer_thread_func");

  buffer_thread_init(buf);

  pthread_cleanup_push(buffer_thread_cleanup, buf);

  DEBUG("Start main play loop");

  /* This test is safe since curfill will never decrease and eos will
     never be unset. */
  while ( !(buf->eos && buf->curfill == 0) && !buf->abort_write) {

    if (buf->cancel_flag || sig_request.cancel)
      break;

    DEBUG("Check for something to play");
    /* Block until we can play something */
    LOCK_MUTEX (buf->mutex);
    if (buf->prebuffering || 
	buf->paused || 
	(buf->curfill < buf->audio_chunk_size && !buf->eos)) {

      DEBUG("Waiting for more data to play.");
      COND_WAIT(buf->playback_cond, buf->mutex);
    }

    DEBUG("Ready to play");

    UNLOCK_MUTEX(buf->mutex);

    if (buf->cancel_flag || sig_request.cancel)
      break;

    /* Don't need to lock buffer while running actions since position
       won't change.  We clear out any actions before we compute the
       dequeue size so we don't consider actions that need to
       run right now.  */
    execute_actions(buf, &buf->actions, buf->position);

    LOCK_MUTEX(buf->mutex);

    /* Need to be locked while we check things. */
    write_amount = compute_dequeue_size(buf, buf->audio_chunk_size);

    UNLOCK_MUTEX(buf->mutex);
 
    if(write_amount){ /* we might have been woken spuriously */
      /* No need to lock mutex here because the other thread will
         NEVER reduce the number of bytes stored in the buffer */
      DEBUG1("Sending %d bytes to the audio device", write_amount);
      write_amount = buf->write_func(buf->buffer + buf->start, write_amount,
                                     /* Only set EOS if this is the last chunk */
                                     write_amount == buf->curfill ? buf->eos : 0,
                                     buf->write_arg);

      if (!write_amount) {
        DEBUG("Error writing to the audio device. Aborting.");
        buffer_abort_write(buf);
      }

      LOCK_MUTEX(buf->mutex);

      buf->curfill -= write_amount;
      buf->position += write_amount;
      buf->start = (buf->start + write_amount) % buf->size;
      DEBUG1("Updated buffer fill, curfill = %ld", buf->curfill);

      /* If we've essentially emptied the buffer and prebuffering is enabled,
         we need to do another prebuffering session */
      if (!buf->eos && (buf->curfill < buf->audio_chunk_size))
        buf->prebuffering = buf->prebuffer_size > 0;
    }else{
      DEBUG("Woken spuriously");
    }

    /* Signal a waiting decoder thread that they can put more audio into the
       buffer */
    DEBUG("Signal decoder thread that buffer space is available");
    COND_SIGNAL(buf->write_cond);

    UNLOCK_MUTEX(buf->mutex);
  }

  pthread_cleanup_pop(1);
  DEBUG("exiting buffer_thread_func");

  return 0;
}
Пример #18
0
/* Function used to submit a SIS command to the adapter */
static int pqisrc_send_sis_cmd(pqisrc_softstate_t *softs,
					uint32_t *mb)
{
	int ret = PQI_STATUS_SUCCESS;
	int i = 0;
	uint32_t timeout = SIS_CMD_COMPLETE_TIMEOUT;

	int val;

	DBG_FUNC("IN\n");


	/* Copy Command to mailbox */
	for (i = 0; i < 6; i++)
		PCI_MEM_PUT32(softs, &softs->ioa_reg->mb[i], 
            LEGACY_SIS_SRCV_MAILBOX+i*4, LE_32(mb[i]));
    
	PCI_MEM_PUT32(softs, &softs->ioa_reg->ioa_to_host_db_clr, 
		LEGACY_SIS_ODBR_R, LE_32(0x1000));

	/* Submit the command */
	PCI_MEM_PUT32(softs, &softs->ioa_reg->host_to_ioa_db, 
		LEGACY_SIS_IDBR, LE_32(SIS_CMD_SUBMIT));

#ifdef SIS_POLL_WAIT
	/* Wait for 20  milli sec to poll */
	OS_BUSYWAIT(SIS_POLL_START_WAIT_TIME);
#endif

	val = PCI_MEM_GET32(softs, &softs->ioa_reg->ioa_to_host_db, LEGACY_SIS_ODBR_R);

	DBG_FUNC("val : %x\n",val);
	/* Spin waiting for the command to complete */
	COND_WAIT((PCI_MEM_GET32(softs, &softs->ioa_reg->ioa_to_host_db, LEGACY_SIS_ODBR_R) &
		SIS_CMD_COMPLETE), timeout);
	if (!timeout) {
		DBG_ERR("Sync command %x, timedout\n", mb[0]);
		ret = PQI_STATUS_TIMEOUT;
		goto err_out;
	}
	/* Check command status */
	mb[0] = LE_32(PCI_MEM_GET32(softs, &softs->ioa_reg->mb[0], LEGACY_SIS_SRCV_MAILBOX));

	if (mb[0] != SIS_CMD_STATUS_SUCCESS) {
		DBG_ERR("SIS cmd failed with status = 0x%x\n",
			mb[0]);
		ret = PQI_STATUS_FAILURE;
		goto err_out;
	}

	/* Copy the mailbox back  */
	for (i = 1; i < 6; i++)
		mb[i] =	LE_32(PCI_MEM_GET32(softs, &softs->ioa_reg->mb[i], LEGACY_SIS_SRCV_MAILBOX+i*4));

	DBG_FUNC("OUT\n");
	return ret;

err_out:
	DBG_FUNC("OUT failed\n");
	return ret;
}
Пример #19
0
int spi_transfer_blocking(const spi_bus_t spi_num, const uint8_t ctas, const uint32_t cs,
             const spi_transfer_flag_t cont, const uint8_t *data_out,
             uint8_t *data_in, size_t count_out, size_t count_in)
{
  SPI_Type *spi_dev = SPI[spi_num];
  /* Frame size in bits */
  unsigned short frame_size = ((SPI[spi_num]->CTAR[ctas] & SPI_CTAR_FMSZ_MASK) >> SPI_CTAR_FMSZ_SHIFT) + 1;

  /* Array stride in bytes */
  /* unsigned int stride = (frame_size / 8) + 1; */

  uint32_t common_flags = SPI_PUSHR_CTAS(ctas) | SPI_PUSHR_PCS(cs);

  uint32_t spi_pushr;

  /* this may yield unaligned memory accesses because of uint8_t* casted to
   * uint16_t*. Using the DMA module will avoid this. */
  if (frame_size > 8) {
    /* Error! not implemented yet */
    DEBUGGER_BREAK(BREAK_INVALID_PARAM);
    while(1);
  }

  while (count_out > 0) {
    spi_pushr = common_flags;
    spi_pushr |= SPI_PUSHR_TXDATA(*data_out);
    ++data_out; /* or += stride */
    --count_out;

    /* See if we are at the end yet */
    if((count_out > 0) || (count_in > 0) || (cont == SPI_TRANSFER_CONT)) {
      spi_pushr |= SPI_PUSHR_CONT_MASK;
    }

    /* Clear transfer complete flag */
    spi_dev->SR |= SPI_SR_TCF_MASK;

    /* Set waiting flag */
    spi_waiting_flag[spi_num] = 1;

    /* Shift a frame out/in */
    spi_dev->PUSHR = spi_pushr;

    /* Wait for transfer complete */
    COND_WAIT(spi_waiting_flag[spi_num] != 0);

    /* Do a dummy read in order to allow for the next byte to be read */
    uint8_t tmp = spi_dev->POPR;
    (void)tmp; /* Suppress warning about unused value. */

  }

  /* Prepare read */
  spi_pushr = common_flags;
  spi_pushr |= SPI_PUSHR_TXDATA(SPI_IDLE_DATA);
  spi_pushr |= SPI_PUSHR_CONT_MASK;

  /* Do a number of reads */
  while (count_in > 0) {
    --count_in;
    /* See if we are at the end yet */
    if((count_in < 1) && (cont != SPI_TRANSFER_CONT)) {
      /* Disable CS line after the next transfer */
      spi_pushr &= ~(SPI_PUSHR_CONT_MASK);
    }

    /* Clear transfer complete flag */
    spi_dev->SR |= SPI_SR_TCF_MASK;

    /* Set waiting flag */
    spi_waiting_flag[spi_num] = 1;

    /* Shift a frame out/in */
    spi_dev->PUSHR = spi_pushr;

    /* Wait for transfer complete */
    COND_WAIT(spi_waiting_flag[spi_num] != 0);

    (*data_in) = spi_dev->POPR;
    ++data_in; /* or += stride */
  }

  return 0;
}