git.gag.com Git - fw/openocd/blob - src/flash/str7x.c

   1 /***************************************************************************\r
   2  *   Copyright (C) 2005 by Dominic Rath                                    *\r
   3  *   Dominic.Rath@gmx.de                                                   *\r
   4  *                                                                         *\r
   5  *   This program is free software; you can redistribute it and/or modify  *\r
   6  *   it under the terms of the GNU General Public License as published by  *\r
   7  *   the Free Software Foundation; either version 2 of the License, or     *\r
   8  *   (at your option) any later version.                                   *\r
   9  *                                                                         *\r
  10  *   This program is distributed in the hope that it will be useful,       *\r
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
  13  *   GNU General Public License for more details.                          *\r
  14  *                                                                         *\r
  15  *   You should have received a copy of the GNU General Public License     *\r
  16  *   along with this program; if not, write to the                         *\r
  17  *   Free Software Foundation, Inc.,                                       *\r
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
  19  ***************************************************************************/\r
  20 #ifdef HAVE_CONFIG_H\r
  21 #include "config.h"\r
  22 #endif\r
  23 \r
  24 #include "replacements.h"\r
  25 \r
  26 #include "str7x.h"\r
  27 #include "flash.h"\r
  28 #include "target.h"\r
  29 #include "log.h"\r
  30 #include "armv4_5.h"\r
  31 #include "algorithm.h"\r
  32 #include "binarybuffer.h"\r
  33 \r
  34 #include <stdlib.h>\r
  35 #include <string.h>\r
  36 #include <unistd.h>\r
  37 \r
  38 str7x_mem_layout_t mem_layout[] = {\r
  39         {0x00000000, 0x02000, 0x01},\r
  40         {0x00002000, 0x02000, 0x02},\r
  41         {0x00004000, 0x02000, 0x04},\r
  42         {0x00006000, 0x02000, 0x08},\r
  43         {0x00008000, 0x08000, 0x10},\r
  44         {0x00010000, 0x10000, 0x20},\r
  45         {0x00020000, 0x10000, 0x40},\r
  46         {0x00030000, 0x10000, 0x80},\r
  47         {0x000C0000, 0x02000, 0x10000},\r
  48         {0x000C2000, 0x02000, 0x20000},\r
  49 };\r
  50 \r
  51 int str7x_register_commands(struct command_context_s *cmd_ctx);\r
  52 int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);\r
  53 int str7x_erase(struct flash_bank_s *bank, int first, int last);\r
  54 int str7x_protect(struct flash_bank_s *bank, int set, int first, int last);\r
  55 int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);\r
  56 int str7x_probe(struct flash_bank_s *bank);\r
  57 int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
  58 int str7x_protect_check(struct flash_bank_s *bank);\r
  59 int str7x_erase_check(struct flash_bank_s *bank);\r
  60 int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size);\r
  61 \r
  62 int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
  63 \r
  64 flash_driver_t str7x_flash =\r
  65 {\r
  66         .name = "str7x",\r
  67         .register_commands = str7x_register_commands,\r
  68         .flash_bank_command = str7x_flash_bank_command,\r
  69         .erase = str7x_erase,\r
  70         .protect = str7x_protect,\r
  71         .write = str7x_write,\r
  72         .probe = str7x_probe,\r
  73         .auto_probe = str7x_probe,\r
  74         .erase_check = str7x_erase_check,\r
  75         .protect_check = str7x_protect_check,\r
  76         .info = str7x_info\r
  77 };\r
  78 \r
  79 int str7x_register_commands(struct command_context_s *cmd_ctx)\r
  80 {\r
  81         command_t *str7x_cmd = register_command(cmd_ctx, NULL, "str7x", NULL, COMMAND_ANY, NULL);\r
  82         \r
  83         register_command(cmd_ctx, str7x_cmd, "disable_jtag", str7x_handle_disable_jtag_command, COMMAND_EXEC,\r
  84                                          "disable jtag access");\r
  85                                          \r
  86         return ERROR_OK;\r
  87 }\r
  88 \r
  89 int str7x_get_flash_adr(struct flash_bank_s *bank, u32 reg)\r
  90 {\r
  91         return (bank->base | reg);\r
  92 }\r
  93 \r
  94 int str7x_build_block_list(struct flash_bank_s *bank)\r
  95 {\r
  96         str7x_flash_bank_t *str7x_info = bank->driver_priv;\r
  97 \r
  98         int i;\r
  99         int num_sectors = 0, b0_sectors = 0, b1_sectors = 0;\r
 100                 \r
 101         switch (bank->size)\r
 102         {\r
 103                 case 16 * 1024:\r
 104                         b0_sectors = 2;\r
 105                         break;\r
 106                 case 64 * 1024:\r
 107                         b0_sectors = 5;\r
 108                         break;\r
 109                 case 128 * 1024:\r
 110                         b0_sectors = 6;\r
 111                         break;\r
 112                 case 256 * 1024:\r
 113                         b0_sectors = 8;\r
 114                         break;\r
 115                 default:\r
 116                         ERROR("BUG: unknown bank->size encountered");\r
 117                         exit(-1);\r
 118         }\r
 119         \r
 120         if( str7x_info->bank1 == 1 )\r
 121         {\r
 122                 b1_sectors += 2;\r
 123         }\r
 124         \r
 125         num_sectors = b0_sectors + b1_sectors;\r
 126         \r
 127         bank->num_sectors = num_sectors;\r
 128         bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);\r
 129         str7x_info->sector_bits = malloc(sizeof(u32) * num_sectors);\r
 130         str7x_info->sector_bank = malloc(sizeof(u32) * num_sectors);\r
 131         \r
 132         num_sectors = 0;\r
 133         \r
 134         for (i = 0; i < b0_sectors; i++)\r
 135         {\r
 136                 bank->sectors[num_sectors].offset = mem_layout[i].sector_start;\r
 137                 bank->sectors[num_sectors].size = mem_layout[i].sector_size;\r
 138                 bank->sectors[num_sectors].is_erased = -1;\r
 139                 bank->sectors[num_sectors].is_protected = 1;\r
 140                 str7x_info->sector_bank[num_sectors] = 0;\r
 141                 str7x_info->sector_bits[num_sectors++] = mem_layout[i].sector_bit;\r
 142         }\r
 143         \r
 144         if (b1_sectors)\r
 145         {\r
 146                 for (i = 8; i < 10; i++)\r
 147                 {\r
 148                         bank->sectors[num_sectors].offset = mem_layout[i].sector_start;\r
 149                         bank->sectors[num_sectors].size = mem_layout[i].sector_size;\r
 150                         bank->sectors[num_sectors].is_erased = -1;\r
 151                         bank->sectors[num_sectors].is_protected = 1;\r
 152                         str7x_info->sector_bank[num_sectors] = 1;\r
 153                         str7x_info->sector_bits[num_sectors++] = mem_layout[i].sector_bit;\r
 154                 }\r
 155         }\r
 156         \r
 157         return ERROR_OK;\r
 158 }\r
 159 \r
 160 /* flash bank str7x <base> <size> 0 0 <target#> <str71_variant>\r
 161  */\r
 162 int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)\r
 163 {\r
 164         str7x_flash_bank_t *str7x_info;\r
 165         \r
 166         if (argc < 7)\r
 167         {\r
 168                 WARNING("incomplete flash_bank str7x configuration");\r
 169                 return ERROR_FLASH_BANK_INVALID;\r
 170         }\r
 171         \r
 172         str7x_info = malloc(sizeof(str7x_flash_bank_t));\r
 173         bank->driver_priv = str7x_info;\r
 174         \r
 175         /* set default bits for str71x flash */\r
 176         str7x_info->bank1 = 1;\r
 177         str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA1|FLASH_BSYA0);\r
 178         str7x_info->disable_bit = (1<<1);\r
 179         \r
 180         if (strcmp(args[6], "STR71x") == 0)\r
 181         {\r
 182                 if (bank->base != 0x40000000)\r
 183                 {\r
 184                         WARNING("overriding flash base address for STR71x device with 0x40000000");\r
 185                         bank->base = 0x40000000;\r
 186                 }\r
 187         }\r
 188         else if (strcmp(args[6], "STR73x") == 0)\r
 189         {\r
 190                 str7x_info->bank1 = 0;\r
 191                 str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA0);\r
 192                 \r
 193                 if (bank->base != 0x80000000)\r
 194                 {\r
 195                         WARNING("overriding flash base address for STR73x device with 0x80000000");\r
 196                         bank->base = 0x80000000;\r
 197                 }\r
 198         }\r
 199         else if (strcmp(args[6], "STR75x") == 0)\r
 200         {\r
 201                 str7x_info->disable_bit = (1<<0);\r
 202                 \r
 203                 if (bank->base != 0x20000000)\r
 204                 {\r
 205                         WARNING("overriding flash base address for STR75x device with 0x20000000");\r
 206                         bank->base = 0x20000000;\r
 207                 }\r
 208         }\r
 209         else\r
 210         {\r
 211                 ERROR("unknown STR7x variant: '%s'", args[6]);\r
 212                 free(str7x_info);\r
 213                 return ERROR_FLASH_BANK_INVALID;\r
 214         }\r
 215 \r
 216         str7x_build_block_list(bank);\r
 217         \r
 218         str7x_info->write_algorithm = NULL;\r
 219         \r
 220         return ERROR_OK;\r
 221 }\r
 222 \r
 223 u32 str7x_status(struct flash_bank_s *bank)\r
 224 {\r
 225         target_t *target = bank->target;\r
 226         u32 retval;\r
 227 \r
 228         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), &retval);\r
 229 \r
 230         return retval;\r
 231 }\r
 232 \r
 233 u32 str7x_result(struct flash_bank_s *bank)\r
 234 {\r
 235         target_t *target = bank->target;\r
 236         u32 retval;\r
 237 \r
 238         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_ER), &retval);\r
 239         \r
 240         return retval;\r
 241 }\r
 242 \r
 243 int str7x_blank_check(struct flash_bank_s *bank, int first, int last)\r
 244 {\r
 245         target_t *target = bank->target;\r
 246         u8 *buffer;\r
 247         int i;\r
 248         int nBytes;\r
 249         \r
 250         if ((first < 0) || (last > bank->num_sectors))\r
 251                 return ERROR_FLASH_SECTOR_INVALID;\r
 252 \r
 253         if (bank->target->state != TARGET_HALTED)\r
 254         {\r
 255                 return ERROR_TARGET_NOT_HALTED;\r
 256         }\r
 257         \r
 258         buffer = malloc(256);\r
 259         \r
 260         for (i = first; i <= last; i++)\r
 261         {\r
 262                 bank->sectors[i].is_erased = 1;\r
 263 \r
 264                 target->type->read_memory(target, bank->base + bank->sectors[i].offset, 4, 256/4, buffer);\r
 265                 \r
 266                 for (nBytes = 0; nBytes < 256; nBytes++)\r
 267                 {\r
 268                         if (buffer[nBytes] != 0xFF)\r
 269                         {\r
 270                                 bank->sectors[i].is_erased = 0;\r
 271                                 break;\r
 272                         }\r
 273                 }       \r
 274         }\r
 275         \r
 276         free(buffer);\r
 277 \r
 278         return ERROR_OK;\r
 279 }\r
 280 \r
 281 int str7x_protect_check(struct flash_bank_s *bank)\r
 282 {\r
 283         str7x_flash_bank_t *str7x_info = bank->driver_priv;\r
 284         target_t *target = bank->target;\r
 285         \r
 286         int i;\r
 287         u32 retval;\r
 288 \r
 289         if (bank->target->state != TARGET_HALTED)\r
 290         {\r
 291                 return ERROR_TARGET_NOT_HALTED;\r
 292         }\r
 293 \r
 294         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVWPAR), &retval);\r
 295 \r
 296         for (i = 0; i < bank->num_sectors; i++)\r
 297         {\r
 298                 if (retval & str7x_info->sector_bits[i])\r
 299                         bank->sectors[i].is_protected = 0;\r
 300                 else\r
 301                         bank->sectors[i].is_protected = 1;\r
 302         }\r
 303 \r
 304         return ERROR_OK;\r
 305 }\r
 306 \r
 307 int str7x_erase(struct flash_bank_s *bank, int first, int last)\r
 308 {\r
 309         str7x_flash_bank_t *str7x_info = bank->driver_priv;\r
 310         target_t *target = bank->target;\r
 311         \r
 312         int i;\r
 313         u32 cmd;\r
 314         u32 retval;\r
 315         u32 b0_sectors = 0, b1_sectors = 0;\r
 316         \r
 317         for (i = first; i <= last; i++)\r
 318         {\r
 319                 if (str7x_info->sector_bank[i] == 0)\r
 320                         b0_sectors |= str7x_info->sector_bits[i];\r
 321                 else if (str7x_info->sector_bank[i] == 1)\r
 322                         b1_sectors |= str7x_info->sector_bits[i];\r
 323                 else\r
 324                         ERROR("BUG: str7x_info->sector_bank[i] neither 0 nor 1 (%i)", str7x_info->sector_bank[i]);\r
 325         }\r
 326         \r
 327         if (b0_sectors)\r
 328         {\r
 329                 DEBUG("b0_sectors: 0x%x", b0_sectors);\r
 330                 \r
 331                 /* clear FLASH_ER register */   \r
 332                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);\r
 333                 \r
 334                 cmd = FLASH_SER;\r
 335                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 336                 \r
 337                 cmd = b0_sectors;\r
 338                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR1), cmd);\r
 339                 \r
 340                 cmd = FLASH_SER|FLASH_WMS;\r
 341                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 342                 \r
 343                 while (((retval = str7x_status(bank)) & str7x_info->busy_bits)){\r
 344                         usleep(1000);\r
 345                 }\r
 346                 \r
 347                 retval = str7x_result(bank);\r
 348                 \r
 349                 if (retval)\r
 350                 {\r
 351                         ERROR("error erasing flash bank, FLASH_ER: 0x%x", retval);\r
 352                         return ERROR_FLASH_OPERATION_FAILED;\r
 353                 }\r
 354         }\r
 355         \r
 356         if (b1_sectors)\r
 357         {\r
 358                 DEBUG("b1_sectors: 0x%x", b1_sectors);\r
 359                 \r
 360                 /* clear FLASH_ER register */   \r
 361                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);\r
 362                 \r
 363                 cmd = FLASH_SER;\r
 364                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 365                 \r
 366                 cmd = b1_sectors;\r
 367                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR1), cmd);\r
 368                 \r
 369                 cmd = FLASH_SER|FLASH_WMS;\r
 370                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 371                 \r
 372                 while (((retval = str7x_status(bank)) & str7x_info->busy_bits)){\r
 373                         usleep(1000);\r
 374                 }\r
 375                 \r
 376                 retval = str7x_result(bank);\r
 377                 \r
 378                 if (retval)\r
 379                 {\r
 380                         ERROR("error erasing flash bank, FLASH_ER: 0x%x", retval);\r
 381                         return ERROR_FLASH_OPERATION_FAILED;\r
 382                 }\r
 383         }\r
 384         \r
 385         for (i = first; i <= last; i++)\r
 386                 bank->sectors[i].is_erased = 1;\r
 387 \r
 388         return ERROR_OK;\r
 389 }\r
 390 \r
 391 int str7x_protect(struct flash_bank_s *bank, int set, int first, int last)\r
 392 {\r
 393         str7x_flash_bank_t *str7x_info = bank->driver_priv;\r
 394         target_t *target = bank->target;\r
 395         int i;\r
 396         u32 cmd;\r
 397         u32 retval;\r
 398         u32 protect_blocks;\r
 399         \r
 400         if (bank->target->state != TARGET_HALTED)\r
 401         {\r
 402                 return ERROR_TARGET_NOT_HALTED;\r
 403         }\r
 404         \r
 405         protect_blocks = 0xFFFFFFFF;\r
 406 \r
 407         if (set)\r
 408         {\r
 409                 for (i = first; i <= last; i++)\r
 410                         protect_blocks &= ~(str7x_info->sector_bits[i]);\r
 411         }\r
 412         \r
 413         /* clear FLASH_ER register */   \r
 414         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);\r
 415 \r
 416         cmd = FLASH_SPR;\r
 417         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 418         \r
 419         cmd = str7x_get_flash_adr(bank, FLASH_NVWPAR);\r
 420         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), cmd);\r
 421         \r
 422         cmd = protect_blocks;\r
 423         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), cmd);\r
 424         \r
 425         cmd = FLASH_SPR|FLASH_WMS;\r
 426         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 427         \r
 428         while (((retval = str7x_status(bank)) & str7x_info->busy_bits)){\r
 429                 usleep(1000);\r
 430         }\r
 431         \r
 432         retval = str7x_result(bank);\r
 433         \r
 434         DEBUG("retval: 0x%8.8x", retval);\r
 435         \r
 436         if (retval & FLASH_ERER)\r
 437                 return ERROR_FLASH_SECTOR_NOT_ERASED;\r
 438         else if (retval & FLASH_WPF)\r
 439                 return ERROR_FLASH_OPERATION_FAILED;\r
 440 \r
 441         return ERROR_OK;\r
 442 }\r
 443 \r
 444 int str7x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)\r
 445 {\r
 446         str7x_flash_bank_t *str7x_info = bank->driver_priv;\r
 447         target_t *target = bank->target;\r
 448         u32 buffer_size = 8192;\r
 449         working_area_t *source;\r
 450         u32 address = bank->base + offset;\r
 451         reg_param_t reg_params[6];\r
 452         armv4_5_algorithm_t armv4_5_info;\r
 453         int retval = ERROR_OK;\r
 454         \r
 455         u32 str7x_flash_write_code[] = {\r
 456                                         /* write:                               */\r
 457                 0xe3a04201, /*  mov r4, #0x10000000     */\r
 458                 0xe5824000, /*  str r4, [r2, #0x0]      */\r
 459                 0xe5821010, /*  str r1, [r2, #0x10]     */\r
 460                 0xe4904004, /*  ldr r4, [r0], #4        */\r
 461                 0xe5824008, /*  str r4, [r2, #0x8]      */\r
 462                 0xe4904004, /*  ldr r4, [r0], #4        */\r
 463                 0xe582400c, /*  str r4, [r2, #0xc]      */\r
 464                 0xe3a04209, /*  mov r4, #0x90000000     */\r
 465                 0xe5824000, /*  str r4, [r2, #0x0]      */\r
 466                             /* busy:                            */\r
 467                 0xe5924000, /*  ldr r4, [r2, #0x0]      */\r
 468                 0xe1140005,     /*      tst r4, r5                      */\r
 469                 0x1afffffc, /*  bne busy                        */\r
 470                 0xe5924014, /*  ldr r4, [r2, #0x14]     */\r
 471                 0xe31400ff, /*  tst r4, #0xff           */\r
 472                 0x03140c01, /*  tsteq r4, #0x100        */\r
 473                 0x1a000002, /*  bne exit                        */\r
 474                 0xe2811008, /*  add r1, r1, #0x8        */\r
 475                 0xe2533001, /*  subs r3, r3, #1         */\r
 476                 0x1affffec, /*  bne write                       */\r
 477                                         /* exit:                                */\r
 478                 0xeafffffe, /*  b exit                          */\r
 479         };\r
 480         \r
 481         /* flash write code */\r
 482         if (target_alloc_working_area(target, 4 * 20, &str7x_info->write_algorithm) != ERROR_OK)\r
 483         {\r
 484                 WARNING("no working area available, can't do block memory writes");\r
 485                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
 486         };\r
 487         \r
 488         target_write_buffer(target, str7x_info->write_algorithm->address, 20 * 4, (u8*)str7x_flash_write_code);\r
 489 \r
 490         /* memory buffer */\r
 491         while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)\r
 492         {\r
 493                 buffer_size /= 2;\r
 494                 if (buffer_size <= 256)\r
 495                 {\r
 496                         /* if we already allocated the writing code, but failed to get a buffer, free the algorithm */\r
 497                         if (str7x_info->write_algorithm)\r
 498                                 target_free_working_area(target, str7x_info->write_algorithm);\r
 499                         \r
 500                         WARNING("no large enough working area available, can't do block memory writes");\r
 501                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
 502                 }\r
 503         }\r
 504         \r
 505         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;\r
 506         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;\r
 507         armv4_5_info.core_state = ARMV4_5_STATE_ARM;\r
 508         \r
 509         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);\r
 510         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);\r
 511         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);\r
 512         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);\r
 513         init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);\r
 514         init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);\r
 515         \r
 516         while (count > 0)\r
 517         {\r
 518                 u32 thisrun_count = (count > (buffer_size / 8)) ? (buffer_size / 8) : count;\r
 519                 \r
 520                 target_write_buffer(target, source->address, thisrun_count * 8, buffer);\r
 521                 \r
 522                 buf_set_u32(reg_params[0].value, 0, 32, source->address);\r
 523                 buf_set_u32(reg_params[1].value, 0, 32, address);\r
 524                 buf_set_u32(reg_params[2].value, 0, 32, str7x_get_flash_adr(bank, FLASH_CR0));\r
 525                 buf_set_u32(reg_params[3].value, 0, 32, thisrun_count);\r
 526                 buf_set_u32(reg_params[5].value, 0, 32, str7x_info->busy_bits);\r
 527         \r
 528                 if ((retval = target->type->run_algorithm(target, 0, NULL, 6, reg_params, str7x_info->write_algorithm->address, str7x_info->write_algorithm->address + (19 * 4), 10000, &armv4_5_info)) != ERROR_OK)\r
 529                 {\r
 530                         ERROR("error executing str7x flash write algorithm");\r
 531                         break;\r
 532                 }\r
 533         \r
 534                 if (buf_get_u32(reg_params[4].value, 0, 32) != 0x00)\r
 535                 {\r
 536                         retval = ERROR_FLASH_OPERATION_FAILED;\r
 537                         break;\r
 538                 }\r
 539                 \r
 540                 buffer += thisrun_count * 8;\r
 541                 address += thisrun_count * 8;\r
 542                 count -= thisrun_count;\r
 543         }\r
 544         \r
 545         target_free_working_area(target, source);\r
 546         target_free_working_area(target, str7x_info->write_algorithm);\r
 547         \r
 548         destroy_reg_param(&reg_params[0]);\r
 549         destroy_reg_param(&reg_params[1]);\r
 550         destroy_reg_param(&reg_params[2]);\r
 551         destroy_reg_param(&reg_params[3]);\r
 552         destroy_reg_param(&reg_params[4]);\r
 553         destroy_reg_param(&reg_params[5]);\r
 554         \r
 555         return retval;\r
 556 }\r
 557 \r
 558 int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)\r
 559 {\r
 560         target_t *target = bank->target;\r
 561         str7x_flash_bank_t *str7x_info = bank->driver_priv;\r
 562         u32 dwords_remaining = (count / 8);\r
 563         u32 bytes_remaining = (count & 0x00000007);\r
 564         u32 address = bank->base + offset;\r
 565         u32 bytes_written = 0;\r
 566         u32 cmd;\r
 567         u32 retval;\r
 568         u32 check_address = offset;\r
 569         int i;\r
 570         \r
 571         if (offset & 0x7)\r
 572         {\r
 573                 WARNING("offset 0x%x breaks required 8-byte alignment", offset);\r
 574                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;\r
 575         }\r
 576         \r
 577         for (i = 0; i < bank->num_sectors; i++)\r
 578         {\r
 579                 u32 sec_start = bank->sectors[i].offset;\r
 580                 u32 sec_end = sec_start + bank->sectors[i].size;\r
 581                 \r
 582                 /* check if destination falls within the current sector */\r
 583                 if ((check_address >= sec_start) && (check_address < sec_end))\r
 584                 {\r
 585                         /* check if destination ends in the current sector */\r
 586                         if (offset + count < sec_end)\r
 587                                 check_address = offset + count;\r
 588                         else\r
 589                                 check_address = sec_end;\r
 590                 }\r
 591         }\r
 592         \r
 593         if (check_address != offset + count)\r
 594                 return ERROR_FLASH_DST_OUT_OF_BANK;\r
 595                 \r
 596         /* clear FLASH_ER register */   \r
 597         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);\r
 598 \r
 599         /* multiple dwords (8-byte) to be programmed? */\r
 600         if (dwords_remaining > 0) \r
 601         {\r
 602                 /* try using a block write */\r
 603                 if ((retval = str7x_write_block(bank, buffer, offset, dwords_remaining)) != ERROR_OK)\r
 604                 {\r
 605                         if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)\r
 606                         {\r
 607                                 /* if block write failed (no sufficient working area),\r
 608                                  * we use normal (slow) single dword accesses */ \r
 609                                 WARNING("couldn't use block writes, falling back to single memory accesses");\r
 610                         }\r
 611                         else if (retval == ERROR_FLASH_OPERATION_FAILED)\r
 612                         {\r
 613                                 /* if an error occured, we examine the reason, and quit */\r
 614                                 retval = str7x_result(bank);\r
 615                                 \r
 616                                 ERROR("flash writing failed with error code: 0x%x", retval);\r
 617                                 return ERROR_FLASH_OPERATION_FAILED;\r
 618                         }\r
 619                 }\r
 620                 else\r
 621                 {\r
 622                         buffer += dwords_remaining * 8;\r
 623                         address += dwords_remaining * 8;\r
 624                         dwords_remaining = 0;\r
 625                 }\r
 626         }\r
 627 \r
 628         while (dwords_remaining > 0)\r
 629         {\r
 630                 // command\r
 631                 cmd = FLASH_DWPG;\r
 632                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 633                 \r
 634                 // address\r
 635                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), address);\r
 636                 \r
 637                 // data word 1\r
 638                 target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, buffer + bytes_written);\r
 639                 bytes_written += 4;\r
 640                 \r
 641                 // data word 2\r
 642                 target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR1), 4, 1, buffer + bytes_written);\r
 643                 bytes_written += 4;\r
 644                 \r
 645                 /* start programming cycle */\r
 646                 cmd = FLASH_DWPG | FLASH_WMS;\r
 647                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 648                 \r
 649                 while (((retval = str7x_status(bank)) & str7x_info->busy_bits))\r
 650                 {\r
 651                         usleep(1000);\r
 652                 }\r
 653                 \r
 654                 retval = str7x_result(bank);\r
 655                 \r
 656                 if (retval & FLASH_PGER)\r
 657                         return ERROR_FLASH_OPERATION_FAILED;\r
 658                 else if (retval & FLASH_WPF)\r
 659                         return ERROR_FLASH_OPERATION_FAILED;\r
 660 \r
 661                 dwords_remaining--;\r
 662                 address += 8;\r
 663         }\r
 664         \r
 665         if (bytes_remaining)\r
 666         {\r
 667                 u8 last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};\r
 668                 int i = 0;\r
 669                                 \r
 670                 while(bytes_remaining > 0)\r
 671                 {\r
 672                         last_dword[i++] = *(buffer + bytes_written); \r
 673                         bytes_remaining--;\r
 674                         bytes_written++;\r
 675                 }\r
 676                 \r
 677                 // command\r
 678                 cmd = FLASH_DWPG;\r
 679                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 680                 \r
 681                 // address\r
 682                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), address);\r
 683                 \r
 684                 // data word 1\r
 685                 target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, last_dword);\r
 686                 bytes_written += 4;\r
 687                 \r
 688                 // data word 2\r
 689                 target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR1), 4, 1, last_dword + 4);\r
 690                 bytes_written += 4;\r
 691                 \r
 692                 /* start programming cycle */\r
 693                 cmd = FLASH_DWPG | FLASH_WMS;\r
 694                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);\r
 695                 \r
 696                 while (((retval = str7x_status(bank)) & str7x_info->busy_bits))\r
 697                 {\r
 698                         usleep(1000);\r
 699                 }\r
 700                 \r
 701                 retval = str7x_result(bank);\r
 702                 \r
 703                 if (retval & FLASH_PGER)\r
 704                         return ERROR_FLASH_OPERATION_FAILED;\r
 705                 else if (retval & FLASH_WPF)\r
 706                         return ERROR_FLASH_OPERATION_FAILED;\r
 707         }\r
 708                 \r
 709         return ERROR_OK;\r
 710 }\r
 711 \r
 712 int str7x_probe(struct flash_bank_s *bank)\r
 713 {\r
 714         return ERROR_OK;\r
 715 }\r
 716 \r
 717 int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
 718 {\r
 719         return ERROR_OK;\r
 720 }\r
 721 \r
 722 int str7x_erase_check(struct flash_bank_s *bank)\r
 723 {\r
 724         return str7x_blank_check(bank, 0, bank->num_sectors - 1);\r
 725 }\r
 726 \r
 727 int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size)\r
 728 {\r
 729         snprintf(buf, buf_size, "str7x flash driver info" );\r
 730         return ERROR_OK;\r
 731 }\r
 732 \r
 733 int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
 734 {\r
 735         flash_bank_t *bank;\r
 736         target_t *target = NULL;\r
 737         str7x_flash_bank_t *str7x_info = NULL;\r
 738         \r
 739         u32 flash_cmd;\r
 740         u32 retval;\r
 741         u16 ProtectionLevel = 0;\r
 742         u16 ProtectionRegs;\r
 743         \r
 744         if (argc < 1)\r
 745         {\r
 746                 command_print(cmd_ctx, "str7x disable_jtag <bank>");\r
 747                 return ERROR_OK;        \r
 748         }\r
 749         \r
 750         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
 751         if (!bank)\r
 752         {\r
 753                 command_print(cmd_ctx, "str7x disable_jtag <bank> ok");\r
 754                 return ERROR_OK;\r
 755         }\r
 756         \r
 757         str7x_info = bank->driver_priv;\r
 758         \r
 759         target = bank->target;\r
 760         \r
 761         if (target->state != TARGET_HALTED)\r
 762         {\r
 763                 return ERROR_TARGET_NOT_HALTED;\r
 764         }\r
 765         \r
 766         /* first we get protection status */\r
 767         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR0), &retval);\r
 768 \r
 769         if (!(retval & str7x_info->disable_bit))\r
 770         {\r
 771                 ProtectionLevel = 1;\r
 772         }\r
 773         \r
 774         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR1), &retval);\r
 775         ProtectionRegs = ~(retval >> 16);\r
 776 \r
 777         while (((ProtectionRegs) != 0) && (ProtectionLevel < 16))\r
 778         {\r
 779                 ProtectionRegs >>= 1;\r
 780                 ProtectionLevel++;\r
 781         }\r
 782         \r
 783         if (ProtectionLevel == 0)\r
 784         {\r
 785                 flash_cmd = FLASH_SPR;\r
 786                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);\r
 787                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), 0x4010DFB8);\r
 788                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), 0xFFFFFFFD);\r
 789                 flash_cmd = FLASH_SPR | FLASH_WMS;\r
 790                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);\r
 791         }\r
 792         else\r
 793         {\r
 794                 flash_cmd = FLASH_SPR;\r
 795                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);\r
 796                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), 0x4010DFBC);\r
 797                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), ~(1<<(15+ProtectionLevel)));\r
 798                 flash_cmd = FLASH_SPR | FLASH_WMS;\r
 799                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);\r
 800         }\r
 801         \r
 802         return ERROR_OK;\r
 803 }\r
 804 \r