aboutsummaryrefslogtreecommitdiffstats
path: root/lib/lufa/Demos/Device/ClassDriver/Keyboard/Keyboard.c
blob: 99799f6d53fdff5581a31930d5777187df21bab8 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
/*
             LUFA Library
     Copyright (C) Dean Camera, 2017.

  dean [at] fourwalledcubicle [dot] com
           www.lufa-lib.org
*/

/*
  Copyright 2017  Dean Camera (dean [at] fourwalledcubicle [dot] com)

  Permission to use, copy, modify, distribute, and sell this
  software and its documentation for any purpose is hereby granted
  without fee, provided that the above copyright notice appear in
  all copies and that both that the copyright notice and this
  permission notice and warranty disclaimer appear in supporting
  documentation, and that the name of the author not be used in
  advertising or publicity pertaining to distribution of the
  software without specific, written prior permission.

  The author disclaims all warranties with regard to this
  software, including all implied warranties of merchantability
  and fitness.  In no event shall the author be liable for any
  special, indirect or consequential damages or any damages
  whatsoever resulting from loss of use, data or profits, whether
  in an action of contract, negligence or other tortious action,
  arising out of or in connection with the use or performance of
  this software.
*/

/** \file
 *
 *  Main source file for the Keyboard demo. This file contains the main tasks of
 *  the demo and is responsible for the initial application hardware configuration.
 */

#include "Keyboard.h"

/** Buffer to hold the previously generated Keyboard HID report, for comparison purposes inside the HID class driver. */
static uint8_t PrevKeyboardHIDReportBuffer[sizeof(USB_KeyboardReport_Data_t)];

/** LUFA HID Class driver interface configuration and state information. This structure is
 *  passed to all HID Class driver functions, so that multiple instances of the same class
 *  within a device can be differentiated from one another.
 */
USB_ClassInfo_HID_Device_t Keyboard_HID_Interface =
	{
		.Config =
			{
				.InterfaceNumber              = INTERFACE_ID_Keyboard,
				.ReportINEndpoint             =
					{
						.Address              = KEYBOARD_EPADDR,
						.Size                 = KEYBOARD_EPSIZE,
						.Banks                = 1,
					},
				.PrevReportINBuffer           = PrevKeyboardHIDReportBuffer,
				.PrevReportINBufferSize       = sizeof(PrevKeyboardHIDReportBuffer),
			},
	};


/** Main program entry point. This routine contains the overall program flow, including initial
 *  setup of all components and the main program loop.
 */
int main(void)
{
	SetupHardware();

	LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
	GlobalInterruptEnable();

	for (;;)
	{
		HID_Device_USBTask(&Keyboard_HID_Interface);
		USB_USBTask();
	}
}

/** Configures the board hardware and chip peripherals for the demo's functionality. */
void SetupHardware()
{
#if (ARCH == ARCH_AVR8)
	/* Disable watchdog if enabled by bootloader/fuses */
	MCUSR &= ~(1 << WDRF);
	wdt_disable();

	/* Disable clock division */
	clock_prescale_set(clock_div_1);
#elif (ARCH == ARCH_XMEGA)
	/* Start the PLL to multiply the 2MHz RC oscillator to 32MHz and switch the CPU core to run from it */
	XMEGACLK_StartPLL(CLOCK_SRC_INT_RC2MHZ, 2000000, F_CPU);
	XMEGACLK_SetCPUClockSource(CLOCK_SRC_PLL);

	/* Start the 32MHz internal RC oscillator and start the DFLL to increase it to 48MHz using the USB SOF as a reference */
	XMEGACLK_StartInternalOscillator(CLOCK_SRC_INT_RC32MHZ);
	XMEGACLK_StartDFLL(CLOCK_SRC_INT_RC32MHZ, DFLL_REF_INT_USBSOF, F_USB);

	PMIC.CTRL = PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
#endif

	/* Hardware Initialization */
	Joystick_Init();
	LEDs_Init();
	Buttons_Init();
	USB_Init();
}

/** Event handler for the library USB Connection event. */
void EVENT_USB_Device_Connect(void)
{
	LEDs_SetAllLEDs(LEDMASK_USB_ENUMERATING);
}

/** Event handler for the library USB Disconnection event. */
void EVENT_USB_Device_Disconnect(void)
{
	LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
}

/** Event handler for the library USB Configuration Changed event. */
void EVENT_USB_Device_ConfigurationChanged(void)
{
	bool ConfigSuccess = true;

	ConfigSuccess &= HID_Device_ConfigureEndpoints(&Keyboard_HID_Interface);

	USB_Device_EnableSOFEvents();

	LEDs_SetAllLEDs(ConfigSuccess ? LEDMASK_USB_READY : LEDMASK_USB_ERROR);
}

/** Event handler for the library USB Control Request reception event. */
void EVENT_USB_Device_ControlRequest(void)
{
	HID_Device_ProcessControlRequest(&Keyboard_HID_Interface);
}

/** Event handler for the USB device Start Of Frame event. */
void EVENT_USB_Device_StartOfFrame(void)
{
	HID_Device_MillisecondElapsed(&Keyboard_HID_Interface);
}

/** HID class driver callback function for the creation of HID reports to the host.
 *
 *  \param[in]     HIDInterfaceInfo  Pointer to the HID class interface configuration structure being referenced
 *  \param[in,out] ReportID    Report ID requested by the host if non-zero, otherwise callback should set to the generated report ID
 *  \param[in]     ReportType  Type of the report to create, either HID_REPORT_ITEM_In or HID_REPORT_ITEM_Feature
 *  \param[out]    ReportData  Pointer to a buffer where the created report should be stored
 *  \param[out]    ReportSize  Number of bytes written in the report (or zero if no report is to be sent)
 *
 *  \return Boolean \c true to force the sending of the report, \c false to let the library determine if it needs to be sent
 */
