StoreBuffer(仮)

StoreBuffer とりあえずコンパイル通った版。

#include <stdio.h>
#include <string.h>
#include "type.h"
#include "util.h"
#include "Dmac.h"
#include "StoreBuffer.h"




#define MAX_STORE_BUFFER_NUM    4
#define MAX_LINE_NUM            32
#define MIN_LINE_SIZE           16




typedef struct STORE_BUFFER_T {
    UINT32                      buffer;         /* store buffer */
    UINT32                      capacity;       /* size of buffer in bytes */
    UINT32                      lineSize;       /* size of cache line in bytes */
    UINT32                      lineNum;        /* count of cache line */
    UINT32                      lineUsed;       /* count of used cache line */
    UINT32                      lineDict[MAX_LINE_NUM]; /* high bit: destination address.
                                                           low  bit: index of cache line.
                                                           The number of bits changes according to
                                                           the lineSize.
                                                           */
    UINT32                      destBeg;        /* begin address of destination */
    UINT32                      destEnd;        /* end address of destination (exclude this) */
    UINT8                       flag;           /* flag */
} STORE_BUFFER_T;

/* STORE_BUFFER_T.flag */
#define FLAG_USED               0x01
#define FLAG_PRELOAD            0x02



typedef struct STORE_BUFFER_WORK_T {
    STORE_BUFFER_T              storeBuffer[MAX_STORE_BUFFER_NUM];
} STORE_BUFFER_WORK_T;




PRIVATE STORE_BUFFER_WORK_T     sMtkStoreBufferWork;



#define work                    (&sMtkStoreBufferWork)
#define getItem(id)             (&work->storeBuffer[id])
#define checkID(id)             ((0 <= id) && (id < MAX_STORE_BUFFER_NUM))



PRIVATE UINT32 MostLeftHighBit(UINT32 data);
PRIVATE void* GetLine(STORE_BUFFER_T* self, void* dest);
PRIVATE UINT32 GetDict(STORE_BUFFER_T* self, void* dest);
PRIVATE UINT32* SearchDict(STORE_BUFFER_T* self, void* dest);
PRIVATE UINT32* AllocDict(STORE_BUFFER_T* self, void* dest);
PRIVATE void Preload(STORE_BUFFER_T* self, UINT32 dict);
PRIVATE void RotateMruDict(STORE_BUFFER_T* self, UINT32 index);
PRIVATE void FlushLruDict(STORE_BUFFER_T* self);
PRIVATE BOOL CheckDestRange(STORE_BUFFER_T* self, void* dest);
PRIVATE UINT32 WriteLine(STORE_BUFFER_T* self, void* line, const void* dest, const void* src, UINT32 size);
PRIVATE UINT32 FillLine(STORE_BUFFER_T* self, void* line, const void* dest, INT filler, UINT32 size);
PRIVATE void ShellSort(UINT32* arr, UINT32 num);
PRIVATE void JoinWrite(STORE_BUFFER_T* self);




void MtkInitializeStoreBuffer(void)
{
    memset(work, 0, sizeof(*work));
}

INT MtkCreateStoreBuffer(void* buffer, UINT32 capacity, UINT32 lineSize, void* destBeg, void* destEnd)
{
    INT id;
    STORE_BUFFER_T* sb;
    INT i;

    for (id = 0; id < MAX_STORE_BUFFER_NUM; ++id) {
        sb = getItem(id);
        if (MTK_BTST(sb->flag, FLAG_USED) == 0) {
            break;
        }
    }
    if (id >= MAX_STORE_BUFFER_NUM) {
        return INVALID_STORE_BUFFER_ID;
    }

    lineSize = MostLeftHighBit(lineSize);
    lineSize = MTK_MIN(lineSize, capacity);
    lineSize = MTK_MAX(lineSize, MIN_LINE_SIZE);

    sb->buffer   = (UINT32) buffer;
    sb->capacity = capacity;
    sb->lineSize = lineSize;
    sb->lineNum  = capacity / lineSize;
    sb->lineNum  = MTK_MIN(sb->lineNum, MAX_LINE_NUM);
    sb->lineUsed = 0;
    for (i = 0; i < MAX_LINE_NUM; ++i) {
        sb->lineDict[i] = i;                    /* set cache line id into low bits */
    }
    sb->destBeg  = (UINT32) destBeg;
    sb->destEnd  = (UINT32) destEnd;
    sb->flag     = FLAG_USED;

    return id;
}

void MtkDestroyStoreBuffer(INT id)
{
    STORE_BUFFER_T* sb;

    if (! checkID(id)) {
        return;
    }

    MtkFlushStoreBuffer(id);

    sb = getItem(id);

    MTK_BCLR(sb->flag, FLAG_USED);
}

