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