bool CALLBACK_HID_Device_CreateHIDReport(USB_ClassInfo_HID_Device_t* const HIDInterfaceInfo,
                                         uint8_t* const ReportID,
                                         const uint8_t ReportType,
                                         void* ReportData,
                                         uint16_t* const ReportSize)
{
	USB_KeyboardReport_Data_t* KeyboardReport = (USB_KeyboardReport_Data_t*)ReportData;

	uint8_t JoyStatus_LCL    = Joystick_GetStatus();
	uint8_t ButtonStatus_LCL = Buttons_GetStatus();

	uint8_t UsedKeyCodes = 0;

	if (JoyStatus_LCL & JOY_UP)
	  KeyboardReport->KeyCode[UsedKeyCodes++] = HID_KEYBOARD_SC_A;
	else if (JoyStatus_LCL & JOY_DOWN)
	  KeyboardReport->KeyCode[UsedKeyCodes++] = HID_KEYBOARD_SC_B;

	if (JoyStatus_LCL & JOY_LEFT)
	  KeyboardReport->KeyCode[UsedKeyCodes++] = HID_KEYBOARD_SC_C;
	else if (JoyStatus_LCL & JOY_RIGHT)
	  KeyboardReport->KeyCode[UsedKeyCodes++] = HID_KEYBOARD_SC_D;

	if (JoyStatus_LCL & JOY_PRESS)
	  KeyboardReport->KeyCode[UsedKeyCodes++] = HID_KEYBOARD_SC_E;

	if (ButtonStatus_LCL & BUTTONS_BUTTON1)
	  KeyboardReport->KeyCode[UsedKeyCodes++] = HID_KEYBOARD_SC_F;

	if (UsedKeyCodes)
	  KeyboardReport->Modifier = HID_KEYBOARD_MODIFIER_LEFTSHIFT;

	*ReportSize = sizeof(USB_KeyboardReport_Data_t);
	return false;
}

/** HID class driver callback function for the processing of HID reports from the host.
 *
 *  \param[in] HIDInterfaceInfo  Pointer to the HID class interface configuration structure being referenced
 *  \param[in] ReportID    Report ID of the received report from the host
 *  \param[in] ReportType  The type of report that the host has sent, either HID_REPORT_ITEM_Out or HID_REPORT_ITEM_Feature
 *  \param[in] ReportData  Pointer to a buffer where the received report has been stored
 *  \param[in] ReportSize  Size in bytes of the received HID report
 */
void CALLBACK_HID_Device_ProcessHIDReport(USB_ClassInfo_HID_Device_t* const HIDInterfaceInfo,
                                          const uint8_t ReportID,
                                          const uint8_t ReportType,
                                          const void* ReportData,
                                          const uint16_t ReportSize)
{
	uint8_t  LEDMask   = LEDS_NO_LEDS;
	uint8_t* LEDReport = (uint8_t*)ReportData;

	if (*LEDReport & HID_KEYBOARD_LED_NUMLOCK)
	  LEDMask |= LEDS_LED1;

	if (*LEDReport & HID_KEYBOARD_LED_CAPSLOCK)
	  LEDMask |= LEDS_LED3;

	if (*LEDReport & HID_KEYBOARD_LED_SCROLLLOCK)
	  LEDMask |= LEDS_LED4;

	LEDs_SetAllLEDs(LEDMask);
}
4'>994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345
/*
 * platform.c: handling x86 platform related MMIO instructions
 *
 * Copyright (c) 2004, Intel Corporation.
 * Copyright (c) 2005, International Business Machines Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
 * version 2, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 * Place - Suite 330, Boston, MA 02111-1307 USA.
 */

#include <xen/config.h>
#include <xen/types.h>
#include <xen/mm.h>
#include <xen/domain_page.h>
#include <asm/page.h>
#include <xen/event.h>
#include <xen/trace.h>
#include <xen/sched.h>
#include <asm/regs.h>
#include <asm/x86_emulate.h>
#include <asm/paging.h>
#include <asm/hvm/hvm.h>
#include <asm/hvm/support.h>
#include <asm/hvm/io.h>
#include <public/hvm/ioreq.h>

#include <xen/lib.h>
#include <xen/sched.h>
#include <asm/current.h>

#define DECODE_success  1
#define DECODE_failure  0

#define mk_operand(size_reg, index, seg, flag) \
    (((size_reg) << 24) | ((index) << 16) | ((seg) << 8) | (flag))

#if defined (__x86_64__)
static inline long __get_reg_value(unsigned long reg, int size)
{
    switch ( size ) {
    case BYTE_64:
        return (char)(reg & 0xFF);
    case WORD:
        return (short)(reg & 0xFFFF);
    case LONG:
        return (int)(reg & 0xFFFFFFFF);
    case QUAD:
        return (long)(reg);
    default:
        printk("Error: (__get_reg_value) Invalid reg size\n");
        domain_crash_synchronous();
    }
}

long get_reg_value(int size, int index, int seg, struct cpu_user_regs *regs)
{
    if ( size == BYTE ) {
        switch ( index ) {
        case 0: /* %al */
            return (char)(regs->rax & 0xFF);
        case 1: /* %cl */
            return (char)(regs->rcx & 0xFF);
        case 2: /* %dl */
            return (char)(regs->rdx & 0xFF);
        case 3: /* %bl */
            return (char)(regs->rbx & 0xFF);
        case 4: /* %ah */
            return (char)((regs->rax & 0xFF00) >> 8);
        case 5: /* %ch */
            return (char)((regs->rcx & 0xFF00) >> 8);
        case 6: /* %dh */
            return (char)((regs->rdx & 0xFF00) >> 8);
        case 7: /* %bh */
            return (char)((regs->rbx & 0xFF00) >> 8);
        default:
            printk("Error: (get_reg_value) Invalid index value\n");
            domain_crash_synchronous();
        }
        /* NOTREACHED */
    }

    switch ( index ) {
    case 0: return __get_reg_value(regs->rax, size);
    case 1: return __get_reg_value(regs->rcx, size);
    case 2: return __get_reg_value(regs->rdx, size);
    case 3: return __get_reg_value(regs->rbx, size);
    case 4: return __get_reg_value(regs->rsp, size);
    case 5: return __get_reg_value(regs->rbp, size);
    case 6: return __get_reg_value(regs->rsi, size);
    case 7: return __get_reg_value(regs->rdi, size);
    case 8: return __get_reg_value(regs->r8, size);
    case 9: return __get_reg_value(regs->r9, size);
    case 10: return __get_reg_value(regs->r10, size);
    case 11: return __get_reg_value(regs->r11, size);
    case 12: return __get_reg_value(regs->r12, size);
    case 13: return __get_reg_value(regs->r13, size);
    case 14: return __get_reg_value(regs->r14, size);
    case 15: return __get_reg_value(regs->r15, size);
    default:
        printk("Error: (get_reg_value) Invalid index value\n");
        domain_crash_synchronous();
    }
}
#elif defined (__i386__)
static inline long __get_reg_value(unsigned long reg, int size)
{
    switch ( size ) {
    case WORD:
        return (short)(reg & 0xFFFF);
    case LONG:
        return (int)(reg & 0xFFFFFFFF);
    default:
        printk("Error: (__get_reg_value) Invalid reg size\n");
        domain_crash_synchronous();
    }
}

