VBLADE/vblade-master/aoe.c

   1 // aoe.c: the ATA over Ethernet virtual EtherDrive (R) blade
   2 #define _GNU_SOURCE
   3 #include "config.h"
   4 #include <stdio.h>
   5 #include <string.h>
   6 #include <stdlib.h>
   7 #include <unistd.h>
   8 #include <sys/types.h>
   9 #include <sys/stat.h>
  10 #include <fcntl.h>
  11 #include <netinet/in.h>
  12 #include "dat.h"
  13 #include "fns.h"
  14
  15 enum {
  16         Nmasks= 32,
  17         Nsrr= 256,
  18         Alen= 6,
  19 };
  20
  21 uchar masks[Nmasks*Alen];
  22 int nmasks;
  23 uchar srr[Nsrr*Alen];
  24 int nsrr;
  25 char config[Nconfig];
  26 int nconfig = 0;
  27 int maxscnt = 2;
  28 char *ifname;
  29 int bufcnt = Bufcount;
  30
  31 #ifndef O_BINARY
  32 #define O_BINARY 0
  33 #endif
  34
  35 typedef unsigned long long u64_t;
  36 typedef unsigned int    u32_t;
  37
  38 #pragma pack(4)
  39 typedef struct ventoy_img_chunk
  40 {
  41     u32_t img_start_sector; // sector size: 2KB
  42     u32_t img_end_sector;   // included
  43
  44     u64_t disk_start_sector; // in disk_sector_size
  45     u64_t disk_end_sector;   // included
  46 }ventoy_img_chunk;
  47
  48 typedef struct ventoy_disk_map
  49 {
  50     u64_t img_start_sector;
  51     u64_t img_end_sector;
  52     u64_t disk_start_sector;
  53     u64_t disk_end_sector;
  54 }ventoy_disk_map;
  55 #pragma pack()
  56
  57 static int verbose = 0;
  58 static u64_t g_iso_file_size = 0;
  59 static int g_img_map_num = 0;
  60 static ventoy_disk_map *g_img_map = NULL;
  61
  62 static ventoy_disk_map * vtoydm_get_img_map_data(const char *img_map_file, int *plen)
  63 {
  64     int i;
  65     int len;
  66     int rc = 1;
  67     u64_t sector_num;
  68     FILE *fp = NULL;
  69     ventoy_img_chunk *chunk = NULL;
  70     ventoy_disk_map *map = NULL;
  71
  72     fp = fopen(img_map_file, "rb");
  73     if (NULL == fp)
  74     {
  75         fprintf(stderr, "Failed to open file %s\n", img_map_file);
  76         return NULL;
  77     }
  78
  79     fseek(fp, 0, SEEK_END);
  80     len = (int)ftell(fp);
  81     fseek(fp, 0, SEEK_SET);
  82
  83     chunk = (ventoy_img_chunk *)malloc(len);
  84     if (NULL == chunk)
  85     {
  86         fprintf(stderr, "Failed to malloc memory len:%d\n", len);
  87         goto end;
  88     }
  89
  90     if (fread(chunk, 1, len, fp) != len)
  91     {
  92         fprintf(stderr, "Failed to read file\n");
  93         goto end;
  94     }
  95
  96     if (len % sizeof(ventoy_img_chunk))
  97     {
  98         fprintf(stderr, "image map file size %d is not aligned with %d\n",
  99                 len, (int)sizeof(ventoy_img_chunk));
 100         goto end;
 101     }
 102
 103     map = (ventoy_disk_map *)malloc((len / sizeof(ventoy_img_chunk)) * sizeof(ventoy_disk_map));
 104     if (NULL == map)
 105     {
 106         fprintf(stderr, "Failed to malloc memory\n");
 107         goto end;
 108     }
 109
 110     for (i = 0; i < len / sizeof(ventoy_img_chunk); i++)
 111     {
 112         sector_num = chunk[i].img_end_sector - chunk[i].img_start_sector + 1;
 113         g_iso_file_size += sector_num * 2048;
 114
 115         map[i].img_start_sector = chunk[i].img_start_sector << 2;
 116         map[i].img_end_sector = (chunk[i].img_end_sector << 2) + 3;
 117         map[i].disk_start_sector = chunk[i].disk_start_sector;
 118         map[i].disk_end_sector = chunk[i].disk_end_sector;
 119     }
 120
 121     rc = 0;
 122 end:
 123     fclose(fp);
 124
 125     if (chunk)
 126     {
 127         free(chunk);
 128         chunk = NULL;
 129     }
 130
 131     *plen = len;
 132     return map;
 133 }
 134
 135 static void parse_img_chunk(const char *img_map_file)
 136 {
 137     int len;
 138
 139     g_img_map = vtoydm_get_img_map_data(img_map_file, &len);
 140     if (g_img_map)
 141     {
 142         g_img_map_num = len / sizeof(ventoy_img_chunk);
 143     }
 144 }
 145
 146 static u64_t get_disk_sector(u64_t lba)
 147 {
 148     int i;
 149     ventoy_disk_map *cur = g_img_map;
 150
 151     for (i = 0; i < g_img_map_num; i++, cur++)
 152     {
 153         if (lba >= cur->img_start_sector && lba <= cur->img_end_sector)
 154         {
 155             return (lba - cur->img_start_sector) + cur->disk_start_sector;
 156         }
 157     }
 158
 159     return 0;
 160 }
 161
 162 int getsec(int fd, uchar *place, vlong lba, int nsec)
 163 {
 164     int i;
 165     int count = 0;
 166     u64_t last_sector;
 167     u64_t sector;
 168
 169     count = 1;
 170     last_sector = get_disk_sector((u64_t)lba);
 171
 172     for (i = 1; i < nsec; i++)
 173     {
 174         sector = get_disk_sector((u64_t)(lba + i));
 175         if (sector == (last_sector + count))
 176         {
 177             count++;
 178         }
 179         else
 180         {
 181             lseek(fd, last_sector * 512, SEEK_SET);
 182             read(fd, place, count * 512);
 183
 184             last_sector = sector;
 185             count = 1;
 186         }
 187     }
 188
 189     lseek(fd, last_sector * 512, SEEK_SET);
 190     read(fd, place, count * 512);
 191
 192         return nsec * 512;
 193 }
 194 // read only
 195 int putsec(int fd, uchar *place, vlong lba, int nsec)
 196 {
 197     return nsec * 512;
 198 }
 199
 200
 201 void
 202 aoead(int fd)   // advertise the virtual blade
 203 {
 204         uchar buf[2000];
 205         Conf *p;
 206         int i;
 207
 208         p = (Conf *)buf;
 209         memset(p, 0, sizeof *p);
 210         memset(p->h.dst, 0xff, 6);
 211         memmove(p->h.src, mac, 6);
 212         p->h.type = htons(0x88a2);
 213         p->h.flags = Resp;
 214         p->h.maj = htons(shelf);
 215         p->h.min = slot;
 216         p->h.cmd = Config;
 217         p->bufcnt = htons(bufcnt);
 218         p->scnt = maxscnt = (getmtu(sfd, ifname) - sizeof (Ata)) / 512;
 219         p->firmware = htons(FWV);
 220         p->vercmd = 0x10 | Qread;
 221         memcpy(p->data, config, nconfig);
 222         p->len = htons(nconfig);
 223         if (nmasks == 0)
 224         if (putpkt(fd, buf, sizeof *p - sizeof p->data + nconfig) == -1) {
 225                 perror("putpkt aoe id");
 226                 return;
 227         }
 228         for (i=0; i<nmasks; i++) {
 229                 memcpy(p->h.dst, &masks[i*Alen], Alen);
 230                 if (putpkt(fd, buf, sizeof *p - sizeof p->data + nconfig) == -1)
 231                         perror("putpkt aoe id");
 232         }
 233 }
 234
 235 int
 236 isbcast(uchar *ea)
 237 {
 238         uchar *b = (uchar *)"\377\377\377\377\377\377";
 239
 240         return memcmp(ea, b, 6) == 0;
 241 }
 242
 243 long long
 244 getlba(uchar *p)
 245 {
 246         vlong v;
 247         int i;
 248
 249         v = 0;
 250         for (i = 0; i < 6; i++)
 251                 v |= (vlong)(*p++) << i * 8;
 252         return v;
 253 }
 254
 255 int
 256 aoeata(Ata *p, int pktlen)      // do ATA reqeust
 257 {
 258         Ataregs r;
 259         int len = 60;
 260         int n;
 261
 262         r.lba = getlba(p->lba);
 263         r.sectors = p->sectors;
 264         r.feature = p->err;
 265         r.cmd = p->cmd;
 266         if (r.cmd != 0xec)
 267         if (!rrok(p->h.src)) {
 268                 p->h.flags |= Error;
 269                 p->h.error = Res;
 270                 return len;
 271         }
 272         if (atacmd(&r, (uchar *)(p+1), maxscnt*512, pktlen - sizeof(*p)) < 0) {
 273                 p->h.flags |= Error;
 274                 p->h.error = BadArg;
 275                 return len;
 276         }
 277         if (!(p->aflag & Write))
 278         if ((n = p->sectors)) {
 279                 n -= r.sectors;
 280                 len = sizeof (Ata) + (n*512);
 281         }
 282         p->sectors = r.sectors;
 283         p->err = r.err;
 284         p->cmd = r.status;
 285         return len;
 286 }
 287
 288 #define QCMD(x) ((x)->vercmd & 0xf)
 289
 290 // yes, this makes unnecessary copies.
 291
 292 int
 293 confcmd(Conf *p, int payload)   // process conf request
 294 {
 295         int len;
 296
 297         len = ntohs(p->len);
 298         if (QCMD(p) != Qread)
 299         if (len > Nconfig || len > payload)
 300                 return 0;       // if you can't play nice ...
 301         switch (QCMD(p)) {
 302         case Qtest:
 303                 if (len != nconfig)
 304                         return 0;
 305                 // fall thru
 306         case Qprefix:
 307                 if (len > nconfig)
 308                         return 0;
 309                 if (memcmp(config, p->data, len))
 310                         return 0;
 311                 // fall thru
 312         case Qread:
 313                 break;
 314         case Qset:
 315                 if (nconfig)
 316                 if (nconfig != len || memcmp(config, p->data, len)) {
 317                         p->h.flags |= Error;
 318                         p->h.error = ConfigErr;
 319                         break;
 320                 }
 321                 // fall thru
 322         case Qfset:
 323                 nconfig = len;
 324                 memcpy(config, p->data, nconfig);
 325                 break;
 326         default:
 327                 p->h.flags |= Error;
 328                 p->h.error = BadArg;
 329         }
 330         memmove(p->data, config, nconfig);
 331         p->len = htons(nconfig);
 332         p->bufcnt = htons(bufcnt);
 333         p->scnt = maxscnt = (getmtu(sfd, ifname) - sizeof (Ata)) / 512;
 334         p->firmware = htons(FWV);
 335         p->vercmd = 0x10 | QCMD(p);     // aoe v.1
 336         return nconfig + sizeof *p - sizeof p->data;
 337 }
 338
 339 static int
 340 aoesrr(Aoesrr *sh, int len)
 341 {
 342         uchar *m, *e;
 343         int n;
 344
 345         e = (uchar *) sh + len;
 346         m = (uchar *) sh + Nsrrhdr;
 347         switch (sh->rcmd) {
 348         default:
 349 e:              sh->h.error = BadArg;
 350                 sh->h.flags |= Error;
 351                 break;
 352         case 1: // set
 353                 if (!rrok(sh->h.src)) {
 354                         sh->h.error = Res;
 355                         sh->h.flags |= Error;
 356                         break;
 357                 }
 358         case 2: // force set
 359                 n = sh->nmacs * 6;
 360                 if (e < m + n)
 361                         goto e;
 362                 nsrr = sh->nmacs;
 363                 memmove(srr, m, n);
 364         case 0: // read
 365                 break;
 366         }
 367         sh->nmacs = nsrr;
 368         n = nsrr * 6;
 369         memmove(m, srr, n);
 370         return Nsrrhdr + n;
 371 }
 372
 373 static int
 374 addmask(uchar *ea)
 375 {
 376
 377         uchar *p, *e;
 378
 379         p = masks;
 380         e = p + nmasks;
 381         for (; p<e; p += 6)
 382                 if (!memcmp(p, ea, 6))
 383                         return 2;
 384         if (nmasks >= Nmasks)
 385                 return 0;
 386         memmove(p, ea, 6);
 387         nmasks++;
 388         return 1;
 389 }
 390
 391 static void
 392 rmmask(uchar *ea)
 393 {
 394         uchar *p, *e;
 395
 396         p = masks;
 397         e = p + nmasks;
 398         for (; p<e; p+=6)
 399                 if (!memcmp(p, ea, 6)) {
 400                         memmove(p, p+6, e-p-6);
 401                         nmasks--;
 402                         return;
 403                 }
 404 }
 405
 406 static int
 407 aoemask(Aoemask *mh, int len)
 408 {
 409         Mdir *md, *mdi, *mde;
 410         int i, n;
 411
 412         n = 0;
 413         md = mdi = (Mdir *) ((uchar *)mh + Nmaskhdr);
 414         switch (mh->cmd) {
 415         case Medit:
 416                 mde = md + mh->nmacs;
 417                 for (; md<mde; md++) {
 418                         switch (md->cmd) {
 419                         case MDdel:
 420                                 rmmask(md->mac);
 421                                 continue;
 422                         case MDadd:
 423                                 if (addmask(md->mac))
 424                                         continue;
 425                                 mh->merror = MEfull;
 426                                 mh->nmacs = md - mdi;
 427                                 goto e;
 428                         case MDnop:
 429                                 continue;
 430                         default:
 431                                 mh->merror = MEbaddir;
 432                                 mh->nmacs = md - mdi;
 433                                 goto e;
 434                         }
 435                 }
 436                 // success.  fall thru to return list
 437         case Mread:
 438                 md = mdi;
 439                 for (i=0; i<nmasks; i++) {
 440                         md->res = md->cmd = 0;
 441                         memmove(md->mac, &masks[i*6], 6);
 442                         md++;
 443                 }
 444                 mh->merror = 0;
 445                 mh->nmacs = nmasks;
 446                 n = sizeof *md * nmasks;
 447                 break;
 448         default:
 449                 mh->h.flags |= Error;
 450                 mh->h.error = BadArg;
 451         }
 452 e:      return n + Nmaskhdr;
 453 }
 454
 455 void
 456 doaoe(Aoehdr *p, int n)
 457 {
 458         int len;
 459
 460         switch (p->cmd) {
 461         case ATAcmd:
 462                 if (n < Natahdr)
 463                         return;
 464                 len = aoeata((Ata*)p, n);
 465                 break;
 466         case Config:
 467                 if (n < Ncfghdr)
 468                         return;
 469                 len = confcmd((Conf *)p, n);
 470                 break;
 471         case Mask:
 472                 if (n < Nmaskhdr)
 473                         return;
 474                 len = aoemask((Aoemask *)p, n);
 475                 break;
 476         case Resrel:
 477                 if (n < Nsrrhdr)
 478                         return;
 479                 len = aoesrr((Aoesrr *)p, n);
 480                 break;
 481         default:
 482                 p->error = BadCmd;
 483                 p->flags |= Error;
 484                 len = n;
 485                 break;
 486         }
 487         if (len <= 0)
 488                 return;
 489         memmove(p->dst, p->src, 6);
 490         memmove(p->src, mac, 6);
 491         p->maj = htons(shelf);
 492         p->min = slot;
 493         p->flags |= Resp;
 494         if (putpkt(sfd, (uchar *) p, len) == -1) {
 495                 perror("write to network");
 496                 exit(1);
 497         }
 498 }
 499
 500 void
 501 aoe(void)
 502 {
 503         Aoehdr *p;
 504         uchar *buf;
 505         int n, sh;
 506         long pagesz;
 507         enum { bufsz = 1<<16, };
 508
 509         if ((pagesz = sysconf(_SC_PAGESIZE)) < 0) {
 510                 perror("sysconf");
 511                 exit(1);
 512         }
 513         if ((buf = malloc(bufsz + pagesz)) == NULL) {
 514                 perror("malloc");
 515                 exit(1);
 516         }
 517         n = (size_t) buf + sizeof(Ata);
 518         if (n & (pagesz - 1))
 519                 buf += pagesz - (n & (pagesz - 1));
 520
 521         aoead(sfd);
 522
 523         for (;;) {
 524                 n = getpkt(sfd, buf, bufsz);
 525                 if (n < 0) {
 526                         perror("read network");
 527                         exit(1);
 528                 }
 529                 if (n < sizeof(Aoehdr))
 530                         continue;
 531                 p = (Aoehdr *) buf;
 532                 if (ntohs(p->type) != 0x88a2)
 533                         continue;
 534                 if (p->flags & Resp)
 535                         continue;
 536                 sh = ntohs(p->maj);
 537                 if (sh != shelf && sh != (ushort)~0)
 538                         continue;
 539                 if (p->min != slot && p->min != (uchar)~0)
 540                         continue;
 541                 if (nmasks && !maskok(p->src))
 542                         continue;
 543                 doaoe(p, n);
 544         }
 545 }
 546
 547 void
 548 usage(void)
 549 {
 550         fprintf(stderr, "usage: %s [-b bufcnt] [-o offset] [-l length] [-d ] [-s] [-r] [ -m mac[,mac...] ] shelf slot netif filename\n",
 551                 progname);
 552         exit(1);
 553 }
 554
 555 /* parseether from plan 9 */
 556 int
 557 parseether(uchar *to, char *from)
 558 {
 559         char nip[4];
 560         char *p;
 561         int i;
 562
 563         p = from;
 564         for(i = 0; i < 6; i++){
 565                 if(*p == 0)
 566                         return -1;
 567                 nip[0] = *p++;
 568                 if(*p == 0)
 569                         return -1;
 570                 nip[1] = *p++;
 571                 nip[2] = 0;
 572                 to[i] = strtoul(nip, 0, 16);
 573                 if(*p == ':')
 574                         p++;
 575         }
 576         return 0;
 577 }
 578
 579 void
 580 setmask(char *ml)
 581 {
 582         char *p;
 583         int n;
 584
 585         for (; ml; ml=p) {
 586                 p = strchr(ml, ',');
 587                 if (p)
 588                         *p++ = '\0';
 589                 n = parseether(&masks[nmasks*Alen], ml);
 590                 if (n < 0)
 591                         fprintf(stderr, "ignoring mask %s, parseether failure\n", ml);
 592                 else
 593                         nmasks++;
 594         }
 595 }
 596
 597 int
 598 maskok(uchar *ea)
 599 {
 600         int i, ok = 0;
 601
 602         for (i=0; !ok && i<nmasks; i++)
 603                 ok = memcmp(ea, &masks[i*Alen], Alen) == 0;
 604         return ok;
 605 }
 606
 607 int
 608 rrok(uchar *ea)
 609 {
 610         int i, ok = 0;
 611
 612         if (nsrr == 0)
 613                 return 1;
 614         for (i=0; !ok && i<nsrr; i++)
 615                 ok = memcmp(ea, &srr[i*Alen], Alen) == 0;
 616         return ok;
 617 }
 618
 619 void
 620 setserial(int sh, int sl)
 621 {
 622         char h[32];
 623
 624         h[0] = 0;
 625         gethostname(h, sizeof h);
 626         snprintf(serial, Nserial, "%d.%d:%.*s", sh, sl, (int) sizeof h, h);
 627 }
 628
 629 int
 630 main(int argc, char **argv)
 631 {
 632         int ch, omode = 0, readonly = 0;
 633         vlong length = 0;
 634         char *end;
 635     char filepath[300] = {0};
 636
 637     /* Avoid to be killed by systemd */
 638     if (access("/etc/initrd-release", F_OK) >= 0)
 639         {
 640                 argv[0][0] = '@';
 641         }
 642
 643         bufcnt = Bufcount;
 644         offset = 0;
 645         setbuf(stdin, NULL);
 646         progname = *argv;
 647         while ((ch = getopt(argc, argv, "b:dsrm:f:tv::o:l:")) != -1) {
 648                 switch (ch) {
 649                 case 'b':
 650                         bufcnt = atoi(optarg);
 651                         break;
 652                 case 'd':
 653 #ifdef O_DIRECT
 654                         omode |= O_DIRECT;
 655 #endif
 656                         break;
 657                 case 's':
 658                         omode |= O_SYNC;
 659                         break;
 660                 case 'r':
 661                         readonly = 1;
 662                         break;
 663                 case 'm':
 664                         setmask(optarg);
 665                         break;
 666         case 't':
 667             return 0;
 668         case 'v':
 669             verbose = 1;
 670             break;
 671         case 'f':
 672             strncpy(filepath, optarg, sizeof(filepath) - 1);
 673             break;
 674                 case 'o':
 675                         offset = strtoll(optarg, &end, 0);
 676                         if (end == optarg || offset < 0)
 677                                 usage();
 678                         break;
 679                 case 'l':
 680                         length = strtoll(optarg, &end, 0);
 681                         if (end == optarg || length < 1)
 682                                 usage();
 683                         break;
 684                 case '?':
 685                 default:
 686                         usage();
 687                 }
 688         }
 689         argc -= optind;
 690         argv += optind;
 691         if (argc != 4 || bufcnt <= 0)
 692                 usage();
 693         omode |= readonly ? O_RDONLY : O_RDWR;
 694     parse_img_chunk(filepath);
 695         bfd = open(argv[3], omode);
 696         if (bfd == -1) {
 697                 perror("open");
 698                 exit(1);
 699         }
 700         shelf = atoi(argv[0]);
 701         slot = atoi(argv[1]);
 702         setserial(shelf, slot);
 703         size = g_iso_file_size; //getsize(bfd);
 704         size /= 512;
 705         if (size <= offset) {
 706                 if (offset)
 707                         fprintf(stderr,
 708                                 "Offset %lld too large for %lld-sector export\n",
 709                                 offset,
 710                                 size);
 711                 else
 712                         fputs("0-sector file size is too small\n", stderr);
 713                 exit(1);
 714         }
 715         size -= offset;
 716         if (length) {
 717                 if (length > size) {
 718                         fprintf(stderr, "Length %llu too big - exceeds size of file!\n", offset);
 719                         exit(1);
 720                 }
 721                 size = length;
 722         }
 723         ifname = argv[2];
 724         sfd = dial(ifname, bufcnt);
 725         if (sfd < 0)
 726                 return 1;
 727         getea(sfd, ifname, mac);
 728
 729     if (verbose) {
 730         printf("pid %ld: e%d.%d, %lld sectors %s\n",
 731                     (long) getpid(), shelf, slot, size,
 732                     readonly ? "O_RDONLY" : "O_RDWR");
 733     }
 734
 735         fflush(stdout);
 736         atainit();
 737         aoe();
 738         return 0;
 739 }
 740