/*******************************************************************
  Simple program to dump and write physical or device provided
  memory locations from Linux userspace

  rdwrmem.c       - main and only file

  (C) Copyright 2004 - 2017 by Pavel Pisa
      e-mail:   pisa@cmp.felk.cvut.cz
      homepage: http://cmp.felk.cvut.cz/~pisa
      work:     http://www.pikron.com/
      license:  any combination GPL, LGPL, MPL or BSD licenses

 *******************************************************************/

#include <sys/mman.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <malloc.h>
#include <string.h>
#include <byteswap.h>
#include <getopt.h>
#include <inttypes.h>

char *memdev="/dev/mem";

int write_flg;
int dump_flg=0;
int binary_dump_flg=0;
int fill_flg;
void *fill_pat_val;
int fill_pat_len;
char *file_format=NULL;
int nommap_flg=0;

int blockmode=0;
int erase_block=-1;
int mem_type=0;
unsigned long mem_start=0;
unsigned long mem_length=0;
unsigned long mem_offs=0;

int mem_dump(void *buf, unsigned long start, unsigned long len, int blen)
{
  unsigned long addr=start;
  volatile unsigned char *p=buf;
  int i;

  while(len){
    printf("%08lX:",addr);
    i=len>16?16:len;
    addr+=i;
    len-=i;
    while(i>0){
      switch(blen){
	case 4:
	  i-=4;
          printf("%08X%c",*(volatile uint32_t*)p,i>0?' ':'\n');
	  p+=4;
	  break;
	case 2:
	  i-=2;
          printf("%04X%c",*(volatile uint16_t*)p,i>0?' ':'\n');
	  p+=2;
	  break;
	default:
	  i--;
          printf("%02X%c",*(volatile uint8_t*)(p++),i>0?' ':'\n');
	  break;
      }
    }
  }
  return 0;
}

int mem_binary(void *buf, unsigned long start, unsigned long len, int blen)
{
  volatile unsigned char *p=buf;
  uint16_t u16;
  uint32_t u32;

  while(len){
    if(blen > len)
      blen = len;
    switch(blen){
      case 4:
        len-=4;
        u32 = *(volatile uint32_t*)p;
        fwrite(&u32,4,1,stdout);
        p+=4;
        break;
      case 3:
      case 2:
        len-=2;
        u16 = *(volatile uint16_t*)p;
        fwrite(&u16,2,1,stdout);
        p+=2;
        break;
      default:
        len-=1;
        fwrite((uint8_t*)p,1,1,stdout);
        p+=1;
        break;
    }
  }
  return 0;
}

int mem_fill(void *buf, unsigned long start, unsigned long len,
		unsigned char *pat_val, int pat_len, int blen)
{
  volatile unsigned char *p=buf;
  unsigned char *r;
  unsigned char *pat_end=pat_val+pat_len;
  uint16_t u16;
  uint32_t u32;

  if(pat_len<=0) return -1;
  if(!len) return 0;

  switch(blen){
    case 4:
      r=pat_val;
      for(;;){
        u32=*(r++);
	if(r>=pat_end) r=pat_val;
        u32|=(uint32_t)*(r++) << 8;
	if(r>=pat_end) r=pat_val;
        u32|=(uint32_t)*(r++) << 16;
	if(r>=pat_end) r=pat_val;
        u32|=(uint32_t)*(r++) << 24;
	if(r>=pat_end) r=pat_val;
        *(volatile uint32_t*)p=u32;
	p+=4;
        if(len<=4) break;
	len-=4;
      }
      break;
    case 2:
      r=pat_val;
      for(;;){
        u16=*(r++);
	if(r>=pat_end) r=pat_val;
        u16|=(unsigned)*(r++) << 8;
	if(r>=pat_end) r=pat_val;
        *(volatile uint16_t*)p=u16;
	p+=2;
        if(len<=2) break;
	len-=2;
      }
    default:
      r=pat_val;
      while(len--){
        *(volatile uint8_t*)(p++)=*(r++);
	if(r>=pat_end) r=pat_val;
      }
      break;
  }

  return 0;
}


/*----------------------------------------------------------------*/


int si_long(char **ps,long *val,int base)
{
  char *p;
  *val=strtol(*ps,&p,base);
  if(*ps==p) return -1;
  *ps=p;
  return 1;
}

int si_ulong(char **ps,unsigned long *val,int base)
{
  char *p;
  *val=strtoul(*ps,&p,base);
  if(*ps==p) return -1;
  *ps=p;
  return 1;
}

int add_to_arr(void **pdata,int *plen,int base,char *str, int blen)
{
  char *s=str;
  unsigned long val;
  unsigned char *p;
  int i;

  if(!blen)
    blen=1;
  if(blen>4)
    return -1;

  do{
    while(*s && strchr(", \t:;",*s)) s++;
    if(!*s) break;
    if(si_ulong(&s,&val,base)<0){
      return -1;
    }
    if(*pdata==NULL){
      *plen=0;
      *pdata=p=malloc(blen);
    }else{
      p=realloc(*pdata,*plen+blen);
      if(p==NULL) return -1;
      *pdata=p;
    }

    p+=*plen;
    for(i=blen;i--;){
      *(p++)=val;
      val>>=8;
    }
    (*plen)+=blen;
  } while(1);
  return 1;
}

/*----------------------------------------------------------------*/

static void
usage(void)
{
  printf("usage: rdwrmem <parameters>\n");
  printf("  -d, --dev  <name>        name of device [%s]\n",memdev);
  printf("  -b, --blockmode          access block length (usually 1, 2 or 4)\n");
  printf("  -m, --mem-dump           dump memory range defined by -s -l -t\n");
  printf("  -N, --no-mmap            do not use mmap, switch to read/write instead\n");
  printf("  -s, --start  <addr>      start address of transfer\n");
  printf("  -l, --length <num>       length of upload block\n");
  printf("  -V, --version            show version\n");
  printf("  -h, --help               this usage screen\n");
  printf("  -F, --fill <pattern>     comma separated values to fill from -s\n");
  printf("                           length -l or pattern, -b for step\n");
}


