blob: 29c9166484295bfb61b2de1088d4fd10e686f665 [file] [log] [blame]
wdenk43d96162003-03-06 00:02:04 +00001/*
2 * (C) Copyright 2002
3 * Kyle Harris, Nexus Technologies, Inc. kharris@nexus-tech.net
4 *
5 * (C) Copyright 2002
6 * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
7 * Marius Groeger <mgroeger@sysgo.de>
8 *
9 * (C) Copyright 2002
10 * Robert Schwebel, Pengutronix, <r.schwebel@pengutronix.de>
11 *
wdenkdc7c9a12003-03-26 06:55:25 +000012 * (C) Copyright 2002
wdenk8bde7f72003-06-27 21:31:46 +000013 * Auerswald GmbH & Co KG, Germany
wdenk3e386912003-04-05 00:53:31 +000014 * Kai-Uwe Bloem <kai-uwe.bloem@auerswald.de>
wdenkdc7c9a12003-03-26 06:55:25 +000015 *
wdenk43d96162003-03-06 00:02:04 +000016 * See file CREDITS for list of people who contributed to this
17 * project.
18 *
19 * This program is free software; you can redistribute it and/or
20 * modify it under the terms of the GNU General Public License as
21 * published by the Free Software Foundation; either version 2 of
22 * the License, or (at your option) any later version.
23 *
24 * This program is distributed in the hope that it will be useful,
25 * but WITHOUT ANY WARRANTY; without even the implied warranty of
26 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
27 * GNU General Public License for more details.
28 *
29 * You should have received a copy of the GNU General Public License
30 * along with this program; if not, write to the Free Software
31 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
32 * MA 02111-1307 USA
33 */
34
35#include <common.h>
36#include <asm/arch/pxa-regs.h>
37
wdenk47cd00f2003-03-06 13:39:27 +000038#if defined CFG_JFFS_CUSTOM_PART
39#include <jffs2/jffs2.h>
40#endif
41
42/* Debugging macros ------------------------------------------------------ */
43
44#undef FLASH_DEBUG
wdenk47cd00f2003-03-06 13:39:27 +000045
46/* Some debug macros */
47#if (FLASH_DEBUG > 2 )
48#define PRINTK3(args...) printf(args)
49#else
50#define PRINTK3(args...)
51#endif
52
53#if FLASH_DEBUG > 1
54#define PRINTK2(args...) printf(args)
55#else
56#define PRINTK2(args...)
57#endif
58
59#ifdef FLASH_DEBUG
60#define PRINTK(args...) printf(args)
61#else
62#define PRINTK(args...)
63#endif
64
65/* ------------------------------------------------------------------------ */
66
67/* Development system: we have only 16 MB Flash */
68#ifdef CONFIG_MTD_INNOKOM_16MB
69#define FLASH_BANK_SIZE 0x01000000 /* 16 MB (during development) */
70#define MAIN_SECT_SIZE 0x00020000 /* 128k per sector */
71#endif
72
73/* Production system: we have 64 MB Flash */
74#ifdef CONFIG_MTD_INNOKOM_64MB
75#define FLASH_BANK_SIZE 0x04000000 /* 64 MB */
76#define MAIN_SECT_SIZE 0x00020000 /* 128k per sector */
77#endif
wdenk43d96162003-03-06 00:02:04 +000078
79flash_info_t flash_info[CFG_MAX_FLASH_BANKS];
80
81
wdenk47cd00f2003-03-06 13:39:27 +000082#if defined CFG_JFFS_CUSTOM_PART
83
84/**
85 * jffs2_part_info - get information about a JFFS2 partition
86 *
87 * @part_num: number of the partition you want to get info about
88 * @return: struct part_info* in case of success, 0 if failure
89 */
90
91static struct part_info part;
wdenk3bac3512003-03-12 10:41:04 +000092static int current_part = -1;
wdenk47cd00f2003-03-06 13:39:27 +000093
94#ifdef CONFIG_MTD_INNOKOM_16MB
95#ifdef CONFIG_MTD_INNOKOM_64MB
96#error Please define only one CONFIG_MTD_INNOKOM_XXMB option.
97#endif
98struct part_info* jffs2_part_info(int part_num) {
wdenk3bac3512003-03-12 10:41:04 +000099 void *jffs2_priv_saved = part.jffs2_priv;
wdenk47cd00f2003-03-06 13:39:27 +0000100
101 PRINTK2("jffs2_part_info: part_num=%i\n",part_num);
102
wdenk3bac3512003-03-12 10:41:04 +0000103 if (current_part == part_num)
104 return &part;
105
wdenk47cd00f2003-03-06 13:39:27 +0000106 /* u-boot partition */
107 if(part_num==0){
wdenk47cd00f2003-03-06 13:39:27 +0000108 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000109
wdenk47cd00f2003-03-06 13:39:27 +0000110 part.offset=(char*)0x00000000;
111 part.size=256*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000112
wdenk47cd00f2003-03-06 13:39:27 +0000113 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000114 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000115
116 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
117 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk47cd00f2003-03-06 13:39:27 +0000118 }
119
120 /* primary OS+firmware partition */
121 if(part_num==1){
wdenk47cd00f2003-03-06 13:39:27 +0000122 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000123
wdenk47cd00f2003-03-06 13:39:27 +0000124 part.offset=(char*)0x00040000;
125 part.size=768*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000126
wdenk47cd00f2003-03-06 13:39:27 +0000127 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000128 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000129
130 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
131 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk47cd00f2003-03-06 13:39:27 +0000132 }
wdenk8bde7f72003-06-27 21:31:46 +0000133
wdenk47cd00f2003-03-06 13:39:27 +0000134 /* secondary OS+firmware partition */
135 if(part_num==2){
wdenk47cd00f2003-03-06 13:39:27 +0000136 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000137
wdenk47cd00f2003-03-06 13:39:27 +0000138 part.offset=(char*)0x00100000;
139 part.size=8*1024*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000140
wdenk47cd00f2003-03-06 13:39:27 +0000141 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000142 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000143
144 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
145 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk47cd00f2003-03-06 13:39:27 +0000146 }
147
148 /* data partition */
149 if(part_num==3){
wdenk47cd00f2003-03-06 13:39:27 +0000150 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000151
wdenk47cd00f2003-03-06 13:39:27 +0000152 part.offset=(char*)0x00900000;
153 part.size=7*1024*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000154
wdenk47cd00f2003-03-06 13:39:27 +0000155 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000156 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000157
158 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
159 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk3bac3512003-03-12 10:41:04 +0000160 }
wdenk8bde7f72003-06-27 21:31:46 +0000161
wdenk3bac3512003-03-12 10:41:04 +0000162 if (current_part == part_num) {
163 part.usr_priv = &current_part;
164 part.jffs2_priv = jffs2_priv_saved;
wdenk47cd00f2003-03-06 13:39:27 +0000165 return &part;
166 }
167
168 PRINTK("jffs2_part_info: end of partition table\n");
169 return 0;
170}
171#endif /* CONFIG_MTD_INNOKOM_16MB */
172
173#ifdef CONFIG_MTD_INNOKOM_64MB
174#ifdef CONFIG_MTD_INNOKOM_16MB
175#error Please define only one CONFIG_MTD_INNOKOM_XXMB option.
176#endif
177struct part_info* jffs2_part_info(int part_num) {
wdenk3bac3512003-03-12 10:41:04 +0000178 void *jffs2_priv_saved = part.jffs2_priv;
wdenk47cd00f2003-03-06 13:39:27 +0000179
180 PRINTK2("jffs2_part_info: part_num=%i\n",part_num);
181
wdenk3bac3512003-03-12 10:41:04 +0000182 if (current_part == part_num)
183 return &part;
184
wdenk47cd00f2003-03-06 13:39:27 +0000185 /* u-boot partition */
186 if(part_num==0){
wdenk47cd00f2003-03-06 13:39:27 +0000187 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000188
wdenk47cd00f2003-03-06 13:39:27 +0000189 part.offset=(char*)0x00000000;
190 part.size=256*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000191
wdenk47cd00f2003-03-06 13:39:27 +0000192 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000193 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000194
195 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
196 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk47cd00f2003-03-06 13:39:27 +0000197 }
198
199 /* primary OS+firmware partition */
200 if(part_num==1){
wdenk47cd00f2003-03-06 13:39:27 +0000201 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000202
wdenk47cd00f2003-03-06 13:39:27 +0000203 part.offset=(char*)0x00040000;
204 part.size=16*1024*1024-128*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000205
wdenk47cd00f2003-03-06 13:39:27 +0000206 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000207 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000208
209 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
210 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk47cd00f2003-03-06 13:39:27 +0000211 }
wdenk8bde7f72003-06-27 21:31:46 +0000212
wdenk47cd00f2003-03-06 13:39:27 +0000213 /* secondary OS+firmware partition */
214 if(part_num==2){
wdenk47cd00f2003-03-06 13:39:27 +0000215 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000216
wdenk47cd00f2003-03-06 13:39:27 +0000217 part.offset=(char*)0x01020000;
218 part.size=16*1024*1024-128*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000219
wdenk47cd00f2003-03-06 13:39:27 +0000220 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000221 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000222
223 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
224 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk47cd00f2003-03-06 13:39:27 +0000225 }
226
227 /* data partition */
228 if(part_num==3){
wdenk47cd00f2003-03-06 13:39:27 +0000229 memset(&part, 0, sizeof(part));
wdenk8bde7f72003-06-27 21:31:46 +0000230
wdenk47cd00f2003-03-06 13:39:27 +0000231 part.offset=(char*)0x02000000;
232 part.size=32*1024*1024;
wdenk8bde7f72003-06-27 21:31:46 +0000233
wdenk47cd00f2003-03-06 13:39:27 +0000234 /* Mark the struct as ready */
wdenk3bac3512003-03-12 10:41:04 +0000235 current_part = part_num;
wdenk47cd00f2003-03-06 13:39:27 +0000236
237 PRINTK("part.offset = 0x%08x\n",(unsigned int)part.offset);
238 PRINTK("part.size = 0x%08x\n",(unsigned int)part.size);
wdenk3bac3512003-03-12 10:41:04 +0000239 }
wdenk8bde7f72003-06-27 21:31:46 +0000240
wdenk3bac3512003-03-12 10:41:04 +0000241 if (current_part == part_num) {
242 part.usr_priv = &current_part;
243 part.jffs2_priv = jffs2_priv_saved;
wdenk47cd00f2003-03-06 13:39:27 +0000244 return &part;
245 }
246
247 PRINTK("jffs2_part_info: end of partition table\n");
248 return 0;
249}
250#endif /* CONFIG_MTD_INNOKOM_64MB */
251#endif /* defined CFG_JFFS_CUSTOM_PART */
252
253
wdenk43d96162003-03-06 00:02:04 +0000254/**
255 * flash_init: - initialize data structures for flash chips
256 *
257 * @return: size of the flash
258 */
259
260ulong flash_init(void)
261{
262 int i, j;
263 ulong size = 0;
264
265 for (i = 0; i < CFG_MAX_FLASH_BANKS; i++) {
266 ulong flashbase = 0;
267 flash_info[i].flash_id =
268 (INTEL_MANUFACT & FLASH_VENDMASK) |
269 (INTEL_ID_28F128J3 & FLASH_TYPEMASK);
270 flash_info[i].size = FLASH_BANK_SIZE;
271 flash_info[i].sector_count = CFG_MAX_FLASH_SECT;
272 memset(flash_info[i].protect, 0, CFG_MAX_FLASH_SECT);
273
274 switch (i) {
275 case 0:
276 flashbase = PHYS_FLASH_1;
277 break;
278 default:
wdenk5f535fe2003-09-18 09:21:33 +0000279 panic("configured too many flash banks!\n");
wdenk43d96162003-03-06 00:02:04 +0000280 break;
281 }
282 for (j = 0; j < flash_info[i].sector_count; j++) {
283 flash_info[i].start[j] = flashbase + j*MAIN_SECT_SIZE;
284 }
285 size += flash_info[i].size;
286 }
287
wdenk47cd00f2003-03-06 13:39:27 +0000288 /* Protect u-boot sectors */
wdenk43d96162003-03-06 00:02:04 +0000289 flash_protect(FLAG_PROTECT_SET,
290 CFG_FLASH_BASE,
wdenk47cd00f2003-03-06 13:39:27 +0000291 CFG_FLASH_BASE + (256*1024) - 1,
wdenk43d96162003-03-06 00:02:04 +0000292 &flash_info[0]);
293
294#ifdef CFG_ENV_IS_IN_FLASH
295 flash_protect(FLAG_PROTECT_SET,
296 CFG_ENV_ADDR,
297 CFG_ENV_ADDR + CFG_ENV_SIZE - 1,
298 &flash_info[0]);
299#endif
300
301 return size;
302}
303
304
305/**
306 * flash_print_info: - print information about the flash situation
307 *
308 * @param info:
309 */
310
311void flash_print_info (flash_info_t *info)
312{
313 int i, j;
314
315 for (j=0; j<CFG_MAX_FLASH_BANKS; j++) {
316
317 switch (info->flash_id & FLASH_VENDMASK) {
318
319 case (INTEL_MANUFACT & FLASH_VENDMASK):
320 printf("Intel: ");
321 break;
322 default:
323 printf("Unknown Vendor ");
324 break;
325 }
326
327 switch (info->flash_id & FLASH_TYPEMASK) {
328
329 case (INTEL_ID_28F128J3 & FLASH_TYPEMASK):
330 printf("28F128J3 (128Mbit)\n");
331 break;
332 default:
333 printf("Unknown Chip Type\n");
334 return;
335 }
336
wdenk8bde7f72003-06-27 21:31:46 +0000337 printf(" Size: %ld MB in %d Sectors\n",
wdenk43d96162003-03-06 00:02:04 +0000338 info->size >> 20, info->sector_count);
339
340 printf(" Sector Start Addresses:");
341 for (i = 0; i < info->sector_count; i++) {
342 if ((i % 5) == 0) printf ("\n ");
wdenk8bde7f72003-06-27 21:31:46 +0000343
wdenk43d96162003-03-06 00:02:04 +0000344 printf (" %08lX%s", info->start[i],
345 info->protect[i] ? " (RO)" : " ");
346 }
347 printf ("\n");
348 info++;
349 }
350}
351
352
353/**
354 * flash_erase: - erase flash sectors
355 *
356 */
357
358int flash_erase(flash_info_t *info, int s_first, int s_last)
359{
360 int flag, prot, sect;
361 int rc = ERR_OK;
362
363 if (info->flash_id == FLASH_UNKNOWN)
364 return ERR_UNKNOWN_FLASH_TYPE;
365
366 if ((s_first < 0) || (s_first > s_last)) {
367 return ERR_INVAL;
368 }
369
370 if ((info->flash_id & FLASH_VENDMASK) != (INTEL_MANUFACT & FLASH_VENDMASK))
371 return ERR_UNKNOWN_FLASH_VENDOR;
wdenk8bde7f72003-06-27 21:31:46 +0000372
wdenk43d96162003-03-06 00:02:04 +0000373 prot = 0;
374 for (sect=s_first; sect<=s_last; ++sect) {
375 if (info->protect[sect]) prot++;
376 }
377
378 if (prot) return ERR_PROTECTED;
379
380 /*
381 * Disable interrupts which might cause a timeout
382 * here. Remember that our exception vectors are
383 * at address 0 in the flash, and we don't want a
384 * (ticker) exception to happen while the flash
385 * chip is in programming mode.
386 */
387
388 flag = disable_interrupts();
389
390 /* Start erase on unprotected sectors */
391 for (sect = s_first; sect<=s_last && !ctrlc(); sect++) {
392
393 printf("Erasing sector %2d ... ", sect);
394
wdenk47cd00f2003-03-06 13:39:27 +0000395 PRINTK("\n");
396
wdenk43d96162003-03-06 00:02:04 +0000397 /* arm simple, non interrupt dependent timer */
398 reset_timer_masked();
399
400 if (info->protect[sect] == 0) { /* not protected */
wdenk47cd00f2003-03-06 13:39:27 +0000401 u16 * volatile addr = (u16 * volatile)(info->start[sect]);
wdenk43d96162003-03-06 00:02:04 +0000402
wdenk47cd00f2003-03-06 13:39:27 +0000403 PRINTK("unlocking sector\n");
404 *addr = 0x0060;
405 *addr = 0x00d0;
406 *addr = 0x00ff;
wdenk43d96162003-03-06 00:02:04 +0000407
wdenk47cd00f2003-03-06 13:39:27 +0000408 PRINTK("erasing sector\n");
409 *addr = 0x0020;
410 PRINTK("confirming erase\n");
411 *addr = 0x00D0;
wdenk43d96162003-03-06 00:02:04 +0000412
wdenk47cd00f2003-03-06 13:39:27 +0000413 while ((*addr & 0x0080) != 0x0080) {
414 PRINTK(".");
wdenk43d96162003-03-06 00:02:04 +0000415 if (get_timer_masked() > CFG_FLASH_ERASE_TOUT) {
wdenk47cd00f2003-03-06 13:39:27 +0000416 *addr = 0x00B0; /* suspend erase*/
417 *addr = 0x00FF; /* read mode */
wdenk43d96162003-03-06 00:02:04 +0000418 rc = ERR_TIMOUT;
419 goto outahere;
420 }
421 }
wdenk8bde7f72003-06-27 21:31:46 +0000422
wdenk47cd00f2003-03-06 13:39:27 +0000423 PRINTK("clearing status register\n");
wdenk8bde7f72003-06-27 21:31:46 +0000424 *addr = 0x0050;
wdenk47cd00f2003-03-06 13:39:27 +0000425 PRINTK("resetting to read mode");
wdenk8bde7f72003-06-27 21:31:46 +0000426 *addr = 0x00FF;
wdenk43d96162003-03-06 00:02:04 +0000427 }
wdenk8bde7f72003-06-27 21:31:46 +0000428
wdenk43d96162003-03-06 00:02:04 +0000429 printf("ok.\n");
430 }
431
432 if (ctrlc()) printf("User Interrupt!\n");
433
434 outahere:
435
436 /* allow flash to settle - wait 10 ms */
437 udelay_masked(10000);
438
439 if (flag) enable_interrupts();
440
441 return rc;
442}
443
444
445/**
446 * write_word: - copy memory to flash
447 *
448 * @param info:
449 * @param dest:
450 * @param data:
451 * @return:
452 */
453
454static int write_word (flash_info_t *info, ulong dest, ushort data)
455{
wdenk47cd00f2003-03-06 13:39:27 +0000456 volatile u16 *addr = (u16 *)dest, val;
wdenk43d96162003-03-06 00:02:04 +0000457 int rc = ERR_OK;
458 int flag;
459
460 /* Check if Flash is (sufficiently) erased */
461 if ((*addr & data) != data) return ERR_NOT_ERASED;
462
463 /*
464 * Disable interrupts which might cause a timeout
465 * here. Remember that our exception vectors are
466 * at address 0 in the flash, and we don't want a
467 * (ticker) exception to happen while the flash
468 * chip is in programming mode.
469 */
470 flag = disable_interrupts();
471
472 /* clear status register command */
473 *addr = 0x50;
474
475 /* program set-up command */
476 *addr = 0x40;
477
478 /* latch address/data */
479 *addr = data;
480
481 /* arm simple, non interrupt dependent timer */
482 reset_timer_masked();
483
484 /* wait while polling the status register */
485 while(((val = *addr) & 0x80) != 0x80) {
486 if (get_timer_masked() > CFG_FLASH_WRITE_TOUT) {
487 rc = ERR_TIMOUT;
488 *addr = 0xB0; /* suspend program command */
489 goto outahere;
490 }
491 }
492
493 if(val & 0x1A) { /* check for error */
494 printf("\nFlash write error %02x at address %08lx\n",
495 (int)val, (unsigned long)dest);
496 if(val & (1<<3)) {
497 printf("Voltage range error.\n");
498 rc = ERR_PROG_ERROR;
499 goto outahere;
500 }
501 if(val & (1<<1)) {
502 printf("Device protect error.\n");
503 rc = ERR_PROTECTED;
504 goto outahere;
505 }
506 if(val & (1<<4)) {
507 printf("Programming error.\n");
508 rc = ERR_PROG_ERROR;
509 goto outahere;
510 }
511 rc = ERR_PROG_ERROR;
512 goto outahere;
513 }
514
515 outahere:
516
517 *addr = 0xFF; /* read array command */
518 if (flag) enable_interrupts();
519
520 return rc;
521}
522
523
524/**
525 * write_buf: - Copy memory to flash.
526 *
527 * @param info:
528 * @param src: source of copy transaction
529 * @param addr: where to copy to
530 * @param cnt: number of bytes to copy
531 *
532 * @return error code
533 */
534
535int write_buff (flash_info_t *info, uchar *src, ulong addr, ulong cnt)
536{
537 ulong cp, wp;
538 ushort data;
539 int l;
540 int i, rc;
541
542 wp = (addr & ~1); /* get lower word aligned address */
543
544 /*
545 * handle unaligned start bytes
546 */
547 if ((l = addr - wp) != 0) {
548 data = 0;
549 for (i=0, cp=wp; i<l; ++i, ++cp) {
550 data = (data >> 8) | (*(uchar *)cp << 8);
551 }
552 for (; i<2 && cnt>0; ++i) {
553 data = (data >> 8) | (*src++ << 8);
554 --cnt;
555 ++cp;
556 }
557 for (; cnt==0 && i<2; ++i, ++cp) {
558 data = (data >> 8) | (*(uchar *)cp << 8);
559 }
560
561 if ((rc = write_word(info, wp, data)) != 0) {
562 return (rc);
563 }
564 wp += 2;
565 }
566
567 /*
568 * handle word aligned part
569 */
570 while (cnt >= 2) {
571 /* data = *((vushort*)src); */
572 data = *((ushort*)src);
573 if ((rc = write_word(info, wp, data)) != 0) {
574 return (rc);
575 }
576 src += 2;
577 wp += 2;
578 cnt -= 2;
579 }
580
581 if (cnt == 0) return ERR_OK;
582
583 /*
584 * handle unaligned tail bytes
585 */
586 data = 0;
587 for (i=0, cp=wp; i<2 && cnt>0; ++i, ++cp) {
588 data = (data >> 8) | (*src++ << 8);
589 --cnt;
590 }
591 for (; i<2; ++i, ++cp) {
592 data = (data >> 8) | (*(uchar *)cp << 8);
593 }
594
595 return write_word(info, wp, data);
596}