1  /* -*- mode: c; tab-width: 8 -*-
  2   */
  3  
  4  #include "queue.h"
  5  
  6  bgrt_st_t bgrt_queue_init_cs(bgrt_queue_t *q, uint16_t size, uint16_t flags)
  7  {
  8  	BGRT_ASSERT(q, "#queue must not be NULL");
  9  	BGRT_ASSERT(q->deq, "#queue->deq must not be NULL");
 10  	BGRT_ASSERT(((flags & Q_CS) || q->enq.sem), "#queue->enq.sem must not be NULL");
 11  
 12  	q->size = size -= 1;
 13  	q->flags = flags;
 14  	q->head = q->tail = 0;
 15  	bgrt_sem_init_cs(q->deq, (flags & Q_REV) ? size : 0);
 16  	if ((flags & Q_CS))
 17  		q->enq.counter = size;
 18  	else
 19  		bgrt_sem_init_cs(q->enq.sem, (flags & Q_REV) ? 0 : size);
 20  
 21  	return BGRT_ST_OK;
 22  }
 23  
 24  bgrt_st_t bgrt_queue_init(bgrt_queue_t *q, uint16_t size, uint16_t flags)
 25  {
 26  	bgrt_st_t st;
 27  	BGRT_INT_LOCK();
 28  	st = bgrt_queue_init_cs(q, size, flags);
 29  	BGRT_INT_FREE();
 30  	return st;
 31  }
 32  
 33  /*
 34   *
 35   */
 36  static void * _queue_idx_inc(bgrt_queue_t *q, bool enqueue)
 37  {
 38  	_Atomic(uint16_t) *obj;
 39  	uint16_t idx, next;
 40  
 41  	enqueue ^= !!(q->flags & Q_REV);
 42  	obj = enqueue ? &q->head : &q->tail;
 43  	idx = *obj;
 44  
 45  	do {
 46  		next = (idx + 1) & q->size;
 47  	} while(!atomic_compare_exchange_strong(obj, &idx, next));
 48  
 49  	return &q->queue[idx];
 50  }
 51  
 52  /*
 53   *
 54   */
 55  bgrt_st_t bgrt_queue_post_fn(bgrt_queue_t *q, void *msg,
 56  			     _queue_sem_t sem_lock_fn,
 57  			     _queue_post_cb_t post_cb)
 58  {
 59  	bgrt_st_t st;
 60  	void **p;
 61  
 62  	if ((st = sem_lock_fn(q->enq.sem)) != BGRT_ST_OK)
 63  		return st;
 64  
 65  	p = _queue_idx_inc(q, true);
 66  	if (post_cb)
 67  		post_cb(p, msg);
 68  	else
 69  		*p = msg;
 70  
 71  	return bgrt_sem_free(q->deq);
 72  }
 73  
 74  bgrt_st_t bgrt_queue_post_cs_fn(bgrt_queue_t *q, void *msg,
 75  				_queue_post_cb_t post_cb)
 76  {
 77  	void **p;
 78  
 79  	if (q->enq.counter == 0)
 80  		return BGRT_ST_ROLL;
 81  
 82  	atomic_fetch_sub(&q->enq.counter, 1);
 83  
 84  	p = _queue_idx_inc(q, true);
 85  	if (post_cb)
 86  		post_cb(p, msg);
 87  	else
 88  		*p = msg;
 89  
 90  	return bgrt_sem_free_cs(q->deq);
 91  }
 92  
 93  bgrt_st_t bgrt_queue_fetch_fn(bgrt_queue_t *q, void **msg,
 94  			      _queue_sem_t sem_lock_fn)
 95  {
 96  	bool q_sw = !!(q->flags & Q_SW);
 97  	bool q_cs = !!(q->flags & Q_CS);
 98  	bgrt_st_t st;
 99  	void **p;
100  	void *t;
101  
102  	if ((st = sem_lock_fn(q->deq)) != BGRT_ST_OK)
103  		return st;
104  
105  	p = _queue_idx_inc(q, false);
106  	t = *p;
107  
108  	if (q_sw) *p = *msg;
109  	*msg = t;
110  
111  	return q_cs ?
112  		atomic_fetch_add(&q->enq.counter, 1), BGRT_ST_OK :
113  		bgrt_sem_free(q->enq.sem);
114  }