mflash.c 37.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14
/***************************************************************************
 *   Copyright (C) 2007-2008 by unsik Kim <donari75@gmail.com>             *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   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 19 20 21
 ***************************************************************************/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

22
#include "mflash.h"
23
#include <target/target.h>
24
#include <helper/time_support.h>
25
#include <helper/fileio.h>
26
#include <helper/log.h>
27

28 29 30 31
static int s3c2440_set_gpio_to_output(struct mflash_gpio_num gpio);
static int s3c2440_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val);
static int pxa270_set_gpio_to_output(struct mflash_gpio_num gpio);
static int pxa270_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val);
32

33
static struct mflash_bank *mflash_bank;
34

35
static struct mflash_gpio_drv pxa270_gpio = {
36 37 38 39 40
	.name = "pxa270",
	.set_gpio_to_output = pxa270_set_gpio_to_output,
	.set_gpio_output_val = pxa270_set_gpio_output_val
};

41
static struct mflash_gpio_drv s3c2440_gpio = {
42 43 44 45 46
	.name = "s3c2440",
	.set_gpio_to_output = s3c2440_set_gpio_to_output,
	.set_gpio_output_val = s3c2440_set_gpio_output_val
};

47 48 49 50
static struct mflash_gpio_drv *mflash_gpio[] = {
	&pxa270_gpio,
	&s3c2440_gpio,
	NULL
51 52 53 54 55 56 57 58 59 60 61
};

#define PXA270_GAFR0_L 0x40E00054
#define PXA270_GAFR3_U 0x40E00070
#define PXA270_GAFR3_U_RESERVED_BITS  0xfffc0000u
#define PXA270_GPDR0 0x40E0000C
#define PXA270_GPDR3 0x40E0010C
#define PXA270_GPDR3_RESERVED_BITS  0xfe000000u
#define PXA270_GPSR0 0x40E00018
#define PXA270_GPCR0 0x40E00024

62
static int pxa270_set_gpio_to_output(struct mflash_gpio_num gpio)
63
{
64
	uint32_t addr, value, mask;
Zachary T Welch's avatar
Zachary T Welch committed
65
	struct target *target = mflash_bank->target;
66 67
	int ret;

68
	/* remove alternate function. */
69 70 71 72
	mask = 0x3u << (gpio.num & 0xF)*2;

	addr = PXA270_GAFR0_L + (gpio.num >> 4) * 4;

73 74
	ret = target_read_u32(target, addr, &value);
	if (ret != ERROR_OK)
75 76 77 78 79 80
		return ret;

	value &= ~mask;
	if (addr == PXA270_GAFR3_U)
		value &= ~PXA270_GAFR3_U_RESERVED_BITS;

81 82
	ret = target_write_u32(target, addr, value);
	if (ret != ERROR_OK)
83 84
		return ret;

85
	/* set direction to output */
86 87 88 89
	mask = 0x1u << (gpio.num & 0x1F);

	addr = PXA270_GPDR0 + (gpio.num >> 5) * 4;

90 91
	ret = target_read_u32(target, addr, &value);
	if (ret != ERROR_OK)
92 93 94 95 96 97 98 99 100 101
		return ret;

	value |= mask;
	if (addr == PXA270_GPDR3)
		value &= ~PXA270_GPDR3_RESERVED_BITS;

	ret = target_write_u32(target, addr, value);
	return ret;
}

102
static int pxa270_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val)
103
{
104
	uint32_t addr, value, mask;
Zachary T Welch's avatar
Zachary T Welch committed
105
	struct target *target = mflash_bank->target;
106 107 108 109
	int ret;

	mask = 0x1u << (gpio.num & 0x1F);

110
	if (val)
111
		addr = PXA270_GPSR0 + (gpio.num >> 5) * 4;
112
	else
113 114
		addr = PXA270_GPCR0 + (gpio.num >> 5) * 4;

115 116
	ret = target_read_u32(target, addr, &value);
	if (ret != ERROR_OK)
117 118 119 120 121 122 123 124 125 126 127 128 129 130
		return ret;

	value |= mask;

	ret = target_write_u32(target, addr, value);

	return ret;
}

