static void test_right_value(void)
{
int a = 10;
int b = 20;
int c = 30;
int *item;
queue *q = queue_new(3);
queue_add(q, &a);
queue_add(q, &b);
queue_add(q, &c);
item = queue_remove(q);
info("item = %d", *item);
assert(*item == 10);
queue_add(q, &a);
item = queue_remove(q);
info("item = %d", *item);
assert(*item == 20);
item = queue_remove(q);
info("item = %d", *item);
assert(*item == 30);
item = queue_remove(q);
info("item = %d", *item);
assert(*item == 10);
info("count = %d", queue_count(q));
assert(queue_count(q) == 0);
}
static void test_queue_full(void)
{
char *a = "hello";
char *b = "goodbye";
queue *q = queue_new(1);
assert(queue_add(q, a));
info("count = %d", queue_count(q));
assert(queue_count(q) == 1);
assert(!queue_add(q, b));
info("count = %d", queue_count(q));
assert(queue_count(q) == 1);
queue_destroy(q);
}
/*
* Free the queue
*/
static void queue_free(QUEUE *s)
{
while (queue_count(s))
{
queue_remove(s);
}
}
static int rxe_peek_cq(struct ib_cq *ibcq, int wc_cnt)
{
struct rxe_cq *cq = to_rcq(ibcq);
int count = queue_count(cq->queue);
return (count > wc_cnt) ? wc_cnt : count;
}
static int motion_sense_read_fifo(int argc, char **argv)
{
int count, i;
struct ec_response_motion_sensor_data v;
if (argc < 1)
return EC_ERROR_PARAM_COUNT;
/* Limit the amount of data to avoid saturating the UART buffer */
count = MIN(queue_count(&motion_sense_fifo), 16);
for (i = 0; i < count; i++) {
queue_peek_units(&motion_sense_fifo, &v, i, 1);
if (v.flags & (MOTIONSENSE_SENSOR_FLAG_TIMESTAMP |
MOTIONSENSE_SENSOR_FLAG_FLUSH)) {
uint64_t timestamp;
memcpy(×tamp, v.data, sizeof(v.data));
ccprintf("Timestamp: 0x%016lx%s\n", timestamp,
(v.flags & MOTIONSENSE_SENSOR_FLAG_FLUSH ?
" - Flush" : ""));
} else {
ccprintf("%d %d: %-5d %-5d %-5d\n", i, v.sensor_num,
v.data[X], v.data[Y], v.data[Z]);
}
}
return EC_SUCCESS;
}
static int rxe_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
struct ib_send_wr **bad_wr)
{
int err = 0;
struct rxe_qp *qp = to_rqp(ibqp);
unsigned int mask;
unsigned int length = 0;
int i;
int must_sched;
if (unlikely(!qp->valid)) {
*bad_wr = wr;
return -EINVAL;
}
if (unlikely(qp->req.state < QP_STATE_READY)) {
*bad_wr = wr;
return -EINVAL;
}
while (wr) {
mask = wr_opcode_mask(wr->opcode, qp);
if (unlikely(!mask)) {
err = -EINVAL;
*bad_wr = wr;
break;
}
if (unlikely((wr->send_flags & IB_SEND_INLINE) &&
!(mask & WR_INLINE_MASK))) {
err = -EINVAL;
*bad_wr = wr;
break;
}
length = 0;
for (i = 0; i < wr->num_sge; i++)
length += wr->sg_list[i].length;
err = post_one_send(qp, wr, mask, length);
if (err) {
*bad_wr = wr;
break;
}
wr = wr->next;
}
/*
* Must sched in case of GSI QP because ib_send_mad() hold irq lock,
* and the requester call ip_local_out_sk() that takes spin_lock_bh.
*/
must_sched = (qp_type(qp) == IB_QPT_GSI) ||
(queue_count(qp->sq.queue) > 1);
rxe_run_task(&qp->req.task, must_sched);
return err;
}
static void usb_flush(struct consumer const *consumer)
{
struct usb_stream_config const *config =
DOWNCAST(consumer, struct usb_stream_config, consumer);
while (tx_fifo_is_ready(config) && queue_count(consumer->queue))
;
}
static void uart_written(struct consumer const *consumer, size_t count)
{
struct usart_config const *config =
DOWNCAST(consumer, struct usart_config, consumer);
if (uartn_tx_ready(config->uart), queue_count(consumer->queue))
uartn_tx_start(config->uart);
}
uint16_t uart_can_read(uint8_t channel)
{
uart_channel* uartChannel = look_up_uart_channel(channel);
while (low_level_read_from_uart(uartChannel));
return queue_count(&uartChannel->receiveQueue);
}
static void motion_sense_get_fifo_info(
struct ec_response_motion_sense_fifo_info *fifo_info)
{
fifo_info->size = motion_sense_fifo.buffer_units;
mutex_lock(&g_sensor_mutex);
fifo_info->count = queue_count(&motion_sense_fifo);
fifo_info->total_lost = motion_sense_fifo_lost;
mutex_unlock(&g_sensor_mutex);
fifo_info->timestamp = __hw_clock_source_read();
}
uint32_t msgqueue_count( struct msgqueue * self )
{
uint32_t rc = 0;
pthread_spin_lock( &self->lock );
rc = queue_count( self->queue );
pthread_spin_unlock( &self->lock );
return rc;
}
void get_data_from_usb(struct usart_config const *config)
{
struct queue const *uart_out = config->consumer.queue;
int c;
/* Copy output from buffer until TX fifo full or output buffer empty */
while (queue_count(uart_out) && QUEUE_REMOVE_UNITS(uart_out, &c, 1))
uartn_write_char(config->uart, c);
/* If output buffer is empty, disable transmit interrupt */
if (!queue_count(uart_out))
uartn_tx_stop(config->uart);
/*
* Make sure rx fifo is cleared after any input to the console to ensure
* user will be able to see their input as they are typing instead of
* only when the RX FIFO reaches the level set by RXILVL.
*/
hook_call_deferred(config->deferred, 1 * MSEC);
}
static void *consumer(void *data)
{
queue *q = (queue *)data;
info("removed item = %s", (char *)queue_remove(q));
sleep(4);
info("removed item = %s", (char *)queue_remove(q));
info("count = %d", queue_count(q));
return NULL;
}
/*
* Creates a new queue with a copy of the items of the given queue.
*
* The given number of items (count) are copied from the play-queue (shuffle = 0) or shuffle-queue (shuffle = 1)
* starting with the item at the given index (0-based).
*
* If count == 0, all items from the given index up to the end of the queue will be returned.
*
* @param queue The queue
* @param index Index of the first item in the queue
* @param count Maximum number of items to copy (if 0 all remaining items after index)
* @param shuffle If 0 the play-queue, if 1 the shuffle queue
* @return A new queue with the specified items
*/
struct queue *
queue_new_byindex(struct queue *queue, unsigned int index, unsigned int count, char shuffle)
{
struct queue *qi;
struct queue_item *qii;
struct queue_item *item;
int i;
unsigned int qlength;
int qii_size;
qi = queue_new();
qlength = queue_count(queue);
qii_size = qlength - index;
if (count > 0 && count < qii_size)
qii_size = count;
if (qii_size <= 0)
{
return qi;
}
item = queueitem_get_byindex(queue, index, shuffle);
if (!item)
return NULL;
i = 0;
for (; item != queue->head && i < qii_size; item = item_next(item, shuffle))
{
qii = malloc(sizeof(struct queue_item));
qii->id = item->id;
qii->item_id = item->item_id;
qii->len_ms = item->len_ms;
qii->data_kind = item->data_kind;
qii->media_kind = item->media_kind;
qii->next = qii;
qii->prev = qii;
qii->shuffle_next = qii;
qii->shuffle_prev = qii;
queue_add(qi, qii);
// queue_add(...) changes the queue item-id, reset the item-id to the original value
qii->item_id = item->item_id;
i++;
}
return qi;
}
size_t
async_queue_count(AsyncQueue *queue)
{
assert(queue != NULL);
pthread_mutex_lock(&queue->mutex);
size_t count = queue_count(&queue->queue);
pthread_mutex_unlock(&queue->mutex);
return count;
}
static void usart_flush(struct consumer const *consumer)
{
struct usart_config const *config =
DOWNCAST(consumer, struct usart_config, consumer);
/*
* Enable USART interrupt. This causes the USART interrupt handler to
* start fetching from the TX queue if it wasn't already.
*/
STM32_USART_CR1(config->hw->base) |= STM32_USART_CR1_TXEIE;
while (queue_count(consumer->queue))
;
}
static void test_basic(void)
{
queue *q = queue_new(2);
pthread_t producer_tid, consumer_tid;
pthread_create(&producer_tid, NULL, &producer, q);
pthread_create(&consumer_tid, NULL, &consumer, q);
pthread_join(producer_tid, NULL);
pthread_join(consumer_tid, NULL);
assert(queue_count(q) == 0);
queue_destroy(q);
}
/* copies elements from original q to new q and then swaps the contents of the
* two q headers. This is so that if anyone is holding a pointer to q it will
* still work
*/
static int resize_finish(struct rxe_queue *q, struct rxe_queue *new_q,
unsigned int num_elem)
{
if (!queue_empty(q) && (num_elem < queue_count(q)))
return -EINVAL;
while (!queue_empty(q)) {
memcpy(producer_addr(new_q), consumer_addr(q),
new_q->elem_size);
advance_producer(new_q);
advance_consumer(q);
}
swap(*q, *new_q);
return 0;
}
/*
* Build Deterministic Finite Automata from NFA
*/
static void
Convert_NFA_To_DFA (ACSM_STRUCT * acsm)
{
int r, s;
int i;
QUEUE q, *queue = &q;
/* Init a Queue */
queue_init (queue);
/* Add the state 0 transitions 1st */
for (i = 0; i < ALPHABET_SIZE; i++)
{
s = acsm->acsmStateTable[0].NextState[i];
if (s)
{
queue_add (queue, s);
}
}
/* Start building the next layer of transitions */
while (queue_count (queue) > 0)
{
r = queue_remove (queue);
/* State is a branch state */
for (i = 0; i < ALPHABET_SIZE; i++)
{
if ((s = acsm->acsmStateTable[r].NextState[i]) != ACSM_FAIL_STATE)
{
queue_add (queue, s);
}
else
{
acsm->acsmStateTable[r].NextState[i] =
acsm->acsmStateTable[acsm->acsmStateTable[r].FailState].
NextState[i];
}
}
}
/* Clean up the queue */
queue_free (queue);
}
int usb_vprintf(const char *format, va_list args)
{
int ret;
struct queue state;
if (is_readonly)
return EC_SUCCESS;
ret = usb_wait_console();
if (ret)
return ret;
state = tx_q;
ret = vfnprintf(__tx_char, &state, format, args);
if (queue_count(&state))
handle_output();
return ret;
}
static void *producer(void *data)
{
char *a = "hello";
char *b = "goodbye";
queue *q = (queue *)data;
info("Adding a=%s", a);
queue_add(q, a);
sleep(2);
info("Adding b=%s", b);
queue_add(q, b);
info("count = %d", queue_count(q));
/* wait until all is consumed */
while (!queue_empty(q))
sleep(1);
return NULL;
}
static enum resp_states get_srq_wqe(struct rxe_qp *qp)
{
struct rxe_srq *srq = qp->srq;
struct rxe_queue *q = srq->rq.queue;
struct rxe_recv_wqe *wqe;
struct ib_event ev;
if (srq->error)
return RESPST_ERR_RNR;
spin_lock_bh(&srq->rq.consumer_lock);
wqe = queue_head(q);
if (!wqe) {
spin_unlock_bh(&srq->rq.consumer_lock);
return RESPST_ERR_RNR;
}
/* note kernel and user space recv wqes have same size */
memcpy(&qp->resp.srq_wqe, wqe, sizeof(qp->resp.srq_wqe));
qp->resp.wqe = &qp->resp.srq_wqe.wqe;
advance_consumer(q);
if (srq->limit && srq->ibsrq.event_handler &&
(queue_count(q) < srq->limit)) {
srq->limit = 0;
goto event;
}
spin_unlock_bh(&srq->rq.consumer_lock);
return RESPST_CHK_LENGTH;
event:
spin_unlock_bh(&srq->rq.consumer_lock);
ev.device = qp->ibqp.device;
ev.element.srq = qp->ibqp.srq;
ev.event = IB_EVENT_SRQ_LIMIT_REACHED;
srq->ibsrq.event_handler(&ev, srq->ibsrq.srq_context);
return RESPST_CHK_LENGTH;
}
static void button_queue_wait(struct queue_event *evp, int timeout)
{
/* Loop once after wait time if boosted in order to unboost and wait the
full remaining time */
do
{
int ticks = timeout;
if (ticks == 0) /* TIMEOUT_NOBLOCK */
;
else if (ticks > 0)
{
if (button_boosted && ticks > BUTTON_UNBOOST_TMO)
ticks = BUTTON_UNBOOST_TMO;
timeout -= ticks;
}
else /* TIMEOUT_BLOCK (ticks < 0) */
{
if (button_boosted)
ticks = BUTTON_UNBOOST_TMO;
}
queue_wait_w_tmo(&button_queue, evp, ticks);
if (evp->id != SYS_TIMEOUT)
{
/* GUI boost build gets immediate kick, otherwise at least 3
messages had to be there */
#ifndef HAVE_GUI_BOOST
if (queue_count(&button_queue) >= 2)
#endif
button_boost(true);
break;
}
if (button_boosted && TIME_AFTER(current_tick, button_unboost_tick))
button_boost(false);
}
while (timeout);
}
int usb_puts(const char *outstr)
{
int ret;
struct queue state;
if (is_readonly)
return EC_SUCCESS;
ret = usb_wait_console();
if (ret)
return ret;
state = tx_q;
while (*outstr)
if (__tx_char(&state, *outstr++))
break;
if (queue_count(&state))
handle_output();
return *outstr ? EC_ERROR_OVERFLOW : EC_SUCCESS;
}
int32_t msgqueue_push( struct msgqueue * self, struct task * task, uint8_t isnotify )
{
int32_t rc = -1;
uint32_t isbc = 0;
pthread_spin_lock( &self->lock );
rc = queue_push( self->queue, task );
if ( isnotify != 0 )
{
isbc = queue_count( self->queue );
}
pthread_spin_unlock( &self->lock );
if ( rc == 0 && isbc == 1 )
{
char buf[1] = {0};
write( self->pushfd, buf, 1 );
}
return rc;
}
long button_get(bool block)
{
struct queue_event ev;
int pending_count = queue_count(&button_queue);
#ifdef HAVE_ADJUSTABLE_CPU_FREQ
/* Control the CPU boost trying to keep queue empty. */
if (pending_count == 0)
button_boost(false);
else if (pending_count > 2)
button_boost(true);
#endif
if ( block || pending_count )
{
queue_wait(&button_queue, &ev);
button_data = ev.data;
return ev.id;
}
return BUTTON_NONE;
}
int main()
{
/* queue.h testing */
uint32_t i,x,y,z;
void* data;
for(x=0;x<1000;x++)
{
queue_t* q= queue_create(500);
if(q==NULL) fprintf(stderr,"Error create");;
for(y=0;y<600;y++)
{
data = malloc(10);
for(i=0;i<10;i++)
{
((uint8_t*)data)[i] = i;
}
if(queue_add(q,data)!=0) fprintf(stderr,"Error add");
if(y%3==1)
{
data = queue_get(q);
if(data==NULL) fprintf(stderr,"Error data");
free(data);
}
}
uint32_t count = queue_count(q);
for(z=0;z<count;z++)
{
data = queue_get(q);
if(data==NULL) fprintf(stderr,"Error get");;
free(data);
}
if(queue_count(q)!=0) fprintf(stderr,"Error count");
}
queue_t* q= queue_create(2);
data = malloc(2);
((uint32_t*) data)[0] = 1;
((uint32_t*) data)[1] = 2;
if(queue_add(q,data)!=0)return -1;
data = malloc(2);
((uint32_t*) data)[0] = 3;
((uint32_t*) data)[1] = 4;
if(queue_add(q,data)!=0)return -1;
data = queue_get(q);
if(data==NULL) return -1;
printf("%d%d",((uint32_t*) data)[0],((uint32_t*) data)[1]);
free(data);
data = malloc(2);
((uint32_t*) data)[0] = 5;
((uint32_t*) data)[1] = 6;
if(queue_add(q,data)!=0)return -1;
data = queue_get(q);
if(data==NULL) return -1;
printf("%d%d",((uint32_t*) data)[0],((uint32_t*) data)[1]);
free(data);
data = queue_get(q);
if(data==NULL) return -1;
printf("%d%d",((uint32_t*) data)[0],((uint32_t*) data)[1]);
free(data);
if(queue_count(q)!=0) return -1;
printf("\nEverything is OK!!!!!\n");
getchar();
printf("void %d\n",sizeof(void*));
/* List testing */
clock_t t1 = clock();
for(i=0;i<10000;i++)
{
list_t* list = list_ncreate(100);
list_destroy(list);
}
clock_t t2 = clock();
double dif = difftime ( t2, t1 )/10000;
printf("average time: %f \n",dif);
list_t* list = list_ncreate(100000);
for(i=0;i<1000;i++)
{
data = malloc(10000);
*((int*)data) = i%10;
if(list_add(list,data)!=0) {printf("List remove error");return 60;}
}
//list_add(list,data);
printf("list count: %d\n",list_count(list));
//getchar();
for(i=0;i<900;i++)
{
if(list_remove(list,0)!=0)
{
printf("List remove error");
return -1;
}
}
printf("list count: %d\n",list_count(list));
for(i=0;i<list_count(list);i++)
{
printf("%d\n",*((int*)(list_get(list,i))));
}
list_destroy(list);
printf("List is ok");
int* po = (int*) malloc(sizeof(int*));
free(po);
free(po);
return 0;
}
static uint16_t usb_spi_read_packet(struct usb_spi_config const *config)
{
return QUEUE_REMOVE_UNITS(config->consumer.queue, config->buffer,
queue_count(config->consumer.queue));
}
/*
* Build Non-Deterministic Finite Automata
*/
static void
Build_NFA (ACSM_STRUCT * acsm)
{
int r, s;
int i;
QUEUE q, *queue = &q;
ACSM_PATTERN * mlist=0;
ACSM_PATTERN * px=0;
/* Init a Queue */
queue_init (queue);
/* Add the state 0 transitions 1st */
for (i = 0; i < ALPHABET_SIZE; i++)
{
s = acsm->acsmStateTable[0].NextState[i];
if (s)
{
queue_add (queue, s);
acsm->acsmStateTable[s].FailState = 0;
}
}
/* Build the fail state transitions for each valid state */
while (queue_count (queue) > 0)
{
r = queue_remove (queue);
/* Find Final States for any Failure */
for (i = 0; i < ALPHABET_SIZE; i++)
{
int fs, next;
if ((s = acsm->acsmStateTable[r].NextState[i]) != ACSM_FAIL_STATE)
{
queue_add (queue, s);
fs = acsm->acsmStateTable[r].FailState;
/*
* Locate the next valid state for 'i' starting at s
*/
while ((next=acsm->acsmStateTable[fs].NextState[i]) ==
ACSM_FAIL_STATE)
{
fs = acsm->acsmStateTable[fs].FailState;
}
/*
* Update 's' state failure state to point to the next valid state
*/
acsm->acsmStateTable[s].FailState = next;
/*
* Copy 'next'states MatchList to 's' states MatchList,
* we copy them so each list can be AC_FREE'd later,
* else we could just manipulate pointers to fake the copy.
*/
for (mlist = acsm->acsmStateTable[next].MatchList;
mlist != NULL ;
mlist = mlist->next)
{
px = CopyMatchListEntry (mlist);
if( !px )
{
//FatalError("*** Out of memory Initializing Aho Corasick in acsmx.c ****");
}
/* Insert at front of MatchList */
px->next = acsm->acsmStateTable[s].MatchList;
acsm->acsmStateTable[s].MatchList = px;
}
}
}
}
/* Clean up the queue */
queue_free (queue);
}
int button_queue_count( void )
{
return queue_count(&button_queue);
}