long get_reg_value(int size, int index, int seg, struct cpu_user_regs *regs)
{
    if ( size == BYTE ) {
        switch ( index ) {
        case 0: /* %al */
            return (char)(regs->eax & 0xFF);
        case 1: /* %cl */
            return (char)(regs->ecx & 0xFF);
        case 2: /* %dl */
            return (char)(regs->edx & 0xFF);
        case 3: /* %bl */
            return (char)(regs->ebx & 0xFF);
        case 4: /* %ah */
            return (char)((regs->eax & 0xFF00) >> 8);
        case 5: /* %ch */
            return (char)((regs->ecx & 0xFF00) >> 8);
        case 6: /* %dh */
            return (char)((regs->edx & 0xFF00) >> 8);
        case 7: /* %bh */
            return (char)((regs->ebx & 0xFF00) >> 8);
        default:
            printk("Error: (get_reg_value) Invalid index value\n");
            domain_crash_synchronous();
        }
    }

    switch ( index ) {
    case 0: return __get_reg_value(regs->eax, size);
    case 1: return __get_reg_value(regs->ecx, size);
    case 2: return __get_reg_value(regs->edx, size);
    case 3: return __get_reg_value(regs->ebx, size);
    case 4: return __get_reg_value(regs->esp, size);
    case 5: return __get_reg_value(regs->ebp, size);
    case 6: return __get_reg_value(regs->esi, size);
    case 7: return __get_reg_value(regs->edi, size);
    default:
        printk("Error: (get_reg_value) Invalid index value\n");
        domain_crash_synchronous();
    }
}
#endif

static inline unsigned char *check_prefix(unsigned char *inst,
                                          struct hvm_io_op *mmio_op,
                                          unsigned char *ad_size,
                                          unsigned char *op_size,
                                          unsigned char *seg_sel,
                                          unsigned char *rex_p)
{
    while ( 1 ) {
        switch ( *inst ) {
            /* rex prefix for em64t instructions */
        case 0x40 ... 0x4f:
            *rex_p = *inst;
            break;
        case 0xf3: /* REPZ */
            mmio_op->flags = REPZ;
            break;
        case 0xf2: /* REPNZ */
            mmio_op->flags = REPNZ;
            break;
        case 0xf0: /* LOCK */
            break;
        case 0x2e: /* CS */
        case 0x36: /* SS */
        case 0x3e: /* DS */
        case 0x26: /* ES */
        case 0x64: /* FS */
        case 0x65: /* GS */
            *seg_sel = *inst;
            break;
        case 0x66: /* 32bit->16bit */
            *op_size = WORD;
            break;
        case 0x67:
            *ad_size = WORD;
            break;
        default:
            return inst;
        }
        inst++;
    }
}

static inline unsigned long get_immediate(int ad_size, const unsigned char *inst, int op_size)
{
    int mod, reg, rm;
    unsigned long val = 0;
    int i;

    mod = (*inst >> 6) & 3;
    reg = (*inst >> 3) & 7;
    rm = *inst & 7;

    inst++; //skip ModR/M byte
    if ( ad_size != WORD && mod != 3 && rm == 4 ) {
        rm = *inst & 7;
        inst++; //skip SIB byte
    }

    switch ( mod ) {
    case 0:
        if ( ad_size == WORD ) {
            if ( rm == 6 )
                inst = inst + 2; //disp16, skip 2 bytes
        }
        else {
            if ( rm == 5 )
                inst = inst + 4; //disp32, skip 4 bytes
        }
        break;
    case 1:
        inst++; //disp8, skip 1 byte
        break;
    case 2:
        if ( ad_size == WORD )
            inst = inst + 2; //disp16, skip 2 bytes
        else
            inst = inst + 4; //disp32, skip 4 bytes
        break;
    }

    if ( op_size == QUAD )
        op_size = LONG;

    for ( i = 0; i < op_size; i++ ) {
        val |= (*inst++ & 0xff) << (8 * i);
    }

    return val;
}

static inline unsigned long get_immediate_sign_ext(
    int ad_size, const unsigned char *inst, int op_size)
{
    unsigned long result = get_immediate(ad_size, inst, op_size);
    if ( op_size == BYTE )
        return (int8_t)result;
    if ( op_size == WORD )
        return (int16_t)result;
    return (int32_t)result;
}

static inline int get_index(const unsigned char *inst, unsigned char rex)
{
    int mod, reg, rm;
    int rex_r, rex_b;

    mod = (*inst >> 6) & 3;
    reg = (*inst >> 3) & 7;
    rm = *inst & 7;

    rex_r = (rex >> 2) & 1;
    rex_b = rex & 1;

    //Only one operand in the instruction is register
    if ( mod == 3 ) {
        return (rm + (rex_b << 3));
    } else {
        return (reg + (rex_r << 3));
    }
    return 0;
}

static void init_instruction(struct hvm_io_op *mmio_op)
{
    mmio_op->instr = 0;

    mmio_op->flags = 0;

    mmio_op->operand[0] = 0;
    mmio_op->operand[1] = 0;
    mmio_op->immediate = 0;
}

#define GET_OP_SIZE_FOR_BYTE(size_reg)      \
    do {                                    \
        if ( rex )                          \
            (size_reg) = BYTE_64;           \
        else                                \
            (size_reg) = BYTE;              \
    } while( 0 )

#define GET_OP_SIZE_FOR_NONEBYTE(op_size)   \
    do {                                    \
        if ( rex & 0x8 )                    \
            (op_size) = QUAD;               \
        else if ( (op_size) != WORD )       \
            (op_size) = LONG;               \
    } while( 0 )


/*
 * Decode mem,accumulator operands (as in <opcode> m8/m16/m32, al,ax,eax)
 */
static inline int mem_acc(unsigned char size, struct hvm_io_op *mmio)
{
    mmio->operand[0] = mk_operand(size, 0, 0, MEMORY);
    mmio->operand[1] = mk_operand(size, 0, 0, REGISTER);
    return DECODE_success;
}

/*
 * Decode accumulator,mem operands (as in <opcode> al,ax,eax, m8/m16/m32)
 */
static inline int acc_mem(unsigned char size, struct hvm_io_op *mmio)
{
    mmio->operand[0] = mk_operand(size, 0, 0, REGISTER);
    mmio->operand[1] = mk_operand(size, 0, 0, MEMORY);
    return DECODE_success;
}

/*
 * Decode mem,reg operands (as in <opcode> r32/16, m32/16)
 */
static int mem_reg(unsigned char size, unsigned char *opcode,
                   struct hvm_io_op *mmio_op, unsigned char rex)
{
    int index = get_index(opcode + 1, rex);

    mmio_op->operand[0] = mk_operand(size, 0, 0, MEMORY);
    mmio_op->operand[1] = mk_operand(size, index, 0, REGISTER);
    return DECODE_success;
}

