commit 6ad11fb0332e3a405ecf7fb8da50e67bfe60cd45
Author: Nikhil Dasan <n-dasan@ti.com>
Date:   Wed Jul 19 11:10:36 2023 +0530

    Additional Sensor Config
    
    Signed-off-by: Nikhil Dasan <n-dasan@ti.com>

diff --git a/sensor_drv/src/iss_sensors.c b/sensor_drv/src/iss_sensors.c
index 093bc6c..257cb66 100755
--- a/sensor_drv/src/iss_sensors.c
+++ b/sensor_drv/src/iss_sensors.c
@@ -61,6 +61,8 @@
  */
 #include <iss_sensors.h>
 #include <iss_sensor_if.h>
+#include <ti/osal/TaskP.h>
+#include <ti/osal/SemaphoreP.h>
 #include <iss_sensor_priv.h>
 #include <app_remote_service.h>
 #include <app_ipc.h>
@@ -77,6 +79,13 @@
 #include <ti/board/src/j784s4_evm/include/board_i2c_io_exp.h>
 #endif
 
+#define APP_SAMPLE_RX_TASK_STACK_SIZE   (128*1024u)
+#define APP_SAMPLE_RX_TASK_PRI          (3u)
+static uint8_t g_app_sample_stack[APP_SAMPLE_RX_TASK_STACK_SIZE]
+__attribute__ ((section(".bss:taskStackSection")))
+__attribute__ ((aligned(8192)))
+    ;
+
 /**
  * @{
  * I2C Addresses for serialisers/Sensors attached to the UB960
@@ -119,8 +128,19 @@
  *  Globals
  *******************************************************************************
  */
+
+typedef struct
+{
+    uint32_t sensor_features_requested;
+    uint32_t channel_mask;
+    char * sensor_name;
+    SemaphoreP_Handle glock;
+} IssSensors_conf;
+
 IssSensors_Handle *gIssSensorTable[ISS_SENSORS_MAX_SUPPORTED_SENSOR];
 
+IssSensors_conf gIssObj;
+
 #define DS90UB9702
 
 /* Fusion2 board detect flag */
@@ -601,11 +621,90 @@ static int32_t setupI2CInst(uint8_t i2cInst)
     return status;
 }
 
