1// SPDX-License-Identifier: GPL-2.0-or-later
  2/*
  3 *  Fault Injection Test harness (FI)
  4 *  Copyright (C) Intel Crop.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
  5 */
  6
  7/*  Id: pf_in.c,v 1.1.1.1 2002/11/12 05:56:32 brlock Exp
  8 *  Copyright by Intel Crop., 2002
  9 *  Louis Zhuang (louis.zhuang@intel.com)
 10 *
 11 *  Bjorn Steinbrink (B.Steinbrink@gmx.de), 2007
 12 */
 13
 14#include <linux/ptrace.h> /* struct pt_regs */
 15#include "pf_in.h"
 16
 17#ifdef __i386__
 18/* IA32 Manual 3, 2-1 */
 19static unsigned char prefix_codes[] = {
 20	0xF0, 0xF2, 0xF3, 0x2E, 0x36, 0x3E, 0x26, 0x64,
 21	0x65, 0x66, 0x67
 22};
 23/* IA32 Manual 3, 3-432*/
 24static unsigned int reg_rop[] = {
 25	0x8A, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F
 26};
 27static unsigned int reg_wop[] = { 0x88, 0x89, 0xAA, 0xAB };
 28static unsigned int imm_wop[] = { 0xC6, 0xC7 };
 29/* IA32 Manual 3, 3-432*/
 30static unsigned int rw8[] = { 0x88, 0x8A, 0xC6, 0xAA };
 31static unsigned int rw32[] = {
 32	0x89, 0x8B, 0xC7, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F, 0xAB
 33};
 34static unsigned int mw8[] = { 0x88, 0x8A, 0xC6, 0xB60F, 0xBE0F, 0xAA };
 35static unsigned int mw16[] = { 0xB70F, 0xBF0F };
 36static unsigned int mw32[] = { 0x89, 0x8B, 0xC7, 0xAB };
 37static unsigned int mw64[] = {};
 38#else /* not __i386__ */
 39static unsigned char prefix_codes[] = {
 40	0x66, 0x67, 0x2E, 0x3E, 0x26, 0x64, 0x65, 0x36,
 41	0xF0, 0xF3, 0xF2,
 42	/* REX Prefixes */
 43	0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47,
 44	0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f
 45};
 46/* AMD64 Manual 3, Appendix A*/
 47static unsigned int reg_rop[] = {
 48	0x8A, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F
 49};
 50static unsigned int reg_wop[] = { 0x88, 0x89, 0xAA, 0xAB };
 51static unsigned int imm_wop[] = { 0xC6, 0xC7 };
 52static unsigned int rw8[] = { 0xC6, 0x88, 0x8A, 0xAA };
 53static unsigned int rw32[] = {
 54	0xC7, 0x89, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F, 0xAB
 55};
 56/* 8 bit only */
 57static unsigned int mw8[] = { 0xC6, 0x88, 0x8A, 0xB60F, 0xBE0F, 0xAA };
 58/* 16 bit only */
 59static unsigned int mw16[] = { 0xB70F, 0xBF0F };
 60/* 16 or 32 bit */
 61static unsigned int mw32[] = { 0xC7 };
 62/* 16, 32 or 64 bit */
 63static unsigned int mw64[] = { 0x89, 0x8B, 0xAB };
 64#endif /* not __i386__ */
 65
 66struct prefix_bits {
 67	unsigned shorted:1;
 68	unsigned enlarged:1;
 69	unsigned rexr:1;
 70	unsigned rex:1;
 71};
 72
 73static int skip_prefix(unsigned char *addr, struct prefix_bits *prf)
 74{
 75	int i;
 76	unsigned char *p = addr;
 77	prf->shorted = 0;
 78	prf->enlarged = 0;
 79	prf->rexr = 0;
 80	prf->rex = 0;
 81
 82restart:
 83	for (i = 0; i < ARRAY_SIZE(prefix_codes); i++) {
 84		if (*p == prefix_codes[i]) {
 85			if (*p == 0x66)
 86				prf->shorted = 1;
 87#ifdef __amd64__
 88			if ((*p & 0xf8) == 0x48)
 89				prf->enlarged = 1;
 90			if ((*p & 0xf4) == 0x44)
 91				prf->rexr = 1;
 92			if ((*p & 0xf0) == 0x40)
 93				prf->rex = 1;
 94#endif
 95			p++;
 96			goto restart;
 97		}
 98	}
 99
100	return (p - addr);
101}
102
103static int get_opcode(unsigned char *addr, unsigned int *opcode)
104{
105	int len;
106
107	if (*addr == 0x0F) {
108		/* 0x0F is extension instruction */
109		*opcode = *(unsigned short *)addr;
110		len = 2;
111	} else {
112		*opcode = *addr;
113		len = 1;
114	}
115
116	return len;
117}
118
119#define CHECK_OP_TYPE(opcode, array, type) \
120	for (i = 0; i < ARRAY_SIZE(array); i++) { \
121		if (array[i] == opcode) { \
122			rv = type; \
123			goto exit; \
124		} \
125	}
126
127enum reason_type get_ins_type(unsigned long ins_addr)
128{
129	unsigned int opcode;
130	unsigned char *p;
131	struct prefix_bits prf;
132	int i;
133	enum reason_type rv = OTHERS;
134
135	p = (unsigned char *)ins_addr;
136	p += skip_prefix(p, &prf);
137	p += get_opcode(p, &opcode);
138
139	CHECK_OP_TYPE(opcode, reg_rop, REG_READ);
140	CHECK_OP_TYPE(opcode, reg_wop, REG_WRITE);
141	CHECK_OP_TYPE(opcode, imm_wop, IMM_WRITE);
142
143exit:
144	return rv;
145}
146#undef CHECK_OP_TYPE
147
148static unsigned int get_ins_reg_width(unsigned long ins_addr)
149{
150	unsigned int opcode;
151	unsigned char *p;
152	struct prefix_bits prf;
153	int i;
154
155	p = (unsigned char *)ins_addr;
156	p += skip_prefix(p, &prf);
157	p += get_opcode(p, &opcode);
158
159	for (i = 0; i < ARRAY_SIZE(rw8); i++)
160		if (rw8[i] == opcode)
161			return 1;
162
163	for (i = 0; i < ARRAY_SIZE(rw32); i++)
164		if (rw32[i] == opcode)
165			return prf.shorted ? 2 : (prf.enlarged ? 8 : 4);
166
167	printk(KERN_ERR "mmiotrace: Unknown opcode 0x%02x\n", opcode);
168	return 0;
169}
170
171unsigned int get_ins_mem_width(unsigned long ins_addr)
172{
173	unsigned int opcode;
174	unsigned char *p;
175	struct prefix_bits prf;
176	int i;
177
178	p = (unsigned char *)ins_addr;
179	p += skip_prefix(p, &prf);
180	p += get_opcode(p, &opcode);
181
182	for (i = 0; i < ARRAY_SIZE(mw8); i++)
183		if (mw8[i] == opcode)
184			return 1;
185
186	for (i = 0; i < ARRAY_SIZE(mw16); i++)
187		if (mw16[i] == opcode)
188			return 2;
189
190	for (i = 0; i < ARRAY_SIZE(mw32); i++)
191		if (mw32[i] == opcode)
192			return prf.shorted ? 2 : 4;
193
194	for (i = 0; i < ARRAY_SIZE(mw64); i++)
195		if (mw64[i] == opcode)
196			return prf.shorted ? 2 : (prf.enlarged ? 8 : 4);
197
198	printk(KERN_ERR "mmiotrace: Unknown opcode 0x%02x\n", opcode);
199	return 0;
200}
201
202/*
203 * Define register ident in mod/rm byte.
204 * Note: these are NOT the same as in ptrace-abi.h.
205 */
206enum {
207	arg_AL = 0,
208	arg_CL = 1,
209	arg_DL = 2,
210	arg_BL = 3,
211	arg_AH = 4,
212	arg_CH = 5,
213	arg_DH = 6,
214	arg_BH = 7,
215
216	arg_AX = 0,
217	arg_CX = 1,
218	arg_DX = 2,
219	arg_BX = 3,
220	arg_SP = 4,
221	arg_BP = 5,
222	arg_SI = 6,
223	arg_DI = 7,
224#ifdef __amd64__
225	arg_R8  = 8,
226	arg_R9  = 9,
227	arg_R10 = 10,
228	arg_R11 = 11,
229	arg_R12 = 12,
230	arg_R13 = 13,
231	arg_R14 = 14,
232	arg_R15 = 15
233#endif
234};
235
236static unsigned char *get_reg_w8(int no, int rex, struct pt_regs *regs)
237{
238	unsigned char *rv = NULL;
239
240	switch (no) {
241	case arg_AL:
242		rv = (unsigned char *)&regs->ax;
243		break;
244	case arg_BL:
245		rv = (unsigned char *)&regs->bx;
246		break;
247	case arg_CL:
248		rv = (unsigned char *)&regs->cx;
249		break;
250	case arg_DL:
251		rv = (unsigned char *)&regs->dx;
252		break;
253#ifdef __amd64__
254	case arg_R8:
255		rv = (unsigned char *)&regs->r8;
256		break;
257	case arg_R9:
258		rv = (unsigned char *)&regs->r9;
259		break;
260	case arg_R10:
261		rv = (unsigned char *)&regs->r10;
262		break;
263	case arg_R11:
264		rv = (unsigned char *)&regs->r11;
265		break;
266	case arg_R12:
267		rv = (unsigned char *)&regs->r12;
268		break;
269	case arg_R13:
270		rv = (unsigned char *)&regs->r13;
271		break;
272	case arg_R14:
273		rv = (unsigned char *)&regs->r14;
274		break;
275	case arg_R15:
276		rv = (unsigned char *)&regs->r15;
277		break;
278#endif
279	default:
280		break;
281	}
282
283	if (rv)
284		return rv;
285
286	if (rex) {
287		/*
288		 * If REX prefix exists, access low bytes of SI etc.
289		 * instead of AH etc.
290		 */
291		switch (no) {
292		case arg_SI:
293			rv = (unsigned char *)&regs->si;
294			break;
295		case arg_DI:
296			rv = (unsigned char *)&regs->di;
297			break;
298		case arg_BP:
299			rv = (unsigned char *)&regs->bp;
300			break;
301		case arg_SP:
302			rv = (unsigned char *)&regs->sp;
303			break;
304		default:
305			break;
306		}
307	} else {
308		switch (no) {
309		case arg_AH:
310			rv = 1 + (unsigned char *)&regs->ax;
311			break;
312		case arg_BH:
313			rv = 1 + (unsigned char *)&regs->bx;
314			break;
315		case arg_CH:
316			rv = 1 + (unsigned char *)&regs->cx;
317			break;
318		case arg_DH:
319			rv = 1 + (unsigned char *)&regs->dx;
320			break;
321		default:
322			break;
323		}
324	}
325
326	if (!rv)
327		printk(KERN_ERR "mmiotrace: Error reg no# %d\n", no);
328
329	return rv;
330}
331
332static unsigned long *get_reg_w32(int no, struct pt_regs *regs)
333{
334	unsigned long *rv = NULL;
335
336	switch (no) {
337	case arg_AX:
338		rv = &regs->ax;
339		break;
340	case arg_BX:
341		rv = &regs->bx;
342		break;
343	case arg_CX:
344		rv = &regs->cx;
345		break;
346	case arg_DX:
347		rv = &regs->dx;
348		break;
349	case arg_SP:
350		rv = &regs->sp;
351		break;
352	case arg_BP:
353		rv = &regs->bp;
354		break;
355	case arg_SI:
356		rv = &regs->si;
357		break;
358	case arg_DI:
359		rv = &regs->di;
360		break;
361#ifdef __amd64__
362	case arg_R8:
363		rv = &regs->r8;
364		break;
365	case arg_R9:
366		rv = &regs->r9;
367		break;
368	case arg_R10:
369		rv = &regs->r10;
370		break;
371	case arg_R11:
372		rv = &regs->r11;
373		break;
374	case arg_R12:
375		rv = &regs->r12;
376		break;
377	case arg_R13:
378		rv = &regs->r13;
379		break;
380	case arg_R14:
381		rv = &regs->r14;
382		break;
383	case arg_R15:
384		rv = &regs->r15;
385		break;
386#endif
387	default:
388		printk(KERN_ERR "mmiotrace: Error reg no# %d\n", no);
389	}
390
391	return rv;
392}
393
394unsigned long get_ins_reg_val(unsigned long ins_addr, struct pt_regs *regs)
395{
396	unsigned int opcode;
397	int reg;
398	unsigned char *p;
399	struct prefix_bits prf;
400	int i;
401
402	p = (unsigned char *)ins_addr;
403	p += skip_prefix(p, &prf);
404	p += get_opcode(p, &opcode);
405	for (i = 0; i < ARRAY_SIZE(reg_rop); i++)
406		if (reg_rop[i] == opcode)
407			goto do_work;
408
409	for (i = 0; i < ARRAY_SIZE(reg_wop); i++)
410		if (reg_wop[i] == opcode)
411			goto do_work;
412
413	printk(KERN_ERR "mmiotrace: Not a register instruction, opcode "
414							"0x%02x\n", opcode);
415	goto err;
416
417do_work:
418	/* for STOS, source register is fixed */
419	if (opcode == 0xAA || opcode == 0xAB) {
420		reg = arg_AX;
421	} else {
422		unsigned char mod_rm = *p;
423		reg = ((mod_rm >> 3) & 0x7) | (prf.rexr << 3);
424	}
425	switch (get_ins_reg_width(ins_addr)) {
426	case 1:
427		return *get_reg_w8(reg, prf.rex, regs);
428
429	case 2:
430		return *(unsigned short *)get_reg_w32(reg, regs);
431
432	case 4:
433		return *(unsigned int *)get_reg_w32(reg, regs);
434
435#ifdef __amd64__
436	case 8:
437		return *(unsigned long *)get_reg_w32(reg, regs);
438#endif
439
440	default:
441		printk(KERN_ERR "mmiotrace: Error width# %d\n", reg);
442	}
443
444err:
445	return 0;
446}
447
448unsigned long get_ins_imm_val(unsigned long ins_addr)
449{
450	unsigned int opcode;
451	unsigned char mod_rm;
452	unsigned char mod;
453	unsigned char *p;
454	struct prefix_bits prf;
455	int i;
456
457	p = (unsigned char *)ins_addr;
458	p += skip_prefix(p, &prf);
459	p += get_opcode(p, &opcode);
460	for (i = 0; i < ARRAY_SIZE(imm_wop); i++)
461		if (imm_wop[i] == opcode)
462			goto do_work;
463
464	printk(KERN_ERR "mmiotrace: Not an immediate instruction, opcode "
465							"0x%02x\n", opcode);
466	goto err;
467
468do_work:
469	mod_rm = *p;
470	mod = mod_rm >> 6;
471	p++;
472	switch (mod) {
473	case 0:
474		/* if r/m is 5 we have a 32 disp (IA32 Manual 3, Table 2-2)  */
475		/* AMD64: XXX Check for address size prefix? */
476		if ((mod_rm & 0x7) == 0x5)
477			p += 4;
478		break;
479
480	case 1:
481		p += 1;
482		break;
483
484	case 2:
485		p += 4;
486		break;
487
488	case 3:
489	default:
490		printk(KERN_ERR "mmiotrace: not a memory access instruction "
491						"at 0x%lx, rm_mod=0x%02x\n",
492						ins_addr, mod_rm);
493	}
494
495	switch (get_ins_reg_width(ins_addr)) {
496	case 1:
497		return *(unsigned char *)p;
498
499	case 2:
500		return *(unsigned short *)p;
501
502	case 4:
503		return *(unsigned int *)p;
504
505#ifdef __amd64__
506	case 8:
507		return *(unsigned long *)p;
508#endif
509
510	default:
511		printk(KERN_ERR "mmiotrace: Error: width.\n");
512	}
513
514err:
515	return 0;
516}

 
  1/*
  2 *  Fault Injection Test harness (FI)
  3 *  Copyright (C) Intel Crop.
  4 *
  5 *  This program is free software; you can redistribute it and/or
  6 *  modify it under the terms of the GNU General Public License
  7 *  as published by the Free Software Foundation; either version 2
  8 *  of the License, or (at your option) any later version.
  9 *
 10 *  This program is distributed in the hope that it will be useful,
 11 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 *  GNU General Public License for more details.
 14 *
 15 *  You should have received a copy of the GNU General Public License
 16 *  along with this program; if not, write to the Free Software
 17 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307,
 18 *  USA.
 19 *
 20 */
 21
 22/*  Id: pf_in.c,v 1.1.1.1 2002/11/12 05:56:32 brlock Exp
 23 *  Copyright by Intel Crop., 2002
 24 *  Louis Zhuang (louis.zhuang@intel.com)
 25 *
 26 *  Bjorn Steinbrink (B.Steinbrink@gmx.de), 2007
 27 */
 28
 29#include <linux/ptrace.h> /* struct pt_regs */
 30#include "pf_in.h"
 31
 32#ifdef __i386__
 33/* IA32 Manual 3, 2-1 */
 34static unsigned char prefix_codes[] = {
 35	0xF0, 0xF2, 0xF3, 0x2E, 0x36, 0x3E, 0x26, 0x64,
 36	0x65, 0x66, 0x67
 37};
 38/* IA32 Manual 3, 3-432*/
 39static unsigned int reg_rop[] = {
 40	0x8A, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F
 41};
 42static unsigned int reg_wop[] = { 0x88, 0x89, 0xAA, 0xAB };
 43static unsigned int imm_wop[] = { 0xC6, 0xC7 };
 44/* IA32 Manual 3, 3-432*/
 45static unsigned int rw8[] = { 0x88, 0x8A, 0xC6, 0xAA };
 46static unsigned int rw32[] = {
 47	0x89, 0x8B, 0xC7, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F, 0xAB
 48};
 49static unsigned int mw8[] = { 0x88, 0x8A, 0xC6, 0xB60F, 0xBE0F, 0xAA };
 50static unsigned int mw16[] = { 0xB70F, 0xBF0F };
 51static unsigned int mw32[] = { 0x89, 0x8B, 0xC7, 0xAB };
 52static unsigned int mw64[] = {};
 53#else /* not __i386__ */
 54static unsigned char prefix_codes[] = {
 55	0x66, 0x67, 0x2E, 0x3E, 0x26, 0x64, 0x65, 0x36,
 56	0xF0, 0xF3, 0xF2,
 57	/* REX Prefixes */
 58	0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47,
 59	0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f
 60};
 61/* AMD64 Manual 3, Appendix A*/
 62static unsigned int reg_rop[] = {
 63	0x8A, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F
 64};
 65static unsigned int reg_wop[] = { 0x88, 0x89, 0xAA, 0xAB };
 66static unsigned int imm_wop[] = { 0xC6, 0xC7 };
 67static unsigned int rw8[] = { 0xC6, 0x88, 0x8A, 0xAA };
 68static unsigned int rw32[] = {
 69	0xC7, 0x89, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F, 0xAB
 70};
 71/* 8 bit only */
 72static unsigned int mw8[] = { 0xC6, 0x88, 0x8A, 0xB60F, 0xBE0F, 0xAA };
 73/* 16 bit only */
 74static unsigned int mw16[] = { 0xB70F, 0xBF0F };
 75/* 16 or 32 bit */
 76static unsigned int mw32[] = { 0xC7 };
 77/* 16, 32 or 64 bit */
 78static unsigned int mw64[] = { 0x89, 0x8B, 0xAB };
 79#endif /* not __i386__ */
 80
 81struct prefix_bits {
 82	unsigned shorted:1;
 83	unsigned enlarged:1;
 84	unsigned rexr:1;
 85	unsigned rex:1;
 86};
 87
 88static int skip_prefix(unsigned char *addr, struct prefix_bits *prf)
 89{
 90	int i;
 91	unsigned char *p = addr;
 92	prf->shorted = 0;
 93	prf->enlarged = 0;
 94	prf->rexr = 0;
 95	prf->rex = 0;
 96
 97restart:
 98	for (i = 0; i < ARRAY_SIZE(prefix_codes); i++) {
 99		if (*p == prefix_codes[i]) {
100			if (*p == 0x66)
101				prf->shorted = 1;
102#ifdef __amd64__
103			if ((*p & 0xf8) == 0x48)
104				prf->enlarged = 1;
105			if ((*p & 0xf4) == 0x44)
106				prf->rexr = 1;
107			if ((*p & 0xf0) == 0x40)
108				prf->rex = 1;
109#endif
110			p++;
111			goto restart;
112		}
113	}
114
115	return (p - addr);
116}
117
118static int get_opcode(unsigned char *addr, unsigned int *opcode)
119{
120	int len;
121
122	if (*addr == 0x0F) {
123		/* 0x0F is extension instruction */
124		*opcode = *(unsigned short *)addr;
125		len = 2;
126	} else {
127		*opcode = *addr;
128		len = 1;
129	}
130
131	return len;
132}
133
134#define CHECK_OP_TYPE(opcode, array, type) \
135	for (i = 0; i < ARRAY_SIZE(array); i++) { \
136		if (array[i] == opcode) { \
137			rv = type; \
138			goto exit; \
139		} \
140	}
141
142enum reason_type get_ins_type(unsigned long ins_addr)
143{
144	unsigned int opcode;
145	unsigned char *p;
146	struct prefix_bits prf;
147	int i;
148	enum reason_type rv = OTHERS;
149
150	p = (unsigned char *)ins_addr;
151	p += skip_prefix(p, &prf);
152	p += get_opcode(p, &opcode);
153
154	CHECK_OP_TYPE(opcode, reg_rop, REG_READ);
155	CHECK_OP_TYPE(opcode, reg_wop, REG_WRITE);
156	CHECK_OP_TYPE(opcode, imm_wop, IMM_WRITE);
157
158exit:
159	return rv;
160}
161#undef CHECK_OP_TYPE
162
163static unsigned int get_ins_reg_width(unsigned long ins_addr)
164{
165	unsigned int opcode;
166	unsigned char *p;
167	struct prefix_bits prf;
168	int i;
169
170	p = (unsigned char *)ins_addr;
171	p += skip_prefix(p, &prf);
172	p += get_opcode(p, &opcode);
173
174	for (i = 0; i < ARRAY_SIZE(rw8); i++)
175		if (rw8[i] == opcode)
176			return 1;
177
178	for (i = 0; i < ARRAY_SIZE(rw32); i++)
179		if (rw32[i] == opcode)
180			return prf.shorted ? 2 : (prf.enlarged ? 8 : 4);
181
182	printk(KERN_ERR "mmiotrace: Unknown opcode 0x%02x\n", opcode);
183	return 0;
184}
185
186unsigned int get_ins_mem_width(unsigned long ins_addr)
187{
188	unsigned int opcode;
189	unsigned char *p;
190	struct prefix_bits prf;
191	int i;
192
193	p = (unsigned char *)ins_addr;
194	p += skip_prefix(p, &prf);
195	p += get_opcode(p, &opcode);
196
197	for (i = 0; i < ARRAY_SIZE(mw8); i++)
198		if (mw8[i] == opcode)
199			return 1;
200
201	for (i = 0; i < ARRAY_SIZE(mw16); i++)
202		if (mw16[i] == opcode)
203			return 2;
204
205	for (i = 0; i < ARRAY_SIZE(mw32); i++)
206		if (mw32[i] == opcode)
207			return prf.shorted ? 2 : 4;
208
209	for (i = 0; i < ARRAY_SIZE(mw64); i++)
210		if (mw64[i] == opcode)
211			return prf.shorted ? 2 : (prf.enlarged ? 8 : 4);
212
213	printk(KERN_ERR "mmiotrace: Unknown opcode 0x%02x\n", opcode);
214	return 0;
215}
216
217/*
218 * Define register ident in mod/rm byte.
219 * Note: these are NOT the same as in ptrace-abi.h.
220 */
221enum {
222	arg_AL = 0,
223	arg_CL = 1,
224	arg_DL = 2,
225	arg_BL = 3,
226	arg_AH = 4,
227	arg_CH = 5,
228	arg_DH = 6,
229	arg_BH = 7,
230
231	arg_AX = 0,
232	arg_CX = 1,
233	arg_DX = 2,
234	arg_BX = 3,
235	arg_SP = 4,
236	arg_BP = 5,
237	arg_SI = 6,
238	arg_DI = 7,
239#ifdef __amd64__
240	arg_R8  = 8,
241	arg_R9  = 9,
242	arg_R10 = 10,
243	arg_R11 = 11,
244	arg_R12 = 12,
245	arg_R13 = 13,
246	arg_R14 = 14,
247	arg_R15 = 15
248#endif
249};
250
251static unsigned char *get_reg_w8(int no, int rex, struct pt_regs *regs)
252{
253	unsigned char *rv = NULL;
254
255	switch (no) {
256	case arg_AL:
257		rv = (unsigned char *)&regs->ax;
258		break;
259	case arg_BL:
260		rv = (unsigned char *)&regs->bx;
261		break;
262	case arg_CL:
263		rv = (unsigned char *)&regs->cx;
264		break;
265	case arg_DL:
266		rv = (unsigned char *)&regs->dx;
267		break;
268#ifdef __amd64__
269	case arg_R8:
270		rv = (unsigned char *)&regs->r8;
271		break;
272	case arg_R9:
273		rv = (unsigned char *)&regs->r9;
274		break;
275	case arg_R10:
276		rv = (unsigned char *)&regs->r10;
277		break;
278	case arg_R11:
279		rv = (unsigned char *)&regs->r11;
280		break;
281	case arg_R12:
282		rv = (unsigned char *)&regs->r12;
283		break;
284	case arg_R13:
285		rv = (unsigned char *)&regs->r13;
286		break;
287	case arg_R14:
288		rv = (unsigned char *)&regs->r14;
289		break;
290	case arg_R15:
291		rv = (unsigned char *)&regs->r15;
292		break;
293#endif
294	default:
295		break;
296	}
297
298	if (rv)
299		return rv;
300
301	if (rex) {
302		/*
303		 * If REX prefix exists, access low bytes of SI etc.
304		 * instead of AH etc.
305		 */
306		switch (no) {
307		case arg_SI:
308			rv = (unsigned char *)&regs->si;
309			break;
310		case arg_DI:
311			rv = (unsigned char *)&regs->di;
312			break;
313		case arg_BP:
314			rv = (unsigned char *)&regs->bp;
315			break;
316		case arg_SP:
317			rv = (unsigned char *)&regs->sp;
318			break;
319		default:
320			break;
321		}
322	} else {
323		switch (no) {
324		case arg_AH:
325			rv = 1 + (unsigned char *)&regs->ax;
326			break;
327		case arg_BH:
328			rv = 1 + (unsigned char *)&regs->bx;
329			break;
330		case arg_CH:
331			rv = 1 + (unsigned char *)&regs->cx;
332			break;
333		case arg_DH:
334			rv = 1 + (unsigned char *)&regs->dx;
335			break;
336		default:
337			break;
338		}
339	}
340
341	if (!rv)
342		printk(KERN_ERR "mmiotrace: Error reg no# %d\n", no);
343
344	return rv;
345}
346
347static unsigned long *get_reg_w32(int no, struct pt_regs *regs)
348{
349	unsigned long *rv = NULL;
350
351	switch (no) {
352	case arg_AX:
353		rv = &regs->ax;
354		break;
355	case arg_BX:
356		rv = &regs->bx;
357		break;
358	case arg_CX:
359		rv = &regs->cx;
360		break;
361	case arg_DX:
362		rv = &regs->dx;
363		break;
364	case arg_SP:
365		rv = &regs->sp;
366		break;
367	case arg_BP:
368		rv = &regs->bp;
369		break;
370	case arg_SI:
371		rv = &regs->si;
372		break;
373	case arg_DI:
374		rv = &regs->di;
375		break;
376#ifdef __amd64__
377	case arg_R8:
378		rv = &regs->r8;
379		break;
380	case arg_R9:
381		rv = &regs->r9;
382		break;
383	case arg_R10:
384		rv = &regs->r10;
385		break;
386	case arg_R11:
387		rv = &regs->r11;
388		break;
389	case arg_R12:
390		rv = &regs->r12;
391		break;
392	case arg_R13:
393		rv = &regs->r13;
394		break;
395	case arg_R14:
396		rv = &regs->r14;
397		break;
398	case arg_R15:
399		rv = &regs->r15;
400		break;
401#endif
402	default:
403		printk(KERN_ERR "mmiotrace: Error reg no# %d\n", no);
404	}
405
406	return rv;
407}
408
409unsigned long get_ins_reg_val(unsigned long ins_addr, struct pt_regs *regs)
410{
411	unsigned int opcode;
412	int reg;
413	unsigned char *p;
414	struct prefix_bits prf;
415	int i;
416
417	p = (unsigned char *)ins_addr;
418	p += skip_prefix(p, &prf);
419	p += get_opcode(p, &opcode);
420	for (i = 0; i < ARRAY_SIZE(reg_rop); i++)
421		if (reg_rop[i] == opcode)
422			goto do_work;
423
424	for (i = 0; i < ARRAY_SIZE(reg_wop); i++)
425		if (reg_wop[i] == opcode)
426			goto do_work;
427
428	printk(KERN_ERR "mmiotrace: Not a register instruction, opcode "
429							"0x%02x\n", opcode);
430	goto err;
431
432do_work:
433	/* for STOS, source register is fixed */
434	if (opcode == 0xAA || opcode == 0xAB) {
435		reg = arg_AX;
436	} else {
437		unsigned char mod_rm = *p;
438		reg = ((mod_rm >> 3) & 0x7) | (prf.rexr << 3);
439	}
440	switch (get_ins_reg_width(ins_addr)) {
441	case 1:
442		return *get_reg_w8(reg, prf.rex, regs);
443
444	case 2:
445		return *(unsigned short *)get_reg_w32(reg, regs);
446
447	case 4:
448		return *(unsigned int *)get_reg_w32(reg, regs);
449
450#ifdef __amd64__
451	case 8:
452		return *(unsigned long *)get_reg_w32(reg, regs);
453#endif
454
455	default:
456		printk(KERN_ERR "mmiotrace: Error width# %d\n", reg);
457	}
458
459err:
460	return 0;
461}
462
463unsigned long get_ins_imm_val(unsigned long ins_addr)
464{
465	unsigned int opcode;
466	unsigned char mod_rm;
467	unsigned char mod;
468	unsigned char *p;
469	struct prefix_bits prf;
470	int i;
471
472	p = (unsigned char *)ins_addr;
473	p += skip_prefix(p, &prf);
474	p += get_opcode(p, &opcode);
475	for (i = 0; i < ARRAY_SIZE(imm_wop); i++)
476		if (imm_wop[i] == opcode)
477			goto do_work;
478
479	printk(KERN_ERR "mmiotrace: Not an immediate instruction, opcode "
480							"0x%02x\n", opcode);
481	goto err;
482
483do_work:
484	mod_rm = *p;
485	mod = mod_rm >> 6;
486	p++;
487	switch (mod) {
488	case 0:
489		/* if r/m is 5 we have a 32 disp (IA32 Manual 3, Table 2-2)  */
490		/* AMD64: XXX Check for address size prefix? */
491		if ((mod_rm & 0x7) == 0x5)
492			p += 4;
493		break;
494
495	case 1:
496		p += 1;
497		break;
498
499	case 2:
500		p += 4;
501		break;
502
503	case 3:
504	default:
505		printk(KERN_ERR "mmiotrace: not a memory access instruction "
506						"at 0x%lx, rm_mod=0x%02x\n",
507						ins_addr, mod_rm);
508	}
509
510	switch (get_ins_reg_width(ins_addr)) {
511	case 1:
512		return *(unsigned char *)p;
513
514	case 2:
515		return *(unsigned short *)p;
516
517	case 4:
518		return *(unsigned int *)p;
519
520#ifdef __amd64__
521	case 8:
522		return *(unsigned long *)p;
523#endif
524
525	default:
526		printk(KERN_ERR "mmiotrace: Error: width.\n");
527	}
528
529err:
530	return 0;
531}