/*
 * Decode reg,mem operands (as in <opcode> m32/16, r32/16)
 */
static int reg_mem(unsigned char size, unsigned char *opcode,
                   struct hvm_io_op *mmio_op, unsigned char rex)
{
    int index = get_index(opcode + 1, rex);

    mmio_op->operand[0] = mk_operand(size, index, 0, REGISTER);
    mmio_op->operand[1] = mk_operand(size, 0, 0, MEMORY);
    return DECODE_success;
}

static int mmio_decode(int address_bytes, unsigned char *opcode,
                       struct hvm_io_op *mmio_op,
                       unsigned char *ad_size, unsigned char *op_size,
                       unsigned char *seg_sel)
{
    unsigned char size_reg = 0;
    unsigned char rex = 0;
    int index;

    *ad_size = 0;
    *op_size = 0;
    *seg_sel = 0;
    init_instruction(mmio_op);

    opcode = check_prefix(opcode, mmio_op, ad_size, op_size, seg_sel, &rex);

    switch ( address_bytes )
    {
    case 2:
        if ( *op_size == WORD )
            *op_size = LONG;
        else if ( *op_size == LONG )
            *op_size = WORD;
        else if ( *op_size == 0 )
            *op_size = WORD;
        if ( *ad_size == WORD )
            *ad_size = LONG;
        else if ( *ad_size == LONG )
            *ad_size = WORD;
        else if ( *ad_size == 0 )
            *ad_size = WORD;
        break;
    case 4:
        if ( *op_size == 0 )
            *op_size = LONG;
        if ( *ad_size == 0 )
            *ad_size = LONG;
        break;
#ifdef __x86_64__
    case 8:
        if ( *op_size == 0 )
            *op_size = rex & 0x8 ? QUAD : LONG;
        if ( *ad_size == WORD )
            *ad_size = LONG;
        else if ( *ad_size == 0 )
            *ad_size = QUAD;
        break;
#endif
    }

    /* the operands order in comments conforms to AT&T convention */

    switch ( *opcode ) {

    case 0x00: /* add r8, m8 */
        mmio_op->instr = INSTR_ADD;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x03: /* add m32/16, r32/16 */
        mmio_op->instr = INSTR_ADD;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return mem_reg(*op_size, opcode, mmio_op, rex);

    case 0x08: /* or r8, m8 */	
        mmio_op->instr = INSTR_OR;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x09: /* or r32/16, m32/16 */
        mmio_op->instr = INSTR_OR;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return reg_mem(*op_size, opcode, mmio_op, rex);

    case 0x0A: /* or m8, r8 */
        mmio_op->instr = INSTR_OR;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return mem_reg(size_reg, opcode, mmio_op, rex);

    case 0x0B: /* or m32/16, r32/16 */
        mmio_op->instr = INSTR_OR;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return mem_reg(*op_size, opcode, mmio_op, rex);

    case 0x20: /* and r8, m8 */
        mmio_op->instr = INSTR_AND;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x21: /* and r32/16, m32/16 */
        mmio_op->instr = INSTR_AND;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return reg_mem(*op_size, opcode, mmio_op, rex);

    case 0x22: /* and m8, r8 */
        mmio_op->instr = INSTR_AND;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return mem_reg(size_reg, opcode, mmio_op, rex);

    case 0x23: /* and m32/16, r32/16 */
        mmio_op->instr = INSTR_AND;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return mem_reg(*op_size, opcode, mmio_op, rex);

    case 0x2B: /* sub m32/16, r32/16 */
        mmio_op->instr = INSTR_SUB;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return mem_reg(*op_size, opcode, mmio_op, rex);

    case 0x30: /* xor r8, m8 */
        mmio_op->instr = INSTR_XOR;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x31: /* xor r32/16, m32/16 */
        mmio_op->instr = INSTR_XOR;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return reg_mem(*op_size, opcode, mmio_op, rex);

    case 0x32: /* xor m8, r8 */
        mmio_op->instr = INSTR_XOR;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return mem_reg(size_reg, opcode, mmio_op, rex);

    case 0x38: /* cmp r8, m8 */
        mmio_op->instr = INSTR_CMP;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x39: /* cmp r32/16, m32/16 */
        mmio_op->instr = INSTR_CMP;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return reg_mem(*op_size, opcode, mmio_op, rex);

    case 0x3A: /* cmp m8, r8 */
        mmio_op->instr = INSTR_CMP;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return mem_reg(size_reg, opcode, mmio_op, rex);

    case 0x3B: /* cmp m32/16, r32/16 */
        mmio_op->instr = INSTR_CMP;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return mem_reg(*op_size, opcode, mmio_op, rex);

    case 0x80:
    case 0x81:
    case 0x83:
    {
        unsigned char ins_subtype = (opcode[1] >> 3) & 7;

        if ( opcode[0] == 0x80 ) {
            *op_size = BYTE;
            GET_OP_SIZE_FOR_BYTE(size_reg);
        } else {
            GET_OP_SIZE_FOR_NONEBYTE(*op_size);
            size_reg = *op_size;
        }

        /* opcode 0x83 always has a single byte operand */
        if ( opcode[0] == 0x83 )
            mmio_op->immediate =
                get_immediate_sign_ext(*ad_size, opcode + 1, BYTE);
        else
            mmio_op->immediate =
                get_immediate_sign_ext(*ad_size, opcode + 1, *op_size);

        mmio_op->operand[0] = mk_operand(size_reg, 0, 0, IMMEDIATE);
        mmio_op->operand[1] = mk_operand(size_reg, 0, 0, MEMORY);

        switch ( ins_subtype ) {
        case 0: /* add $imm, m32/16 */
            mmio_op->instr = INSTR_ADD;
            return DECODE_success;

        case 1: /* or $imm, m32/16 */
            mmio_op->instr = INSTR_OR;
            return DECODE_success;

        case 4: /* and $imm, m32/16 */
            mmio_op->instr = INSTR_AND;
            return DECODE_success;

        case 5: /* sub $imm, m32/16 */
            mmio_op->instr = INSTR_SUB;
            return DECODE_success;

        case 6: /* xor $imm, m32/16 */
            mmio_op->instr = INSTR_XOR;
            return DECODE_success;

        case 7: /* cmp $imm, m32/16 */
            mmio_op->instr = INSTR_CMP;
            return DECODE_success;

        default:
            printk("%x/%x, This opcode isn't handled yet!\n",
                   *opcode, ins_subtype);
            return DECODE_failure;
        }
    }

    case 0x84:  /* test r8, m8 */
        mmio_op->instr = INSTR_TEST;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x85: /* test r16/32, m16/32 */
        mmio_op->instr = INSTR_TEST;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return reg_mem(*op_size, opcode, mmio_op, rex);

    case 0x86:  /* xchg m8, r8 */
        mmio_op->instr = INSTR_XCHG;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x87:  /* xchg m16/32, r16/32 */
        mmio_op->instr = INSTR_XCHG;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return reg_mem(*op_size, opcode, mmio_op, rex);

    case 0x88: /* mov r8, m8 */
        mmio_op->instr = INSTR_MOV;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return reg_mem(size_reg, opcode, mmio_op, rex);

    case 0x89: /* mov r32/16, m32/16 */
        mmio_op->instr = INSTR_MOV;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return reg_mem(*op_size, opcode, mmio_op, rex);

    case 0x8A: /* mov m8, r8 */
        mmio_op->instr = INSTR_MOV;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return mem_reg(size_reg, opcode, mmio_op, rex);

    case 0x8B: /* mov m32/16, r32/16 */
        mmio_op->instr = INSTR_MOV;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return mem_reg(*op_size, opcode, mmio_op, rex);

    case 0xA0: /* mov <addr>, al */
        mmio_op->instr = INSTR_MOV;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return mem_acc(size_reg, mmio_op);

    case 0xA1: /* mov <addr>, ax/eax */
        mmio_op->instr = INSTR_MOV;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return mem_acc(*op_size, mmio_op);

    case 0xA2: /* mov al, <addr> */
        mmio_op->instr = INSTR_MOV;
        *op_size = BYTE;
        GET_OP_SIZE_FOR_BYTE(size_reg);
        return acc_mem(size_reg, mmio_op);

    case 0xA3: /* mov ax/eax, <addr> */
        mmio_op->instr = INSTR_MOV;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return acc_mem(*op_size, mmio_op);

    case 0xA4: /* movsb */
        mmio_op->instr = INSTR_MOVS;
        *op_size = BYTE;
        return DECODE_success;

    case 0xA5: /* movsw/movsl */
        mmio_op->instr = INSTR_MOVS;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return DECODE_success;

    case 0xAA: /* stosb */
        mmio_op->instr = INSTR_STOS;
        *op_size = BYTE;
        return DECODE_success;

    case 0xAB: /* stosw/stosl */
        mmio_op->instr = INSTR_STOS;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return DECODE_success;

    case 0xAC: /* lodsb */
        mmio_op->instr = INSTR_LODS;
        *op_size = BYTE;
        return DECODE_success;

    case 0xAD: /* lodsw/lodsl */
        mmio_op->instr = INSTR_LODS;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        return DECODE_success;

    case 0xC6:
        if ( ((opcode[1] >> 3) & 7) == 0 ) { /* mov $imm8, m8 */
            mmio_op->instr = INSTR_MOV;
            *op_size = BYTE;

            mmio_op->operand[0] = mk_operand(*op_size, 0, 0, IMMEDIATE);
            mmio_op->immediate  =
                    get_immediate(*ad_size, opcode + 1, *op_size);
            mmio_op->operand[1] = mk_operand(*op_size, 0, 0, MEMORY);

            return DECODE_success;
        } else
            return DECODE_failure;

    case 0xC7:
        if ( ((opcode[1] >> 3) & 7) == 0 ) { /* mov $imm16/32, m16/32 */
            mmio_op->instr = INSTR_MOV;
            GET_OP_SIZE_FOR_NONEBYTE(*op_size);

            mmio_op->operand[0] = mk_operand(*op_size, 0, 0, IMMEDIATE);
            mmio_op->immediate =
                    get_immediate_sign_ext(*ad_size, opcode + 1, *op_size);
            mmio_op->operand[1] = mk_operand(*op_size, 0, 0, MEMORY);

            return DECODE_success;
        } else
            return DECODE_failure;

    case 0xF6:
    case 0xF7:
        if ( ((opcode[1] >> 3) & 7) == 0 ) { /* test $imm8/16/32, m8/16/32 */
            mmio_op->instr = INSTR_TEST;

            if ( opcode[0] == 0xF6 ) {
                *op_size = BYTE;
                GET_OP_SIZE_FOR_BYTE(size_reg);
            } else {
                GET_OP_SIZE_FOR_NONEBYTE(*op_size);
                size_reg = *op_size;
            }

            mmio_op->operand[0] = mk_operand(size_reg, 0, 0, IMMEDIATE);
            mmio_op->immediate =
                    get_immediate_sign_ext(*ad_size, opcode + 1, *op_size);
            mmio_op->operand[1] = mk_operand(size_reg, 0, 0, MEMORY);

            return DECODE_success;
        } else
            return DECODE_failure;

    case 0xFE:
    case 0xFF:
    {
        unsigned char ins_subtype = (opcode[1] >> 3) & 7;

        if ( opcode[0] == 0xFE ) {
            *op_size = BYTE;
            GET_OP_SIZE_FOR_BYTE(size_reg);
        } else {
            GET_OP_SIZE_FOR_NONEBYTE(*op_size);
            size_reg = *op_size;
        }

        mmio_op->immediate = 1;
        mmio_op->operand[0] = mk_operand(size_reg, 0, 0, IMMEDIATE);
        mmio_op->operand[1] = mk_operand(size_reg, 0, 0, MEMORY);

        switch ( ins_subtype ) {
        case 0: /* inc */
            mmio_op->instr = INSTR_ADD;
            return DECODE_success;

        case 1: /* dec */
            mmio_op->instr = INSTR_SUB;
            return DECODE_success;

        case 6: /* push */
            mmio_op->instr = INSTR_PUSH;
            mmio_op->operand[0] = mmio_op->operand[1];
            return DECODE_success;

        default:
            printk("%x/%x, This opcode isn't handled yet!\n",
                   *opcode, ins_subtype);
            return DECODE_failure;
        }
    }

    case 0x0F:
        break;

    default:
        printk("%x, This opcode isn't handled yet!\n", *opcode);
        return DECODE_failure;
    }

    switch ( *++opcode ) {
    case 0xB6: /* movzx m8, r16/r32/r64 */
        mmio_op->instr = INSTR_MOVZX;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        index = get_index(opcode + 1, rex);
        mmio_op->operand[0] = mk_operand(BYTE, 0, 0, MEMORY);
        mmio_op->operand[1] = mk_operand(*op_size, index, 0, REGISTER);
        return DECODE_success;

    case 0xB7: /* movzx m16, r32/r64 */
        mmio_op->instr = INSTR_MOVZX;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        index = get_index(opcode + 1, rex);
        mmio_op->operand[0] = mk_operand(WORD, 0, 0, MEMORY);
        mmio_op->operand[1] = mk_operand(*op_size, index, 0, REGISTER);
        return DECODE_success;

    case 0xBE: /* movsx m8, r16/r32/r64 */
        mmio_op->instr = INSTR_MOVSX;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        index = get_index(opcode + 1, rex);
        mmio_op->operand[0] = mk_operand(BYTE, 0, 0, MEMORY);
        mmio_op->operand[1] = mk_operand(*op_size, index, 0, REGISTER);
        return DECODE_success;

    case 0xBF: /* movsx m16, r32/r64 */
        mmio_op->instr = INSTR_MOVSX;
        GET_OP_SIZE_FOR_NONEBYTE(*op_size);
        index = get_index(opcode + 1, rex);
        mmio_op->operand[0] = mk_operand(WORD, 0, 0, MEMORY);
        mmio_op->operand[1] = mk_operand(*op_size, index, 0, REGISTER);
        return DECODE_success;

    case 0xA3: /* bt r32, m32 */
        mmio_op->instr = INSTR_BT;
        index = get_index(opcode + 1, rex);
        *op_size = LONG;
        mmio_op->operand[0] = mk_operand(*op_size, index, 0, REGISTER);
        mmio_op->operand[1] = mk_operand(*op_size, 0, 0, MEMORY);
        return DECODE_success;

    case 0xBA:
        if ( ((opcode[1] >> 3) & 7) == 4 ) /* BT $imm8, m16/32/64 */
        {
            mmio_op->instr = INSTR_BT;
            GET_OP_SIZE_FOR_NONEBYTE(*op_size);
            mmio_op->operand[0] = mk_operand(BYTE, 0, 0, IMMEDIATE);
            mmio_op->immediate =
                    (signed char)get_immediate(*ad_size, opcode + 1, BYTE);
            mmio_op->operand[1] = mk_operand(*op_size, 0, 0, MEMORY);
            return DECODE_success;
        }
        else
        {
            printk("0f %x, This opcode subtype isn't handled yet\n", *opcode);
            return DECODE_failure;
        }

    default:
        printk("0f %x, This opcode isn't handled yet\n", *opcode);
        return DECODE_failure;
    }
}

