diff options
Diffstat (limited to 'src/target/firmware/comm/msgb.c')
-rw-r--r-- | src/target/firmware/comm/msgb.c | 134 |
1 files changed, 134 insertions, 0 deletions
diff --git a/src/target/firmware/comm/msgb.c b/src/target/firmware/comm/msgb.c new file mode 100644 index 00000000..8245ed20 --- /dev/null +++ b/src/target/firmware/comm/msgb.c @@ -0,0 +1,134 @@ +/* (C) 2008-2010 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <sys/types.h> + +#include <debug.h> + +#include <comm/msgb.h> + +#include <calypso/backlight.h> + +#define NO_TALLOC + +void *tall_msgb_ctx; + +#ifdef NO_TALLOC +/* This is a poor mans static allocator for msgb objects */ +#define MSGB_DATA_SIZE 256 +#define MSGB_NUM 16 +struct supermsg { + uint8_t allocated; + struct msgb msg; + uint8_t buf[MSGB_DATA_SIZE]; +}; +static struct supermsg msgs[MSGB_NUM]; +static void *_talloc_zero(void *ctx, unsigned int size, const char *name) +{ + unsigned int i; + if (size > sizeof(struct msgb) + MSGB_DATA_SIZE) + goto panic; + for (i = 0; i < ARRAY_SIZE(msgs); i++) { + if (!msgs[i].allocated) { + msgs[i].allocated = 1; + memset(&msgs[i].msg, 0, sizeof(&msgs[i].msg)); + memset(&msgs[i].buf, 0, sizeof(&msgs[i].buf)); + return &msgs[i].msg; + } + } + +panic: + while (1) { + bl_level(++i % 50); + delay_ms(50); + } + return NULL; +} +static void talloc_free(void *msg) +{ + struct supermsg *smsg = container_of(msg, struct supermsg, msg); + smsg->allocated = 0; +} +#endif + +struct msgb *msgb_alloc(uint16_t size, const char *name) +{ + struct msgb *msg; + + msg = _talloc_zero(tall_msgb_ctx, sizeof(*msg) + size, name); + + if (!msg) { + cons_puts("unable to allocate msgb\n"); + return NULL; + } + + msg->data_len = size; + msg->len = 0; + msg->data = msg->_data; + + msg->head = msg->data; + msg->data = msg->data; + /* reset tail pointer */ + msg->tail = msg->data; + + return msg; +} + +void msgb_free(struct msgb *m) +{ + talloc_free(m); +} + +void msgb_enqueue(struct llist_head *queue, struct msgb *msg) +{ + llist_add_tail(&msg->list, queue); +} + +struct msgb *msgb_dequeue(struct llist_head *queue) +{ + struct llist_head *lh; + + if (llist_empty(queue)) + return NULL; + + lh = queue->next; + llist_del(lh); + + return llist_entry(lh, struct msgb, list); +} + +void msgb_reset(struct msgb *msg) +{ + msg->len = 0; + msg->len = 0; + msg->data = msg->_data; + + msg->head = msg->data; + msg->data = msg->data; + /* reset tail pointer */ + msg->tail = msg->data; + + /* reset pointers */ + msg->l2h = NULL; + msg->l3h = NULL; +} |