socks5proxy.c 30 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889
  1. /*
  2. * Slitheen - a decoy routing system for censorship resistance
  3. * Copyright (C) 2017 Cecylia Bocovich (cbocovic@uwaterloo.ca)
  4. *
  5. * This program is free software: you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License as published by
  7. * the Free Software Foundation, version 3.
  8. *
  9. * This program is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. * GNU General Public License for more details.
  13. *
  14. * You should have received a copy of the GNU General Public License
  15. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16. *
  17. * Additional permission under GNU GPL version 3 section 7
  18. *
  19. * If you modify this Program, or any covered work, by linking or combining
  20. * it with the OpenSSL library (or a modified version of that library),
  21. * containing parts covered by the terms of the OpenSSL Licence and the
  22. * SSLeay license, the licensors of this Program grant you additional
  23. * permission to convey the resulting work. Corresponding Source for a
  24. * non-source form of such a combination shall include the source code
  25. * for the parts of the OpenSSL library used as well as that of the covered
  26. * work.
  27. */
  28. #include <stdio.h>
  29. #include <stdlib.h>
  30. #include <unistd.h>
  31. #include <stdint.h>
  32. #include <errno.h>
  33. #include <string.h>
  34. #include <sys/socket.h>
  35. #include <sys/types.h>
  36. #include <sys/stat.h>
  37. #include <netinet/in.h>
  38. #include <netdb.h>
  39. #include <pthread.h>
  40. #include <fcntl.h>
  41. #include <arpa/inet.h>
  42. #include <openssl/bio.h>
  43. #include <openssl/evp.h>
  44. #include <openssl/buffer.h>
  45. #include <openssl/rand.h>
  46. #include "socks5proxy.h"
  47. #include "crypto.h"
  48. #include "tagging.h"
  49. #include "util.h"
  50. #define DEBUG
  51. static connection_table *connections;
  52. typedef struct {
  53. int32_t in;
  54. int32_t out;
  55. } ous_pipes;
  56. int main(void){
  57. int listen_socket;
  58. struct sockaddr_in address;
  59. struct sockaddr_in remote_addr;
  60. socklen_t addr_size;
  61. connections = calloc(1, sizeof(connection_table));
  62. connections->first = NULL;
  63. int32_t ous_in[2];
  64. if(pipe(ous_in) < 0){
  65. printf("Failed to create pipe\n");
  66. return 1;
  67. }
  68. int32_t ous_out[2];
  69. if(pipe(ous_out) < 0){
  70. printf("Failed to create pipe\n");
  71. return 1;
  72. }
  73. ous_pipes pipes;
  74. pipes.in = ous_in[0];
  75. pipes.out = ous_out[1];
  76. /* Spawn a thread to communicate with OUS */
  77. pthread_t *ous_thread = calloc(1, sizeof(pthread_t));
  78. pthread_create(ous_thread, NULL, ous_IO, (void *) &pipes);
  79. ous_pipes mux_pipes;
  80. mux_pipes.in = ous_in[1];
  81. mux_pipes.out = ous_out[0];
  82. /* Spawn a thread for multiplexing and demultiplexing */
  83. pthread_t *demux_thread = calloc(1, sizeof(pthread_t));
  84. pthread_create(demux_thread, NULL, demultiplex_data, (void *) &mux_pipes);
  85. pthread_t *mux_thread = calloc(1, sizeof(pthread_t));
  86. pthread_create(mux_thread, NULL, multiplex_data, (void *) &mux_pipes);
  87. if (!(listen_socket = socket(AF_INET, SOCK_STREAM, 0))){
  88. printf("Error creating socket\n");
  89. fflush(stdout);
  90. return 1;
  91. }
  92. address.sin_family = AF_INET;
  93. address.sin_addr.s_addr = INADDR_ANY;
  94. address.sin_port = htons(1080);
  95. int enable = 1;
  96. if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)) <0 ){
  97. printf("Error setting sockopt\n");
  98. return 1;
  99. }
  100. if(bind(listen_socket, (struct sockaddr *) &address, sizeof(address))){
  101. printf("Error binding socket\n");
  102. fflush(stdout);
  103. return 1;
  104. }
  105. if(listen(listen_socket, 10) < 0){
  106. printf("Error listening\n");
  107. fflush(stdout);
  108. close(listen_socket);
  109. exit(1);
  110. }
  111. uint8_t last_id = 1;
  112. printf("Ready for listening\n");
  113. for(;;){
  114. addr_size = sizeof(remote_addr);
  115. int new_socket;
  116. new_socket = accept(listen_socket, (struct sockaddr *) &remote_addr,
  117. &addr_size);
  118. if(new_socket < 0){
  119. perror("accept");
  120. exit(1);
  121. }
  122. printf("New connection\n");
  123. //assign a new stream_id and create a pipe for the session
  124. connection *new_conn = calloc(1, sizeof(connection));
  125. new_conn->stream_id = last_id++;
  126. new_conn->socket = new_socket;
  127. new_conn->state = NEW_STREAM;
  128. new_conn->next = NULL;
  129. if(connections->first == NULL){
  130. connections->first = new_conn;
  131. printf("Added first connection with id: %d\n", new_conn->stream_id);
  132. fflush(stdout);
  133. } else {
  134. connection *last = connections->first;
  135. while(last->next != NULL){
  136. last = last->next;
  137. }
  138. last->next = new_conn;
  139. printf("Added connection with id: %d at %p\n", new_conn->stream_id, last->next);
  140. fflush(stdout);
  141. }
  142. }
  143. return 0;
  144. }
  145. /*
  146. * Responsible for communicating with the OUS. Upstream data is read from the pipes of individual
  147. * streams and sent to the OUS. Downstream data is read from the OUS, demultiplexed according t
  148. * stream ID, and sent to the corresponding stream.
  149. */
  150. void *ous_IO(void *args){
  151. ous_pipes *pipes = (ous_pipes *) args;
  152. int32_t ous_in = pipes->in;
  153. int32_t ous_out = pipes->out;
  154. //generate Slitheen ID
  155. uint8_t slitheen_id[SLITHEEN_ID_LEN];
  156. uint8_t shared_secret[16];
  157. generate_slitheen_id(slitheen_id, shared_secret);
  158. #ifdef DEBUG
  159. printf("Randomly generated slitheen id: ");
  160. int i;
  161. for(i=0; i< SLITHEEN_ID_LEN; i++){
  162. printf("%02x ", slitheen_id[i]);
  163. }
  164. printf("\n");
  165. #endif
  166. // Calculate super encryption keys
  167. generate_super_keys(shared_secret);
  168. printf("Generated super encrypt keys\n");
  169. char *encoded_bytes = NULL;
  170. base64_encode(slitheen_id, SLITHEEN_ID_LEN, &encoded_bytes);
  171. printf("Encoded ID\n");
  172. //give encoded slitheen ID to ous
  173. struct sockaddr_in ous_addr;
  174. ous_addr.sin_family = AF_INET;
  175. inet_pton(AF_INET, "127.0.0.1", &(ous_addr.sin_addr));
  176. ous_addr.sin_port = htons(57173);
  177. int32_t ous = socket(AF_INET, SOCK_STREAM, 0);
  178. if(ous < 0){
  179. printf("Failed to make socket\n");
  180. pthread_exit(NULL);
  181. }
  182. int32_t error = connect(ous, (struct sockaddr *) &ous_addr, sizeof (struct sockaddr));
  183. if(error < 0){
  184. printf("Error connecting to OUS\n");
  185. pthread_exit(NULL);
  186. }
  187. printf("Connected to OUS\n");
  188. uint16_t len = htons(strlen(encoded_bytes));
  189. int32_t bytes_sent = send(ous, (unsigned char *) &len, sizeof(uint16_t), 0);
  190. bytes_sent += send(ous, encoded_bytes, ntohs(len), 0);
  191. printf("Wrote %d bytes to OUS_in: %x\n %s\n", bytes_sent, len, encoded_bytes);
  192. uint8_t *buffer = emalloc(BUFSIZ);
  193. int32_t buffer_len = BUFSIZ;
  194. int32_t bytes_read;
  195. /* Select on proxy pipes, demux thread, and ous to send and receive data*/
  196. for(;;){
  197. fd_set read_fds;
  198. fd_set write_fds;
  199. int32_t nfds = ous;
  200. if(ous_in > nfds)
  201. nfds = ous_in;
  202. if(ous_out > nfds)
  203. nfds = ous_out;
  204. FD_ZERO(&read_fds);
  205. FD_ZERO(&write_fds);
  206. FD_SET(ous_in, &read_fds);
  207. FD_SET(ous, &read_fds);
  208. FD_SET(ous_out, &write_fds);
  209. FD_SET(ous, &write_fds);
  210. if(select(nfds+1, &read_fds, &write_fds, NULL, NULL) < 0){
  211. fprintf(stderr, "Select error\n");
  212. break;
  213. }
  214. if(FD_ISSET(ous_in, &read_fds) && FD_ISSET(ous, &write_fds)){
  215. bytes_read = read(ous_in, buffer, buffer_len);
  216. #ifdef DEBUG
  217. printf("Received %d bytes from multiplexer\n", bytes_read);
  218. for(int i=0; i< bytes_read; i++){
  219. printf("%02x ", buffer[i]);
  220. }
  221. printf("\n");
  222. fflush(stdout);
  223. #endif
  224. if(bytes_read > 0){
  225. bytes_sent = send(ous, buffer, bytes_read, 0);
  226. #ifdef DEBUG
  227. printf("Sent %d bytes to OUS\n", bytes_sent);
  228. for(int i=0; i< bytes_sent; i++){
  229. printf("%02x ", buffer[i]);
  230. }
  231. printf("\n");
  232. fflush(stdout);
  233. #endif
  234. if(bytes_sent <= 0){
  235. fprintf(stderr, "Connection to OUS closed\n");
  236. break;
  237. }
  238. } else if (bytes_read == 0) {
  239. fprintf(stderr, "Connection to multiplexer closed\n");
  240. break;
  241. } else {
  242. fprintf(stderr, "Error reading from multiplexer\n");
  243. break;
  244. }
  245. }
  246. if(FD_ISSET(ous, &read_fds) && FD_ISSET(ous_out, &write_fds)){
  247. bytes_read = recv(ous, buffer, 4, 0);
  248. #ifdef DEBUG
  249. printf("Received %d bytes from OUS\n", bytes_read);
  250. for(int i=0; i< bytes_read; i++){
  251. printf("%02x ", buffer[i]);
  252. }
  253. printf("\n");
  254. fflush(stdout);
  255. #endif
  256. if (bytes_read <= 0) {
  257. fprintf(stderr, "Connection to OUS closed\n");
  258. break;
  259. }
  260. uint32_t *chunk_len = (uint32_t*) buffer;
  261. fprintf(stderr, "Length of this chunk: %u\n", *chunk_len);
  262. bytes_read = recv(ous, buffer, *chunk_len, 0);
  263. #ifdef DEBUG
  264. printf("Received %d bytes from OUS\n", bytes_read);
  265. for(int i=0; i< bytes_read; i++){
  266. printf("%02x ", buffer[i]);
  267. }
  268. printf("\n");
  269. fflush(stdout);
  270. #endif
  271. if(bytes_read > 0){
  272. bytes_sent = write(ous_out, buffer, bytes_read);
  273. #ifdef DEBUG
  274. printf("Sent %d bytes to demultiplexer\n", bytes_sent);
  275. for(int i=0; i< bytes_sent; i++){
  276. printf("%02x ", buffer[i]);
  277. }
  278. printf("\n");
  279. fflush(stdout);
  280. #endif
  281. if(bytes_sent <= 0){
  282. fprintf(stderr, "Connection to demultiplexer closed\n");
  283. break;
  284. }
  285. } else if (bytes_read == 0) {
  286. fprintf(stderr, "Connection to OUS closed\n");
  287. break;
  288. } else {
  289. fprintf(stderr, "Error reading from OUS\n");
  290. break;
  291. }
  292. }
  293. }
  294. fprintf(stderr, "Closing OUS\n");
  295. close(ous);
  296. close(ous_in);
  297. close(ous_out);
  298. free(buffer);
  299. pthread_exit(NULL);
  300. }
  301. /*
  302. * Continuously read from all stream sockets and pass data to ous
  303. */
  304. void *multiplex_data(void *args){
  305. ous_pipes *pipes = (ous_pipes *) args;
  306. int32_t buffer_len = BUFSIZ;
  307. uint8_t *buffer = ecalloc(1, buffer_len);
  308. int32_t bytes_read;
  309. uint8_t *response = ecalloc(1, BUFSIZ);
  310. /* Select on stream sockets and ous_in pipe to send and receive data*/
  311. for(;;){
  312. fd_set read_fds;
  313. fd_set write_fds;
  314. int32_t nfds = 0;
  315. FD_ZERO(&read_fds);
  316. FD_ZERO(&write_fds);
  317. //add all stream sockets to read_fds
  318. connection *conn = connections->first;
  319. while(conn != NULL){
  320. if(conn->socket > nfds)
  321. nfds = conn->socket;
  322. FD_SET(conn->socket, &read_fds);
  323. conn = conn->next;
  324. }
  325. FD_SET(pipes->in, &write_fds);
  326. if(pipes->in > nfds)
  327. nfds = pipes->in;
  328. struct timeval tv;
  329. tv.tv_sec = 3;
  330. tv.tv_usec = 0;
  331. if(select(nfds+1, &read_fds, &write_fds, NULL, &tv) < 0){
  332. fprintf(stderr, "Select error\n");
  333. break;
  334. }
  335. struct slitheen_up_hdr *up_hdr;
  336. uint16_t len;
  337. char *encoded_bytes = NULL;
  338. conn = connections->first;
  339. while(conn != NULL){
  340. uint8_t stream_id = conn->stream_id;
  341. if(FD_ISSET(conn->socket, &read_fds) && FD_ISSET(pipes->in, &write_fds)){
  342. printf("Reading from stream %d\n", conn->stream_id);
  343. bytes_read = recv(conn->socket, buffer, buffer_len, 0);
  344. if(bytes_read < 0){
  345. close(conn->socket);
  346. conn = conn->next;
  347. remove_connection(stream_id);
  348. continue;
  349. } else if(bytes_read == 0){
  350. //socket is closed
  351. printf("Closing connection for stream %d sockfd.\n", conn->stream_id);
  352. fflush(stdout);
  353. if(conn->state == CONNECTED){
  354. //Send close message to slitheen proxy
  355. up_hdr = (struct slitheen_up_hdr *) buffer;
  356. up_hdr->stream_id = conn->stream_id;
  357. up_hdr->len = 0;
  358. base64_encode(buffer, 20, &encoded_bytes);
  359. len = htons(strlen(encoded_bytes));
  360. int32_t bytes_sent = write(pipes->in, (unsigned char *) &len, sizeof(uint16_t));
  361. bytes_sent += write(pipes->in, encoded_bytes, ntohs(len));
  362. printf("Wrote %d bytes to ous\n", bytes_sent);
  363. printf("Closing message: %s\n", encoded_bytes);
  364. free(encoded_bytes);
  365. encoded_bytes = NULL;
  366. }
  367. close(conn->socket);
  368. conn = conn->next;
  369. remove_connection(stream_id);
  370. continue;
  371. }
  372. switch(conn->state){
  373. case NEW_STREAM:
  374. printf("Received new stream data from stream %d\n", conn->stream_id);
  375. #ifdef DEBUG
  376. printf("Received %d bytes (id %d):\n", bytes_read, conn->stream_id);
  377. for(int i=0; i< bytes_read; i++){
  378. printf("%02x ", buffer[i]);
  379. }
  380. printf("\n");
  381. fflush(stdout);
  382. #endif
  383. //Respond to methods negotiation
  384. struct socks_method_req *clnt_meth = (struct socks_method_req *) buffer;
  385. uint8_t *p = buffer + 2;
  386. if(clnt_meth->version != 0x05){
  387. close(conn->socket);
  388. printf("Client supplied invalid version: %02x\n", clnt_meth->version);
  389. fflush(stdout);
  390. conn = conn->next;
  391. remove_connection(stream_id);
  392. continue;
  393. }
  394. int responded = 0;
  395. int bytes_sent;
  396. for(int i=0; i< clnt_meth->num_methods; i++){
  397. if(p[0] == 0x00){//send response with METH= 0x00
  398. response[0] = 0x05;
  399. response[1] = 0x00;
  400. send(conn->socket, response, 2, 0);
  401. responded = 1;
  402. }
  403. p++;
  404. }
  405. if(!responded){//respond with METH= 0xFF
  406. response[0] = 0x05;
  407. response[1] = 0xFF;
  408. send(conn->socket, response, 2, 0);
  409. close(conn->socket);
  410. conn = conn->next;
  411. remove_connection(stream_id);
  412. continue;
  413. }
  414. conn->state = NEGOTIATED;
  415. break;
  416. case NEGOTIATED:
  417. printf("Received negotiation data from stream %d\n", conn->stream_id);
  418. #ifdef DEBUG
  419. printf("Received %d bytes (id %d):\n", bytes_read, conn->stream_id);
  420. for(int i=0; i< bytes_read; i++){
  421. printf("%02x ", buffer[i]);
  422. }
  423. printf("\n");
  424. fflush(stdout);
  425. #endif
  426. //Respond to say connection was accepted
  427. response[0] = 0x05;
  428. response[1] = 0x00;
  429. response[2] = 0x00;
  430. response[3] = 0x01;
  431. *((uint32_t *) (response + 4)) = 0;
  432. *((uint16_t *) (response + 8)) = 0;
  433. send(conn->socket, response, 10, 0); //TODO: add check for send
  434. memmove(buffer+sizeof(struct slitheen_up_hdr), buffer, bytes_read);
  435. up_hdr = (struct slitheen_up_hdr *) buffer;
  436. up_hdr->stream_id = conn->stream_id;
  437. up_hdr->len = htons(bytes_read);
  438. bytes_read+= sizeof(struct slitheen_up_hdr);
  439. base64_encode(buffer, bytes_read, &encoded_bytes);
  440. len = htons(strlen(encoded_bytes));
  441. bytes_sent = write(pipes->in, (unsigned char *) &len, sizeof(uint16_t));
  442. bytes_sent += write(pipes->in, encoded_bytes, ntohs(len));
  443. printf("Wrote %d bytes to ous\n", bytes_sent);
  444. free(encoded_bytes);
  445. encoded_bytes = NULL;
  446. conn->state = CONNECTED;
  447. break;
  448. case CONNECTED:
  449. printf("Received application data from stream %d\n", conn->stream_id);
  450. #ifdef DEBUG_UPSTREAM
  451. printf("Received %d data bytes from sockfd (id %d):\n", bytes_read, conn->stream_id);
  452. for(i=0; i< bytes_read; i++){
  453. printf("%02x ", buffer[i]);
  454. }
  455. printf("\n");
  456. printf("%s\n", buffer);
  457. fflush(stdout);
  458. #endif
  459. memmove(buffer+sizeof(struct slitheen_up_hdr), buffer, bytes_read);
  460. up_hdr = (struct slitheen_up_hdr *) buffer;
  461. up_hdr->stream_id = conn->stream_id;
  462. up_hdr->len = htons(bytes_read);
  463. bytes_read+= sizeof(struct slitheen_up_hdr);
  464. base64_encode(buffer, bytes_read, &encoded_bytes);
  465. len = htons(strlen(encoded_bytes));
  466. bytes_sent = write(pipes->in, (unsigned char *) &len, sizeof(uint16_t));
  467. bytes_sent += write(pipes->in, encoded_bytes, ntohs(len));
  468. printf("Wrote %d bytes to ous\n", bytes_sent);
  469. #ifdef DEBUG_UPSTREAM
  470. printf("Sent to OUS (%d bytes): %x %s\n",bytes_sent, len, encoded_bytes);
  471. #endif
  472. free(encoded_bytes);
  473. encoded_bytes = NULL;
  474. break;
  475. default:
  476. fprintf(stderr, "Wrong connection state\n");
  477. close(conn->socket);
  478. conn = conn->next;
  479. remove_connection(stream_id);
  480. break;
  481. }
  482. }
  483. conn = conn->next;
  484. }
  485. }
  486. free(response);
  487. pthread_exit(NULL);
  488. }
  489. /* Read blocks of covert data from the OUS. Determine the stream id and the length of
  490. * the block and then write the data to the correct thread to be passed to the browser
  491. */
  492. void *demultiplex_data(void *args){
  493. ous_pipes *pipes = (ous_pipes *) args;
  494. int32_t buffer_len = BUFSIZ;
  495. uint8_t *buffer = calloc(1, buffer_len);
  496. uint8_t *p;
  497. uint8_t *partial_block = NULL;
  498. uint32_t partial_block_len = 0;
  499. uint32_t resource_remaining = 0;
  500. uint64_t expected_next_count = 1;
  501. data_block *saved_data = NULL;
  502. for(;;){
  503. printf("Demux thread waiting to read\n");
  504. int32_t bytes_read = read(pipes->out, buffer, buffer_len-partial_block_len);
  505. if(bytes_read > 0){
  506. int32_t bytes_remaining = bytes_read;
  507. p = buffer;
  508. //didn't read a full slitheen block last time
  509. if(partial_block_len > 0){
  510. //process first part of slitheen info
  511. memmove(buffer+partial_block_len, buffer, bytes_read);
  512. memcpy(buffer, partial_block, partial_block_len);
  513. bytes_remaining += partial_block_len;
  514. free(partial_block);
  515. partial_block = NULL;
  516. partial_block_len = 0;
  517. }
  518. while(bytes_remaining > 0){
  519. if(resource_remaining <= 0){//we're at a new resource
  520. //the first value for a new resource will be the resource length,
  521. //followed by a newline
  522. uint8_t *end_ptr;
  523. resource_remaining = strtol((const char *) p, (char **) &end_ptr, 10);
  524. #ifdef DEBUG_PARSE
  525. printf("Starting new resource of len %d bytes\n", resource_remaining);
  526. printf("Resource len bytes:\n");
  527. int i;
  528. for(i=0; i< (end_ptr - p) + 1; i++){
  529. printf("%02x ", ((const char *) p)[i]);
  530. }
  531. printf("\n");
  532. #endif
  533. if(resource_remaining == 0){
  534. bytes_remaining -= (end_ptr - p) + 1;
  535. p += (end_ptr - p) + 1;
  536. } else {
  537. bytes_remaining -= (end_ptr - p) + 1;
  538. p += (end_ptr - p) + 1;
  539. }
  540. continue;
  541. }
  542. if(resource_remaining < SLITHEEN_HEADER_LEN){
  543. printf("ERROR: Resource remaining doesn't fit header len.\n");
  544. resource_remaining = 0;
  545. bytes_remaining = 0;
  546. break;
  547. }
  548. if(bytes_remaining < SLITHEEN_HEADER_LEN){
  549. #ifdef DEBUG_PARSE
  550. printf("Partial header: ");
  551. int i;
  552. for(i = 0; i< bytes_remaining; i++){
  553. printf("%02x ", p[i]);
  554. }
  555. printf("\n");
  556. #endif
  557. if(partial_block != NULL) printf("UH OH (PB)\n");
  558. partial_block = calloc(1, bytes_remaining);
  559. memcpy(partial_block, p, bytes_remaining);
  560. partial_block_len = bytes_remaining;
  561. bytes_remaining = 0;
  562. break;
  563. }
  564. //decrypt header to see if we have entire block
  565. uint8_t *tmp_header = malloc(SLITHEEN_HEADER_LEN);
  566. memcpy(tmp_header, p, SLITHEEN_HEADER_LEN);
  567. peek_header(tmp_header);
  568. struct slitheen_hdr *sl_hdr = (struct slitheen_hdr *) tmp_header;
  569. //first see if sl_hdr corresponds to a valid stream. If not, ignore rest of read bytes
  570. #ifdef DEBUG_PARSE
  571. printf("Slitheen header:\n");
  572. int i;
  573. for(i = 0; i< SLITHEEN_HEADER_LEN; i++){
  574. printf("%02x ", tmp_header[i]);
  575. }
  576. printf("\n");
  577. #endif
  578. if(ntohs(sl_hdr->len) > resource_remaining){
  579. printf("ERROR: slitheen block doesn't fit in resource remaining!\n");
  580. resource_remaining = 0;
  581. bytes_remaining = 0;
  582. break;
  583. }
  584. if(ntohs(sl_hdr->len) > bytes_remaining){
  585. if(partial_block != NULL) printf("UH OH (PB)\n");
  586. partial_block = calloc(1, ntohs(sl_hdr->len));
  587. memcpy(partial_block, p, bytes_remaining);
  588. partial_block_len = bytes_remaining;
  589. bytes_remaining = 0;
  590. free(tmp_header);
  591. break;
  592. }
  593. super_decrypt(p);
  594. sl_hdr = (struct slitheen_hdr *) p;
  595. free(tmp_header);
  596. p += SLITHEEN_HEADER_LEN;
  597. bytes_remaining -= SLITHEEN_HEADER_LEN;
  598. resource_remaining -= SLITHEEN_HEADER_LEN;
  599. if((!sl_hdr->len) && (sl_hdr->garbage)){
  600. #ifdef DEBUG_PARSE
  601. printf("%d Garbage bytes\n", ntohs(sl_hdr->garbage));
  602. #endif
  603. p += ntohs(sl_hdr->garbage);
  604. bytes_remaining -= ntohs(sl_hdr->garbage);
  605. resource_remaining -= ntohs(sl_hdr->garbage);
  606. continue;
  607. }
  608. int32_t sock =-1;
  609. if(connections->first == NULL){
  610. printf("Error: there are no connections\n");
  611. } else {
  612. connection *last = connections->first;
  613. if (last->stream_id == sl_hdr->stream_id){
  614. sock = last->socket;
  615. }
  616. while(last->next != NULL){
  617. last = last->next;
  618. if (last->stream_id == sl_hdr->stream_id){
  619. sock = last->socket;
  620. }
  621. }
  622. }
  623. if(sock == -1){
  624. printf("No stream id exists. Possibly invalid header\n");
  625. break;
  626. }
  627. #ifdef DEBUG_PARSE
  628. printf("Received information for stream id: %d of length: %u\n", sl_hdr->stream_id, ntohs(sl_hdr->len));
  629. #endif
  630. //figure out how much to skip
  631. int32_t padding = 0;
  632. if(ntohs(sl_hdr->len) %16){
  633. padding = 16 - ntohs(sl_hdr->len)%16;
  634. }
  635. p += 16; //IV
  636. //check counter to see if we are missing data
  637. if(sl_hdr->counter > expected_next_count){
  638. //save any future data
  639. printf("Received header with count %lu. Expected count %lu.\n",
  640. sl_hdr->counter, expected_next_count);
  641. if((saved_data == NULL) || (saved_data->count > sl_hdr->counter)){
  642. data_block *new_block = malloc(sizeof(data_block));
  643. new_block->count = sl_hdr->counter;
  644. new_block->len = ntohs(sl_hdr->len);
  645. new_block->data = malloc(ntohs(sl_hdr->len));
  646. memcpy(new_block->data, p, ntohs(sl_hdr->len));
  647. new_block->socket = sock;
  648. new_block->next = saved_data;
  649. saved_data = new_block;
  650. } else {
  651. data_block *last = saved_data;
  652. while((last->next != NULL) && (last->next->count < sl_hdr->counter)){
  653. last = last->next;
  654. }
  655. data_block *new_block = malloc(sizeof(data_block));
  656. new_block->count = sl_hdr->counter;
  657. new_block->len = ntohs(sl_hdr->len);
  658. new_block->data = malloc(ntohs(sl_hdr->len));
  659. memcpy(new_block->data, p, ntohs(sl_hdr->len));
  660. new_block->socket = sock;
  661. new_block->next = last->next;
  662. last->next = new_block;
  663. }
  664. } else {
  665. int32_t bytes_sent = send(sock, p, ntohs(sl_hdr->len), 0);
  666. if(bytes_sent <= 0){
  667. printf("Error writing to socket for stream id %d\n", sl_hdr->stream_id);
  668. }
  669. //increment expected counter
  670. expected_next_count++;
  671. }
  672. //now check to see if there is saved data to write out
  673. if(saved_data != NULL){
  674. data_block *current_block = saved_data;
  675. while((current_block != NULL) && (expected_next_count == current_block->count)){
  676. int32_t bytes_sent = send(current_block->socket, current_block->data,
  677. current_block->len, 0);
  678. if(bytes_sent <= 0){
  679. printf("Error writing to socket for stream id %d\n", sl_hdr->stream_id);
  680. }
  681. expected_next_count++;
  682. saved_data = current_block->next;
  683. free(current_block->data);
  684. free(current_block);
  685. current_block = saved_data;
  686. }
  687. }
  688. p += ntohs(sl_hdr->len); //encrypted data
  689. p += 16; //mac
  690. p += padding;
  691. p += ntohs(sl_hdr->garbage);
  692. bytes_remaining -= ntohs(sl_hdr->len) + 16 + padding + 16 + ntohs(sl_hdr->garbage);
  693. resource_remaining -= ntohs(sl_hdr->len) + 16 + padding + 16 + ntohs(sl_hdr->garbage);
  694. }
  695. } else {
  696. printf("Error: read %d bytes from OUS_out\n", bytes_read);
  697. goto err;
  698. }
  699. }
  700. err:
  701. free(buffer);
  702. close(pipes->out);
  703. pthread_exit(NULL);
  704. }
  705. int remove_connection(uint16_t stream_id){
  706. connection *last = connections->first;
  707. connection *prev = last;
  708. while(last != NULL){
  709. if(last->stream_id == stream_id){
  710. if(last == connections->first){
  711. connections->first = last->next;
  712. } else {
  713. prev->next = last->next;
  714. }
  715. free(last);
  716. printf("Removed stream id %d from connections table\n", stream_id);
  717. break;
  718. }
  719. prev = last;
  720. last = last->next;
  721. }
  722. return 1;
  723. }