/**
* @brief Output queue write with timeout.
* @details The function writes data from a buffer to an output queue. The
* operation completes when the specified amount of data has been
* transferred or after the specified timeout or if the queue has
* been reset.
* @note The function is not atomic, if you need atomicity it is suggested
* to use a semaphore or a mutex for mutual exclusion.
* @note The callback is invoked after writing each character into the
* buffer.
*
* @param[in] oqp pointer to an @p OutputQueue structure
* @param[out] bp pointer to the data buffer
* @param[in] n the maximum amount of data to be transferred, the
* value 0 is reserved
* @param[in] time the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_IMMEDIATE immediate timeout.
* - @a TIME_INFINITE no timeout.
* .
* @return The number of bytes effectively transferred.
*
* @api
*/
size_t chOQWriteTimeout(OutputQueue *oqp, const uint8_t *bp,
size_t n, systime_t time) {
qnotify_t nfy = oqp->q_notify;
size_t w = 0;
chDbgCheck(n > 0, "chOQWriteTimeout");
chSysLock();
while (TRUE) {
while (chOQIsFullI(oqp)) {
if (qwait((GenericQueue *)oqp, time) != Q_OK) {
chSysUnlock();
return w;
}
}
oqp->q_counter--;
*oqp->q_wrptr++ = *bp++;
if (oqp->q_wrptr >= oqp->q_top)
oqp->q_wrptr = oqp->q_buffer;
if (nfy)
nfy(oqp);
chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
w++;
if (--n == 0)
return w;
chSysLock();
}
}
/**
* @brief Input queue read with timeout.
* @details The function reads data from an input queue into a buffer. The
* operation completes when the specified amount of data has been
* transferred or after the specified timeout or if the queue has
* been reset.
* @note The function is not atomic, if you need atomicity it is suggested
* to use a semaphore or a mutex for mutual exclusion.
* @note The callback is invoked before reading each character from the
* buffer or before entering the state @p THD_STATE_WTQUEUE.
*
* @param[in] iqp pointer to an @p InputQueue structure
* @param[out] bp pointer to the data buffer
* @param[in] n the maximum amount of data to be transferred, the
* value 0 is reserved
* @param[in] time the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_IMMEDIATE immediate timeout.
* - @a TIME_INFINITE no timeout.
* .
* @return The number of bytes effectively transferred.
*
* @api
*/
size_t chIQReadTimeout(InputQueue *iqp, uint8_t *bp,
size_t n, systime_t time) {
qnotify_t nfy = iqp->q_notify;
size_t r = 0;
chDbgCheck(n > 0, "chIQReadTimeout");
chSysLock();
while (TRUE) {
if (nfy)
nfy(iqp);
while (chIQIsEmptyI(iqp)) {
if (qwait((GenericQueue *)iqp, time) != Q_OK) {
chSysUnlock();
return r;
}
}
iqp->q_counter--;
*bp++ = *iqp->q_rdptr++;
if (iqp->q_rdptr >= iqp->q_top)
iqp->q_rdptr = iqp->q_buffer;
chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
r++;
if (--n == 0)
return r;
chSysLock();
}
}
/*
* get next block from a queue (up to a limit)
*/
Block*
qbread(Queue *q, int len)
{
Block *b, *nb;
int n;
qlock(&q->rlock);
if(waserror()){
qunlock(&q->rlock);
nexterror();
}
ilock(q);
switch(qwait(q)){
case 0:
/* queue closed */
iunlock(q);
qunlock(&q->rlock);
poperror();
return nil;
case -1:
/* multiple reads on a closed queue */
iunlock(q);
error(q->err);
}
/* if we get here, there's at least one block in the queue */
b = qremove(q);
n = BLEN(b);
/* split block if it's too big and this is not a message queue */
nb = b;
if(n > len){
if((q->state&Qmsg) == 0){
n -= len;
b = allocb(n);
memmove(b->wp, nb->rp+len, n);
b->wp += n;
qputback(q, b);
}
nb->wp = nb->rp + len;
}
/* restart producer */
qwakeup_iunlock(q);
poperror();
qunlock(&q->rlock);
return nb;
}
/*
* get next block from a queue (up to a limit)
*/
struct block *qbread(struct queue *q, int len)
{
ERRSTACK(1);
struct block *b, *nb;
int n;
qlock(&q->rlock);
if (waserror()) {
qunlock(&q->rlock);
nexterror();
}
spin_lock_irqsave(&q->lock);
if (!qwait(q)) {
/* queue closed */
spin_unlock_irqsave(&q->lock);
qunlock(&q->rlock);
poperror();
return NULL;
}
/* if we get here, there's at least one block in the queue */
b = qremove(q);
n = BLEN(b);
/* split block if it's too big and this is not a message queue */
nb = b;
if (n > len) {
PANIC_EXTRA(b);
if ((q->state & Qmsg) == 0) {
n -= len;
b = allocb(n);
memmove(b->wp, nb->rp + len, n);
b->wp += n;
qputback(q, b);
}
nb->wp = nb->rp + len;
}
/* restart producer */
qwakeup_iunlock(q);
poperror();
qunlock(&q->rlock);
return nb;
}
/**
* @brief Output queue write with timeout.
* @details This function writes a byte value to an output queue. If the queue
* is full then the calling thread is suspended until there is space
* in the queue or a timeout occurs.
* @note The callback is invoked after writing the character into the
* buffer.
*
* @param[in] oqp pointer to an @p OutputQueue structure
* @param[in] b the byte value to be written in the queue
* @param[in] time the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_IMMEDIATE immediate timeout.
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval Q_OK if the operation succeeded.
* @retval Q_TIMEOUT if the specified time expired.
* @retval Q_RESET if the queue has been reset.
*
* @api
*/
msg_t chOQPutTimeout(OutputQueue *oqp, uint8_t b, systime_t time) {
chSysLock();
while (chOQIsFullI(oqp)) {
msg_t msg;
if ((msg = qwait((GenericQueue *)oqp, time)) < Q_OK) {
chSysUnlock();
return msg;
}
}
oqp->q_counter--;
*oqp->q_wrptr++ = b;
if (oqp->q_wrptr >= oqp->q_top)
oqp->q_wrptr = oqp->q_buffer;
if (oqp->q_notify)
oqp->q_notify(oqp);
chSysUnlock();
return Q_OK;
}
/**
* @brief Input queue read with timeout.
* @details This function reads a byte value from an input queue. If the queue
* is empty then the calling thread is suspended until a byte arrives
* in the queue or a timeout occurs.
* @note The callback is invoked before reading the character from the
* buffer or before entering the state @p THD_STATE_WTQUEUE.
*
* @param[in] iqp pointer to an @p InputQueue structure
* @param[in] time the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_IMMEDIATE immediate timeout.
* - @a TIME_INFINITE no timeout.
* .
* @return A byte value from the queue.
* @retval Q_TIMEOUT if the specified time expired.
* @retval Q_RESET if the queue has been reset.
*
* @api
*/
msg_t chIQGetTimeout(InputQueue *iqp, systime_t time) {
uint8_t b;
chSysLock();
if (iqp->q_notify)
iqp->q_notify(iqp);
while (chIQIsEmptyI(iqp)) {
msg_t msg;
if ((msg = qwait((GenericQueue *)iqp, time)) < Q_OK) {
chSysUnlock();
return msg;
}
}
iqp->q_counter--;
b = *iqp->q_rdptr++;
if (iqp->q_rdptr >= iqp->q_top)
iqp->q_rdptr = iqp->q_buffer;
chSysUnlock();
return b;
}
/*
* read a queue. if no data is queued, post a Block
* and wait on its Rendez.
*/
long
qread(Queue *q, void *vp, int len)
{
Block *b, *first, **l;
int m, n;
qlock(&q->rlock);
if(waserror()){
qunlock(&q->rlock);
nexterror();
}
ilock(q);
again:
switch(qwait(q)){
case 0:
/* queue closed */
iunlock(q);
qunlock(&q->rlock);
poperror();
return 0;
case -1:
/* multiple reads on a closed queue */
iunlock(q);
error(q->err);
}
/* if we get here, there's at least one block in the queue */
if(q->state & Qcoalesce){
/* when coalescing, 0 length blocks just go away */
b = q->bfirst;
if(BLEN(b) <= 0){
freeb(qremove(q));
goto again;
}
/* grab the first block plus as many
* following blocks as will completely
* fit in the read.
*/
n = 0;
l = &first;
m = BLEN(b);
for(;;) {
*l = qremove(q);
l = &b->next;
n += m;
b = q->bfirst;
if(b == nil)
break;
m = BLEN(b);
if(n+m > len)
break;
}
} else {
first = qremove(q);
n = BLEN(first);
}
/* copy to user space outside of the ilock */
iunlock(q);
b = bl2mem(vp, first, len);
ilock(q);
/* take care of any left over partial block */
if(b != nil){
n -= BLEN(b);
if(q->state & Qmsg)
freeb(b);
else
qputback(q, b);
}
/* restart producer */
qwakeup_iunlock(q);
poperror();
qunlock(&q->rlock);
return n;
}
/*
* read a queue. if no data is queued, post a struct block
* and wait on its Rendez.
*/
long qread(struct queue *q, void *vp, int len)
{
ERRSTACK(1);
struct block *b, *first, **l;
int m, n;
qlock(&q->rlock);
if (waserror()) {
qunlock(&q->rlock);
nexterror();
}
spin_lock_irqsave(&q->lock);
again:
if (!qwait(q)) {
/* queue closed */
spin_unlock_irqsave(&q->lock);
qunlock(&q->rlock);
poperror();
return 0;
}
/* if we get here, there's at least one block in the queue */
// TODO: Consider removing the Qcoalesce flag and force a coalescing
// strategy by default.
if (q->state & Qcoalesce) {
/* when coalescing, 0 length blocks just go away */
b = q->bfirst;
if (BLEN(b) <= 0) {
freeb(qremove(q));
goto again;
}
/* grab the first block plus as many
* following blocks as will completely
* fit in the read.
*/
n = 0;
l = &first;
m = BLEN(b);
for (;;) {
*l = qremove(q);
l = &b->next;
n += m;
b = q->bfirst;
if (b == NULL)
break;
m = BLEN(b);
if (n + m > len)
break;
}
} else {
first = qremove(q);
n = BLEN(first);
}
/* copy to user space outside of the ilock */
spin_unlock_irqsave(&q->lock);
b = bl2mem(vp, first, len);
spin_lock_irqsave(&q->lock);
/* take care of any left over partial block */
if (b != NULL) {
n -= BLEN(b);
if (q->state & Qmsg)
freeb(b);
else
qputback(q, b);
}
/* restart producer */
qwakeup_iunlock(q);
poperror();
qunlock(&q->rlock);
return n;
}
