--- /dev/null
+diff --git a/hw/atapi-pt.c b/hw/atapi-pt.c
+index 92f2934..c2a24ec 100644
+--- a/hw/atapi-pt.c
++++ b/hw/atapi-pt.c
+@@ -1,19 +1,25 @@
++#include <scsi/sg.h>
+ #include <utime.h>
+ #include <sys/stat.h>
+ #include <fcntl.h>
++#include <pthread.h>
++#include <sys/ioctl.h>
+
+ #undef DEBUG_IDE_ATAPI_PT
+
+ #define MSF_TO_FRAMES(M, S, F) (((M) * CD_SECS + (S)) * CD_FRAMES + (F))
++static int log_fd=-1;
+
+ #ifdef DEBUG_IDE_ATAPI_PT
+-# define DEBUG_PRINTF(Args...) printf(Args)
++# define DEBUG_PRINTF(Args...) do { char buf[1024]; int len; if (log_fd==-1) log_fd=open("/tmp/cdrom.log",O_WRONLY | O_CREAT | O_APPEND,0666); len=sprintf(buf,Args); write(log_fd,buf,len); } while (1==0)
++//#undef fprintf
++//# define DEBUG_PRINTF(Args...) fprintf(stderr, Args)
+ # define CHECK_SAME_VALUE(Val1, Val2) \
+ do { \
+- if ((Val1) != (Val2)) \
+- printf("[\e[1;32m!VALUE\e[m] %s:%d, %s=%d %s=%d\n", \
+- __PRETTY_FUNCTION__, __LINE__, #Val1, (Val1), \
+- #Val2, (Val2)); \
++ if ((Val1) != (Val2)) \
++ printf("[\e[1;32m!VALUE\e[m] %s:%d, %s=%d %s=%d\n", \
++ __PRETTY_FUNCTION__, __LINE__, #Val1, (Val1), \
++ #Val2, (Val2)); \
+ } while (0)
+ #else
+ # define DEBUG_PRINTF(Args...)
+@@ -676,14 +682,58 @@ static void ide_atapi_pt_error(IDEState *s)
+ ide_set_irq(s);
+ }
+
+-static void ide_atapi_pt_do_sg_io(IDEState *s)
++static void *ide_atapi_pt_sgio_worker_thread(void *arg)
+ {
+- struct sg_io_v4 *cmd = &s->atapi_pt.cmd;
+- BDRVRawState *raw_state = s->bs->opaque;
+ int r;
+- uint8_t cmd_code = s->atapi_pt.request[0];
+- uint32_t din_desired;
++ volatile struct sg_io_v4 *cmd;
++ BDRVRawState *raw_state;
++ IDEState *s = (volatile IDEState *)arg;
++
++ for(;;) {
++ pthread_mutex_lock(&s->atapi_pt.sgio_mutex);
++ pthread_cond_wait(&s->atapi_pt.sgio_cv, &s->atapi_pt.sgio_mutex);
++
++ /* Send command and wait for reply, SG_IO ioctl*/
++ cmd = &s->atapi_pt.cmd;
++ raw_state = s->bs->opaque;
++ r = ioctl(raw_state->fd, 0x2285, cmd);
++
++ /* Unlock _before_ signalling parent */
++ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
++ write(s->atapi_pt.sgio_wfd, &r, sizeof(int));
++ }
++}
++
++static int ide_atapi_pt_aio_flush(void *unused)
++{
++ return 0;
++}
++
++static void ide_atapi_pt_do_sg_io_complete(void *unused);
++
++static void ide_atapi_pt_setup_sgio_thread(IDEState *s)
++{
++ int fds[2];
++
++ DEBUG_PRINTF("%s\n", __FUNCTION__);
++ if (pipe(fds) < 0) {
++ fprintf(stderr, "atapi-pt failed to create pipe: %m\n");
++ exit(1);
++ }
++ s->atapi_pt.sgio_rfd = fds[0];
++ s->atapi_pt.sgio_wfd = fds[1];
++
++ pthread_mutex_init(&s->atapi_pt.sgio_mutex, NULL);
++ pthread_cond_init (&s->atapi_pt.sgio_cv, NULL);
++ pthread_create(&s->atapi_pt.sgio_thread, NULL, ide_atapi_pt_sgio_worker_thread, (void *)s);
++ qemu_aio_set_fd_handler(s->atapi_pt.sgio_rfd, ide_atapi_pt_do_sg_io_complete, NULL, ide_atapi_pt_aio_flush, (void *)s);
++}
+
++/* Call with ide_atapi_sgio_mutex held */
++static void ide_atapi_pt_do_sg_io(IDEState *s, int timeout)
++{
++ 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 +741,27 @@ 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;
+
+- /* Send command and wait for reply, SG_IO ioctl*/
+- r = ioctl(raw_state->fd, 0x2285, cmd);
++ /* 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);
++}
++
++static void ide_atapi_pt_do_sg_io_complete(void *arg)
++{
++ IDEState *s = (IDEState *)arg;
++ struct sg_io_v4 *cmd = &s->atapi_pt.cmd;
++ BDRVRawState *raw_state = s->bs->opaque;
++ int r;
++ uint8_t cmd_code = s->atapi_pt.request[0];
++ uint32_t din_desired;
++
++ DEBUG_PRINTF("%s\n", __FUNCTION__);
++ assert(s);
++
++ /* 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_GET_EVENT_STATUS_NOTIFICATION)
+ {
+@@ -836,12 +904,14 @@ static void ide_atapi_pt_do_sg_io(IDEState *s)
+ ide_atapi_cmd_reply(s, din_desired, cmd->din_xfer_len);
+ }
+
++/* Call with ide_atapi_sgio_mutex held */
+ static void ide_atapi_pt_dout_fetch_pio_done(IDEState *s)
+ {
+ ide_transfer_stop(s);
+- ide_atapi_pt_do_sg_io(s);
++ ide_atapi_pt_do_sg_io(s, 0);
+ }
+
++/* Call with ide_atapi_sgio_mutex held */
+ static void ide_atapi_pt_dout_fetch_dma_done(void *opaque, int ret)
+ {
+ BMDMAState *bm = opaque;
+@@ -850,13 +920,15 @@ static void ide_atapi_pt_dout_fetch_dma_done(void *opaque, int ret)
+
+ if (ret < 0) {
+ ide_atapi_io_error(s, ret);
++ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
+ return;
+ }
+
+ i = dma_buf_rw(bm, 0);
+- ide_atapi_pt_do_sg_io(s);
++ ide_atapi_pt_do_sg_io(s, 0);
+ }
+
++/* Call with ide_atapi_sgio_mutex held */
+ static void ide_atapi_pt_wcmd(IDEState *s)
+ {
+ if (s->atapi_dma)
+@@ -1005,11 +1077,20 @@ static int ide_atapi_pt_read_cd_block_size(const uint8_t *io_buffer)
+ return block_size;
+ }
+
++
+ static void ide_atapi_pt_cmd(IDEState *s)
+ {
++ int timeout;
+ struct sg_io_v4 *cmd = &s->atapi_pt.cmd;
+ uint8_t cmd_code;
+
++
++ 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 +1102,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
++ timeout = 0;
+
+ s->status |= BUSY_STAT;
+
+@@ -1054,14 +1136,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);
+
++ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
+ return;
+
+ case GPCMD_BLANK: // bigger timeout while blanking
+- cmd->timeout = 1000 * 60 * 80; // 80 mins
++ timeout = 1000 * 60 * 80; // 80 mins
+ break;
+
+ case GPCMD_CLOSE_TRACK:
+- cmd->timeout = 1000 * 60 * 5; // 5 mins
++ timeout = 1000 * 60 * 5; // 5 mins
+ break;
+
+ case GPCMD_WRITE_BUFFER:
+@@ -1072,6 +1155,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);
++ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
+ return;
+ }
+
+@@ -1091,6 +1175,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);
++ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
+ return;
+ }
+
+@@ -1152,6 +1237,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);
++ pthread_mutex_unlock(&s->atapi_pt.sgio_mutex);
+ return;
+ }
+ break;
+@@ -1169,5 +1255,5 @@ static void ide_atapi_pt_cmd(IDEState *s)
+ return;
+ }
+
+- ide_atapi_pt_do_sg_io(s);
++ ide_atapi_pt_do_sg_io(s, timeout);
+ }
+diff --git a/hw/ide.c b/hw/ide.c
+index 6c1d60a..2b58728 100644
+--- a/hw/ide.c
++++ b/hw/ide.c
+@@ -435,6 +435,12 @@ typedef struct ATAPIPassThroughState
+
+ time_t new_cd_time;
+ time_t eject_time;
++
++ pthread_t sgio_thread;
++ pthread_mutex_t sgio_mutex;
++ pthread_cond_t sgio_cv;
++ int sgio_rfd;
++ int sgio_wfd;
+ } ATAPIPassThroughState;
+ #endif /* __linux__ */
+
+@@ -3220,6 +3226,7 @@ static void ide_init2(IDEState *ide_state,
+ }
+ #ifdef __linux__
+ else if (bdrv_get_type_hint(s->bs) == BDRV_TYPE_CDROM_PT) {
++ ide_atapi_pt_setup_sgio_thread(s);
+ // BDRVRawState *raw_state = s->bs->opaque;
+ s->is_cdrom = 1;
+ s->atapi_cmd = ide_atapi_pt_cmd;
projects / xenclient / ioemu-pq.git / commitdiff