Reduce small block size to 32x32, and parametrize (#defines instead of hardcoding)
[rdpsrv] / rdpsrv.c
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <unistd.h>
4 #include <errno.h>
5 #include <sys/select.h>
6 #include <sys/socket.h>
7 #include <sys/ioctl.h>
8 #include <arpa/inet.h>
9
10 #include "rdesktop.h"
11 #include "scancodes.h"
12
13 const int tcp_port_rdp = 3389;
14 int create_server_socket();
15 int serve_client();
16
17 int main()
18 {
19         int server_sock = create_server_socket();
20         for ( ;; ) {
21                 iso_recv_connect(server_sock);
22                 printf("Got connection.\n");
23                 serve_client();
24                 printf("Client closed.\n");
25         }
26 }
27
28 int create_server_socket()
29 {
30         int server_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
31         const unsigned int one = 1, zero = 0;
32         struct sockaddr_in addr;
33         int err;
34
35         setsockopt(server_sock, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
36         ioctl(server_sock, FIONBIO, &zero);
37
38         addr.sin_family = AF_INET;
39         addr.sin_addr.s_addr = INADDR_ANY;
40         addr.sin_port = htons(tcp_port_rdp);
41
42         do {
43                 err = bind(server_sock, (struct sockaddr *)&addr, sizeof(struct sockaddr));
44
45                 if (err == -1) {
46                         perror("bind()");
47
48                         /* try to recover from recoverable errors... */
49                         if (errno == ENOMEM || errno == EADDRINUSE) {
50                                 puts("Waiting 1 sec before trying again...");
51                                 sleep(1);
52                         } else {
53                                 puts("Giving up.");
54                                 exit(1);
55                         }
56                 }
57         } while (err == -1);
58
59         listen(server_sock, 20);
60         return server_sock;
61 }
62                         
63 int map_scancode_to_vnc_key(int scancode)
64 {
65         switch (scancode)
66         {
67         case SCANCODE_CHAR_1 ... SCANCODE_CHAR_9:
68                 return scancode + '1' - SCANCODE_CHAR_1;
69         case SCANCODE_CHAR_0:
70                 return '0';
71         case SCANCODE_CHAR_BACKSPACE:
72                 return 0xff08;
73         case SCANCODE_CHAR_TAB:
74                 return 0xff09;
75         case SCANCODE_CHAR_Q ... SCANCODE_CHAR_P:
76                 return "QWERTYUIOP"[scancode - SCANCODE_CHAR_Q];
77         case SCANCODE_CHAR_A ... SCANCODE_CHAR_L:
78                 return "ASDFGHJKL"[scancode - SCANCODE_CHAR_A];
79         case SCANCODE_CHAR_Z ... SCANCODE_CHAR_M:
80                 return "ZXCVBNM"[scancode - SCANCODE_CHAR_Z];
81         default:
82                 printf("Unknown scancode %x, ignoring\n", scancode);
83                 return -1;
84         }
85 }
86
87 void handle_input_pdu(STREAM s, int vnc_sock)
88 {
89         uint32 time;
90         uint16 message_type, device_flags, param1, param2;
91         uint16 num_events;
92         int i;
93         char buf[256];
94         static int mouse1_down = 0, mouse2_down = 0;
95         int vnc_key;
96
97         in_uint16_le(s, num_events);   // number of events
98         in_uint8s(s, 2);        // pad
99
100         for (i = 0; i < num_events; ++i) {
101                 rdp_recv_input(s, &time, &message_type, &device_flags, &param1, &param2);
102                 printf("Input event at time %u\n", time);
103                 
104                 switch (message_type) {
105                 case RDP_INPUT_SYNCHRONIZE:
106                         printf("- Type: Synchronize (ignored)\n");
107                         break;
108                 case RDP_INPUT_CODEPOINT:
109                         printf("- Type: Codepoint (ignored)\n");
110                         break;
111                 case RDP_INPUT_VIRTKEY:
112                         printf("- Type: Virtual key (ignored)\n");
113                         break;
114                 case RDP_INPUT_SCANCODE:
115                         printf("- Type: Scancode\n");
116                         printf("- Device flags: %x\n", device_flags);
117                         printf("- Key: %x\n", param1);
118                         
119                         vnc_key = map_scancode_to_vnc_key(param1);
120                         
121                         if (vnc_key != -1) {
122                                 buf[0] = 4; // message type
123                                 buf[1] = (device_flags & KBD_FLAG_DOWN) ? 1 : 0; // down flag
124                                 buf[2] = 0; // padding
125                                 buf[3] = 0;
126                                 buf[4] = vnc_key >> 24;  // keysym
127                                 buf[5] = (vnc_key >> 16) & 0xff;
128                                 buf[6] = (vnc_key >> 8) & 0xff;
129                                 buf[7] = vnc_key & 0xff;
130                                 write(vnc_sock, buf, 8);
131                         }
132                         
133                         break;
134                 case RDP_INPUT_MOUSE:
135                         printf("- Type: Mouse\n");
136                         printf("- Device flags: %x\n", device_flags);
137                         printf("- Position: (%u,%u)\n", param1, param2);
138
139                         if (device_flags & MOUSE_FLAG_BUTTON1)
140                                  mouse1_down = (device_flags & MOUSE_FLAG_DOWN) ? 0x01 : 0;
141                         if (device_flags & MOUSE_FLAG_BUTTON2)
142                                  mouse2_down = (device_flags & MOUSE_FLAG_DOWN) ? 0x02 : 0;
143                 
144                         printf("button mask = %x\n", mouse1_down | mouse2_down);
145                         
146                         buf[0] = 5; // message type
147                         buf[1] = mouse1_down | mouse2_down; // button mask
148                         buf[2] = param1 >> 8;
149                         buf[3] = param1 & 0xff;
150                         buf[4] = param2 >> 8;
151                         buf[5] = param2 & 0xff;
152                         write(vnc_sock, buf, 6);
153                         
154                         break;
155                 default:
156                         printf("- Unknown type %x\n", message_type);
157                         break;
158                 }
159                 printf("\n");
160         }
161         
162         // re-request the entire framebuffer
163         buf[0] = 3; // message type
164         buf[1] = 1; // incremental
165         buf[2] = 0; // xpos
166         buf[3] = 0;
167         buf[4] = 0; // ypos
168         buf[5] = 0;
169         buf[6] = 640 >> 8; // width
170         buf[7] = 640 & 0xff;
171         buf[8] = 480 >> 8; // height
172         buf[9] = 480 & 0xff;
173
174         write(vnc_sock, buf, 10);
175 }
176
177 void handle_control_pdu(STREAM s)
178 {
179         uint16 action, userid;
180         uint32 control_id;
181
182         in_uint16_le(s, action);
183         in_uint16_le(s, userid);
184         in_uint32(s, control_id);
185         
186         printf("Control PDU: action=%hu userid=%hu control_id=%u\n", action, userid, control_id);
187
188         switch (action) {
189         case RDP_CTL_COOPERATE:
190                 printf("Cooperate\n");
191                 rdp_send_control(RDP_CTL_COOPERATE);
192                 break;
193         case RDP_CTL_REQUEST_CONTROL:
194                 printf("Client requesting control; granting\n");
195                 rdp_send_control(RDP_CTL_GRANT_CONTROL);
196                 break;
197         default:
198                 printf("Unhandled\n");
199         }
200 }
201
202 void
203 handle_font2_pdu(STREAM s)
204 {
205         printf("FONT2 PDU, responding with magic\n");
206
207         s = rdp_init_data(8);
208         out_uint16_le(s, 0);
209         out_uint16_le(s, 0);
210         out_uint16_le(s, 3);
211         out_uint16_le(s, 4);
212         s_mark_end(s);
213         rdp_send_data(s, 0x28);
214 }
215
216 struct ServerInitialization {
217         unsigned short width;
218         unsigned short height;
219         unsigned char pixelformat[16]; // FIXME
220         unsigned int name_len;
221 };
222
223 int vnc_init()
224 {
225         char buf[256];
226         int vnc_sock = tcp_connect("127.0.0.1", 5901);
227         struct ServerInitialization si;
228
229         // get handshake
230         if (read(vnc_sock, buf, 12) != 12)
231                 error("short read on handshake\n");
232         write(vnc_sock, "RFB 003.003\n", 12);
233
234         // get auth
235         if (read(vnc_sock, buf, 4) != 4)
236                 error("short read on auth\n");
237
238         if (buf[3] != 1)
239                 error("auth needed or connection failed\n");
240
241         // send shared flag
242         buf[0] = 1;
243         write(vnc_sock, buf, 1);
244
245         // read the server initialization
246         if (read(vnc_sock, &si, sizeof(struct ServerInitialization)) != sizeof(struct ServerInitialization))
247                 error("short read on SI");
248
249         printf("Server is %u x %u\n", ntohs(si.width), ntohs(si.height));
250
251         if (read(vnc_sock, buf, ntohl(si.name_len)) != ntohl(si.name_len))
252                 error("short read on server name\n");
253
254 //      printf("Server name is '%*s' (%u bytes)\n", ntohl(si.name_len), buf, ntohl(si.name_len));
255
256         // we can only accept raw encoding
257         buf[0] = 2; // message type
258         buf[1] = 0; // padding
259         buf[2] = 0; // number of encodings
260         buf[3] = 1;
261         buf[4] = 0; // raw encoding
262         buf[5] = 0; 
263         buf[6] = 0;
264         buf[7] = 0;
265
266         write(vnc_sock, buf, 8);
267         
268         // request the entire framebuffer
269         buf[0] = 3; // message type
270         buf[1] = 0; // incremental
271         buf[2] = 0; // xpos
272         buf[3] = 0;
273         buf[4] = 0; // ypos
274         buf[5] = 0;
275         buf[6] = 640 >> 8; // width
276         buf[7] = 640 & 0xff;
277         buf[8] = 480 >> 8; // height
278         buf[9] = 480 & 0xff;
279
280         write(vnc_sock, buf, 10);
281         
282         printf("Connected to VNC!\n");
283
284         return vnc_sock;
285 }
286
287 struct vnc_rectangle {
288         unsigned short x, y, width, height;
289         unsigned int encoding;
290 };
291
292 #define SBX 32
293 #define SBY 32
294
295 void handle_vnc_fbupdate(int vnc_sock)
296 {
297         struct vnc_rectangle rect;
298         unsigned char *data, *ptr, *src, *dst;
299         unsigned short num_rect;
300         int ret, data_left, i, xt, yt;
301         unsigned char smallblock[SBX * SBY * 3];
302
303         if (read(vnc_sock, &num_rect, 2) != 2)
304                 error("short read on num_rect\n");
305
306         for (i = 0; i < ntohs(num_rect); ++i) {
307                 int width, height;
308                 
309                 if (read(vnc_sock, &rect, sizeof(struct vnc_rectangle)) != sizeof(struct vnc_rectangle))
310                         error("short read on vnc_rectangle\n");
311                 
312                 if (ntohl(rect.encoding) != 0)
313                         error("unsupported encoding\n");
314
315                 width = ntohs(rect.width);
316                 height = ntohs(rect.height);
317                 
318                 printf("UPDATE: %ux%u at (%u,%u)\n", width, height,
319                         ntohs(rect.x), ntohs(rect.y));
320                 
321                 ptr = data = xmalloc(width * height * 4);
322                 data_left = width * height * 4;
323
324                 while (data_left > 0) {
325                         ret = read(vnc_sock, ptr, data_left);
326                         if (ret <= 0)
327                                 error("error on data read\n");
328
329                         ptr += ret;
330                         data_left -= ret;
331                 }
332
333                 // push our 32-bit RGB data into small enough chunks
334                 // (64x64) so we are sure we can send them without
335                 // everything crashing and stuff :-)
336                 for (yt = 0; yt < (height + (SBY-1)) / SBY; ++yt) {
337                         for (xt = 0; xt < (width + (SBX-1)) / SBX; ++xt) {
338                                 int x, y;
339                                 int bw = width - xt * SBX;
340                                 int bh = height - yt * SBY;
341                                 if (bw > SBX)
342                                         bw = SBX;
343                                 if (bh > SBY)
344                                         bh = SBY;
345                         
346                                 dst = smallblock;
347                                 for (y = 0; y < bh; ++y) {
348                                         src = data + ((yt * SBY + (bh - 1 - y)) * width + (xt * SBX)) * 4;
349                                         for (x = 0; x < bw; ++x) {
350                                                 *dst++ = *src++;
351                                                 *dst++ = *src++;
352                                                 *dst++ = *src++;
353                                                 ++src;
354                                         }
355                                 }
356
357                                 rdp_send_bitmap_update(ntohs(rect.x) + xt * SBX, ntohs(rect.y) + yt * SBY, bw, bh, smallblock);
358                         }
359                 }
360                 
361                 xfree(data);
362         }
363 }
364
365 int listen_on_vnc = 0;
366
367 int serve_client()
368 {
369         int vnc_sock = vnc_init();
370         
371         if (!mcs_recv_connect_initial())
372                 error("MCS_CONNECT_INITIAL recv failed");
373         mcs_send_connect_response();
374
375         for ( ;; ) {
376                 uint8 type, data_pdu_type;
377                 uint16 id;
378                 STREAM s;
379
380                 fd_set readfs;
381                 FD_ZERO(&readfs);
382                 FD_SET(tcp_get_socket(), &readfs);
383                 FD_SET(vnc_sock, &readfs);
384                 
385                 select(FD_SETSIZE, &readfs, NULL, NULL, NULL);
386
387                 // activity on RDP socket?
388                 if (FD_ISSET(tcp_get_socket(), &readfs)) {
389                         if ((s = rdp_recv(&type)) != NULL) {
390                                 switch (type) {
391                                 case RDP_PDU_DATA:
392                                         in_uint8s(s, 8);        /* shareid, pad, streamid, length */
393                                         in_uint8(s, data_pdu_type);
394                                         in_uint8s(s, 3);        /* compress_type, compress_len */
395
396                                         switch (data_pdu_type) {
397                                         case RDP_DATA_PDU_INPUT:
398                                                 handle_input_pdu(s, vnc_sock);
399                                                 listen_on_vnc = 1;
400                                                 break;
401                                         case RDP_DATA_PDU_CONTROL:
402                                                 handle_control_pdu(s);
403                                                 break;
404                                         case RDP_DATA_PDU_SYNCHRONISE:
405                                                 in_uint16_le(s, id);
406                                                 printf("Synchronise, id=%u\n", id);
407                                                 rdp_send_synchronise(id);
408                                                 break;
409                                         case RDP_DATA_PDU_FONT2:
410                                                 handle_font2_pdu(s);
411                                                 break;
412                                         default:
413                                                 printf("Unknown data PDU type %u\n", data_pdu_type);
414                                         };
415                                         break;
416                                 case RDP_PDU_CONFIRM_ACTIVE:
417                                         printf("Client confirms activity\n");
418                                         break;
419                                 default:
420                                         printf("Unknown RDP PDU type %u\n", type);
421                                 }
422                         }
423                 }
424
425                 // activity on VNC socket?
426                 if (FD_ISSET(vnc_sock, &readfs) && listen_on_vnc) {
427                         unsigned char buf[256];
428
429                         printf("Activity on VNC socket!\n");
430                         
431                         if (read(vnc_sock, buf, 1) != 1)
432                                 error("short read on vnc_sock\n");
433
434                         switch (buf[0]) {
435                         case 0:
436                                 // frame buffer update!
437                                 printf("Framebuffer update\n");
438                                 if (read(vnc_sock, buf, 1) != 1)
439                                         error("short read on vnc_sock\n");
440                                 handle_vnc_fbupdate(vnc_sock);
441                                 break;
442                         case 2: // bell
443                                 printf("\a\n");
444                                 break;
445                         case 3:
446                                 printf("New text in clipboard, ignoring\n");
447                                 if (read(vnc_sock, buf, 3) != 3)
448                                         error("short read on vnc_sock\n");
449                                 if (read(vnc_sock, buf, 4) != 4)
450                                         error("short read on vnc_sock\n");
451                                 if (read(vnc_sock, buf, (buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3]) != 4)
452                                         error("short read on vnc_sock\n");
453                                 
454                         default:
455                                 printf("Unknown VNC server message %x\n", buf[0]);
456                                 exit(1);
457                         }
458                 }
459         }
460 }