/* * rtcpu-loader - AP4 SH Core Linux backing code, 20110228 Magnus Damm * * A small Linux program that executes a SH Linux kernel on the RT-CPU. * * Compile for Linux running on the ARM side using: * $ arm-cross-gcc -o rtcpu-loader rtcpu-loader.c * * Includes code from the lguest, many thanks to Rusty Russell. * For more info see linux kernel source Documentation/lguest/lguest.c * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; version 2 of the License. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ #include #include #include #include #include #include #include #include #include #include #include #include #include #define IRQ_EARLY_CONSOLE 0 /* must match kernel irq setup */ #define IRQ_POWER_OFF 1 /* must match kernel irq setup */ #define IRQ_TIMER 2 /* must match kernel irq setup */ #define IRQ_VIRTIO 3 /* must match kernel irq setup */ #define IRQ_CONSOLE_RX 4 #define IRQ_CONSOLE_TX 5 #define IRQ_NET_RX 6 #define IRQ_NET_TX 7 #define IRQ_NR 16 static int fgets_with_openclose(char *fname, char *buf, size_t maxlen) { FILE *fp; if ((fp = fopen(fname, "r")) != NULL) { fgets(buf, maxlen, fp); fclose(fp); return strlen(buf); } else { return -1; } } struct uio_device { char *name; char *path; int fd; }; #define MAXUIOIDS 100 #define MAXNAMELEN 256 static int locate_uio_device(char *name, struct uio_device *udp) { char fname[MAXNAMELEN], buf[MAXNAMELEN]; int uio_id, i; for (uio_id = 0; uio_id < MAXUIOIDS; uio_id++) { sprintf(fname, "/sys/class/uio/uio%d/name", uio_id); if (fgets_with_openclose(fname, buf, MAXNAMELEN) < 0) continue; if (strncmp(name, buf, strlen(name)) == 0) break; } if (uio_id >= MAXUIOIDS) return -1; udp->name = strdup(buf); udp->path = strdup(fname); udp->path[strlen(udp->path) - 4] = '\0'; sprintf(buf, "/dev/uio%d", uio_id); udp->fd = open(buf, O_RDWR|O_SYNC /*| O_NONBLOCK*/); if (udp->fd < 0) { perror("open"); return -1; } return 0; } struct uio_map { unsigned long address; unsigned long size; void *iomem; }; static int setup_uio_map(struct uio_device *udp, int nr, struct uio_map *ump) { char fname[MAXNAMELEN], buf[MAXNAMELEN]; sprintf(fname, "%s/maps/map%d/addr", udp->path, nr); if (fgets_with_openclose(fname, buf, MAXNAMELEN) <= 0) return -1; ump->address = strtoul(buf, NULL, 0); sprintf(fname, "%s/maps/map%d/size", udp->path, nr); if (fgets_with_openclose(fname, buf, MAXNAMELEN) <= 0) return -1; ump->size = strtoul(buf, NULL, 0); ump->iomem = mmap(0, ump->size, PROT_READ|PROT_WRITE, MAP_SHARED, udp->fd, nr * getpagesize()); if (ump->iomem == MAP_FAILED) return -1; return 0; } struct uio_device uio_dev; struct uio_map uio_mmio, uio_mem; typedef unsigned char __u8; typedef unsigned short __u16; typedef unsigned long ___u32; typedef unsigned long long __u64; struct lguest_device_desc { /* The device type: console, network, disk etc. Type 0 terminates. */ __u8 type; /* The number of virtqueues (first in config array) */ __u8 num_vq; /* * The number of bytes of feature bits. Multiply by 2: one for host * features and one for Guest acknowledgements. */ __u8 feature_len; /* The number of bytes of the config array after virtqueues. */ __u8 config_len; /* A status byte, written by the Guest. */ __u8 status; __u8 config[0]; }; /*D:135 * This is how we expect the device configuration field for a virtqueue * to be laid out in config space. */ struct lguest_vqconfig { /* The number of entries in the virtio_ring */ __u16 num; /* The interrupt we get when something happens. */ __u16 irq; /* The page number of the virtio ring for this device. */ ___u32 pfn; }; /*:*/ #define RING_NR 256 /* Virtio ring descriptors: 16 bytes. These can chain together via "next". */ struct vring_desc { /* Address (guest-physical). */ __u64 addr; /* Length. */ ___u32 len; /* The flags as indicated above. */ __u16 flags; /* We chain unused descriptors via this, too */ __u16 next; }; struct vring_avail { __u16 flags; __u16 idx; __u16 ring[]; }; /* u32 is used here for ids for padding reasons. */ struct vring_used_elem { /* Index of start of used descriptor chain. */ ___u32 id; /* Total length of the descriptor chain which was used (written to) */ ___u32 len; }; struct vring_used { __u16 flags; __u16 idx; volatile struct vring_used_elem ring[]; }; struct vring { unsigned int num; volatile struct vring_desc *desc; volatile struct vring_avail *avail; volatile struct vring_used *used; }; static inline void vring_init(volatile struct vring *vr, unsigned int num, void *p, unsigned long align) { vr->num = num; vr->desc = p; vr->avail = p + num*sizeof(struct vring_desc); vr->used = (void *)(((unsigned long)&vr->avail->ring[num] + align-1) & ~(align - 1)); } static inline unsigned vring_size(unsigned int num, unsigned long align) { return ((sizeof(struct vring_desc) * num + sizeof(__u16) * (2 + num) + align - 1) & ~(align - 1)) + sizeof(__u16) * 2 + sizeof(struct vring_used_elem) * num; } struct __vq { unsigned int local; volatile struct vring vring; }; volatile struct __vq console_in; volatile struct __vq console_out; volatile struct __vq net_in; volatile struct __vq net_out; void setup_vq(volatile struct __vq *vq, unsigned long pfn) { unsigned long offset; offset = (pfn << 12) - uio_mem.address; vq->local = 0; memset(uio_mem.iomem + offset, 0, vring_size(RING_NR, 4096)); vring_init(&vq->vring, RING_NR, uio_mem.iomem + offset, 4096); } #define wmb() __asm__ __volatile__("" : : : "memory") int vq_do(volatile struct __vq *vq, int input, int output) { volatile struct vring *vring = &vq->vring; volatile struct vring_desc *desc; volatile struct vring_used_elem *used; int idx = vring->avail->idx; int i, len; desc = vring->desc; if (vq->local == idx) return 0; i = vq->vring.avail->ring[vq->local % vq->vring.num]; wmb(); if (output) { write(0, (desc[i].addr - uio_mem.address) + uio_mem.iomem, desc[i].len); len = 0; } if (input) { unsigned char in_buf; len = read(1, &in_buf, 1); if (len > 0) *(unsigned char *)((desc[i].addr - uio_mem.address) + uio_mem.iomem) = in_buf; if (len <= 0) return -1; } wmb(); used = &vring->used->ring[vring->used->idx % vring->num]; used->id = i; used->len = len; wmb(); vring->used->idx++; vq->local++; return 1; } static int vq_first(volatile struct __vq *vq, unsigned long *addrp, int *lenp, int *headp) { volatile struct vring *vring = &vq->vring; volatile struct vring_desc *desc; int idx = vring->avail->idx; int i; desc = vring->desc; if (vq->local == idx) return 0; i = vq->vring.avail->ring[vq->local % vq->vring.num]; wmb(); *addrp = desc[i].addr; *lenp = desc[i].len; *headp = i; return 1; } static int vq_next(volatile struct __vq *vq, unsigned long *addrp, int *lenp, int *currp) { volatile struct vring *vring = &vq->vring; volatile struct vring_desc *desc = vring->desc; int i; /* handle looped descriptor */ if (desc[*currp].flags & 1) { i = desc[*currp].next; *addrp = desc[i].addr; *lenp = desc[i].len; *currp = i; return 1; } return 0; } static void *uio_addr(unsigned long addr) { return uio_mem.iomem + (addr - uio_mem.address); } static void vq_used(volatile struct __vq *vq, int len, int head) { volatile struct vring *vring = &vq->vring; volatile struct vring_used_elem *used; used = &vring->used->ring[vring->used->idx % vring->num]; used->id = head; used->len = len; wmb(); vring->used->idx++; vq->local++; } int vq_do_net(volatile struct __vq *vq, int input, int output) { struct iovec iov[vq->vring.num]; int sum = 0; unsigned long addr; int len; int head; int curr; int k, i; int n = 0; int m; if (vq_first(vq, &addr, &len, &head)) { curr = head; do { iov[n].iov_base = uio_addr(addr); iov[n].iov_len = len; n++; } while (vq_next(vq, &addr, &len, &curr)); if (output) { sum = writev(output, iov, n); len = 0; } if (input) { sum = readv(input, iov, n); if (sum <= 0) return -1; len = sum; } #if 0 printf("iov %d/%d: %d vectors of total %d bytes:\n", input, output, n, sum); m = 0; for (i = 0; i < n; i++) { printf("%d: ", i); for (k = 0; k < iov[i].iov_len; k++) if (m < sum) { printf("%02x ", *(unsigned char *)(iov[i].iov_base + k)); m++; } printf("\n"); } #endif vq_used(vq, len, head); return 1; } return 0; } unsigned long setup_virtio_device(void *buf, int type, int irq_in, unsigned long pfn_in, volatile struct __vq *vq_in, int irq_out, unsigned long pfn_out, volatile struct __vq *vq_out) { struct lguest_device_desc *dd = buf; struct lguest_vqconfig vq0, vq1; dd->type = type; /* VIRTIO_ID_... */ dd->num_vq = 2; memset(&vq0, 0, sizeof(vq0)); vq0.num = RING_NR; vq0.irq = irq_in; vq0.pfn = pfn_in; memcpy(buf + sizeof(*dd), &vq0, sizeof(vq0)); setup_vq(vq_in, vq0.pfn); memset(&vq1, 0, sizeof(vq1)); vq1.num = RING_NR; vq1.irq = irq_out; vq1.pfn = pfn_out; memcpy(buf + sizeof(*dd) + sizeof(vq0), &vq1, sizeof(vq1)); setup_vq(vq_out, vq1.pfn); return sizeof(*dd) + sizeof(vq0) + sizeof(vq1); } static int setup_tun(char *name) { struct ifreq ifr; int fd, err; char *clonedev = "/dev/net/tun"; if ((fd = open(clonedev, O_RDWR)) < 0) return 0; memset(&ifr, 0, sizeof(ifr)); ifr.ifr_flags = IFF_TAP | IFF_NO_PI | IFF_VNET_HDR; strncpy(ifr.ifr_name, name, IFNAMSIZ); if ((err = ioctl(fd, TUNSETIFF, (void *) &ifr)) < 0) { close(fd); return 0; } return fd; } /* The original tty settings to restore on exit. */ static struct termios orig_term; static void cleanup_devices(void) { /* If we saved off the original terminal settings, restore them now. */ if (orig_term.c_lflag & (ISIG|ICANON|ECHO)) tcsetattr(STDIN_FILENO, TCSANOW, &orig_term); } static void dump_pending(char *str, unsigned long *pending) { #if 0 int k; printf("%s ", str); for (k = 0; k < IRQ_NR; k++) printf("%d ", pending[k]); printf("\n"); #endif } static struct timeval select_time; static struct timeval *select_timeval; /* NULL - always blocking by default */ static struct timeval last_time; static struct timeval next_time; static void update_next_time(void) { next_time.tv_usec += 10000; if (next_time.tv_usec / 1000000) { next_time.tv_usec %= 1000000; next_time.tv_sec += 1; } } static void update_last_time(void) { last_time = next_time; update_next_time(); } static void init_timer(void) { gettimeofday(&last_time, NULL); next_time = last_time; update_next_time(); select_timeval = &select_time; } static int pending_timer(void) { struct timeval now; gettimeofday(&now, NULL); if ((now.tv_sec >= next_time.tv_sec) || ((now.tv_sec == next_time.tv_sec) && (now.tv_usec >= next_time.tv_usec))) { update_last_time(); return 1; } return 0; } static void update_timeout(void) { struct timeval now; int usec; gettimeofday(&now, NULL); if (next_time.tv_sec > now.tv_sec) { usec = (next_time.tv_sec - now.tv_sec) * 1000000; usec += next_time.tv_usec - now.tv_usec; } else { if (next_time.tv_usec > now.tv_usec) usec = next_time.tv_usec - now.tv_usec; else usec = 0; } select_time.tv_sec = 0; select_time.tv_usec = usec % 1000000; } int main(int argc, char *argv[]) { int ret; unsigned long offset = 0; int tun_fd = 0; if (argc < 2) { fprintf(stderr, "usage: %s sh-binary [offset]\n", argv[0]); return 1; } ret = locate_uio_device("MFIS", &uio_dev); if (ret < 0) return ret; ret = setup_uio_map(&uio_dev, 0, &uio_mmio); if (ret < 0) return ret; ret = setup_uio_map(&uio_dev, 1, &uio_mem); if (ret < 0) return ret; if (argc >= 3) { offset = strtoul(argv[2], NULL, 0); /* fill the up the offset window with ADD opcodes */ memset(uio_mem.iomem, 0x3c, offset); } /* If we can save the initial standard input settings... */ if (tcgetattr(STDIN_FILENO, &orig_term) == 0) { struct termios term = orig_term; /* If we exit this restores the tty. */ atexit(cleanup_devices); /* * Then we turn off echo, line buffering and ^C etc: We want a * raw input stream to the Guest. */ term.c_lflag &= ~(ISIG|ICANON|ECHO); tcsetattr(STDIN_FILENO, TCSANOW, &term); } { int fd = open(argv[1], O_RDONLY); int n; if (fd < 0) { perror("open"); return -1; } n = read(fd, uio_mem.iomem + offset, uio_mem.size - offset); if (n <= 0) { perror("read"); return -1; } } /* Clear pending interrupts in MFIS */ { unsigned long *p = (uio_mmio.iomem + 0x14); /* mfiseicr */ *p = 0; } /* Enable interrupt in UIO driver */ { unsigned long enable = 1; write(uio_dev.fd, &enable, sizeof(u_long)); } /* Setup lguest VIRTIO device descriptor at top 1M of memory. * The kernel must have a matching end-of-memory setup, * either using mem=63M kernel command line option or * using CONFIG_MEMORY_SIZE=0x03f00000 (on a 64M system). */ { unsigned long top_offset = 1 << 20; /* 1M for virtio */ unsigned long max_pfn; unsigned long offset; max_pfn = (uio_mem.address + uio_mem.size - top_offset) >> 12; offset = (max_pfn << 12) - uio_mem.address; /* clear the page to begin with */ memset(uio_mem.iomem + offset, 0x00, 4096); /* setup console device */ offset += setup_virtio_device(uio_mem.iomem + offset, 3, /* VIRTIO_ID_CONSOLE */ IRQ_CONSOLE_RX, max_pfn + 16, &console_in, IRQ_CONSOLE_TX, max_pfn + 32, &console_out); /* setup network device if possible */ tun_fd = setup_tun("tap0"); if (tun_fd) { setup_virtio_device(uio_mem.iomem + offset, 1, /* VIRTIO_ID_NET */ IRQ_NET_RX, max_pfn + 48, &net_in, IRQ_NET_TX, max_pfn + 64, &net_out); } } { unsigned long *mfiseicr = uio_mmio.iomem + 0x14; unsigned long *mfisiicr = uio_mmio.iomem + 0x10; unsigned long *a2s_enabled = uio_mmio.iomem + 0x10100; unsigned long *a2s_pending = uio_mmio.iomem + 0x10200; unsigned long *s2a_pending = uio_mmio.iomem + 0x10300; int a2s_irq, s2a_irq; int timer_enabled = 0; fd_set rfds; int ret; int hi_fd; int tmp; /* clear soft interrupt data structures */ memset(a2s_enabled, 0, 0x100); memset(a2s_pending, 0, 0x100); memset(s2a_pending, 0, 0x100); /* Boot RT-CPU by generating MFI IRQ to RT-CPU */ *mfisiicr = 1; while (1) { FD_ZERO(&rfds); FD_SET(uio_dev.fd, &rfds); FD_SET(1, &rfds); hi_fd = uio_dev.fd; if (tun_fd) FD_SET(tun_fd, &rfds); if (tun_fd > hi_fd) hi_fd = tun_fd; ret = select(hi_fd + 1, &rfds, NULL, NULL, select_timeval); a2s_irq = s2a_irq = 0; /* incoming data from UIO */ if (ret && FD_ISSET(uio_dev.fd, &rfds)) { unsigned long enable = 1; unsigned long n_pending; read(uio_dev.fd, &n_pending, sizeof(u_long)); dump_pending("s2a", s2a_pending); *mfiseicr = 0; write(uio_dev.fd, &enable, sizeof(u_long)); s2a_irq = 1; } /* incoming data from stdin */ if (ret && FD_ISSET(1, &rfds)) { vq_do(&console_in, 1, 0); a2s_pending[IRQ_CONSOLE_RX] = 1; a2s_irq |= a2s_enabled[IRQ_CONSOLE_RX]; } /* incoming data from the network */ if (ret && tun_fd && FD_ISSET(tun_fd, &rfds)) { if (vq_do_net(&net_in, tun_fd, 0)) { a2s_pending[IRQ_NET_RX] = 1; a2s_irq |= a2s_enabled[IRQ_NET_RX]; } } /* only generate timer ticks after requested by slave */ if (s2a_irq && s2a_pending[IRQ_TIMER]) { s2a_pending[IRQ_TIMER] = 0; if (!timer_enabled) { init_timer(); timer_enabled = 1; } } if (timer_enabled && pending_timer()) { a2s_pending[IRQ_TIMER] = 1; a2s_irq |= a2s_enabled[IRQ_TIMER]; } if (s2a_irq && s2a_pending[IRQ_EARLY_CONSOLE]) { printf("%c", s2a_pending[IRQ_EARLY_CONSOLE]); s2a_pending[IRQ_EARLY_CONSOLE] = 0; } if (s2a_irq && s2a_pending[IRQ_CONSOLE_TX]) { s2a_pending[IRQ_CONSOLE_TX] = 0; while (vq_do(&console_out, 0, 1)) ; a2s_pending[IRQ_CONSOLE_TX] = 1; a2s_irq |= a2s_enabled[IRQ_CONSOLE_TX]; } if (s2a_irq && s2a_pending[IRQ_NET_TX] && tun_fd) { s2a_pending[IRQ_NET_TX] = 0; while (vq_do_net(&net_out, 0, tun_fd)) ; a2s_pending[IRQ_NET_TX] = 1; a2s_irq |= a2s_enabled[IRQ_NET_TX]; } if (s2a_irq && s2a_pending[IRQ_POWER_OFF]) { s2a_pending[IRQ_POWER_OFF] = 0; exit(0); } /* trigger RT-CPU IRQ */ if (a2s_irq) { dump_pending("a2s", a2s_pending); *mfisiicr = 1; } update_timeout(); } } return 0; }