diff --git a/hw/atapi-pt.c b/hw/atapi-pt.c
-index 92f2934..d38d536 100644
+index 92f2934..6a7fab1 100644
--- a/hw/atapi-pt.c
+++ b/hw/atapi-pt.c
-@@ -1,19 +1,30 @@
+@@ -1,29 +1,43 @@
+#include <scsi/sg.h>
#include <utime.h>
+#include <time.h>
#ifdef DEBUG_IDE_ATAPI_PT
-# define DEBUG_PRINTF(Args...) printf(Args)
+# define DEBUG_PRINTF(Args...) atapi_dprintf(Args)
++# define DEBUG_HEXDUMP(addr, count) atapi_dhexdump(addr, count)
# define CHECK_SAME_VALUE(Val1, Val2) \
do { \
- if ((Val1) != (Val2)) \
} while (0)
#else
# define DEBUG_PRINTF(Args...)
-@@ -21,9 +32,10 @@
++# define DEBUG_HEXDUMP(addr, count)
+ # define CHECK_SAME_VALUE(Val1, Val2)
#endif /* DEBUG_IDE_ATAPI_PT */
/* The generic packet command opcodes for CD/DVD Logical Units,
* From Table 57 of the SFF8090 Ver. 3 (Mt. Fuji) draft standard. */
-@@ -482,6 +494,40 @@ static const char *atapi_sense_to_str(int key, int asc, int ascq)
+@@ -482,6 +496,66 @@ static const char *atapi_sense_to_str(int key, int asc, int ascq)
}
+ struct timeval tv;
+ va_list args;
+ int l;
++ static int sol = 1;
+ if (debug_enabled == 0)
+ return;
+ if (debug_enabled < 0) {
+ sprintf(debugbuf, IDE_ATAPI_PT_DEBUG_FILE_TEMPLATE, domid);
+ debug_fd=open(debugbuf, O_WRONLY | O_CREAT | O_APPEND, 0666);
+ }
-+ gettimeofday(&tv, NULL);
-+ l = snprintf(debugbuf, sizeof(debugbuf)-1, "[%02d:%02d:%02d.%03ld] ",
-+ tmp->tm_hour, tmp->tm_min, tmp->tm_sec, tv.tv_usec/1000);
++ l = 0;
++ if (sol) {
++ gettimeofday(&tv, NULL);
++ l = snprintf(debugbuf, sizeof(debugbuf)-1, "[%02d:%02d:%02d.%03ld] ",
++ tmp->tm_hour, tmp->tm_min, tmp->tm_sec, tv.tv_usec/1000);
++ }
+ va_start(args, fmt);
-+ vsnprintf(debugbuf+l, sizeof(debugbuf)-1-l, fmt, args);
++ l += vsnprintf(debugbuf+l, sizeof(debugbuf)-1-l, fmt, args);
+ va_end(args);
++ if ((l>0) && debugbuf[l-1] == '\n') {
++ sol = 1;
++ } else {
++ sol = 0;
++ }
+ debugbuf[sizeof(debugbuf)-1] = '\0';
+ write(debug_fd, debugbuf, strlen(debugbuf));
+}
+
++static void atapi_dhexdump(const void* address, uint32_t len)
++{
++ const unsigned char* p = address;
++ int i, j;
++
++ for (i = 0; i < len; i += 16) {
++ for (j = 0; j < 16 && i + j < len; j++)
++ atapi_dprintf("%02x ", p[i + j]);
++ for (; j < 16; j++)
++ atapi_dprintf(" ");
++ atapi_dprintf(" ");
++ for (j = 0; j < 16 && i + j < len; j++)
++ atapi_dprintf("%c", (p[i + j] < ' ' || p[i + j] > 0x7f) ? '.' : p[i + j]);
++ atapi_dprintf("\n");
++ }
++}
++
+
/* For each SCSI command we need to know up to 3 data sizes. These are:
* 1. The amount of data to send to the LU.
* 2. The size of the buffer provided for data sent back from the LU.
-@@ -676,14 +722,58 @@ static void ide_atapi_pt_error(IDEState *s)
+@@ -676,14 +750,58 @@ static void ide_atapi_pt_error(IDEState *s)
ide_set_irq(s);
}
+{
+ int fds[2];
+
-+ DEBUG_PRINTF("%s\n", __FUNCTION__);
++ //DEBUG_PRINTF("%s\n", __FUNCTION__);
+ if (pipe(fds) < 0) {
+ fprintf(stderr, "atapi-pt failed to create pipe: %m\n");
+ exit(1);
+/* Call with ide_atapi_sgio_mutex held */
+static void ide_atapi_pt_do_sg_io(IDEState *s, int timeout)
+{
-+ DEBUG_PRINTF("%s\n", __FUNCTION__);
++ //DEBUG_PRINTF("%s\n", __FUNCTION__);
+ struct sg_io_v4 *cmd = &s->atapi_pt.cmd;
assert(cmd->din_xfer_len != (__u32)-1);
s->atapi_pt.sense.error_code = 0;
-@@ -691,9 +781,27 @@ static void ide_atapi_pt_do_sg_io(IDEState *s)
+@@ -691,9 +809,41 @@ static void ide_atapi_pt_do_sg_io(IDEState *s)
s->atapi_pt.sense.asc = 0;
s->atapi_pt.sense.ascq = 0;
+ cmd->timeout = timeout ? timeout : 15000;
+
++ if ((s->atapi_pt.request[0] == GPCMD_REPORT_KEY) || (s->atapi_pt.request[0] == GPCMD_SEND_KEY)) {
++ if (cmd->dout_xfer_len > 0) {
++ DEBUG_PRINTF("write:\n");
++ DEBUG_HEXDUMP(cmd->dout_xferp, cmd->dout_xfer_len);
++ }
++ }
++
+ /* Poke worker thread to send command using SG_IO ioctl */
+ pthread_cond_signal(&s->atapi_pt.sgio_cv);
+ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
+ uint8_t cmd_code = s->atapi_pt.request[0];
+ uint32_t din_desired;
+
-+ DEBUG_PRINTF("%s\n", __FUNCTION__);
++ //DEBUG_PRINTF("%s\n", __FUNCTION__);
+ assert(s);
- /* Send command and wait for reply, SG_IO ioctl*/
- r = ioctl(raw_state->fd, 0x2285, cmd);
+ /* Get return code of ioctl from worker thread's fd */
+ read(s->atapi_pt.sgio_rfd, &r, sizeof(int));
++
++ if ((s->atapi_pt.request[0] == GPCMD_REPORT_KEY) || (s->atapi_pt.request[0] == GPCMD_SEND_KEY)) {
++ if (cmd->din_xfer_len > 0) {
++ DEBUG_PRINTF("read:\n");
++ DEBUG_HEXDUMP(cmd->din_xferp, cmd->din_xfer_len);
++ }
++ }
if(s->atapi_pt.request[0] == GPCMD_GET_EVENT_STATUS_NOTIFICATION)
{
-@@ -746,16 +854,18 @@ static void ide_atapi_pt_do_sg_io(IDEState *s)
+@@ -746,16 +896,18 @@ static void ide_atapi_pt_do_sg_io(IDEState *s)
if(stat(IDE_ATAPI_PT_NEW_CD_FILE, &file_stat) == 0)
{
else if(stat(IDE_ATAPI_PT_EJECT_CD_FILE, &file_stat) == 0 &&
s->atapi_pt.eject_time < file_stat.st_ctime)
{
-@@ -832,16 +942,17 @@ static void ide_atapi_pt_do_sg_io(IDEState *s)
+@@ -832,16 +984,17 @@ static void ide_atapi_pt_do_sg_io(IDEState *s)
if(din_desired == (__u32)-1)
din_desired = cmd->din_xfer_len;
static void ide_atapi_pt_dout_fetch_dma_done(void *opaque, int ret)
{
BMDMAState *bm = opaque;
-@@ -850,13 +961,15 @@ static void ide_atapi_pt_dout_fetch_dma_done(void *opaque, int ret)
+@@ -850,13 +1003,15 @@ static void ide_atapi_pt_dout_fetch_dma_done(void *opaque, int ret)
if (ret < 0) {
ide_atapi_io_error(s, ret);
static void ide_atapi_pt_wcmd(IDEState *s)
{
if (s->atapi_dma)
-@@ -1005,11 +1118,20 @@ static int ide_atapi_pt_read_cd_block_size(const uint8_t *io_buffer)
+@@ -1005,11 +1160,20 @@ static int ide_atapi_pt_read_cd_block_size(const uint8_t *io_buffer)
return block_size;
}
uint8_t cmd_code;
+
-+ DEBUG_PRINTF("%s (before mutex)\n", __FUNCTION__);
++ //DEBUG_PRINTF("%s (before mutex)\n", __FUNCTION__);
+ if (pthread_mutex_trylock(&s->atapi_pt.sgio_mutex)) {
+ fprintf(stderr, "ide_atapi_pt_cmd() called with existing request processing - ignored!\n");
+ return;
memset(cmd, 0, sizeof(*cmd));
memcpy(s->atapi_pt.request, s->io_buffer, ATAPI_PACKET_SIZE);
cmd_code = s->atapi_pt.request[0];
-@@ -1021,6 +1143,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
+@@ -1021,6 +1185,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
cmd->response = (__u64)&s->atapi_pt.sense;
cmd->max_response_len = sizeof(s->atapi_pt.sense);
cmd->timeout = 15000; // 15 seconds
s->status |= BUSY_STAT;
-@@ -1054,14 +1177,15 @@ static void ide_atapi_pt_cmd(IDEState *s)
+@@ -1043,6 +1208,13 @@ static void ide_atapi_pt_cmd(IDEState *s)
+ atapi_data_sizes[cmd_code].alloc_block_size);
+
+
++ if ((cmd_code == GPCMD_REPORT_KEY) || (cmd_code == GPCMD_SEND_KEY)) {
++ DEBUG_PRINTF("Command dump:\n");
++ DEBUG_HEXDUMP(s->atapi_pt.request, 11);
++ DEBUG_PRINTF("dout_xfer_len: %d din_xfer_len: %d\n",
++ cmd->dout_xfer_len, cmd->din_xfer_len);
++ }
++
+ /* A few commands need special attention */
+ switch(cmd_code)
+ {
+@@ -1054,14 +1226,15 @@ static void ide_atapi_pt_cmd(IDEState *s)
ide_atapi_pt_set_error(s, SENSE_ILLEGAL_REQUEST,
ASC_INV_FIELD_IN_CMD_PACKET, 0x70);
break;
case GPCMD_WRITE_BUFFER:
-@@ -1072,6 +1196,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
+@@ -1072,6 +1245,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
s->io_buffer[1] & 7);
ide_atapi_pt_set_error(s, SENSE_ILLEGAL_REQUEST,
ASC_ILLEGAL_OPCODE, 0x70);
return;
}
-@@ -1091,6 +1216,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
+@@ -1091,6 +1265,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
int size = 8 + s->atapi_pt.sense.add_sense_len;
memcpy(s->io_buffer, &s->atapi_pt.sense, sizeof (s->atapi_pt.sense));
ide_atapi_cmd_reply(s, size, max_size);
return;
}
-@@ -1152,6 +1278,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
+@@ -1152,6 +1327,7 @@ static void ide_atapi_pt_cmd(IDEState *s)
s->io_buffer[10]);
ide_atapi_pt_set_error(s, SENSE_ILLEGAL_REQUEST,
ASC_ILLEGAL_OPCODE, 0x70);
return;
}
break;
-@@ -1169,5 +1296,5 @@ static void ide_atapi_pt_cmd(IDEState *s)
+@@ -1160,7 +1336,10 @@ static void ide_atapi_pt_cmd(IDEState *s)
+ if(cmd->dout_xfer_len == (__u32)-1)
+ {
+ DEBUG_PRINTF("[UNHANDLED SCSI COMMAND] 0x%02x\n", cmd_code);
+- exit(1);
++ ide_atapi_pt_set_error(s, SENSE_ILLEGAL_REQUEST,
++ ASC_ILLEGAL_OPCODE, 0x70);
++ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
++ return;
+ }
+
+ if(cmd->dout_xfer_len > 0)
+@@ -1169,5 +1348,5 @@ static void ide_atapi_pt_cmd(IDEState *s)
return;
}
projects / xenclient / ioemu-pq.git / commitdiff