BOOL MtkSetPreload(INT id,  BOOL preload)
{
    STORE_BUFFER_T* sb;

    if (! checkID(id)) {
        return FALSE;
    }

    sb = getItem(id);

    if (preload) {
        MTK_BSET(sb->flag, FLAG_PRELOAD);
    }
    else {
        MTK_BCLR(sb->flag, FLAG_PRELOAD);
    }

    return TRUE;
}

BOOL MtkWriteStoreBuffer(INT id, void* dest, const void* src, UINT32 size)
{
    STORE_BUFFER_T* sb;
    void* line;
    UINT32 written;

    if (! checkID(id)) {
        return FALSE;
    }

    sb = getItem(id);

    if (! CheckDestRange(sb, dest)) {
        return FALSE;
    }

    do {
        line    = GetLine(sb, dest);
        written = WriteLine(sb, line, dest, src, size);
        dest    = (      void*)((UINT32)dest + written);
        src     = (const void*)((UINT32)src  + written);
        size   -= written;
    } while (size);

    return TRUE;
}

BOOL MtkFillStoreBuffer(INT id, void* dest, INT filler, UINT32 size)
{
    STORE_BUFFER_T* sb;
    void* line;
    UINT32 written;

    if (! checkID(id)) {
        return FALSE;
    }

    sb = getItem(id);

    if (! CheckDestRange(sb, dest)) {
        return FALSE;
    }

    do {
        line    = GetLine(sb, dest);
        written = FillLine(sb, line, dest, filler, size);
        dest    = (      void*)((UINT32)dest + written);
        size   -= written;
    } while (size);

    return TRUE;
}

BOOL MtkFlushStoreBuffer(INT id)
{
    STORE_BUFFER_T* sb;

    if (! checkID(id)) {
        return FALSE;
    }

    sb = getItem(id);

    /* lineDict[]をソートする */
    ShellSort(sb->lineDict, sb->lineUsed);

    /* destが連続している領域は一気に出力 */
    JoinWrite(sb);

    /* preload を ON にする */
    MTK_BSET(sb->flag, FLAG_PRELOAD);

    return TRUE;
}








PRIVATE UINT32 MostLeftHighBit(UINT32 data)
{
    UINT32 tester = 0x80000000;
    while (tester) {
        if (MTK_BTST(data, tester)) {
            return tester;
        }
        tester >>= 1;
    }
    return 0;
}

PRIVATE void* GetLine(STORE_BUFFER_T* self, void* dest)
{
    UINT32 dict = GetDict(self, dest);
    UINT32 mask = ~(self->lineSize - 1);
    UINT32 index;
    UINT32 line;

    index = MTK_BTST(dict, ~mask);
    line  = self->buffer + self->lineSize * index;

    return (void*)line;
}

PRIVATE UINT32 GetDict(STORE_BUFFER_T* self, void* dest)
{
    UINT32* dict;

    dict = SearchDict(self, dest);
    if (! dict) {
        dict = AllocDict(self, dest);
        if (! dict) {
            FlushLruDict(self);
            dict = AllocDict(self, dest);
        }
    }

    return *dict;
}

PRIVATE UINT32* SearchDict(STORE_BUFFER_T* self, void* dest)
{
    UINT32 mask = ~(self->lineSize - 1);
    INT i;

    /* serch backward for search MRU entry first */
    for (i = self->lineUsed - 1; i >= 0; --i) {
        if (MTK_BTST((UINT32)dest, mask) == MTK_BTST(self->lineDict[i], mask)) {
            RotateMruDict(self, i);
            return &self->lineDict[self->lineUsed - 1];
        }
    }

    return NULL;
}

PRIVATE UINT32* AllocDict(STORE_BUFFER_T* self, void* dest)
{
    UINT32 mask = ~(self->lineSize - 1);
    UINT32 i;

    if (self->lineUsed >= self->lineNum) {
        return NULL;
    }

    i = self->lineUsed++;

    /* @note low bit(cache line id) was saved in lineDict[i]*/
    MTK_BCLR(self->lineDict[i], mask);
    MTK_BSET(self->lineDict[i], MTK_BTST((UINT32)dest, mask));

    if (MTK_BTST(self->flag, FLAG_PRELOAD)) {
        Preload(self, self->lineDict[i]);
    }

    return &self->lineDict[i];
}

PRIVATE void Preload(STORE_BUFFER_T* self, UINT32 dict)
{
    UINT32 mask = ~(self->lineSize - 1);
    UINT32 dest;
    UINT32 index;
    UINT32 line;

    dest  = MTK_BTST(dict, mask);
    index = MTK_BTST(dict, ~mask);
    line  = self->buffer + self->lineSize * index;

    ReadDmac((void*)line, (void*)dest, self->lineSize);
}

