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

741 lines
14 KiB
C

// aoe.c: the ATA over Ethernet virtual EtherDrive (R) blade
#define _GNU_SOURCE
#include "config.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <netinet/in.h>
#include "dat.h"
#include "fns.h"
enum {
Nmasks= 32,
Nsrr= 256,
Alen= 6,
};
uchar masks[Nmasks*Alen];
int nmasks;
uchar srr[Nsrr*Alen];
int nsrr;
char config[Nconfig];
int nconfig = 0;
int maxscnt = 2;
char *ifname;
int bufcnt = Bufcount;
#ifndef O_BINARY
#define O_BINARY 0
#endif
typedef unsigned long long u64_t;
typedef unsigned int u32_t;
#pragma pack(4)
typedef struct ventoy_img_chunk
{
u32_t img_start_sector; // sector size: 2KB
u32_t img_end_sector; // included
u64_t disk_start_sector; // in disk_sector_size
u64_t disk_end_sector; // included
}ventoy_img_chunk;
typedef struct ventoy_disk_map
{
u64_t img_start_sector;
u64_t img_end_sector;
u64_t disk_start_sector;
u64_t disk_end_sector;
}ventoy_disk_map;
#pragma pack()
static int verbose = 0;
static u64_t g_iso_file_size = 0;
static int g_img_map_num = 0;
static ventoy_disk_map *g_img_map = NULL;
static ventoy_disk_map * vtoydm_get_img_map_data(const char *img_map_file, int *plen)
{
int i;
int len;
int rc = 1;
u64_t sector_num;
FILE *fp = NULL;
ventoy_img_chunk *chunk = NULL;
ventoy_disk_map *map = NULL;
fp = fopen(img_map_file, "rb");
if (NULL == fp)
{
fprintf(stderr, "Failed to open file %s\n", img_map_file);
return NULL;
}
fseek(fp, 0, SEEK_END);
len = (int)ftell(fp);
fseek(fp, 0, SEEK_SET);
chunk = (ventoy_img_chunk *)malloc(len);
if (NULL == chunk)
{
fprintf(stderr, "Failed to malloc memory len:%d\n", len);
goto end;
}
if (fread(chunk, 1, len, fp) != len)
{
fprintf(stderr, "Failed to read file\n");
goto end;
}
if (len % sizeof(ventoy_img_chunk))
{
fprintf(stderr, "image map file size %d is not aligned with %d\n",
len, (int)sizeof(ventoy_img_chunk));
goto end;
}
map = (ventoy_disk_map *)malloc((len / sizeof(ventoy_img_chunk)) * sizeof(ventoy_disk_map));
if (NULL == map)
{
fprintf(stderr, "Failed to malloc memory\n");
goto end;
}
for (i = 0; i < len / sizeof(ventoy_img_chunk); i++)
{
sector_num = chunk[i].img_end_sector - chunk[i].img_start_sector + 1;
g_iso_file_size += sector_num * 2048;
map[i].img_start_sector = chunk[i].img_start_sector << 2;
map[i].img_end_sector = (chunk[i].img_end_sector << 2) + 3;
map[i].disk_start_sector = chunk[i].disk_start_sector;
map[i].disk_end_sector = chunk[i].disk_end_sector;
}
rc = 0;
end:
fclose(fp);
if (chunk)
{
free(chunk);
chunk = NULL;
}
*plen = len;
return map;
}
static void parse_img_chunk(const char *img_map_file)
{
int len;
g_img_map = vtoydm_get_img_map_data(img_map_file, &len);
if (g_img_map)
{
g_img_map_num = len / sizeof(ventoy_img_chunk);
}
}
static u64_t get_disk_sector(u64_t lba)
{
int i;
ventoy_disk_map *cur = g_img_map;
for (i = 0; i < g_img_map_num; i++, cur++)
{
if (lba >= cur->img_start_sector && lba <= cur->img_end_sector)
{
return (lba - cur->img_start_sector) + cur->disk_start_sector;
}
}
return 0;
}
int getsec(int fd, uchar *place, vlong lba, int nsec)
{
int i;
int count = 0;
u64_t last_sector;
u64_t sector;
count = 1;
last_sector = get_disk_sector((u64_t)lba);
for (i = 1; i < nsec; i++)
{
sector = get_disk_sector((u64_t)(lba + i));
if (sector == (last_sector + count))
{
count++;
}
else
{
lseek(fd, last_sector * 512, SEEK_SET);
read(fd, place, count * 512);
last_sector = sector;
count = 1;
}
}
lseek(fd, last_sector * 512, SEEK_SET);
read(fd, place, count * 512);
return nsec * 512;
}
// read only
int putsec(int fd, uchar *place, vlong lba, int nsec)
{
return nsec * 512;
}
void
aoead(int fd) // advertise the virtual blade
{
uchar buf[2000];
Conf *p;
int i;
p = (Conf *)buf;
memset(p, 0, sizeof *p);
memset(p->h.dst, 0xff, 6);
memmove(p->h.src, mac, 6);
p->h.type = htons(0x88a2);
p->h.flags = Resp;
p->h.maj = htons(shelf);
p->h.min = slot;
p->h.cmd = Config;
p->bufcnt = htons(bufcnt);
p->scnt = maxscnt = (getmtu(sfd, ifname) - sizeof (Ata)) / 512;
p->firmware = htons(FWV);
p->vercmd = 0x10 | Qread;
memcpy(p->data, config, nconfig);
p->len = htons(nconfig);
if (nmasks == 0)
if (putpkt(fd, buf, sizeof *p - sizeof p->data + nconfig) == -1) {
perror("putpkt aoe id");
return;
}
for (i=0; i<nmasks; i++) {
memcpy(p->h.dst, &masks[i*Alen], Alen);
if (putpkt(fd, buf, sizeof *p - sizeof p->data + nconfig) == -1)
perror("putpkt aoe id");
}
}
int
isbcast(uchar *ea)
{
uchar *b = (uchar *)"\377\377\377\377\377\377";
return memcmp(ea, b, 6) == 0;
}
long long
getlba(uchar *p)
{
vlong v;
int i;
v = 0;
for (i = 0; i < 6; i++)
v |= (vlong)(*p++) << i * 8;
return v;
}
int
aoeata(Ata *p, int pktlen) // do ATA reqeust
{
Ataregs r;
int len = 60;
int n;
r.lba = getlba(p->lba);
r.sectors = p->sectors;
r.feature = p->err;
r.cmd = p->cmd;
if (r.cmd != 0xec)
if (!rrok(p->h.src)) {
p->h.flags |= Error;
p->h.error = Res;
return len;
}
if (atacmd(&r, (uchar *)(p+1), maxscnt*512, pktlen - sizeof(*p)) < 0) {
p->h.flags |= Error;
p->h.error = BadArg;
return len;
}
if (!(p->aflag & Write))
if ((n = p->sectors)) {
n -= r.sectors;
len = sizeof (Ata) + (n*512);
}
p->sectors = r.sectors;
p->err = r.err;
p->cmd = r.status;
return len;
}
#define QCMD(x) ((x)->vercmd & 0xf)
// yes, this makes unnecessary copies.
int
confcmd(Conf *p, int payload) // process conf request
{
int len;
len = ntohs(p->len);
if (QCMD(p) != Qread)
if (len > Nconfig || len > payload)
return 0; // if you can't play nice ...
switch (QCMD(p)) {
case Qtest:
if (len != nconfig)
return 0;
// fall thru
case Qprefix:
if (len > nconfig)
return 0;
if (memcmp(config, p->data, len))
return 0;
// fall thru
case Qread:
break;
case Qset:
if (nconfig)
if (nconfig != len || memcmp(config, p->data, len)) {
p->h.flags |= Error;
p->h.error = ConfigErr;
break;
}
// fall thru
case Qfset:
nconfig = len;
memcpy(config, p->data, nconfig);
break;
default:
p->h.flags |= Error;
p->h.error = BadArg;
}
memmove(p->data, config, nconfig);
p->len = htons(nconfig);
p->bufcnt = htons(bufcnt);
p->scnt = maxscnt = (getmtu(sfd, ifname) - sizeof (Ata)) / 512;
p->firmware = htons(FWV);
p->vercmd = 0x10 | QCMD(p); // aoe v.1
return nconfig + sizeof *p - sizeof p->data;
}
static int
aoesrr(Aoesrr *sh, int len)
{
uchar *m, *e;
int n;
e = (uchar *) sh + len;
m = (uchar *) sh + Nsrrhdr;
switch (sh->rcmd) {
default:
e: sh->h.error = BadArg;
sh->h.flags |= Error;
break;
case 1: // set
if (!rrok(sh->h.src)) {
sh->h.error = Res;
sh->h.flags |= Error;
break;
}
case 2: // force set
n = sh->nmacs * 6;
if (e < m + n)
goto e;
nsrr = sh->nmacs;
memmove(srr, m, n);
case 0: // read
break;
}
sh->nmacs = nsrr;
n = nsrr * 6;
memmove(m, srr, n);
return Nsrrhdr + n;
}
static int
addmask(uchar *ea)
{
uchar *p, *e;
p = masks;
e = p + nmasks;
for (; p<e; p += 6)
if (!memcmp(p, ea, 6))
return 2;
if (nmasks >= Nmasks)
return 0;
memmove(p, ea, 6);
nmasks++;
return 1;
}
static void
rmmask(uchar *ea)
{
uchar *p, *e;
p = masks;
e = p + nmasks;
for (; p<e; p+=6)
if (!memcmp(p, ea, 6)) {
memmove(p, p+6, e-p-6);
nmasks--;
return;
}
}
static int
aoemask(Aoemask *mh, int len)
{
Mdir *md, *mdi, *mde;
int i, n;
n = 0;
md = mdi = (Mdir *) ((uchar *)mh + Nmaskhdr);
switch (mh->cmd) {
case Medit:
mde = md + mh->nmacs;
for (; md<mde; md++) {
switch (md->cmd) {
case MDdel:
rmmask(md->mac);
continue;
case MDadd:
if (addmask(md->mac))
continue;
mh->merror = MEfull;
mh->nmacs = md - mdi;
goto e;
case MDnop:
continue;
default:
mh->merror = MEbaddir;
mh->nmacs = md - mdi;
goto e;
}
}
// success. fall thru to return list
case Mread:
md = mdi;
for (i=0; i<nmasks; i++) {
md->res = md->cmd = 0;
memmove(md->mac, &masks[i*6], 6);
md++;
}
mh->merror = 0;
mh->nmacs = nmasks;
n = sizeof *md * nmasks;
break;
default:
mh->h.flags |= Error;
mh->h.error = BadArg;
}
e: return n + Nmaskhdr;
}
void
doaoe(Aoehdr *p, int n)
{
int len;
switch (p->cmd) {
case ATAcmd:
if (n < Natahdr)
return;
len = aoeata((Ata*)p, n);
break;
case Config:
if (n < Ncfghdr)
return;
len = confcmd((Conf *)p, n);
break;
case Mask:
if (n < Nmaskhdr)
return;
len = aoemask((Aoemask *)p, n);
break;
case Resrel:
if (n < Nsrrhdr)
return;
len = aoesrr((Aoesrr *)p, n);
break;
default:
p->error = BadCmd;
p->flags |= Error;
len = n;
break;
}
if (len <= 0)
return;
memmove(p->dst, p->src, 6);
memmove(p->src, mac, 6);
p->maj = htons(shelf);
p->min = slot;
p->flags |= Resp;
if (putpkt(sfd, (uchar *) p, len) == -1) {
perror("write to network");
exit(1);
}
}
void
aoe(void)
{
Aoehdr *p;
uchar *buf;
int n, sh;
long pagesz;
enum { bufsz = 1<<16, };
if ((pagesz = sysconf(_SC_PAGESIZE)) < 0) {
perror("sysconf");
exit(1);
}
if ((buf = malloc(bufsz + pagesz)) == NULL) {
perror("malloc");
exit(1);
}
n = (size_t) buf + sizeof(Ata);
if (n & (pagesz - 1))
buf += pagesz - (n & (pagesz - 1));
aoead(sfd);
for (;;) {
n = getpkt(sfd, buf, bufsz);
if (n < 0) {
perror("read network");
exit(1);
}
if (n < sizeof(Aoehdr))
continue;
p = (Aoehdr *) buf;
if (ntohs(p->type) != 0x88a2)
continue;
if (p->flags & Resp)
continue;
sh = ntohs(p->maj);
if (sh != shelf && sh != (ushort)~0)
continue;
if (p->min != slot && p->min != (uchar)~0)
continue;
if (nmasks && !maskok(p->src))
continue;
doaoe(p, n);
}
}
void
usage(void)
{
fprintf(stderr, "usage: %s [-b bufcnt] [-o offset] [-l length] [-d ] [-s] [-r] [ -m mac[,mac...] ] shelf slot netif filename\n",
progname);
exit(1);
}
/* parseether from plan 9 */
int
parseether(uchar *to, char *from)
{
char nip[4];
char *p;
int i;
p = from;
for(i = 0; i < 6; i++){
if(*p == 0)
return -1;
nip[0] = *p++;
if(*p == 0)
return -1;
nip[1] = *p++;
nip[2] = 0;
to[i] = strtoul(nip, 0, 16);
if(*p == ':')
p++;
}
return 0;
}
void
setmask(char *ml)
{
char *p;
int n;
for (; ml; ml=p) {
p = strchr(ml, ',');
if (p)
*p++ = '\0';
n = parseether(&masks[nmasks*Alen], ml);
if (n < 0)
fprintf(stderr, "ignoring mask %s, parseether failure\n", ml);
else
nmasks++;
}
}
int
maskok(uchar *ea)
{
int i, ok = 0;
for (i=0; !ok && i<nmasks; i++)
ok = memcmp(ea, &masks[i*Alen], Alen) == 0;
return ok;
}
int
rrok(uchar *ea)
{
int i, ok = 0;
if (nsrr == 0)
return 1;
for (i=0; !ok && i<nsrr; i++)
ok = memcmp(ea, &srr[i*Alen], Alen) == 0;
return ok;
}
void
setserial(int sh, int sl)
{
char h[32];
h[0] = 0;
gethostname(h, sizeof h);
snprintf(serial, Nserial, "%d.%d:%.*s", sh, sl, (int) sizeof h, h);
}
int
main(int argc, char **argv)
{
int ch, omode = 0, readonly = 0;
vlong length = 0;
char *end;
char filepath[300] = {0};
/* Avoid to be killed by systemd */
if (access("/etc/initrd-release", F_OK) >= 0)
{
argv[0][0] = '@';
}
bufcnt = Bufcount;
offset = 0;
setbuf(stdin, NULL);
progname = *argv;
while ((ch = getopt(argc, argv, "b:dsrm:f:tv::o:l:")) != -1) {
switch (ch) {
case 'b':
bufcnt = atoi(optarg);
break;
case 'd':
#ifdef O_DIRECT
omode |= O_DIRECT;
#endif
break;
case 's':
omode |= O_SYNC;
break;
case 'r':
readonly = 1;
break;
case 'm':
setmask(optarg);
break;
case 't':
return 0;
case 'v':
verbose = 1;
break;
case 'f':
strncpy(filepath, optarg, sizeof(filepath) - 1);
break;
case 'o':
offset = strtoll(optarg, &end, 0);
if (end == optarg || offset < 0)
usage();
break;
case 'l':
length = strtoll(optarg, &end, 0);
if (end == optarg || length < 1)
usage();
break;
case '?':
default:
usage();
}
}
argc -= optind;
argv += optind;
if (argc != 4 || bufcnt <= 0)
usage();
omode |= readonly ? O_RDONLY : O_RDWR;
parse_img_chunk(filepath);
bfd = open(argv[3], omode);
if (bfd == -1) {
perror("open");
exit(1);
}
shelf = atoi(argv[0]);
slot = atoi(argv[1]);
setserial(shelf, slot);
size = g_iso_file_size; //getsize(bfd);
size /= 512;
if (size <= offset) {
if (offset)
fprintf(stderr,
"Offset %lld too large for %lld-sector export\n",
offset,
size);
else
fputs("0-sector file size is too small\n", stderr);
exit(1);
}
size -= offset;
if (length) {
if (length > size) {
fprintf(stderr, "Length %llu too big - exceeds size of file!\n", offset);
exit(1);
}
size = length;
}
ifname = argv[2];
sfd = dial(ifname, bufcnt);
if (sfd < 0)
return 1;
getea(sfd, ifname, mac);
if (verbose) {
printf("pid %ld: e%d.%d, %lld sectors %s\n",
(long) getpid(), shelf, slot, size,
readonly ? "O_RDONLY" : "O_RDWR");
}
fflush(stdout);
atainit();
aoe();
return 0;
}