Ventoy/VBLADE/vblade-master/ata.c
2020-04-05 00:08:01 +08:00

185 lines
3.3 KiB
C

// ata.c: ATA simulator for vblade
#include "config.h"
#include <string.h>
#include <stdio.h>
#include <sys/types.h>
#include "dat.h"
#include "fns.h"
enum {
// err bits
UNC = 1<<6,
MC = 1<<5,
IDNF = 1<<4,
MCR = 1<<3,
ABRT = 1<<2,
NM = 1<<1,
// status bits
BSY = 1<<7,
DRDY = 1<<6,
DF = 1<<5,
DRQ = 1<<3,
ERR = 1<<0,
};
static ushort ident[256];
static void
setfld(ushort *a, int idx, int len, char *str) // set field in ident
{
uchar *p;
p = (uchar *)(a+idx);
while (len > 0) {
if (*str == 0)
p[1] = ' ';
else
p[1] = *str++;
if (*str == 0)
p[0] = ' ';
else
p[0] = *str++;
p += 2;
len -= 2;
}
}
static void
setlba28(ushort *ident, vlong lba)
{
uchar *cp;
cp = (uchar *) &ident[60];
*cp++ = lba;
*cp++ = lba >>= 8;
*cp++ = lba >>= 8;
*cp++ = (lba >>= 8) & 0xf;
}
static void
setlba48(ushort *ident, vlong lba)
{
uchar *cp;
cp = (uchar *) &ident[100];
*cp++ = lba;
*cp++ = lba >>= 8;
*cp++ = lba >>= 8;
*cp++ = lba >>= 8;
*cp++ = lba >>= 8;
*cp++ = lba >>= 8;
}
static void
setushort(ushort *a, int i, ushort n)
{
uchar *p;
p = (uchar *)(a+i);
*p++ = n & 0xff;
*p++ = n >> 8;
}
void
atainit(void)
{
char buf[64];
setushort(ident, 47, 0x8000);
setushort(ident, 49, 0x0200);
setushort(ident, 50, 0x4000);
setushort(ident, 83, 0x5400);
setushort(ident, 84, 0x4000);
setushort(ident, 86, 0x1400);
setushort(ident, 87, 0x4000);
setushort(ident, 93, 0x400b);
setfld(ident, 27, 40, "Coraid EtherDrive vblade");
sprintf(buf, "V%d", VBLADE_VERSION);
setfld(ident, 23, 8, buf);
setfld(ident, 10, 20, serial);
}
/* The ATA spec is weird in that you specify the device size as number
* of sectors and then address the sectors with an offset. That means
* with LBA 28 you shouldn't see an LBA of all ones. Still, we don't
* check for that.
*/
int
atacmd(Ataregs *p, uchar *dp, int ndp, int payload) // do the ata cmd
{
vlong lba;
ushort *ip;
int n;
enum { MAXLBA28SIZE = 0x0fffffff };
extern int maxscnt;
p->status = 0;
switch (p->cmd) {
default:
p->status = DRDY | ERR;
p->err = ABRT;
return 0;
case 0xe7: // flush cache
return 0;
case 0xec: // identify device
if (p->sectors != 1 || ndp < 512)
return -1;
memmove(dp, ident, 512);
ip = (ushort *)dp;
if (size & ~MAXLBA28SIZE)
setlba28(ip, MAXLBA28SIZE);
else
setlba28(ip, size);
setlba48(ip, size);
p->err = 0;
p->status = DRDY;
p->sectors = 0;
return 0;
case 0xe5: // check power mode
p->err = 0;
p->sectors = 0xff; // the device is active or idle
p->status = DRDY;
return 0;
case 0x20: // read sectors
case 0x30: // write sectors
lba = p->lba & MAXLBA28SIZE;
break;
case 0x24: // read sectors ext
case 0x34: // write sectors ext
lba = p->lba & 0x0000ffffffffffffLL; // full 48
break;
}
// we ought not be here unless we are a read/write
if (p->sectors > maxscnt || p->sectors*512 > ndp)
return -1;
if (lba + p->sectors > size) {
p->err = IDNF;
p->status = DRDY | ERR;
p->lba = lba;
return 0;
}
if (p->cmd == 0x20 || p->cmd == 0x24)
n = getsec(bfd, dp, lba+offset, p->sectors);
else {
// packet should be big enough to contain the data
if (payload < 512 * p->sectors)
return -1;
n = putsec(bfd, dp, lba+offset, p->sectors);
}
n /= 512;
if (n != p->sectors) {
p->err = ABRT;
p->status = ERR;
} else
p->err = 0;
p->status |= DRDY;
p->lba += n;
p->sectors -= n;
return 0;
}