From 962f618f270c1e61d7044d42edfdcfa5cfb8baec Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Sat, 21 May 2011 12:07:02 +0200 Subject: [PATCH] add disktimeout feature --- Makefile | 2 +- TODO | 3 - configfile.c | 4 + configfile.h | 3 + disktimeout.c | 271 +++++++++++++++++++++++++++ disktimeout.h | 7 + event.c | 11 +- lcd.c | 27 ++- qnapd.c | 18 +- qnapd.conf | 15 ++ sgio.c | 492 ++++++++++++++++++++++++++++++++++++++++++++++++++ sgio.h | 230 +++++++++++++++++++++++ 12 files changed, 1070 insertions(+), 13 deletions(-) create mode 100644 disktimeout.c create mode 100644 disktimeout.h create mode 100644 sgio.c create mode 100644 sgio.h diff --git a/Makefile b/Makefile index 83a9ed9..9013763 100644 --- a/Makefile +++ b/Makefile @@ -43,6 +43,6 @@ install: $(TARGET) install -d -m 755 $(DESTDIR)$(LOG_DIR) clean: - rm -rf $(BUILD_DIR) $(TARGET) + rm -rf $(BUILD_DIR) $(TARGET) $(TARGET).log $(TARGET).pid include $(shell find $(BUILD_DIR) -name \*.d 2> /dev/null) diff --git a/TODO b/TODO index 5d6b904..b63741d 100644 --- a/TODO +++ b/TODO @@ -4,8 +4,5 @@ - disk utilization (MB/GB) - mntpoints given in config -- disk temperature / idle state - - devicenames / timeoutvalue given in config - - network usage? - cpu usage? diff --git a/configfile.c b/configfile.c index 05a28b9..67f5a5e 100644 --- a/configfile.c +++ b/configfile.c @@ -236,6 +236,10 @@ struct strtoken * strtokenize(const char *input, const char *delim, int maxfield char *ptr = (char *)&tokens->field[maxfields]; strcpy(ptr, input); + tokens->input = input; + tokens->delim = delim; + tokens->maxfields = maxfields; + int i; char *tmp; diff --git a/configfile.h b/configfile.h index 63c2edb..472f9b9 100644 --- a/configfile.h +++ b/configfile.h @@ -13,6 +13,9 @@ int config_get_strings(const char *section_str, const char *option, void *privdata); struct strtoken { + const char *input; + const char *delim; + int maxfields; int count; char *field[0]; }; diff --git a/disktimeout.c b/disktimeout.c new file mode 100644 index 0000000..f6dd1f8 --- /dev/null +++ b/disktimeout.c @@ -0,0 +1,271 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "configfile.h" +#include "event.h" +#include "list.h" +#include "logging.h" + +#include "sgio.h" +int verbose = 0; +int prefer_ata12 = 0; + +#define F_PRESENT 0x0001 +#define F_STANDBY 0x0002 + +struct disk_entry { + struct list_head list; + const char device[16]; + unsigned int timeout; + + int flags; + + unsigned int sectors_rd; + unsigned int sectors_wr; + struct event_timeout *timeout_event; +}; + +static LIST_HEAD(disk_list); +static struct event_timeout *check_timeout; + +static int diskto_timeout_cb(void *privdata) +{ + struct disk_entry *entry = (struct disk_entry *) privdata; + entry->timeout_event = NULL; + + if (!(entry->flags & F_PRESENT)) + return -1; + + int fd = open(entry->device, O_RDONLY | O_NONBLOCK); + if (fd < 0) { + log_print(LOG_WARN, "%s: failed to open %s", __FUNCTION__, entry->device); + return -1; + } + + log_print(LOG_INFO, "disktimeout: %s standby after %ds", entry->device, entry->timeout); + + unsigned char args[4] = { ATA_OP_STANDBYNOW1, 0, 0, 0}; + if (do_drive_cmd(fd, args)) { + args[0] = ATA_OP_STANDBYNOW2; + if (do_drive_cmd(fd, args)) { + log_print(LOG_WARN, "%s: do_drive_cmd(ATA_OP_STANDBYNOW) failed on %s", __FUNCTION__, entry->device); + close(fd); + return -1; + } + } + + entry->flags |= F_STANDBY; + close(fd); + return -1; +} + +static int diskto_add_disk(struct strtoken *tokens, void *privdata) +{ + if (tokens->count != 2) { + log_print(LOG_WARN, "%s(): invalid config line '%s'", __FUNCTION__, tokens->input); + return -1; + } + + struct disk_entry *entry = malloc(sizeof(struct disk_entry)); + if (entry == NULL) { + log_print(LOG_WARN, "%s(): out of memory", __FUNCTION__); + return -1; + } + + strncpy((char *)entry->device, tokens->field[0], sizeof(entry->device)); + + char *tmp; + entry->timeout = strtol(tokens->field[1], &tmp, 0); + if (*tmp != '\0' && !isspace(*tmp)) { + log_print(LOG_WARN, "%s(): invalid timeout value '%s'", __FUNCTION__, tokens->field[1]); + free(entry); + return -1; + } + + entry->flags = 0; + entry->sectors_rd = 0; + entry->sectors_wr = 0; + + struct timeval tv = { .tv_sec = entry->timeout }; + entry->timeout_event = event_add_timeout(&tv, diskto_timeout_cb, (void *)entry); + + log_print(LOG_INFO, "disktimeout: watching '%s' (standby timeout: %ds)", entry->device, entry->timeout); + list_add_tail(&entry->list, &disk_list); + return 0; +} + +static int diskto_check_cb(void *privdata) +{ + struct disk_entry *entry; + list_for_each_entry(entry, &disk_list, list) { + entry->flags &= ~(F_PRESENT); + } + + FILE *fp = fopen("/proc/diskstats", "r"); + if (fp == NULL) { + log_print(LOG_WARN, "%s(): fopen()", __FUNCTION__); + return -1; + } + + char buffer[256]; + while (fgets(buffer, sizeof(buffer), fp) != NULL) { + int major, minor; + char device[16]; + unsigned int val[11]; + + /* field - description + * 1 - major number + * 2 - minor mumber + * 3 - device name + * 4 - reads completed successfully + * 5 - reads merged + * 6 - sectors read + * 7 - time spent reading (ms) + * 8 - writes completed + * 9 - writes merged + * 10 - sectors written + * 11 - time spent writing (ms) + * 12 - I/Os currently in progress + * 13 - time spent doing I/Os (ms) + * 14 - weighted time spent doing I/Os (ms) + */ + int cnt = sscanf(buffer, "%d %d %s %u %u %u %u %u %u %u %u %u %u %u", + &major, &minor, device, &val[0], + &val[1], &val[2], &val[3], &val[4], + &val[5], &val[6], &val[7], &val[8], + &val[9], &val[10]); + + if (cnt != 14) { + log_print(LOG_WARN, "%s(): invalid diskstat line: '%s'", __FUNCTION__, buffer); + continue; + } + + struct disk_entry *entry; + list_for_each_entry(entry, &disk_list, list) { + char *dev = strrchr(entry->device, '/'); + if (dev == NULL || *(dev +1) == '\0') + continue; + + if (strncmp(dev +1, device, strlen(device)) != 0) + continue; + + entry->flags |= F_PRESENT; + + if (entry->sectors_rd == val[2] && entry->sectors_wr == val[6]) + continue; + + entry->flags &= ~(F_STANDBY); + entry->sectors_rd = val[2]; + entry->sectors_wr = val[6]; + + /* remove old timeout */ + if (entry->timeout_event != NULL) + event_remove_timeout(entry->timeout_event); + + /* set new one */ + struct timeval tv = { .tv_sec = entry->timeout }; + entry->timeout_event = event_add_timeout(&tv, diskto_timeout_cb, (void *)entry); + } + } + + fclose(fp); + + list_for_each_entry(entry, &disk_list, list) { + if (!(entry->flags & F_PRESENT)) + continue; + + int fd = open(entry->device, O_RDONLY | O_NONBLOCK); + if (fd < 0) { + log_print(LOG_ERROR, "%s(): failed to open %s", __FUNCTION__, entry->device); + continue; + } + + unsigned char args[4] = { ATA_OP_CHECKPOWERMODE1, 0, 0, 0 }; + if (do_drive_cmd(fd, args)) { + args[0] = ATA_OP_CHECKPOWERMODE2; + if (do_drive_cmd(fd, args)) { + log_print(LOG_WARN, "%s: do_drive_cmd(ATA_OP_CHECKPOWERMODE) failed on %s", __FUNCTION__, entry->device); + close(fd); + continue; + } + } + + /* args[2]: + * 0x00 - standby + * 0x40 - NVcache_spindown + * 0x41 - NVcache_spinup + * 0x80 - idle + * 0xFF - active/idle + */ + + if (args[2] != 0x00) { + if (entry->flags & F_STANDBY) { + log_print(LOG_INFO, "disktimeout: %s is awake, starting timer", entry->device, entry->timeout); + + /* device is not in standby mode, start timer */ + if (entry->timeout_event == NULL) { + struct timeval tv = { .tv_sec = entry->timeout }; + entry->timeout_event = event_add_timeout(&tv, diskto_timeout_cb, (void *)entry); + } + entry->flags &= ~(F_STANDBY); + } + + } else { + if (!(entry->flags & F_STANDBY)) { + log_print(LOG_INFO, "disktimeout: %s standby, stopping timer", entry->device, entry->timeout); + + /* device is in standby, stop timer */ + if (entry->timeout_event != NULL) + event_remove_timeout(entry->timeout_event); + + entry->flags |= F_STANDBY; + } + } + } + + return 0; +} + +int disktimeout_exit(void) +{ + struct disk_entry *entry, *tmp; + + list_for_each_entry_safe(entry, tmp, &disk_list, list) { + if (entry->timeout_event != NULL) { + event_remove_timeout(entry->timeout_event); + entry->timeout_event = NULL; + } + + list_del(&entry->list); + free(entry); + } + + if (check_timeout != NULL) { + event_remove_timeout(check_timeout); + check_timeout = NULL; + } + return 0; +} + +int disktimeout_init(void) +{ + int diskcount = config_get_strtokens("disktimeout", "disk", ",", 2, diskto_add_disk, NULL); + if (diskcount <= 0) + return -1; + + int check_interval = 0; + config_get_int("disktimeout", "check_interval", &check_interval, 60); + + struct timeval tv = { .tv_sec = check_interval }; + check_timeout = event_add_timeout(&tv, diskto_check_cb, NULL); + + return 0; +} diff --git a/disktimeout.h b/disktimeout.h new file mode 100644 index 0000000..a6f26e2 --- /dev/null +++ b/disktimeout.h @@ -0,0 +1,7 @@ +#ifndef _DISKTIMEOUT_H_ +#define _DISKTIMEOUT_H_ + +int disktimeout_exit(void); +int disktimeout_init(void); + +#endif /* _DISKTIMEOUT_H_ */ diff --git a/event.c b/event.c index b5a3898..ffc4769 100644 --- a/event.c +++ b/event.c @@ -206,8 +206,8 @@ int event_loop(int (*pre_select_cb)(int *maxfd, void *readfds, void *writefds, s while (1) { /* default value if no application timeout is present */ struct timeval timeout = { - .tv_sec = 3600, - .tv_usec = 0, + .tv_sec = -1, + .tv_usec = -1, }; if (!list_empty(&event_timeout_list)) { @@ -277,7 +277,12 @@ int event_loop(int (*pre_select_cb)(int *maxfd, void *readfds, void *writefds, s if (pre_select_cb != NULL && pre_select_cb(&maxfd, (void *)&readfds, (void *)&writefds, &timeout, privdata) != 0) break; - int retval = select(maxfd, &readfds, &writefds, NULL, &timeout); + int retval; + if (timeout.tv_sec == -1 && timeout.tv_usec == -1) + retval = select(maxfd, &readfds, &writefds, NULL, NULL); + else + retval = select(maxfd, &readfds, &writefds, NULL, &timeout); + if (retval < 0 && errno == EINTR) { errno = 0; continue; diff --git a/lcd.c b/lcd.c index 8abf6d7..f173cb5 100644 --- a/lcd.c +++ b/lcd.c @@ -63,6 +63,26 @@ struct lcddev { void lcd_close(struct lcddev *dev) { + if (dev->reset_timeout) { + event_remove_timeout(dev->reset_timeout); + dev->reset_timeout = NULL; + } + + if (dev->backlight_timeout) { + event_remove_timeout(dev->backlight_timeout); + dev->backlight_timeout = NULL; + } + + if (dev->scroll_timeout) { + event_remove_timeout(dev->scroll_timeout); + dev->scroll_timeout = NULL; + } + + if (dev->read_event) { + event_remove_fd(dev->read_event); + dev->read_event = NULL; + } + if (dev->fakedevice_event == NULL) { tcsetattr(dev->fd, TCSANOW, &dev->oldtio); @@ -74,6 +94,7 @@ void lcd_close(struct lcddev *dev) } close(dev->fd); + free(dev); } static int lcd_open(struct lcddev *dev, const char *device) @@ -403,10 +424,10 @@ struct lcddev * lcd_init(const char *devicename, memset(dev, 0, sizeof(struct lcddev)); int retval; - if (devicename != NULL) { - retval = lcd_open(dev, devicename); - } else { + if (strncmp(devicename, "dummy", 5) == 0) { retval = lcd_fakedevice_open(dev); + } else { + retval = lcd_open(dev, devicename); } if (retval < 0) { diff --git a/qnapd.c b/qnapd.c index 39de58b..94e5751 100644 --- a/qnapd.c +++ b/qnapd.c @@ -28,6 +28,7 @@ #include #include "configfile.h" +#include "disktimeout.h" #include "event.h" #include "lcd.h" #include "logging.h" @@ -186,14 +187,25 @@ int main(int argc, char *argv[]) log_print(LOG_EVERYTIME, PROGNAME" started (pid:%d)", getpid()); while (1) { - struct lcddev *dev = lcd_init(NULL /*LCD_DEVICE*/, &button_callback, NULL); - if (dev == NULL) + + const char *lcddevice = config_get_string("global", "lcddevice", NULL); + struct lcddev *dev = NULL; + if (lcddevice != NULL) { + dev = lcd_init(lcddevice, &button_callback, NULL); + if (dev == NULL) + break; + } + + if (disktimeout_init() < 0) break; /* exited on restart / SIGUSR1 */ event_loop(check_restart, NULL, (void *)&restart_var); - lcd_close(dev); + disktimeout_exit(); + + if (dev != NULL) + lcd_close(dev); config_free(); diff --git a/qnapd.conf b/qnapd.conf index e69de29..820f16e 100644 --- a/qnapd.conf +++ b/qnapd.conf @@ -0,0 +1,15 @@ +[global] + +#lcddevice /dev/ttyS0 +#lcddevice dummy + +pidfile qnapd.pid +logfile qnapd.log + +[disktimeout] +check_interval 60 + +disk /dev/sda,7200 +disk /dev/sdb,7200 +disk /dev/sdc,7200 +disk /dev/sdd,7200 diff --git a/sgio.c b/sgio.c new file mode 100644 index 0000000..41f2ff6 --- /dev/null +++ b/sgio.c @@ -0,0 +1,492 @@ +/* sgio.c - by Mark Lord (C) 2007 -- freely distributable */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sgio.h" +//#include "hdparm.h" + +#include + +extern int verbose; +extern int prefer_ata12; + +/* + * Taskfile layout for SG_ATA_16 cdb: + * + * LBA48: + * cdb[ 3] = hob_feat + * cdb[ 5] = hob_nsect + * cdb[ 7] = hob_lbal + * cdb[ 9] = hob_lbam + * cdb[11] = hob_lbah + * + * LBA28/LBA48: + * cdb[ 4] = feat + * cdb[ 6] = nsect + * cdb[ 8] = lbal + * cdb[10] = lbam + * cdb[12] = lbah + * cdb[13] = device + * cdb[14] = command + * + * Taskfile layout for SG_ATA_12 cdb: + * + * cdb[ 3] = feat + * cdb[ 4] = nsect + * cdb[ 5] = lbal + * cdb[ 6] = lbam + * cdb[ 7] = lbah + * cdb[ 8] = device + * cdb[ 9] = command + * + * dxfer_direction choices: + * SG_DXFER_TO_DEV, SG_DXFER_FROM_DEV, SG_DXFER_NONE + */ + +static inline int needs_lba48 (__u8 ata_op, __u64 lba, unsigned int nsect) +{ + const __u64 lba28_limit = (1<<28) - 1; + + switch (ata_op) { + case ATA_OP_READ_PIO_EXT: + case ATA_OP_READ_DMA_EXT: + case ATA_OP_WRITE_PIO_EXT: + case ATA_OP_WRITE_DMA_EXT: + case ATA_OP_READ_VERIFY_EXT: + case ATA_OP_WRITE_UNC_EXT: + case ATA_OP_READ_NATIVE_MAX_EXT: + case ATA_OP_SET_MAX_EXT: + case ATA_OP_FLUSHCACHE_EXT: + return 1; + } + if (lba >= lba28_limit) + return 1; + if (nsect) { + if (nsect > 0xff) + return 1; + if ((lba + nsect - 1) >= lba28_limit) + return 1; + } + return 0; +} + +void tf_init (struct ata_tf *tf, __u8 ata_op, __u64 lba, unsigned int nsect) +{ + memset(tf, 0, sizeof(*tf)); + tf->command = ata_op; + tf->dev = ATA_USING_LBA; + tf->lob.lbal = lba; + tf->lob.lbam = lba >> 8; + tf->lob.lbah = lba >> 16; + tf->lob.nsect = nsect; + if (needs_lba48(ata_op, lba, nsect)) { + tf->is_lba48 = 1; + tf->hob.nsect = nsect >> 8; + tf->hob.lbal = lba >> 24; + tf->hob.lbam = lba >> 32; + tf->hob.lbah = lba >> 40; + } else { + tf->dev |= (lba >> 24) & 0x0f; + } +} + +#ifdef SG_IO + +__u64 tf_to_lba (struct ata_tf *tf) +{ + __u32 lba24, lbah; + __u64 lba64; + + lba24 = (tf->lob.lbah << 16) | (tf->lob.lbam << 8) | (tf->lob.lbal); + if (tf->is_lba48) + lbah = (tf->hob.lbah << 16) | (tf->hob.lbam << 8) | (tf->hob.lbal); + else + lbah = (tf->dev & 0x0f); + lba64 = (((__u64)lbah) << 24) | (__u64)lba24; + return lba64; +} + +enum { + SG_CDB2_TLEN_NODATA = 0 << 0, + SG_CDB2_TLEN_FEAT = 1 << 0, + SG_CDB2_TLEN_NSECT = 2 << 0, + + SG_CDB2_TLEN_BYTES = 0 << 2, + SG_CDB2_TLEN_SECTORS = 1 << 2, + + SG_CDB2_TDIR_TO_DEV = 0 << 3, + SG_CDB2_TDIR_FROM_DEV = 1 << 3, + + SG_CDB2_CHECK_COND = 1 << 5, +}; + +static void dump_bytes (const char *prefix, unsigned char *p, int len) +{ + int i; + + if (prefix) + fprintf(stderr, "%s: ", prefix); + for (i = 0; i < len; ++i) + fprintf(stderr, " %02x", p[i]); + fprintf(stderr, "\n"); +} + +int sg16 (int fd, int rw, int dma, struct ata_tf *tf, + void *data, unsigned int data_bytes, unsigned int timeout_secs) +{ + unsigned char cdb[SG_ATA_16_LEN]; + unsigned char sb[32], *desc; + struct scsi_sg_io_hdr io_hdr; + + memset(&cdb, 0, sizeof(cdb)); + memset(&sb, 0, sizeof(sb)); + memset(&io_hdr, 0, sizeof(struct scsi_sg_io_hdr)); + + if (dma) { + //cdb[1] = data ? (rw ? SG_ATA_PROTO_UDMA_OUT : SG_ATA_PROTO_UDMA_IN) : SG_ATA_PROTO_NON_DATA; + cdb[1] = data ? SG_ATA_PROTO_DMA : SG_ATA_PROTO_NON_DATA; + } else { + cdb[1] = data ? (rw ? SG_ATA_PROTO_PIO_OUT : SG_ATA_PROTO_PIO_IN) : SG_ATA_PROTO_NON_DATA; + } + cdb[ 2] = SG_CDB2_CHECK_COND; + if (data) { + cdb[2] |= SG_CDB2_TLEN_NSECT | SG_CDB2_TLEN_SECTORS; + cdb[2] |= rw ? SG_CDB2_TDIR_TO_DEV : SG_CDB2_TDIR_FROM_DEV; + } + + if (!prefer_ata12 || tf->is_lba48) { + cdb[ 0] = SG_ATA_16; + cdb[ 4] = tf->lob.feat; + cdb[ 6] = tf->lob.nsect; + cdb[ 8] = tf->lob.lbal; + cdb[10] = tf->lob.lbam; + cdb[12] = tf->lob.lbah; + cdb[13] = tf->dev; + cdb[14] = tf->command; + if (tf->is_lba48) { + cdb[ 1] |= SG_ATA_LBA48; + cdb[ 3] = tf->hob.feat; + cdb[ 5] = tf->hob.nsect; + cdb[ 7] = tf->hob.lbal; + cdb[ 9] = tf->hob.lbam; + cdb[11] = tf->hob.lbah; + } + io_hdr.cmd_len = SG_ATA_16_LEN; + } else { + cdb[ 0] = SG_ATA_12; + cdb[ 3] = tf->lob.feat; + cdb[ 4] = tf->lob.nsect; + cdb[ 5] = tf->lob.lbal; + cdb[ 6] = tf->lob.lbam; + cdb[ 7] = tf->lob.lbah; + cdb[ 8] = tf->dev; + cdb[ 9] = tf->command; + io_hdr.cmd_len = SG_ATA_12_LEN; + } + + io_hdr.interface_id = 'S'; + io_hdr.mx_sb_len = sizeof(sb); + io_hdr.dxfer_direction = data ? (rw ? SG_DXFER_TO_DEV : SG_DXFER_FROM_DEV) : SG_DXFER_NONE; + io_hdr.dxfer_len = data ? data_bytes : 0; + io_hdr.dxferp = data; + io_hdr.cmdp = cdb; + io_hdr.sbp = sb; + io_hdr.pack_id = tf_to_lba(tf); + io_hdr.timeout = (timeout_secs ? timeout_secs : 5) * 1000; /* msecs */ + + if (verbose) + dump_bytes("outgoing cdb", cdb, sizeof(cdb)); + if (ioctl(fd, SG_IO, &io_hdr) == -1) { + if (verbose) + perror("ioctl(fd,SG_IO)"); + return -1; /* SG_IO not supported */ + } + if (verbose) + fprintf(stderr, "SG_IO: ATA_%u status=0x%x, host_status=0x%x, driver_status=0x%x\n", + io_hdr.cmd_len, io_hdr.status, io_hdr.host_status, io_hdr.driver_status); + + if (io_hdr.host_status || io_hdr.driver_status != SG_DRIVER_SENSE + || (io_hdr.status && io_hdr.status != SG_CHECK_CONDITION)) + { + if (verbose) + fprintf(stderr, "SG_IO: bad response (not CHECK_CONDITION)\n"); + errno = EBADE; + return -1; + } + + desc = sb + 8; + if (sb[0] != 0x72 || sb[7] < 14 || desc[0] != 0x09 || desc[1] < 0x0c) { + if (verbose) + dump_bytes("SG_IO: bad/missing sense data, sb[]", sb, sizeof(sb)); + errno = EBADE; + return -1; + } + + if (verbose) + dump_bytes("SG_IO: sb[]", sb, sizeof(sb)); + + if (verbose) { + int len = desc[1], maxlen = sizeof(sb) - 8 - 2; + if (len > maxlen) + len = maxlen; + dump_bytes("SG_IO: desc[]", desc, len); + } + + tf->is_lba48 = desc[ 2] & 1; + tf->error = desc[ 3]; + tf->lob.nsect = desc[ 5]; + tf->lob.lbal = desc[ 7]; + tf->lob.lbam = desc[ 9]; + tf->lob.lbah = desc[11]; + tf->dev = desc[12]; + tf->status = desc[13]; + tf->hob.feat = 0; + if (tf->is_lba48) { + tf->hob.nsect = desc[ 4]; + tf->hob.lbal = desc[ 6]; + tf->hob.lbam = desc[ 8]; + tf->hob.lbah = desc[10]; + } else { + tf->hob.nsect = 0; + tf->hob.lbal = 0; + tf->hob.lbam = 0; + tf->hob.lbah = 0; + } + + if (verbose) + fprintf(stderr, " ATA_%u stat=%02x err=%02x nsect=%02x lbal=%02x lbam=%02x lbah=%02x dev=%02x\n", + io_hdr.cmd_len, tf->status, tf->error, tf->lob.nsect, tf->lob.lbal, tf->lob.lbam, tf->lob.lbah, tf->dev); + + if (tf->status & (ATA_STAT_ERR | ATA_STAT_DRQ)) { + if (verbose) { + fprintf(stderr, "I/O error, ata_op=0x%02x ata_status=0x%02x ata_error=0x%02x\n", + tf->command, tf->status, tf->error); + } + errno = EIO; + return -1; + } + return 0; +} + +#endif /* SG_IO */ + +int do_drive_cmd (int fd, unsigned char *args) +{ +#ifdef SG_IO + + struct ata_tf tf; + void *data = NULL; + unsigned int data_bytes = 0; + int rc; + + if (args == NULL) + goto use_legacy_ioctl; + /* + * Reformat and try to issue via SG_IO: + */ + if (args[3]) { + data_bytes = args[3] * 512; + data = args + 4; + } + tf_init(&tf, args[0], 0, args[1]); + tf.lob.feat = args[2]; + if (tf.command == ATA_OP_SMART) { + tf.lob.nsect = args[3]; + tf.lob.lbal = args[1]; + tf.lob.lbam = 0x4f; + tf.lob.lbah = 0xc2; + } + + rc = sg16(fd, SG_READ, SG_PIO, &tf, data, data_bytes, 0); + if (rc == -1) { + if (errno == EINVAL || errno == ENODEV) + goto use_legacy_ioctl; + } + + if (rc == 0 || errno == EIO) { + args[0] = tf.status; + args[1] = tf.error; + args[2] = tf.lob.nsect; + } + return rc; + +use_legacy_ioctl: +#endif /* SG_IO */ + if (verbose) + fprintf(stderr, "Trying legacy HDIO_DRIVE_CMD\n"); + return ioctl(fd, HDIO_DRIVE_CMD, args); +} + +int do_taskfile_cmd (int fd, struct hdio_taskfile *r, unsigned int timeout_secs) +{ + int rc; +#ifdef SG_IO + struct ata_tf tf; + void *data = NULL; + unsigned int data_bytes = 0; + int rw = SG_READ; + /* + * Reformat and try to issue via SG_IO: + */ + tf_init(&tf, 0, 0, 0); +#if 1 /* debugging */ + if (verbose) { + printf("oflags.lob_all=0x%02x, flags={", r->oflags.lob_all); + if (r->oflags.lob.feat) printf(" feat"); + if (r->oflags.lob.lbal) printf(" lbal"); + if (r->oflags.lob.nsect)printf(" nsect"); + if (r->oflags.lob.lbam) printf(" lbam"); + if (r->oflags.lob.lbah) printf(" lbah"); + if (r->oflags.lob.dev) printf(" dev"); + if (r->oflags.lob.command) printf(" command"); + printf(" }\n"); + printf("oflags.hob_all=0x%02x, flags={", r->oflags.hob_all); + if (r->oflags.hob.feat) printf(" feat"); + if (r->oflags.hob.lbal) printf(" lbal"); + if (r->oflags.hob.nsect)printf(" nsect"); + if (r->oflags.hob.lbam) printf(" lbam"); + if (r->oflags.hob.lbah) printf(" lbah"); + printf(" }\n"); + } +#endif + if (r->oflags.lob.feat) tf.lob.feat = r->lob.feat; + if (r->oflags.lob.lbal) tf.lob.lbal = r->lob.lbal; + if (r->oflags.lob.nsect) tf.lob.nsect = r->lob.nsect; + if (r->oflags.lob.lbam) tf.lob.lbam = r->lob.lbam; + if (r->oflags.lob.lbah) tf.lob.lbah = r->lob.lbah; + if (r->oflags.lob.dev) tf.dev = r->lob.dev; + if (r->oflags.lob.command) tf.command = r->lob.command; + if (r->oflags.hob_all || r->iflags.hob_all) { + tf.is_lba48 = 1; + if (r->oflags.hob.feat) tf.hob.feat = r->hob.feat; + if (r->oflags.hob.lbal) tf.hob.lbal = r->hob.lbal; + if (r->oflags.hob.nsect)tf.hob.nsect = r->hob.nsect; + if (r->oflags.hob.lbam) tf.hob.lbam = r->hob.lbam; + if (r->oflags.hob.lbah) tf.hob.lbah = r->hob.lbah; + if (verbose) + fprintf(stderr, "using LBA48 taskfile\n"); + } + switch (r->cmd_req) { + case TASKFILE_CMD_REQ_OUT: + case TASKFILE_CMD_REQ_RAW_OUT: + data_bytes = r->obytes; + data = r->data; + rw = SG_WRITE; + break; + case TASKFILE_CMD_REQ_IN: + data_bytes = r->ibytes; + data = r->data; + break; + } + + rc = sg16(fd, rw, SG_PIO, &tf, data, data_bytes, timeout_secs); + if (rc == -1) { + if (errno == EINVAL || errno == ENODEV) + goto use_legacy_ioctl; + } + + if (rc == 0 || errno == EIO) { + if (r->iflags.lob.feat) r->lob.feat = tf.error; + if (r->iflags.lob.lbal) r->lob.lbal = tf.lob.lbal; + if (r->iflags.lob.nsect) r->lob.nsect = tf.lob.nsect; + if (r->iflags.lob.lbam) r->lob.lbam = tf.lob.lbam; + if (r->iflags.lob.lbah) r->lob.lbah = tf.lob.lbah; + if (r->iflags.lob.dev) r->lob.dev = tf.dev; + if (r->iflags.lob.command) r->lob.command = tf.status; + if (r->iflags.hob.feat) r->hob.feat = tf.hob.feat; + if (r->iflags.hob.lbal) r->hob.lbal = tf.hob.lbal; + if (r->iflags.hob.nsect) r->hob.nsect = tf.hob.nsect; + if (r->iflags.hob.lbam) r->hob.lbam = tf.hob.lbam; + if (r->iflags.hob.lbah) r->hob.lbah = tf.hob.lbah; + } + return rc; + +use_legacy_ioctl: +#else + timeout_secs = 0; /* keep compiler happy */ +#endif /* SG_IO */ + if (verbose) + fprintf(stderr, "trying legacy HDIO_DRIVE_TASKFILE\n"); + errno = 0; + rc = ioctl(fd, HDIO_DRIVE_TASKFILE, r); + if (verbose) { + int err = errno; + fprintf(stderr, "rc=%d, errno=%d, returned ATA registers: ", rc, err); + if (r->iflags.lob.feat) fprintf(stderr, " er=%02x", r->lob.feat); + if (r->iflags.lob.nsect) fprintf(stderr, " ns=%02x", r->lob.nsect); + if (r->iflags.lob.lbal) fprintf(stderr, " ll=%02x", r->lob.lbal); + if (r->iflags.lob.lbam) fprintf(stderr, " lm=%02x", r->lob.lbam); + if (r->iflags.lob.lbah) fprintf(stderr, " lh=%02x", r->lob.lbah); + if (r->iflags.lob.dev) fprintf(stderr, " dh=%02x", r->lob.dev); + if (r->iflags.lob.command) fprintf(stderr, " st=%02x", r->lob.command); + if (r->iflags.hob.feat) fprintf(stderr, " err=%02x", r->hob.feat); + if (r->iflags.hob.nsect) fprintf(stderr, " err=%02x", r->hob.nsect); + if (r->iflags.hob.lbal) fprintf(stderr, " err=%02x", r->hob.lbal); + if (r->iflags.hob.lbam) fprintf(stderr, " err=%02x", r->hob.lbam); + if (r->iflags.hob.lbah) fprintf(stderr, " err=%02x", r->hob.lbah); + fprintf(stderr, "\n"); + errno = err; + } + if (rc == -1 && errno == EINVAL) { + fprintf(stderr, "The running kernel lacks CONFIG_IDE_TASK_IOCTL support for this device.\n"); + errno = EINVAL; + } + return rc; +} + +void init_hdio_taskfile (struct hdio_taskfile *r, __u8 ata_op, int rw, int force_lba48, + __u64 lba, unsigned int nsect, int data_bytes) +{ + memset(r, 0, sizeof(struct hdio_taskfile) + data_bytes); + if (!data_bytes) { + r->dphase = TASKFILE_DPHASE_NONE; + r->cmd_req = TASKFILE_CMD_REQ_NODATA; + } else if (rw == RW_WRITE) { + r->dphase = TASKFILE_DPHASE_PIO_OUT; + r->cmd_req = TASKFILE_CMD_REQ_RAW_OUT; + r->obytes = data_bytes; + } else { /* rw == RW_READ */ + r->dphase = TASKFILE_DPHASE_PIO_IN; + r->cmd_req = TASKFILE_CMD_REQ_IN; + r->ibytes = data_bytes; + } + r->lob.command = ata_op; + r->oflags.lob.command = 1; + r->oflags.lob.dev = 1; + r->oflags.lob.lbal = 1; + r->oflags.lob.lbam = 1; + r->oflags.lob.lbah = 1; + r->oflags.lob.nsect = 1; + + r->iflags.lob.command = 1; + r->iflags.lob.feat = 1; + + r->lob.nsect = nsect; + r->lob.lbal = lba; + r->lob.lbam = lba >> 8; + r->lob.lbah = lba >> 16; + r->lob.dev = 0xa0 | ATA_USING_LBA; + + if (needs_lba48(ata_op, lba, nsect) || force_lba48) { + r->hob.nsect = nsect >> 8; + r->hob.lbal = lba >> 24; + r->hob.lbam = lba >> 32; + r->hob.lbah = lba >> 40; + r->oflags.hob.nsect = 1; + r->oflags.hob.lbal = 1; + r->oflags.hob.lbam = 1; + r->oflags.hob.lbah = 1; + } else { + r->lob.dev |= (lba >> 24) & 0x0f; + } +} diff --git a/sgio.h b/sgio.h new file mode 100644 index 0000000..7ad2d11 --- /dev/null +++ b/sgio.h @@ -0,0 +1,230 @@ +/* prototypes and stuff for ATA command ioctls */ + +#include + +enum { + ATA_OP_READ_PIO = 0x20, + ATA_OP_READ_PIO_ONCE = 0x21, + ATA_OP_READ_LONG = 0x22, + ATA_OP_READ_LONG_ONCE = 0x23, + ATA_OP_READ_PIO_EXT = 0x24, + ATA_OP_READ_DMA_EXT = 0x25, + ATA_OP_READ_FPDMA = 0x60, // NCQ + ATA_OP_WRITE_PIO = 0x30, + ATA_OP_WRITE_LONG = 0x32, + ATA_OP_WRITE_LONG_ONCE = 0x33, + ATA_OP_WRITE_PIO_EXT = 0x34, + ATA_OP_WRITE_DMA_EXT = 0x35, + ATA_OP_WRITE_FPDMA = 0x61, // NCQ + ATA_OP_READ_VERIFY = 0x40, + ATA_OP_READ_VERIFY_ONCE = 0x41, + ATA_OP_READ_VERIFY_EXT = 0x42, + ATA_OP_WRITE_UNC_EXT = 0x45, // lba48, no data, uses feat reg + ATA_OP_FORMAT_TRACK = 0x50, + ATA_OP_DOWNLOAD_MICROCODE = 0x92, + ATA_OP_STANDBYNOW2 = 0x94, + ATA_OP_CHECKPOWERMODE2 = 0x98, + ATA_OP_SLEEPNOW2 = 0x99, + ATA_OP_PIDENTIFY = 0xa1, + ATA_OP_READ_NATIVE_MAX = 0xf8, + ATA_OP_READ_NATIVE_MAX_EXT = 0x27, + ATA_OP_SMART = 0xb0, + ATA_OP_DCO = 0xb1, + ATA_OP_ERASE_SECTORS = 0xc0, + ATA_OP_READ_DMA = 0xc8, + ATA_OP_WRITE_DMA = 0xca, + ATA_OP_DOORLOCK = 0xde, + ATA_OP_DOORUNLOCK = 0xdf, + ATA_OP_STANDBYNOW1 = 0xe0, + ATA_OP_IDLEIMMEDIATE = 0xe1, + ATA_OP_SETIDLE = 0xe3, + ATA_OP_SET_MAX = 0xf9, + ATA_OP_SET_MAX_EXT = 0x37, + ATA_OP_SET_MULTIPLE = 0xc6, + ATA_OP_CHECKPOWERMODE1 = 0xe5, + ATA_OP_SLEEPNOW1 = 0xe6, + ATA_OP_FLUSHCACHE = 0xe7, + ATA_OP_FLUSHCACHE_EXT = 0xea, + ATA_OP_IDENTIFY = 0xec, + ATA_OP_SETFEATURES = 0xef, + ATA_OP_SECURITY_SET_PASS = 0xf1, + ATA_OP_SECURITY_UNLOCK = 0xf2, + ATA_OP_SECURITY_ERASE_PREPARE = 0xf3, + ATA_OP_SECURITY_ERASE_UNIT = 0xf4, + ATA_OP_SECURITY_FREEZE_LOCK = 0xf5, + ATA_OP_SECURITY_DISABLE = 0xf6, +}; + +/* + * Some useful ATA register bits + */ +enum { + ATA_USING_LBA = (1 << 6), + ATA_STAT_DRQ = (1 << 3), + ATA_STAT_ERR = (1 << 0), +}; + +/* + * Useful parameters for init_hdio_taskfile(): + */ +enum { RW_READ = 0, + RW_WRITE = 1, + LBA28_OK = 0, + LBA48_FORCE = 1, +}; + +/* + * Definitions and structures for use with SG_IO + ATA_16: + */ +struct ata_lba_regs { + __u8 feat; + __u8 nsect; + __u8 lbal; + __u8 lbam; + __u8 lbah; +}; +struct ata_tf { + __u8 dev; + __u8 command; + __u8 error; + __u8 status; + __u8 is_lba48; + struct ata_lba_regs lob; + struct ata_lba_regs hob; +}; + +/* + * Definitions and structures for use with HDIO_DRIVE_TASKFILE: + */ + +enum { + /* + * These (redundantly) specify the category of the request + */ + TASKFILE_CMD_REQ_NODATA = 0, /* ide: IDE_DRIVE_TASK_NO_DATA */ + TASKFILE_CMD_REQ_IN = 2, /* ide: IDE_DRIVE_TASK_IN */ + TASKFILE_CMD_REQ_OUT = 3, /* ide: IDE_DRIVE_TASK_OUT */ + TASKFILE_CMD_REQ_RAW_OUT= 4, /* ide: IDE_DRIVE_TASK_RAW_WRITE */ + /* + * These specify the method of transfer (pio, dma, multi, ..) + */ + TASKFILE_DPHASE_NONE = 0, /* ide: TASKFILE_IN */ + TASKFILE_DPHASE_PIO_IN = 1, /* ide: TASKFILE_IN */ + TASKFILE_DPHASE_PIO_OUT = 4, /* ide: TASKFILE_OUT */ +}; + +struct reg_flags { + union { + unsigned lob_all : 8; + struct { + unsigned data : 1; + unsigned feat : 1; + unsigned lbal : 1; + unsigned nsect : 1; + unsigned lbam : 1; + unsigned lbah : 1; + unsigned dev : 1; + unsigned command : 1; + } lob; + }; + union { + unsigned hob_all : 8; + struct { + unsigned data : 1; + unsigned feat : 1; + unsigned lbal : 1; + unsigned nsect : 1; + unsigned lbam : 1; + unsigned lbah : 1; + unsigned dev : 1; + unsigned command : 1; + } hob; + }; +}; + +struct taskfile_regs { + __u8 data; + __u8 feat; + __u8 nsect; + __u8 lbal; + __u8 lbam; + __u8 lbah; + __u8 dev; + __u8 command; +}; + +struct hdio_taskfile { + struct taskfile_regs lob; + struct taskfile_regs hob; + struct reg_flags oflags; + struct reg_flags iflags; + int dphase; + int cmd_req; /* IDE command_type */ + unsigned long obytes; + unsigned long ibytes; + __u16 data[0]; +}; + +struct scsi_sg_io_hdr { + int interface_id; + int dxfer_direction; + unsigned char cmd_len; + unsigned char mx_sb_len; + unsigned short iovec_count; + unsigned int dxfer_len; + void * dxferp; + unsigned char * cmdp; + void * sbp; + unsigned int timeout; + unsigned int flags; + int pack_id; + void * usr_ptr; + unsigned char status; + unsigned char masked_status; + unsigned char msg_status; + unsigned char sb_len_wr; + unsigned short host_status; + unsigned short driver_status; + int resid; + unsigned int duration; + unsigned int info; +}; + +#ifndef SG_DXFER_NONE + #define SG_DXFER_NONE -1 + #define SG_DXFER_TO_DEV -2 + #define SG_DXFER_FROM_DEV -3 + #define SG_DXFER_TO_FROM_DEV -4 +#endif + +#define SG_READ 0 +#define SG_WRITE 1 + +#define SG_PIO 0 +#define SG_DMA 1 + +#define SG_CHECK_CONDITION 0x02 +#define SG_DRIVER_SENSE 0x08 + +#define SG_ATA_16 0x85 +#define SG_ATA_16_LEN 16 + +#define SG_ATA_12 0xa1 +#define SG_ATA_12_LEN 12 + +#define SG_ATA_LBA48 1 +#define SG_ATA_PROTO_NON_DATA ( 3 << 1) +#define SG_ATA_PROTO_PIO_IN ( 4 << 1) +#define SG_ATA_PROTO_PIO_OUT ( 5 << 1) +#define SG_ATA_PROTO_DMA ( 6 << 1) +#define SG_ATA_PROTO_UDMA_IN (11 << 1) /* not yet supported in libata */ +#define SG_ATA_PROTO_UDMA_OUT (12 << 1) /* not yet supported in libata */ + +void tf_init (struct ata_tf *tf, __u8 ata_op, __u64 lba, unsigned int nsect); +__u64 tf_to_lba (struct ata_tf *tf); +int sg16 (int fd, int rw, int dma, struct ata_tf *tf, void *data, unsigned int data_bytes, unsigned int timeout_secs); +int do_drive_cmd (int fd, unsigned char *args); +int do_taskfile_cmd (int fd, struct hdio_taskfile *r, unsigned int timeout_secs); +int dev_has_sgio (int fd); +void init_hdio_taskfile (struct hdio_taskfile *r, __u8 ata_op, int rw, int force_lba48, + __u64 lba, unsigned int nsect, int data_bytes);