diff options
Diffstat (limited to 'board/c2mon')
| -rw-r--r-- | board/c2mon/c2mon.c | 19 |
1 files changed, 16 insertions, 3 deletions
diff --git a/board/c2mon/c2mon.c b/board/c2mon/c2mon.c index 171d06eba0a..0fdcebe373d 100644 --- a/board/c2mon/c2mon.c +++ b/board/c2mon/c2mon.c @@ -228,8 +228,8 @@ static long int dram_size (long int mamr_value, long int *base, volatile immap_t *immap = (immap_t *) CFG_IMMR; volatile memctl8xx_t *memctl = &immap->im_memctl; volatile long int *addr; - ulong cnt, val; - ulong save[32]; /* to make test non-destructive */ + ulong cnt, val, size; + ulong save[32]; /* to make test non-destructive */ unsigned char i = 0; memctl->memc_mamr = mamr_value; @@ -248,7 +248,13 @@ static long int dram_size (long int mamr_value, long int *base, /* check at base address */ if ((val = *addr) != 0) { + /* Restore the original data before leaving the function. + */ *addr = save[i]; + for (cnt = 1; cnt <= maxsize / sizeof(long); cnt <<= 1) { + addr = (volatile ulong *) base + cnt; + *addr = save[--i]; + } return (0); } @@ -259,7 +265,14 @@ static long int dram_size (long int mamr_value, long int *base, *addr = save[--i]; if (val != (~cnt)) { - return (cnt * sizeof (long)); + size = cnt * sizeof (long); + /* Restore the original data before returning + */ + for (cnt <<= 1; cnt <= maxsize / sizeof (long); cnt <<= 1) { + addr = (volatile ulong *) base + cnt; + *addr = save[--i]; + } + return (size); } } return (maxsize); |