vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Stages/vp_stages_io_com.c

   1 /**
   2  *  \brief    VP Stages. Com stage declaration
   3  *  \author   Sylvain Gaeremynck <sylvain.gaeremynck@parrot.fr>
   4  *  \author   Aurelien Morelle <aurelien.morelle@parrot.fr>
   5  *  \author   Thomas Landais <thomas.landais@parrot.fr>
   6  *  \version  2.0
   7  *  \date     first release 16/03/2007
   8  *  \date     modification  19/03/2007
   9  */
  10
  11 #include <VP_Stages/vp_stages_io_com.h>
  12 #include <VP_Api/vp_api_error.h>
  13 #include <VP_Os/vp_os_print.h>
  14 #include <VP_Os/vp_os_malloc.h>
  15 #include <VP_Os/vp_os_delay.h>
  16
  17 C_RESULT
  18 vp_stages_input_com_stage_open(vp_stages_input_com_config_t *cfg)
  19 {
  20   C_RESULT res;
  21
  22   res = vp_com_init(cfg->com);
  23
  24   if( VP_SUCCEEDED(res) )
  25   {
  26     vp_com_local_config(cfg->com, cfg->config);
  27
  28     if(cfg->connection && !cfg->connection->is_up)
  29     {
  30       res = vp_com_connect(cfg->com, cfg->connection, 1);
  31     }
  32   }
  33
  34   if( VP_SUCCEEDED(res) )
  35     res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, 0);
  36
  37   if( VP_SUCCEEDED(res) )
  38   {
  39     if(cfg->socket.type == VP_COM_SERVER)
  40     {
  41       res = vp_com_wait_connections(cfg->com, &cfg->socket, &cfg->socket_client, 1);
  42     }
  43     else
  44     {
  45       vp_os_memcpy(&cfg->socket_client, &cfg->socket, sizeof(vp_com_socket_t));
  46     }
  47     vp_com_sockopt(cfg->com, &cfg->socket_client, cfg->sockopt);
  48   }
  49
  50   // \todo test
  51   return res;
  52 }
  53
  54
  55 C_RESULT
  56 vp_stages_input_com_stage_transform(vp_stages_input_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
  57 {
  58   vp_os_mutex_lock(&out->lock);
  59
  60   if(out->status == VP_API_STATUS_INIT)
  61   {
  62     out->numBuffers = 1;
  63     out->size = cfg->buffer_size;
  64     out->buffers = (int8_t **) vp_os_malloc (sizeof(int8_t *)+out->size*sizeof(int8_t));
  65     out->buffers[0] = (int8_t *)(out->buffers+1);
  66     out->indexBuffer = 0;
  67     // out->lineSize not used
  68
  69     out->status = VP_API_STATUS_PROCESSING;
  70   }
  71
  72   if(out->status == VP_API_STATUS_PROCESSING && cfg->read != NULL)
  73   {
  74     out->size = cfg->buffer_size;
  75
  76     if(VP_FAILED(cfg->read(&cfg->socket_client, out->buffers[0], &out->size)))
  77       out->status = VP_API_STATUS_ERROR;
  78   }
  79
  80   vp_os_mutex_unlock(&out->lock);
  81
  82   return (VP_SUCCESS);
  83 }
  84
  85
  86 C_RESULT
  87 vp_stages_input_com_stage_close(vp_stages_input_com_config_t *cfg)
  88 {
  89   // \todo test
  90
  91   // \todo Faire ca dans le transform en detectant la fin d'une connection
  92   vp_com_close(cfg->com, &cfg->socket_client);
  93
  94   if(cfg->socket.type == VP_COM_SERVER)
  95     vp_com_close(cfg->com, &cfg->socket);
  96
  97   if(cfg->connection && cfg->connection->is_up)
  98     vp_com_disconnect(cfg->com);
  99
 100   vp_com_shutdown(cfg->com);
 101
 102   return (VP_SUCCESS);
 103 }
 104
 105
 106 C_RESULT
 107 vp_stages_output_com_stage_open(vp_stages_output_com_config_t *cfg)
 108 {
 109   C_RESULT res;
 110
 111   res = vp_com_init(cfg->com);
 112
 113   if( VP_SUCCEEDED(res) )
 114   {
 115     vp_com_local_config(cfg->com, cfg->config);
 116
 117     if(cfg->connection && !cfg->connection->is_up)
 118     {
 119       res = vp_com_connect(cfg->com, cfg->connection, 1);
 120     }
 121   }
 122
 123   if( VP_SUCCEEDED(res) && VP_FAILED(vp_com_open(cfg->com, &cfg->socket, 0, &cfg->write)))
 124     res = C_FAIL;
 125
 126   if( VP_SUCCEEDED(res) )
 127   {
 128     if(cfg->socket.type == VP_COM_SERVER)
 129     {
 130       res = vp_com_wait_connections(cfg->com, &cfg->socket, &cfg->socket_client, 1);
 131     }
 132     else
 133     {
 134       vp_os_memcpy(&cfg->socket_client, &cfg->socket, sizeof(vp_com_socket_t));
 135     }
 136
 137     vp_com_sockopt(cfg->com, &cfg->socket_client, cfg->sockopt);
 138   }
 139
 140   // \todo test
 141   return res;
 142 }
 143
 144
 145 C_RESULT
 146 vp_stages_output_com_stage_transform(vp_stages_output_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
 147 {
 148   vp_os_mutex_lock(&out->lock);
 149
 150   if(out->status == VP_API_STATUS_INIT)
 151   {
 152     out->status = VP_API_STATUS_PROCESSING;
 153   }
 154
 155   // \todo test
 156   if(out->status != VP_API_STATUS_ERROR)
 157   {
 158     out->size = in->size;
 159     if(in->size > 0 && cfg->write != NULL)
 160     {
 161       cfg->write(&cfg->socket_client, &in->buffers[in->indexBuffer][0], &out->size);
 162     }
 163
 164     out->status = in->status;
 165   }
 166   // \todo test
 167
 168   vp_os_mutex_unlock(&out->lock);
 169
 170   return (VP_SUCCESS);
 171 }
 172
 173
 174 C_RESULT
 175 vp_stages_output_com_stage_close(vp_stages_output_com_config_t *cfg)
 176 {
 177   // \todo Faire ca dans le transform en detectant la fin d'une connection
 178   vp_com_close(cfg->com, &cfg->socket_client);
 179
 180   if(cfg->socket.type == VP_COM_SERVER)
 181     vp_com_close(cfg->com, &cfg->socket);
 182
 183   // \todo test
 184   if(cfg->connection && cfg->connection->is_up)
 185      vp_com_disconnect(cfg->com);
 186
 187   vp_com_shutdown(cfg->com);
 188
 189   return (VP_SUCCESS);
 190 }