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;
}
