ASCIIHexDecodeFilter::ASCIIHexDecodeFilter(InputStream *pSource) : CompoundFilter(pSource)
{
unsigned char *ptr;
int c, n;
m_pBuffer = new unsigned char[pSource->Available() * 2];
ptr = m_pBuffer;
while (true)
{
c = pSource->Read();
if (c == '>')
break;
if (c <= ' ')
continue;
n = HtoD(c) << 4;
c = pSource->Read();
n |= HtoD(c);
*ptr++ = n;
if (c == '>')
break;
}
m_pStream = new ByteArrayInputStream(m_pBuffer, ptr - m_pBuffer);
}
bool nvmed_rw_verify_area(NVMED_HANDLE* nvmed_handle,
unsigned long __start_lba, unsigned int len) {
NVMED *nvmed = HtoD(nvmed_handle);
NVMED_DEVICE_INFO *dev_info = nvmed->dev_info;
unsigned long start_lba = nvmed->dev_info->start_sect + __start_lba;
if(start_lba < dev_info->start_sect)
return false;
if((dev_info->start_sect + dev_info->nr_sects) < start_lba)
return false;
if((dev_info->start_sect + dev_info->nr_sects)
< (start_lba + len))
return false;
return true;
}
/*
* Destroy MQ Handle
*/
int nvmed_handle_destroy_mq(NVMED_HANDLE* nvmed_handle) {
int i;
if(nvmed_handle == NULL) return -NVMED_NOENTRY;
if(!FLAG_ISSET(nvmed_handle, HANDLE_MQ)) {
nvmed_printf("%s: failed to destroy MQ - not MQ\n",
HtoD(nvmed_handle)->ns_path);
return -NVMED_FAULT;
}
for(i=1; i<nvmed_handle->num_mq; i++) {
pthread_spin_lock(&nvmed_handle->queue_mq[i]->mngt_lock);
nvmed_handle->queue_mq[i]->numHandle--;
pthread_spin_unlock(&nvmed_handle->queue_mq[i]->mngt_lock);
}
free(nvmed_handle->queue_mq);
return nvmed_handle_destroy(nvmed_handle);
}
/*
* I/O Completion of specific I/O
* target_id : submission id
*/
void nvmed_io_polling(NVMED_HANDLE* nvmed_handle, u16 target_id) {
NVMED* nvmed;
NVMED_QUEUE* nvmed_queue;
NVMED_IOD* iod;
volatile struct nvme_completion *cqe;
u16 head, phase;
nvmed_queue = HtoQ(nvmed_handle);
nvmed = HtoD(nvmed_handle);
pthread_spin_lock(&nvmed_queue->cq_lock);
while(1) {
head = nvmed_queue->cq_head;
phase = nvmed_queue->cq_phase;
iod = nvmed_queue->iod_arr + target_id;
if(iod->status == IO_COMPLETE) {
break;
}
cqe = (volatile struct nvme_completion *)&nvmed_queue->cqes[head];
for (;;) {
if((cqe->status & 1) == nvmed_queue->cq_phase)
break;
}
if(++head == nvmed->dev_info->q_depth) {
head = 0;
phase = !phase;
}
iod = nvmed_queue->iod_arr + cqe->command_id;
nvmed_complete_iod(iod);
COMPILER_BARRIER();
*(volatile u32 *)nvmed_queue->cq_db = head;
nvmed_queue->cq_head = head;
nvmed_queue->cq_phase = phase;
}
pthread_spin_unlock(&nvmed_queue->cq_lock);
}
/* Get CACHE from free list or evict */
NVMED_CACHE* nvmed_get_cache(NVMED_HANDLE* nvmed_handle) {
NVMED* nvmed = HtoD(nvmed_handle);
NVMED_CACHE *cache = nvmed->free_head.tqh_first;
NVMED_CACHE *__cache;
NVMED_CACHE *ret_cache;
int i;
unsigned int start_lpaddr, end_lpaddr;
TAILQ_HEAD(cache_list, nvmed_cache) temp_head;
pthread_rwlock_wrlock(&nvmed->cache_radix_lock);
pthread_spin_lock(&nvmed->cache_list_lock);
if(cache==NULL) {
//HEAD -> LRU, //TAIL -> MRU
//EVICT - LRU
cache = nvmed->lru_head.tqh_first;
if(!FLAG_ISSET(cache, CACHE_DIRTY)) {
TAILQ_REMOVE(&nvmed->lru_head, cache, cache_list);
LIST_REMOVE(cache, handle_cache_list);
radix_tree_delete(&nvmed->cache_root, cache->lpaddr);
FLAG_SET_FORCE(cache, 0);
ret_cache = cache;
}
else {
TAILQ_INIT(&temp_head);
while(FLAG_ISSET_SYNC(cache, CACHE_LOCKED) || cache->ref != 0) {
usleep(1);
}
start_lpaddr = cache->lpaddr;
end_lpaddr = cache->lpaddr;
__cache = cache->cache_list.tqe_next;
TAILQ_REMOVE(&nvmed->lru_head, cache, cache_list);
radix_tree_delete(&nvmed->cache_root, cache->lpaddr);
TAILQ_INSERT_HEAD(&temp_head, cache, cache_list);
for(i=1; i<NVMED_CACHE_FORCE_EVICT_MAX; i++) {
cache = __cache;
if(FLAG_ISSET_SYNC(cache, CACHE_LOCKED)) break;
if(!FLAG_ISSET(cache, CACHE_DIRTY)) break;
if(start_lpaddr != 0 && cache->lpaddr == start_lpaddr-1 ) {
//front_merge
start_lpaddr--;
__cache = cache->cache_list.tqe_next;
TAILQ_REMOVE(&nvmed->lru_head, cache, cache_list);
radix_tree_delete(&nvmed->cache_root, cache->lpaddr);
TAILQ_INSERT_HEAD(&temp_head, cache, cache_list);
continue;
}
else if(cache->lpaddr == end_lpaddr+1) {
//back_merge
end_lpaddr++;
__cache = cache->cache_list.tqe_next;
TAILQ_REMOVE(&nvmed->lru_head, cache, cache_list);
radix_tree_delete(&nvmed->cache_root, cache->lpaddr);
TAILQ_INSERT_TAIL(&temp_head, cache, cache_list);
continue;
}
else {
break;
}
}
if(FLAG_ISSET(cache, CACHE_DIRTY))
nvmed_cache_io_rw(nvmed_handle, nvme_cmd_write, temp_head.tqh_first,
start_lpaddr * PAGE_SIZE, (end_lpaddr - start_lpaddr) * PAGE_SIZE,
HANDLE_SYNC_IO);
cache = temp_head.tqh_first;
FLAG_SET_FORCE(cache, 0);
ret_cache = cache;
TAILQ_REMOVE(&temp_head, cache, cache_list);
LIST_REMOVE(cache, handle_cache_list);
while(temp_head.tqh_first != NULL) {
TAILQ_REMOVE(&temp_head, temp_head.tqh_first, cache_list);
TAILQ_INSERT_HEAD(&nvmed->free_head, temp_head.tqh_first, cache_list);
nvmed->num_cache_usage--;
}
}
}
else {
// Remove From Free Queue
TAILQ_REMOVE(&nvmed->free_head, cache, cache_list);
FLAG_UNSET_SYNC(cache, CACHE_FREE);
if(FLAG_ISSET(cache, CACHE_UNINIT)) {
memset(cache->ptr, 0, PAGE_SIZE);
virt_to_phys(nvmed, cache->ptr, &cache->paddr, 4096);
FLAG_UNSET_SYNC(cache, CACHE_UNINIT);
}
ret_cache = cache;
}
INIT_SYNC(ret_cache->ref);
pthread_spin_unlock(&nvmed->cache_list_lock);
pthread_rwlock_unlock(&nvmed->cache_radix_lock);
return ret_cache;
}
/*
* Send I/O to submission queue and ring SQ Doorbell
*/
ssize_t nvmed_io(NVMED_HANDLE* nvmed_handle, u8 opcode,
u64 prp1, u64 prp2, void* prp2_addr, NVMED_CACHE *__cache,
unsigned long start_lba, unsigned int len, int flags, NVMED_AIO_CTX* context) {
NVMED_QUEUE* nvmed_queue;
NVMED* nvmed;
struct nvme_command *cmnd;
NVMED_IOD* iod;
u16 target_id;
NVMED_CACHE *cache = NULL;
int i, num_cache;
nvmed_queue = HtoQ(nvmed_handle);
nvmed = HtoD(nvmed_handle);
pthread_spin_lock(&nvmed_queue->sq_lock);
while(1) {
target_id = nvmed_queue->iod_pos++;
iod = nvmed_queue->iod_arr + target_id;
if(nvmed_queue->iod_pos == nvmed->dev_info->q_depth)
nvmed_queue->iod_pos = 0;
if(iod->status != IO_INIT)
break;
}
iod->sq_id = nvmed_queue->sq_tail;
iod->prp_addr = prp2_addr;
iod->prp_pa = prp2;
iod->status = IO_INIT;
iod->num_cache = 0;
iod->cache = NULL;
iod->nvmed_handle = nvmed_handle;
iod->context = context;
if(iod->context!=NULL) {
iod->context->num_init_io++;
iod->context->status = AIO_PROCESS;
}
if(__cache != NULL) {
num_cache = len / PAGE_SIZE;
cache = __cache;
iod->cache = calloc(len / PAGE_SIZE, sizeof(NVMED_CACHE*));
for(i=0; i<num_cache; i++) {
iod->cache[i] = cache;
cache = cache->cache_list.tqe_next;
}
iod->num_cache = num_cache;
}
cmnd = &nvmed_queue->sq_cmds[nvmed_queue->sq_tail];
memset(cmnd, 0, sizeof(*cmnd));
//remap start_lba
start_lba += nvmed->dev_info->start_sect;
switch(opcode) {
case nvme_cmd_flush:
cmnd->rw.opcode = nvme_cmd_flush;
cmnd->rw.command_id = target_id;
cmnd->rw.nsid = nvmed->dev_info->ns_id;
break;
case nvme_cmd_write:
case nvme_cmd_read:
cmnd->rw.opcode = opcode;
cmnd->rw.command_id = target_id;
cmnd->rw.nsid = nvmed->dev_info->ns_id;
cmnd->rw.prp1 = prp1;
cmnd->rw.prp2 = prp2;
cmnd->rw.slba = start_lba >> nvmed->dev_info->lba_shift;
cmnd->rw.length = (len >> nvmed->dev_info->lba_shift) - 1;
cmnd->rw.control = 0;
cmnd->rw.dsmgmt = 0;
break;
case nvme_cmd_dsm:
cmnd->dsm.opcode = nvme_cmd_dsm;
cmnd->dsm.command_id = target_id;
cmnd->dsm.nsid = nvmed->dev_info->ns_id;
cmnd->dsm.prp1 = prp1;
cmnd->dsm.prp2 = 0;
cmnd->dsm.nr = 0;
cmnd->dsm.attributes = NVME_DSMGMT_AD;
break;
}
if(++nvmed_queue->sq_tail == nvmed->dev_info->q_depth)
nvmed_queue->sq_tail = 0;
COMPILER_BARRIER();
*(volatile u32 *)nvmed_queue->sq_db = nvmed_queue->sq_tail;
pthread_spin_unlock(&nvmed_queue->sq_lock);
/* If Sync I/O => Polling */
if(__FLAG_ISSET(flags, HANDLE_SYNC_IO)) {
nvmed_io_polling(nvmed_handle, target_id);
}
return len;
}
/*
* Make PRP List for Multiple page I/O from user buffer
*/
int make_prp_list(NVMED_HANDLE* nvmed_handle, void* buf,
unsigned long lba_offs, unsigned int io_size, u64* __paBase,
u64* prp1, u64* prp2, void** prp2_addr) {
unsigned int startBufPos = lba_offs / PAGE_SIZE;
unsigned int numBuf = io_size / PAGE_SIZE;
unsigned int i;
u64 *prpTmp;
u64 *prpBuf;
u64 *paBase = __paBase;
u64 __prp1, __prp2;
u64 paList[64];
unsigned int bufOffs;
*prp2_addr = NULL;
if(io_size % PAGE_SIZE > 0) numBuf ++;
if(paBase == NULL) {
numBuf = virt_to_phys(HtoD(nvmed_handle), buf, paList, numBuf * PAGE_SIZE);
bufOffs = (unsigned long)buf % PAGE_SIZE;
__prp1 = paList[0] + bufOffs;
if(numBuf == 1) {
__prp2 = 0;
}
else if(numBuf == 2) {
__prp2 = paList[1];
}
else {
prpBuf = nvmed_handle_get_prp(nvmed_handle, &__prp2);
*prp2_addr = prpBuf;
for(i = 1; i < numBuf; i++) {
prpBuf[i-1] = paList[i];
}
}
}
else {
paBase += startBufPos;
prpTmp = paBase;
__prp1 = *prpTmp;
if(numBuf == 1) {
__prp2 = 0;
}
else if(numBuf == 2) {
__prp2 = *(prpTmp+1);
}
else {
prpBuf = nvmed_handle_get_prp(nvmed_handle, &__prp2);
*prp2_addr = prpBuf;
for(i = 1; i < numBuf; i++) {
prpBuf[i-1] = paBase[i];
}
}
}
*prp1 = __prp1;
*prp2 = __prp2;
return 0;
}