int inst_copy_from_guest(unsigned char *buf, unsigned long guest_eip, int inst_len)
{
    if ( inst_len > MAX_INST_LEN || inst_len <= 0 )
        return 0;
    if ( hvm_copy_from_guest_virt(buf, guest_eip, inst_len) )
        return 0;
    return inst_len;
}

void send_pio_req(unsigned long port, unsigned long count, int size,
                  paddr_t value, int dir, int df, int value_is_ptr)
{
    struct vcpu *v = current;
    vcpu_iodata_t *vio;
    ioreq_t *p;

    if ( size == 0 || count == 0 ) {
        printk("null pio request? port %lx, count %lx, "
               "size %d, value %"PRIpaddr", dir %d, value_is_ptr %d.\n",
               port, count, size, value, dir, value_is_ptr);
    }

    vio = get_ioreq(v);
    if ( vio == NULL ) {
        printk("bad shared page: %lx\n", (unsigned long) vio);
        domain_crash_synchronous();
    }

    p = &vio->vp_ioreq;
    if ( p->state != STATE_IOREQ_NONE )
        printk("WARNING: send pio with something already pending (%d)?\n",
               p->state);

    p->dir = dir;
    p->data_is_ptr = value_is_ptr;

    p->type = IOREQ_TYPE_PIO;
    p->size = size;
    p->addr = port;
    p->count = count;
    p->df = df;

    p->io_count++;

    p->data = value;

    if ( hvm_portio_intercept(p) )
    {
        p->state = STATE_IORESP_READY;
        hvm_io_assist();
        return;
    }

    hvm_send_assist_req(v);
}

