static void console_save(unsigned char *buffer,int length)
{
msgqueue_t *msg;
/*
* Get a pointer to the last message in the queue. If
* it's full, preprare to allocate a new one
*/
msg = (msgqueue_t *) console_msgq.q_prev;
if (q_isempty(&(console_msgq)) || (msg->len == MSGQUEUESIZE)) {
msg = NULL;
}
/*
* Stuff characters into message chunks till we're done
*/
while (length) {
/*
* New chunk
*/
if (msg == NULL) {
msg = (msgqueue_t *) KMALLOC(sizeof(msgqueue_t),0);
if (msg == NULL) return;
msg->len = 0;
q_enqueue(&console_msgq,(queue_t *) msg);
/*
* Remove chunks to prevent chewing too much memory
*/
while (q_count(&console_msgq) > MSGQUEUEMAX) {
msgqueue_t *dropmsg;
dropmsg = (msgqueue_t *) q_deqnext(&console_msgq);
if (dropmsg) KFREE(dropmsg);
}
}
/*
* Save text. If we run off the end of the buffer, prepare
* to allocate a new one
*/
msg->data[msg->len++] = *buffer++;
length--;
if (msg->len == MSGQUEUESIZE) msg = NULL;
}
}
bool
my_queue_mb_remove(struct my_queue *q, void *item, unsigned *pcount)
{
bool res = false;
unsigned head = MY_ACCESS_ONCE(q->head);
unsigned tail = q->tail;
unsigned count = q_count(head, tail, q->num_elems);
if (count >= 1) {
memcpy(item, &q->data[tail * q->sizeof_elem], q->sizeof_elem);
smp_mb();
q->tail = (tail + 1) & (q->num_elems - 1);
res = true;
count--;
}
if (pcount != NULL)
*pcount = count;
return (res);
}
bool
my_queue_remove(struct my_queue *q, void **pitem, unsigned *pcount)
{
q_lock(q);
bool res = false;
unsigned head = q->head;
unsigned tail = q->tail;
unsigned count = q_count(head, tail, q->size);
if (count >= 1) {
*pitem = q->elems[tail];
q->tail = (tail + 1) & (q->size - 1);
res = true;
count--;
}
q_unlock(q);
if (pcount)
*pcount = count;
return (res);
}