freebsd.c

Virtual EtherDrive&#174 blade AoE target.

/*
 * Copyright (c) 2005, Stacey Son <sson (at) verio (dot) net>
 * All rights reserved.
 */

// freebsd.c: low level access routines for FreeBSD
#include "config.h"
#include <sys/types.h>
#include <sys/socket.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/time.h>

#include <netinet/in.h>
#include <net/ethernet.h>
#include <net/bpf.h>
#include <net/if.h>
#include <net/if_arp.h>
#include <net/if_dl.h>
#include <net/route.h>

#include <sys/ioctl.h>
#include <sys/types.h>
#include <net/if.h>
#include <sys/stat.h>
#include <sys/disklabel.h>
#include <sys/select.h>
#include <sys/sysctl.h>

#include <fcntl.h>
#include <errno.h>

#include "dat.h"
#include "fns.h"

#define BPF_DEV "/dev/bpf0"

/* Packet buffer for getpkt() */
static uchar *pktbuf = NULL;
static int pktbufsz = 0;

int
dial(char *eth)
{
	char m;
	int fd = -1;
	struct bpf_version bv;
	u_int v;
	unsigned bufsize, linktype;
	char device[sizeof BPF_DEV];
	struct ifreq ifr;

	/* packet filter for bpf */
	struct bpf_insn bpf_insns[] = {
	  /* Load the type into register */
	  BPF_STMT(BPF_LD+BPF_H+BPF_ABS, 12),
	  /* Does it match AoE Type (0x88a2)? No, goto INVALID */
	  BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, 0x88a2, 0, 10),
	  /* Load the flags into register */
	  BPF_STMT(BPF_LD+BPF_B+BPF_ABS, 14),
	  /* Check to see if the Resp flag is set */
	  BPF_STMT(BPF_ALU+BPF_AND+BPF_K, Resp),
	  /* Yes, goto INVALID */
	  BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, 0, 0, 7),
	  /* Load the command into register */
	  BPF_STMT(BPF_LD+BPF_B+BPF_ABS, 19),
	  /* Is this a ATAcmd? No, goto VALID */
	  BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, ATAcmd, 0, 4),
	  /* Load the shelf number into register */
	  BPF_STMT(BPF_LD+BPF_H+BPF_ABS, 16),
	  /* Does it match shelf number? No, goto INVALID */
	  BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, (u_int) shelf, 0, 3),
	  /* Load the slot number into register */
	  BPF_STMT(BPF_LD+BPF_B+BPF_ABS, 18),
	  /* Does it match shelf number? No, goto INVALID */
	  BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, (u_int) slot, 0, 1),
	  /* VALID: return -1 (allow the packet to be read) */
	  BPF_STMT(BPF_RET+BPF_K, (u_int)-1),
	  /* INVALID: return 0 (ignore the packet) */
	  BPF_STMT(BPF_RET+BPF_K, 0),
	};

	struct bpf_program bpf_program = {
	  sizeof(bpf_insns)/sizeof(struct bpf_insn),
	  bpf_insns
	};


	strncpy(device, BPF_DEV, sizeof BPF_DEV);

	/* find a bpf device we can use, check /dev/bpf[0-9] */
	for (m = '0'; m <= '9'; m++) {
		device[sizeof(BPF_DEV)-2] = m;

		if ((fd = open(device, O_RDWR)) > 0)
			break;
	}

	if (fd < 0) {
		perror("open");
		return -1;
	}

	if (ioctl(fd, BIOCVERSION, &bv) < 0) {
		perror("BIOCVERSION");
		goto bad;
	}

	if (bv.bv_major != BPF_MAJOR_VERSION ||
	    bv.bv_minor < BPF_MINOR_VERSION) {
		fprintf(stderr,
			"kernel bpf filter out of date\n");
		goto bad;
	}

	/*
	 * Try finding a good size for the buffer; 65536 may be too
	 * big, so keep cutting it in half until we find a size
	 * that works, or run out of sizes to try.
	 *
	 */
	for (v = 65536; v != 0; v >>= 1) {
		(void) ioctl(fd, BIOCSBLEN, (caddr_t)&v);

		(void)strncpy(ifr.ifr_name, eth,
			sizeof(ifr.ifr_name));
		if (ioctl(fd, BIOCSETIF, (caddr_t)&ifr) >= 0)
			break;  /* that size worked; we're done */

		if (errno != ENOBUFS) {
			fprintf(stderr, "BIOCSETIF: %s: %s\n",
					eth, strerror(errno));
			goto bad;
		}
	}
	if (v == 0) {
		fprintf(stderr, 
			"BIOCSBLEN: %s: No buffer size worked\n", eth);
		goto bad;
	}

	/* Allocate memory for the packet buffer */
	pktbufsz = v;
	if ((pktbuf = malloc(pktbufsz)) == NULL) {
		perror("malloc");
		goto bad;
	}

	/* Don't wait for buffer to be full or timeout */
	v = 1;
	if (ioctl(fd, BIOCIMMEDIATE, &v) < 0) {
		perror("BIOCIMMEDIATE");
		goto bad;
	}

	/* Only read incoming packets */
	v = 0;
	if (ioctl(fd, BIOCSSEESENT, &v) < 0) {
		perror("BIOCSSEESENT");
		goto bad;
	}

	/* Don't complete ethernet hdr */
	v = 1;
	if (ioctl(fd, BIOCSHDRCMPLT, &v) < 0) {
		perror("BIOCSHDRCMPLT");
		goto bad;
	}

	/* Get the data link layer type. */
	if (ioctl(fd, BIOCGDLT, (caddr_t)&v) < 0) {
		perror("BIOCGDLT");
		goto bad;
	}
	linktype = v;

	/* Get the filter buf size */
	if (ioctl(fd, BIOCGBLEN, (caddr_t)&v) < 0) {
		perror("BIOCGBLEN");
		goto bad;
	}
	bufsize = v;

	if (ioctl(fd, BIOCSETF, (caddr_t)&bpf_program) < 0) {
		perror("BIOSETF");
		goto bad;
	} 

	return(fd);