static void send_mmio_req(unsigned char type, unsigned long gpa,
                          unsigned long count, int size, paddr_t value,
                          int dir, int df, int value_is_ptr)
{
    struct vcpu *v = current;
    vcpu_iodata_t *vio;
    ioreq_t *p;

    if ( size == 0 || count == 0 ) {
        printk("null mmio request? type %d, gpa %lx, "
               "count %lx, size %d, value %"PRIpaddr"x, dir %d, "
               "value_is_ptr %d.\n",
               type, gpa, count, size, value, dir, value_is_ptr);
    }

    vio = get_ioreq(v);
    if (vio == NULL) {
        printk("bad shared page\n");
        domain_crash_synchronous();
    }

    p = &vio->vp_ioreq;

    if ( p->state != STATE_IOREQ_NONE )
        printk("WARNING: send mmio with something already pending (%d)?\n",
               p->state);
    p->dir = dir;
    p->data_is_ptr = value_is_ptr;

    p->type = type;
    p->size = size;
    p->addr = gpa;
    p->count = count;
    p->df = df;

    p->io_count++;

    p->data = value;

    if ( hvm_mmio_intercept(p) || hvm_buffered_io_intercept(p) )
    {
        p->state = STATE_IORESP_READY;
        hvm_io_assist();
        return;
    }

    hvm_send_assist_req(v);
}

void send_timeoffset_req(unsigned long timeoff)
{
    ioreq_t p[1];

    if ( timeoff == 0 )
        return;

    memset(p, 0, sizeof(*p));

    p->type = IOREQ_TYPE_TIMEOFFSET;
    p->size = 4;
    p->dir = IOREQ_WRITE;
    p->data = timeoff;

    p->state = STATE_IOREQ_READY;

    if ( !hvm_buffered_io_send(p) )
        printk("Unsuccessful timeoffset update\n");
}

/* Ask ioemu mapcache to invalidate mappings. */
void send_invalidate_req(void)
{
    struct vcpu *v = current;
    vcpu_iodata_t *vio;
    ioreq_t *p;

    vio = get_ioreq(v);
    if ( vio == NULL )
    {
        printk("bad shared page: %lx\n", (unsigned long) vio);
        domain_crash_synchronous();
    }

    p = &vio->vp_ioreq;
    if ( p->state != STATE_IOREQ_NONE )
        printk("WARNING: send invalidate req with something "
               "already pending (%d)?\n", p->state);

    p->type = IOREQ_TYPE_INVALIDATE;
    p->size = 4;
    p->dir = IOREQ_WRITE;
    p->data = ~0UL; /* flush all */
    p->io_count++;

    hvm_send_assist_req(v);
}

