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();
}
