src/devmem2.c

   1 ///////////////////////////////////////////////////////////////////////////
   2 // Workfile: devmem2.c (Implementation)
   3 // Author: Jan-Derk Bakker, Daniel Giritzer
   4 // Date: 2017-06-02
   5 // Description: Simple module to read/write from/to any location in memory.
   6 // Remarks: assumes a 4k page size and that sizeof(unsigned long) == 4
   7 // Revision: 1.2
   8 // Copyright (C) 2000, Jan-Derk Bakker (J.D.Bakker@its.tudelft.nl)
   9 // Copyright (C) 2017, Daniel Giritzer (giri@nwrk.biz)
  10 ///////////////////////////////////////////////////////////////////////////
  11 #include <stdio.h>
  12 #include <stdlib.h>
  13 #include <unistd.h>
  14 #include <string.h>
  15 #include <string.h>
  16 #include <errno.h>
  17 #include <signal.h>
  18 #include <fcntl.h>
  19 #include <ctype.h>
  20 #include <termios.h>
  21 #include <sys/types.h>
  22 #include <sys/mman.h>
  23
  24 #include "devmem2.h"
  25
  26 #define MAP_SIZE 4096UL
  27 #define MAP_MASK (MAP_SIZE - 1)
  28
  29
  30 /////////////////////////////////////////////////
  31 /// \brief This function is called by the module
  32 ///        if an error occurs.
  33 ///
  34 /// \param char* Custom error Message.
  35 /////////////////////////////////////////////////
  36 static void ERROR(char* custom)
  37 {
  38     //print custom error message
  39     fprintf(stderr, "Error: %s \n", custom);
  40
  41     //print further information
  42     fprintf(stderr, "Info: %d, %s\n",
  43             errno, strerror(errno));
  44 }
  45
  46 /////////////////////////////////////////////////
  47 /// \brief This function allows writing to memory
  48 ///        on the specified address.
  49 ///
  50 /// \param unsigned long Address to write to
  51 /// \param unsigned long Value to write
  52 /// \param int Access type, should be 'w' (word),
  53 ///        'h' (halfword), 'b'(byte)
  54 /////////////////////////////////////////////////
  55 int writemem(unsigned long address, unsigned long writeval, int access_type)
  56 {
  57     int fd;
  58     void *map_base = 0;
  59     void *virt_addr = 0;
  60
  61     off_t target = address;
  62
  63     // Try to open /dev/mem
  64     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
  65     {
  66         ERROR("Opening /dev/mem/ failed!");
  67         return EXIT_FAILURE;
  68     };
  69
  70     // Map one page
  71     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
  72
  73     // Check if mapping was successful
  74     if(map_base == (void *) -1)
  75     {
  76         //Print error on failure
  77         ERROR("Mapping Memory Page Failed!");
  78         return EXIT_FAILURE;
  79     }
  80
  81     //calculate virtual address
  82     virt_addr = map_base + (target & MAP_MASK);
  83
  84     //write value to memory
  85     switch(access_type)
  86     {
  87     case 'b':
  88         *((unsigned char *) virt_addr) = writeval;
  89         break;
  90     case 'h':
  91         *((unsigned short *) virt_addr) = writeval;
  92         break;
  93     case 'w':
  94         *((unsigned long *) virt_addr) = writeval;
  95         break;
  96     default:
  97         ERROR("Wrong access_type set!");
  98         break;
  99     }
 100
 101     //unmap memory
 102     if(munmap(map_base, MAP_SIZE) == -1)
 103     {
 104         ERROR("Unmapping Memory Page Failed!");
 105         return EXIT_FAILURE;
 106     }
 107
 108     close(fd);
 109     return EXIT_SUCCESS;
 110 }
 111
 112 /////////////////////////////////////////////////
 113 /// \brief This function allows reading the memory
 114 ///        on the specified address.
 115 ///
 116 /// \param char* Address to read from
 117 /// \param int Access type, should be 'w' (word),
 118 ///        'h' (halfword), 'b'(byte)
 119 /// \param unsigned long* pointer result variable
 120 /////////////////////////////////////////////////
 121 int readmem(unsigned long address, int access_type, unsigned long *result)
 122 {
 123     int fd;
 124     void *map_base, *virt_addr;
 125     unsigned long read_result = 0;
 126
 127     off_t target = address;
 128
 129     //check if result variable is a nullpointer
 130     if(result == NULL)
 131     {
 132         //Print error on failure
 133         ERROR("Result Parameter is a nullpointer!");
 134         return EXIT_FAILURE;
 135     }
 136
 137
 138     // Try to open /dev/mem
 139     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
 140     {
 141         //Print error on failure
 142         ERROR("Opening /dev/mem/ failed!");
 143         return EXIT_FAILURE;
 144     };
 145
 146     // Map one page
 147     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
 148
 149     // Check if mapping was successful
 150     if(map_base == (void *) -1)
 151     {
 152         ERROR("Mapping Memory Page Failed!");
 153         return EXIT_FAILURE;
 154     }
 155
 156     //calculate virtual address
 157     virt_addr = map_base + (target & MAP_MASK);
 158
 159     //read value
 160     switch(access_type)
 161     {
 162     case 'b':
 163         read_result = *((unsigned char *) virt_addr);
 164         break;
 165     case 'h':
 166         read_result = *((unsigned short *) virt_addr);
 167         break;
 168     case 'w':
 169         read_result = *((unsigned long *) virt_addr);
 170         break;
 171     default:
 172         ERROR("Wrong access_type set!");
 173         break;
 174     }
 175
 176     // unmap memory
 177     if(munmap(map_base, MAP_SIZE) == -1)
 178     {
 179         ERROR("Unmapping Memory Page Failed!");
 180         return EXIT_FAILURE;
 181     }
 182
 183     // return result
 184     *result = read_result;
 185
 186     close(fd);
 187     return EXIT_SUCCESS;
 188 }
 189