#define S3C2440_GPACON 0x56000000
#define S3C2440_GPADAT 0x56000004
#define S3C2440_GPJCON 0x560000d0
#define S3C2440_GPJDAT 0x560000d4

131
static int s3c2440_set_gpio_to_output(struct mflash_gpio_num gpio)
132
{
133
	uint32_t data, mask, gpio_con;
Zachary T Welch's avatar
Zachary T Welch committed
134
	struct target *target = mflash_bank->target;
135 136
	int ret;

137
	if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h')
138
		gpio_con = S3C2440_GPACON + (gpio.port[0] - 'a') * 0x10;
139
	else if (gpio.port[0] == 'j')
140
		gpio_con = S3C2440_GPJCON;
141
	else {
zwelch's avatar
zwelch committed
142
		LOG_ERROR("mflash: invalid port %d%s", gpio.num, gpio.port);
143
		return ERROR_COMMAND_SYNTAX_ERROR;
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162
	}

	ret = target_read_u32(target, gpio_con, &data);

	if (ret == ERROR_OK) {
		if (gpio.port[0] == 'a') {
			mask = 1 << gpio.num;
			data &= ~mask;
		} else {
			mask = 3 << gpio.num * 2;
			data &= ~mask;
			data |= (1 << gpio.num * 2);
		}

		ret = target_write_u32(target, gpio_con, data);
	}
	return ret;
}

163
static int s3c2440_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val)
164
{
165
	uint32_t data, mask, gpio_dat;
Zachary T Welch's avatar
Zachary T Welch committed
166
	struct target *target = mflash_bank->target;
167 168
	int ret;

169
	if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h')
170
		gpio_dat = S3C2440_GPADAT + (gpio.port[0] - 'a') * 0x10;
171
	else if (gpio.port[0] == 'j')
172
		gpio_dat = S3C2440_GPJDAT;
173
	else {
zwelch's avatar
zwelch committed
174
		LOG_ERROR("mflash: invalid port %d%s", gpio.num, gpio.port);
175
		return ERROR_COMMAND_SYNTAX_ERROR;
176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191
	}

	ret = target_read_u32(target, gpio_dat, &data);

	if (ret == ERROR_OK) {
		mask = 1 << gpio.num;
		if (val)
			data |= mask;
		else
			data &= ~mask;

		ret = target_write_u32(target, gpio_dat, data);
	}
	return ret;
}

192
static int mg_hdrst(uint8_t level)
193 194 195 196
{
	return mflash_bank->gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, level);
}

197
static int mg_init_gpio(void)
198
{
zwelch's avatar
zwelch committed
199
	int ret;
200
	struct mflash_gpio_drv *gpio_drv = mflash_bank->gpio_drv;
201

zwelch's avatar
zwelch committed
202 203 204
	ret = gpio_drv->set_gpio_to_output(mflash_bank->rst_pin);
	if (ret != ERROR_OK)
		return ret;
205

zwelch's avatar
zwelch committed
206 207 208
	ret = gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, 1);

	return ret;
209 210
}

