git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Com/linux/vp_com_serial.c

   1 // Header ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   2
   3 /**
   4  *  \brief    Com Api for video sdk. Private declarations.
   5  *  \author   Aurelien Morelle <aurelien.morelle@parrot.com>
   6  *  \version  1.0
   7  *  \date     24/07/2007
   8  */
   9
  10 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  11
  12 // Include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  13
  14 #include <stdio.h>
  15 #include <unistd.h>
  16 #include <sys/types.h>
  17 #include <sys/stat.h>
  18 #include <fcntl.h>
  19 #include <errno.h>
  20
  21 #include <VP_Com/vp_com.h>
  22 #include <VP_Com/vp_com_error.h>
  23
  24 #include <VP_Os/vp_os_assert.h>
  25 #include <VP_Os/vp_os_malloc.h>
  26 #include <VP_Os/vp_os_signal.h>
  27 #include <VP_Os/vp_os_print.h>
  28 #include <VP_Os/vp_os_delay.h>
  29
  30 #include <VP_Api/vp_api_error.h>
  31
  32 #include "vp_com_serial.h"
  33
  34 // Static ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  35
  36 static struct termios tio_save;
  37 static vp_os_mutex_t wait_sync_mutex;
  38 static vp_os_mutex_t write_sync_mutex;
  39
  40 // Forward declarations /////////////////////////////////////////////////////////////////////////////////////////////////////////
  41
  42 static C_RESULT vp_com_serial_wait_sync(vp_com_serial_config_t* config, vp_com_socket_t* socket);
  43 static C_RESULT vp_com_serial_write_sync(vp_com_serial_config_t* config, vp_com_socket_t* socket);
  44
  45 static C_RESULT vp_com_serial_read (vp_com_socket_t* socket, int8_t* buffer, int32_t* size);
  46 static C_RESULT vp_com_serial_write(vp_com_socket_t* socket, const int8_t* buffer, int32_t* size);
  47
  48 // Functions declaration ////////////////////////////////////////////////////////////////////////////////////////////////////////
  49
  50 C_RESULT vp_com_serial_init(void)
  51 {
  52   vp_os_mutex_init(&wait_sync_mutex);
  53   vp_os_mutex_init(&write_sync_mutex);
  54
  55   return VP_COM_OK;
  56 }
  57
  58 C_RESULT vp_com_serial_shutdown()
  59 {
  60   // nothing
  61   return VP_COM_OK;
  62 }
  63
  64 C_RESULT vp_com_serial_local_config(vp_com_serial_config_t* config)
  65 {
  66   C_RESULT res = VP_COM_OK;
  67
  68   // Serial port must be opened before changing config
  69
  70   return res;
  71 }
  72
  73 C_RESULT vp_com_serial_connect(vp_com_t* vp_com, vp_com_connection_t* connection, int32_t numAttempts)
  74 {
  75   C_RESULT res = VP_COM_OK;
  76
  77   res = (connection == NULL) ? VP_COM_PARAMERROR : VP_COM_OK;
  78   VP_COM_CHECK( res );
  79
  80   connection->is_up = 1;
  81
  82   return VP_COM_OK;
  83 }
  84
  85 C_RESULT vp_com_serial_disconnect(vp_com_serial_config_t* config, vp_com_connection_t* connection)
  86 {
  87   C_RESULT res = (connection == NULL) ? VP_COM_PARAMERROR : VP_COM_OK;
  88   VP_COM_CHECK( res );
  89
  90   connection->is_up = 0;
  91
  92   return VP_COM_OK;
  93 }
  94
  95 C_RESULT vp_com_serial_open(vp_com_serial_config_t* config, vp_com_connection_t* connection, vp_com_socket_t* socket, Read* read, Write* write)
  96 {
  97   struct termios tio;
  98   speed_t speed;
  99
 100   VP_OS_ASSERT(config->blocking == 0 || config->blocking == 1);
 101
 102   if(config->blocking == 0)
 103     socket->priv = (void *)open(&config->itfName[0], O_RDWR|O_NOCTTY|O_NONBLOCK);
 104   else
 105     socket->priv = (void *)open(&config->itfName[0], O_RDWR|O_NOCTTY);
 106
 107   if(((int)socket->priv) == -1)
 108     {
 109       PRINT("Unable to \"open\" serial device");
 110       return (VP_COM_ERROR);
 111     }
 112
 113   /* get current serial port settings */
 114   if(tcgetattr((int)socket->priv, &tio) != 0)
 115     {
 116       PRINT("Serial device configuration failure (%s)", strerror(errno));
 117       close((int)socket->priv);
 118       return (VP_COM_ERROR);
 119     }
 120
 121   tio_save = tio;
 122
 123   if(config->sync)
 124     {
 125       /* set serial settings */
 126       speed = (speed_t)config->initial_baudrate;
 127       cfsetispeed(&tio, speed);
 128       cfsetospeed(&tio, speed);
 129       cfmakeraw(&tio);
 130
 131       if(tcsetattr((int)socket->priv, TCSANOW, &tio) != 0)
 132         {
 133           PRINT("Serial device configuration failure (%s)", strerror(errno));
 134           close((int)socket->priv);
 135           return (VP_COM_ERROR);
 136         }
 137
 138       if(socket->type == VP_COM_CLIENT)
 139         {
 140           if(FAILED(vp_com_serial_write_sync(config, socket)))
 141             return (VP_COM_ERROR);
 142           vp_os_delay(VP_COM_SYNC_DELAY);
 143         }
 144       else if(socket->type == VP_COM_SERVER)
 145         {
 146           if(FAILED(vp_com_serial_wait_sync(config, socket)))
 147             return (VP_COM_ERROR);
 148           vp_os_delay(VP_COM_SYNC_DELAY);
 149         }
 150     }
 151
 152   /* set serial settings */
 153   speed = (speed_t)config->baudrate;
 154   cfsetispeed(&tio, speed);
 155   cfsetospeed(&tio, speed);
 156   cfmakeraw(&tio);
 157
 158   if(tcsetattr((int)socket->priv, TCSANOW, &tio) != 0)
 159     {
 160       PRINT("Serial device configuration failure (%s)", strerror(errno));
 161       close((int)socket->priv);
 162       return (VP_COM_ERROR);
 163     }
 164
 165   if(read) *read = (Read) vp_com_serial_read;
 166   if(write) *write = (Write) vp_com_serial_write;
 167
 168   return (VP_COM_OK);
 169 }
 170
 171 C_RESULT vp_com_serial_close(vp_com_socket_t* socket)
 172 {
 173   if(tcsetattr((int)socket->priv, TCSANOW, &tio_save) != 0)
 174     {
 175       PRINT("Serial device configuration failure (%s)", strerror(errno));
 176       close((int)socket->priv);
 177       return (VP_COM_ERROR);
 178     }
 179
 180   close((int)socket->priv);
 181
 182   return VP_COM_OK;
 183 }
 184
 185 C_RESULT vp_com_serial_wait_connections(vp_com_connection_t** c, vp_com_socket_t* server, vp_com_socket_t* client, int32_t queueLength)
 186 {
 187   vp_os_memcpy(client, server, sizeof(vp_com_socket_t));
 188   return VP_COM_OK;
 189 }
 190
 191 C_RESULT vp_com_serial_network_adapter_lookup(vp_com_network_adapter_lookup_t callback)
 192 {
 193   // nothing
 194   return VP_COM_OK;
 195 }
 196
 197 C_RESULT vp_com_serial_inquire(const char* deviceName, vp_com_inquiry_t callback, uint32_t timeout)
 198 {
 199   // nothing
 200   return VP_COM_OK;
 201 }
 202
 203 // Static functions /////////////////////////////////////////////////////////////////////////////////////////////////////////////
 204
 205 static C_RESULT vp_com_serial_read (vp_com_socket_t* socket, int8_t* buffer, int32_t* size)
 206 {
 207   *size = read((int)socket->priv, buffer, *size);
 208
 209   if(*size == -1 && errno != EAGAIN)
 210     {
 211       return VP_COM_ERROR;
 212     }
 213   else
 214     {
 215       if(*size == -1)
 216         *size = 0;
 217 #if 0
 218       int i;
 219       for(i = 0 ; i < red ; i++)
 220         {
 221           PRINT("%02X", (buffer+(*size-remain))[i]);
 222         }
 223       PRINT(" (%d)\n", red);
 224 #endif
 225     }
 226
 227   return VP_COM_OK;
 228 }
 229
 230 static C_RESULT vp_com_serial_write(vp_com_socket_t* socket, const int8_t* buffer, int32_t* size)
 231 {
 232   uint32_t remain = *size, written = 0;
 233
 234   while(remain > 0)
 235     {
 236       written = write((int)socket->priv, buffer+(*size-remain), remain);
 237
 238       if(written == -1 && errno != EAGAIN)
 239         return VP_COM_ERROR;
 240       else if(written == -1)
 241         written = 0;
 242
 243       remain -= written;
 244     }
 245
 246   return VP_COM_OK;
 247 }
 248
 249 static C_RESULT vp_com_serial_wait_sync(vp_com_serial_config_t* config, vp_com_socket_t* socket)
 250 {
 251   uint32_t nb;
 252   uint8_t c;
 253
 254   vp_os_mutex_lock(&wait_sync_mutex);
 255
 256   if(!config->sync_done)
 257     {
 258       nb = 0;
 259       do
 260         {
 261           if(-1 == read((int)socket->priv, &c, sizeof(int8_t)))
 262             return (FAIL);
 263           if(c == (VP_COM_SYNC_STRING)[nb])
 264             {
 265               nb++;
 266             }
 267           else
 268             {
 269               nb = 0;
 270             }
 271         }
 272       while(nb != sizeof(VP_COM_SYNC_STRING));
 273
 274       config->sync_done = 1;
 275     }
 276
 277   vp_os_mutex_unlock(&wait_sync_mutex);
 278
 279   return (SUCCESS);
 280 }
 281
 282 static C_RESULT vp_com_serial_write_sync(vp_com_serial_config_t* config, vp_com_socket_t* socket)
 283 {
 284   uint32_t i;
 285   uint8_t c;
 286
 287   vp_os_mutex_lock(&write_sync_mutex);
 288
 289   if(!config->sync_done)
 290     {
 291       for(i = 0 ; i < sizeof(VP_COM_SYNC_STRING) ; i++)
 292         {
 293           c = (VP_COM_SYNC_STRING)[i];
 294           if(-1 == write((int)socket->priv, &c, sizeof(int8_t)))
 295             {
 296               return (FAIL);
 297             }
 298         }
 299     }
 300
 301   config->sync_done = 1;
 302
 303   vp_os_mutex_unlock(&write_sync_mutex);
 304
 305   return (SUCCESS);
 306 }
 307
 308 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////