device/google/contexthub/firmware/os/core/simpleQ.c

/*
 * Copyright (C) 2016 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <simpleQ.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <heap.h>


struct SimpleQueueEntry {
    uint32_t nextIdx     : 31;
    uint32_t discardable :  1;
    uint8_t data[];
};

struct SimpleQueue {
    SimpleQueueForciblyDiscardCbkF discardCbk;
    uint32_t head, tail, num, freeHead, entrySz;
    uint8_t data[];
};

#define SIMPLE_QUEUE_IDX_NONE (SIMPLE_QUEUE_MAX_ELEMENTS + 1)

//no bounds checking
static inline struct SimpleQueueEntry *simpleQueueGetNth(struct SimpleQueue* sq, uint32_t n)
{
    return (struct SimpleQueueEntry*)(sq->data + n * sq->entrySz);
}

static inline uint32_t simpleQueueGetIdx(struct SimpleQueue* sq, const struct SimpleQueueEntry *e)
{
    return (((const uint8_t*)e) - sq->data) / sq->entrySz;
}

struct SimpleQueue* simpleQueueAlloc(uint32_t numEntries, uint32_t entrySz, SimpleQueueForciblyDiscardCbkF forceDiscardCbk)
{
    uint32_t i, sz = sizeof(struct SimpleQueue) + (sizeof(struct SimpleQueueEntry) + entrySz) * numEntries;
    struct SimpleQueue *sq;

    if (numEntries > SIMPLE_QUEUE_MAX_ELEMENTS)
        return NULL;

    sq = heapAlloc(sz);
    if (!sq)
        return NULL;

    memset(sq, 0, sz);

    sq->discardCbk = forceDiscardCbk;
    sq->head = SIMPLE_QUEUE_IDX_NONE;
    sq->tail = SIMPLE_QUEUE_IDX_NONE;
    sq->entrySz = entrySz + sizeof(struct SimpleQueueEntry);
    sq->freeHead = 0;
    sq->num = numEntries;

    //init the freelist
    for (i = 0; i < numEntries - 1; i++)
        simpleQueueGetNth(sq, i)->nextIdx = i + 1;

    simpleQueueGetNth(sq, numEntries - 1)->nextIdx = SIMPLE_QUEUE_IDX_NONE;

    return sq;
}

void simpleQueueDestroy(struct SimpleQueue* sq)
{
    SimpleQueueForciblyDiscardCbkF discard = sq->discardCbk;
    struct SimpleQueueEntry *cur;
    uint32_t i;

    for (i = sq->head; i != SIMPLE_QUEUE_IDX_NONE; i = cur->nextIdx) {
        cur = simpleQueueGetNth(sq, i);
        discard(cur->data, true);
    }

    heapFree(sq);
}

bool simpleQueueDequeue(struct SimpleQueue* sq, void *data)
{
    struct SimpleQueueEntry *e;
    uint32_t head;

    if (!sq || sq->head == SIMPLE_QUEUE_IDX_NONE)
        return false;

    head = sq->head;
    e = simpleQueueGetNth(sq, head);

    sq->head = e->nextIdx;
    if (sq->tail == head)
        sq->tail = SIMPLE_QUEUE_IDX_NONE;

    memcpy(data, e->data, sq->entrySz - sizeof(struct SimpleQueueEntry));

    e->nextIdx = sq->freeHead;
    sq->freeHead = simpleQueueGetIdx(sq, e);

    return true;
}

//if this is called, we need to discard at least one entry. we prefer to discard the oldest item
static struct SimpleQueueEntry* simpleQueueAllocWithDiscard(struct SimpleQueue* sq)
{
    struct SimpleQueueEntry* cur = NULL;
    uint32_t idx, prev = SIMPLE_QUEUE_IDX_NONE;

    for (idx = sq->head; idx != SIMPLE_QUEUE_IDX_NONE; prev=idx, idx = cur->nextIdx) {
        cur = simpleQueueGetNth(sq, idx);

        if (!cur->discardable)
            continue;

        //try to discard it
        if (sq->discardCbk(cur->data, false)) {

            //unlink
            if (prev == SIMPLE_QUEUE_IDX_NONE)
                sq->head = cur->nextIdx;
            else
                simpleQueueGetNth(sq, prev)->nextIdx = cur->nextIdx;
            if (sq->tail == idx)
                sq->tail = prev;

            return cur;
        }
    }

    return NULL;
}

bool simpleQueueEnqueue(struct SimpleQueue* sq, const void *data, int length, bool possiblyDiscardable)
{
    struct SimpleQueueEntry *e = NULL;

    if (length > sq->entrySz - sizeof(struct SimpleQueueEntry))
        return false;

    //first try a simple alloc
    if (sq->freeHead != SIMPLE_QUEUE_IDX_NONE) {
        e = simpleQueueGetNth(sq, sq->freeHead);
        sq->freeHead = e->nextIdx;
    }

    //if no luck, it gets complicated
    if (!e)
        e = simpleQueueAllocWithDiscard(sq);

    //and we may have to give up
    if (!e)
        return false;

    //link it in
    e->nextIdx = SIMPLE_QUEUE_IDX_NONE;
    if (sq->head == SIMPLE_QUEUE_IDX_NONE) // head = none implies tail = none
        sq->head = simpleQueueGetIdx(sq, e);
    else
        simpleQueueGetNth(sq, sq->tail)->nextIdx = simpleQueueGetIdx(sq, e);
    sq->tail = simpleQueueGetIdx(sq, e);

    //fill in the data
    memcpy(e->data, data, length);
    e->discardable = possiblyDiscardable ? 1 : 0;

    return true;
}