static void mmio_operands(int type, unsigned long gpa,
                          struct hvm_io_op *mmio_op,
                          unsigned char op_size)
{
    unsigned long value = 0;
    int df, index, size_reg;
    struct cpu_user_regs *regs = &mmio_op->io_context;

    df = regs->eflags & X86_EFLAGS_DF ? 1 : 0;

    size_reg = operand_size(mmio_op->operand[0]);

    if ( mmio_op->operand[0] & REGISTER ) {            /* dest is memory */
        index = operand_index(mmio_op->operand[0]);
        value = get_reg_value(size_reg, index, 0, regs);
        send_mmio_req(type, gpa, 1, op_size, value, IOREQ_WRITE, df, 0);
    } else if ( mmio_op->operand[0] & IMMEDIATE ) {    /* dest is memory */
        value = mmio_op->immediate;
        send_mmio_req(type, gpa, 1, op_size, value, IOREQ_WRITE, df, 0);
    } else if ( mmio_op->operand[0] & MEMORY ) {       /* dest is register */
        /* send the request and wait for the value */
        if ( (mmio_op->instr == INSTR_MOVZX) ||
             (mmio_op->instr == INSTR_MOVSX) )
            send_mmio_req(type, gpa, 1, size_reg, 0, IOREQ_READ, df, 0);
        else
            send_mmio_req(type, gpa, 1, op_size, 0, IOREQ_READ, df, 0);
    } else {
        printk("%s: invalid dest mode.\n", __func__);
        domain_crash_synchronous();
    }
}

#define GET_REPEAT_COUNT() \
     (mmio_op->flags & REPZ ? (ad_size == WORD ? regs->ecx & 0xFFFF : regs->ecx) : 1)


