/* Return 0 if ok or non zero otherwise */
static int inv_iob_list(struct nvshm_handle *handle, struct nvshm_iobuf *iob)
{
struct nvshm_iobuf *phy_list, *leaf;
phy_list = iob;
while (phy_list) {
leaf = phy_list;
while (leaf) {
/* Check leaf address before any operation on it */
/* Cannot use nvshm_iobuf_check because iobuf */
/* is not invalidated so content will be wrong */
if (ADDR_OUTSIDE(leaf, handle->ipc_base_virt,
handle->ipc_size)) {
return -EIO;
}
/* Invalidate iobuf */
INV_CPU_DCACHE(leaf, sizeof(struct nvshm_iobuf));
/* Check iobuf */
if (nvshm_iobuf_check(leaf))
return -EIO;
/* Invalidate associated data */
if (leaf->length) {
INV_CPU_DCACHE(NVSHM_B2A(handle,
(int)leaf->npduData
+ leaf->dataOffset),
leaf->length);
}
if (leaf->sg_next)
leaf = NVSHM_B2A(handle, leaf->sg_next);
else
leaf = NULL;
}
if (phy_list->next)
phy_list = NVSHM_B2A(handle, phy_list->next);
else
phy_list = NULL;
}
return 0;
}
struct nvshm_iobuf *nvshm_queue_get(struct nvshm_handle *handle)
{
struct nvshm_iobuf *dummy, *ret;
if (!handle->shared_queue_head) {
pr_err("%s: Queue not init!\n", __func__);
return NULL;
}
dummy = handle->shared_queue_head;
/* Invalidate lower part of iobuf - upper part can be written by AP */
INV_CPU_DCACHE(&dummy->qnext,
sizeof(struct nvshm_iobuf) / 2);
ret = NVSHM_B2A(handle, dummy->qnext);
if (dummy->qnext == NULL)
return NULL;
/* Invalidate iobuf(s) and check validity */
handle->errno = inv_iob_list(handle, ret);
if (handle->errno) {
pr_err("%s: queue corruption\n", __func__);
return NULL;
}
handle->shared_queue_head = ret;
/* Update queue_bb_offset for debug purpose */
handle->conf->queue_bb_offset = (int)ret
- (int)handle->ipc_base_virt;
if ((handle->conf->queue_bb_offset < 0) ||
(handle->conf->queue_bb_offset > handle->conf->shmem_size))
pr_err("%s: out of bound descriptor offset %d addr 0x%p/0x%p\n",
__func__,
handle->conf->queue_bb_offset,
ret,
NVSHM_A2B(handle, ret));
pr_debug("%s (%p)->%p->(%p)\n", __func__,
dummy, ret, ret->qnext);
dummy->qnext = NULL;
nvshm_iobuf_free(dummy);
return ret;
}
int nvshm_iobuf_init(struct nvshm_handle *handle)
{
struct nvshm_iobuf *iob;
int ndesc, desc, datasize;
unsigned char *dataptr;
pr_debug("%s instance %d\n", __func__, handle->instance);
spin_lock_init(&alloc.lock);
/* Clear BBC free list */
alloc.bbc_pool_head = alloc.bbc_pool_tail = NULL;
alloc.free_count = 0;
ndesc = handle->desc_size / sizeof(struct nvshm_iobuf) ;
alloc.nbuf = ndesc;
alloc.free_count = 0;
datasize = handle->data_size / ndesc;
spin_lock(&alloc.lock);
if (handle->shared_queue_tail != handle->desc_base_virt) {
pr_err("%s initial tail != desc_base_virt not supported yet\n",
__func__);
}
iob = (struct nvshm_iobuf *)handle->desc_base_virt;
dataptr = handle->data_base_virt;
/* Invalidate all data region */
INV_CPU_DCACHE(dataptr, handle->data_size);
/* Clear all desc region */
memset(handle->desc_base_virt, 0, handle->desc_size);
/* Dummy queue element */
iob->npduData = NVSHM_A2B(handle, dataptr);
dataptr += datasize;
iob->dataOffset = NVSHM_DEFAULT_OFFSET;
iob->totalLength = datasize;
iob->chan = -1;
iob->next = NULL;
iob->pool_id = NVSHM_AP_POOL_ID;
iob->ref = 1;
alloc.free_pool_head = ++iob;
for (desc = 1; desc < (ndesc-1); desc++) {
iob->npduData = NVSHM_A2B(handle, dataptr);
dataptr += datasize;
iob->dataOffset = NVSHM_DEFAULT_OFFSET;
iob->totalLength = datasize;
iob->next = NVSHM_A2B(handle, (void *)iob +
sizeof(struct nvshm_iobuf));
iob->pool_id = NVSHM_AP_POOL_ID;
iob++;
}
/* Untied last */
iob->npduData = NVSHM_A2B(handle, dataptr);
iob->dataOffset = NVSHM_DEFAULT_OFFSET;
iob->totalLength = datasize;
iob->pool_id = NVSHM_AP_POOL_ID;
iob->next = NULL;
alloc.free_pool_tail = iob;
/* Flush all descriptor region */
FLUSH_CPU_DCACHE(handle->desc_base_virt,
(long)handle->desc_size);
spin_unlock(&alloc.lock);
return 0;
}