211
static int mg_dsk_wait(mg_io_type_wait wait_local, uint32_t time_var)
212
{
213
	uint8_t status, error;
Zachary T Welch's avatar
Zachary T Welch committed
214
	struct target *target = mflash_bank->target;
215
	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
zwelch's avatar
zwelch committed
216
	int ret;
zwelch's avatar
zwelch committed
217
	long long t = 0;
218

Zachary T Welch's avatar
Zachary T Welch committed
219 220
	struct duration bench;
	duration_start(&bench);
221

Øyvind Harboe's avatar
Øyvind Harboe committed
222
	while (time_var) {
223

zwelch's avatar
zwelch committed
224 225 226
		ret = target_read_u8(target, mg_task_reg + MG_REG_STATUS, &status);
		if (ret != ERROR_OK)
			return ret;
227

228
		if (status & mg_io_rbit_status_busy) {
229
			if (wait_local == mg_io_wait_bsy)
230 231
				return ERROR_OK;
		} else {
232
			switch (wait_local) {
233 234 235 236 237 238 239 240 241 242 243 244 245 246 247
				case mg_io_wait_not_bsy:
					return ERROR_OK;
				case mg_io_wait_rdy_noerr:
					if (status & mg_io_rbit_status_ready)
						return ERROR_OK;
					break;
				case mg_io_wait_drq_noerr:
					if (status & mg_io_rbit_status_data_req)
						return ERROR_OK;
					break;
				default:
					break;
			}

			/* Now we check the error condition! */
248
			if (status & mg_io_rbit_status_error) {
zwelch's avatar
zwelch committed
249 250 251
				ret = target_read_u8(target, mg_task_reg + MG_REG_ERROR, &error);
				if (ret != ERROR_OK)
					return ret;
252

zwelch's avatar
zwelch committed
253 254 255
				LOG_ERROR("mflash: io error 0x%02x", error);

				return ERROR_MG_IO;
256 257
			}

258
			switch (wait_local) {
259 260 261
				case mg_io_wait_rdy:
					if (status & mg_io_rbit_status_ready)
						return ERROR_OK;
262
					/* fallthrough */
263 264 265
				case mg_io_wait_drq:
					if (status & mg_io_rbit_status_data_req)
						return ERROR_OK;
266
					/* fallthrough */
267 268 269 270 271
				default:
					break;
			}
		}

Zachary T Welch's avatar
Zachary T Welch committed
272 273 274 275 276
		ret = duration_measure(&bench);
		if (ERROR_OK == ret)
			t = duration_elapsed(&bench) * 1000.0;
		else
			LOG_ERROR("mflash: duration measurement failed: %d", ret);
277

Øyvind Harboe's avatar
Øyvind Harboe committed
278
		if (t > time_var)
279 280
			break;
	}
281

zwelch's avatar
zwelch committed
282 283
	LOG_ERROR("mflash: timeout occured");
	return ERROR_MG_TIMEOUT;
284 285
}

286
static int mg_dsk_srst(uint8_t on)
287
{
Zachary T Welch's avatar
Zachary T Welch committed
288
	struct target *target = mflash_bank->target;
289
	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
290
	uint8_t value;
291 292
	int ret;

293 294
	ret = target_read_u8(target, mg_task_reg + MG_REG_DRV_CTRL, &value);
	if (ret != ERROR_OK)
295 296
		return ret;

297
	if (on)
298
		value |= (mg_io_rbit_devc_srst);
299
	else
300 301 302 303 304 305
		value &= ~mg_io_rbit_devc_srst;

	ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_CTRL, value);
	return ret;
}

306
static int mg_dsk_io_cmd(uint32_t sect_num, uint32_t cnt, uint8_t cmd)
307
{
Zachary T Welch's avatar
Zachary T Welch committed
308
	struct target *target = mflash_bank->target;
309
	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
310
	uint8_t value;
zwelch's avatar
zwelch committed
311
	int ret;
312

zwelch's avatar
zwelch committed
313 314 315
	ret = mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL);
	if (ret != ERROR_OK)
		return ret;
316

317
	value = mg_io_rval_dev_drv_master | mg_io_rval_dev_lba_mode | ((sect_num >> 24) & 0xf);
318

zwelch's avatar
zwelch committed
319
	ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_HEAD, value);
320 321 322 323
	ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, (uint8_t)cnt);
	ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_NUM, (uint8_t)sect_num);
	ret |= target_write_u8(target, mg_task_reg + MG_REG_CYL_LOW, (uint8_t)(sect_num >> 8));
	ret |= target_write_u8(target, mg_task_reg + MG_REG_CYL_HIGH, (uint8_t)(sect_num >> 16));
324

zwelch's avatar
zwelch committed
325 326
	if (ret != ERROR_OK)
		return ret;
327

zwelch's avatar
zwelch committed
328
	return target_write_u8(target, mg_task_reg + MG_REG_COMMAND, cmd);
329 330 331 332
}

