lib/minip/pktbuf.c

/*
 * Copyright (c) 2014 Brian Swetland
 * Copyright (c) 2014-2015 Christopher Anderson
 *
 * Use of this source code is governed by a MIT-style
 * license that can be found in the LICENSE file or at
 * https://opensource.org/licenses/MIT
 */

#include <assert.h>
#include <lk/debug.h>
#include <lk/trace.h>
#include <printf.h>
#include <string.h>
#include <malloc.h>

#include <kernel/thread.h>
#include <kernel/semaphore.h>
#include <kernel/spinlock.h>
#include <lib/pktbuf.h>
#include <lib/pool.h>
#include <lk/init.h>

#if WITH_KERNEL_VM
#include <kernel/vm.h>
#endif

#define LOCAL_TRACE 0

static pool_t pktbuf_pool;
static semaphore_t pktbuf_sem;
static spin_lock_t lock;


/* Take an object from the pool of pktbuf objects to act as a header or buffer.  */
static void *get_pool_object(void) {
    pool_t *entry;
    spin_lock_saved_state_t state;

    sem_wait(&pktbuf_sem);
    spin_lock_irqsave(&lock, state);
    entry = pool_alloc(&pktbuf_pool);
    spin_unlock_irqrestore(&lock, state);

    return (pktbuf_pool_object_t *) entry;

}

/* Return an object to thje pktbuf object pool. */
static void free_pool_object(pktbuf_pool_object_t *entry, bool reschedule) {
    DEBUG_ASSERT(entry);
    spin_lock_saved_state_t state;

    spin_lock_irqsave(&lock, state);
    pool_free(&pktbuf_pool, entry);
    spin_unlock_irqrestore(&lock, state);
    sem_post(&pktbuf_sem, reschedule);
}

/* Callback used internally to place a pktbuf_pool_object back in the pool after
 * it was used as a buffer for another pktbuf
 */
static void free_pktbuf_buf_cb(void *buf, void *arg) {
    free_pool_object((pktbuf_pool_object_t *)buf, true);
}

/* Add a buffer to a pktbuf. Header space for prepending data is adjusted based on
 * header_sz. cb is called when the pktbuf is freed / released by the driver level
 * and should handle proper management / freeing of the buffer pointed to by the iovec.
 *
 * It's important to note that there is a flag to note that the buffer is cached and should
 * be properly handled via the appropriate driver when it's time to deal with buffer
 * descriptors.
 */
void pktbuf_add_buffer(pktbuf_t *p, u8 *buf, u32 len, uint32_t header_sz, uint32_t flags,
                       pktbuf_free_callback cb, void *cb_args) {
    DEBUG_ASSERT(p);
    DEBUG_ASSERT(buf);
    DEBUG_ASSERT(header_sz < len);

    p->buffer = buf;
    p->blen = len;
    p->data = p->buffer + header_sz;
    p->dlen = 0;
    p->flags = PKTBUF_FLAG_EOF | flags;
    p->cb = cb;
    p->cb_args = cb_args;

    /* If we're using a VM then this may be a virtual address, look up to see
     * if there is an associated physical address we can store. If not, then
     * stick with the address as presented to us.
     */
#if WITH_KERNEL_VM
    p->phys_base = vaddr_to_paddr(buf) | (uintptr_t) buf % PAGE_SIZE;
#else
    p->phys_base = (uintptr_t) buf;
#endif
}

pktbuf_t *pktbuf_alloc(void) {
    pktbuf_t *p = NULL;
    void *buf = NULL;

    p = get_pool_object();
    if (!p) {
        return NULL;
    }

    buf = get_pool_object();
    if (!buf) {
        free_pool_object((pktbuf_pool_object_t *)p, false);
        return NULL;
    }

    memset(p, 0, sizeof(pktbuf_t));
    pktbuf_add_buffer(p, buf, PKTBUF_SIZE, PKTBUF_MAX_HDR, 0, free_pktbuf_buf_cb, NULL);
    return p;
}

pktbuf_t *pktbuf_alloc_empty(void) {
    pktbuf_t *p = (pktbuf_t *) get_pool_object();

    p->flags = PKTBUF_FLAG_EOF;
    return p;
}

int pktbuf_free(pktbuf_t *p, bool reschedule) {
    DEBUG_ASSERT(p);

    if (p->cb) {
        p->cb(p->buffer, p->cb_args);
    }
    free_pool_object((pktbuf_pool_object_t *)p, false);

    return 1;
}

void pktbuf_append_data(pktbuf_t *p, const void *data, size_t sz) {
    if (pktbuf_avail_tail(p) < sz) {
        panic("pktbuf_append_data: overflow");
    }

    memcpy(p->data + p->dlen, data, sz);
    p->dlen += sz;
}

void *pktbuf_append(pktbuf_t *p, size_t sz) {
    if (pktbuf_avail_tail(p) < sz) {
        panic("pktbuf_append: overflow");
    }

    void *data = p->data + p->dlen;
    p->dlen += sz;

    return data;
}

void *pktbuf_prepend(pktbuf_t *p, size_t sz) {
    if (pktbuf_avail_head(p) < sz) {
        panic("pktbuf_prepend: not enough space");
    }

    p->dlen += sz;
    p->data -= sz;

    return p->data;
}

void *pktbuf_consume(pktbuf_t *p, size_t sz) {
    void *data = p->data;

    if (sz > p->dlen) {
        return NULL;
    }

    p->data += sz;
    p->dlen -= sz;

    return data;
}

void pktbuf_consume_tail(pktbuf_t *p, size_t sz) {
    if (sz > p->dlen) {
        p->dlen = 0;
        return;
    }

    p->dlen -= sz;
}

void pktbuf_dump(pktbuf_t *p) {
    printf("pktbuf data %p, buffer %p, dlen %u, data offset %lu, phys_base %p\n",
           p->data, p->buffer, p->dlen, (uintptr_t) p->data - (uintptr_t) p->buffer,
           (void *)p->phys_base);
}

static void pktbuf_init(uint level) {
    void *slab;

#if LK_DEBUGLEVEL > 0
    printf("pktbuf: creating %u pktbuf entries of size %zu (total %zu)\n",
           PKTBUF_POOL_SIZE, sizeof(struct pktbuf_pool_object),
           PKTBUF_POOL_SIZE * sizeof(struct pktbuf_pool_object));
#endif

#if WITH_KERNEL_VM
    if (vmm_alloc_contiguous(vmm_get_kernel_aspace(), "pktbuf",
                             PKTBUF_POOL_SIZE * sizeof(struct pktbuf_pool_object),
                             &slab, 0, 0, ARCH_MMU_FLAG_CACHED) < 0) {
        printf("Failed to initialize pktbuf hdr slab\n");
        return;
    }
#else
    slab = memalign(CACHE_LINE, PKTBUF_POOL_SIZE * sizeof(pktbuf_pool_object_t));
#endif

    pool_init(&pktbuf_pool, sizeof(struct pktbuf_pool_object), CACHE_LINE, PKTBUF_POOL_SIZE, slab);
    sem_init(&pktbuf_sem, PKTBUF_POOL_SIZE);
}

LK_INIT_HOOK(pktbuf, pktbuf_init, LK_INIT_LEVEL_THREADING);