PRIVATE void RotateMruDict(STORE_BUFFER_T* self, UINT32 index)
{
    UINT32 tmp = self->lineDict[index];
    UINT32 i;

    for (i = index; i < self->lineNum - 1; ++i) {
        self->lineDict[i] = self->lineDict[i + 1];
    }
    self->lineDict[self->lineNum - 1] = tmp;
}

PRIVATE void FlushLruDict(STORE_BUFFER_T* self)
{
    UINT32 mask = ~(self->lineSize -1);
    UINT32 dict;
    UINT32 dest;
    UINT32 index;
    UINT32 line;
    UINT32 size;

    /*
     * LRU: Least Recently Used
     * lineDict[0] is always LRU dict.
     */
    dict  = self->lineDict[0];
    dest  = MTK_BTST(dict, mask);
    index = MTK_BTST(dict, ~mask);
    line  = self->buffer + self->lineSize * index;
    size  = self->lineSize;

    if (dest < self->destBeg) {
        UINT32 offset = MTK_BTST(self->destBeg, ~mask);
        dest  = self->destBeg;
        size -= offset;
        line += offset;
    }

    if (self->destEnd <= (dest + size)) {
        size = self->destEnd - dest;
    }

    /* printf("FlushLruDict: dest=%08x line=%08x size=%d\n", (INT)dest, (INT)line, (INT)size); */

    WriteDmac((void*)dest, (void*)line, size);

    RotateMruDict(self, 0);
    self->lineUsed--;
    MTK_BSET(self->flag, FLAG_PRELOAD);
}

PRIVATE BOOL CheckDestRange(STORE_BUFFER_T* self, void* dest)
{
    UINT32 mask = ~(self->lineSize - 1);
    UINT32 dest0, beg, end;

    dest0 = MTK_BTST((UINT32)dest, mask);
    beg   = MTK_BTST(self->destBeg, mask);
    end   = MTK_BTST(self->destEnd, mask);

    if ((dest0 < beg) || (end <= dest0)) {
        return FALSE;
    }

    return TRUE;
}

PRIVATE UINT32 WriteLine(STORE_BUFFER_T* self, void* line, const void* dest, const void* src, UINT32 size)
{
    UINT32 mask   = ~(self->lineSize - 1);
    UINT32 offset = MTK_BTST((UINT32)dest, ~mask);
    UINT32 pos    = (UINT32) line + offset;
    UINT32 len    = self->lineSize - offset;
    len = MTK_MIN(len, size);

    memcpy((void*)pos, src, len);

    return len;
}

PRIVATE UINT32 FillLine(STORE_BUFFER_T* self, void* line, const void* dest, INT filler, UINT32 size)
{
    UINT32 mask   = ~(self->lineSize - 1);
    UINT32 offset = MTK_BTST((UINT32)dest, ~mask);
    UINT32 pos    = (UINT32) line + offset;
    UINT32 len    = self->lineSize - offset;
    len = MTK_MIN(len, size);

    memset((void*)pos, filler, len);

    return len;
}

PRIVATE void ShellSort(UINT32* arr, UINT32 num)
{
    UINT32 h;
    UINT32 i, j;

    h = 1;
    while (h < num / 9) {
        h = 3 * h + 1;
    }

    for (; h > 0; h /= 3) {
        for (i = h; i < num; ++i) {
            for (j = i; j >= h; j -= h) {
                if (arr[j - h] > arr[j]) {
                    UINT32 tmp = arr[j - h];
                    arr[j - h] = arr[j];
                    arr[j]     = tmp;
                }
            }
        }
    }
}

PRIVATE void JoinWrite(STORE_BUFFER_T* self)
{
    UINT32 mask = ~(self->lineSize - 1);
    UINT32 dict;
    UINT32 dest;
    UINT32 index;
    UINT32 line;
    UINT32 size;
    UINT32 i;

    LockDmac();

    for (i = 0; i < self->lineUsed; ++i) {
        dict  = self->lineDict[i];
        dest  = MTK_BTST(dict, mask);
        index = MTK_BTST(dict, ~mask);
        line  = self->buffer + self->lineSize * index;
        size  = self->lineSize;

        if (dest < self->destBeg) {
            dest = self->destBeg;
            size = size - (self->destBeg - dest);
            line += MTK_BTST(self->destBeg, ~mask);
        }

        if (self->destEnd <= (dest + size)) {
            size = self->destEnd - dest;
        }

        CacheDmac((void*) dest, (void*) line, size);
    }
    FlushDmac();
    UnlockDmac();
}