quark: updating license to GPLv2+
[fw/openocd] / src / target / x86_32_common.c
1 /*
2  * Copyright(c) 2013 Intel Corporation.
3  *
4  * Adrian Burns (adrian.burns@intel.com)
5  * Thomas Faust (thomas.faust@intel.com)
6  * Ivan De Cesaris (ivan.de.cesaris@intel.com)
7  * Julien Carreno (julien.carreno@intel.com)
8  * Jeffrey Maxwell (jeffrey.r.maxwell@intel.com)
9  *
10  * This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful, but
16  * WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
18  * General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
23  *
24  * Contact Information:
25  * Intel Corporation
26  */
27
28 /*
29  * @file
30  * This implements generic x86 32 bit memory and breakpoint operations.
31  */
32
33 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36
37 #include <helper/log.h>
38
39 #include "target.h"
40 #include "target_type.h"
41 #include "register.h"
42 #include "breakpoints.h"
43 #include "x86_32_common.h"
44
45 static int set_debug_regs(struct target *t, uint32_t address,
46                         uint8_t bp_num, uint8_t bp_type, uint8_t bp_length);
47 static int unset_debug_regs(struct target *t, uint8_t bp_num);
48 static int read_mem(struct target *t, uint32_t size,
49                         uint32_t addr, uint8_t *buf);
50 static int write_mem(struct target *t, uint32_t size,
51                         uint32_t addr, const uint8_t *buf);
52 static int calcaddr_pyhsfromlin(struct target *t, uint32_t addr,
53                         uint32_t *physaddr);
54 static int read_phys_mem(struct target *t, uint32_t phys_address,
55                         uint32_t size, uint32_t count, uint8_t *buffer);
56 static int write_phys_mem(struct target *t, uint32_t phys_address,
57                         uint32_t size, uint32_t count, const uint8_t *buffer);
58 static int set_breakpoint(struct target *target,
59                         struct breakpoint *breakpoint);
60 static int unset_breakpoint(struct target *target,
61                         struct breakpoint *breakpoint);
62 static int set_watchpoint(struct target *target,
63                         struct watchpoint *watchpoint);
64 static int unset_watchpoint(struct target *target,
65                         struct watchpoint *watchpoint);
66 static int read_hw_reg_to_cache(struct target *t, int num);
67 static int write_hw_reg_from_cache(struct target *t, int num);
68
69 int x86_32_get_gdb_reg_list(struct target *t,
70                         struct reg **reg_list[], int *reg_list_size,
71                         enum target_register_class reg_class)
72 {
73
74         struct x86_32_common *x86_32 = target_to_x86_32(t);
75         int i;
76         *reg_list_size = x86_32->cache->num_regs;
77         LOG_DEBUG("num_regs=%d, reg_class=%d", (*reg_list_size), reg_class);
78         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
79         if (*reg_list == NULL) {
80                 LOG_ERROR("%s out of memory", __func__);
81                 return ERROR_FAIL;
82         }
83         /* this will copy the values from our reg list to gdbs */
84         for (i = 0; i < (*reg_list_size); i++) {
85                 (*reg_list)[i] = &x86_32->cache->reg_list[i];
86                 LOG_DEBUG("value %s = %08" PRIx32, x86_32->cache->reg_list[i].name,
87                                 buf_get_u32(x86_32->cache->reg_list[i].value, 0, 32));
88         }
89         return ERROR_OK;
90 }
91
92 int x86_32_common_init_arch_info(struct target *t, struct x86_32_common *x86_32)
93 {
94         t->arch_info = x86_32;
95         x86_32->common_magic = X86_32_COMMON_MAGIC;
96         x86_32->num_hw_bpoints = MAX_DEBUG_REGS;
97         x86_32->hw_break_list = calloc(x86_32->num_hw_bpoints,
98                                 sizeof(struct x86_32_dbg_reg));
99         if (x86_32->hw_break_list == NULL) {
100                 LOG_ERROR("%s out of memory", __func__);
101                 return ERROR_FAIL;
102         }
103         x86_32->curr_tap = t->tap;
104         x86_32->fast_data_area = NULL;
105         x86_32->flush = 1;
106         x86_32->read_hw_reg_to_cache = read_hw_reg_to_cache;
107         x86_32->write_hw_reg_from_cache = write_hw_reg_from_cache;
108         return ERROR_OK;
109 }
110
111 int x86_32_common_mmu(struct target *t, int *enabled)
112 {
113         *enabled = true;
114         return ERROR_OK;
115 }
116
117 int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physical)
118 {
119         struct x86_32_common *x86_32 = target_to_x86_32(t);
120
121         /*
122          * We need to ignore 'segmentation' for now, as OpenOCD can't handle
123          * segmented addresses.
124          * In protected mode that is almost OK, as (almost) any known OS is using
125          * flat segmentation. In real mode we use use the base of the DS segment,
126          * as we don't know better ...
127          */
128
129         uint32_t cr0 = buf_get_u32(x86_32->cache->reg_list[CR0].value, 0, 32);
130         if (!(cr0 & CR0_PG)) {
131                 /* target halted in real mode */
132                 /* TODO: needs validation !!! */
133                 uint32_t dsb = buf_get_u32(x86_32->cache->reg_list[DSB].value, 0, 32);
134                 *physical = dsb + address;
135
136         } else {
137                 /* target halted in protected mode */
138                 if (calcaddr_pyhsfromlin(t, address, physical) != ERROR_OK) {
139                         LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
140                                         __func__, address);
141                         return ERROR_FAIL;
142                 }
143         }
144         return ERROR_OK;
145 }
146
147 int x86_32_common_read_phys_mem(struct target *t, uint32_t phys_address,
148                         uint32_t size, uint32_t count, uint8_t *buffer)
149 {
150         struct x86_32_common *x86_32 = target_to_x86_32(t);
151         int error;
152
153         error = read_phys_mem(t, phys_address, size, count, buffer);
154         if (error != ERROR_OK)
155                 return error;
156
157         /* After reading memory from target, we must replace software breakpoints
158          * with the original instructions again.
159          */
160         struct swbp_mem_patch *iter = x86_32->swbbp_mem_patch_list;
161         while (iter != NULL) {
162                 if (iter->physaddr >= phys_address && iter->physaddr < phys_address+(size*count)) {
163                         uint32_t offset = iter->physaddr - phys_address;
164                         buffer[offset] = iter->orig_byte;
165                 }
166                 iter = iter->next;
167         }
168         return ERROR_OK;
169 }
170
171 static int read_phys_mem(struct target *t, uint32_t phys_address,
172                         uint32_t size, uint32_t count, uint8_t *buffer)
173 {
174         int retval = ERROR_OK;
175         bool pg_disabled = false;
176         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
177                         phys_address, size, count, buffer);
178         struct x86_32_common *x86_32 = target_to_x86_32(t);
179
180         if (check_not_halted(t))
181                 return ERROR_TARGET_NOT_HALTED;
182         if (!count || !buffer || !phys_address) {
183                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
184                                 __func__, count, buffer, phys_address);
185                 return ERROR_COMMAND_ARGUMENT_INVALID;
186         }
187
188         /* to access physical memory, switch off the CR0.PG bit */
189         if (x86_32->is_paging_enabled(t)) {
190                 retval = x86_32->disable_paging(t);
191                 if (retval != ERROR_OK) {
192                         LOG_ERROR("%s could not disable paging", __func__);
193                         return retval;
194                 }
195                 pg_disabled = true;
196         }
197
198         for (uint32_t i = 0; i < count; i++) {
199                 switch (size) {
200                 case BYTE:
201                         retval = read_mem(t, size, phys_address + i, buffer + i);
202                         break;
203                 case WORD:
204                         retval = read_mem(t, size, phys_address + i * 2, buffer + i * 2);
205                         break;
206                 case DWORD:
207                         retval = read_mem(t, size, phys_address + i * 4, buffer + i * 4);
208                         break;
209                 default:
210                         LOG_ERROR("%s invalid read size", __func__);
211                         break;
212                 }
213         }
214         /* restore CR0.PG bit if needed (regardless of retval) */
215         if (pg_disabled) {
216                 retval = x86_32->enable_paging(t);
217                 if (retval != ERROR_OK) {
218                         LOG_ERROR("%s could not enable paging", __func__);
219                         return retval;
220                 }
221                 pg_disabled = true;
222         }
223         /* TODO: After reading memory from target, we must replace
224          * software breakpoints with the original instructions again.
225          * Solve this with the breakpoint fix
226          */
227         return retval;
228 }
229
230 int x86_32_common_write_phys_mem(struct target *t, uint32_t phys_address,
231                         uint32_t size, uint32_t count, const uint8_t *buffer)
232 {
233         struct x86_32_common *x86_32 = target_to_x86_32(t);
234         int error = ERROR_OK;
235         uint8_t *newbuffer = NULL;
236
237         check_not_halted(t);
238         if (!count || !buffer || !phys_address) {
239                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
240                                 __func__, count, buffer, phys_address);
241                 return ERROR_COMMAND_ARGUMENT_INVALID;
242         }
243         /* Before writing memory to target, we must update software breakpoints
244          * with the new instructions and patch the memory buffer with the
245          * breakpoint instruction.
246          */
247         newbuffer = malloc(size*count);
248         if (newbuffer == NULL) {
249                 LOG_ERROR("%s out of memory", __func__);
250                 return ERROR_FAIL;
251         }
252         memcpy(newbuffer, buffer, size*count);
253         struct swbp_mem_patch *iter = x86_32->swbbp_mem_patch_list;
254         while (iter != NULL) {
255                 if (iter->physaddr >= phys_address && iter->physaddr < phys_address+(size*count)) {
256                         uint32_t offset = iter->physaddr - phys_address;
257                         newbuffer[offset] = SW_BP_OPCODE;
258
259                         /* update the breakpoint */
260                         struct breakpoint *pbiter = t->breakpoints;
261                         while (pbiter != NULL && pbiter->unique_id != iter->swbp_unique_id)
262                                 pbiter = pbiter->next;
263                         if (pbiter)
264                                 pbiter->orig_instr[0] = buffer[offset];
265                 }
266                 iter = iter->next;
267         }
268
269         error = write_phys_mem(t, phys_address, size, count, newbuffer);
270         free(newbuffer);
271         return error;
272 }
273
274 static int write_phys_mem(struct target *t, uint32_t phys_address,
275                         uint32_t size, uint32_t count, const uint8_t *buffer)
276 {
277         int retval = ERROR_OK;
278         bool pg_disabled = false;
279         struct x86_32_common *x86_32 = target_to_x86_32(t);
280         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
281                         phys_address, size, count, buffer);
282
283         check_not_halted(t);
284         if (!count || !buffer || !phys_address) {
285                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
286                                 __func__, count, buffer, phys_address);
287                 return ERROR_COMMAND_ARGUMENT_INVALID;
288         }
289         /* TODO: Before writing memory to target, we must update
290          * software breakpoints with the new instructions and
291          * patch the memory buffer with the breakpoint instruction.
292          * Solve this with the breakpoint fix
293          */
294
295         /* to access physical memory, switch off the CR0.PG bit */
296         if (x86_32->is_paging_enabled(t)) {
297                 retval = x86_32->disable_paging(t);
298                 if (retval != ERROR_OK) {
299                         LOG_ERROR("%s could not disable paging", __func__);
300                         return retval;
301                 }
302                 pg_disabled = true;
303         }
304         for (uint32_t i = 0; i < count; i++) {
305                 switch (size) {
306                 case BYTE:
307                         retval = write_mem(t, size, phys_address + i, buffer + i);
308                         break;
309                 case WORD:
310                         retval = write_mem(t, size, phys_address + i * 2, buffer + i * 2);
311                         break;
312                 case DWORD:
313                         retval = write_mem(t, size, phys_address + i * 4, buffer + i * 4);
314                         break;
315                 default:
316                         LOG_DEBUG("invalid read size");
317                         break;
318                 }
319         }
320         /* restore CR0.PG bit if needed (regardless of retval) */
321         if (pg_disabled) {
322                 retval = x86_32->enable_paging(t);
323                 if (retval != ERROR_OK) {
324                         LOG_ERROR("%s could not enable paging", __func__);
325                         return retval;
326                 }
327         }
328         return retval;
329 }
330
331 static int read_mem(struct target *t, uint32_t size,
332                         uint32_t addr, uint8_t *buf)
333 {
334         struct x86_32_common *x86_32 = target_to_x86_32(t);
335
336         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
337         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
338         int retval = x86_32->write_hw_reg(t, EAX, addr, 0);
339         if (retval != ERROR_OK) {
340                 LOG_ERROR("%s error write EAX", __func__);
341                 return retval;
342         }
343
344         switch (size) {
345                 case BYTE:
346                         if (use32)
347                                 retval = x86_32->submit_instruction(t, MEMRDB32);
348                         else
349                                 retval = x86_32->submit_instruction(t, MEMRDB16);
350                         break;
351                 case WORD:
352                         if (use32)
353                                 retval = x86_32->submit_instruction(t, MEMRDH32);
354                         else
355                                 retval = x86_32->submit_instruction(t, MEMRDH16);
356                         break;
357                 case DWORD:
358                         if (use32)
359                                 retval = x86_32->submit_instruction(t, MEMRDW32);
360                         else
361                                 retval = x86_32->submit_instruction(t, MEMRDW16);
362                         break;
363                 default:
364                         LOG_ERROR("%s invalid read mem size", __func__);
365                         break;
366         }
367
368         /* read_hw_reg() will write to 4 bytes (uint32_t)
369          * Watch out, the buffer passed into read_mem() might be 1 or 2 bytes.
370          */
371         uint32_t regval;
372         retval = x86_32->read_hw_reg(t, EDX, &regval, 0);
373
374         if (retval != ERROR_OK) {
375                 LOG_ERROR("%s error read EDX", __func__);
376                 return retval;
377         }
378         for (uint8_t i = 0; i < size; i++)
379                 buf[i] = (regval >> (i*8)) & 0x000000FF;
380
381         retval = x86_32->transaction_status(t);
382         if (retval != ERROR_OK) {
383                 LOG_ERROR("%s error on mem read", __func__);
384                 return retval;
385         }
386         return retval;
387 }
388
389 static int write_mem(struct target *t, uint32_t size,
390                         uint32_t addr, const uint8_t *buf)
391 {
392         uint32_t i = 0;
393         uint32_t buf4bytes = 0;
394         int retval = ERROR_OK;
395         struct x86_32_common *x86_32 = target_to_x86_32(t);
396
397         for (i = 0; i < size; ++i) {
398                 buf4bytes = buf4bytes << 8; /* first time we only shift 0s */
399                 buf4bytes += buf[(size-1)-i]; /* it was hard to write, should be hard to read! */
400         }
401         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
402         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
403         retval = x86_32->write_hw_reg(t, EAX, addr, 0);
404         if (retval != ERROR_OK) {
405                 LOG_ERROR("%s error write EAX", __func__);
406                 return retval;
407         }
408
409         /* write_hw_reg() will write to 4 bytes (uint32_t)
410          * Watch out, the buffer passed into write_mem() might be 1 or 2 bytes.
411          */
412         retval = x86_32->write_hw_reg(t, EDX, buf4bytes, 0);
413         if (retval != ERROR_OK) {
414                 LOG_ERROR("%s error write EDX", __func__);
415                 return retval;
416         }
417         switch (size) {
418                 case BYTE:
419                         if (use32)
420                                 retval = x86_32->submit_instruction(t, MEMWRB32);
421                         else
422                                 retval = x86_32->submit_instruction(t, MEMWRB16);
423                         break;
424                 case WORD:
425                         if (use32)
426                                 retval = x86_32->submit_instruction(t, MEMWRH32);
427                         else
428                                 retval = x86_32->submit_instruction(t, MEMWRH16);
429                         break;
430                 case DWORD:
431                         if (use32)
432                                 retval = x86_32->submit_instruction(t, MEMWRW32);
433                         else
434                                 retval = x86_32->submit_instruction(t, MEMWRW16);
435                         break;
436                 default:
437                         LOG_ERROR("%s invalid write mem size", __func__);
438                         return ERROR_FAIL;
439         }
440         retval = x86_32->transaction_status(t);
441         if (retval != ERROR_OK) {
442                 LOG_ERROR("%s error on mem write", __func__);
443                 return retval;
444         }
445         return retval;
446 }
447
448 int calcaddr_pyhsfromlin(struct target *t, uint32_t addr, uint32_t *physaddr)
449 {
450         uint8_t entry_buffer[8];
451
452         if (physaddr == NULL || t == NULL)
453                 return ERROR_FAIL;
454
455         struct x86_32_common *x86_32 = target_to_x86_32(t);
456
457         /* The 'user-visible' CR0.PG should be set - otherwise the function shouldn't be called
458          * (Don't check the CR0.PG on the target, this might be temporally disabled at this point)
459          */
460         uint32_t cr0 = buf_get_u32(x86_32->cache->reg_list[CR0].value, 0, 32);
461         if (!(cr0 & CR0_PG)) {
462                 /* you are wrong in this function, never mind */
463                 *physaddr = addr;
464                 return ERROR_OK;
465         }
466
467         uint32_t cr4 = buf_get_u32(x86_32->cache->reg_list[CR4].value, 0, 32);
468         bool isPAE = cr4 & 0x00000020; /* PAE - Physical Address Extension */
469
470         uint32_t cr3 = buf_get_u32(x86_32->cache->reg_list[CR3].value, 0, 32);
471         if (isPAE) {
472                 uint32_t pdpt_base = cr3 & 0xFFFFF000; /* lower 12 bits of CR3 must always be 0 */
473                 uint32_t pdpt_index = (addr & 0xC0000000) >> 30; /* A[31:30] index to PDPT */
474                 uint32_t pdpt_addr = pdpt_base + (8 * pdpt_index);
475                 if (x86_32_common_read_phys_mem(t, pdpt_addr, 4, 2, entry_buffer) != ERROR_OK) {
476                         LOG_ERROR("%s couldn't read page directory pointer table entry at 0x%08" PRIx32,
477                                         __func__, pdpt_addr);
478                         return ERROR_FAIL;
479                 }
480                 uint64_t pdpt_entry = target_buffer_get_u64(t, entry_buffer);
481                 if (!(pdpt_entry & 0x0000000000000001)) {
482                         LOG_ERROR("%s page directory pointer table entry at 0x%08" PRIx32 " is not present",
483                                         __func__, pdpt_addr);
484                         return ERROR_FAIL;
485                 }
486
487                 uint32_t pd_base = pdpt_entry & 0xFFFFF000; /* A[31:12] is PageTable/Page Base Address */
488                 uint32_t pd_index = (addr & 0x3FE00000) >> 21; /* A[29:21] index to PD entry with PAE */
489                 uint32_t pd_addr = pd_base + (8 * pd_index);
490                 if (x86_32_common_read_phys_mem(t, pd_addr, 4, 2, entry_buffer) != ERROR_OK) {
491                         LOG_ERROR("%s couldn't read page directory entry at 0x%08" PRIx32,
492                                         __func__, pd_addr);
493                         return ERROR_FAIL;
494                 }
495                 uint64_t pd_entry = target_buffer_get_u64(t, entry_buffer);
496                 if (!(pd_entry & 0x0000000000000001)) {
497                         LOG_ERROR("%s page directory entry at 0x%08" PRIx32 " is not present",
498                                         __func__, pd_addr);
499                         return ERROR_FAIL;
500                 }
501
502                 /* PS bit in PD entry is indicating 4KB or 2MB page size */
503                 if (pd_entry & 0x0000000000000080) {
504
505                         uint32_t page_base = (uint32_t)(pd_entry & 0x00000000FFE00000); /* [31:21] */
506                         uint32_t offset = addr & 0x001FFFFF; /* [20:0] */
507                         *physaddr = page_base + offset;
508                         return ERROR_OK;
509
510                 } else {
511
512                         uint32_t pt_base = (uint32_t)(pd_entry & 0x00000000FFFFF000); /*[31:12]*/
513                         uint32_t pt_index = (addr & 0x001FF000) >> 12; /*[20:12]*/
514                         uint32_t pt_addr = pt_base + (8 * pt_index);
515                         if (x86_32_common_read_phys_mem(t, pt_addr, 4, 2, entry_buffer) != ERROR_OK) {
516                                 LOG_ERROR("%s couldn't read page table entry at 0x%08" PRIx32, __func__, pt_addr);
517                                 return ERROR_FAIL;
518                         }
519                         uint64_t pt_entry = target_buffer_get_u64(t, entry_buffer);
520                         if (!(pt_entry & 0x0000000000000001)) {
521                                 LOG_ERROR("%s page table entry at 0x%08" PRIx32 " is not present", __func__, pt_addr);
522                                 return ERROR_FAIL;
523                         }
524
525                         uint32_t page_base = (uint32_t)(pt_entry & 0x00000000FFFFF000); /*[31:12]*/
526                         uint32_t offset =  addr & 0x00000FFF; /*[11:0]*/
527                         *physaddr = page_base + offset;
528                         return ERROR_OK;
529                 }
530         } else {
531                 uint32_t pd_base = cr3 & 0xFFFFF000; /* lower 12 bits of CR3 must always be 0 */
532                 uint32_t pd_index = (addr & 0xFFC00000) >> 22; /* A[31:22] index to PD entry */
533                 uint32_t pd_addr = pd_base + (4 * pd_index);
534                 if (x86_32_common_read_phys_mem(t, pd_addr, 4, 1, entry_buffer) != ERROR_OK) {
535                         LOG_ERROR("%s couldn't read page directory entry at 0x%08" PRIx32, __func__, pd_addr);
536                         return ERROR_FAIL;
537                 }
538                 uint32_t pd_entry = target_buffer_get_u32(t, entry_buffer);
539                 if (!(pd_entry & 0x00000001)) {
540                         LOG_ERROR("%s page directory entry at 0x%08" PRIx32 " is not present", __func__, pd_addr);
541                         return ERROR_FAIL;
542                 }
543
544                 /* Bit 7 in page directory entry is page size.
545                  */
546                 if (pd_entry & 0x00000080) {
547                         /* 4MB pages */
548                         uint32_t page_base = pd_entry & 0xFFC00000;
549                         *physaddr = page_base + (addr & 0x003FFFFF);
550
551                 } else {
552                         /* 4KB pages */
553                         uint32_t pt_base = pd_entry & 0xFFFFF000; /* A[31:12] is PageTable/Page Base Address */
554                         uint32_t pt_index = (addr & 0x003FF000) >> 12; /* A[21:12] index to page table entry */
555                         uint32_t pt_addr = pt_base + (4 * pt_index);
556                         if (x86_32_common_read_phys_mem(t, pt_addr, 4, 1, entry_buffer) != ERROR_OK) {
557                                 LOG_ERROR("%s couldn't read page table entry at 0x%08" PRIx32, __func__, pt_addr);
558                                 return ERROR_FAIL;
559                         }
560                         uint32_t pt_entry = target_buffer_get_u32(t, entry_buffer);
561                         if (!(pt_entry & 0x00000001)) {
562                                 LOG_ERROR("%s page table entry at 0x%08" PRIx32 " is not present", __func__, pt_addr);
563                                 return ERROR_FAIL;
564                         }
565                         uint32_t page_base = pt_entry & 0xFFFFF000; /* A[31:12] is PageTable/Page Base Address */
566                         *physaddr = page_base + (addr & 0x00000FFF); /* A[11:0] offset to 4KB page in linear address */
567                 }
568         }
569         return ERROR_OK;
570 }
571
572 int x86_32_common_read_memory(struct target *t, uint32_t addr,
573                         uint32_t size, uint32_t count, uint8_t *buf)
574 {
575         int retval = ERROR_OK;
576         struct x86_32_common *x86_32 = target_to_x86_32(t);
577         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
578                         addr, size, count, buf);
579         check_not_halted(t);
580         if (!count || !buf || !addr) {
581                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
582                                 __func__, count, buf, addr);
583                 return ERROR_COMMAND_ARGUMENT_INVALID;
584         }
585
586         if (x86_32->is_paging_enabled(t)) {
587                 /* all memory accesses from debugger must be physical (CR0.PG == 0)
588                  * conversion to physical address space needed
589                  */
590                 retval = x86_32->disable_paging(t);
591                 if (retval != ERROR_OK) {
592                         LOG_ERROR("%s could not disable paging", __func__);
593                         return retval;
594                 }
595                 uint32_t physaddr = 0;
596                 if (calcaddr_pyhsfromlin(t, addr, &physaddr) != ERROR_OK) {
597                         LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32, __func__, addr);
598                         retval = ERROR_FAIL;
599                 }
600                 /* TODO: !!! Watch out for page boundaries
601                  * for every 4kB, the physical address has to be re-calculated
602                  * This should be fixed together with bulk memory reads
603                  */
604
605                 if (retval == ERROR_OK
606                         && x86_32_common_read_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
607                         LOG_ERROR("%s failed to read memory from physical address 0x%08" PRIx32, __func__, physaddr);
608                         retval = ERROR_FAIL;
609                 }
610                 /* restore PG bit if it was cleared prior (regardless of retval) */
611                 retval = x86_32->enable_paging(t);
612                 if (retval != ERROR_OK) {
613                         LOG_ERROR("%s could not enable paging", __func__);
614                         return retval;
615                 }
616         } else {
617                 /* paging is off - linear address is physical address */
618                 if (x86_32_common_read_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
619                         LOG_ERROR("%s failed to read memory from address 0%08" PRIx32, __func__, addr);
620                         retval = ERROR_FAIL;
621                 }
622         }
623
624         return retval;
625 }
626
627 int x86_32_common_write_memory(struct target *t, uint32_t addr,
628                         uint32_t size, uint32_t count, const uint8_t *buf)
629 {
630         int retval = ERROR_OK;
631         struct x86_32_common *x86_32 = target_to_x86_32(t);
632         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
633                         addr, size, count, buf);
634         check_not_halted(t);
635         if (!count || !buf || !addr) {
636                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
637                                         __func__, count, buf, addr);
638                 return ERROR_COMMAND_ARGUMENT_INVALID;
639         }
640         if (x86_32->is_paging_enabled(t)) {
641                 /* all memory accesses from debugger must be physical (CR0.PG == 0)
642                  * conversion to physical address space needed
643                  */
644                 retval = x86_32->disable_paging(t);
645                 if (retval != ERROR_OK) {
646                         LOG_ERROR("%s could not disable paging", __func__);
647                         return retval;
648                 }
649                 uint32_t physaddr = 0;
650                 if (calcaddr_pyhsfromlin(t, addr, &physaddr) != ERROR_OK) {
651                         LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
652                                         __func__, addr);
653                         retval = ERROR_FAIL;
654                 }
655                 /* TODO: !!! Watch out for page boundaries
656                  * for every 4kB, the physical address has to be re-calculated
657                  * This should be fixed together with bulk memory reads
658                  */
659                 if (retval == ERROR_OK
660                         && x86_32_common_write_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
661                         LOG_ERROR("%s failed to write memory to physical address 0x%08" PRIx32,
662                                         __func__, physaddr);
663                         retval = ERROR_FAIL;
664                 }
665                 /* restore PG bit if it was cleared prior (regardless of retval) */
666                 retval = x86_32->enable_paging(t);
667                 if (retval != ERROR_OK) {
668                         LOG_ERROR("%s could not enable paging", __func__);
669                         return retval;
670                 }
671         } else {
672
673                 /* paging is off - linear address is physical address */
674                 if (x86_32_common_write_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
675                         LOG_ERROR("%s failed to write memory to address 0x%08" PRIx32,
676                                         __func__, addr);
677                         retval = ERROR_FAIL;
678                 }
679         }
680         return retval;
681 }
682
683 int x86_32_common_read_io(struct target *t, uint32_t addr,
684                         uint32_t size, uint8_t *buf)
685 {
686         struct x86_32_common *x86_32 = target_to_x86_32(t);
687         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
688         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
689         int retval = ERROR_FAIL;
690         bool pg_disabled = false;
691         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", buf=%p", addr, size, buf);
692         check_not_halted(t);
693         if (!buf || !addr) {
694                 LOG_ERROR("%s invalid params buf=%p, addr=%08" PRIx32, __func__, buf, addr);
695                 return retval;
696         }
697         retval = x86_32->write_hw_reg(t, EDX, addr, 0);
698         if (retval != ERROR_OK) {
699                 LOG_ERROR("%s error EDX write", __func__);
700                 return retval;
701         }
702         /* to access physical memory, switch off the CR0.PG bit */
703         if (x86_32->is_paging_enabled(t)) {
704                 retval = x86_32->disable_paging(t);
705                 if (retval != ERROR_OK) {
706                         LOG_ERROR("%s could not disable paging", __func__);
707                         return retval;
708                 }
709                 pg_disabled = true;
710         }
711         switch (size) {
712                 case BYTE:
713                         if (use32)
714                                 retval = x86_32->submit_instruction(t, IORDB32);
715                         else
716                                 retval = x86_32->submit_instruction(t, IORDB16);
717                         break;
718                 case WORD:
719                         if (use32)
720                                 retval = x86_32->submit_instruction(t, IORDH32);
721                         else
722                                 retval = x86_32->submit_instruction(t, IORDH16);
723                         break;
724                 case DWORD:
725                         if (use32)
726                                 retval = x86_32->submit_instruction(t, IORDW32);
727                         else
728                                 retval = x86_32->submit_instruction(t, IORDW16);
729                         break;
730                 default:
731                         LOG_ERROR("%s invalid read io size", __func__);
732                         return ERROR_FAIL;
733         }
734         /* restore CR0.PG bit if needed */
735         if (pg_disabled) {
736                 retval = x86_32->enable_paging(t);
737                 if (retval != ERROR_OK) {
738                         LOG_ERROR("%s could not enable paging", __func__);
739                         return retval;
740                 }
741                 pg_disabled = false;
742         }
743         uint32_t regval = 0;
744         retval = x86_32->read_hw_reg(t, EAX, &regval, 0);
745         if (retval != ERROR_OK) {
746                 LOG_ERROR("%s error on read EAX", __func__);
747                 return retval;
748         }
749         for (uint8_t i = 0; i < size; i++)
750                 buf[i] = (regval >> (i*8)) & 0x000000FF;
751         retval = x86_32->transaction_status(t);
752         if (retval != ERROR_OK) {
753                 LOG_ERROR("%s error on io read", __func__);
754                 return retval;
755         }
756         return retval;
757 }
758
759 int x86_32_common_write_io(struct target *t, uint32_t addr,
760                         uint32_t size, const uint8_t *buf)
761 {
762         struct x86_32_common *x86_32 = target_to_x86_32(t);
763         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
764         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
765         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", buf=%p", addr, size, buf);
766         check_not_halted(t);
767         int retval = ERROR_FAIL;
768         bool pg_disabled = false;
769         if (!buf || !addr) {
770                 LOG_ERROR("%s invalid params buf=%p, addr=0x%08" PRIx32, __func__, buf, addr);
771                 return retval;
772         }
773         /* no do the write */
774         retval = x86_32->write_hw_reg(t, EDX, addr, 0);
775         if (retval != ERROR_OK) {
776                 LOG_ERROR("%s error on EDX write", __func__);
777                 return retval;
778         }
779         uint32_t regval = 0;
780         for (uint8_t i = 0; i < size; i++)
781                 regval += (buf[i] << (i*8));
782         retval = x86_32->write_hw_reg(t, EAX, regval, 0);
783         if (retval != ERROR_OK) {
784                 LOG_ERROR("%s error on EAX write", __func__);
785                 return retval;
786         }
787         /* to access physical memory, switch off the CR0.PG bit */
788         if (x86_32->is_paging_enabled(t)) {
789                 retval = x86_32->disable_paging(t);
790                 if (retval != ERROR_OK) {
791                         LOG_ERROR("%s could not disable paging", __func__);
792                         return retval;
793                 }
794                 pg_disabled = true;
795         }
796         switch (size) {
797                 case BYTE:
798                         if (use32)
799                                 retval = x86_32->submit_instruction(t, IOWRB32);
800                         else
801                                 retval = x86_32->submit_instruction(t, IOWRB16);
802                         break;
803                 case WORD:
804                         if (use32)
805                                 retval = x86_32->submit_instruction(t, IOWRH32);
806                         else
807                                 retval = x86_32->submit_instruction(t, IOWRH16);
808                         break;
809                 case DWORD:
810                         if (use32)
811                                 retval = x86_32->submit_instruction(t, IOWRW32);
812                         else
813                                 retval = x86_32->submit_instruction(t, IOWRW16);
814                         break;
815                 default:
816                         LOG_ERROR("%s invalid write io size", __func__);
817                         return ERROR_FAIL;
818         }
819         /* restore CR0.PG bit if needed */
820         if (pg_disabled) {
821                 retval = x86_32->enable_paging(t);
822                 if (retval != ERROR_OK) {
823                         LOG_ERROR("%s could not enable paging", __func__);
824                         return retval;
825                 }
826                 pg_disabled = false;
827         }
828         retval = x86_32->transaction_status(t);
829         if (retval != ERROR_OK) {
830                 LOG_ERROR("%s error on io write", __func__);
831                 return retval;
832         }
833         return retval;
834 }
835
836 int x86_32_common_add_watchpoint(struct target *t, struct watchpoint *wp)
837 {
838         check_not_halted(t);
839         /* set_watchpoint() will return ERROR_TARGET_RESOURCE_NOT_AVAILABLE if all
840          * hardware registers are gone
841          */
842         return set_watchpoint(t, wp);
843 }
844
845 int x86_32_common_remove_watchpoint(struct target *t, struct watchpoint *wp)
846 {
847         if (check_not_halted(t))
848                 return ERROR_TARGET_NOT_HALTED;
849         if (wp->set)
850                 unset_watchpoint(t, wp);
851         return ERROR_OK;
852 }
853
854 int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp)
855 {
856         LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
857         if (check_not_halted(t))
858                 return ERROR_TARGET_NOT_HALTED;
859         /* set_breakpoint() will return ERROR_TARGET_RESOURCE_NOT_AVAILABLE if all
860          * hardware registers are gone (for hardware breakpoints)
861          */
862         return set_breakpoint(t, bp);
863 }
864
865 int x86_32_common_remove_breakpoint(struct target *t, struct breakpoint *bp)
866 {
867         LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
868         if (check_not_halted(t))
869                 return ERROR_TARGET_NOT_HALTED;
870         if (bp->set)
871                 unset_breakpoint(t, bp);
872
873         return ERROR_OK;
874 }
875
876 static int set_debug_regs(struct target *t, uint32_t address,
877                         uint8_t bp_num, uint8_t bp_type, uint8_t bp_length)
878 {
879         struct x86_32_common *x86_32 = target_to_x86_32(t);
880         LOG_DEBUG("addr=0x%08" PRIx32 ", bp_num=%" PRIu8 ", bp_type=%" PRIu8 ", pb_length=%" PRIu8,
881                         address, bp_num, bp_type, bp_length);
882
883         /* DR7 - set global enable */
884         uint32_t dr7 = buf_get_u32(x86_32->cache->reg_list[DR7].value, 0, 32);
885
886         if (bp_length != 1 && bp_length != 2 && bp_length != 4)
887                 return ERROR_FAIL;
888
889         if (DR7_BP_FREE(dr7, bp_num))
890                 DR7_GLOBAL_ENABLE(dr7, bp_num);
891         else {
892                 LOG_ERROR("%s dr7 error, already enabled, val=%08" PRIx32, __func__, dr7);
893                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
894         }
895
896         switch (bp_type) {
897                 case 0:
898                         /* 00 - only on instruction execution */
899                         DR7_SET_EXE(dr7, bp_num);
900                         DR7_SET_LENGTH(dr7, bp_num, bp_length);
901                 break;
902                 case 1:
903                         /* 01 - only on data writes */
904                         DR7_SET_WRITE(dr7, bp_num);
905                         DR7_SET_LENGTH(dr7, bp_num, bp_length);
906                 break;
907                 case 2:
908                         /* 10 UNSUPPORTED - an I/O read and I/O write */
909                         LOG_ERROR("%s unsupported feature bp_type=%d", __func__, bp_type);
910                         return ERROR_FAIL;
911                 break;
912                 case 3:
913                         /* on data read or data write */
914                         DR7_SET_ACCESS(dr7, bp_num);
915                         DR7_SET_LENGTH(dr7, bp_num, bp_length);
916                 break;
917                 default:
918                         LOG_ERROR("%s invalid request [only 0-3] bp_type=%d", __func__, bp_type);
919                         return ERROR_FAIL;
920         }
921
922         /* update regs in the reg cache ready to be written to hardware
923          * when we exit PM
924         */
925         buf_set_u32(x86_32->cache->reg_list[bp_num+DR0].value, 0, 32, address);
926         x86_32->cache->reg_list[bp_num+DR0].dirty = 1;
927         x86_32->cache->reg_list[bp_num+DR0].valid = 1;
928         buf_set_u32(x86_32->cache->reg_list[DR6].value, 0, 32, PM_DR6);
929         x86_32->cache->reg_list[DR6].dirty = 1;
930         x86_32->cache->reg_list[DR6].valid = 1;
931         buf_set_u32(x86_32->cache->reg_list[DR7].value, 0, 32, dr7);
932         x86_32->cache->reg_list[DR7].dirty = 1;
933         x86_32->cache->reg_list[DR7].valid = 1;
934         return ERROR_OK;
935 }
936
937 static int unset_debug_regs(struct target *t, uint8_t bp_num)
938 {
939         struct x86_32_common *x86_32 = target_to_x86_32(t);
940         LOG_DEBUG("bp_num=%" PRIu8, bp_num);
941
942         uint32_t dr7 = buf_get_u32(x86_32->cache->reg_list[DR7].value, 0, 32);
943
944         if (!(DR7_BP_FREE(dr7, bp_num))) {
945                 DR7_GLOBAL_DISABLE(dr7, bp_num);
946         } else {
947                 LOG_ERROR("%s dr7 error, not enabled, val=0x%08" PRIx32, __func__, dr7);
948                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
949         }
950         /* this will clear rw and len bits */
951         DR7_RESET_RWLEN_BITS(dr7, bp_num);
952
953         /* update regs in the reg cache ready to be written to hardware
954          * when we exit PM
955         */
956         buf_set_u32(x86_32->cache->reg_list[bp_num+DR0].value, 0, 32, 0);
957         x86_32->cache->reg_list[bp_num+DR0].dirty = 1;
958         x86_32->cache->reg_list[bp_num+DR0].valid = 1;
959         buf_set_u32(x86_32->cache->reg_list[DR6].value, 0, 32, PM_DR6);
960         x86_32->cache->reg_list[DR6].dirty = 1;
961         x86_32->cache->reg_list[DR6].valid = 1;
962         buf_set_u32(x86_32->cache->reg_list[DR7].value, 0, 32, dr7);
963         x86_32->cache->reg_list[DR7].dirty = 1;
964         x86_32->cache->reg_list[DR7].valid = 1;
965         return ERROR_OK;
966 }
967
968 static int set_hwbp(struct target *t, struct breakpoint *bp)
969 {
970         struct x86_32_common *x86_32 = target_to_x86_32(t);
971         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
972         uint8_t hwbp_num = 0;
973
974         while (debug_reg_list[hwbp_num].used && (hwbp_num < x86_32->num_hw_bpoints))
975                 hwbp_num++;
976         if (hwbp_num >= x86_32->num_hw_bpoints) {
977                 LOG_ERROR("%s no free hw breakpoint bpid=0x%" PRIx32, __func__, bp->unique_id);
978                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
979         }
980         if (set_debug_regs(t, bp->address, hwbp_num, DR7_BP_EXECUTE, 1) != ERROR_OK)
981                 return ERROR_FAIL;
982         bp->set = hwbp_num + 1;
983         debug_reg_list[hwbp_num].used = 1;
984         debug_reg_list[hwbp_num].bp_value = bp->address;
985         LOG_USER("%s hardware breakpoint %" PRIu32 " set at 0x%08" PRIx32 " (hwreg=%" PRIu8 ")", __func__,
986                         bp->unique_id, debug_reg_list[hwbp_num].bp_value, hwbp_num);
987         return ERROR_OK;
988 }
989
990 static int unset_hwbp(struct target *t, struct breakpoint *bp)
991 {
992         struct x86_32_common *x86_32 = target_to_x86_32(t);
993         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
994         int hwbp_num = bp->set - 1;
995
996         if ((hwbp_num < 0) || (hwbp_num >= x86_32->num_hw_bpoints)) {
997                 LOG_ERROR("%s invalid breakpoint number=%d, bpid=%" PRIu32,
998                                 __func__, hwbp_num, bp->unique_id);
999                 return ERROR_OK;
1000         }
1001
1002         if (unset_debug_regs(t, hwbp_num) != ERROR_OK)
1003                 return ERROR_FAIL;
1004         debug_reg_list[hwbp_num].used = 0;
1005         debug_reg_list[hwbp_num].bp_value = 0;
1006
1007         LOG_USER("%s hardware breakpoint %" PRIu32 " removed from 0x%08" PRIx32 " (hwreg=%d)",
1008                         __func__, bp->unique_id, bp->address, hwbp_num);
1009         return ERROR_OK;
1010 }
1011
1012 static int set_swbp(struct target *t, struct breakpoint *bp)
1013 {
1014         struct x86_32_common *x86_32 = target_to_x86_32(t);
1015         LOG_DEBUG("id %" PRIx32, bp->unique_id);
1016         uint32_t physaddr;
1017         uint8_t opcode = SW_BP_OPCODE;
1018         uint8_t readback;
1019
1020         if (calcaddr_pyhsfromlin(t, bp->address, &physaddr) != ERROR_OK)
1021                 return ERROR_FAIL;
1022         if (read_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
1023                 return ERROR_FAIL;
1024
1025         LOG_DEBUG("set software breakpoint - orig byte=0x%02" PRIx8 "", *bp->orig_instr);
1026
1027         /* just write the instruction trap byte */
1028         if (write_phys_mem(t, physaddr, 1, 1, &opcode))
1029                 return ERROR_FAIL;
1030
1031         /* verify that this is not invalid/read-only memory */
1032         if (read_phys_mem(t, physaddr, 1, 1, &readback))
1033                 return ERROR_FAIL;
1034
1035         if (readback != SW_BP_OPCODE) {
1036                 LOG_ERROR("%s software breakpoint error at 0x%08" PRIx32 ", check memory",
1037                                 __func__, bp->address);
1038                 LOG_ERROR("%s readback=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
1039                                 __func__, readback, *bp->orig_instr);
1040                 return ERROR_FAIL;
1041         }
1042         bp->set = SW_BP_OPCODE; /* just non 0 */
1043
1044         /* add the memory patch */
1045         struct swbp_mem_patch *new_patch = malloc(sizeof(struct swbp_mem_patch));
1046         if (new_patch == NULL) {
1047                 LOG_ERROR("%s out of memory", __func__);
1048                 return ERROR_FAIL;
1049         }
1050         new_patch->next = NULL;
1051         new_patch->orig_byte = *bp->orig_instr;
1052         new_patch->physaddr = physaddr;
1053         new_patch->swbp_unique_id = bp->unique_id;
1054
1055         struct swbp_mem_patch *addto = x86_32->swbbp_mem_patch_list;
1056         if (addto == NULL)
1057                 x86_32->swbbp_mem_patch_list = new_patch;
1058         else {
1059                 while (addto->next != NULL)
1060                         addto = addto->next;
1061                 addto->next = new_patch;
1062         }
1063         LOG_USER("%s software breakpoint %" PRIu32 " set at 0x%08" PRIx32,
1064                         __func__, bp->unique_id, bp->address);
1065         return ERROR_OK;
1066 }
1067
1068 static int unset_swbp(struct target *t, struct breakpoint *bp)
1069 {
1070         struct x86_32_common *x86_32 = target_to_x86_32(t);
1071         LOG_DEBUG("id %" PRIx32, bp->unique_id);
1072         uint32_t physaddr;
1073         uint8_t current_instr;
1074
1075         /* check that user program has not modified breakpoint instruction */
1076         if (calcaddr_pyhsfromlin(t, bp->address, &physaddr) != ERROR_OK)
1077                 return ERROR_FAIL;
1078         if (read_phys_mem(t, physaddr, 1, 1, &current_instr))
1079                 return ERROR_FAIL;
1080
1081         if (current_instr == SW_BP_OPCODE) {
1082                 if (write_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
1083                         return ERROR_FAIL;
1084         } else {
1085                 LOG_ERROR("%s software breakpoint remove error at 0x%08" PRIx32 ", check memory",
1086                                 __func__, bp->address);
1087                 LOG_ERROR("%s current=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
1088                                 __func__, current_instr, *bp->orig_instr);
1089                 return ERROR_FAIL;
1090         }
1091
1092         /* remove from patch */
1093         struct swbp_mem_patch *iter = x86_32->swbbp_mem_patch_list;
1094         if (iter != NULL) {
1095                 if (iter->swbp_unique_id == bp->unique_id) {
1096                         /* it's the first item */
1097                         x86_32->swbbp_mem_patch_list = iter->next;
1098                         free(iter);
1099                 } else {
1100                         while (iter->next != NULL && iter->next->swbp_unique_id != bp->unique_id)
1101                                 iter = iter->next;
1102                         if (iter->next != NULL) {
1103                                 /* it's the next one */
1104                                 struct swbp_mem_patch *freeme = iter->next;
1105                                 iter->next = iter->next->next;
1106                                 free(freeme);
1107                         }
1108                 }
1109         }
1110
1111         LOG_USER("%s software breakpoint %" PRIu32 " removed from 0x%08" PRIx32,
1112                         __func__, bp->unique_id, bp->address);
1113         return ERROR_OK;
1114 }
1115
1116 static int set_breakpoint(struct target *t, struct breakpoint *bp)
1117 {
1118         int error = ERROR_OK;
1119         struct x86_32_common *x86_32 = target_to_x86_32(t);
1120         LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
1121         if (bp->set) {
1122                 LOG_ERROR("breakpoint already set");
1123                 return error;
1124         }
1125         if (bp->type == BKPT_HARD) {
1126                 error = set_hwbp(t, bp);
1127                 if (error != ERROR_OK) {
1128                         LOG_ERROR("%s error setting hardware breakpoint at 0x%08" PRIx32,
1129                                         __func__, bp->address);
1130                         return error;
1131                 }
1132         } else {
1133                 if (x86_32->sw_bpts_supported(t)) {
1134                         error = set_swbp(t, bp);
1135                         if (error != ERROR_OK) {
1136                                 LOG_ERROR("%s error setting software breakpoint at 0x%08" PRIx32,
1137                                                 __func__, bp->address);
1138                                 return error;
1139                         }
1140                 } else {
1141                         LOG_ERROR("%s core doesn't support SW breakpoints", __func__);
1142                         error = ERROR_FAIL;
1143                         return ERROR_FAIL;
1144                 }
1145         }
1146         return error;
1147 }
1148
1149 static int unset_breakpoint(struct target *t, struct breakpoint *bp)
1150 {
1151         LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
1152         if (!bp->set) {
1153                 LOG_WARNING("breakpoint not set");
1154                 return ERROR_OK;
1155         }
1156
1157         if (bp->type == BKPT_HARD) {
1158                 if (unset_hwbp(t, bp) != ERROR_OK) {
1159                         LOG_ERROR("%s error removing hardware breakpoint at 0x%08" PRIx32,
1160                                         __func__, bp->address);
1161                         return ERROR_FAIL;
1162                 }
1163         } else {
1164                 if (unset_swbp(t, bp) != ERROR_OK) {
1165                         LOG_ERROR("%s error removing software breakpoint at 0x%08" PRIx32,
1166                                         __func__, bp->address);
1167                         return ERROR_FAIL;
1168                 }
1169         }
1170         bp->set = 0;
1171         return ERROR_OK;
1172 }
1173
1174 static int set_watchpoint(struct target *t, struct watchpoint *wp)
1175 {
1176         struct x86_32_common *x86_32 = target_to_x86_32(t);
1177         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
1178         int wp_num = 0;
1179         LOG_DEBUG("type=%d, addr=0x%08" PRIx32, wp->rw, wp->address);
1180
1181         if (wp->set) {
1182                 LOG_ERROR("%s watchpoint already set", __func__);
1183                 return ERROR_OK;
1184         }
1185
1186         if (wp->rw == WPT_READ) {
1187                 LOG_ERROR("%s no support for 'read' watchpoints, use 'access' or 'write'"
1188                                 , __func__);
1189                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1190         }
1191
1192         while (debug_reg_list[wp_num].used && (wp_num < x86_32->num_hw_bpoints))
1193                 wp_num++;
1194         if (wp_num >= x86_32->num_hw_bpoints) {
1195                 LOG_ERROR("%s no debug registers left", __func__);
1196                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1197         }
1198
1199         if (wp->length != 4 && wp->length != 2 && wp->length != 1) {
1200                 LOG_ERROR("%s only watchpoints of length 1, 2 or 4 are supported", __func__);
1201                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1202         }
1203
1204         switch (wp->rw) {
1205                 case WPT_WRITE:
1206                         if (set_debug_regs(t, wp->address, wp_num,
1207                                                 DR7_BP_WRITE, wp->length) != ERROR_OK) {
1208                                 return ERROR_FAIL;
1209                         }
1210                         break;
1211                 case WPT_ACCESS:
1212                         if (set_debug_regs(t, wp->address, wp_num, DR7_BP_READWRITE,
1213                                                 wp->length) != ERROR_OK) {
1214                                 return ERROR_FAIL;
1215                         }
1216                         break;
1217                 default:
1218                         LOG_ERROR("%s only 'access' or 'write' watchpoints are supported", __func__);
1219                         break;
1220         }
1221         wp->set = wp_num + 1;
1222         debug_reg_list[wp_num].used = 1;
1223         debug_reg_list[wp_num].bp_value = wp->address;
1224         LOG_USER("'%s' watchpoint %d set at 0x%08" PRIx32 " with length %" PRIu32 " (hwreg=%d)",
1225                         wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
1226                         "write" : wp->rw == WPT_ACCESS ? "access" : "?",
1227                         wp->unique_id, wp->address, wp->length, wp_num);
1228         return ERROR_OK;
1229 }
1230
1231 static int unset_watchpoint(struct target *t, struct watchpoint *wp)
1232 {
1233         struct x86_32_common *x86_32 = target_to_x86_32(t);
1234         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
1235         LOG_DEBUG("type=%d, addr=0x%08" PRIx32, wp->rw, wp->address);
1236         if (!wp->set) {
1237                 LOG_WARNING("watchpoint not set");
1238                 return ERROR_OK;
1239         }
1240
1241         int wp_num = wp->set - 1;
1242         if ((wp_num < 0) || (wp_num >= x86_32->num_hw_bpoints)) {
1243                 LOG_DEBUG("Invalid FP Comparator number in watchpoint");
1244                 return ERROR_OK;
1245         }
1246         if (unset_debug_regs(t, wp_num) != ERROR_OK)
1247                 return ERROR_FAIL;
1248
1249         debug_reg_list[wp_num].used = 0;
1250         debug_reg_list[wp_num].bp_value = 0;
1251         wp->set = 0;
1252
1253         LOG_USER("'%s' watchpoint %d removed from 0x%08" PRIx32 " with length %" PRIu32 " (hwreg=%d)",
1254                         wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
1255                         "write" : wp->rw == WPT_ACCESS ? "access" : "?",
1256                         wp->unique_id, wp->address, wp->length, wp_num);
1257
1258         return ERROR_OK;
1259 }
1260
1261 static int read_hw_reg_to_cache(struct target *t, int num)
1262 {
1263         uint32_t reg_value;
1264         struct x86_32_common *x86_32 = target_to_x86_32(t);
1265
1266         if (check_not_halted(t))
1267                 return ERROR_TARGET_NOT_HALTED;
1268         if ((num < 0) || (num >= x86_32->get_num_user_regs(t)))
1269                 return ERROR_COMMAND_SYNTAX_ERROR;
1270         if (x86_32->read_hw_reg(t, num, &reg_value, 1) != ERROR_OK) {
1271                 LOG_ERROR("%s fail for %s", x86_32->cache->reg_list[num].name, __func__);
1272                 return ERROR_FAIL;
1273         }
1274         LOG_DEBUG("reg %s value 0x%08" PRIx32,
1275                         x86_32->cache->reg_list[num].name, reg_value);
1276         return ERROR_OK;
1277 }
1278
1279 static int write_hw_reg_from_cache(struct target *t, int num)
1280 {
1281         struct x86_32_common *x86_32 = target_to_x86_32(t);
1282         if (check_not_halted(t))
1283                 return ERROR_TARGET_NOT_HALTED;
1284         if ((num < 0) || (num >= x86_32->get_num_user_regs(t)))
1285                 return ERROR_COMMAND_SYNTAX_ERROR;
1286         if (x86_32->write_hw_reg(t, num, 0, 1) != ERROR_OK) {
1287                 LOG_ERROR("%s fail for %s", x86_32->cache->reg_list[num].name, __func__);
1288                 return ERROR_FAIL;
1289         }
1290         LOG_DEBUG("reg %s value 0x%08" PRIx32, x86_32->cache->reg_list[num].name,
1291                         buf_get_u32(x86_32->cache->reg_list[num].value, 0, 32));
1292         return ERROR_OK;
1293 }
1294
1295 /* x86 32 commands */
1296 static void handle_iod_output(struct command_context *cmd_ctx,
1297                 struct target *target, uint32_t address, unsigned size,
1298                 unsigned count, const uint8_t *buffer)
1299 {
1300         const unsigned line_bytecnt = 32;
1301         unsigned line_modulo = line_bytecnt / size;
1302
1303         char output[line_bytecnt * 4 + 1];
1304         unsigned output_len = 0;
1305
1306         const char *value_fmt;
1307         switch (size) {
1308         case 4:
1309                 value_fmt = "%8.8x ";
1310                 break;
1311         case 2:
1312                 value_fmt = "%4.4x ";
1313                 break;
1314         case 1:
1315                 value_fmt = "%2.2x ";
1316                 break;
1317         default:
1318                 /* "can't happen", caller checked */
1319                 LOG_ERROR("%s invalid memory read size: %u", __func__, size);
1320                 return;
1321         }
1322
1323         for (unsigned i = 0; i < count; i++) {
1324                 if (i % line_modulo == 0) {
1325                         output_len += snprintf(output + output_len,
1326                                         sizeof(output) - output_len,
1327                                         "0x%8.8x: ",
1328                                         (unsigned)(address + (i*size)));
1329                 }
1330
1331                 uint32_t value = 0;
1332                 const uint8_t *value_ptr = buffer + i * size;
1333                 switch (size) {
1334                 case 4:
1335                         value = target_buffer_get_u32(target, value_ptr);
1336                         break;
1337                 case 2:
1338                         value = target_buffer_get_u16(target, value_ptr);
1339                         break;
1340                 case 1:
1341                         value = *value_ptr;
1342                 }
1343                 output_len += snprintf(output + output_len,
1344                                 sizeof(output) - output_len,
1345                                 value_fmt, value);
1346
1347                 if ((i % line_modulo == line_modulo - 1) || (i == count - 1)) {
1348                         command_print(cmd_ctx, "%s", output);
1349                         output_len = 0;
1350                 }
1351         }
1352 }
1353
1354 COMMAND_HANDLER(handle_iod_command)
1355 {
1356         if (CMD_ARGC != 1)
1357                 return ERROR_COMMAND_SYNTAX_ERROR;
1358
1359         uint32_t address;
1360         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
1361         if (address > 0xffff) {
1362                 LOG_ERROR("%s IA-32 I/O space is 2^16, 0x%08" PRIx32 " exceeds max", __func__, address);
1363                 return ERROR_COMMAND_SYNTAX_ERROR;
1364         }
1365
1366         unsigned size = 0;
1367         switch (CMD_NAME[2]) {
1368         case 'w':
1369                 size = 4;
1370                 break;
1371         case 'h':
1372                 size = 2;
1373                 break;
1374         case 'b':
1375                 size = 1;
1376                 break;
1377         default:
1378                 return ERROR_COMMAND_SYNTAX_ERROR;
1379         }
1380         unsigned count = 1;
1381         uint8_t *buffer = calloc(count, size);
1382         struct target *target = get_current_target(CMD_CTX);
1383         int retval = x86_32_common_read_io(target, address, size, buffer);
1384         if (ERROR_OK == retval)
1385                 handle_iod_output(CMD_CTX, target, address, size, count, buffer);
1386         free(buffer);
1387         return retval;
1388 }
1389
1390 static int target_fill_io(struct target *target,
1391                 uint32_t address,
1392                 unsigned data_size,
1393                 /* value */
1394                 uint32_t b)
1395 {
1396         LOG_DEBUG("address=0x%08" PRIx32 ", data_size=%u, b=0x%08" PRIx32,
1397                         address, data_size, b);
1398         uint8_t target_buf[data_size];
1399         switch (data_size) {
1400         case 4:
1401                 target_buffer_set_u32(target, target_buf, b);
1402                 break;
1403         case 2:
1404                 target_buffer_set_u16(target, target_buf, b);
1405                 break;
1406         case 1:
1407                 target_buf[0] = (b & 0x0ff);
1408                 break;
1409         default:
1410                 exit(-1);
1411         }
1412         return x86_32_common_write_io(target, address, data_size, target_buf);
1413 }
1414
1415 COMMAND_HANDLER(handle_iow_command)
1416 {
1417         if (CMD_ARGC != 2)
1418                 return ERROR_COMMAND_SYNTAX_ERROR;
1419         uint32_t address;
1420         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
1421         uint32_t value;
1422         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
1423         struct target *target = get_current_target(CMD_CTX);
1424
1425         unsigned wordsize;
1426         switch (CMD_NAME[2]) {
1427                 case 'w':
1428                         wordsize = 4;
1429                         break;
1430                 case 'h':
1431                         wordsize = 2;
1432                         break;
1433                 case 'b':
1434                         wordsize = 1;
1435                         break;
1436                 default:
1437                         return ERROR_COMMAND_SYNTAX_ERROR;
1438         }
1439         return target_fill_io(target, address, wordsize, value);
1440 }
1441
1442 static const struct command_registration x86_32_exec_command_handlers[] = {
1443         {
1444                 .name = "iww",
1445                 .mode = COMMAND_EXEC,
1446                 .handler = handle_iow_command,
1447                 .help = "write I/O port word",
1448                 .usage = "port data[word]",
1449         },
1450         {
1451                 .name = "iwh",
1452                 .mode = COMMAND_EXEC,
1453                 .handler = handle_iow_command,
1454                 .help = "write I/O port halfword",
1455                 .usage = "port data[halfword]",
1456         },
1457         {
1458                 .name = "iwb",
1459                 .mode = COMMAND_EXEC,
1460                 .handler = handle_iow_command,
1461                 .help = "write I/O port byte",
1462                 .usage = "port data[byte]",
1463         },
1464         {
1465                 .name = "idw",
1466                 .mode = COMMAND_EXEC,
1467                 .handler = handle_iod_command,
1468                 .help = "display I/O port word",
1469                 .usage = "port",
1470         },
1471         {
1472                 .name = "idh",
1473                 .mode = COMMAND_EXEC,
1474                 .handler = handle_iod_command,
1475                 .help = "display I/O port halfword",
1476                 .usage = "port",
1477         },
1478         {
1479                 .name = "idb",
1480                 .mode = COMMAND_EXEC,
1481                 .handler = handle_iod_command,
1482                 .help = "display I/O port byte",
1483                 .usage = "port",
1484         },
1485
1486         COMMAND_REGISTRATION_DONE
1487 };
1488
1489 const struct command_registration x86_32_command_handlers[] = {
1490         {
1491                 .name = "x86_32",
1492                 .mode = COMMAND_ANY,
1493                 .help = "x86_32 target commands",
1494                 .usage = "",
1495                 .chain = x86_32_exec_command_handlers,
1496         },
1497         COMMAND_REGISTRATION_DONE
1498 };