/*
* ======== Stream_write ========
*/
SizeT Stream_submit(Stream_Object *obj, Ptr buf, SizeT nmadus,
DriverTypes_PacketCmd cmd, UInt timeout, Error_Block *eb)
{
SizeT size;
Assert_isTrue((obj->issued == 0), Stream_A_pendingReclaims);
Assert_isTrue(Sync_query(obj->complete, ISync_Q_BLOCKING),
Stream_A_syncNonBlocking);
Stream_issueX(obj, buf, nmadus, NULL, cmd, eb);
if (Error_check(eb)) {
return (0);
}
size = Stream_reclaim(obj, NULL, timeout, NULL, eb);
if (Error_check(eb)) {
if (Error_getId(eb) == Stream_E_timeout) {
Stream_abort(obj, eb);
size = Stream_reclaim(obj, NULL, timeout, NULL, eb);
}
}
return (size);
}
/**
* @name domxtmgr_register_connection
* @brief Register tunnel connection in connection table
* @param hComponentRealHandle : Real Component handles
* @param szCompName : Name of the component
* @param eCompCoreId : Component Native core ID
* @param pRpcSkelHandle : Pointer to RPC skeleton handle
* @param cComponentRcmSvrName : Name of component server
* @param hConnectionHandle : Handle to the connected component
* @param eConnectedCoreId : TUnneled CoreID
* @param eb : Error block to raise errors
* @return none
*/
static Void domxtmgr_register_connection (OmxTypes_OMX_HANDLETYPE
hComponentRealHandle,
Char *szCompName,
DomxTypes_coreType eCompCoreId,
OmxTypes_OMX_PTR pRpcSkelHandle,
Char *cComponentRcmSvrName,
OmxTypes_OMX_HANDLETYPE
hConnectionHandle,
DomxTypes_coreType eConnectedCoreId,
Error_Block *eb)
{
Int32 nCompIdx;
DOMX_UTL_TRACE_FUNCTION_ENTRY_LEVEL1 ();
nCompIdx = domxtmgr_get_component_registry_index (hComponentRealHandle);
if (-1 == nCompIdx)
{
domxtmgr_register_component (hComponentRealHandle,
szCompName,
eCompCoreId,
pRpcSkelHandle, cComponentRcmSvrName, eb);
if (FALSE == Error_check (eb))
{
nCompIdx = domxtmgr_get_component_registry_index (hComponentRealHandle);
DOMX_UTL_TRACE_FUNCTION_ASSERT ((nCompIdx != -1),
"Invalid compIndex");
}
}
else
{
/* Ensure component native coreId is same as what is already registered */
DOMX_UTL_TRACE_FUNCTION_ASSERT ((eCompCoreId ==
DomxTunnelMgr_module->connectionInfoTbl.
elem[nCompIdx][0].eCompCoreId),
"Invalid compCoreId");
DOMX_UTL_TRACE_FUNCTION_ASSERT ((strcmp
((OMX_STRING) DomxTunnelMgr_module->
connectionInfoTbl.elem[nCompIdx][0].
cComponentName, szCompName) == 0),
"Invalid compName");
if (NULL != pRpcSkelHandle)
{
DOMX_UTL_TRACE_FUNCTION_ASSERT ((pRpcSkelHandle ==
DomxTunnelMgr_module->connectionInfoTbl.
elem[nCompIdx][0].sRpcSkelInfo.
pRpcSkelHandle),
"Invalid rpcSkelHandle");
}
}
if (FALSE == Error_check (eb))
{
domxtmgr_add_component_connection_info (nCompIdx, eConnectedCoreId,
hConnectionHandle);
}
DOMX_UTL_TRACE_FUNCTION_EXIT_LEVEL1 (OMX_ErrorNone);
}
/*
* ======== Task_postInit ========
* Function to be called during module startup to complete the
* initialization of any statically created or constructed task.
* Initialize stack.
* Build Initial stack image.
* Add task to corresponding ready Queue.
*
* returns (0) and clean 'eb' on success
* returns (0) and 'eb' if Task_SupportProxy_start() fails.
* returns (n) and 'eb' for number of successful createFxn() calls iff
* one of the createFxn() calls fails
*/
Int Task_postInit(Task_Object *tsk, Error_Block *eb)
{
UInt tskKey, hwiKey;
Queue_Handle readyQ;
#ifndef ti_sysbios_knl_Task_DISABLE_ALL_HOOKS
Int i;
#endif
tsk->context = Task_SupportProxy_start(tsk,
(Task_SupportProxy_FuncPtr)Task_enter,
(Task_SupportProxy_FuncPtr)Task_exit,
eb);
if (Error_check(eb)) {
return (0);
}
tsk->mode = Task_Mode_READY;
tsk->pendElem = NULL;
#ifndef ti_sysbios_knl_Task_DISABLE_ALL_HOOKS
for (i = 0; i < Task_hooks.length; i++) {
tsk->hookEnv[i] = (Ptr)0;
if (Task_hooks.elem[i].createFxn != NULL) {
Task_hooks.elem[i].createFxn(tsk, eb);
if (Error_check(eb)) {
return (i);
}
}
}
#endif
if (tsk->priority < 0) {
tsk->mask = 0;
tsk->readyQ = Task_Module_State_inactiveQ();
Queue_put(tsk->readyQ, (Queue_Elem *)tsk);
}
else {
tsk->mask = 1 << tsk->priority;
readyQ = Queue_Object_get(Task_module->readyQ, tsk->priority);
tsk->readyQ = readyQ;
tskKey = Task_disable();
hwiKey = Hwi_disable();
Task_unblock(tsk);
Hwi_restore(hwiKey);
Task_restore(tskKey);
}
return (0);
}
/**
* @name domxtmgr_map_connection_handle
* @brief Map component handle to connection handle
* @param nCompIdx : Component index in connection table
* @param ePeerCompCoreId : Tunneled peer coreID
* @param hTunneledPeerMap : Pointer to handle of Peer component
* @param eb : Error block to raise errors
* @return none
*/
static Void domxtmgr_map_connection_handle (Int nCompIdx,
DomxTypes_coreType ePeerCompCoreId,
OmxRpc_Handle pRpcStubHandle,
OmxTypes_OMX_HANDLETYPE
*hTunneledPeerMap,
Error_Block * eb)
{
OmxTypes_OMX_HANDLETYPE hCompConnectionHandle;
DOMX_UTL_TRACE_FUNCTION_ENTRY_LEVEL1 ();
Log_print6 (Diags_USER1, "Entered: %s (%d, %d, 0x%x, 0x%x, 0x%x)",
(xdc_IArg) __FUNCTION__, nCompIdx,
ePeerCompCoreId, (xdc_IArg) pRpcStubHandle,
(xdc_IArg) hTunneledPeerMap, (xdc_IArg) eb);
*hTunneledPeerMap = NULL;
if (FALSE == Error_check (eb))
{
domxtmgr_get_connection_handle (nCompIdx, ePeerCompCoreId,
&hCompConnectionHandle);
if (NULL == hCompConnectionHandle)
{
domxtmgr_create_connection (nCompIdx, ePeerCompCoreId,
pRpcStubHandle, &hCompConnectionHandle, eb);
if (FALSE == Error_check (eb))
{
domxtmgr_add_component_connection_info (nCompIdx,
ePeerCompCoreId,
hCompConnectionHandle);
domxtmgr_get_connection_handle (nCompIdx, ePeerCompCoreId,
&hCompConnectionHandle);
DOMX_UTL_TRACE_FUNCTION_ASSERT ((NULL != hCompConnectionHandle),
"CompConnectionHandle is NULL");
}
}
else
{
domxtmgr_add_component_connection_info (nCompIdx,
ePeerCompCoreId,
hCompConnectionHandle);
}
}
if (FALSE == Error_check (eb))
{
*hTunneledPeerMap = hCompConnectionHandle;
}
DOMX_UTL_TRACE_FUNCTION_EXIT_LEVEL1 (OMX_ErrorNone);
}
Int SystemCfg_createResources(SystemCfg_Object *obj)
{
Error_Block eb;
Int status = 0;
HeapBuf_Params heapBufP;
IHeap_Handle heapH;
Log_print1(Diags_ENTRY | Diags_INFO, "--> "FXNN": (obj=0x%x)",(IArg)obj);
Error_init(&eb);
/* allocate heap backing store from SR_0 heap */
heapH = (IHeap_Handle)SharedRegion_getHeap(0);
obj->rcmHeapBufSize = 5 * 128;
obj->rcmHeapBufBase = Memory_alloc(heapH, obj->rcmHeapBufSize, 128, &eb);
if (Error_check(&eb)) {
Log_error1(FXNN": out of memory: size=%u", obj->rcmHeapBufSize);
status = -1;
goto leave;
}
/* create heap for messages */
HeapBuf_Params_init(&heapBufP);
heapBufP.blockSize = 128; // header = 52 B, payload = 76 B
heapBufP.numBlocks = 5; // 5 messages, total heap storage = 640 B
heapBufP.align = 128; // align on cache line boundary
heapBufP.buf = obj->rcmHeapBufBase; // heap storage base address
heapBufP.bufSize = obj->rcmHeapBufSize; // heap storage size
obj->rcmHeapH = HeapBuf_create(&heapBufP, &eb);
if (Error_check(&eb)) {
Log_error0(FXNN": HeapBuf_create() failed");
status = -1;
goto leave;
}
/* register this heap with MessageQ */
Log_print2(Diags_INFO,
FXNN": MessageQ_registerHeap: (rcmHeapH: 0x%x, heapId: %d)",
(IArg)(obj->rcmHeapH), (IArg)SystemCfg_RcmMsgHeapId_CompDev);
MessageQ_registerHeap((Ptr)(obj->rcmHeapH), SystemCfg_RcmMsgHeapId_CompDev);
leave:
Log_print1(Diags_EXIT, "<-- "FXNN": %d", (IArg)status);
return(status);
}
/*
* ======== Hello_waitForEvent ========
*/
static UInt32 Hello_waitForEvent(Void)
{
UInt32 event;
Error_Block eb;
if (Module.error >= App_E_FAILURE) {
event = Module.error;
goto leave;
}
Error_init(&eb);
/* use counting semaphore to wait for next event */
SemThread_pend(Module.semH, SemThread_FOREVER, &eb);
if (Error_check(&eb)) {
event = Module.error;
goto leave;
}
/* remove next command from queue */
event = Module.eventQue[Module.tail];
Module.tail = (Module.tail + 1) % QUEUESIZE;
leave:
return(event);
}
/*
* ======== task1 ========
*/
Void task1(UArg arg0, UArg arg1)
{
Error_Block eb;
Int value;
Error_init(&eb);
System_printf("Running task1 function\n");
/*
* Showing a case where we call a function that takes an Error_Block.
* This function calls another function which can fail.
* The Error_Block to passed up to the caller in case an error occurs.
*/
value = canFail1(&eb, 11);
System_printf("Value returned from canFail1 = %d\n", value);
if (Error_check(&eb)) {
/* Should get here */
System_printf("Incorrect value used. Must be a multiple of 2\n");
}
/*
* Showing a case where a Memory_alloc() is called but no Error_Block
* is passed. This Memory_alloc() will fail because the size is larger
* than the available memory in the default heap.
*
* This call will cause the program to abort. The error message can
* be found in the LoggerBuf ROV view.
*/
Memory_alloc(NULL, 0xffff, 0, NULL);
}
Int SystemCfg_createResources(SystemCfg_Object *obj)
{
Error_Block eb;
Int status = 0;
HeapBufMP_Params heapParams;
Log_print1(Diags_ENTRY | Diags_INFO, "--> "FXNN": (obj=0x%x)",(IArg)obj);
Error_init(&eb);
/* create heap for rcm messages */
HeapBufMP_Params_init(&heapParams);
heapParams.name = SystemCfg_RcmMsgHeapName_CompDev;
heapParams.regionId = 0;
heapParams.blockSize = 128; // header = 52 B, payload = 76 B
heapParams.numBlocks = 5; // 5 messages, total heap storage = 640 B
heapParams.align = 128; // align on cache line boundary
obj->rcmHeapH = HeapBufMP_create(&heapParams);
if (Error_check(&eb)) {
Log_error0(FXNN": HeapBuf_create() failed");
status = -1;
goto leave;
}
/* register this heap with MessageQ */
MessageQ_registerHeap((Ptr)(obj->rcmHeapH), SystemCfg_RcmMsgHeapId_CompDev);
leave:
Log_print1(Diags_EXIT, "<-- "FXNN": %d", (IArg)status);
return(status);
}
/*
* ======== Hwi_Instance_init ========
*/
Int Hwi_Instance_init(Hwi_Object *hwi, Int intNum, Hwi_FuncPtr fxn, const Hwi_Params *params, Error_Block *eb)
{
Int status;
if (Hwi_module->dispatchTable[intNum] != NULL) {
Error_raise(eb, Hwi_E_alreadyDefined, intNum, 0);
return (1);
}
Hwi_module->dispatchTable[intNum] = hwi;
Hwi_reconfig(hwi, fxn, params);
#ifndef ti_sysbios_hal_Hwi_DISABLE_ALL_HOOKS
if (Hwi_hooks.length > 0) {
/* Allocate environment space for each hook instance. */
hwi->hookEnv = Memory_calloc(Hwi_Object_heap(),
Hwi_hooks.length * sizeof(Ptr), 0, eb);
if (hwi->hookEnv == NULL) {
return (1);
}
}
#endif
hwi->irp = 0;
status = postInit(hwi, eb);
if (Error_check(eb)) {
return (2 + status);
}
return (0);
}
Void SystemCfg_notifyCB__P(UInt16 procId, UInt16 lineId, UInt32 eventNum,
UArg arg, UInt32 payload)
{
Error_Block eb;
struct SystemCfg *stateObj = (struct SystemCfg *)arg;
Log_print4(Diags_ENTRY,
"--> %s: (prodId: %d, eventNum: %d, payload: 0x%x)",
(IArg)FXNN, (IArg)procId, (IArg)eventNum, (IArg)payload);
Error_init(&eb);
switch (Global_EvtMask & payload) {
case Global_EvtCreateDone:
case Global_EvtReady:
case Global_EvtCloseDone:
case Global_EvtDone:
SemThread_post(stateObj->semH, &eb);
if (Error_check(&eb)) {
/* Log_error() */
Log_print3(
Diags_USER8,
"Error: %s, line %d: %s: SemThread_post() returned error",
(IArg)__FILE__, (IArg)__LINE__, (IArg)FXNN);
}
break;
}
Log_print1(Diags_EXIT, "<-- %s:", (IArg)FXNN);
}
/*
* ======== main ========
*/
Int main(Int argc, Char* argv[])
{
Error_Block eb;
Task_Params taskParams;
Log_print0(Diags_INFO, LOGSTR(1));
/* must initialize the error block before using it */
Error_init(&eb);
/* create main thread (interrupts not enabled in main on BIOS) */
Task_Params_init(&taskParams);
taskParams.instance->name = "smain";
taskParams.arg0 = (UArg)argc;
taskParams.arg1 = (UArg)argv;
taskParams.stackSize = 0x700;
Task_create(smain, &taskParams, &eb);
if (Error_check(&eb)) {
System_abort((String)(LOGSTR(13)));
}
/* start scheduler, this never returns */
BIOS_start();
/* should never get here */
return(0);
}
Int main(Int argc, Char* argv[])
{
Error_Block eb;
Task_Params taskParams;
Registry_Result result;
Log_print0(Diags_ENTRY, "--> main:");
/* must initialize the error block before using it */
Error_init(&eb);
/* create main thread (interrupts not enabled in main on BIOS) */
Task_Params_init(&taskParams);
taskParams.instance->name = "smain";
taskParams.stackSize = 0x1000;
Task_create(smain, &taskParams, &eb);
if (Error_check(&eb)) {
System_abort("main: failed to create application startup thread");
}
/* register with xdc.runtime to get a diags mask */
result = Registry_addModule(&Registry_CURDESC, MODULE_NAME);
Assert_isTrue(result == Registry_SUCCESS, (Assert_Id)NULL);
/* start scheduler, this never returns */
BIOS_start();
/* should never get here */
Log_print0(Diags_EXIT, "<-- main:");
return (0);
}
/*
* ======== main ========
*/
Int main(Int argc, Char* argv[])
{
Error_Block eb;
Task_Params taskParams;
Log_print0(Diags_ENTRY, "--> main:");
/* must initialize the error block before using it */
Error_init(&eb);
/* create main thread (interrupts not enabled in main on BIOS) */
Task_Params_init(&taskParams);
taskParams.instance->name = "smain";
taskParams.stackSize = 0x1000;
Task_create(smain, &taskParams, &eb);
if (Error_check(&eb)) {
System_abort("main: failed to create application startup thread");
}
/* start scheduler, this never returns */
BIOS_start();
/* should never get here */
Log_print0(Diags_EXIT, "<-- main:");
return (0);
}
Int main(Int argc, Char* argv[])
{
Error_Block eb;
Task_Params taskParams;
Log_print3(Diags_ENTRY,
"--> %s: (argc: %d, argv: 0x%x)", (IArg)FXNN, (IArg)argc, (IArg)argv);
/* must initialize the error block before using it */
Error_init(&eb);
/* initialize ipc layer */
Ipc_start();
/* create main thread (interrupts not enabled in main on BIOS) */
Task_Params_init(&taskParams);
taskParams.instance->name = "AppMain_main__P";
taskParams.arg0 = (UArg)argc;
taskParams.arg1 = (UArg)argv;
taskParams.stackSize = 0x4000;
Task_create(AppMain_main__P, &taskParams, &eb);
if (Error_check(&eb)) {
System_abort("main() failed to create application startup thread");
}
/* start scheduler, this never returns */
BIOS_start();
/* should never get here */
Log_print1(Diags_EXIT, "<-- %s: should never get here", (IArg)FXNN);
return(0);
}
/*
* ======== hwiPostInit ========
* Function to be called during module startup to complete the
* initialization of any statically created or constructed Hwi.
* returns (0) and clean 'eb' on success
* returns 'eb' *and* 'n' for number of successful createFxn() calls iff
* one of the createFxn() calls fails
*/
static Int hwiPostInit (Hwi_Object *hwi, Error_Block *eb)
{
Int i;
#ifndef ti_sysbios_hal_Hwi_DISABLE_ALL_HOOKS
for (i = 0; i < Hwi_hooks.length; i++) {
hwi->hookEnv[i] = (Ptr)0;
if (Hwi_hooks.elem[i].createFxn != NULL) {
Hwi_hooks.elem[i].createFxn((IHwi_Handle)hwi, eb);
if (Error_check(eb)) {
return (i);
}
}
}
#endif
Hwi_setPriority(hwi->intNum, hwi->priority);
Hwi_setType(hwi->intNum, hwi->type);
if (hwi->type == Hwi_Type_IRQ) {
Hwi_plug(hwi->intNum, Hwi_dispatchIRQ);
}
else {
Hwi_plug(hwi->intNum, (Hwi_PlugFuncPtr)(hwi->fxn));
}
return (0);
}
void Kinectv2::UseCoordinate()
{
hResult = pSensor->get_CoordinateMapper(&pCoordinateMapper);
if(FAILED(hResult)){
Error_check("get_CoordinateMapper");
}
}
Int SystemCfg_closeSharedResources(SystemCfg_AppFxn appShutdownFxn, Ptr arg)
{
Error_Block eb;
Int status = 0;
struct SystemCfg *stateObj = &SystemCfg_State;
Log_print1(Diags_ENTRY, "--> %s: ()", (IArg)FXNN);
Error_init(&eb);
/* invoke the application shutdown function */
if (appShutdownFxn != NULL) {
status = appShutdownFxn(arg);
if (status < 0) {
goto leave;
}
}
/* close shared resources from remote core */
/* <add code here> */
/* send close done event to remote core */
Log_print0(Diags_USER1, FXNN": send EvtCloseDone to remote core");
status = Notify_sendEvent(stateObj->hostProcId, Global_NotifyLineId,
Global_HostDspEvtNum, Global_EvtCloseDone, TRUE);
if (status < 0) {
/* Log_error() */
Log_print4(Diags_USER8,
"Error: %s, line %d: %s: Notify_sendEvent() returned error %d",
(IArg)__FILE__, (IArg)__LINE__, (IArg)FXNN, (IArg)status);
goto leave;
}
/* wait for close event from remote core */
Log_print0(Diags_USER1, FXNN": waiting for EvtCloseDone event...");
SemThread_pend(stateObj->semH, SemThread_FOREVER, &eb);
if (Error_check(&eb)) {
/* Log_error() */
Log_print3(Diags_USER8,
"Error: %s, line %d: %s: SemThread_pend() returned with error",
(IArg)__FILE__, (IArg)__LINE__, (IArg)FXNN);
status = -1;
goto leave;
}
Log_print0(Diags_USER1, FXNN": ...received EvtCloseDone event");
leave:
Log_print2(Diags_EXIT, "<-- %s: %d", (IArg)FXNN, (IArg)status);
return(status);
}
/*
* ======== ThreadSupport_Instance_init ========
*/
Int ThreadSupport_Instance_init(ThreadSupport_Handle obj,
ThreadSupport_RunFxn fxn, const ThreadSupport_Params* params,
Error_Block* eb)
{
Task_Params tpars;
Semaphore_Handle bios6sem;
bios6sem = ThreadSupport_Instance_State_join_sem(obj);
Task_Params_init(&tpars);
tpars.arg0 = (UArg)obj;
if (params->stackSize != 0) {
tpars.stackSize = params->stackSize;
}
tpars.env = obj;
tpars.instance->name = params->instance->name;
if (params->osPriority != ThreadSupport_INVALID_OS_PRIORITY) {
tpars.priority = params->osPriority;
}
else {
if (params->priority == ThreadSupport_Priority_LOWEST) {
tpars.priority = ThreadSupport_lowestPriority;
}
else if (params->priority == ThreadSupport_Priority_BELOW_NORMAL) {
tpars.priority = ThreadSupport_belowNormalPriority;
}
else if (params->priority == ThreadSupport_Priority_NORMAL) {
tpars.priority = ThreadSupport_normalPriority;
}
else if (params->priority == ThreadSupport_Priority_ABOVE_NORMAL) {
tpars.priority = ThreadSupport_aboveNormalPriority;
}
else if (params->priority == ThreadSupport_Priority_HIGHEST) {
tpars.priority = ThreadSupport_highestPriority;
}
else {
Error_raise(eb, ThreadSupport_E_priority, params->priority, 0);
return (ThreadSupport_PRI_FAILURE);
}
}
obj->tls = params->tls;
obj->startFxn = fxn;
obj->startFxnArg = params->arg;
Semaphore_construct(Semaphore_struct(bios6sem), 0, NULL);
obj->task = Task_create(&ThreadSupport_runStub, &tpars, eb);
if (Error_check(eb)) {
return (ThreadSupport_TASK_FAILURE);
}
return (0);
}
void Kinectv2::UseColorImage()
{
hResult = pSensor->get_ColorFrameSource(&pColorSource);
if(FAILED(hResult)){
Error_check("get_ColorFrameSource");
}
hResult = pColorSource->OpenReader(&pColorReader);
if(FAILED(hResult)){
Error_check("OpenReader");
}
hResult = pColorSource->get_FrameDescription(&pColorDescription);
if(FAILED(hResult)) {
Error_check("get_FrameDescription");
}
pColorDescription->get_Width(&colorWidth);
pColorDescription->get_Height(&colorHeight);
colorBuffer.resize(colorHeight * colorWidth);
}
Kinectv2::Kinectv2()
{
hResult = S_OK;
hResult = GetDefaultKinectSensor(&pSensor);
if(FAILED(hResult)){
Error_check("GetDefaultKinectSensor");
}
hResult = pSensor->Open();
if(FAILED(hResult)){
Error_check("Open()");
}
colorWidth = 0;
colorHeight = 0;
depthHeight = 0;
depthWidth = 0;
pColorFrame = nullptr;
pDepthFrame = nullptr;
}
/*
* ======== canFail1 ========
*/
Int canFail1(Error_Block *eb, UInt val)
{
canFail2(eb, val);
if (Error_check(eb)) {
return (-1);
}
else {
return (val * 10);
}
}
/*
* ======== main ========
*/
Int main()
{
Task_Params taskParams;
Error_Block eb;
Memory_Stats stats;
Error_init(&eb);
/* Picking a stackSize such that the second Task_create() will fail */
Memory_getStats(Memory_defaultHeapInstance, &stats);
Task_Params_init(&taskParams);
taskParams.priority = 1;
taskParams.stackSize = (stats.totalFreeSize/2) + 64;
/*
* Create two tasks, The first one succeeds and the second one fails
* We catch the second failure in the Error_Block
*/
tsk1 = Task_create(task1, &taskParams, &eb);
if (Error_check(&eb)) {
/* Should not get here */
System_printf("First Task_create() failed\n");
BIOS_exit(0);
}
Error_init(&eb);
tsk2 = Task_create(task2, &taskParams, &eb);
if (Error_check(&eb)) {
/* Should get here */
System_printf("Second Task_create() failed\n");
}
BIOS_start(); /* does not return */
return(0);
}
/*
* ======== Swi_Instance_init ========
*/
Int Swi_Instance_init(Swi_Object *swi, Swi_FuncPtr fxn,
const Swi_Params *params, Error_Block *eb)
{
Int status;
Assert_isTrue((BIOS_swiEnabled == TRUE), Swi_A_swiDisabled);
Queue_elemClear(&swi->qElem);
swi->fxn = fxn;
swi->arg0 = params->arg0;
swi->arg1 = params->arg1;
if (params->priority == ~0) {
swi->priority = Swi_numPriorities - 1;
}
else {
swi->priority = params->priority;
}
Assert_isTrue((swi->priority < Swi_numPriorities),
Swi_A_badPriority);
swi->mask = 1 << swi->priority;
swi->posted = FALSE;
swi->initTrigger = swi->trigger = params->trigger;
swi->readyQ = Queue_Object_get(Swi_module->readyQ, swi->priority);
#ifndef ti_sysbios_knl_Swi_DISABLE_ALL_HOOKS
if (Swi_hooks.length > 0) {
swi->hookEnv = Memory_alloc(Swi_Object_heap(),
Swi_hooks.length * sizeof(Ptr), 0, eb);
if (swi->hookEnv == NULL) {
return (1); /* see 2 below */
}
}
#endif
status = Swi_postInit(swi, eb);
/*
* floor of 2 here is to differentiate Swi_postInit errors
* from Instance_init errors
*/
if (Error_check(eb)) {
return (2 + status);
}
return (0);
}
void Kinectv2::UseDepthImage()
{
hResult = pSensor->get_DepthFrameSource(&pDepthSource);
if(FAILED(hResult)){
Error_check("get_DepthFrameSource");
}
hResult = pDepthSource->OpenReader(&pDepthReader);
if(FAILED(hResult)){
Error_check("OpenReader");
}
hResult = pDepthSource->get_FrameDescription(&pDepthDescription);
if(FAILED(hResult)){
Error_check("get_FrameDescription");
}
pDepthDescription->get_Width(&depthWidth);
pDepthDescription->get_Height(&depthHeight);
depthBuffer.resize(depthHeight * depthWidth);
depths.resize(SAVE_SIZE);
for(int i = 0;i < SAVE_SIZE;i++)
depths.at(i).resize(depthHeight * depthWidth);
time = 0;
}
/*
* ======== Stream_postInit ========
*/
Int Stream_postInit(Stream_Object *obj, Error_Block *eb)
{
List_Handle freeList;
UInt i, len;
IDriver_Handle drvHandle;
DriverAdapter_Params adapPrms;
freeList = Stream_Instance_State_freeList(obj);
/*
* Split the buffer into packets and add to freeList
*/
for (i = 0; i < obj->maxIssues; i++) {
List_put(freeList, (List_Elem *)&obj->packets[i]);
}
len = Stream_match(obj->name, &obj->convHandle, eb);
if (len == 0) {
/* Look in DriverTable */
obj->drvAdapHdl = TRUE;
len = DriverTable_match(obj->name, &drvHandle, eb);
if (len == 0) {
/* The name was not found */
Error_raise(eb, Stream_E_notFound, obj->name, 0);
return (3);
}
else {
DriverAdapter_Params_init(&adapPrms);
adapPrms.driverHandle = drvHandle;
obj->convHandle = DriverAdapter_Handle_upCast(
DriverAdapter_create(&adapPrms, eb));
if (obj->convHandle == NULL) {
return (4);
}
}
}
else {
obj->drvAdapHdl = FALSE;
}
IConverter_open(obj->convHandle, ((obj->name) + len), obj->mode,
obj->chanParams, Stream_internalCallback, (UArg)obj, eb);
if (Error_check(eb)) {
return (5);
}
return (0);
}
int InitMutex(wolfSSL_Mutex* m)
{
Semaphore_Params params;
Error_Block eb;
Error_init(&eb);
Semaphore_Params_init(¶ms);
params.mode = Semaphore_Mode_BINARY;
*m = Semaphore_create(1, ¶ms, &eb);
if( Error_check( &eb ) )
{
Error_raise( &eb, Error_E_generic, "Failed to Create the semaphore.",NULL);
} else return 0;
}
/*
* ======== Memory_alloc ========
* If eb has an error already set, we preserve the error if we can. We
* could do better by creating a separate error block for the proxy
* allocation, but this would cost more than it's worth; the normal case
* would pay a constant time overhead for the rare case of allocation
* failing when eb already has an error set.
*/
Ptr Memory_alloc(IHeap_Handle heap, SizeT size, SizeT align, Error_Block *eb)
{
Ptr block;
Bool prior = Error_check(eb);
/* if align == 0, use default alignment */
if (align == 0) {
align = Memory_getMaxDefaultTypeAlign();
}
/* allocate using a non-NULL appropriate heap */
block = Memory_HeapProxy_alloc(heap ? heap : Memory_defaultHeapInstance,
size, align, eb);
/* if the allocator returned NULL and either
* the error was already set or
* it didn't set the error
*/
if (block == NULL && (prior || !Error_check(eb))) {
Error_raise(eb, Error_E_memory, (IArg)heap, (IArg)size);
}
return (block);
}
/*
* ======== Power_registerConstraint ========
* Register an operational constraint with Power.
*
*/
Power_Status Power_registerConstraint(Power_Constraint type, UArg value,
Power_ConstraintHandle *handle)
{
Power_Status status = Power_SOK;
Power_ConstraintObj * pConstraint;
Error_Block eb;
/* check for NULL handle pointer */
if (handle == NULL) {
status = Power_EINVALIDPOINTER;
}
/* else, check for invalid constraint type */
else if ((type < Power_DISALLOWED_CPU_SETPOINT_MASK) ||
(type > Power_DISALLOWEDSLEEPSTATE_MASK)) {
status = Power_EINVALIDVALUE;
}
/* else, allocate a constraint object */
else {
Error_init(&eb);
pConstraint = Memory_calloc(Power_Object_heap(),
sizeof(Power_ConstraintObj), 0, &eb);
if ((pConstraint == NULL) || Error_check(&eb)) {
status = Power_EFAIL;
}
/* fill in and enqueue the constraint, update aggregate constraints */
else {
/* fill in constraint object elements */
pConstraint->type = type;
pConstraint->value = value;
/* place constraint object on the constraints queue */
Queue_put(Power_Module_State_constraintsQ(),
(Queue_Elem*) pConstraint);
/* update aggregated constraints with the new constraint */
Power_updateConstraints(type, value);
/* set out parameters */
*handle = (UArg *) pConstraint;
}
}
return (status);
}
/*
* ======== Power_registerNotify ========
* Register a function to be called on a specific power event.
*
*/
Power_Status Power_registerNotify(Power_Event eventType, UInt eventMask,
Fxn notifyFxn, UArg clientArg, Power_NotifyHandle * notifyHandle,
Fxn *delayedCompletionFxn)
{
Power_NotifyObj * pNotify;
Queue_Handle notifyQ;
Error_Block eb;
/* check for out of range event type */
if ((eventType < 0) || (eventType >= Power_INVALIDEVENT)) {
return (Power_EINVALIDEVENT);
}
/* check for NULL pointers for notifyFxn and out params */
if ((notifyFxn == NULL) || (notifyHandle == NULL) ||
(delayedCompletionFxn == NULL)) {
return (Power_EINVALIDPOINTER);
}
/* allocate a notification object */
Error_init(&eb);
pNotify = Memory_calloc(Power_Object_heap(), sizeof(Power_NotifyObj), 0,
&eb);
if ((pNotify == NULL) || Error_check(&eb)) {
return (Power_EFAIL);
}
/* fill in notify object elements */
pNotify->eventType = eventType;
pNotify->notifyFxn = notifyFxn;
pNotify->clientArg = clientArg;
pNotify->eventMask = eventMask;
/* determine the appropriate notification queue */
notifyQ = (Queue_Handle)((UInt8 *)(Power_module->notifyQ) +
(UInt)(eventType * (2 * sizeof(Ptr))));
/* place notify object on appropriate event queue */
Queue_put(notifyQ, (Queue_Elem*) pNotify);
/* set out parameters */
*notifyHandle = pNotify;
*delayedCompletionFxn =
(Fxn) ti_sysbios_family_c674_Power_delayCompletionFxns[eventType];
return(Power_SOK);
}
/*
* ======== Hwi_postInit ========
* Function to be called during module startup to complete the
* initialization of any statically created or constructed Hwi.
* returns (0) and clean 'eb' on success
* returns 'eb' *and* 'n' for number of successful createFxn() calls iff
* one of the createFxn() calls fails
*/
Int Hwi_postInit (Hwi_Object *hwi, Error_Block *eb)
{
#ifndef ti_sysbios_hal_Hwi_DISABLE_ALL_HOOKS
Int i;
for (i = 0; i < Hwi_hooks.length; i++) {
hwi->hookEnv[i] = (Ptr)0;
if (Hwi_hooks.elem[i].createFxn != NULL) {
Hwi_hooks.elem[i].createFxn((IHwi_Handle)hwi, eb);
if (Error_check(eb)) {
return (i);
}
}
}
#endif
return (0);
}