static int mg_dsk_drv_info(void)
{
Zachary T Welch's avatar
Zachary T Welch committed
333
	struct target *target = mflash_bank->target;
334
	uint32_t mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
zwelch's avatar
zwelch committed
335
	int ret;
336

337 338
	ret = mg_dsk_io_cmd(0, 1, mg_io_cmd_identify);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
339
		return ret;
340

David Brownell's avatar
David Brownell committed
341 342
	ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
343
		return ret;
344

zwelch's avatar
zwelch committed
345
	LOG_INFO("mflash: read drive info");
346

347
	if (!mflash_bank->drv_info)
348
		mflash_bank->drv_info = malloc(sizeof(struct mg_drv_info));
349

David Brownell's avatar
David Brownell committed
350 351
	ret = target_read_memory(target, mg_buff, 2,
			sizeof(mg_io_type_drv_info) >> 1,
352
			(uint8_t *)&mflash_bank->drv_info->drv_id);
zwelch's avatar
zwelch committed
353 354
	if (ret != ERROR_OK)
		return ret;
355

356 357 358
	mflash_bank->drv_info->tot_sects =
		(uint32_t)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
		+ mflash_bank->drv_info->drv_id.total_user_addressable_sectors_lo;
359

360 361 362
	return target_write_u8(target,
		mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
		mg_io_cmd_confirm_read);
363 364
}

zwelch's avatar
zwelch committed
365
static int mg_mflash_rst(void)
366
{
zwelch's avatar
zwelch committed
367
	int ret;
368

369 370
	ret = mg_init_gpio();
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
371
		return ret;
372

373 374
	ret = mg_hdrst(0);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
375
		return ret;
376

377 378
	ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
379
		return ret;
380

381 382
	ret = mg_hdrst(1);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
383
		return ret;
384

385 386
	ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
387
		return ret;
388

389 390
	ret = mg_dsk_srst(1);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
391
		return ret;
392

393 394
	ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
395
		return ret;
396

397 398
	ret = mg_dsk_srst(0);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
399 400
		return ret;

401 402
	ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
403
		return ret;
404

zwelch's avatar
zwelch committed
405 406 407 408 409 410 411
	LOG_INFO("mflash: reset ok");

	return ERROR_OK;
}

static int mg_mflash_probe(void)
{
412 413
	int ret = mg_mflash_rst();
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
414
		return ret;
415

zwelch's avatar
zwelch committed
416
	return mg_dsk_drv_info();
417 418
}

419
COMMAND_HANDLER(mg_probe_cmd)
420 421 422 423 424 425
{
	int ret;

	ret = mg_mflash_probe();

	if (ret == ERROR_OK) {
426 427 428 429
		command_print(CMD_CTX,
			"mflash (total %" PRIu32 " sectors) found at 0x%8.8" PRIx32 "",
			mflash_bank->drv_info->tot_sects,
			mflash_bank->base);
430 431 432 433 434
	}

	return ret;
}

435
static int mg_mflash_do_read_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
436
{
437
	uint32_t i, address;
438
	int ret;
Zachary T Welch's avatar
Zachary T Welch committed
439
	struct target *target = mflash_bank->target;
440
	uint8_t *buff_ptr = buff;
441

442 443
	ret = mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
444
		return ret;
445 446 447

	address = mflash_bank->base + MG_BUFFER_OFFSET;

Zachary T Welch's avatar
Zachary T Welch committed
448 449
	struct duration bench;
	duration_start(&bench);
450 451

	for (i = 0; i < sect_cnt; i++) {
zwelch's avatar
zwelch committed
452 453 454 455 456 457 458
		ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
		if (ret != ERROR_OK)
			return ret;

		ret = target_read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
		if (ret != ERROR_OK)
			return ret;
459 460 461

		buff_ptr += MG_MFLASH_SECTOR_SIZE;

462 463 464
		ret = target_write_u8(target,
				mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
				mg_io_cmd_confirm_read);
zwelch's avatar
zwelch committed
465 466
		if (ret != ERROR_OK)
			return ret;
467

468 469
		LOG_DEBUG("mflash: %" PRIu32 " (0x%8.8" PRIx32 ") sector read", sect_num + i,
			(sect_num + i) * MG_MFLASH_SECTOR_SIZE);
470

Zachary T Welch's avatar
Zachary T Welch committed
471 472
		ret = duration_measure(&bench);
		if ((ERROR_OK == ret) && (duration_elapsed(&bench) > 3)) {
duane's avatar
duane committed
473
			LOG_INFO("mflash: read %" PRIu32 "'th sectors", sect_num + i);
Zachary T Welch's avatar
Zachary T Welch committed
474
			duration_start(&bench);
475 476 477
		}
	}

zwelch's avatar
zwelch committed
478
	return mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
479 480
}

