int audited_save_pending_cmd(audited_cmd_t *fns, void *req, char *audit_string) { int cmd_id = get_free_pending_id(); uint64_t epoch_nonce, epoch_offset; void *audit_nonce; size_t audit_nonce_len; int rv; assert(fns); assert(audit_string); rv = svc_time_elapsed_us(&epoch_nonce, &epoch_offset); CHECK_RV(rv, -1, "svc_time_elapsed_us"); audit_nonce_len = 256; audit_nonce = malloc(audit_nonce_len); CHECK_MEM(audit_nonce, -1); rv = svc_utpm_rand_block(audit_nonce, audit_nonce_len); CHECK_RV(rv, -1, "svc_utpm_rand_block"); pending_cmds[cmd_id] = (audited_pending_cmd_t) { .audit_string = audit_string, .req = req, .fns = fns, .epoch_nonce = epoch_nonce, .epoch_offset = epoch_offset, .audit_nonce = audit_nonce, .audit_nonce_len = audit_nonce_len, }; out: return cmd_id; }
void DeviceManager::EnumerateDevices() { XnStatus nRetVal = XN_STATUS_OK; xn::NodeInfoList deviceList; XnMapOutputMode mapMode; mapMode.nXRes = XN_VGA_X_RES; mapMode.nYRes = XN_VGA_Y_RES; mapMode.nFPS = 30; nRetVal = context_->EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, deviceList, NULL); CHECK_RV(nRetVal, "Enumerate"); for(xn::NodeInfoList::Iterator iter = deviceList.Begin(); iter != deviceList.End(); ++iter) { xn::NodeInfo node = (*iter); SensorDevice* sensor = new SensorDevice(); char deviceName[256]; //Create Device Context in the global production tree nRetVal = context_->CreateProductionTree(node); CHECK_RV(nRetVal, "Create Device"); Query query; query.AddNeededNode(node.GetInstanceName()); xnOSMemCopy(deviceName, node.GetInstanceName(), xnOSStrLen(node.GetInstanceName())); sensor->SetDeviceName(deviceName); //Create Image and Depth generators for this device nRetVal = context_->CreateAnyProductionTree(XN_NODE_TYPE_IMAGE, &query, *(sensor->GetImageGenerator())); CHECK_RV(nRetVal, "Create Image Generator"); nRetVal = context_->CreateAnyProductionTree(XN_NODE_TYPE_DEPTH, &query, *(sensor->GetDepthGenerator())); CHECK_RV(nRetVal, "Create Depth Generator"); sensor->GetDepthGenerator()->SetMapOutputMode(mapMode); sensor->GetImageGenerator()->SetMapOutputMode(mapMode); //Align the RGB and Depth Cameras sensor->AlignSensors(); devices_.push_back(sensor); } context_->StartGeneratingAll(); }
//if offset=0, use now cursor as start yf_int_t yf_cb_fhead_set(yf_circular_buf_t* cb, yf_s32_t offset) { CHECK_RV(offset < 0, YF_ERROR); if (offset == 0) { cb->head_index = cb->cursor_index; cb->head_offset = cb->cursor_offset; return YF_OK; } yf_s32_t fsize = yf_cb_fsize(cb); if (offset > fsize) return YF_ERROR; __yf_circular_buf_advance_front(cb->head_index, cb->head_offset, offset, cb); //check cursor pos yf_s32_t cpos = yf_cb_ftell(cb); if (cpos < 0 || cpos > yf_cb_fsize(cb)) { cb->cursor_index = cb->head_index; cb->cursor_offset = cb->head_offset; } return YF_OK; }
yf_int_t yf_tpl_hash_init(yf_tpl_hash_t* hash, yf_uint_t suggest_buckets, yf_tpl_hash_pt hfunc, yf_tpl_cmp_pt cmp) { yf_u32_t i1 = 0; yf_memzero(hash, sizeof(yf_tpl_hash_t)); hash->hash = hfunc; hash->cmp = cmp; for ( i1 = 0; i1 < YF_ARRAY_SIZE(_yf_tpl_hash_num_primes); i1++ ) { if (_yf_tpl_hash_num_primes[i1] >= suggest_buckets) { suggest_buckets = _yf_tpl_hash_num_primes[i1]; break; } } hash->bucket_size = suggest_buckets; hash->buckets = yf_alloc(suggest_buckets * sizeof(yf_tpl_hash_link_t*)); CHECK_RV(hash->buckets == NULL, -1); for ( i1 = 0; i1 < suggest_buckets ; i1++ ) { yf_init_slist_head(hash->buckets[i1]); } return 0; }
yf_int_t yf_cb_ftruncate(yf_circular_buf_t* cb, yf_s32_t size) { CHECK_RV(size < 0, YF_ERROR); yf_s32_t osize = yf_cb_fsize(cb); yf_s32_t dsize = size - osize; if (dsize == 0) return YF_OK; else if (dsize > 0) { yf_s32_t more_bytes = size - yf_cb_ftell(cb); yf_s32_t free_wsize = yf_cb_space_enlarge(cb, more_bytes); if (free_wsize < more_bytes) return YF_ERROR; //process tail i+o _yf_cb_space_write_tail(cb, free_wsize, more_bytes); return YF_OK; } dsize = -dsize; __yf_circular_buf_advance_back(cb->tail_index, cb->tail_offset, dsize, cb); yf_s32_t cpos = yf_cb_ftell(cb); if (cpos < 0 || cpos > size) { cb->cursor_index = cb->tail_index; cb->cursor_offset = cb->tail_offset; } return YF_OK; }
int DeviceManager::GetCameraDataByDeviceIndex(const unsigned int index, CameraDataPacket* dataPacket) { if(index > devices_.size()) { printf("Index invalid, requested index %d, only %d devices available!\n", index, devices_.size()); return -1; } XnStatus nRetVal = XN_STATUS_OK; nRetVal = context_->WaitAnyUpdateAll(); CHECK_RV(nRetVal, "WaitAnyUpdateAll() failed"); if(devices_[index]->GetDepthGenerator()->IsValid()) { devices_[index]->GetDepthGenerator()->GetMetaData(*(devices_[index]->GetDepthMD())); } if(devices_[index]->GetImageGenerator()->IsValid()) { devices_[index]->GetImageGenerator()->GetMetaData(*(devices_[index]->GetImageMD())); } dataPacket->depthMap_ = devices_[index]->GetDepthMD()->Data(); dataPacket->imageMap_ = devices_[index]->GetImageMD()->RGB24Data(); return 0; }
yf_s32_t yf_cb_fread(yf_circular_buf_t* cb, yf_s32_t bytes, yf_int_t flags, char** rbuf) { yf_s32_t rbytes = 0; yf_s32_t roffset = cb->cursor_offset; yf_s16_t rindex = cb->cursor_index, tindex = cb->tail_index; yf_s32_t rest_rsize = yf_circular_buf_rest_rsize(cb); bytes = yf_min(rest_rsize, bytes); CHECK_RV(bytes <= 0, bytes); _yf_cb_preprocess_cursor(rindex, roffset, cb); rbytes = cb->buf_size - roffset; if (likely(rbytes >= bytes)) { *rbuf = cb->buf[rindex] + roffset; if (flags != YF_READ_PEEK) { cb->cursor_index = rindex; cb->cursor_offset = roffset + bytes;//may cursor_offset == buf_size } return bytes; } char* b = NULL; b = yf_palloc(cb->mpool, bytes); assert(b); *rbuf = b; b = yf_cpymem(b, cb->buf[rindex] + roffset, rbytes); rindex = yf_cb_increase(rindex, cb->buf_used); //rest num rbytes = bytes - rbytes; while (1) { if (likely(rbytes <= cb->buf_size)) { b = yf_cpymem(b, cb->buf[rindex], rbytes); if (flags != YF_READ_PEEK) { cb->cursor_index = rindex; cb->cursor_offset = rbytes; } break; } else { b = yf_cpymem(b, cb->buf[rindex], cb->buf_size); rindex = yf_cb_increase(rindex, cb->buf_used); rbytes -= cb->buf_size; } } return bytes; }
yf_s32_t yf_cb_fwrite(yf_circular_buf_t* cb, char* buf, yf_s32_t bytes) { yf_s32_t wbytes = 0; yf_s32_t free_wsize = yf_circular_buf_free_wsize(cb); yf_s32_t woffset = cb->cursor_offset; yf_s16_t windex = cb->cursor_index; CHECK_RV(bytes <= 0, bytes); if (unlikely(free_wsize < bytes)) { free_wsize = yf_cb_space_enlarge(cb, bytes); windex = cb->cursor_index; } bytes = yf_min(bytes, free_wsize); //process tail i+o _yf_cb_space_write_tail(cb, free_wsize, bytes); _yf_cb_preprocess_cursor(windex, woffset, cb); wbytes = cb->buf_size - woffset; if (likely(wbytes >= bytes)) { yf_memcpy(cb->buf[windex] + woffset, buf, bytes); cb->cursor_index = windex; cb->cursor_offset = woffset + bytes; return bytes; } yf_memcpy(cb->buf[windex] + woffset, buf, wbytes); windex = yf_cb_increase(windex, cb->buf_used); buf += wbytes; //rest num wbytes = bytes - wbytes; while (1) { if (likely(wbytes <= cb->buf_size)) { yf_memcpy(cb->buf[windex], buf, wbytes); cb->cursor_index = windex; cb->cursor_offset = wbytes; break; } else { yf_memcpy(cb->buf[windex], buf, cb->buf_size); windex = yf_cb_increase(windex, cb->buf_used); wbytes -= cb->buf_size; buf += cb->buf_size; } } return bytes; }
yf_int_t yf_circular_buf_init(yf_circular_buf_t* cb, yf_s32_t buf_size, yf_log_t * log) { yf_memzero(cb, sizeof(yf_circular_buf_t)); cb->buf_size = yf_align_2pow(buf_size); cb->buf_size_pow = yf_bit_cnt(cb->buf_size) - 1; cb->mpool = yf_create_pool(yf_pagesize >> 1, log); CHECK_RV(cb->mpool == NULL, YF_ERROR); return YF_OK; }
yf_int_t yf_strerror_init(void) { yf_err_no_end = 0; yf_int_t list_size = YF_MAX_ERR_NO; yf_err_str_list = yf_alloc(sizeof(yf_str_t) * list_size); CHECK_RV(yf_err_str_list == NULL, YF_ERROR); for (yf_err_no_end = 0; yf_err_no_end < YF_MAX_ERR_NO; ++yf_err_no_end) { yf_errno = 0; char* p_errstr = strerror(yf_err_no_end); if (yf_err_no_end >= list_size) { list_size = list_size*3/2; yf_err_str_list = yf_realloc(yf_err_str_list, list_size); CHECK_RV(yf_err_str_list == NULL, YF_ERROR); } if (yf_errno == YF_EINVAL || p_errstr == NULL || yf_strncmp(p_errstr, "Unknown error", 13) == 0) { yf_err_str_list[yf_err_no_end] = yf_unknown_err_str; continue; } size_t len = yf_strlen(p_errstr); char* str_alloc = yf_alloc(len + 1); CHECK_RV(str_alloc == NULL, YF_ERROR); yf_strncpy(str_alloc, p_errstr, len); yf_err_str_list[yf_err_no_end].data = str_alloc; yf_err_str_list[yf_err_no_end].len = len; } return YF_OK; }
void SensorDevice::AlignSensors() { XnStatus nRetVal = XN_STATUS_OK; //Align depth image to RGB Camera image XnBool isSupported = depthG_.IsCapabilitySupported("AlternativeViewPoint"); if(TRUE == isSupported) { std::cout << "Aligning depth to RGB is supported with device." << std::endl; nRetVal = depthG_.GetAlternativeViewPointCap().SetViewPoint(imageG_); CHECK_RV(nRetVal, "AlignSensors"); } }
audited_err_t audited_check_cmd_auth(audited_pending_cmd_t *cmd, const void* audit_token, size_t audit_token_len) { uint64_t epoch_nonce, epoch_offset; int svc_rv; int crypto_rv; audited_err_t rv=AUDITED_ENONE; SHA256_CTX sha256_ctx; uint8_t digest[SHA256_DIGEST_LENGTH]; /* check time first, in case signature verification time is significant */ svc_rv = svc_time_elapsed_us(&epoch_nonce, &epoch_offset); CHECK_RV(svc_rv, AUDITED_ESVC | (svc_rv<<8), "svc_time_elapsed_us"); if(epoch_nonce != cmd->epoch_nonce || (epoch_offset - cmd->epoch_offset) > AUDITED_TIMEOUT_US) { rv = AUDITED_ETIMEOUT; goto out; } /* check signature */ /* compute digest of expected message */ crypto_rv = SHA256_Init(&sha256_ctx); CHECK(crypto_rv, AUDITED_ECRYPTO, "SHA256_Init"); crypto_rv = SHA256_Update(&sha256_ctx, cmd->audit_nonce, cmd->audit_nonce_len); CHECK(crypto_rv, AUDITED_ECRYPTO, "SHA256_Update"); crypto_rv = SHA256_Update(&sha256_ctx, cmd->audit_string, strlen(cmd->audit_string)); /* null not included */ CHECK(crypto_rv, AUDITED_ECRYPTO, "SHA256_Update"); crypto_rv = SHA256_Final(&digest[0], &sha256_ctx); CHECK(crypto_rv, AUDITED_ECRYPTO, "SHA256_Final"); crypto_rv = RSA_verify(NID_sha256, digest, SHA256_DIGEST_LENGTH, (unsigned char*)audit_token, audit_token_len, audit_pub_key); CHECK(crypto_rv, AUDITED_EBADSIG, "RSA_verify"); out: return rv; }
//ret writed space yf_s32_t yf_cb_space_write_alloc(yf_circular_buf_t* cb, yf_s32_t bytes , char*** rbufs, yf_s32_t* woffset) { char** b = NULL; yf_s32_t free_wsize = yf_circular_buf_free_wsize(cb); yf_s32_t wbytes = 0; yf_s16_t wbsize = 0; yf_s16_t windex = cb->cursor_index; CHECK_RV(bytes <= 0, bytes); if (unlikely(free_wsize < bytes)) { free_wsize = yf_cb_space_enlarge(cb, bytes); windex = cb->cursor_index; } bytes = yf_min(bytes, free_wsize); //same with read impl if (*rbufs == NULL) *rbufs = _yf_cb_bufs_alloc(cb, bytes); b = *rbufs; *woffset = cb->cursor_offset; _yf_cb_preprocess_cursor(windex, *woffset, cb); wbytes = cb->buf_size - *woffset; *b = cb->buf[windex]; if (wbytes >= bytes) { return bytes; } ++b; //rest num wbytes = bytes - wbytes; wbsize = yf_circular_buf_bsize(cb, wbytes); windex = yf_cb_increase(windex, cb->buf_used); yf_cb_space_gwrite(cb, b, windex, wbsize); return bytes; }
yf_log_t *yf_log_open(yf_uint_t log_level, yf_u32_t log_max_len, void* init_ctx) { if (log_max_len < 1024) { fprintf(stderr, "log cache size too small, log open failed..."); return NULL; } yf_log_t* yf_log = yf_log_actions.log_open(log_level, log_max_len, init_ctx); CHECK_RV(yf_log == NULL, NULL); yf_log->log_level = log_level; yf_log->each_log_max_len = log_max_len; yf_lock_init(&yf_log->lock); yf_log->log_actions = (void*)&yf_log_actions; return yf_log; }
static yf_log_t* _log_default_open(yf_uint_t log_level, yf_u32_t log_max_len, void* init_ctx) { char* path = init_ctx; yf_u32_t path_len = 0; if (path) path_len = yf_strlen(path); yf_u32_t head_size = yf_align_mem(sizeof(yf_log_t)); yf_u32_t ctx_size = yf_align_mem(sizeof(_log_default_ctx_t)); yf_u32_t all_size = head_size + ctx_size + sizeof(yf_file_t) + log_max_len + path_len; yf_log_t* log = (yf_log_t*)yf_alloc(all_size); CHECK_RV(log==NULL, NULL); _log_default_ctx_t* log_ctx = yf_mem_off(log, head_size); log->log_ctx = log_ctx; log_ctx->file = yf_mem_off(log_ctx, ctx_size); log_ctx->log_buf = yf_mem_off(log_ctx->file, sizeof(yf_file_t)); if (path == NULL) { log_ctx->file->fd = yf_stderr; return log; } log_ctx->file->name.data = log_ctx->log_buf + log_max_len; log_ctx->file->name.len = path_len; yf_memcpy(log_ctx->file->name.data, path, path_len); log_ctx->file->fd = yf_open_file(path, YF_FILE_APPEND, YF_FILE_CREATE_OR_OPEN, YF_FILE_DEFAULT_ACCESS); if (log_ctx->file->fd == YF_INVALID_FILE) { log_ctx->file->fd = yf_stderr; } return log; }
audited_err_t audited_start_cmd(const Audited__StartReq *startreq, Audited__StartRes *startres) { audited_err_t rv=0; ProtobufCMessage *req=NULL; audited_cmd_t *cmd_desc=NULL; char *audit_string=NULL; uint32_t pending_cmd_id; bool saved_pending_cmd=false; if(!did_init) { rv = AUDITED_EBADSTATE; goto out; } if (startreq->cmd >= audited_cmds_num) { rv = AUDITED_EBADAUDITEDCMD; goto out; } cmd_desc = &audited_cmds[startreq->cmd]; assert(cmd_desc); req = protobuf_c_message_unpack(cmd_desc->req_descriptor, NULL, startreq->cmd_input.len, startreq->cmd_input.data); CHECK(req, AUDITED_EDECODE, "protobuf_c_message_unpack"); assert(cmd_desc->audit_string); startres->svc_err = cmd_desc->audit_string(req, &audit_string); CHECK_RV((unsigned int)startres->svc_err, rv, "audit_string()"); pending_cmd_id = audited_save_pending_cmd(cmd_desc, req, audit_string); CHECK(pending_cmd_id >= 0, AUDITED_ESAVE, "audited_save_pending_cmd"); saved_pending_cmd=true; startres->res = malloc(sizeof(Audited__StartRes__Res)); CHECK_MEM(startres->res, AUDITED_ENOMEM); *(startres->res) = (Audited__StartRes__Res) { .base = PROTOBUF_C_MESSAGE_INIT (&audited__start_res__res__descriptor), .pending_cmd_id = pending_cmd_id, .audit_nonce.data = pending_cmds[pending_cmd_id].audit_nonce, .audit_nonce.len = pending_cmds[pending_cmd_id].audit_nonce_len, .audit_string = audit_string, }; out: if (rv || startres->svc_err) { if (saved_pending_cmd) { audited_release_pending_cmd_id(pending_cmd_id); } else { free(audit_string); if(req) { protobuf_c_message_free_unpacked(req, NULL); } } free(startres->res); startres->res=NULL; } return rv; } audited_err_t audited_execute_cmd(const Audited__ExecuteReq *exec_req, Audited__ExecuteRes *exec_res) { audited_err_t rv; audited_pending_cmd_t *cmd=NULL; ProtobufCMessage *res=NULL; bool got_res=false; if(!did_init) { rv = AUDITED_EBADSTATE; goto out; } cmd = audited_pending_cmd_of_id(exec_req->pending_cmd_id); CHECK(cmd >= 0, AUDITED_EBADCMDHANDLE, "audited_pending_cmd_of_id(%d)", (unsigned int)exec_req->pending_cmd_id); rv = audited_check_cmd_auth(cmd, exec_req->audit_token.data, exec_req->audit_token.len); CHECK_RV(rv, rv, "audited_check_cmd_auth"); assert(cmd->fns); assert(cmd->fns->execute); assert(cmd->fns->res_descriptor); res = malloc(cmd->fns->res_descriptor->sizeof_message); CHECK_MEM(res, AUDITED_ENOMEM); protobuf_c_message_init(cmd->fns->res_descriptor, res); exec_res->svc_err = cmd->fns->execute(cmd->req, res); CHECK_RV((unsigned int)exec_res->svc_err, rv, "execute()"); got_res=true; exec_res->cmd_output.len = protobuf_c_message_get_packed_size(res); exec_res->cmd_output.data = malloc(exec_res->cmd_output.len); CHECK_MEM(exec_res->cmd_output.data, AUDITED_ENOMEM); protobuf_c_message_pack(res, exec_res->cmd_output.data); exec_res->has_cmd_output=true; out: if (got_res && cmd->fns->release_res) { cmd->fns->release_res(res); } if (res) { free(res); res=NULL; } audited_release_pending_cmd_id(exec_req->pending_cmd_id); return rv; }
//ret read size yf_s32_t yf_cb_space_read(yf_circular_buf_t* cb, yf_s32_t bytes , char*** rbufs, yf_s32_t* roffset, yf_int_t flags) { char** b = NULL; yf_s32_t rbytes = 0; yf_s16_t rbsize = 0; yf_s16_t rindex = cb->cursor_index, tindex = cb->tail_index; yf_s32_t rest_rsize = yf_circular_buf_rest_rsize(cb); CHECK_RV(bytes <= 0, bytes); if (*rbufs == NULL) *rbufs = _yf_cb_bufs_alloc(cb, bytes); b = *rbufs; *roffset = cb->cursor_offset; _yf_cb_preprocess_cursor(rindex, *roffset, cb); if (unlikely(rest_rsize <= bytes)) { rbytes = rest_rsize; rbsize = yf_cb_diff(tindex, rindex, cb->buf_used, YF_CBR) + 1; yf_cb_space_gread(cb, b, rindex, rbsize); if (flags != YF_READ_PEEK) { cb->cursor_index = tindex; cb->cursor_offset = cb->tail_offset; } return rbytes; } *b = cb->buf[rindex]; rbytes = cb->buf_size - *roffset; if (likely(rbytes >= bytes)) { if (flags != YF_READ_PEEK) { cb->cursor_index = rindex; cb->cursor_offset = *roffset + bytes;//may cursor_offset == buf_size } return bytes; } ++b; assert(tindex != rindex); //rest num rbytes = bytes - rbytes; rbsize = yf_circular_buf_bsize(cb, rbytes); if (flags != YF_READ_PEEK) { cb->cursor_index = yf_cb_add(rindex, rbsize, cb->buf_used); cb->cursor_offset = yf_cb_rest_mod(rbytes, cb->buf_size); } rindex = yf_cb_increase(rindex, cb->buf_used); yf_cb_space_gread(cb, b, rindex, rbsize); return bytes; }