mirror of https://github.com/ventoy/Ventoy.git
186 lines
3.3 KiB
C
186 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;
|
|
}
|
|
|