bad:
	close(fd);
	return(-1);
}

int
getea(int s, char *eth, uchar *ea)
{
	int mib[6];
	size_t len;
	char *buf, *next, *end;
	struct if_msghdr *ifm;
	struct sockaddr_dl *sdl;
	

	mib[0] = CTL_NET; 	mib[1] = AF_ROUTE;
	mib[2] = 0; 		mib[3] = AF_LINK;
	mib[4] = NET_RT_IFLIST;	mib[5] = 0;

	if (sysctl(mib, 6, NULL, &len, NULL, 0) < 0) {
		return (-1);
	}

	if (!(buf = (char *) malloc(len))) {
		return (-1);
	}
	
	if (sysctl(mib, 6, buf, &len, NULL, 0) < 0) {
		free(buf);
		return (-1);
	}
	end = buf + len;

	for (next = buf; next < end; next += ifm->ifm_msglen) {
		ifm = (struct if_msghdr *)next;
		if (ifm->ifm_type == RTM_IFINFO) {
			sdl = (struct sockaddr_dl *)(ifm + 1);
			if (strncmp(&sdl->sdl_data[0], eth, 
					sdl->sdl_nlen) == 0) {
				memcpy(ea, LLADDR(sdl), ETHER_ADDR_LEN);
				break;
			}

		}

	}

	free(buf);
	return(0);
}


int
getsec(int fd, uchar *place, vlong lba, int nsec)
{
	return pread(fd, place, nsec * 512, lba * 512);
}

int
putsec(int fd, uchar *place, vlong lba, int nsec)
{
	return pwrite(fd, place, nsec * 512, lba * 512);
}

static int pktn = 0;
static uchar *pktbp = NULL;

int
getpkt(int fd, uchar *buf, int sz)
{
	register struct bpf_hdr *bh;
	register int pktlen, retlen;
	
	if (pktn <= 0) { 
		if ((pktn = read(fd, pktbuf, pktbufsz)) < 0) {
			perror("read");
			exit(1);
		}
		pktbp = pktbuf;
	}

	bh = (struct bpf_hdr *) pktbp;
	retlen = (int) bh->bh_caplen;
	/* This memcpy() is currently needed */ 
	memcpy(buf, (void *)(pktbp + bh->bh_hdrlen),
		retlen > sz ? sz : retlen);
	pktlen = bh->bh_hdrlen + bh->bh_caplen; 
	
	pktbp = pktbp + BPF_WORDALIGN(pktlen);
	pktn  -= (int) BPF_WORDALIGN(pktlen);

	return retlen; 
}

int
putpkt(int fd, uchar *buf, int sz)
{
	return write(fd, buf, sz);
}

int
getmtu(int fd, char *name)
{
	return 1500;
}

vlong
getsize(int fd)
{
	vlong size;
	struct stat s;
	int n;
	struct disklabel lab;

	// Try getting disklabel from block dev
	if ((n = ioctl(fd, DIOCGDINFO, lab)) != -1) {  
		size = lab.d_secsize * lab.d_secperunit;
	} else {
		// must not be a block special dev
		if (fstat(fd, &s) == -1) {
			perror("getsize");
			exit(1);
		}
		size = s.st_size;
	}
	printf("ioctl returned %d\n", n);
	printf("%lld bytes\n", size);
	return size;
}