sim_bram_helpers_c.c

   1 #include <stdio.h>
   2 #include <stdbool.h>
   3 #include <stdint.h>
   4 #include <stdlib.h>
   5 #include <string.h>
   6 #include <unistd.h>
   7 #include <fcntl.h>
   8 #include <sys/mman.h>
   9 #include <sys/types.h>
  10 #include <sys/stat.h>
  11
  12 #undef DEBUG
  13
  14 #define ALIGN_UP(VAL, SIZE)     (((VAL) + ((SIZE)-1)) & ~((SIZE)-1))
  15
  16 #define vhpi0   2       /* forcing 0 */
  17 #define vhpi1   3       /* forcing 1 */
  18
  19 struct int_bounds
  20 {
  21         int left;
  22         int right;
  23         char dir;
  24         unsigned int len;
  25 };
  26
  27 struct fat_pointer
  28 {
  29         void *base;
  30         struct int_bounds *bounds;
  31 };
  32
  33 static char *from_string(void *__p)
  34 {
  35         struct fat_pointer *p = __p;
  36         unsigned long len = p->bounds->len;
  37         char *m;
  38
  39         m = malloc(len+1);
  40         if (!m) {
  41                 perror("malloc");
  42                 exit(1);
  43         }
  44
  45         memcpy(m, p->base, len);
  46         m[len] = 0x0;
  47
  48         return m;
  49 }
  50
  51 static uint64_t from_std_logic_vector(unsigned char *p, unsigned long len)
  52 {
  53         unsigned long ret = 0;
  54
  55         if (len > 64) {
  56                 fprintf(stderr, "%s: invalid length %lu\n", __func__, len);
  57                 exit(1);
  58         }
  59
  60         for (unsigned long i = 0; i < len; i++) {
  61                 unsigned char bit;
  62
  63                 if (*p == vhpi0) {
  64                         bit = 0;
  65                 } else if (*p == vhpi1) {
  66                         bit = 1;
  67                 } else {
  68                         fprintf(stderr, "%s: bad bit %d\n", __func__, *p);
  69                         bit = 0;
  70                 }
  71
  72                 ret = (ret << 1) | bit;
  73                 p++;
  74         }
  75
  76         return ret;
  77 }
  78
  79 static void to_std_logic_vector(unsigned long val, unsigned char *p,
  80                                 unsigned long len)
  81 {
  82         if (len > 64) {
  83                 fprintf(stderr, "%s: invalid length %lu\n", __func__, len);
  84                 exit(1);
  85         }
  86
  87         for (unsigned long i = 0; i < len; i++) {
  88                 if ((val >> (len-1-i) & 1))
  89                         *p = vhpi1;
  90                 else
  91                         *p = vhpi0;
  92
  93                 p++;
  94         }
  95 }
  96
  97 #define MAX_REGIONS 128
  98
  99 struct ram_behavioural {
 100         char *filename;
 101         unsigned long size;
 102         void *m;
 103 };
 104
 105 static struct ram_behavioural behavioural_regions[MAX_REGIONS];
 106 static unsigned long region_nr;
 107
 108 unsigned long behavioural_initialize(void *__f, unsigned long size)
 109 {
 110         struct ram_behavioural *r;
 111         int fd;
 112         struct stat buf;
 113         unsigned long tmp_size;
 114         void *mem;
 115
 116         if (region_nr == MAX_REGIONS) {
 117                 fprintf(stderr, "%s: too many regions, bump MAX_REGIONS\n", __func__);
 118                 exit(1);
 119         }
 120
 121         r = &behavioural_regions[region_nr];
 122
 123         r->filename = from_string(__f);
 124         r->size = ALIGN_UP(size, getpagesize());
 125
 126         fd = open(r->filename, O_RDWR);
 127         if (fd == -1) {
 128                 fprintf(stderr, "%s: could not open %s\n", __func__,
 129                         r->filename);
 130                 exit(1);
 131         }
 132
 133         if (fstat(fd, &buf)) {
 134                 perror("fstat");
 135                 exit(1);
 136         }
 137
 138         /* XXX Do we need to truncate the underlying file? */
 139         tmp_size = ALIGN_UP(buf.st_size, getpagesize());
 140
 141         if (r->size > tmp_size) {
 142                 void *m;
 143
 144                 /*
 145                  * We have to pad the file. Allocate the total size, then
 146                  * create a space for the file.
 147                  */
 148                 mem = mmap(NULL, r->size, PROT_READ|PROT_WRITE,
 149                                 MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
 150                 if (mem == MAP_FAILED) {
 151                         perror("mmap");
 152                         exit(1);
 153                 }
 154
 155                 if (tmp_size) {
 156                         munmap(mem, tmp_size);
 157
 158                         m = mmap(mem, tmp_size, PROT_READ|PROT_WRITE,
 159                                         MAP_PRIVATE|MAP_FIXED, fd, 0);
 160                         if (m == MAP_FAILED) {
 161                                 perror("mmap");
 162                                 exit(1);
 163                         }
 164                         if (m != mem) {
 165                                 fprintf(stderr, "%s: mmap(MAP_FIXED) failed\n",
 166                                         __func__);
 167                                 exit(1);
 168                         }
 169                 }
 170         } else {
 171                 mem = mmap(NULL, tmp_size, PROT_READ|PROT_WRITE, MAP_PRIVATE,
 172                                 fd, 0);
 173                 if (mem == MAP_FAILED) {
 174                         perror("mmap");
 175                         exit(1);
 176                 }
 177         }
 178
 179         behavioural_regions[region_nr].m = mem;
 180         return region_nr++;
 181 }
 182
 183 void behavioural_read(unsigned char *__val, unsigned char *__addr,
 184                         unsigned long sel, int identifier)
 185 {
 186         struct ram_behavioural *r;
 187         unsigned long val = 0;
 188         unsigned long addr = from_std_logic_vector(__addr, 64);
 189         unsigned char *p;
 190
 191         if (identifier > region_nr) {
 192                 fprintf(stderr, "%s: bad index %d\n", __func__, identifier);
 193                 exit(1);
 194         }
 195
 196         r = &behavioural_regions[identifier];
 197
 198         for (unsigned long i = 0; i < 8; i++) {
 199 #if 0
 200                 /* sel only used on writes */
 201                 if (!(sel & (1UL << i)))
 202                         continue;
 203 #endif
 204
 205                 if ((addr + i) > r->size) {
 206                         fprintf(stderr, "%s: bad memory access %lx %lx\n", __func__,
 207                                 addr+i, r->size);
 208                         exit(1);
 209                 }
 210
 211                 p = (unsigned char *)(((unsigned long)r->m) + addr + i);
 212                 val |= (((unsigned long)*p) << (i*8));
 213         }
 214
 215 #ifdef DEBUG
 216         printf("MEM behave %d read  %016lx addr %016lx sel %02lx\n", identifier, val,
 217                 addr, sel);
 218 #endif
 219
 220         to_std_logic_vector(val, __val, 64);
 221 }
 222
 223 void behavioural_write(unsigned char *__val, unsigned char *__addr,
 224                         unsigned int sel, int identifier)
 225 {
 226         struct ram_behavioural *r;
 227         unsigned long val = from_std_logic_vector(__val, 64);
 228         unsigned long addr = from_std_logic_vector(__addr, 64);
 229         unsigned char *p;
 230
 231         if (identifier > region_nr) {
 232                 fprintf(stderr, "%s: bad index %d\n", __func__, identifier);
 233                 exit(1);
 234         }
 235
 236         r = &behavioural_regions[identifier];
 237
 238         p = (unsigned char *)(((unsigned long)r->m) + addr);
 239
 240 #ifdef DEBUG
 241         printf("MEM behave %d write %016lx addr %016lx sel %02x\n", identifier, val,
 242                 addr, sel);
 243 #endif
 244
 245         for (unsigned long i = 0; i < 8; i++) {
 246                 if (!(sel & (1UL << i)))
 247                         continue;
 248
 249                 if ((addr + i) > r->size) {
 250                         fprintf(stderr, "%s: bad memory access %lx %lx\n", __func__,
 251                                 addr+i, r->size);
 252                         exit(1);
 253                 }
 254
 255                 p = (unsigned char *)(((unsigned long)r->m) + addr + i);
 256                 *p = (val >> (i*8)) & 0xff;
 257         }
 258 }