static void do_up(rwlock_t *r, int status) {
assert(r);
assert(critical_allows(CRITICAL_SCHED_LOCK));
sched_lock();
{
WAITQ_WAIT(&r->wq, !tryenter_sched_lock(r, status));
}
sched_unlock();
}
static int scsi_write(block_dev_t *bdev, char *buffer, size_t count,
blkno_t blkno) {
struct scsi_dev *sdev;
int blksize;
unsigned int lba;
char *bp;
int ret;
assert(bdev);
assert(buffer);
sdev = bdev->privdata;
blksize = sdev->blk_size;
struct scsi_cmd cmd = scsi_cmd_template_write10;
cmd.scmd_olen = blksize;
if (!sdev) {
return -ENODEV;
}
scsi_disk_lock(bdev);
for (lba = blkno, bp = buffer;
count >= blksize;
lba++, count -= blksize, bp += blksize) {
cmd.scmd_lba = lba;
cmd.scmd_obuf = bp;
ret = WAITQ_WAIT(&sdev->wq, scsi_wait_cmd_complete(sdev, &cmd));
if (!ret) {
ret = scsi_wake_res(sdev);
}
if (ret) {
break;
}
}
scsi_disk_unlock(bdev);
if (ret && bp == buffer) {
return ret;
}
return bp - buffer;
}
static int hd_write_pio(block_dev_t *bdev, char *buffer, size_t count, blkno_t blkno) {
hd_t *hd;
hdc_t *hdc;
int sectsleft;
int nsects;
int n;
int result = 0;
char *bufp;
if (count == 0) {
return 0;
}
bufp = (char *) buffer;
hd = (hd_t *) bdev->privdata;
hdc = hd->hdc;
sectsleft = count / bdev->block_size;
while (sectsleft > 0) {
/* Select drive */
ide_select_drive(hd);
/* Wait for controller ready */
result = ide_wait(hdc, HDCS_DRDY, HDTIMEOUT_DRDY);
if (result != 0) {
hdc->result = -EIO;
break;
}
nsects = 1;
/* Prepare transfer */
hdc->bufp = bufp;
hdc->nsects = nsects;
hdc->result = 0;
hdc->dir = HD_XFER_WRITE;
hdc->active = hd;
hd_setup_transfer(hd, blkno, nsects);
outb(hd->multsect > 1 ? HDCMD_MULTWRITE : HDCMD_WRITE,
hdc->iobase + HDC_COMMAND);
/* Wait for data ready */
if (!(inb(hdc->iobase + HDC_ALT_STATUS) & HDCS_DRQ)) {
result = ide_wait(hdc, HDCS_DRQ, HDTIMEOUT_DRQ);
if (result != 0) {
hdc->result = -EIO;
break;
}
}
/* Write first sector(s) */
n = hd->multsect;
if (n > nsects) {
n = nsects;
}
while (n-- > 0) {
pio_write_buffer(hd, hdc->bufp, bdev->block_size);
hdc->bufp += bdev->block_size;
}
/* Wait until data written */
WAITQ_WAIT(&hdc->waitq, hdc->result);
if (hdc->result < 0) {
break;
}
/* Advance to next */
sectsleft -= nsects;
bufp += nsects * bdev->block_size;
blkno += nsects; /*WTF?*/
}
/* Cleanup */
hdc->dir = HD_XFER_IDLE;
hdc->active = NULL;
/*
* FIXME This assignment crashes writing, because
* hdc->result equal 1 after writing. So, it set result to 1, afterward
* (return result == 0 ? count : result) return 1.
* --Alexander
*
result = hdc->result;
*/
return result == 0 ? count : result;
}
static int hd_read_pio(block_dev_t *bdev, char *buffer, size_t count, blkno_t blkno) {
hd_t *hd;
hdc_t *hdc;
int sectsleft;
int nsects;
int result = 0;
char *bufp;
if (count == 0) {
return 0;
}
bufp = (char *) buffer;
hd = (hd_t *) bdev->privdata;
hdc = hd->hdc;
sectsleft = count / bdev->block_size;
if (count % bdev->block_size) {
sectsleft++;
}
while (sectsleft > 0) {
/* Select drive */
ide_select_drive(hd);
/* Wait for controller ready */
result = ide_wait(hdc, HDCS_DRDY, HDTIMEOUT_DRDY);
if (result != 0) {
hdc->result = -EIO;
break;
}
nsects = 1;
/* Prepare transfer */
hdc->bufp = bufp;
hdc->nsects = nsects;
hdc->result = 0;
hdc->dir = HD_XFER_READ;
hdc->active = hd;
hd_setup_transfer(hd, blkno, nsects);
outb(hd->multsect > 1 ? HDCMD_MULTREAD : HDCMD_READ,
hdc->iobase + HDC_COMMAND);
/* Wait until data read */
WAITQ_WAIT(&hdc->waitq, hdc->result);
if (hdc->result < 0) {
break;
}
/* Advance to next */
sectsleft -= nsects;
bufp += nsects * bdev->block_size;
blkno += nsects; /*WTF?*/
}
/* Cleanup */
hdc->dir = HD_XFER_IDLE;
hdc->active = NULL;
if (0 < hdc->result) {
result = hdc->result = 0;
}
return result == 0 ? count : result;
}
