max80_fw/fw/diskcache.c

/*
 * Simple disk cache; write-through for now
 */

#include "fw.h"
#include "list.h"
#include "sdcard.h"
#include "systime.h"
#include "console.h"

#define CACHE_BLOCKS		512
#define CACHE_BLOCK_BITS	5
#define CACHE_BLOCK_SECTORS	(1 << CACHE_BLOCK_BITS)
#define CACHE_BLOCK_SIZE	(SECTOR_SIZE << CACHE_BLOCK_BITS)

#define CACHE_HASH_SIZE		(CACHE_BLOCKS*2)

typedef sector_t block_t;
#define NO_BLOCK ((block_t)(-1))

struct cache_block {
    struct dll hash;		/* Link in hash chain or free list */
    struct dll lru;		/* Link in LRU chain */
    block_t block;		/* Physical block index */
    char data[CACHE_BLOCK_SIZE];
};

static struct dll __dram_noinit cache_hash[CACHE_HASH_SIZE];
static struct cache_block __dram_noinit disk_cache[CACHE_BLOCKS];

static struct dll lru_list;
static struct dll free_list;

static void disk_cache_init(void)
{
    struct cache_block *bp, *bep;
    struct dll *hp, *hep;

    dll_init(&free_list);
    dll_init(&lru_list);

    bp = disk_cache;
    bep = bp + CACHE_BLOCKS;
    while (bp < bep) {
	dll_insert_head(&free_list, &bp->hash);
	dll_insert_head(&lru_list, &bp->lru);
	bp->block = NO_BLOCK;
	bp++;
    }

    hp  = cache_hash;
    hep = hp + CACHE_HASH_SIZE;
    while (hp < hep)
	dll_init(hp++);
}

static inline __attribute__((const)) struct dll *hash_slot(block_t block)
{
    uint64_t m;
    uint32_t hash;

    m = UINT64_C(0x34f1f85d) * block;
    hash = (m >> 32) + m;

    return &cache_hash[hash % CACHE_BLOCKS];
}

static struct cache_block *disk_cache_find(block_t block)
{
    struct dll *hp, *bhp;
    struct cache_block *bp;

    hp = hash_slot(block);

    for (bhp = hp->next; bhp != hp; bhp = bhp->next) {
	bp = container_of(bhp, struct cache_block, hash);
	if (bp->block == block)
	    return bp;
    }

    return NULL;
}

static void invalidate_block(struct cache_block *bp)
{
    dll_remove(&bp->hash);
    dll_insert_head(&free_list, &bp->hash);
    bp->block = NO_BLOCK;
    dll_demote(&lru_list, &bp->lru);
}

static struct cache_block *disk_cache_get(block_t block, bool do_read)
{
    const sector_t size = sdc.lbasize;
    struct cache_block *bp;

    bp = disk_cache_find(block);

    if (!bp) {
	/* Block not in cache, need to get it */
	sector_t sector = block << CACHE_BLOCK_BITS;
	int sectors = CACHE_BLOCK_SECTORS;

	if (sector >= size)
	    return NULL;

	if (sector + sectors > size)
	    sectors = size - sectors; /* Truncated final block */

	/* Get the oldest block */
	bp = container_of(lru_list.prev, struct cache_block, lru);
	invalidate_block(bp);

	if (do_read &&
	    sdcard_read_sectors(bp->data, sector, sectors) != sectors)
	    return NULL;

	bp->block = block;
	dll_insert_head(hash_slot(block), &bp->hash);
    }

    dll_promote(&lru_list, &bp->lru);
    return bp;
}

/* --------------------------------------------------------------------------
 *  Interface to fatfs
 * ------------------------------------------------------------------------- */

DRESULT disk_initialize(BYTE drive)
{
    if (drive != 0)
	return STA_NOINIT;

    disk_cache_init();

    return sdcard_init();
}

static FATFS sd_fs;

int disk_init(void)
{
    FRESULT rv;
    char label[128];
    uint32_t volid, freeclust;
    FATFS *fs;

    rv = f_mount(&sd_fs, "", 1);
    if (rv != FR_OK) {
	con_printf("sdcard: no volume found\n");
	return -1;
    }

    label[0] = '\0';
    volid = 0;

    f_getlabel("", label, &volid);
    con_printf("sdcard: volume found, label \"%s\", volid %08x\n", label, volid);

    freeclust = 0;
    f_getfree("", &freeclust, &fs);

    con_printf("sdcard: %u/%u clusters free, clusters = %u bytes\n",
	       freeclust, fs->n_fatent - 2, fs->csize << 9);

    mount_abcdrives();

    return 0;
}

DRESULT disk_ioctl(BYTE drive, BYTE command, void *buffer)
{
    if (drive != 0)
	return STA_NOINIT;

    switch (command) {
    case CTRL_SYNC:
	return RES_OK;
    case GET_SECTOR_SIZE:
	*(WORD *)buffer = 512;
	return RES_OK;
    case GET_SECTOR_COUNT:
	*(DWORD *)buffer = sdc.lbasize;
	return RES_OK;
    case GET_BLOCK_SIZE:
	*(DWORD *)buffer = 1; /* XXX */
	return RES_OK;
    default:
	return RES_PARERR;
    }
}

DRESULT disk_read(BYTE drive, BYTE *buffer,
		  LBA_t sectornumber, UINT sectorcount)
{
    (void)drive;

    if (!sectorcount)
	return RES_OK;

    block_t block = sectornumber >> CACHE_BLOCK_BITS;
    block_t last = (sectornumber + sectorcount - 1) >> CACHE_BLOCK_BITS;
    size_t offset = (sectornumber & (CACHE_BLOCK_SECTORS-1)) << SECTOR_SHIFT;
    size_t len = sectorcount << SECTOR_SHIFT;

    while (block <= last) {
	struct cache_block *bp = disk_cache_get(block, true);
	if (!bp)
	    return RES_ERROR;

	size_t bytes = min(CACHE_BLOCK_SIZE - offset, len);

	memcpy(buffer, bp->data + offset, bytes);
	len -= bytes;
	block++;
	offset = 0;
    }

    return RES_OK;
}

DRESULT disk_write(BYTE drive, const BYTE *buffer, LBA_t sectornumber,
		   UINT sectorcount)
{
    (void)drive;

    if (!sectorcount)
	return RES_OK;

    block_t block = sectornumber >> CACHE_BLOCK_BITS;
    block_t last = (sectornumber + sectorcount - 1) >> CACHE_BLOCK_BITS;
    size_t offset = (sectornumber & (CACHE_BLOCK_SECTORS-1)) << SECTOR_SHIFT;
    size_t len = sectorcount << SECTOR_SHIFT;
    size_t size = sdc.lbasize;

    while (block <= last) {
	struct cache_block *bp = disk_cache_get(block, !!offset);
	if (!bp)
	    return RES_ERROR;

	size_t bytes = min(CACHE_BLOCK_SIZE - offset, len);

	memcpy(bp->data + offset, buffer, bytes);

	sector_t sector = block << CACHE_BLOCK_BITS;
	int sectors = CACHE_BLOCK_SECTORS;

	if (sector + sectors > size)
	    sectors = size - sectors; /* Truncated final block */

	if (sdcard_write_sectors(bp->data, block << CACHE_BLOCK_BITS,
				 sectors) != sectors) {
	    invalidate_block(bp);
	    return RES_ERROR;
	}

	len -= bytes;
	block++;
	offset = 0;
    }

    return RES_OK;
}

DWORD get_fattime(void)
{
    return SYSCLOCK_DATETIME;	/* Already in FAT format */
}