release/src-rt/linux/linux-2.6/arch/mips/brcm-boards/bcm947xx/nvram_linux.c

   1 /*
   2  * NVRAM variable manipulation (Linux kernel half)
   3  *
   4  * All Rights Reserved.
   5  *
   6  * THIS SOFTWARE IS OFFERED "AS IS", AND BROADCOM GRANTS NO WARRANTIES OF ANY
   7  * KIND, EXPRESS OR IMPLIED, BY STATUTE, COMMUNICATION OR OTHERWISE. BROADCOM
   8  * SPECIFICALLY DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS
   9  * FOR A SPECIFIC PURPOSE OR NONINFRINGEMENT CONCERNING THIS SOFTWARE.
  10  *
  11  * $Id: nvram_linux.c,v 1.8 2008/07/04 01:15:09 Exp $
  12  */
  13
  14 #include <linux/config.h>
  15 #include <linux/init.h>
  16 #include <linux/module.h>
  17 #include <linux/kernel.h>
  18 #include <linux/string.h>
  19 #include <linux/interrupt.h>
  20 #include <linux/spinlock.h>
  21 #include <linux/slab.h>
  22 #include <linux/bootmem.h>
  23 #include <linux/fs.h>
  24 #include <linux/miscdevice.h>
  25 #include <linux/mtd/mtd.h>
  26 //#include <mtd/mtd-user.h>
  27 #include <asm/addrspace.h>
  28 #include <asm/io.h>
  29 #include <asm/uaccess.h>
  30 #include <asm/cacheflush.h>
  31
  32 #include <typedefs.h>
  33 #include <bcmendian.h>
  34 #include <bcmnvram.h>
  35 #include <bcmutils.h>
  36 #include <bcmdefs.h>
  37 #include <hndsoc.h>
  38 #include <siutils.h>
  39 #include <hndmips.h>
  40 #include <sflash.h>
  41 #include <linux/delay.h>
  42 #ifdef NFLASH_SUPPORT
  43 #include <nflash.h>
  44 #endif
  45
  46 /* Temp buffer to hold the nvram transfered romboot CFE */
  47 char __initdata ram_nvram_buf[NVRAM_SPACE] __attribute__((aligned(PAGE_SIZE)));
  48
  49 /* In BSS to minimize text size and page aligned so it can be mmap()-ed */
  50 static char nvram_buf[NVRAM_SPACE] __attribute__((aligned(PAGE_SIZE)));
  51 static char *nvram_commit_buf = NULL;
  52 #ifdef RTN66U_NVRAM_64K_SUPPORT /*Only for RT-N66U upgrade from nvram 32K -> 64K*/
  53 int nvram_32_reset = 0;
  54 #endif
  55
  56 #define CFE_UPDATE  1 // added by Chen-I for mac/regulation update
  57 #ifdef CFE_UPDATE
  58 //#include <sbextif.h>
  59
  60 extern void bcm947xx_watchdog_disable(void);
  61
  62 #define CFE_SPACE       256*1024
  63 #define CFE_NVRAM_START 0x00000
  64 #define CFE_NVRAM_END   0x01fff
  65 #define CFE_NVRAM_SPACE 64*1024
  66 static struct mtd_info *cfe_mtd = NULL;
  67 static char *CFE_NVRAM_PREFIX="asuscfe";
  68 static char *CFE_NVRAM_COMMIT="asuscfecommit";
  69 static char *CFE_NVRAM_WATCHDOG="asuscfewatchdog";
  70 char *cfe_buf;// = NULL;
  71 struct nvram_header *cfe_nvram_header; // = NULL;
  72
  73 static u_int32_t cfe_offset;
  74 static u_int32_t cfe_embedded_size;
  75 static int get_embedded_block(struct mtd_info *mtd, char *buf, size_t erasesize,
  76                        u_int32_t *offset, struct nvram_header **header, u_int32_t *emb_size);
  77
  78 static int cfe_init(void);
  79 static int cfe_update(const char *keyword, const char *value);
  80 static int cfe_dump(void);
  81 static int cfe_commit(void);
  82 #endif
  83
  84 #ifdef MODULE
  85
  86 #define early_nvram_get(name) nvram_get(name)
  87
  88 #else /* !MODULE */
  89
  90 /* Global SB handle */
  91 extern si_t *bcm947xx_sih;
  92 extern spinlock_t bcm947xx_sih_lock;
  93
  94 /* Convenience */
  95 #define sih bcm947xx_sih
  96 #define sih_lock bcm947xx_sih_lock
  97 #define KB * 1024
  98 #define MB * 1024 * 1024
  99
 100 #define _nvram_safe_get(name) (_nvram_get(name) ? : "")
 101
 102 #define NLS_XFR 1              /* added by Jiahao for WL500gP */
 103 #ifdef NLS_XFR
 104
 105 #include <linux/nls.h>
 106
 107 static char *NLS_NVRAM_U2C="asusnlsu2c";
 108 static char *NLS_NVRAM_C2U="asusnlsc2u";
 109 __u16 unibuf[1024];
 110 char codebuf[1024];
 111 char tmpbuf[1024];
 112
 113 void
 114 asusnls_u2c(char *name)
 115 {
 116         char *codepage;
 117         char *xfrstr;
 118         struct nls_table *nls;
 119         int ret, len;
 120
 121         strcpy(codebuf, name);
 122         codepage=codebuf+strlen(NLS_NVRAM_U2C);
 123         if((xfrstr=strchr(codepage, '_')))
 124         {
 125                 *xfrstr=NULL;
 126                 xfrstr++;
 127                 /* debug message, start */
 128 /*
 129                 printk("%s, xfr from utf8 to %s\n", xfrstr, codepage);
 130                 int j;
 131                 printk("utf8:    %d, ", strlen(xfrstr));
 132                 for(j=0;j<strlen(xfrstr);j++)
 133                         printk("%X ", (unsigned char)xfrstr[j]);
 134                 printk("\n");
 135 */
 136                 /* debug message, end */
 137
 138                 nls=load_nls(codepage);
 139                 if(!nls)
 140                 {
 141                         printk("NLS table is null!!\n");
 142                 }
 143                 else {
 144                         len = 0;
 145                         if (ret=utf8_mbstowcs(unibuf, xfrstr, strlen(xfrstr)))
 146                         {
 147                                 int i;
 148                                 for (i = 0; (i < ret) && unibuf[i]; i++) {
 149                                         int charlen;
 150                                         charlen = nls->uni2char(unibuf[i], &name[len], NLS_MAX_CHARSET_SIZE);
 151                                         if (charlen > 0) {
 152                                                 len += charlen;
 153                                         }
 154                                         else {
 155                                                 //name[len++] = '?';
 156                                                 strcpy(name, "");
 157                                                 unload_nls(nls);
 158                                                 return;
 159                                         }
 160                                 }
 161                                 name[len] = 0;
 162                         }
 163                         unload_nls(nls);
 164                         /* debug message, start */
 165 /*
 166                         int i;
 167                         printk("unicode: %d, ", ret);
 168                         for (i=0;i<ret;i++)
 169                                 printk("%X ", unibuf[i]);
 170                         printk("\n");
 171                         printk("local:   %d, ", strlen(name));
 172                         for (i=0;i<strlen(name);i++)
 173                                 printk("%X ", (unsigned char)name[i]);
 174                         printk("\n");
 175                         printk("local:   %s\n", name);
 176 */
 177                         /* debug message, end */
 178
 179                         if(!len)
 180                         {
 181                                 printk("can not xfr from utf8 to %s\n", codepage);
 182                                 strcpy(name, "");
 183                         }
 184                 }
 185         }
 186         else
 187         {
 188                 strcpy(name, "");
 189         }
 190 }
 191
 192 void
 193 asusnls_c2u(char *name)
 194 {
 195         char *codepage;
 196         char *xfrstr;
 197         struct nls_table *nls;
 198         int ret;
 199
 200         strcpy(codebuf, name);
 201         codepage=codebuf+strlen(NLS_NVRAM_C2U);
 202         if((xfrstr=strchr(codepage, '_')))
 203         {
 204                 *xfrstr=NULL;
 205                 xfrstr++;
 206
 207                 /* debug message, start */
 208 /*
 209                 printk("%s, xfr from %s to utf8\n", xfrstr, codepage);
 210                 printk("local:   %d, ", strlen(xfrstr));
 211                 int j;
 212                 for (j=0;j<strlen(xfrstr);j++)
 213                         printk("%X ", (unsigned char)xfrstr[j]);
 214                 printk("\n");
 215                 printk("local:   %s\n", xfrstr);
 216 */
 217                 /* debug message, end */
 218
 219                 strcpy(name, "");
 220                 nls=load_nls(codepage);
 221                 if(!nls)
 222                 {
 223                         printk("NLS table is null!!\n");
 224                 }
 225                 else
 226                 {
 227                         int charlen;
 228                         int i;
 229                         int len = strlen(xfrstr);
 230                         for (i = 0; len && *xfrstr; i++, xfrstr += charlen, len -= charlen) {   /* string to unicode */
 231                                 charlen = nls->char2uni(xfrstr, len, &unibuf[i]);
 232                                 if (charlen < 1) {
 233                                         //unibuf[i] = 0x003f;     /* a question mark */
 234                                         //charlen = 1;
 235                                         strcpy(name ,"");
 236                                         unload_nls(nls);
 237                                         return;
 238                                 }
 239                         }
 240                         unibuf[i] = 0;
 241                         ret=utf8_wcstombs(name, unibuf, 1024);  /* unicode to utf-8, 1024 is size of array unibuf */
 242                         name[ret]=0;
 243                         unload_nls(nls);
 244                         /* debug message, start */
 245 /*
 246                         int k;
 247                         printk("unicode: %d, ", i);
 248                         for(k=0;k<i;k++)
 249                                 printk("%X ", unibuf[k]);
 250                         printk("\n");
 251                         printk("utf-8:    %s, %d, ", name, strlen(name));
 252                         for (i=0;i<strlen(name);i++)
 253                                 printk("%X ", (unsigned char)name[i]);
 254                         printk("\n");
 255 */
 256                         /* debug message, end */
 257                         if(!ret)
 258                         {
 259                                 printk("can not xfr from %s to utf8\n", codepage);
 260                                 strcpy(name, "");
 261                         }
 262                 }
 263         }
 264         else
 265         {
 266                 strcpy(name, "");
 267         }
 268 }
 269
 270 char *
 271 nvram_xfr(const char *buf)
 272 {
 273         char *name = tmpbuf;
 274         ssize_t ret=0;
 275
 276         //printk("nvram xfr 1: %s\n", buf);
 277         if (copy_from_user(name, buf, strlen(buf)+1)) {
 278                 ret = -EFAULT;
 279                 goto done;
 280         }
 281
 282         if (strncmp(tmpbuf, NLS_NVRAM_U2C, strlen(NLS_NVRAM_U2C))==0)
 283         {
 284                 asusnls_u2c(tmpbuf);
 285         }
 286         else if (strncmp(buf, NLS_NVRAM_C2U, strlen(NLS_NVRAM_C2U))==0)
 287         {
 288                 asusnls_c2u(tmpbuf);
 289         }
 290         else
 291         {
 292                 strcpy(tmpbuf, "");
 293                 //printk("nvram xfr 2: %s\n", tmpbuf);
 294         }
 295
 296         if (copy_to_user(buf, tmpbuf, strlen(tmpbuf)+1))
 297         {
 298                 ret = -EFAULT;
 299                 goto done;
 300         }
 301         //printk("nvram xfr 3: %s\n", tmpbuf);
 302
 303 done:
 304         if(ret==0) return tmpbuf;
 305         else return NULL;
 306 }
 307
 308 #endif
 309
 310 static int
 311 nvram_valid(struct nvram_header *header)
 312 {
 313         return (header->magic == NVRAM_MAGIC) &&
 314                 (header->len >= sizeof(struct nvram_header)) && (header->len <= NVRAM_SPACE)
 315 #if 0
 316                 && (nvram_calc_crc(header) == (uint8) header->crc_ver_init))
 317 #endif
 318         ;
 319 }
 320
 321 /* Probe for NVRAM header */
 322 static int
 323 early_nvram_init(void)
 324 {
 325         struct nvram_header *header;
 326         chipcregs_t *cc;
 327         struct sflash *info = NULL;
 328         int i;
 329         uint32 base, off, lim;
 330         u32 *src, *dst;
 331         uint32 fltype;
 332 #ifdef NFLASH_SUPPORT
 333         struct nflash *nfl_info = NULL;
 334         uint32 blocksize;
 335 #endif
 336         header = (struct nvram_header *)ram_nvram_buf;
 337
 338         if ((cc = si_setcore(sih, CC_CORE_ID, 0)) != NULL) {
 339 #ifdef NFLASH_SUPPORT
 340                 if ((sih->ccrev == 38) && ((sih->chipst & (1 << 4)) != 0)) {
 341                         fltype = NFLASH;
 342                         base = KSEG1ADDR(SI_FLASH1);
 343                 } else
 344 #endif
 345                 {
 346                         fltype = readl(&cc->capabilities) & CC_CAP_FLASH_MASK;
 347                         base = KSEG1ADDR(SI_FLASH2);
 348                 }
 349                 switch (fltype) {
 350                 case PFLASH:
 351                         lim = SI_FLASH2_SZ;
 352                         break;
 353
 354                 case SFLASH_ST:
 355                 case SFLASH_AT:
 356                         if ((info = sflash_init(sih, cc)) == NULL)
 357                                 return -1;
 358                         lim = info->size;
 359                         break;
 360 #ifdef NFLASH_SUPPORT
 361                 case NFLASH:
 362                         if ((nfl_info = nflash_init(sih, cc)) == NULL)
 363                                 return -1;
 364                         lim = SI_FLASH1_SZ;
 365                         break;
 366 #endif
 367                 case FLASH_NONE:
 368                 default:
 369                         return -1;
 370                 }
 371         } else {
 372                 /* extif assumed, Stop at 4 MB */
 373                 base = KSEG1ADDR(SI_FLASH1);
 374                 lim = SI_FLASH1_SZ;
 375         }
 376 #ifdef NFLASH_SUPPORT
 377         if (nfl_info != NULL) {
 378                 blocksize = nfl_info->blocksize;
 379                 off = blocksize;
 380                 while (off <= lim) {
 381                         if (nflash_checkbadb(sih, cc, off) != 0) {
 382                                 off += blocksize;
 383                                 continue;
 384                         }
 385                         header = (struct nvram_header *) KSEG1ADDR(base + off);
 386                         if (header->magic == NVRAM_MAGIC)
 387                                 if (nvram_calc_crc(header) == (uint8) header->crc_ver_init) {
 388                                         goto found;
 389                                 }
 390                         off += blocksize;
 391                 }
 392         } else
 393 #endif
 394         off = FLASH_MIN;
 395
 396 #ifdef RTN66U_NVRAM_64K_SUPPORT
 397         header = (struct nvram_header *) KSEG1ADDR(base + lim - 0x8000);
 398         if(header->magic==0xffffffff) {
 399                 header = (struct nvram_header *) KSEG1ADDR(base + 1 KB);
 400                 if (nvram_valid(header)) {
 401                         nvram_32_reset=1;
 402                         goto found;
 403                 }
 404         }
 405 #endif
 406
 407         while (off <= lim) {
 408                 /* Windowed flash access */
 409                 header = (struct nvram_header *) KSEG1ADDR(base + off - NVRAM_SPACE);
 410                 if (nvram_valid(header))
 411                         goto found;
 412                 off <<= 1;
 413         }
 414
 415         /* Try embedded NVRAM at 4 KB and 1 KB as last resorts */
 416         header = (struct nvram_header *) KSEG1ADDR(base + 4 KB);
 417         if (nvram_valid(header))
 418                 goto found;
 419         header = (struct nvram_header *) KSEG1ADDR(base + 1 KB);
 420         if (nvram_valid(header))
 421                 goto found;
 422
 423         return -1;
 424
 425 found:
 426         src = (u32 *) header;
 427         dst = (u32 *) nvram_buf;
 428         for (i = 0; i < sizeof(struct nvram_header); i += 4)
 429                 *dst++ = *src++;
 430         for (; i < header->len && i < NVRAM_SPACE; i += 4)
 431                 *dst++ = ltoh32(*src++);
 432
 433         return 0;
 434 }
 435
 436 /* Early (before mm or mtd) read-only access to NVRAM */
 437 static char *
 438 early_nvram_get(const char *name)
 439 {
 440         char *var, *value, *end, *eq;
 441
 442         if (!name)
 443                 return NULL;
 444
 445         /* Too early? */
 446         if (sih == NULL)
 447                 return NULL;
 448
 449         if (!nvram_buf[0])
 450                 if (early_nvram_init() != 0) {
 451                         printk("early_nvram_get: Failed reading nvram var %s\n", name);
 452                         return NULL;
 453                 }
 454
 455         /* Look for name=value and return value */
 456         var = &nvram_buf[sizeof(struct nvram_header)];
 457         end = nvram_buf + sizeof(nvram_buf) - 2;
 458         end[0] = end[1] = '\0';
 459         for (; *var; var = value + strlen(value) + 1) {
 460                 if (!(eq = strchr(var, '=')))
 461                         break;
 462                 value = eq + 1;
 463                 if ((eq - var) == strlen(name) && strncmp(var, name, (eq - var)) == 0)
 464                         return value;
 465         }
 466
 467         return NULL;
 468 }
 469
 470 static int
 471 early_nvram_getall(char *buf, int count)
 472 {
 473         char *var, *end;
 474         int len = 0;
 475
 476         /* Too early? */
 477         if (sih == NULL)
 478                 return -1;
 479
 480         if (!nvram_buf[0])
 481                 if (early_nvram_init() != 0) {
 482                         printk("early_nvram_getall: Failed reading nvram var\n");
 483                         return -1;
 484                 }
 485
 486         bzero(buf, count);
 487
 488         /* Write name=value\0 ... \0\0 */
 489         var = &nvram_buf[sizeof(struct nvram_header)];
 490         end = nvram_buf + sizeof(nvram_buf) - 2;
 491         end[0] = end[1] = '\0';
 492         for (; *var; var += strlen(var) + 1) {
 493                 if ((count - len) <= (strlen(var) + 1))
 494                         break;
 495                 len += sprintf(buf + len, "%s", var) + 1;
 496         }
 497
 498         return 0;
 499 }
 500 #endif /* !MODULE */
 501
 502 extern char * _nvram_get(const char *name);
 503 extern int _nvram_set(const char *name, const char *value);
 504 extern int _nvram_unset(const char *name);
 505 extern int _nvram_getall(char *buf, int count);
 506 extern int _nvram_commit(struct nvram_header *header);
 507 extern int _nvram_init(void *sih);
 508 extern void _nvram_exit(void);
 509
 510 /* Globals */
 511 static spinlock_t nvram_lock = SPIN_LOCK_UNLOCKED;
 512 static struct semaphore nvram_sem;
 513 static unsigned long nvram_offset = 0;
 514 static int nvram_major = -1;
 515 static struct class *nvram_class = NULL;
 516 static struct mtd_info *nvram_mtd = NULL;
 517
 518 int
 519 _nvram_read(char *buf)
 520 {
 521         struct nvram_header *header = (struct nvram_header *) buf;
 522         size_t len;
 523         int offset = 0;
 524
 525         if (nvram_mtd) {
 526 #ifdef NFLASH_SUPPORT
 527                 if (nvram_mtd->type == MTD_NANDFLASH)
 528                         offset = 0;
 529                 else
 530 #endif
 531                         offset = nvram_mtd->size - NVRAM_SPACE;
 532         }
 533
 534 #ifdef RTN66U_NVRAM_64K_SUPPORT /*Only for RT-N66U upgrade from nvram 32K -> 64K*/
 535         if (nvram_32_reset==1 ||
 536             !nvram_mtd ||
 537             nvram_mtd->read(nvram_mtd, offset, NVRAM_SPACE, &len, buf) ||
 538             len != NVRAM_SPACE ||
 539             !nvram_valid(header)) {
 540                 nvram_32_reset=0;
 541 #else
 542         if (!nvram_mtd ||
 543             nvram_mtd->read(nvram_mtd, offset, NVRAM_SPACE, &len, buf) ||
 544             len != NVRAM_SPACE ||
 545             !nvram_valid(header)) {
 546 #endif
 547                 /* Maybe we can recover some data from early initialization */
 548                 memcpy(buf, nvram_buf, NVRAM_SPACE);
 549         }
 550
 551         return 0;
 552 }
 553
 554 struct nvram_tuple *
 555 _nvram_realloc(struct nvram_tuple *t, const char *name, const char *value)
 556 {
 557         if ((nvram_offset + strlen(value) + 1) > NVRAM_SPACE)
 558                 return NULL;
 559
 560         if (!t) {
 561                 if (!(t = kmalloc(sizeof(struct nvram_tuple) + strlen(name) + 1, GFP_ATOMIC)))
 562                         return NULL;
 563
 564                 /* Copy name */
 565                 t->name = (char *) &t[1];
 566                 strcpy(t->name, name);
 567
 568                 t->value = NULL;
 569         }
 570
 571         /* Copy value */
 572         if (!t->value || strcmp(t->value, value)) {
 573                 t->value = &nvram_buf[nvram_offset];
 574                 strcpy(t->value, value);
 575                 nvram_offset += strlen(value) + 1;
 576         }
 577
 578         return t;
 579 }
 580
 581 void
 582 _nvram_free(struct nvram_tuple *t)
 583 {
 584         if (!t)
 585                 nvram_offset = 0;
 586         else
 587                 kfree(t);
 588 }
 589
 590 int
 591 nvram_init(void *sih)
 592 {
 593         return 0;
 594 }
 595
 596 int
 597 nvram_set(const char *name, const char *value)
 598 {
 599         unsigned long flags;
 600         int ret;
 601         struct nvram_header *header;
 602
 603         spin_lock_irqsave(&nvram_lock, flags);
 604
 605 #ifdef CFE_UPDATE //write back to default sector as well, Chen-I
 606         if(strncmp(name, CFE_NVRAM_PREFIX, strlen(CFE_NVRAM_PREFIX))==0)
 607         {
 608                 if(strcmp(name, CFE_NVRAM_COMMIT)==0)
 609                         cfe_commit();
 610                 else if(strcmp(name, "asuscfe_dump") == 0)
 611                         ret = cfe_dump();
 612                 else if(strcmp(name, CFE_NVRAM_WATCHDOG)==0)
 613                 {
 614                         bcm947xx_watchdog_disable();
 615                 }
 616                 else
 617                 {
 618                         cfe_update(name+strlen(CFE_NVRAM_PREFIX), value);
 619                         _nvram_set(name+strlen(CFE_NVRAM_PREFIX), value);
 620                 }
 621         }
 622         else
 623 #endif
 624         if ((ret = _nvram_set(name, value))) {
 625                 /* Consolidate space and try again */
 626                 if ((header = kmalloc(NVRAM_SPACE, GFP_ATOMIC))) {
 627                         if (_nvram_commit(header) == 0)
 628                                 ret = _nvram_set(name, value);
 629                         kfree(header);
 630                 }
 631         }
 632         spin_unlock_irqrestore(&nvram_lock, flags);
 633
 634         return ret;
 635 }
 636
 637 char *
 638 real_nvram_get(const char *name)
 639 {
 640         unsigned long flags;
 641         char *value;
 642
 643         spin_lock_irqsave(&nvram_lock, flags);
 644         value = _nvram_get(name);
 645         spin_unlock_irqrestore(&nvram_lock, flags);
 646
 647         return value;
 648 }
 649
 650 char *
 651 nvram_get(const char *name)
 652 {
 653         if (nvram_major >= 0)
 654                 return real_nvram_get(name);
 655         else
 656                 return early_nvram_get(name);
 657 }
 658
 659 int
 660 nvram_unset(const char *name)
 661 {
 662         unsigned long flags;
 663         int ret;
 664
 665         spin_lock_irqsave(&nvram_lock, flags);
 666 #ifdef CFE_UPDATE //unset variable in embedded nvram
 667         if(strncmp(name, CFE_NVRAM_PREFIX, strlen(CFE_NVRAM_PREFIX))==0)
 668         {
 669                 if((ret = cfe_update(name+strlen(CFE_NVRAM_PREFIX), NULL)) == 0)
 670                 {
 671                         ret = _nvram_unset(name+strlen(CFE_NVRAM_PREFIX));
 672                 }
 673         }
 674         else
 675 #endif
 676         ret = _nvram_unset(name);
 677         spin_unlock_irqrestore(&nvram_lock, flags);
 678
 679         return ret;
 680 }
 681
 682 static void
 683 erase_callback(struct erase_info *done)
 684 {
 685         wait_queue_head_t *wait_q = (wait_queue_head_t *) done->priv;
 686         wake_up(wait_q);
 687 }
 688
 689 #ifdef NFLASH_SUPPORT
 690 int
 691 nvram_nflash_commit(void)
 692 {
 693         char *buf;
 694         size_t len, magic_len;
 695         unsigned int i;
 696         int ret;
 697         struct nvram_header *header;
 698         unsigned long flags;
 699         u_int32_t offset;
 700         struct erase_info erase;
 701
 702         if (!(buf = kmalloc(NVRAM_SPACE, GFP_KERNEL))) {
 703                 printk("nvram_commit: out of memory\n");
 704                 return -ENOMEM;
 705         }
 706
 707         down(&nvram_sem);
 708
 709         offset = 0;
 710         header = (struct nvram_header *)buf;
 711         header->magic = NVRAM_MAGIC;
 712         /* reset MAGIC before we regenerate the NVRAM,
 713          * otherwise we'll have an incorrect CRC
 714          */
 715         /* Regenerate NVRAM */
 716         spin_lock_irqsave(&nvram_lock, flags);
 717         ret = _nvram_commit(header);
 718         spin_unlock_irqrestore(&nvram_lock, flags);
 719         if (ret)
 720                 goto done;
 721
 722         /* Write partition up to end of data area */
 723         i = header->len;
 724         ret = nvram_mtd->write(nvram_mtd, offset, i, &len, buf);
 725         if (ret || len != i) {
 726                 printk("nvram_commit: write error\n");
 727                 ret = -EIO;
 728                 goto done;
 729         }
 730
 731 done:
 732         up(&nvram_sem);
 733         kfree(buf);
 734         return ret;
 735 }
 736 #endif
 737
 738 //#define NVRAM2HANDLER 1
 739
 740 #ifdef NVRAM2HANDLER
 741 u_int32_t find_next_header_len(struct nvram_header *header)
 742 {
 743         struct nvram_header *hdrptr;
 744         char *ptr;
 745         u_int32_t i, start;
 746
 747         ptr = (char *)header;
 748
 749         start = ((header->len)/16) * 16;
 750
 751         //printk("header : %x, start : %x\n", ptr, start);
 752
 753         for(i==start;i<NVRAM_SPACE;i+=16)
 754         {
 755                 hdrptr = (struct nvram_header *)&ptr[i];
 756                 if(hdrptr->magic==NVRAM_MAGIC) {
 757                         //printk("got next header: %x %x %x\n", ptr, &ptr[i], hdrptr->len);
 758                         return hdrptr->len;
 759                 }
 760         }
 761
 762         return 0;
 763 }
 764 #endif
 765
 766 int
 767 nvram_commit(void)
 768 {
 769 #if 0
 770         char *buf;
 771 #endif
 772         size_t erasesize, len, magic_len;
 773         unsigned int i;
 774         int ret;
 775         struct nvram_header *header;
 776         unsigned long flags;
 777         u_int32_t offset;
 778         DECLARE_WAITQUEUE(wait, current);
 779         wait_queue_head_t wait_q;
 780         struct erase_info erase;
 781         u_int32_t magic_offset = 0; /* Offset for writing MAGIC # */
 782         u_int32_t nvramlen;
 783
 784         if (!nvram_mtd) {
 785                 printk("nvram_commit: NVRAM not found\n");
 786                 return -ENODEV;
 787         }
 788
 789         if (in_interrupt()) {
 790                 printk("nvram_commit: not committing in interrupt\n");
 791                 return -EINVAL;
 792         }
 793
 794 #ifdef NFLASH_SUPPORT
 795         if (nvram_mtd->type == MTD_NANDFLASH)
 796                 return nvram_nflash_commit();
 797 #endif
 798         /* Backup sector blocks to be erased */
 799         erasesize = ROUNDUP(NVRAM_SPACE, nvram_mtd->erasesize);
 800 #if 0
 801         if (!(buf = kmalloc(erasesize, GFP_KERNEL))) {
 802                 printk("nvram_commit: out of memory\n");
 803                 return -ENOMEM;
 804         }
 805 #endif
 806         down(&nvram_sem);
 807
 808         if ((i = erasesize - NVRAM_SPACE) > 0) {
 809                 offset = nvram_mtd->size - erasesize;
 810                 len = 0;
 811                 ret = nvram_mtd->read(nvram_mtd, offset, i, &len, nvram_commit_buf);
 812                 if (ret || len != i) {
 813                         printk("nvram_commit: read error ret = %d, len = %d/%d\n", ret, len, i);
 814                         ret = -EIO;
 815                         goto done;
 816                 }
 817                 header = (struct nvram_header *)(nvram_commit_buf + i);
 818                 magic_offset = i + ((void *)&header->magic - (void *)header);
 819         } else {
 820                 offset = nvram_mtd->size - NVRAM_SPACE;
 821                 magic_offset = ((void *)&header->magic - (void *)header);
 822                 header = (struct nvram_header *)nvram_commit_buf;
 823         }
 824
 825         /* clear the existing magic # to mark the NVRAM as unusable
 826          * we can pull MAGIC bits low without erase
 827          */
 828         header->magic = NVRAM_CLEAR_MAGIC; /* All zeros magic */
 829         /* Unlock sector blocks */
 830         if (nvram_mtd->unlock)
 831                 nvram_mtd->unlock(nvram_mtd, offset, nvram_mtd->erasesize);
 832         ret = nvram_mtd->write(nvram_mtd, offset + magic_offset, sizeof(header->magic),
 833                 &magic_len, (char *)&header->magic);
 834         if (ret || magic_len != sizeof(header->magic)) {
 835                 printk("nvram_commit: clear MAGIC error\n");
 836                 ret = -EIO;
 837                 goto done;
 838         }
 839
 840         header->magic = NVRAM_MAGIC;
 841         /* reset MAGIC before we regenerate the NVRAM,
 842          * otherwise we'll have an incorrect CRC
 843          */
 844         /* Regenerate NVRAM */
 845         spin_lock_irqsave(&nvram_lock, flags);
 846         ret = _nvram_commit(header);
 847         spin_unlock_irqrestore(&nvram_lock, flags);
 848         if (ret)
 849                 goto done;
 850
 851 #ifdef NVRAM2HANDLER
 852         nvramlen = header->len + find_next_header_len(header);
 853         //printk("nvramlen: %x\n", nvramlen);
 854 #else
 855         nvramlen = header->len;
 856 #endif
 857
 858         /* Erase sector blocks */
 859         init_waitqueue_head(&wait_q);
 860         for (; offset < nvram_mtd->size - NVRAM_SPACE + nvramlen;
 861                 offset += nvram_mtd->erasesize) {
 862
 863                 erase.mtd = nvram_mtd;
 864                 erase.addr = offset;
 865                 erase.len = nvram_mtd->erasesize;
 866                 erase.callback = erase_callback;
 867                 erase.priv = (u_long) &wait_q;
 868
 869                 set_current_state(TASK_INTERRUPTIBLE);
 870                 add_wait_queue(&wait_q, &wait);
 871
 872                 /* Unlock sector blocks */
 873                 if (nvram_mtd->unlock)
 874                         nvram_mtd->unlock(nvram_mtd, offset, nvram_mtd->erasesize);
 875
 876                 if ((ret = nvram_mtd->erase(nvram_mtd, &erase))) {
 877                         set_current_state(TASK_RUNNING);
 878                         remove_wait_queue(&wait_q, &wait);
 879                         printk("nvram_commit: erase error\n");
 880                         goto done;
 881                 }
 882
 883                 /* Wait for erase to finish */
 884                 schedule();
 885                 remove_wait_queue(&wait_q, &wait);
 886         }
 887
 888         /* Write partition up to end of data area */
 889         header->magic = NVRAM_INVALID_MAGIC; /* All ones magic */
 890         offset = nvram_mtd->size - erasesize;
 891         i = erasesize - NVRAM_SPACE + nvramlen;
 892         ret = nvram_mtd->write(nvram_mtd, offset, i, &len, nvram_commit_buf);
 893         if (ret || len != i) {
 894                 printk("nvram_commit: write error\n");
 895                 ret = -EIO;
 896                 goto done;
 897         }
 898
 899         /* Now mark the NVRAM in flash as "valid" by setting the correct
 900          * MAGIC #
 901          */
 902         header->magic = NVRAM_MAGIC;
 903         ret = nvram_mtd->write(nvram_mtd, offset + magic_offset, sizeof(header->magic),
 904                 &magic_len, (char *)&header->magic);
 905         if (ret || magic_len != sizeof(header->magic)) {
 906                 printk("nvram_commit: write MAGIC error\n");
 907                 ret = -EIO;
 908                 goto done;
 909         }
 910
 911         offset = nvram_mtd->size - erasesize;
 912         ret = nvram_mtd->read(nvram_mtd, offset, 4, &len, nvram_commit_buf);
 913
 914 #ifdef RTN66U_NVRAM_64K_SUPPORT /*Only for RT-N66U upgrade from nvram 32K -> 64K*/
 915         if(nvramlen<0x8001){
 916                 char *log_buf;
 917                 u_int32_t offset_t;
 918                 size_t log_len;
 919
 920                 offset_t = 0x18000;
 921                 log_buf =0x01020304;
 922                 ret = nvram_mtd->write(nvram_mtd, offset_t, sizeof(log_buf), &log_len, &log_buf);
 923         }
 924 #endif
 925
 926 done:
 927         up(&nvram_sem);
 928 #if 0
 929         kfree(buf);
 930 #endif
 931         return ret;
 932 }
 933
 934 int
 935 nvram_getall(char *buf, int count)
 936 {
 937         unsigned long flags;
 938         int ret;
 939
 940         spin_lock_irqsave(&nvram_lock, flags);
 941         if (nvram_major >= 0)
 942                 ret = _nvram_getall(buf, count);
 943         else
 944                 ret = early_nvram_getall(buf, count);
 945         spin_unlock_irqrestore(&nvram_lock, flags);
 946
 947         return ret;
 948 }
 949
 950 EXPORT_SYMBOL(nvram_init);
 951 EXPORT_SYMBOL(nvram_get);
 952 EXPORT_SYMBOL(nvram_getall);
 953 EXPORT_SYMBOL(nvram_set);
 954 EXPORT_SYMBOL(nvram_unset);
 955 EXPORT_SYMBOL(nvram_commit);
 956
 957 /* User mode interface below */
 958
 959 static ssize_t
 960 dev_nvram_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 961 {
 962         char tmp[100], *name = tmp, *value;
 963         ssize_t ret;
 964         unsigned long off;
 965
 966         if (count > sizeof(tmp)) {
 967                 if (!(name = kmalloc(count, GFP_KERNEL)))
 968                         return -ENOMEM;
 969         }
 970
 971         if (copy_from_user(name, buf, count)) {
 972                 ret = -EFAULT;
 973                 goto done;
 974         }
 975
 976         if (*name == '\0') {
 977                 /* Get all variables */
 978                 ret = nvram_getall(name, count);
 979                 if (ret == 0) {
 980                         if (copy_to_user(buf, name, count)) {
 981                                 ret = -EFAULT;
 982                                 goto done;
 983                         }
 984                         ret = count;
 985                 }
 986         } else {
 987                 if (!(value = nvram_get(name))) {
 988                         ret = 0;
 989                         goto done;
 990                 }
 991
 992                 /* Provide the offset into mmap() space */
 993                 off = (unsigned long) value - (unsigned long) nvram_buf;
 994
 995                 if (put_user(off, (unsigned long *) buf)) {
 996                         ret = -EFAULT;
 997                         goto done;
 998                 }
 999
1000                 ret = sizeof(unsigned long);
1001         }
1002
1003         flush_cache_all();
1004
1005 done:
1006         if (name != tmp)
1007                 kfree(name);
1008
1009         return ret;
1010 }
1011
1012 static ssize_t
1013 dev_nvram_write(struct file *file, const char *buf, size_t count, loff_t *ppos)
1014 {
1015         char tmp[100], *name = tmp, *value;
1016         ssize_t ret;
1017
1018         if (count > sizeof(tmp)) {
1019                 if (!(name = kmalloc(count, GFP_KERNEL)))
1020                         return -ENOMEM;
1021         }
1022
1023         if (copy_from_user(name, buf, count)) {
1024                 ret = -EFAULT;
1025                 goto done;
1026         }
1027
1028         value = name;
1029         name = strsep(&value, "=");
1030         if (value)
1031                 ret = nvram_set(name, value) ? : count;
1032         else
1033                 ret = nvram_unset(name) ? : count;
1034
1035 done:
1036         if (name != tmp)
1037                 kfree(name);
1038
1039         return ret;
1040 }
1041
1042 static int
1043 dev_nvram_ioctl(struct inode *inode, struct file *file, unsigned int cmd, unsigned long arg)
1044 {
1045         if (cmd != NVRAM_MAGIC)
1046                 return -EINVAL;
1047
1048 #ifndef NLS_XFR
1049         return nvram_commit();
1050 #else
1051         if(arg == 0)
1052                 return nvram_commit();
1053         else {
1054                 if(nvram_xfr((char *)arg)==NULL) return -EFAULT;
1055                 else return 0;
1056         }
1057 #endif  // NLS_XFR
1058 }
1059
1060 static int
1061 dev_nvram_mmap(struct file *file, struct vm_area_struct *vma)
1062 {
1063         unsigned long offset = __pa(nvram_buf) >> PAGE_SHIFT;
1064
1065         if (remap_pfn_range(vma, vma->vm_start, offset,
1066                             vma->vm_end - vma->vm_start,
1067                             vma->vm_page_prot))
1068                 return -EAGAIN;
1069
1070         return 0;
1071 }
1072
1073 static int
1074 dev_nvram_open(struct inode *inode, struct file * file)
1075 {
1076         return 0;
1077 }
1078
1079 static int
1080 dev_nvram_release(struct inode *inode, struct file * file)
1081 {
1082         return 0;
1083 }
1084
1085 static struct file_operations dev_nvram_fops = {
1086         owner:          THIS_MODULE,
1087         open:           dev_nvram_open,
1088         release:        dev_nvram_release,
1089         read:           dev_nvram_read,
1090         write:          dev_nvram_write,
1091         ioctl:          dev_nvram_ioctl,
1092         mmap:           dev_nvram_mmap
1093 };
1094
1095 static void
1096 dev_nvram_exit(void)
1097 {
1098         int order = 0;
1099         struct page *page, *end;
1100
1101         if (nvram_class) {
1102                 class_device_destroy(nvram_class, MKDEV(nvram_major, 0));
1103                 class_destroy(nvram_class);
1104         }
1105
1106         if (nvram_major >= 0)
1107                 unregister_chrdev(nvram_major, "nvram");
1108
1109         if (nvram_mtd)
1110                 put_mtd_device(nvram_mtd);
1111
1112         while ((PAGE_SIZE << order) < NVRAM_SPACE)
1113                 order++;
1114         end = virt_to_page(nvram_buf + (PAGE_SIZE << order) - 1);
1115         for (page = virt_to_page(nvram_buf); page <= end; page++)
1116                 ClearPageReserved(page);
1117
1118         _nvram_exit();
1119 }
1120
1121 static int
1122 dev_nvram_init(void)
1123 {
1124         int order = 0, ret = 0;
1125         struct page *page, *end;
1126         unsigned int i;
1127         osl_t *osh;
1128
1129         /* Allocate and reserve memory to mmap() */
1130         while ((PAGE_SIZE << order) < NVRAM_SPACE)
1131                 order++;
1132         end = virt_to_page(nvram_buf + (PAGE_SIZE << order) - 1);
1133         for (page = virt_to_page(nvram_buf); page <= end; page++) {
1134                 SetPageReserved(page);
1135         }
1136
1137 #if defined(CONFIG_MTD) || defined(CONFIG_MTD_MODULE)
1138         /* Find associated MTD device */
1139         for (i = 0; i < MAX_MTD_DEVICES; i++) {
1140                 nvram_mtd = get_mtd_device(NULL, i);
1141                 if (!IS_ERR(nvram_mtd)) {
1142                         if (!strcmp(nvram_mtd->name, "nvram") &&
1143                             nvram_mtd->size >= NVRAM_SPACE) {
1144                                 break;
1145                         }
1146                         put_mtd_device(nvram_mtd);
1147                 }
1148         }
1149         if (i >= MAX_MTD_DEVICES)
1150                 nvram_mtd = NULL;
1151 #endif
1152
1153         /* Initialize hash table lock */
1154         spin_lock_init(&nvram_lock);
1155
1156         /* Initialize commit semaphore */
1157         init_MUTEX(&nvram_sem);
1158
1159         /* Register char device */
1160         if ((nvram_major = register_chrdev(0, "nvram", &dev_nvram_fops)) < 0) {
1161                 ret = nvram_major;
1162                 goto err;
1163         }
1164
1165         if (si_osh(sih) == NULL) {
1166                 osh = osl_attach(NULL, SI_BUS, FALSE);
1167                 if (osh == NULL) {
1168                         printk("Error allocating osh\n");
1169                         unregister_chrdev(nvram_major, "nvram");
1170                         goto err;
1171                 }
1172                 si_setosh(sih, osh);
1173         }
1174
1175 printk("dev_nvram_init: _nvram_init\n");
1176         /* Initialize hash table */
1177         _nvram_init(sih);
1178
1179         /* Create /dev/nvram handle */
1180         nvram_class = class_create(THIS_MODULE, "nvram");
1181         if (IS_ERR(nvram_class)) {
1182                 printk("Error creating nvram class\n");
1183                 goto err;
1184         }
1185
1186         /* Add the device nvram0 */
1187         class_device_create(nvram_class, NULL, MKDEV(nvram_major, 0), NULL, "nvram");
1188
1189         /* reserve commit read buffer */
1190         /* Backup sector blocks to be erased */
1191         if (!(nvram_commit_buf = kmalloc(ROUNDUP(NVRAM_SPACE, nvram_mtd->erasesize), GFP_KERNEL))) {
1192                 printk("dev_nvram_init: nvram_commit_buf out of memory\n");
1193                 goto err;
1194         }
1195
1196         /* Set the SDRAM NCDL value into NVRAM if not already done */
1197         if (getintvar(NULL, "sdram_ncdl") == 0) {
1198                 unsigned int ncdl;
1199                 char buf[] = "0x00000000";
1200
1201                 if ((ncdl = si_memc_get_ncdl(sih))) {
1202                         sprintf(buf, "0x%08x", ncdl);
1203                         nvram_set("sdram_ncdl", buf);
1204                         nvram_commit();
1205                 }
1206         }
1207
1208         return 0;
1209
1210 err:
1211         dev_nvram_exit();
1212         return ret;
1213 }
1214
1215 #ifdef CFE_UPDATE
1216 int get_embedded_block(struct mtd_info *mtd, char *buf, size_t erasesize,
1217                        u_int32_t *offset, struct nvram_header **header, u_int32_t *emb_size)
1218 {
1219         size_t len;
1220         struct nvram_header *nvh;
1221
1222 #ifdef CONFIG_RTAN23 /*for AMCC RTAN23 */
1223         *offset = mtd->size - erasesize; /*/at the end of mtd */
1224         *emb_size = 8*1024 - 16; /*/8K - 16 byte */
1225         printk("get_embedded_block: mtd->size(%08x) erasesize(%08x) offset(%08x) emb_size(%08x)\n", mtd->size, erasesize, *offset, *emb_size);
1226         cfe_mtd->read(mtd, *offset, erasesize, &len, buf);
1227         if(len != erasesize)
1228                 return -EIO;
1229
1230         /* find nvram header */
1231         nvh = (struct nvram_header *)(buf + erasesize - 8*1024);
1232         if (nvh->magic == NVRAM_MAGIC)
1233         {
1234                 *header = nvh;
1235                 return 0;
1236         }
1237
1238 #else /* for Broadcom WL500 serials */
1239         *offset = 0; /* from the mtd start */
1240         *emb_size = 4096; /* 1K byte */
1241         printk("get_embedded_block: mtd->size(%08x) erasesize(%08x) offset(%08x) emb_size(%08x)\n", mtd->size, erasesize, *offset, *emb_size);
1242         cfe_mtd->read(mtd, *offset, erasesize, &len, buf);
1243         if(len != erasesize)
1244                 return -EIO;
1245
1246         /* find nvram header */
1247         nvh = (struct nvram_header *)(buf + (4 * 1024));
1248         if (nvh->magic == NVRAM_MAGIC)
1249         {
1250                 *header = nvh;
1251                 return 0;
1252         }
1253         nvh = (struct nvram_header *)(buf + (1 * 1024));
1254         if (nvh->magic == NVRAM_MAGIC)
1255         {
1256                 *header = nvh;
1257                 return 0;
1258         }
1259 #endif
1260         printk("get_embedded_block: no nvram magic found\n");
1261         return -ENXIO;
1262 }
1263
1264 static int cfe_init(void)
1265 {
1266         size_t erasesize;
1267         int i;
1268         int ret = 0;
1269 printk("!!! cfe_init !!!\n");
1270         /* Find associated MTD device */
1271         for (i = 0; i < MAX_MTD_DEVICES; i++) {
1272                 cfe_mtd = get_mtd_device(NULL, i);
1273                 if (cfe_mtd != NULL) {
1274                         printk("cfe_init: CFE MTD %x %s %x\n", i, cfe_mtd->name, cfe_mtd->size);
1275                         if (!strcmp(cfe_mtd->name, "pmon"))
1276                                 break;
1277                         put_mtd_device(cfe_mtd);
1278                 }
1279         }
1280
1281         if (i >= MAX_MTD_DEVICES)
1282         {
1283                 printk("cfe_init: No CFE MTD\n");
1284                 cfe_mtd = NULL;
1285                 ret = -ENODEV;
1286         }
1287
1288         if(cfe_mtd == NULL) goto fail;
1289
1290         /* sector blocks to be erased and backup */
1291         erasesize = ROUNDUP(CFE_NVRAM_SPACE, cfe_mtd->erasesize);
1292
1293         printk("cfe_init: block size %d\n", erasesize);
1294         cfe_buf = kmalloc(erasesize, GFP_KERNEL);
1295
1296         if(cfe_buf == NULL)
1297         {
1298                 printk("cfe_init: No CFE Memory\n");
1299                 ret = -ENOMEM;
1300                 goto fail;
1301         }
1302         if((ret = get_embedded_block(cfe_mtd, cfe_buf, erasesize, &cfe_offset, &cfe_nvram_header, &cfe_embedded_size)))
1303                 goto fail;
1304
1305         printk("cfe_init: cfe_nvram_header(%08x)\n", (unsigned int) cfe_nvram_header);
1306         bcm947xx_watchdog_disable();
1307
1308         return 0;
1309
1310 fail:
1311         if (cfe_mtd != NULL)
1312         {
1313                 put_mtd_device(cfe_mtd);
1314                 cfe_mtd=NULL;
1315         }
1316         if(cfe_buf != NULL)
1317         {
1318                 kfree(cfe_buf);
1319                 cfe_buf=NULL;
1320         }
1321         return ret;
1322 }
1323 static int cfe_update(const char *keyword, const char *value)
1324 {
1325         struct nvram_header *header;
1326         uint8 crc;
1327         int ret;
1328         int found = 0;
1329         char *str, *end, *mv_target = NULL, *mv_start = NULL;
1330
1331         if(keyword == NULL || *keyword == 0)
1332                 return 0;
1333
1334         if(cfe_buf == NULL||cfe_mtd == NULL)
1335                 if((ret = cfe_init()))
1336                         return ret;
1337
1338         header = cfe_nvram_header;
1339
1340         printk("cfe_update: before %x %x\n", header->len,  cfe_nvram_header->crc_ver_init&0xff);
1341         str = (char *) &header[1];
1342         end = (char *) header + cfe_embedded_size - 2;
1343         end[0] = end[1] = '\0';
1344
1345         for (; *str; str += strlen(str) + 1)
1346         {
1347                 if(!found)
1348                 {
1349                         if(strncmp(str, keyword, strlen(keyword)) == 0 && str[strlen(keyword)] == '=')
1350                         {
1351                                 printk("cfe_update: !!!! found !!!!\n");
1352                                 found = 1;
1353                                 if(value != NULL && strlen(str) == strlen(keyword) + 1 + strlen(value))
1354                                 {//string length is the same
1355                                         strcpy(str+strlen(keyword)+1, value);
1356                                 }
1357                                 else
1358                                 {
1359                                         mv_target = str;
1360                                         mv_start = str + strlen(str) + 1;
1361                                 }
1362                         }
1363                 }
1364         }
1365         /* str point to the end of all embedded nvram settings */
1366
1367         if(mv_target != NULL)
1368         { /* need to move string */
1369                 int str_len = strlen(mv_target);
1370                 //printk("cfe_update: mv_target(%08x) mv_start(%08x) str(%08x) str_len(%d)\n", (unsigned int)mv_target, (unsigned int)mv_start, (unsigned int)str, str_len);
1371                 if(value != NULL && (str + strlen(keyword) + 1 + strlen(value) + 1 - (str_len + 1)) > end)
1372                         return -ENOSPC;
1373                 memmove(mv_target, mv_start, str - mv_start);
1374                 //printk("cfe_update: memmove done\n");
1375                 str -= (str_len + 1); /* /set str to the end for placing incoming keyword and value there */
1376         }
1377
1378         if(value == NULL)
1379         {
1380                 //printk("cfe_update: do unset\n");
1381         }
1382         else if(!found || mv_target != NULL) /*new or movement */
1383         { /* append the keyword and value here */
1384                 //printk("cfe_update: str(%08x)\n", (unsigned int) str);
1385                 if((str + strlen(keyword) + 1 + strlen(value) + 1) > end)
1386                         return -ENOSPC;
1387                 str += sprintf(str, "%s=%s", keyword, value) + 1;
1388                 //printk("cfe_update: append string\n");
1389         }
1390 /* calc length */
1391         memset(str, 0, cfe_embedded_size+(char *)header - str);
1392         str += 2;
1393         header->len = ROUNDUP(str - (char *) header, 4);
1394         //printk("cfe_update: header len: %x\n", header->len);
1395 /*/calc crc */
1396         crc = nvram_calc_crc(header);
1397         //printk("cfe_update: nvram_calc_crc(header) = 0x%02x\n", crc);
1398         header->crc_ver_init = (header->crc_ver_init & NVRAM_CRC_VER_MASK)|crc;
1399         //printk("cfe_update: after %x %x\n", header->crc_ver_init&0xFF, crc);
1400
1401         return 0;
1402 }
1403 static int cfe_dump(void)
1404 {
1405         unsigned int i;
1406         int ret;
1407         unsigned char *ptr;
1408
1409         if(cfe_buf == NULL||cfe_mtd == NULL)
1410                 if((ret = cfe_init()))
1411                         return ret;
1412
1413         printk("cfe_dump: cfe_buf(%08x), dump 1024 byte\n", (unsigned int)cfe_buf);
1414         for(i=0, ptr=(unsigned char *)cfe_nvram_header - 1024; ptr < (unsigned char *)cfe_nvram_header; i++, ptr++)
1415         {
1416                 if(i%16==0) printk("%04x: %02x ", i, *ptr);
1417                 else if(i%16==15) printk("%02x\n", *ptr);
1418                 else if(i%16==7) printk("%02x - ", *ptr);
1419                 else printk("%02x ", *ptr);
1420         }
1421
1422         printk("\ncfe_dump: cfe_nvram_header(%08x)\n", (unsigned int)cfe_nvram_header);
1423         printk("cfe_dump: cfe_nvram_header->len(0x%08x)\n", cfe_nvram_header->len);
1424
1425         printk("\n####################\n");
1426         for(i=0, ptr=(unsigned char *)cfe_nvram_header; i< cfe_embedded_size; i++, ptr++)
1427         {
1428                 if(i%16==0) printk("%04x: %02x ", i, *ptr);
1429                 else if(i%16==15) printk("%02x\n", *ptr);
1430                 else if(i%16==7) printk("%02x - ", *ptr);
1431                 else printk("%02x ", *ptr);
1432         }
1433         printk("\n####################\n");
1434         ptr = (unsigned char *)&cfe_nvram_header[1];
1435         while(*ptr)
1436         {
1437                 printk("%s\n", ptr);
1438                 ptr += strlen(ptr) + 1;
1439         }
1440         printk("\n####################\n");
1441         for(i=0, ptr=((unsigned char *)cfe_nvram_header) + cfe_embedded_size; i<16; i++, ptr++)
1442         {
1443                 if(i%16==0) printk("%04x: %02x ", i, *ptr);
1444                 else if(i%16==15) printk("%02x\n", *ptr);
1445                 else if(i%16==7) printk("%02x - ", *ptr);
1446                 else printk("%02x ", *ptr);
1447         }
1448         return 0;
1449 }
1450
1451 static int cfe_commit(void)
1452 {
1453         DECLARE_WAITQUEUE(wait, current);
1454         wait_queue_head_t wait_q;
1455         struct erase_info erase;
1456         int ret = 0;
1457         size_t erasesize, len=0;
1458         u_int32_t offset;
1459
1460         if(cfe_mtd == NULL||cfe_buf == NULL)
1461         {
1462                 printk("cfe_commit: do nothing\n");
1463                 return 0;
1464         }
1465
1466 #if 0
1467         ret = cfe_dump();
1468         return ret;
1469 #endif
1470 #if 1
1471         /* Backup sector blocks to be erased */
1472         erasesize = ROUNDUP(CFE_NVRAM_SPACE, cfe_mtd->erasesize);
1473         //printk("cfe_commit: erasesize(%08x) cfe_offset(%08x)\n", erasesize, cfe_offset);
1474
1475         /* Erase sector blocks */
1476         init_waitqueue_head(&wait_q);
1477         for (offset=cfe_offset;offset < cfe_offset+erasesize;offset += cfe_mtd->erasesize) {
1478            printk("cfe_commit: ERASE sector block offset(%08x) cfe_mtd->erasesize(%08x)\n", offset, cfe_mtd->erasesize);
1479            erase.mtd = cfe_mtd;
1480            erase.addr = offset;
1481            erase.len = cfe_mtd->erasesize;
1482            erase.callback = erase_callback;
1483            erase.priv = (u_long) &wait_q;
1484
1485            set_current_state(TASK_INTERRUPTIBLE);
1486            add_wait_queue(&wait_q, &wait);
1487            /* Unlock sector blocks */
1488            if (cfe_mtd->unlock)
1489                    cfe_mtd->unlock(cfe_mtd, offset, cfe_mtd->erasesize);
1490
1491            if ((ret = cfe_mtd->erase(cfe_mtd, &erase))) {
1492                 set_current_state(TASK_RUNNING);
1493                 remove_wait_queue(&wait_q, &wait);
1494                 printk("cfe_commit: erase error\n");
1495                 ret = -EIO;
1496                 goto done;
1497            }
1498
1499            /* Wait for erase to finish */
1500            schedule();
1501            remove_wait_queue(&wait_q, &wait);
1502         }
1503
1504         ret = cfe_mtd->write(cfe_mtd, cfe_offset, erasesize, &len, cfe_buf);
1505         //printk("cfe_commit: MTD_WRITE cfe_offset(%08x) erasesize(%08x) len(%08x) ret(%08x)\n", cfe_offset, erasesize, len, ret);
1506
1507         if (ret || len != erasesize) {
1508            printk("cfe_commit: write error\n");
1509            ret = -EIO;
1510         }
1511
1512 done:
1513         if (cfe_mtd != NULL)
1514         {
1515                 put_mtd_device(cfe_mtd);
1516                 cfe_mtd=NULL;
1517         }
1518         if(cfe_buf != NULL)
1519         {
1520                 kfree(cfe_buf);
1521                 cfe_buf=NULL;
1522         }
1523         //printk("commit: %d\n", ret);
1524         printk("cfe_commit: done %d\n", ret);
1525         return ret;
1526 #endif
1527 }
1528 #endif
1529
1530
1531 module_init(dev_nvram_init);
1532 module_exit(dev_nvram_exit);