sw/speed8/pc/link.c

   1
   2 #include <stdio.h>
   3 #include <unistd.h>
   4 #include <stdlib.h>
   5 #include <stdint.h>
   6 #include <fcntl.h>
   7 #include <string.h>
   8 #include <errno.h>
   9 #include <sys/select.h>
  10 #include <termios.h>
  11
  12 #include "link.h"
  13 #include "log.h"
  14
  15 static int fd=-1;
  16 static struct termios org_settings;
  17 static struct termios work_settings;
  18
  19 int link_open(char * port){
  20   int res;
  21   fd=open(port,O_RDWR);
  22   if (fd<0) return -1;
  23
  24   res=tcgetattr(fd,&org_settings);
  25   if (res!=0) return -1;
  26
  27   res=tcgetattr(fd,&work_settings);
  28   if (res!=0) return -1;
  29
  30   cfmakeraw(&work_settings);
  31   res=tcsetattr(fd, TCSANOW, &work_settings);
  32
  33   if (res!=0) return -1;
  34
  35   return 0;
  36 }
  37
  38 extern void link_close(void){
  39   tcdrain(fd);
  40   tcsetattr(fd, TCSANOW, &org_settings);
  41   close(fd);
  42 }
  43
  44 extern int link_flush(char * port){
  45   int ifd=open(port, O_RDWR);
  46   if (ifd<0) return -1;
  47
  48   fd_set fds;
  49   struct timeval timeout={
  50     .tv_sec=0,
  51     .tv_usec=50000,
  52   };
  53   FD_ZERO(&fds);
  54   FD_SET(ifd,&fds);
  55
  56   char tmpbuf[100];
  57   while (timeout.tv_usec){
  58     //debug("select\n");
  59     select(ifd+1,&fds,NULL,NULL,&timeout);
  60     //debug("select done\n");
  61     if (timeout.tv_sec+timeout.tv_usec)
  62       read(ifd,&tmpbuf, sizeof(tmpbuf));
  63   }
  64   close(ifd);
  65   return 0;
  66 }
  67
  68 int link_write(uint16_t * data, int len){
  69   char * wbuf;
  70   int i;
  71   int remain;
  72   if (fd<0) {
  73     errno=EINVAL;
  74     return -1;
  75   }
  76   wbuf=(char *)malloc(len*2);
  77   if (!wbuf){
  78     return -1;
  79   }
  80   for (i=0; i<len; i++){
  81     wbuf[2*i]=(data[i]>>6)&077;
  82     wbuf[2*i+1]=(data[i])&077;
  83   }
  84   remain=len*2;
  85
  86   while(remain){
  87     int res=write(fd,wbuf+len*2-remain, remain);
  88     if (res<0){
  89       free(wbuf);
  90       return -1;
  91     }
  92     remain-=res;
  93   }
  94   free(wbuf);
  95   return len;
  96 }
  97
  98 int link_read(uint16_t * buffer, int len){
  99   int i;
 100   char * rbuf;
 101   int remain;
 102   rbuf=(char *)malloc(len*2);
 103   if (!rbuf) return -1;
 104   if (!buffer){
 105     errno=EINVAL;
 106     return -1;
 107   }
 108   remain=len*2;
 109   while(remain){
 110     int res=read(fd,rbuf+len*2-remain, remain);
 111     if (res<0){
 112       free(rbuf);
 113       return -1;
 114     }
 115     remain-=res;
 116   }
 117
 118   for (i=0; i<len; i++){
 119     buffer[i]  = rbuf[2*i]&077;
 120     buffer[i] |= rbuf[2*i+1]<<6;
 121     buffer[i] &= 07777;
 122   }
 123   free(rbuf);
 124   return len;
 125 }