void handle_mmio(unsigned long gpa)
{
    unsigned long inst_addr;
    struct hvm_io_op *mmio_op;
    struct cpu_user_regs *regs;
    unsigned char inst[MAX_INST_LEN], ad_size, op_size, seg_sel;
    int i, address_bytes, df, inst_len;
    struct vcpu *v = current;

    mmio_op = &v->arch.hvm_vcpu.io_op;
    regs = &mmio_op->io_context;

    /* Copy current guest state into io instruction state structure. */
    memcpy(regs, guest_cpu_user_regs(), HVM_CONTEXT_STACK_BYTES);
    hvm_store_cpu_guest_regs(v, regs, NULL);

    df = regs->eflags & X86_EFLAGS_DF ? 1 : 0;

    address_bytes = hvm_guest_x86_mode(v);
    if (address_bytes < 2)
        /* real or vm86 modes */
        address_bytes = 2;
    inst_addr = hvm_get_segment_base(v, x86_seg_cs) + regs->eip;
    memset(inst, 0, MAX_INST_LEN);
    inst_len = hvm_instruction_fetch(inst_addr, address_bytes, inst);
    if ( inst_len <= 0 )
    {
        gdprintk(XENLOG_DEBUG, "handle_mmio: failed to get instruction\n");
        /* hvm_instruction_fetch() will have injected a #PF; get out now */
        return;
    }

    if ( mmio_decode(address_bytes, inst, mmio_op, &ad_size,
                     &op_size, &seg_sel) == DECODE_failure ) {
        printk("handle_mmio: failed to decode instruction\n");
        printk("mmio opcode: gpa 0x%lx, len %d:", gpa, inst_len);
        for ( i = 0; i < inst_len; i++ )
            printk(" %02x", inst[i] & 0xFF);
        printk("\n");
        domain_crash_synchronous();
    }

    regs->eip += inst_len; /* advance %eip */
    regs->eflags &= ~X86_EFLAGS_RF;

    switch ( mmio_op->instr ) {
    case INSTR_MOV:
        mmio_operands(IOREQ_TYPE_COPY, gpa, mmio_op, op_size);
        break;

    case INSTR_MOVS:
    {
        unsigned long count = GET_REPEAT_COUNT();
        int sign = regs->eflags & X86_EFLAGS_DF ? -1 : 1;
        unsigned long addr, gfn; 
        paddr_t paddr;
        int dir, size = op_size;

        ASSERT(count);

        /* determine non-MMIO address */
        addr = regs->edi;
        if ( ad_size == WORD )
            addr &= 0xFFFF;
        addr += hvm_get_segment_base(v, x86_seg_es);
        gfn = paging_gva_to_gfn(v, addr);
        paddr = (paddr_t)gfn << PAGE_SHIFT | (addr & ~PAGE_MASK);
        if ( paddr == gpa )
        {
            enum x86_segment seg;

            dir = IOREQ_WRITE;
            addr = regs->esi;
            if ( ad_size == WORD )
                addr &= 0xFFFF;
            switch ( seg_sel )
            {
            case 0x26: seg = x86_seg_es; break;
            case 0x2e: seg = x86_seg_cs; break;
            case 0x36: seg = x86_seg_ss; break;
            case 0:
            case 0x3e: seg = x86_seg_ds; break;
            case 0x64: seg = x86_seg_fs; break;
            case 0x65: seg = x86_seg_gs; break;
            default: domain_crash_synchronous();
            }
            addr += hvm_get_segment_base(v, seg);
            gfn = paging_gva_to_gfn(v, addr);
            paddr = (paddr_t)gfn << PAGE_SHIFT | (addr & ~PAGE_MASK);
        }
        else
            dir = IOREQ_READ;

        if ( gfn == INVALID_GFN ) 
        {
            /* The guest does not have the non-mmio address mapped. 
             * Need to send in a page fault */
            int errcode = 0;
            /* IO read --> memory write */
            if ( dir == IOREQ_READ ) errcode |= PFEC_write_access;
            regs->eip -= inst_len; /* do not advance %eip */
            regs->eflags |= X86_EFLAGS_RF; /* RF was set by original #PF */
            hvm_inject_exception(TRAP_page_fault, errcode, addr);
            return;
        }

        /*
         * In case of a movs spanning multiple pages, we break the accesses
         * up into multiple pages (the device model works with non-continguous
         * physical guest pages). To copy just one page, we adjust %ecx and
         * do not advance %eip so that the next rep;movs copies the next page.
         * Unaligned accesses, for example movsl starting at PGSZ-2, are
         * turned into a single copy where we handle the overlapping memory
         * copy ourself. After this copy succeeds, "rep movs" is executed
         * again.
         */
        if ( (addr & PAGE_MASK) != ((addr + size - 1) & PAGE_MASK) ) {
            unsigned long value = 0;

            mmio_op->flags |= OVERLAP;

            if ( dir == IOREQ_WRITE ) {
                if ( hvm_paging_enabled(v) )
                {
                    int rv = hvm_copy_from_guest_virt(&value, addr, size);
                    if ( rv != 0 ) 
                    {
                        /* Failed on the page-spanning copy.  Inject PF into
                         * the guest for the address where we failed */
                        regs->eip -= inst_len; /* do not advance %eip */
                        regs->eflags |= X86_EFLAGS_RF; /* RF was set by #PF */
                        /* Must set CR2 at the failing address */ 
                        addr += size - rv;
                        gdprintk(XENLOG_DEBUG, "Pagefault on non-io side of a "
                                 "page-spanning MMIO: va=%#lx\n", addr);
                        hvm_inject_exception(TRAP_page_fault, 0, addr);
                        return;
                    }
                }
                else
                    (void) hvm_copy_from_guest_phys(&value, addr, size);
            } else /* dir != IOREQ_WRITE */
                /* Remember where to write the result, as a *VA*.
                 * Must be a VA so we can handle the page overlap 
                 * correctly in hvm_mmio_assist() */
                mmio_op->addr = addr;

            if ( count != 1 )
                regs->eip -= inst_len; /* do not advance %eip */

            send_mmio_req(IOREQ_TYPE_COPY, gpa, 1, size, value, dir, df, 0);
        } else {
            unsigned long last_addr = sign > 0 ? addr + count * size - 1
                                               : addr - (count - 1) * size;

            if ( (addr & PAGE_MASK) != (last_addr & PAGE_MASK) )
            {
                regs->eip -= inst_len; /* do not advance %eip */

                if ( sign > 0 )
                    count = (PAGE_SIZE - (addr & ~PAGE_MASK)) / size;
                else
                    count = (addr & ~PAGE_MASK) / size + 1;
            }

            ASSERT(count);

            send_mmio_req(IOREQ_TYPE_COPY, gpa, count, size, 
                          paddr, dir, df, 1);
        }
        break;
    }

    case INSTR_MOVZX:
    case INSTR_MOVSX:
        mmio_operands(IOREQ_TYPE_COPY, gpa, mmio_op, op_size);
        break;

    case INSTR_STOS:
        /*
         * Since the destination is always in (contiguous) mmio space we don't
         * need to break it up into pages.
         */
        send_mmio_req(IOREQ_TYPE_COPY, gpa,
                      GET_REPEAT_COUNT(), op_size, regs->eax, IOREQ_WRITE, df, 0);
        break;

    case INSTR_LODS:
        /*
         * Since the source is always in (contiguous) mmio space we don't
         * need to break it up into pages.
         */
        mmio_op->operand[0] = mk_operand(op_size, 0, 0, REGISTER);
        send_mmio_req(IOREQ_TYPE_COPY, gpa,
                      GET_REPEAT_COUNT(), op_size, 0, IOREQ_READ, df, 0);
        break;

    case INSTR_OR:
        mmio_operands(IOREQ_TYPE_OR, gpa, mmio_op, op_size);
        break;

    case INSTR_AND:
        mmio_operands(IOREQ_TYPE_AND, gpa, mmio_op, op_size);
        break;

    case INSTR_ADD:
        mmio_operands(IOREQ_TYPE_ADD, gpa, mmio_op, op_size);
        break;

    case INSTR_SUB:
        mmio_operands(IOREQ_TYPE_SUB, gpa, mmio_op, op_size);
        break;

    case INSTR_XOR:
        mmio_operands(IOREQ_TYPE_XOR, gpa, mmio_op, op_size);
        break;

    case INSTR_PUSH:
        if ( ad_size == WORD )
        {
            mmio_op->addr = (uint16_t)(regs->esp - op_size);
            regs->esp = mmio_op->addr | (regs->esp & ~0xffff);
        }
        else
        {
            regs->esp -= op_size;
            mmio_op->addr = regs->esp;
        }
        /* send the request and wait for the value */
        send_mmio_req(IOREQ_TYPE_COPY, gpa, 1, op_size, 0, IOREQ_READ, df, 0);
        break;

    case INSTR_CMP:        /* Pass through */
    case INSTR_TEST:
        /* send the request and wait for the value */
        send_mmio_req(IOREQ_TYPE_COPY, gpa, 1, op_size, 0, IOREQ_READ, df, 0);
        break;

    case INSTR_BT:
    {
        unsigned long value = 0;
        int index, size;

        if ( mmio_op->operand[0] & REGISTER )
        {
            index = operand_index(mmio_op->operand[0]);
            size = operand_size(mmio_op->operand[0]);
            value = get_reg_value(size, index, 0, regs);
        }
        else if ( mmio_op->operand[0] & IMMEDIATE )
        {
            mmio_op->immediate = mmio_op->immediate;
            value = mmio_op->immediate;
        }
        send_mmio_req(IOREQ_TYPE_COPY, gpa + (value >> 5), 1,
                      op_size, 0, IOREQ_READ, df, 0);
        break;
    }

    case INSTR_XCHG:
        if ( mmio_op->operand[0] & REGISTER ) {
            long value;
            unsigned long operand = mmio_op->operand[0];
            value = get_reg_value(operand_size(operand),
                                  operand_index(operand), 0,
                                  regs);
            /* send the request and wait for the value */
            send_mmio_req(IOREQ_TYPE_XCHG, gpa, 1,
                          op_size, value, IOREQ_WRITE, df, 0);
        } else {
            /* the destination is a register */
            long value;
            unsigned long operand = mmio_op->operand[1];
            value = get_reg_value(operand_size(operand),
                                  operand_index(operand), 0,
                                  regs);
            /* send the request and wait for the value */
            send_mmio_req(IOREQ_TYPE_XCHG, gpa, 1,
                          op_size, value, IOREQ_WRITE, df, 0);
        }
        break;

    default:
        printk("Unhandled MMIO instruction\n");
        domain_crash_synchronous();
    }
}

DEFINE_PER_CPU(int, guest_handles_in_xen_space);

/* Note that copy_{to,from}_user_hvm don't set the A and D bits on
   PTEs, and require the PTE to be writable even when they're only
   trying to read from it.  The guest is expected to deal with
   this. */
unsigned long copy_to_user_hvm(void *to, const void *from, unsigned len)
{
    if ( this_cpu(guest_handles_in_xen_space) )
    {
        memcpy(to, from, len);
        return 0;
    }

    return hvm_copy_to_guest_virt((unsigned long)to, (void *)from, len);
}

unsigned long copy_from_user_hvm(void *to, const void *from, unsigned len)
{
    if ( this_cpu(guest_handles_in_xen_space) )
    {
        memcpy(to, from, len);
        return 0;
    }

    return hvm_copy_from_guest_virt(to, (unsigned long)from, len);
}

/*
 * Local variables:
 * mode: C
 * c-set-style: "BSD"
 * c-basic-offset: 4
 * tab-width: 4
 * indent-tabs-mode: nil
 * End:
 */