int main(int argc, char *argv[])
{
  int fd;
  int res;
  unsigned long pagesize;
  unsigned char *mm;
  unsigned char *mem;
  static struct option long_opts[] = {
    { "dev",  1, 0, 'd' },
    { "blockmode",1,0,'b' },
    { "fill",  1, 0, 'F' },
    { "binary-dump",0, 0, 'B' },
    { "mem-dump",0, 0, 'm' },
    { "mem-offs",1, 0, 'O' },
    { "mem-type",1, 0, 't' },
    { "no-mmap" ,0, 0, 'N' },
    { "start", 1, 0, 's' },
    { "length",1, 0, 'l' },
    { "format",1, 0, 'f' },
    { "version",0,0, 'V' },
    { "help",  0, 0, 'h' },
    { 0, 0, 0, 0}
  };
  int opt;

  while ((opt = getopt_long(argc, argv, "d:c:b:F:Bmt:Ns:l:f:Vh",
			    &long_opts[0], NULL)) != EOF)
  switch (opt) {
    case 'd':
      memdev=optarg;
      break;
    case 'b':
      blockmode = strtol(optarg,NULL,0);
      break;
    case 'F':
      fill_flg=1;
      if(add_to_arr((void**)&fill_pat_val,&fill_pat_len,0,optarg, blockmode)<0){
	fprintf(stderr,"%s: incorrect patern data \"%s\"\n",argv[0],optarg);
	exit(2);
      }
      if(!fill_pat_len){
	fprintf(stderr,"%s: incorrect patern data - empty value\n",argv[0]);
	exit(2);
      }
      write_flg=1;
      break;
    case 'B':
      binary_dump_flg=1;
      break;
    case 'm':
      dump_flg=1;
      break;
    case 't':
      mem_type=strtoul(optarg,NULL,0);
      break;
    case 'N':
      nommap_flg=1;
      break;
    case 's':
      mem_start=strtoul(optarg,NULL,0);
      break;
    case 'l':
      mem_length=strtoul(optarg,NULL,0);
      break;
    case 'f':
      file_format=optarg;
      break;
    case 'V':
      fputs("rdwrmem pre alpha\n", stdout);
      exit(0);
    case 'h':
    default:
      usage();
      exit(opt == 'h' ? 0 : 1);
  }

  fd = open(memdev, (write_flg?O_RDWR:O_RDONLY)|O_SYNC);
  if (fd < 0) {
    fprintf(stderr,"%s: cannot open %s\n",argv[0],memdev);
    return 1;
  }

  if(!nommap_flg) {
    unsigned long mem_window_size;

    /*
    pagesize=getpagesize();
    */
    pagesize=sysconf(_SC_PAGESIZE);

    if (fill_flg && !mem_length)
      mem_window_size = fill_pat_len;
    else
      mem_window_size = mem_length;

    mem_window_size += mem_start & (pagesize - 1);
    mem_window_size = (mem_window_size + pagesize - 1) & ~(pagesize - 1);

    mm = mmap(NULL, mem_window_size, write_flg?PROT_WRITE|PROT_READ:PROT_READ,
              MAP_SHARED, fd, mem_start & ~(pagesize-1));
    mem = mm + (mem_start & (pagesize-1));

    if (mm == MAP_FAILED) {
      fprintf(stderr,"%s: mmap error\n",argv[0]);
      return 1;
    }

    //printf("mmap 0x%lx -> %p\n",mem_start,mem);

    //write(fileno(stdout), mm, 0x1000);
    if(dump_flg) {
      mem_dump(mem, mem_start, mem_length?mem_length:1, blockmode);
    }

    if(binary_dump_flg) {
      mem_binary(mem, mem_start, mem_length?mem_length:1, blockmode);
    }

    if(fill_flg) {
      mem_fill(mem, mem_start, mem_length?mem_length:fill_pat_len, fill_pat_val, fill_pat_len, blockmode);
    }

    munmap(mm, mem_window_size);
  } else {
    if(!mem_length) mem_length=1;
    if(blockmode){
      if(mem_length%blockmode)
        mem_length+=blockmode-mem_length%blockmode;
    }

    mem=malloc(mem_length);
    if(!mem){
      fprintf(stderr,"%s: cannot allocate buffer for transfer\n",argv[0]);
      return 1;
    }

    if(dump_flg) {
      res=lseek(fd,mem_start,SEEK_SET);
      if(res!=mem_start){
        fprintf(stderr,"%s: request seek to %ld, but %d returned\n",argv[0],mem_start,res);
      }
      res=read(fd,mem,mem_length);
      if(res!=mem_length){
        fprintf(stderr,"%s: read request %ld bytes, but only %d returned\n",argv[0],mem_length,res);
      }
      if(res>0){
        mem_dump(mem, mem_start, mem_length, blockmode);
      }
    }

    if(fill_flg) {
      mem_fill(mem, mem_start, mem_length?mem_length:fill_pat_len, fill_pat_val, fill_pat_len, blockmode);
      res=lseek(fd,mem_start,SEEK_SET);
      if(res!=mem_start){
        fprintf(stderr,"%s: request seek to %ld, but %d returned\n",argv[0],mem_start,res);
      }
      res=write(fd,mem,mem_length);
      if(res!=mem_length){
        fprintf(stderr,"%s: write request %ld bytes, but only %d processed\n",argv[0],mem_length,res);
      }
    }

    free(mem);
  }
  close(fd);

  return 0;
}