481
static int mg_mflash_read_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
482
{
483
	uint32_t quotient, residue, i;
484
	uint8_t *buff_ptr = buff;
zwelch's avatar
zwelch committed
485
	int ret = ERROR_OK;
486 487 488 489 490

	quotient = sect_cnt >> 8;
	residue = sect_cnt % 256;

	for (i = 0; i < quotient; i++) {
David Brownell's avatar
David Brownell committed
491
		LOG_DEBUG("mflash: sect num : %" PRIu32 " buff : %p",
492
			sect_num, buff_ptr);
zwelch's avatar
zwelch committed
493 494 495 496
		ret = mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
		if (ret != ERROR_OK)
			return ret;

497 498 499 500 501
		sect_num += 256;
		buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
	}

	if (residue) {
David Brownell's avatar
David Brownell committed
502
		LOG_DEBUG("mflash: sect num : %" PRIx32 " buff : %p",
503
			sect_num, buff_ptr);
zwelch's avatar
zwelch committed
504
		return mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
505 506
	}

zwelch's avatar
zwelch committed
507
	return ret;
508 509
}

510
static int mg_mflash_do_write_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt,
Martin Nowak's avatar
Martin Nowak committed
511
	uint8_t cmd)
512
{
513
	uint32_t i, address;
514
	int ret;
Zachary T Welch's avatar
Zachary T Welch committed
515
	struct target *target = mflash_bank->target;
516
	uint8_t *buff_ptr = buff;
517

518 519
	ret = mg_dsk_io_cmd(sect_num, sect_cnt, cmd);
	if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
520
		return ret;
521 522 523

	address = mflash_bank->base + MG_BUFFER_OFFSET;

Zachary T Welch's avatar
Zachary T Welch committed
524 525
	struct duration bench;
	duration_start(&bench);
526 527 528 529

	for (i = 0; i < sect_cnt; i++) {
		ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
		if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
530
			return ret;
531

zwelch's avatar
zwelch committed
532
		ret = target_write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
533
		if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
534
			return ret;
535

536 537
		buff_ptr += MG_MFLASH_SECTOR_SIZE;

538 539 540
		ret = target_write_u8(target,
				mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
				mg_io_cmd_confirm_write);
541
		if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
542
			return ret;
543

544 545
		LOG_DEBUG("mflash: %" PRIu32 " (0x%8.8" PRIx32 ") sector write", sect_num + i,
			(sect_num + i) * MG_MFLASH_SECTOR_SIZE);
546

Zachary T Welch's avatar
Zachary T Welch committed
547 548
		ret = duration_measure(&bench);
		if ((ERROR_OK == ret) && (duration_elapsed(&bench) > 3)) {
duane's avatar
duane committed
549
			LOG_INFO("mflash: wrote %" PRIu32 "'th sectors", sect_num + i);
Zachary T Welch's avatar
Zachary T Welch committed
550
			duration_start(&bench);
551 552 553
		}
	}

zwelch's avatar
zwelch committed
554 555 556 557
	if (cmd == mg_io_cmd_write)
		ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
	else
		ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_LONG);
558 559 560 561

	return ret;
}

562
static int mg_mflash_write_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
563
{
564
	uint32_t quotient, residue, i;
565
	uint8_t *buff_ptr = buff;
zwelch's avatar
zwelch committed
566
	int ret = ERROR_OK;
567 568 569 570 571

	quotient = sect_cnt >> 8;
	residue = sect_cnt % 256;

	for (i = 0; i < quotient; i++) {
572
		LOG_DEBUG("mflash: sect num : %" PRIu32 "buff : %p", sect_num,
duane's avatar
duane committed
573
			buff_ptr);
zwelch's avatar
zwelch committed
574 575 576 577
		ret = mg_mflash_do_write_sects(buff_ptr, sect_num, 256, mg_io_cmd_write);
		if (ret != ERROR_OK)
			return ret;

578 579 580 581 582
		sect_num += 256;
		buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
	}

	if (residue) {
583
		LOG_DEBUG("mflash: sect num : %" PRIu32 " buff : %p", sect_num,
duane's avatar
duane committed
584
			buff_ptr);
zwelch's avatar
zwelch committed
585
		return mg_mflash_do_write_sects(buff_ptr, sect_num, residue, mg_io_cmd_write);
586 587
	}

zwelch's avatar
zwelch committed
588
	return ret;
589 590
}

