static int hub_check_port(struct usb_dev *dev, int port) { struct usb_hub_ps ps; uint32_t time; if (!hub_get_port_status(dev, port, &ps, sizeof(ps))) return false; dprintf("Port Status %04X Port Change %04X\n", le16_to_cpu(ps.wPortStatus), le16_to_cpu(ps.wPortChange)); if (!(le16_to_cpu(ps.wPortStatus) & HUB_PS_POWER)) { hub_set_port_feature(dev, port, HUB_PF_POWER); SLOF_msleep(100); time = SLOF_GetTimer() + USB_TIMEOUT; while (time > SLOF_GetTimer()) { cpu_relax(); hub_get_port_status(dev, port, &ps, sizeof(ps)); if (le16_to_cpu(ps.wPortStatus) & HUB_PS_CONNECTION) { dprintf("power on Port Status %04X Port Change %04X\n", le16_to_cpu(ps.wPortStatus), le16_to_cpu(ps.wPortChange)); break; } } } if (le16_to_cpu(ps.wPortStatus) & HUB_PS_CONNECTION) { hub_set_port_feature(dev, port, HUB_PF_RESET); SLOF_msleep(100); time = SLOF_GetTimer() + USB_TIMEOUT; while (time > SLOF_GetTimer()) { cpu_relax(); hub_get_port_status(dev, port, &ps, sizeof(ps)); if (!(le16_to_cpu(ps.wPortStatus) & HUB_PS_RESET)) { dprintf("reset Port Status %04X Port Change %04X\n", le16_to_cpu(ps.wPortStatus), le16_to_cpu(ps.wPortChange)); return true; } } } return false; }
/** * Read blocks * @param reg pointer to "reg" property * @param buf pointer to destination buffer * @param blocknum block number of the first block that should be read * @param cnt amount of blocks that should be read * @return number of blocks that have been read successfully */ int virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt) { struct vring_desc *desc; int id; static struct virtio_blk_req blkhdr; //struct virtio_blk_config *blkconf; uint64_t capacity; uint32_t vq_size, time; struct vring_desc *vq_desc; /* Descriptor vring */ struct vring_avail *vq_avail; /* "Available" vring */ struct vring_used *vq_used; /* "Used" vring */ volatile uint8_t status = -1; volatile uint16_t *current_used_idx; uint16_t last_used_idx; //printf("virtioblk_read: dev=%p buf=%p blocknum=%li count=%li\n", // dev, buf, blocknum, cnt); /* Check whether request is within disk capacity */ capacity = virtio_get_config(dev, 0, sizeof(capacity)); if (blocknum + cnt - 1 > capacity) { puts("virtioblk_read: Access beyond end of device!"); return 0; } vq_size = virtio_get_qsize(dev, 0); vq_desc = virtio_get_vring_desc(dev, 0); vq_avail = virtio_get_vring_avail(dev, 0); vq_used = virtio_get_vring_used(dev, 0); last_used_idx = vq_used->idx; current_used_idx = &vq_used->idx; /* Set up header */ blkhdr.type = VIRTIO_BLK_T_IN | VIRTIO_BLK_T_BARRIER; blkhdr.ioprio = 1; blkhdr.sector = blocknum; /* Determine descriptor index */ id = (vq_avail->idx * 3) % vq_size; /* Set up virtqueue descriptor for header */ desc = &vq_desc[id]; desc->addr = (uint64_t)&blkhdr; desc->len = sizeof(struct virtio_blk_req); desc->flags = VRING_DESC_F_NEXT; desc->next = (id + 1) % vq_size; /* Set up virtqueue descriptor for data */ desc = &vq_desc[(id + 1) % vq_size]; desc->addr = (uint64_t)buf; desc->len = cnt * 512; desc->flags = VRING_DESC_F_NEXT | VRING_DESC_F_WRITE; desc->next = (id + 2) % vq_size; /* Set up virtqueue descriptor for status */ desc = &vq_desc[(id + 2) % vq_size]; desc->addr = (uint64_t)&status; desc->len = 1; desc->flags = VRING_DESC_F_WRITE; desc->next = 0; vq_avail->ring[vq_avail->idx % vq_size] = id; mb(); vq_avail->idx += 1; /* Tell HV that the queue is ready */ virtio_queue_notify(dev, 0); /* Wait for host to consume the descriptor */ time = SLOF_GetTimer() + VIRTIO_TIMEOUT; while (*current_used_idx == last_used_idx) { // do something better mb(); if (time < SLOF_GetTimer()) break; } if (status == 0) return cnt; printf("virtioblk_read failed! status = %i\n", status); return 0; }
void SLOF_msleep(uint32_t time) { time = SLOF_GetTimer() + time; while (time > SLOF_GetTimer()) cpu_relax(); }