static HRESULT MV_SHM_Unmap_Base(shm_dev_t *shm_dev, size_t Offset)
{
shm_address_t *address_node;
int res = 0;
if (SHM_DEVICE_LOAD_COUNT <= 0) {
MV_SHM_Print("MV_SHM_Unmap_Base shm device"
" not be open\n");
return E_NOTREADY;
}
pthread_rwlock_wrlock(&shm_dev->addr.m_rb_rwlock);
address_node =
MV_SHM_lookup_phyaddress_node(&shm_dev->addr.m_phyaddr_root,
(shm_dev->base.m_base_physaddr + Offset));
if (address_node == NULL) {
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return S_OK;
}
res = MV_SHM_Munmap_Base(address_node->m_virtaddress,
address_node->m_size, shm_dev->mem_type);
if (res == 0) {
MV_SHM_delete_phyaddress_node(
&shm_dev->addr.m_phyaddr_root,address_node);
MV_SHM_delete_virtaddress_node(
&shm_dev->addr.m_virtaddr_root,address_node);
free(address_node);
}
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return S_OK;
}
int MV_SHM_iounmap_and_free_rbtree(shm_device_t *device, size_t Offset)
{
shm_address_t *address_node;
if (device == NULL) {
shm_error("kernel MV_SHM_iounmap_and_free_rbtree error\n");
return -ENODEV;
}
down_read(&device->m_rwsem);
address_node = MV_SHM_lookup_phyaddress_node(
&device->m_phyaddr_root, (Offset + device->m_base));
up_read(&device->m_rwsem);
if (address_node ==NULL) {
return 0;
}
down_write(&device->m_rwsem);
MV_SHM_delete_phyaddress_node(&device->m_phyaddr_root, address_node);
MV_SHM_delete_virtaddress_node(&device->m_virtaddr_root,address_node);
up_write(&device->m_rwsem);
iounmap((void *)(address_node->m_virtaddress));
kfree(address_node);
return 0;
}
static HRESULT MV_SHM_UnmapVirt_Base(shm_dev_t *shm_dev, void* virtaddress)
{
shm_address_t *address_node;
shm_driver_operation_t op;
int res = 0;
if (SHM_DEVICE_LOAD_COUNT <= 0) {
MV_SHM_Print("MV_SHM_UnmapVirt_Base shm device"
" not be open\n");
return E_NOTREADY;
}
pthread_rwlock_wrlock(&shm_dev->addr.m_rb_rwlock);
address_node = MV_SHM_lookup_virtaddress_node(
&shm_dev->addr.m_virtaddr_root,(size_t)virtaddress);
if (address_node == NULL) {
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
MV_SHM_Print("MV_SHM_UnmapVirt_Base shm device"
" not found virtaddress[%08x]\n",virtaddress);
return S_OK;
}
op.m_param1 = address_node->m_phyaddress
- shm_dev->base.m_base_physaddr;
res = MV_SHM_Munmap_Base(address_node->m_virtaddress,
address_node->m_size, shm_dev->mem_type);
if (res == 0) {
MV_SHM_delete_phyaddress_node(
&shm_dev->addr.m_phyaddr_root,address_node);
MV_SHM_delete_virtaddress_node(
&shm_dev->addr.m_virtaddr_root,address_node);
free(address_node);
}
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
MV_SHM_Print("MV_SHM_UnmapVirt_Base shm device"
" free offset[%08x]\n", op.m_param1);
res = ioctl(shm_dev->base.m_fd, SHM_DEVICE_CMD_FREE, &op);
CHECK_NOTERROR("ioctl", res, -1);
return S_OK;
}
static PVOID MV_SHM_GetVirtAddr_Base(shm_dev_t *shm_dev, size_t Offset)
{
shm_address_t *address_node;
shm_driver_operation_t op;
shm_free_t shm_free;
ldns_rbnode_t *node;
if ((SHM_DEVICE_LOAD_COUNT <= 0) ||
((Offset >= shm_dev->base.m_size) &&((shm_dev->mem_type == SHM_CACHE)
|| (shm_dev->mem_type == SHM_NONCACHE)))) {
MV_SHM_Print("user space MV_SHM_GetVirtAddr_Base shm device not"
" open or parameter fail. open[%d] offset[%08x] >= "
"shm_size[%08x] mem_type[%d]\n", SHM_DEVICE_LOAD_COUNT,
Offset, shm_dev->base.m_size, shm_dev->mem_type);
return NULL;
}
pthread_rwlock_wrlock(&shm_dev->addr.m_rb_rwlock);
address_node = MV_SHM_lookup_phyaddress_node(
&(shm_dev->addr.m_phyaddr_root),
(shm_dev->base.m_base_physaddr + Offset));
if (address_node == NULL) {
address_node = malloc_shm_node();
if(address_node == NULL) {
MV_SHM_Print("user space MV_SHM_GetVirtAddr_Base"
" malloc fail\n");
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return NULL;
}
op.m_param1 = shm_dev->base.m_base_physaddr + Offset;
if (MV_SHM_mmap_preparation(shm_dev->base.m_fd, &op) != 0) {
MV_SHM_Print("user space MV_SHM_GetVirtAddr_Base "
"MV_SHM_mmap_preparation fail\n");
free(address_node);
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return NULL;
}
address_node->m_phyaddress = op.m_param1;
address_node->m_size = op.m_param2;
address_node->m_virtaddress = MV_SHM_Mmap_Base(shm_dev,
address_node->m_phyaddress, address_node->m_size);
if (address_node->m_virtaddress == (size_t)MAP_FAILED) {
MV_SHM_Print("user space MV_SHM_GetVirtAddr_Base "
"MV_SHM_Mmap_Base fail\n");
free(address_node);
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
MV_SHM_DumpProcessMemoryMap();
return NULL;
}
if (MV_SHM_insert_phyaddress_node(
&(shm_dev->addr.m_phyaddr_root), address_node) == NULL) {
MV_SHM_Print("SHM:GETV:PHY:0x%x,%d\n", Offset, __LINE__);
MV_SHM_Dump_Node(&(shm_dev->addr.m_phyaddr_root));
node = MV_SHM_delete_phyaddress_node(&(shm_dev->addr.m_phyaddr_root), \
address_node);
MV_SHM_delete_virtaddress_node(&(shm_dev->addr.m_virtaddr_root), \
node->key);
MV_SHM_insert_phyaddress_node(
&(shm_dev->addr.m_phyaddr_root), address_node);
}
if (MV_SHM_insert_virtaddress_node(
&(shm_dev->addr.m_virtaddr_root), address_node) == NULL) {
MV_SHM_Print("SHM:GETV:VIRT:0x%x,%d\n", Offset, __LINE__);
MV_SHM_Dump_Node(&(shm_dev->addr.m_virtaddr_root));
node = MV_SHM_delete_virtaddress_node(&(shm_dev->addr.m_virtaddr_root), \
address_node);
MV_SHM_delete_phyaddress_node(&(shm_dev->addr.m_phyaddr_root), \
node->key);
MV_SHM_insert_virtaddress_node(&(shm_dev->addr.m_virtaddr_root),
address_node);
}
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return (PVOID)(address_node->m_virtaddress+
((Offset + shm_dev->base.m_base_physaddr)
- address_node->m_phyaddress));;
} else {
op.m_param1 = shm_dev->base.m_base_physaddr + Offset;
if (MV_SHM_mmap_preparation(shm_dev->base.m_fd, &op) != 0) {
MV_SHM_Print("user space MV_SHM_GetVirtAddr_Base 2 "
"MV_SHM_mmap_preparation fail\n");
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return NULL;
}
if ((address_node->m_size == op.m_param2) &&
(address_node->m_phyaddress == op.m_param1)) {
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return (PVOID)(address_node->m_virtaddress+
((Offset + shm_dev->base.m_base_physaddr)
- address_node->m_phyaddress));
} else {
shm_free.address = op.m_param1;
shm_free.size = op.m_param2;
shm_free.shm_root = &shm_dev->addr;
shm_free.mem_type = shm_dev->mem_type;
MV_SHM_free_all_node(&shm_free);
address_node = shm_free.node;
if (shm_free.flag == 0) {
address_node->m_phyaddress = op.m_param1;
address_node->m_size = op.m_param2;
address_node->m_virtaddress = MV_SHM_Mmap_Base(shm_dev,
address_node->m_phyaddress, address_node->m_size);
if (address_node->m_virtaddress == (size_t)MAP_FAILED) {
MV_SHM_Print("user space MV_SHM_GetVirtAddr_Base 4 "
"MV_SHM_Mmap_Base fail\n");
free(address_node);
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
MV_SHM_DumpProcessMemoryMap();
return NULL;
}
if (MV_SHM_insert_phyaddress_node(
&(shm_dev->addr.m_phyaddr_root), address_node) == NULL) {
MV_SHM_Print("SHM:GETV:PHY:0x%x,%d\n", Offset, __LINE__);
MV_SHM_Dump_Node(&(shm_dev->addr.m_phyaddr_root));
node = MV_SHM_delete_phyaddress_node(&(shm_dev->addr.m_phyaddr_root), \
address_node);
MV_SHM_delete_virtaddress_node(&(shm_dev->addr.m_virtaddr_root), \
node->key);
MV_SHM_insert_phyaddress_node(
&(shm_dev->addr.m_phyaddr_root), address_node);
}
if (MV_SHM_insert_virtaddress_node(
&(shm_dev->addr.m_virtaddr_root), address_node) == NULL) {
MV_SHM_Print("SHM:GETV:VIRT:0x%x,%d\n", Offset, __LINE__);
MV_SHM_Dump_Node(&(shm_dev->addr.m_virtaddr_root));
node = MV_SHM_delete_virtaddress_node(&(shm_dev->addr.m_virtaddr_root), \
address_node);
MV_SHM_delete_phyaddress_node(&(shm_dev->addr.m_phyaddr_root), \
node->key);
MV_SHM_insert_virtaddress_node(&(shm_dev->addr.m_virtaddr_root),
address_node);
}
}
pthread_rwlock_unlock(&shm_dev->addr.m_rb_rwlock);
return (PVOID)(address_node->m_virtaddress+
((Offset + shm_dev->base.m_base_physaddr)
- address_node->m_phyaddress));;
}
}
}
static int MV_SHM_free_all_node(shm_free_t *shm)
{
int i = 0;
int frist = 0;
ldns_rbnode_t *node;
shm_address_t *tmp;
uint middle1, middle2, middle_size;
shm->flag = 0;
shm->node = NULL;
MV_SHM_Print("SHM:FREE:START:%d\n", __LINE__);
MV_SHM_Dump_Node(&shm->shm_root->m_phyaddr_root);
MV_SHM_Dump_Node(&shm->shm_root->m_virtaddr_root);
node = ldns_rbtree_first(&shm->shm_root->m_phyaddr_root);
while (node != LDNS_RBTREE_NULL) {
tmp = (shm_address_t *)(node->key);
node = ldns_rbtree_next(node);
middle1 = tmp->m_phyaddress + tmp->m_size/2;
middle2 = shm->address + shm->size/2;
middle_size = (tmp->m_size + shm->size)/2;
if (middle1 > middle2)
middle1 = middle1 - middle2;
else
middle1 = middle2 - middle1;
if (middle1 < middle_size) {
MV_SHM_Print("MV_SHM_free_all_node No.[%d], root[%p]"
" Node_phys[%08x] Node_virt[%08x] Node_size[%08x]"
" address[%08x] size[%08x]\n",
i, tmp, tmp->m_phyaddress, tmp->m_virtaddress,
tmp->m_size, shm->address, shm->size);
i++;
if ((tmp->m_phyaddress == shm->address)
&& (tmp->m_size == shm->size)
&& (shm->flag == 0)) {
shm->flag = 1;
if ((shm->node != NULL) && (frist == 1))
free(shm->node);
shm->node = tmp;
} else {
if (MV_SHM_Munmap_Base(tmp->m_virtaddress,
tmp->m_size, shm->mem_type) == 0) {
MV_SHM_delete_phyaddress_node(
&shm->shm_root->m_phyaddr_root, tmp);
MV_SHM_delete_virtaddress_node(
&shm->shm_root->m_virtaddr_root, tmp);
if ((frist == 0) && (shm->flag == 0)) {
frist = 1;
shm->node = tmp;
} else {
free(tmp);
}
}
}
}
}
MV_SHM_Print("SHM:FREE:END:%d\n", __LINE__);
MV_SHM_Dump_Node(&shm->shm_root->m_phyaddr_root);
MV_SHM_Dump_Node(&shm->shm_root->m_virtaddr_root);
return 0;
}
static void *MV_SHM_Base_GetVirtAddr(shm_device_t *shm_dev,
size_t Offset, int mem_type)
{
shm_address_t *address_node;
memory_node_t *tmp;
size_t physaddress = 0;
if (shm_dev == NULL) {
shm_error("kernel MV_SHM_Base_GetVirtAddr parameters"
" error shm_dev is NULL");
return NULL;
}
if (Offset >= shm_dev->m_size) {
shm_error("kernel MV_SHM_Base_GetVirtAddr parameters error"
" shm_dev[%p] Offset[%08x] > shm_size[%08x] mem_type[%d]"
"\n", shm_dev, Offset, shm_dev->m_size, mem_type);
return NULL;
}
physaddress = Offset + shm_dev->m_base;
mutex_lock(&(shm_dev->m_engine->m_mutex));
tmp = memory_engine_lookup_shm_node_for_size(
&(shm_dev->m_engine->m_shm_root), physaddress);
mutex_unlock(&(shm_dev->m_engine->m_mutex));
if (tmp == NULL) {
shm_error("kernel MV_SHM_Base_GetVirtAddr"
" memory_engine_lookup_shm_node_for_size"
" offset[%08x] physaddress[%08x] mem_type[%d]\n",
Offset, (physaddress), mem_type);
return NULL;
}
down_write(&shm_dev->m_rwsem);
address_node =
MV_SHM_lookup_phyaddress_node(
&(shm_dev->m_phyaddr_root), physaddress);
if (address_node == NULL) {
address_node = kmalloc(sizeof(shm_address_t), GFP_KERNEL);
if(address_node == NULL) {
up_write(&shm_dev->m_rwsem);
shm_error("kernel MV_SHM_Base_GetVirtAddr"
" kmalloc fail offset[%08x] mem_type[%d]\n",
Offset, mem_type);
return NULL;
}
address_node->m_phyaddress = tmp->m_phyaddress;
address_node->m_size = MEMNODE_ALIGN_SIZE(tmp);
address_node->m_virtaddress = (size_t)MV_SHM_Base_ioremap(
address_node->m_phyaddress,
address_node->m_size, mem_type);
if (address_node->m_virtaddress == (size_t)NULL) {
kfree(address_node);
up_write(&shm_dev->m_rwsem);
shm_error("kernel MV_SHM_Base_GetVirtAddr"
" MV_SHM_ioremap fail offset[%08x]"
" mem_type[%d]\n",Offset, mem_type);
return NULL;
}
MV_SHM_insert_phyaddress_node(
&(shm_dev->m_phyaddr_root), address_node);
MV_SHM_insert_virtaddress_node(
&(shm_dev->m_virtaddr_root), address_node);
up_write(&shm_dev->m_rwsem);
return (void *)(address_node->m_virtaddress +
((Offset + shm_dev->m_base)
- address_node->m_phyaddress));
} else {
if((address_node->m_phyaddress == tmp->m_phyaddress)
&& (address_node->m_size == MEMNODE_ALIGN_SIZE(tmp))) {
up_write(&shm_dev->m_rwsem);
return (void *)(address_node->m_virtaddress +
((Offset + shm_dev->m_base)
- address_node->m_phyaddress));
} else {
MV_SHM_delete_phyaddress_node(
&(shm_dev->m_phyaddr_root), address_node);
MV_SHM_delete_virtaddress_node(
&(shm_dev->m_virtaddr_root),address_node);
iounmap((void *)(address_node->m_virtaddress));
address_node->m_phyaddress = tmp->m_phyaddress;
address_node->m_size = MEMNODE_ALIGN_SIZE(tmp);
address_node->m_virtaddress = (size_t)MV_SHM_Base_ioremap(
address_node->m_phyaddress,
address_node->m_size, mem_type);
if (address_node->m_virtaddress == (size_t)NULL) {
kfree(address_node);
up_write(&shm_dev->m_rwsem);
shm_error("kernel MV_SHM_Base_GetVirtAddr"
" MV_SHM_ioremap fail offset[%08x]"
" mem_type[%d]\n", Offset, mem_type);
return NULL;
}
MV_SHM_insert_phyaddress_node(
&(shm_dev->m_phyaddr_root), address_node);
MV_SHM_insert_virtaddress_node(
&(shm_dev->m_virtaddr_root), address_node);
up_write(&shm_dev->m_rwsem);
return (void *)(address_node->m_virtaddress +
((Offset + shm_dev->m_base)
- address_node->m_phyaddress));
}
}
}