591
static int mg_mflash_read(uint32_t addr, uint8_t *buff, uint32_t len)
592
{
593 594
	uint8_t *buff_ptr = buff;
	uint8_t sect_buff[MG_MFLASH_SECTOR_SIZE];
595
	uint32_t cur_addr, next_sec_addr, end_addr, cnt, sect_num;
zwelch's avatar
zwelch committed
596
	int ret = ERROR_OK;
597 598 599 600 601 602 603 604 605

	cnt = 0;
	cur_addr = addr;
	end_addr = addr + len;

	if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {

		next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
		sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
zwelch's avatar
zwelch committed
606 607 608
		ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
		if (ret != ERROR_OK)
			return ret;
609 610

		if (end_addr < next_sec_addr) {
611 612 613 614 615 616 617
			memcpy(buff_ptr,
				sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
				end_addr - cur_addr);
			LOG_DEBUG(
				"mflash: copies %" PRIu32 " byte from sector offset 0x%8.8" PRIx32 "",
				end_addr - cur_addr,
				cur_addr);
618 619
			cur_addr = end_addr;
		} else {
620 621 622 623 624 625 626
			memcpy(buff_ptr,
				sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
				next_sec_addr - cur_addr);
			LOG_DEBUG(
				"mflash: copies %" PRIu32 " byte from sector offset 0x%8.8" PRIx32 "",
				next_sec_addr - cur_addr,
				cur_addr);
627 628 629 630 631 632 633 634 635 636 637 638 639 640 641
			buff_ptr += (next_sec_addr - cur_addr);
			cur_addr = next_sec_addr;
		}
	}

	if (cur_addr < end_addr) {

		sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
		next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;

		while (next_sec_addr <= end_addr) {
			cnt++;
			next_sec_addr += MG_MFLASH_SECTOR_SIZE;
		}

642 643 644
		if (cnt) {
			ret = mg_mflash_read_sects(buff_ptr, sect_num, cnt);
			if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
645
				return ret;
646
		}
647 648 649 650 651 652 653

		buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
		cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;

		if (cur_addr < end_addr) {

			sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
zwelch's avatar
zwelch committed
654 655 656 657
			ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
			if (ret != ERROR_OK)
				return ret;

658
			memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
duane's avatar
duane committed
659
			LOG_DEBUG("mflash: copies %u byte", (unsigned)(end_addr - cur_addr));
660 661 662
		}
	}

zwelch's avatar
zwelch committed
663
	return ret;
664 665
}

666
static int mg_mflash_write(uint32_t addr, uint8_t *buff, uint32_t len)
667
{
668 669
	uint8_t *buff_ptr = buff;
	uint8_t sect_buff[MG_MFLASH_SECTOR_SIZE];
670
	uint32_t cur_addr, next_sec_addr, end_addr, cnt, sect_num;
zwelch's avatar
zwelch committed
671
	int ret = ERROR_OK;
672 673 674 675 676 677 678 679 680

	cnt = 0;
	cur_addr = addr;
	end_addr = addr + len;

	if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {

		next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
		sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
zwelch's avatar
zwelch committed
681 682 683
		ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
		if (ret != ERROR_OK)
			return ret;
684 685

		if (end_addr < next_sec_addr) {
686 687 688 689 690 691 692
			memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
				buff_ptr,
				end_addr - cur_addr);
			LOG_DEBUG(
				"mflash: copies %" PRIu32 " byte to sector offset 0x%8.8" PRIx32 "",
				end_addr - cur_addr,
				cur_addr);
693 694
			cur_addr = end_addr;
		} else {
695 696 697 698 699 700 701
			memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
				buff_ptr,
				next_sec_addr - cur_addr);
			LOG_DEBUG(
				"mflash: copies %" PRIu32 " byte to sector offset 0x%8.8" PRIx32 "",
				next_sec_addr - cur_addr,
				cur_addr);
702 703 704 705
			buff_ptr += (next_sec_addr - cur_addr);
			cur_addr = next_sec_addr;
		}

zwelch's avatar
zwelch committed
706 707 708
		ret = mg_mflash_write_sects(sect_buff, sect_num, 1);
		if (ret != ERROR_OK)
			return ret;
