git.gag.com Git - fw/openocd/blob - src/target/avr32_mem.c

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2010 by Oleksandr Tymoshenko <gonzo@bluezbox.com>       *
   5  ***************************************************************************/
   6
   7 #ifdef HAVE_CONFIG_H
   8 #include "config.h"
   9 #endif
  10
  11 #include "target.h"
  12 #include "jtag/jtag.h"
  13 #include "avr32_jtag.h"
  14 #include "avr32_mem.h"
  15
  16 int avr32_jtag_read_memory32(struct avr32_jtag *jtag_info,
  17         uint32_t addr, int count, uint32_t *buffer)
  18 {
  19         int i, retval;
  20         uint32_t data;
  21
  22         for (i = 0; i < count; i++) {
  23                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
  24                                 addr + i*4, &data);
  25
  26                 if (retval != ERROR_OK)
  27                         return retval;
  28
  29                 /* XXX: Assume AVR32 is BE */
  30                 buffer[i] = be_to_h_u32((uint8_t *)&data);
  31         }
  32
  33         return ERROR_OK;
  34 }
  35
  36 int avr32_jtag_read_memory16(struct avr32_jtag *jtag_info,
  37         uint32_t addr, int count, uint16_t *buffer)
  38 {
  39         int i, retval;
  40         uint32_t data;
  41
  42         i = 0;
  43
  44         /* any unaligned half-words? */
  45         if (addr & 3) {
  46                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
  47                                 addr + i*2, &data);
  48
  49                 if (retval != ERROR_OK)
  50                         return retval;
  51
  52                 /* XXX: Assume AVR32 is BE */
  53                 data = be_to_h_u32((uint8_t *)&data);
  54                 buffer[i] = (data >> 16) & 0xffff;
  55                 i++;
  56         }
  57
  58         /* read all complete words */
  59         for (; i < (count & ~1); i += 2) {
  60                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
  61                                 addr + i*2, &data);
  62
  63                 if (retval != ERROR_OK)
  64                         return retval;
  65
  66                 /* XXX: Assume AVR32 is BE */
  67                 data = be_to_h_u32((uint8_t *)&data);
  68                 buffer[i] = data & 0xffff;
  69                 buffer[i+1] = (data >> 16) & 0xffff;
  70         }
  71
  72         /* last halfword */
  73         if (i < count) {
  74                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
  75                                 addr + i*2, &data);
  76
  77                 if (retval != ERROR_OK)
  78                         return retval;
  79
  80                 /* XXX: Assume AVR32 is BE */
  81                 data = be_to_h_u32((uint8_t *)&data);
  82                 buffer[i] = data & 0xffff;
  83         }
  84
  85         return ERROR_OK;
  86 }
  87
  88 int avr32_jtag_read_memory8(struct avr32_jtag *jtag_info,
  89         uint32_t addr, int count, uint8_t *buffer)
  90 {
  91         int i, j, retval;
  92         uint8_t data[4];
  93         i = 0;
  94
  95         /* Do we have non-aligned bytes? */
  96         if (addr & 3) {
  97                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
  98                                 addr + i, (uint32_t *)(void *)data);
  99
 100                 if (retval != ERROR_OK)
 101                         return retval;
 102
 103                 for (j = addr & 3; (j < 4) && (i < count); j++, i++)
 104                         buffer[i] = data[3-j];
 105         }
 106
 107         /* read all complete words */
 108         for (; i < (count & ~3); i += 4) {
 109                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
 110                                 addr + i, (uint32_t *)(void *)data);
 111
 112                 if (retval != ERROR_OK)
 113                         return retval;
 114
 115                 for (j = 0; j < 4; j++)
 116                         buffer[i+j] = data[3-j];
 117         }
 118
 119         /* remaining bytes */
 120         if (i < count) {
 121                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
 122                                 addr + i, (uint32_t *)(void *)data);
 123
 124                 if (retval != ERROR_OK)
 125                         return retval;
 126
 127                 for (j = 0; i + j < count; j++)
 128                         buffer[i+j] = data[3-j];
 129         }
 130
 131         return ERROR_OK;
 132 }
 133
 134 int avr32_jtag_write_memory32(struct avr32_jtag *jtag_info,
 135         uint32_t addr, int count, const uint32_t *buffer)
 136 {
 137         int i, retval;
 138         uint32_t data;
 139
 140         for (i = 0; i < count; i++) {
 141                 /* XXX: Assume AVR32 is BE */
 142                 h_u32_to_be((uint8_t *)&data, buffer[i]);
 143                 retval = avr32_jtag_mwa_write(jtag_info, SLAVE_HSB_UNCACHED,
 144                                 addr + i*4, data);
 145
 146                 if (retval != ERROR_OK)
 147                         return retval;
 148
 149         }
 150
 151         return ERROR_OK;
 152 }
 153
 154 int avr32_jtag_write_memory16(struct avr32_jtag *jtag_info,
 155         uint32_t addr, int count, const uint16_t *buffer)
 156 {
 157         int i, retval;
 158         uint32_t data;
 159         uint32_t data_out;
 160
 161         i = 0;
 162
 163         /*
 164          * Do we have any non-aligned half-words?
 165          */
 166         if (addr & 3) {
 167                 /*
 168                  * mwa_read will read whole world, no need to fiddle
 169                  * with address. It will be truncated in set_addr
 170                  */
 171                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
 172                                 addr, &data);
 173
 174                 if (retval != ERROR_OK)
 175                         return retval;
 176
 177                 data = be_to_h_u32((uint8_t *)&data);
 178                 data = (buffer[i] << 16) | (data & 0xffff);
 179                 h_u32_to_be((uint8_t *)&data_out, data);
 180
 181                 retval = avr32_jtag_mwa_write(jtag_info, SLAVE_HSB_UNCACHED,
 182                                 addr, data_out);
 183
 184                 if (retval != ERROR_OK)
 185                         return retval;
 186
 187                 i++;
 188         }
 189
 190         /* write all complete words */
 191         for (; i < (count & ~1); i += 2) {
 192                 /* XXX: Assume AVR32 is BE */
 193                 data = (buffer[i+1] << 16) | buffer[i];
 194                 h_u32_to_be((uint8_t *)&data_out, data);
 195
 196                 retval = avr32_jtag_mwa_write(jtag_info, SLAVE_HSB_UNCACHED,
 197                                 addr + i*2, data_out);
 198
 199                 if (retval != ERROR_OK)
 200                         return retval;
 201         }
 202
 203         /* last halfword */
 204         if (i < count) {
 205                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
 206                                 addr + i*2, &data);
 207
 208                 if (retval != ERROR_OK)
 209                         return retval;
 210
 211                 data = be_to_h_u32((uint8_t *)&data);
 212                 data &= ~0xffff;
 213                 data |= buffer[i];
 214                 h_u32_to_be((uint8_t *)&data_out, data);
 215
 216                 retval = avr32_jtag_mwa_write(jtag_info, SLAVE_HSB_UNCACHED,
 217                                 addr + i*2, data_out);
 218
 219                 if (retval != ERROR_OK)
 220                         return retval;
 221         }
 222
 223         return ERROR_OK;
 224 }
 225
 226 int avr32_jtag_write_memory8(struct avr32_jtag *jtag_info,
 227         uint32_t addr, int count, const uint8_t *buffer)
 228 {
 229         int i, j, retval;
 230         uint32_t data;
 231         uint32_t data_out;
 232
 233         i = 0;
 234
 235         /*
 236          * Do we have any non-aligned bytes?
 237          */
 238         if (addr & 3) {
 239                 /*
 240                  * mwa_read will read whole world, no need to fiddle
 241                  * with address. It will be truncated in set_addr
 242                  */
 243                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
 244                                 addr, &data);
 245
 246                 if (retval != ERROR_OK)
 247                         return retval;
 248
 249                 data = be_to_h_u32((uint8_t *)&data);
 250                 for (j = addr & 3; (j < 4) && (i < count); j++, i++) {
 251                         data &= ~(0xff << j*8);
 252                         data |= (buffer[i] << j*8);
 253                 }
 254
 255                 h_u32_to_be((uint8_t *)&data_out, data);
 256                 retval = avr32_jtag_mwa_write(jtag_info, SLAVE_HSB_UNCACHED,
 257                                 addr, data_out);
 258
 259                 if (retval != ERROR_OK)
 260                         return retval;
 261         }
 262
 263
 264         /* write all complete words */
 265         for (; i < (count & ~3); i += 4) {
 266                 data = 0;
 267
 268                 for (j = 0; j < 4; j++)
 269                         data |= (buffer[j+i] << j*8);
 270
 271                 h_u32_to_be((uint8_t *)&data_out, data);
 272
 273                 retval = avr32_jtag_mwa_write(jtag_info, SLAVE_HSB_UNCACHED,
 274                                 addr + i, data_out);
 275
 276                 if (retval != ERROR_OK)
 277                         return retval;
 278         }
 279
 280         /*
 281          * Write trailing bytes
 282          */
 283         if (i < count) {
 284                 retval = avr32_jtag_mwa_read(jtag_info, SLAVE_HSB_UNCACHED,
 285                                 addr + i, &data);
 286
 287                 if (retval != ERROR_OK)
 288                         return retval;
 289
 290                 data = be_to_h_u32((uint8_t *)&data);
 291                 for (j = 0; i < count; j++, i++) {
 292                         data &= ~(0xff << j*8);
 293                         data |= (buffer[j+i] << j*8);
 294                 }
 295
 296                 h_u32_to_be((uint8_t *)&data_out, data);
 297
 298                 retval = avr32_jtag_mwa_write(jtag_info, SLAVE_HSB_UNCACHED,
 299                                 addr+i, data_out);
 300
 301                 if (retval != ERROR_OK)
 302                         return retval;
 303         }
 304
 305         return ERROR_OK;
 306 }