Commit | Line | Data |
---|---|---|
a6a4e5d4 PH |
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 | } |