git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Api/vp_api_supervisor.c

   1 /**
   2  *  @file     vp_api_supervisor.c
   3  *  @brief    VP Api. Pipeline supervisor
   4  */
   5
   6
   7 ///////////////////////////////////////////////
   8 // INCLUDES
   9
  10 #include <VP_Api/vp_api_supervisor.h>
  11 #include <VP_Api/vp_api_config.h>
  12 #include <VP_Api/vp_api.h>
  13 #include <VP_Os/vp_os_print.h>
  14 #include <VP_Os/vp_os_malloc.h>
  15 #include <VP_Os/vp_os_assert.h>
  16
  17
  18 ///////////////////////////////////////////////
  19 // STATICS
  20
  21 /** Global array containing pipeline handles.
  22  */
  23 static PIPELINE_ADDRESS pipelines[VP_API_MAX_NUM_PIPELINES] = {(PIPELINE_ADDRESS)NULL};
  24
  25
  26 /**
  27  *  @fn      vp_api_get_message(vp_api_io_pipeline_t *, DEST_HANDLE *, PIPELINE_MSG *, void *, void *)
  28  *  @brief   Get a message.
  29  *
  30  *  Get a message from the pipeline fifo. Internally called by vp_api_handle_messages()
  31  *  @param   pipeline   Pipeline definition
  32  *  @param   dest       Message destination
  33  *  @param   msg_id     Message identifier
  34  *  @param   callback   Optional callback function called after processing of the message
  35  *  @param   param      Optional message parameters
  36  *  @return  C_RESULT : VP_SUCCESS
  37  *  @author  Julien Floret <julien.floret.ext\@parrot.com>
  38  *  @date    28/05/2007
  39  */
  40 static C_RESULT
  41 vp_api_get_message(vp_api_io_pipeline_t *pipeline, DEST_HANDLE *dest, PIPELINE_MSG *msg_id, void **callback, void **param);
  42
  43
  44 ///////////////////////////////////////////////
  45 // CODE
  46
  47 static PIPELINE_HANDLE nb_pipelines = 0;
  48
  49
  50 C_RESULT vp_api_add_pipeline(vp_api_io_pipeline_t *pipeline, PIPELINE_HANDLE *handle)
  51 {
  52   C_RESULT res = VP_SUCCESS;
  53   int i = 0;
  54
  55   VP_OS_ASSERT(nb_pipelines < VP_API_MAX_NUM_PIPELINES);
  56
  57   while(pipelines[i] != ((PIPELINE_ADDRESS)NULL))
  58     {
  59       i++;
  60     }
  61
  62   pipelines[i] = (PIPELINE_ADDRESS) pipeline;
  63   (*handle) = i;
  64   nb_pipelines ++;
  65
  66   return res;
  67 }
  68
  69
  70 C_RESULT vp_api_remove_pipeline(vp_api_io_pipeline_t *pipeline, PIPELINE_HANDLE *handle)
  71 {
  72   C_RESULT res = VP_SUCCESS;
  73
  74   pipelines[*handle] = (PIPELINE_ADDRESS) NULL;
  75   nb_pipelines --;
  76
  77   return res;
  78 }
  79
  80
  81 vp_api_io_pipeline_t * vp_api_get_pipeline(PIPELINE_HANDLE handle)
  82 {
  83   vp_api_io_pipeline_t *pipeline = (vp_api_io_pipeline_t *) pipelines[handle];
  84   return (pipeline);
  85 }
  86
  87
  88 C_RESULT vp_api_post_message(DEST_HANDLE dest, PIPELINE_MSG msg_id, void *callback, void *param)
  89 {
  90   C_RESULT res = VP_SUCCESS;
  91
  92   vp_api_io_pipeline_t *pipeline = (vp_api_io_pipeline_t *) pipelines[dest.pipeline];
  93
  94   /* Do not send the message if the pipeline does not exist yet
  95         This happens when calling the callback function of the 'video_channel' configuration
  96     value at drone startup. */
  97   if (pipeline==NULL) { return VP_FAILURE; }
  98
  99   VP_OS_ASSERT(pipeline->fifo.nb_waiting >= 0);
 100
 101   vp_os_mutex_lock(&pipeline->fifo.mutex);
 102
 103   if((pipeline->fifo.ppost + sizeof(DEST_HANDLE) + sizeof(PIPELINE_MSG) + sizeof(void *) + sizeof(void *)) >= (pipeline->fifo.pbase + VP_API_PIPELINE_FIFO_SIZE))
 104     pipeline->fifo.ppost = pipeline->fifo.pbase;
 105
 106   vp_os_memcpy(pipeline->fifo.ppost, &dest, sizeof(DEST_HANDLE));
 107   pipeline->fifo.ppost += sizeof(DEST_HANDLE);
 108
 109   vp_os_memcpy(pipeline->fifo.ppost, &msg_id, sizeof(PIPELINE_MSG));
 110   pipeline->fifo.ppost += sizeof(PIPELINE_MSG);
 111
 112   if(callback != NULL)
 113     vp_os_memcpy(pipeline->fifo.ppost, &callback, sizeof(void *));
 114   else
 115     vp_os_memset(pipeline->fifo.ppost, 0, sizeof(void *));
 116   pipeline->fifo.ppost += sizeof(void *);
 117
 118   if(param != NULL)
 119     vp_os_memcpy(pipeline->fifo.ppost, &param, sizeof(void *));
 120
 121   else
 122     vp_os_memset(pipeline->fifo.ppost, 0, sizeof(void *));
 123   pipeline->fifo.ppost += sizeof(void *);
 124
 125   pipeline->fifo.nb_waiting ++;
 126
 127   vp_os_mutex_unlock(&pipeline->fifo.mutex);
 128
 129   return res;
 130 }
 131
 132
 133 static C_RESULT vp_api_get_message(vp_api_io_pipeline_t *pipeline, DEST_HANDLE *dest, PIPELINE_MSG *msg_id, void **callback, void **param)
 134 {
 135   C_RESULT res = VP_SUCCESS;
 136
 137   VP_OS_ASSERT(pipeline->fifo.nb_waiting > 0);
 138
 139   vp_os_mutex_lock(&pipeline->fifo.mutex);
 140
 141   if((pipeline->fifo.pget + sizeof(PIPELINE_MSG) + sizeof(void *) + sizeof(void *)) >= (pipeline->fifo.pbase + VP_API_PIPELINE_FIFO_SIZE))
 142     pipeline->fifo.pget = pipeline->fifo.pbase;
 143
 144   if(dest != NULL)
 145     vp_os_memcpy(dest, pipeline->fifo.pget, sizeof(DEST_HANDLE));
 146   pipeline->fifo.pget += sizeof(DEST_HANDLE);
 147
 148   if(msg_id != NULL)
 149     vp_os_memcpy(msg_id, pipeline->fifo.pget, sizeof(PIPELINE_MSG));
 150   pipeline->fifo.pget += sizeof(PIPELINE_MSG);
 151
 152   if(callback != NULL)
 153     vp_os_memcpy(callback, pipeline->fifo.pget, sizeof(void *));
 154   pipeline->fifo.pget += sizeof(void *);
 155
 156   if(param != NULL)
 157     vp_os_memcpy(param, pipeline->fifo.pget, sizeof(void *));
 158   pipeline->fifo.pget += sizeof(void *);
 159
 160   pipeline->fifo.nb_waiting --;
 161
 162   vp_os_mutex_unlock(&pipeline->fifo.mutex);
 163
 164   return res;
 165 }
 166
 167
 168 C_RESULT vp_api_handle_messages(vp_api_io_pipeline_t *pipeline)
 169 {
 170   C_RESULT res = VP_SUCCESS;
 171
 172   DEST_HANDLE dest;
 173   PIPELINE_MSG msg_id;
 174   void *callback = NULL;
 175   void *param = NULL;
 176   uint32_t i;
 177
 178   while(pipeline->fifo.nb_waiting > 0)
 179   {
 180     if(VP_FAILED(vp_api_get_message(pipeline, &dest, &msg_id, &callback, &param)))
 181       res = VP_FAILURE;
 182     else
 183     {
 184       if(dest.stage == VP_API_DEST_PIPELINE_LEVEL)
 185       {
 186         pipeline->handle_msg(pipeline, msg_id, callback, param);
 187       }
 188       else if(dest.stage == VP_API_DEST_STAGE_BROADCAST)
 189       {
 190         for(i=0; i < pipeline->nb_stages; i++)
 191         {
 192           pipeline->stages[i].funcs.handle_msg(pipeline->stages[i].cfg, msg_id, callback, param);
 193         }
 194       }
 195       else if((dest.stage >=0) && (dest.stage < (int16_t)pipeline->nb_stages))
 196       {
 197         if (pipeline->stages[dest.stage].funcs.handle_msg != NULL)
 198           pipeline->stages[dest.stage].funcs.handle_msg(pipeline->stages[dest.stage].cfg, msg_id, callback, param);
 199       }
 200     }
 201   }
 202   return res;
 203 }