mayhem-firmware/firmware/common/fifo.hpp

/*
 * Copyright (C) 2013 Jared Boone, ShareBrained Technology, Inc.
 *
 * This file is part of PortaPack.
 *
 * 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, 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; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifndef __FIFO_H__
#define __FIFO_H__

#include <cstddef>
#include <algorithm>
#include <cstring>
#include <memory>

#include <hal.h>

/* FIFO implementation inspired by Linux kfifo. */

template <typename T>
class FIFO {
   public:
    constexpr FIFO(
        T* data,
        size_t k)
        : _data{data},
          _size{1U << k},
          _in{0},
          _out{0} {
    }

    void reset() {
        _in = _out = 0;
    }

    void reset_in() {
        _in = _out;
    }

    void reset_out() {
        _out = _in;
    }

    size_t len() const {
        return _in - _out;
    }

    size_t unused() const {
        return size() - (_in - _out);
    }

    bool is_empty() const {
        return _in == _out;
    }

    bool is_full() const {
        return unused() == 0;
    }

    bool in(const T& val) {
        if (is_full()) {
            return false;
        }

        _data[_in & mask()] = val;
        smp_wmb();
        _in += 1;

        return true;
    }

    size_t in(const T* const buf, size_t len) {
        const size_t l = unused();
        if (len > l) {
            len = l;
        }

        copy_in(buf, len, _in);
        _in += len;
        return len;
    }

    size_t in_r(const void* const buf, const size_t len) {
        if ((len + recsize()) > unused()) {
            return 0;
        }

        poke_n(len);
        copy_in((const T*)buf, len, _in + recsize());
        _in += len + recsize();
        return len;
    }

    bool out(T& val) {
        if (is_empty()) {
            return false;
        }

        val = _data[_out & mask()];  // Crashes
        smp_wmb();                   // Ok
        _out += 1;                   // Crashes

        return true;
    }

    size_t out(T* const buf, size_t len) {
        len = out_peek(buf, len);
        _out += len;
        return len;
    }

    bool skip() {
        if (is_empty()) {
            return false;
        }

        size_t len = peek_n();
        _out += len + recsize();
        return true;
    }

    size_t peek_r(void* const buf, size_t len) {
        if (is_empty()) {
            return 0;
        }

        size_t n;
        len = out_copy_r((T*)buf, len, &n);
        return len;
    }

    size_t out_r(void* const buf, size_t len) {
        if (is_empty()) {
            return 0;
        }

        size_t n;
        len = out_copy_r((T*)buf, len, &n);
        _out += n + recsize();
        return len;
    }

   private:
    size_t size() const {
        return _size;
    }

    static constexpr size_t esize() {
        return sizeof(T);
    }

    size_t mask() const {
        return size() - 1;
    }

    static constexpr size_t recsize() {
        return 2;
    }

    void smp_wmb() {
        __DMB();
    }

    size_t peek_n() {
        size_t l = _data[_out & mask()];
        if (recsize() > 1) {
            l |= _data[(_out + 1) & mask()] << 8;
        }
        return l;
    }

    void poke_n(const size_t n) {
        _data[_in & mask()] = n & 0xff;
        if (recsize() > 1) {
            _data[(_in + 1) & mask()] = (n >> 8) & 0xff;
        }
    }

    void copy_in(const T* const src, const size_t len, size_t off) {
        off &= mask();
        const size_t l = std::min(len, size() - off);

        memcpy(&_data[off], &src[0], l * esize());
        memcpy(&_data[0], &src[l], (len - l) * esize());
        smp_wmb();
    }

    void copy_out(T* const dst, const size_t len, size_t off) {
        off &= mask();
        const size_t l = std::min(len, size() - off);

        memcpy(&dst[0], &_data[off], l * esize());
        memcpy(&dst[l], &_data[0], (len - l) * esize());
        smp_wmb();
    }

    size_t out_copy_r(void* buf, size_t len, size_t* const n) {
        *n = peek_n();

        if (len > *n) {
            len = *n;
        }

        copy_out((T*)buf, len, _out + recsize());
        return len;
    }

    size_t out_peek(T* const buf, size_t buf_len) {
        const size_t l = len();
        if (buf_len > l) {
            buf_len = l;
        }

        copy_out(buf, buf_len, _out);
        return buf_len;
    }

    T* const _data;
    const size_t _size;
    volatile size_t _in;
    volatile size_t _out;
};

#endif /*__FIFO_H__*/