+static void appSampleRxTaskMain(void *arg0, void *arg1)
+{
+    vx_status status = -1;
+    IssSensors_conf *obj = &gIssObj;
+    uint8_t chId;
+    while(1)
+    {
+        issLogPrintf("WAITING\n");
+        IssSensors_Handle * pSenHndl = NULL;
+        SemaphoreP_pend(obj->glock, SemaphoreP_WAIT_FOREVER);
+        issLogPrintf("ImageSensor_RemoteServiceHandler: IM_SENSOR_CMD_CONFIG Individual \n");
+        pSenHndl = getSensorHandleFromName(obj->sensor_name);
+        if(NULL == pSenHndl)
+        {
+            status = -1;
+            printf("ERROR : NULL handle returned for sensor %s \n", obj->sensor_name);
+        }
+        else
+        {
+            status = 0;
+            int32_t probeStatus;
+            //channel_mask_supported = (1<<pSenHndl->createPrms->num_channels) - 1;
+            //channel_mask &= channel_mask_supported;
+            if(1U == pSenHndl->sensorIntfPrms->sensorBroadcast)
+            {
+                if((obj->channel_mask & 0x0F) == 0x0F)
+                {
+                    issLogPrintf("Configuring all cameras on UB960_0 in broadcast mode \n");
+                    status |= enableUB960Broadcast(0);
+                    status |= IssSensor_Config((void*)pSenHndl, 0, obj->sensor_features_requested);
+                    obj->channel_mask &= 0xF0;
+                }
+
+                if((obj->channel_mask & 0xF0) == 0xF0)
+                {
+                    issLogPrintf("Configuring all cameras on UB960_1 in broadcast mode \n");
+                    status |= enableUB960Broadcast(1);
+                    status |= IssSensor_Config((void*)pSenHndl, 4, obj->sensor_features_requested);
+                    obj->channel_mask &= 0x0F;
+                }
+            }
+            else
+            {
+                issLogPrintf("Disabling broadcast mode \n");
+                disableUB960Broadcast(0);
+                disableUB960Broadcast(1);
+                disableUB960Broadcast(2);
+            }
+
+            issLogPrintf("IM_SENSOR_CMD_CONFIG channel_mask = %x\n", obj->channel_mask);
+
+            if(0 != obj->channel_mask)
+            {
+                chId = 0;
+                while( (obj->channel_mask > 0) && (chId < ISS_SENSORS_MAX_CHANNEL) )
+                {
+                    if((obj->channel_mask & 0x1) == 0x1)
+                    {
+                        probeStatus = pSenHndl->sensorFxns->probe(chId, pSenHndl);
+                        if(probeStatus < 0)
+                        {
+                            printf("Error : sensor probe failed for channel %d \n ", chId);
+                            status |= probeStatus;
+                         }
+                         else
+                         {
+                            issLogPrintf("Configuring camera # %d \n", chId);
+                            status |= IssSensor_Config((void*)pSenHndl, chId, obj->sensor_features_requested);
+                         }
+                    }
+                    chId++;
+                    obj->channel_mask = obj->channel_mask >> 1;
+                }
+            }
+        }
+        printf("IM_SENSOR_CMD_CONFIG returning status = %d\n", status);
+    }
+}
 
 int32_t IssSensor_Init()
 {
     int32_t status = -1;
     uint32_t cnt;
+    IssSensors_conf *obj = &gIssObj;
 
     for(cnt=0;cnt<ISS_SENSORS_MAX_SUPPORTED_SENSOR;cnt++)
     {
@@ -623,6 +722,41 @@ int32_t IssSensor_Init()
            return status;
     }
 
+    SemaphoreP_Params semParams;
+    SemaphoreP_Params_init(&semParams);
+    semParams.mode = SemaphoreP_Mode_BINARY;
+    obj->glock = SemaphoreP_create(0U, &semParams);
+    if(obj->glock==NULL)
+    {
+        appLogPrintf("SAMPLE: Unable to create tx semaphore\n");
+        status = -1;
+    }
+
+    TaskP_Params task_prms;
+    char task_name[12];
+    TaskP_Handle task_handle;
+
+    TaskP_Params_init(&task_prms);
+
+    task_prms.stacksize = APP_SAMPLE_RX_TASK_STACK_SIZE;
+    task_prms.stack = g_app_sample_stack;
+    task_prms.priority = APP_SAMPLE_RX_TASK_PRI;
+    task_prms.arg0 = NULL;
+    task_prms.arg1 = NULL;
+    task_prms.name = (const char*)&task_name[0];
+
+    strncpy(task_name, "CHECK", 12);
+    task_name[12-1] = 0;
+
+    task_handle = (void*)TaskP_create(
+                            &appSampleRxTaskMain,
+                            &task_prms);
+    if(task_handle==NULL)
+    {
+        appLogPrintf("SAMPLE: ERROR: Unable to create RX task \n");
+        status = -1;
+    }
+
     for (cnt = 0U; cnt < ISS_SENSORS_MAX_SUPPORTED_SENSOR; cnt ++)
     {
         gIssSensorTable[cnt] = NULL;
@@ -2152,70 +2286,84 @@ int32_t ImageSensor_RemoteServiceHandler(char *service_name, uint32_t cmd,
             memcpy(&sensor_features_requested, (cmd_param+ISS_SENSORS_MAX_NAME), sizeof(uint32_t));
             issLogPrintf("Application requested features = 0x%x \n ", sensor_features_requested);
             memcpy(&channel_mask, (cmd_param+ISS_SENSORS_MAX_NAME+sizeof(uint32_t)), sizeof(uint32_t));
-            pSenHndl = getSensorHandleFromName(sensor_name);
-            if(NULL == pSenHndl)
+            if(channel_mask == 0x2)
             {
-                status = -1;
-                printf("ERROR : NULL handle returned for sensor %s \n", sensor_name);
+                IssSensors_conf *obj = &gIssObj;
+                obj->sensor_name = sensor_name;
+                obj->sensor_features_requested = sensor_features_requested;
+                obj->channel_mask = channel_mask;
+                SemaphoreP_post(obj->glock);
+                issLogPrintf("Posted \n");
+                status = 0;
             }
             else
             {
-                status = 0;
-                int32_t probeStatus;
-                //channel_mask_supported = (1<<pSenHndl->createPrms->num_channels) - 1;
-                //channel_mask &= channel_mask_supported;
-                if(1U == pSenHndl->sensorIntfPrms->sensorBroadcast)
+                pSenHndl = getSensorHandleFromName(sensor_name);
+                if(NULL == pSenHndl)
                 {
-                    if((channel_mask & 0x0F) == 0x0F)
+                    status = -1;
+                    printf("ERROR : NULL handle returned for sensor %s \n", sensor_name);
+                }
+                else
+                {
+                    status = 0;
+                    int32_t probeStatus;
+                    //channel_mask_supported = (1<<pSenHndl->createPrms->num_channels) - 1;
+                    //channel_mask &= channel_mask_supported;
+                    if(1U == pSenHndl->sensorIntfPrms->sensorBroadcast)
                     {
-                        issLogPrintf("Configuring all cameras on UB960_0 in broadcast mode \n");
-                        status |= enableUB960Broadcast(0);
-                        status |= IssSensor_Config((void*)pSenHndl, 0, sensor_features_requested);
-                        channel_mask &= 0xF0;
-                    }
+                        if((channel_mask & 0x0F) == 0x0F)
+                        {
+                            issLogPrintf("Configuring all cameras on UB960_0 in broadcast mode \n");
+                            status |= enableUB960Broadcast(0);
+                            status |= IssSensor_Config((void*)pSenHndl, 0, sensor_features_requested);
+                            channel_mask &= 0xF0;
+                        }
 
-                    if((channel_mask & 0xF0) == 0xF0)
+                        if((channel_mask & 0xF0) == 0xF0)
+                        {
+                            issLogPrintf("Configuring all cameras on UB960_1 in broadcast mode \n");
+                            status |= enableUB960Broadcast(1);
+                            status |= IssSensor_Config((void*)pSenHndl, 4, sensor_features_requested);
+                            channel_mask &= 0x0F;
+                        }
+                    }
+                    else
                     {
-                        issLogPrintf("Configuring all cameras on UB960_1 in broadcast mode \n");
-                        status |= enableUB960Broadcast(1);
-                        status |= IssSensor_Config((void*)pSenHndl, 4, sensor_features_requested);
-                        channel_mask &= 0x0F;
+                        issLogPrintf("Disabling broadcast mode \n");
+                        disableUB960Broadcast(0);
+                        disableUB960Broadcast(1);
+                        disableUB960Broadcast(2);
                     }
-                }else
-                {
-                    issLogPrintf("Disabling broadcast mode \n");
-                    disableUB960Broadcast(0);
-                    disableUB960Broadcast(1);
-                    disableUB960Broadcast(2);
-                }
 
-                issLogPrintf("IM_SENSOR_CMD_CONFIG channel_mask = %x\n", channel_mask);
+                    issLogPrintf("IM_SENSOR_CMD_CONFIG channel_mask = %x\n", channel_mask);
 
-                if(0 != channel_mask)
-                {
-                    chId = 0;
-                    while( (channel_mask > 0) && (chId < ISS_SENSORS_MAX_CHANNEL) )
+                    if(0 != channel_mask)
                     {
-                        if((channel_mask & 0x1) == 0x1)
+                        chId = 0;
+                        while( (channel_mask > 0) && (chId < ISS_SENSORS_MAX_CHANNEL) )
                         {
-                            probeStatus = pSenHndl->sensorFxns->probe(chId, pSenHndl);
-                            if(probeStatus < 0)
+                            if((channel_mask & 0x1) == 0x1)
                             {
-                                printf("Error : sensor probe failed for channel %d \n ", chId);
-                                status |= probeStatus;
-                             }
-                             else
-                             {
-                                issLogPrintf("Configuring camera # %d \n", chId);
-                                status |= IssSensor_Config((void*)pSenHndl, chId, sensor_features_requested);
-                             }
+                                probeStatus = pSenHndl->sensorFxns->probe(chId, pSenHndl);
+                                if(probeStatus < 0)
+                                {
+                                    printf("Error : sensor probe failed for channel %d \n ", chId);
+                                    status |= probeStatus;
+                                 }
+                                 else
+                                 {
+                                    issLogPrintf("Configuring camera # %d \n", chId);
+                                    status |= IssSensor_Config((void*)pSenHndl, chId, sensor_features_requested);
+                                 }
+                            }
+                            chId++;
+                            channel_mask = channel_mask >> 1;
                         }
-                        chId++;
-                        channel_mask = channel_mask >> 1;
                     }
                 }
+                issLogPrintf("IM_SENSOR_CMD_CONFIG returning status = %d\n", status);
             }
-            issLogPrintf("IM_SENSOR_CMD_CONFIG returning status = %d\n", status);
 
             break;
         case IM_SENSOR_CMD_STREAM_ON:
