diff --git a/src/transport/sharedmem/mpm_transport_sharedmem.c b/src/transport/sharedmem/mpm_transport_sharedmem.c
index 0aefb44bfed0bb914755fde538144b4381498a20..076cc2d70887a5f82a457cb6d901c88bf8ba6f9e 100755 (executable)
#include <errno.h>
#include "mpm_transport_sharedmem.h"
+#include <syslog.h>
+
+#include "uio_module_drv.h"
#define MAX_DEVICE_NAME_LEN 32
+#define MAX_FILE_NAME_LENGTH 128
-static int mpm_transport_get_global_addr (mpm_transport_cfg_t *sp, uint32_t laddr, uint32_t size, uint32_t *gaddr, int *index)
+static int
+mpm_transport_get_mem_details (mpm_transport_cfg_t *sp,
+ uint32_t laddr, uint32_t size, uint32_t *base_address, uint32_t *offset,
+ int *index)
{
int i;
if (sp->mmap[i].local_addr <= laddr &&
sp->mmap[i].local_addr + sp->mmap[i].length >= laddr + size) {
- *gaddr = sp->mmap[i].global_addr + laddr - sp->mmap[i].local_addr;
+ *offset = laddr - sp->mmap[i].local_addr;
+ *base_address = sp->mmap[i].local_addr;
*index = i;
return 0;
}
for (i = 0; i < sp->num_mmap; i++) {
if (sp->mmap[i].global_addr <= laddr &&
sp->mmap[i].global_addr + sp->mmap[i].length >= laddr + size) {
- *gaddr = laddr;
+ *offset = laddr-sp->mmap[i].global_addr;
+ *base_address = sp->mmap[i].global_addr;
*index = i;
return 0;
}
@@ -69,37 +78,140 @@ static int mpm_transport_get_global_addr (mpm_transport_cfg_t *sp, uint32_t ladd
laddr, size, sp->name);
return -1;
}
-
int mpm_transport_sharedmem_open(mpm_transport_cfg_t *sp, mpm_transport_open_t *ocfg)
{
- int i, j;
+ int i, j, k;
mpm_transport_sharedmem_t *td;
+ int fd;
+ char filename[MAX_FILE_NAME_LENGTH];
+ off_t fileSize;
+ struct stat statbuf;
+ char *scratch;
+ char *devname;
+ int fd_index;
td = calloc(1, sizeof(mpm_transport_sharedmem_t));
if (!td) {
mpm_printf(1, "can't allocate memory for transport data \n");
- return -1;
+ return MPM_TRANSPORT_SHM_OPEN_ERR_CALLOC;
}
-
+ td->fd_mem_block.num_fds = 0;
for (i = 0; i < sp->num_mmap; i++) {
/* check if fd for same device name is already opened */
for (j = 0; j < i; j++) {
- if (strncmp(sp->mmap[i].name, sp->mmap[j].name, MAX_DEVICE_NAME_LEN)) {
- continue;
+#ifdef DEBUG
+ mpm_printf(1, "DEBUG: string compare %s : %s\n",
+ sp->mmap[i].devicename, sp->mmap[j].devicename);
+#endif
+ if (!strncmp(sp->mmap[i].devicename, sp->mmap[j].devicename, MAX_DEVICE_NAME_LEN)) {
+ td->fd_index[i] = td->fd_index[j];
+ fd_index = td->fd_index[i];
+ for (k = 0; k < td->fd_mem_block.num_mem_blocks[fd_index]; k++) {
+ if((sp->mmap[i].global_addr >= td->fd_mem_block.mem_block[fd_index][k].base) &&
+ (sp->mmap[i].global_addr < td->fd_mem_block.mem_block[fd_index][k].base
+ + td->fd_mem_block.mem_block[fd_index][k].length)) {
+ td->map_index[i] = k;
+#ifdef DEBUG
+ mpm_printf(1, "DEBUG: Success mmap entry %d: fd_index %d, map_index %d\n",
+ i, fd_index, k);
+#endif
+ break;
+ }
+ }
+ if(k >= td->fd_mem_block.num_mem_blocks[fd_index]) {
+ mpm_printf(1, "Could not find map index for address %x\n",
+ sp->mmap[i].global_addr);
+ }
+ break;
}
- td->fd[i] = td->fd[j];
- break;
}
if (j < i) {
- break;
+ continue;
}
-
- td->fd[i] = open(sp->mmap[i].devicename, (O_RDWR | O_SYNC));
- if (td->fd[i] < 0) {
+ td->fd_mem_block.fd[td->fd_mem_block.num_fds] = open(sp->mmap[i].devicename, (O_RDWR | O_SYNC));
+ if (td->fd_mem_block.fd[td->fd_mem_block.num_fds] < 0) {
mpm_printf(1, "Failed to open %s :Error (%s)",
sp->mmap[i].devicename, strerror(errno));
free(td);
- return -1;
+ return MPM_TRANSPORT_SHM_OPEN_ERR_DEV_OPEN;
+ }
+ td->fd_index[i] = td->fd_mem_block.num_fds;
+ devname = strrchr(sp->mmap[i].devicename, '/')+1;
+ /* Check device tree and record the index of the mmap */
+ snprintf(filename, MAX_FILE_NAME_LENGTH, "/proc/device-tree/soc/%s/reg", devname);
+ if ((fd = open (filename, O_RDONLY)) == -1)
+ {
+ mpm_printf(1, "Failed to open \"%s\" err=%s\n",
+ filename, strerror(errno));
+ return MPM_TRANSPORT_SHM_OPEN_ERR_MEM_FILE_OPEN;
+ }
+ if (fstat (fd, &statbuf) == -1)
+ {
+ mpm_printf (1, "Failed to find size of %s (%s)\n",
+ filename, strerror(errno));
+ return MPM_TRANSPORT_SHM_OPEN_ERR_FIND_FILE_SIZE;
+ }
+ fileSize = statbuf.st_size;
+ td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds] = fileSize / sizeof(mem_block_t);
+ scratch = malloc (sizeof(mem_block_t) * td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds]);
+ if (!td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds] || ! scratch)
+ {
+ mpm_printf (1, "Failed to malloc size blocks for %d\n", td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds]);
+ return MPM_TRANSPORT_SHM_OPEN_ERR_MALLOC;
+ }
+ if ( read (fd, scratch, fileSize) != fileSize)
+ {
+ mpm_printf (1, "Failed to read %d bytes from %s\n",
+ (int)fileSize, filename);
+ return MPM_TRANSPORT_SHM_OPEN_ERR_READ;
+ }
+ close (fd);
+ if ( td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds] > MPM_MAX_MEM_ENTRIES) {
+ mpm_printf (1, "Number of blocks %d exceeds max %d\n",
+ td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds], MPM_MAX_MEM_ENTRIES );
+ return MPM_TRANSPORT_SHM_OPEN_ERR_BLOCKS_EXCEED_MAX;
+ }
+ for (k = 0; k < td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds]; k++)
+ {
+ /* Swizzle from big endian in proc */
+ td->fd_mem_block.mem_block[td->fd_mem_block.num_fds][k].base =
+ (scratch[k*8 + 0] << 24) |
+ (scratch[k*8 + 1] << 16) |
+ (scratch[k*8 + 2] << 8) |
+ (scratch[k*8 + 3] );
+ td->fd_mem_block.mem_block[td->fd_mem_block.num_fds][k].length =
+ (scratch[k*8 + 4] << 24) |
+ (scratch[k*8 + 5] << 16) |
+ (scratch[k*8 + 6] << 8) |
+ (scratch[k*8 + 7] );
+#ifdef DEBUG
+ mpm_printf(1, " DEBUG: Index %d: Base %x, length %x\n", k, td->fd_mem_block.mem_block[td->fd_mem_block.num_fds][k].base,
+ td->fd_mem_block.mem_block[td->fd_mem_block.num_fds][k].length);
+#endif
+ }
+ for (k = 0; k < td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds]; k++)
+ {
+ if((sp->mmap[i].global_addr >= td->fd_mem_block.mem_block[td->fd_mem_block.num_fds][k].base) &&
+ (sp->mmap[i].global_addr < td->fd_mem_block.mem_block[td->fd_mem_block.num_fds][k].base
+ + td->fd_mem_block.mem_block[td->fd_mem_block.num_fds][k].length)) {
+ td->map_index[i] = k;
+ mpm_printf(1, "Success mmap entry %d: fd_index %d, map_index %d\n",
+ i,td->fd_mem_block.num_fds, k);
+ break;
+ }
+ }
+ if(k >= td->fd_mem_block.num_mem_blocks[td->fd_mem_block.num_fds]) {
+ mpm_printf(1, "Could not find map index for address %x\n",
+ sp->mmap[i].global_addr);
+ return MPM_TRANSPORT_SHM_OPEN_ERR_MAP_INDEX;
+ }
+
+ free (scratch);
+ td->fd_mem_block.num_fds++;
+ if ( td->fd_mem_block.num_fds >= MPM_MAX_NUM_FDS ) {
+ mpm_printf (1, "Number of fds %d exceeds max fds %d\n",
+ td->fd_mem_block.num_fds, MPM_MAX_NUM_FDS );
+ return MPM_TRANSPORT_SHM_OPEN_ERR_EXCEED_NUM_FDS;
}
}
@@ -112,8 +224,8 @@ void *mpm_transport_sharedmem_mmap(mpm_transport_cfg_t *sp, uint32_t addr, uint3
{
int index;
int user_index;
- uint32_t gaddr;
- int offset, pg_offset, mmap_length, min_len;
+ uint32_t offset, base_address, base_correction;
+ int pg_offset, mmap_length, min_len;
int page_size = getpagesize();
mpm_transport_sharedmem_t *td = (mpm_transport_sharedmem_t *) sp->td;
@@ -130,27 +242,31 @@ void *mpm_transport_sharedmem_mmap(mpm_transport_cfg_t *sp, uint32_t addr, uint3
return MAP_FAILED;
}
- if(mpm_transport_get_global_addr(sp, addr, length, &gaddr, &index)) {
- mpm_printf(1, "can't find global address for local address 0x%x\n", addr);
+ if(mpm_transport_get_mem_details(sp, addr, length,
+ &base_address, &offset, &index)) {
+ mpm_printf(1, "can't find index and offset for address 0x%x\n", addr);
return MAP_FAILED;
}
+ base_correction = (base_address & (page_size - 1));
+ pg_offset = offset & (~((page_size<< UIO_MODULE_DRV_MAP_OFFSET_SHIFT) - 1));
- offset = gaddr;
- pg_offset = offset & (~(page_size - 1));
-
- min_len = length + offset - pg_offset;
- mmap_length = (min_len & ~(page_size - 1));
- mmap_length = (mmap_length != min_len) ? mmap_length + page_size : mmap_length;
+ mmap_length = length + offset - pg_offset + base_correction;
+// mmap_length = (min_len & ~(page_size - 1));
+// mmap_length = (mmap_length != min_len) ? mmap_length + page_size : mmap_length;
- td->mmap_user[user_index].addr = mmap(NULL, mmap_length, mcfg->mmap_prot, mcfg->mmap_flags,
- td->fd[index], pg_offset);
+ td->mmap_user[user_index].addr = mmap(NULL, mmap_length,
+ mcfg->mmap_prot, mcfg->mmap_flags,
+ td->fd_mem_block.fd[td->fd_index[index]],
+ ((pg_offset) + (td->map_index[index] * getpagesize())));
if (td->mmap_user[user_index].addr == MAP_FAILED) {
- mpm_printf(1, "can't mmap for the address 0x%x with length 0x%x (err: %s)\n",
+ mpm_printf(1, "can't mmap for the address 0x%x with length"
+ " 0x%x (err: %s)\n",
addr, length, strerror(errno));
return MAP_FAILED;
}
td->mmap_user[user_index].size = mmap_length;
- td->mmap_user[user_index].addr_usr = td->mmap_user[user_index].addr + (offset - pg_offset);
+ td->mmap_user[user_index].addr_usr = td->mmap_user[user_index].addr
+ + (offset - pg_offset) + base_correction;
return (td->mmap_user[user_index].addr_usr);
}
@@ -186,28 +302,40 @@ int mpm_transport_sharedmem_munmap(mpm_transport_cfg_t *sp, void *va, uint32_t l
static void *mpm_transport_sharedmem_rw_map (mpm_transport_cfg_t *sp, uint32_t addr, uint32_t length, int *index)
{
- uint32_t gaddr;
+ uint32_t offset, base_address, base_correction;
+ int pg_offset, mmap_length, min_len;
+ int page_size = getpagesize();
mpm_transport_sharedmem_t *td = (mpm_transport_sharedmem_t *) sp->td;
- if(mpm_transport_get_global_addr(sp, addr, length, &gaddr, index)) {
+ if(mpm_transport_get_mem_details(sp, addr, length, &base_address,
+ &offset, index)) {
mpm_printf(1, "can't find global address for local address 0x%x\n", addr);
return 0;
}
+ base_correction = (base_address & (page_size - 1));
+ pg_offset = offset & (~((page_size<< UIO_MODULE_DRV_MAP_OFFSET_SHIFT) - 1));
+ mmap_length = length + offset - pg_offset + base_correction;
+
+ mpm_printf(1, "Debug: Base Address %x, base_correction %x pg_offset %x, mmap_length %x \n",
+ base_address, base_correction, pg_offset, mmap_length);
if (!td->mmap_rw[*index].addr) {
- td->mmap_rw[*index].addr = mmap(NULL, sp->mmap[*index].length, (PROT_READ|PROT_WRITE),
- MAP_SHARED, td->fd[*index], sp->mmap[*index].global_addr);
+ td->mmap_rw[*index].addr = mmap(NULL, mmap_length, (PROT_READ|PROT_WRITE),
+ MAP_SHARED, td->fd_mem_block.fd[td->fd_index[*index]],
+ ((pg_offset) + (td->map_index[*index] * getpagesize())));
if (td->mmap_rw[*index].addr == MAP_FAILED) {
mpm_printf(1, "can't mmap for the address 0x%x with length 0x%x (err: %s)\n",
addr, length, strerror(errno));
return 0;
}
+ td->mmap_rw[*index].addr_usr = td->mmap_rw[*index].addr
+ + (offset - pg_offset) + base_correction;
td->mmap_rw[*index].size = sp->mmap[*index].length;
pthread_mutex_init(&td->mutex_rw[*index], NULL);
}
- return (td->mmap_rw[*index].addr + (gaddr - sp->mmap[*index].global_addr));
+ return (td->mmap_rw[*index].addr_usr);
}
int mpm_transport_sharedmem_read(mpm_transport_cfg_t *sp, uint32_t addr, uint32_t length, char *buf, mpm_transport_read_t *rcfg)
if(td->mmap_rw[i].addr) {
munmap(td->mmap_rw[i].addr, td->mmap_rw[i].size);
}
- if (td->fd[i]) {
- for (j = i + 1; j < MPM_MAX_MMAPS; j++) {
- if (td->fd[i] == td->fd[j]) {
- td->fd[j] = 0;
- }
- }
- fsync(td->fd[i]);
- close(td->fd[i]);
- }
pthread_mutex_destroy(&td->mutex_rw[i]);
}
+ for (i = 0; i < td->fd_mem_block.num_fds; i++) {
+ fsync(td->fd_mem_block.fd[i]);
+ close(td->fd_mem_block.fd[i]);
+ }
if (td) free(td);
-}
\ No newline at end of file
+}