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