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