709 710 711 712 713 714 715 716 717 718 719 720
	}

	if (cur_addr < end_addr) {

		sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
		next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;

		while (next_sec_addr <= end_addr) {
			cnt++;
			next_sec_addr += MG_MFLASH_SECTOR_SIZE;
		}

721 722 723
		if (cnt) {
			ret = mg_mflash_write_sects(buff_ptr, sect_num, cnt);
			if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
724
				return ret;
725
		}
726 727 728 729 730 731 732

		buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
		cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;

		if (cur_addr < end_addr) {

			sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
zwelch's avatar
zwelch committed
733 734 735 736
			ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
			if (ret != ERROR_OK)
				return ret;

737
			memcpy(sect_buff, buff_ptr, end_addr - cur_addr);
duane's avatar
duane committed
738
			LOG_DEBUG("mflash: copies %" PRIu32 " byte", end_addr - cur_addr);
zwelch's avatar
zwelch committed
739
			ret = mg_mflash_write_sects(sect_buff, sect_num, 1);
740 741 742
		}
	}

zwelch's avatar
zwelch committed
743
	return ret;
744 745
}

746
COMMAND_HANDLER(mg_write_cmd)
747
{
Zachary T Welch's avatar
Zachary T Welch committed
748
	uint32_t address, cnt, res, i;
749
	uint8_t *buffer;
750
	struct fileio *fileio;
zwelch's avatar
zwelch committed
751
	int ret;
752

753
	if (CMD_ARGC != 3)
754 755
		return ERROR_COMMAND_SYNTAX_ERROR;

756
	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
757

758
	ret = fileio_open(&fileio, CMD_ARGV[1], FILEIO_READ, FILEIO_BINARY);
zwelch's avatar
zwelch committed
759 760
	if (ret != ERROR_OK)
		return ret;
761

762
	size_t filesize;
zwelch's avatar
zwelch committed
763 764
	buffer = malloc(MG_FILEIO_CHUNK);
	if (!buffer) {
765
		fileio_close(fileio);
766 767
		return ERROR_FAIL;
	}
768
	int retval = fileio_size(fileio, &filesize);
769
	if (retval != ERROR_OK) {
770
		fileio_close(fileio);
Spencer Oliver's avatar
Spencer Oliver committed
771
		free(buffer);
772 773
		return retval;
	}
774

775 776
	cnt = filesize / MG_FILEIO_CHUNK;
	res = filesize % MG_FILEIO_CHUNK;
zwelch's avatar
zwelch committed
777

Zachary T Welch's avatar
Zachary T Welch committed
778 779
	struct duration bench;
	duration_start(&bench);
780

Zachary T Welch's avatar
Zachary T Welch committed
781
	size_t buf_cnt;
zwelch's avatar
zwelch committed
782
	for (i = 0; i < cnt; i++) {
783
		ret = fileio_read(fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt);
784
		if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
785
			goto mg_write_cmd_err;
786 787
		ret = mg_mflash_write(address, buffer, MG_FILEIO_CHUNK);
		if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
788 789 790
			goto mg_write_cmd_err;
		address += MG_FILEIO_CHUNK;
	}
791

zwelch's avatar
zwelch committed
792
	if (res) {
793
		ret = fileio_read(fileio, res, buffer, &buf_cnt);
794
		if (ret != ERROR_OK)
795
			goto mg_write_cmd_err;
796 797
		ret = mg_mflash_write(address, buffer, res);
		if (ret != ERROR_OK)
zwelch's avatar
zwelch committed
798 799
			goto mg_write_cmd_err;
	}
800

801
	if (duration_measure(&bench) == ERROR_OK) {
802 803
		command_print(CMD_CTX, "wrote %zu bytes from file %s "
			"in %fs (%0.3f kB/s)", filesize, CMD_ARGV[1],
804
			duration_elapsed(&bench), duration_kbps(&bench, filesize));
Zachary T Welch's avatar
Zachary T Welch committed
805
	}
806 807

	free(buffer);
808
	fileio_close(fileio);
809 810

	return ERROR_OK;
zwelch's avatar
zwelch committed
811 812

mg_write_cmd_err:
David Brownell's avatar