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, 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 };