├── CtrlCenter.c ├── README.md ├── SimpleChat.c ├── SimpleVPN.c └── list.h /CtrlCenter.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "list.h" 18 | 19 | struct tuple { 20 | char addr[16]; 21 | unsigned short port; 22 | unsigned short id; 23 | unsigned short unused; 24 | } __attribute__((packed)); 25 | 26 | struct ctrl_header { 27 | unsigned short sid; 28 | unsigned short did; 29 | unsigned short num; 30 | unsigned short unused; 31 | } __attribute__((packed)); 32 | 33 | struct control_frame { 34 | struct ctrl_header header; 35 | struct tuple tuple[0]; 36 | } __attribute__((packed)); 37 | 38 | struct client { 39 | struct list_head list; 40 | struct tuple tuple; 41 | int fd; 42 | void *others; 43 | }; 44 | 45 | struct config { 46 | int listen_fd; 47 | struct list_head clients; 48 | unsigned short tot_num; 49 | }; 50 | 51 | 52 | int client_msg_process(int fd, struct config *conf) 53 | { 54 | int ret = 0; 55 | int i = 0; 56 | size_t len = 0; 57 | struct client *peer; 58 | struct sockaddr_in addr; 59 | char *saddr; 60 | int port; 61 | int addr_len = sizeof(struct sockaddr_in); 62 | struct ctrl_header aheader = {0}; 63 | struct tuple newclient; 64 | struct tuple *peers; 65 | struct tuple *peers_base; 66 | struct list_head *tmp; 67 | 68 | 69 | bzero (&addr, sizeof(addr)); 70 | 71 | len = recv(fd, &newclient, sizeof(newclient), 0); 72 | 73 | peer = (struct client *)calloc(1, sizeof(struct client)); 74 | if (!peer) { 75 | return -1; 76 | } 77 | 78 | memcpy(peer->tuple.addr, &newclient.addr, sizeof(struct tuple)); 79 | peer->tuple.port = newclient.port; 80 | peer->fd = fd; 81 | INIT_LIST_HEAD(&peer->list); 82 | aheader.sid = 0; 83 | aheader.num = 0; 84 | 85 | peers_base = peers = (struct tuple*)calloc(conf->tot_num, sizeof(struct tuple)); 86 | peer->tuple.id = conf->tot_num+1; 87 | aheader.did = peer->tuple.id; 88 | conf->tot_num ++; 89 | list_for_each(tmp, &conf->clients) { 90 | struct ctrl_header header = {0}; 91 | struct client *tmp_client = list_entry(tmp, struct client, list); 92 | header.sid = 0; 93 | header.did = tmp_client->tuple.id; 94 | newclient.id = aheader.did; 95 | header.num = 1; 96 | send(tmp_client->fd, &header, sizeof(struct ctrl_header), 0); 97 | send(tmp_client->fd, &newclient, sizeof(struct tuple), 0); 98 | aheader.num += 1; 99 | memcpy(peers->addr, tmp_client->tuple.addr, 16); 100 | peers->port = tmp_client->tuple.port; 101 | peers->id = tmp_client->tuple.id; 102 | peers++; 103 | } 104 | send(peer->fd, (const void *)&aheader, sizeof(struct ctrl_header), 0); 105 | if (aheader.num) { 106 | send(peer->fd, peers_base, aheader.num*sizeof(struct tuple), 0); 107 | printf("send to new client num:%d\n", aheader.num); 108 | } 109 | 110 | list_add_tail(&peer->list, &conf->clients); 111 | 112 | return ret; 113 | 114 | } 115 | 116 | int main_loop(struct config *conf) 117 | { 118 | int ret = 0; 119 | int len = sizeof(struct sockaddr_in); 120 | fd_set rd_set; 121 | 122 | FD_ZERO(&rd_set); 123 | FD_SET(conf->listen_fd, &rd_set); 124 | 125 | while(1) { 126 | int fd; 127 | int nfds; 128 | 129 | nfds = select(FD_SETSIZE, &rd_set, (fd_set *)0,(fd_set *)0, (struct timeval *) 0); 130 | if(nfds < 1) { 131 | perror("server5"); 132 | exit(1); 133 | } 134 | 135 | for(fd = 0; fd < FD_SETSIZE; fd++) { 136 | if(FD_ISSET(fd,&rd_set)) { 137 | if(fd == conf->listen_fd) { 138 | int client_fd; 139 | struct sockaddr_in client_addr; 140 | client_fd = accept(conf->listen_fd, (struct sockaddr *)&client_addr, &len); 141 | FD_SET(client_fd, &rd_set); 142 | } else { 143 | struct list_head *tmp; 144 | int new_client = fd; 145 | list_for_each(tmp, &conf->clients) { 146 | struct client *tmp_client = list_entry(tmp, struct client, list); 147 | if (fd == tmp_client->fd) { 148 | new_client = 0; 149 | } 150 | } 151 | if (new_client) { 152 | client_msg_process(new_client, conf); 153 | } else { 154 | 155 | //TODO 156 | } 157 | } 158 | } 159 | } 160 | } 161 | return ret; 162 | } 163 | 164 | int init_config(struct config *conf, char *srv_addr, unsigned short srv_port) 165 | { 166 | int ret = 0; 167 | int len; 168 | int fd; 169 | struct sockaddr_in addr; 170 | 171 | INIT_LIST_HEAD(&conf->clients); 172 | conf->tot_num = 0; 173 | 174 | fd = socket(AF_INET, SOCK_STREAM, 0); 175 | 176 | addr.sin_family = AF_INET; 177 | addr.sin_addr.s_addr = inet_addr(srv_addr); 178 | addr.sin_port = htons(srv_port); 179 | len = sizeof(addr); 180 | 181 | ret = bind(fd, (struct sockaddr *)&addr, len); 182 | if (ret) { 183 | exit (-1); 184 | } 185 | 186 | ret = listen(fd, 5); 187 | if (ret) { 188 | exit (-1); 189 | } 190 | 191 | conf->listen_fd = fd; 192 | 193 | return ret; 194 | } 195 | 196 | int main(int argc, char **argv) 197 | { 198 | char serverIP[16]; 199 | unsigned short serverPORT; 200 | struct config conf; 201 | 202 | if (argc != 3) { 203 | printf("./a.out serverIP serverPORT localIP localPORT\n"); 204 | } 205 | strcpy(serverIP, argv[1]); 206 | serverPORT = atoi(argv[2]); 207 | 208 | init_config(&conf, serverIP, serverPORT); 209 | 210 | main_loop(&conf); 211 | 212 | return 0; 213 | } 214 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SimpleVPN 2 | 非常简单的一个VPN框架！ 3 | -------------------------------------------------------------------------------- /SimpleChat.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "list.h" 16 | 17 | struct ethernet_header { 18 | unsigned char dest[6]; 19 | unsigned char source[6]; 20 | unsigned short type; 21 | } __attribute__((packed)); 22 | 23 | struct tuple { 24 | char addr[16]; 25 | unsigned short port; 26 | unsigned short id; 27 | unsigned short unused; 28 | } __attribute__((packed)); 29 | 30 | struct ctrl_header { 31 | unsigned short sid; 32 | unsigned short did; 33 | unsigned short num; 34 | unsigned short unused; 35 | } __attribute__((packed)); 36 | 37 | struct node_info { 38 | struct list_head list; 39 | struct tuple tuple; 40 | void *other; 41 | }; 42 | 43 | struct frame { 44 | unsigned short sid; 45 | unsigned short did; 46 | char data[1500]; 47 | int len; 48 | } __attribute__((packed)); 49 | 50 | struct control_frame { 51 | struct ctrl_header header; 52 | struct tuple tuple[0]; 53 | } __attribute__((packed)); 54 | 55 | struct config; 56 | struct process_handler { 57 | struct list_head list; 58 | unsigned char id; 59 | struct node_info *peer; 60 | int (*send)(struct process_handler *this, struct frame *frame); 61 | int (*receive)(struct process_handler *this, struct frame *frame); 62 | struct config *conf; 63 | }; 64 | 65 | struct server { 66 | char addr[16]; 67 | unsigned short port; 68 | void *others; 69 | }; 70 | 71 | struct config { 72 | struct node_info *self; 73 | int con_fd; 74 | int udp_fd; 75 | int ctrl_fd; 76 | struct server server; 77 | struct list_head peers; 78 | struct list_head stack; 79 | struct list_head *first; 80 | struct list_head *last; 81 | int num_handlers; 82 | }; 83 | 84 | 85 | int server_msg_read(struct config *conf) 86 | { 87 | int ret = 0; 88 | int i = 0; 89 | size_t len = 0; 90 | struct node_info *peer; 91 | struct sockaddr_in addr; 92 | char *saddr; 93 | int port; 94 | int addr_len = sizeof(struct sockaddr_in); 95 | struct ctrl_header header = {0}; 96 | struct tuple *peers; 97 | 98 | bzero (&addr, sizeof(addr)); 99 | 100 | len = recvfrom(conf->ctrl_fd, &header, sizeof(header), 0 , (struct sockaddr *)&addr ,&addr_len); 101 | if (len <= 0) { 102 | exit(-1); 103 | } 104 | 105 | conf->self->tuple.id = header.did; 106 | printf("get self ID: %d\n", header.did); 107 | if (header.num == 0) { 108 | goto end; 109 | } 110 | peers = (struct tuple *)calloc(header.num, sizeof(struct tuple)); 111 | if (!peers) { 112 | return -1; 113 | } 114 | 115 | len = recvfrom(conf->ctrl_fd, peers, header.num*sizeof(struct tuple), 0 , (struct sockaddr *)&addr ,&addr_len); 116 | for (i = 0; i < header.num; i++) { 117 | struct node_info *peer = (struct node_info *)calloc(1, sizeof(struct node_info)); 118 | memcpy(peer->tuple.addr, peers->addr, 16); 119 | peer->tuple.port = peers->port; 120 | peer->tuple.id = peers->id; 121 | INIT_LIST_HEAD(&peer->list); 122 | list_add_tail(&peer->list, &conf->peers); 123 | peers++; 124 | } 125 | end: 126 | return ret; 127 | } 128 | 129 | int server_msg_register(struct config *conf) 130 | { 131 | int ret = 0; 132 | int i = 0; 133 | size_t len = 0; 134 | struct node_info *peer; 135 | struct sockaddr_in addr; 136 | int addr_len = sizeof(struct sockaddr_in); 137 | struct ctrl_header header = {0}; 138 | 139 | bzero (&addr, sizeof(addr)); 140 | addr.sin_family = AF_INET; 141 | addr.sin_port = htons(conf->server.port); 142 | addr.sin_addr.s_addr = inet_addr(conf->server.addr); 143 | 144 | 145 | len = sendto(conf->ctrl_fd, &conf->self->tuple, sizeof(struct tuple), 0, (struct sockaddr *)&addr, addr_len); 146 | 147 | return ret; 148 | 149 | } 150 | 151 | int call_stack(struct config *conf, int dir) 152 | { 153 | int ret = 0; 154 | struct process_handler *handler; 155 | struct frame frame = {0}; 156 | int more = 1; 157 | struct list_head *begin; 158 | struct node_info *tmp_peer; 159 | 160 | dir = !!dir; 161 | if (dir) { 162 | begin = conf->first; 163 | } else { 164 | begin = conf->last; 165 | } 166 | handler = list_entry(begin, struct process_handler, list); 167 | tmp_peer = NULL; 168 | 169 | while(handler) { 170 | int preid = handler->id; 171 | handler->peer = tmp_peer; 172 | if (dir && handler->send) { 173 | ret = handler->send(handler, &frame); 174 | } else if (!dir && handler->receive) { 175 | ret = handler->receive(handler, &frame); 176 | } 177 | if (ret) { 178 | break; 179 | } 180 | tmp_peer = handler->peer; 181 | 182 | if (dir && handler->list.next == &conf->stack) { 183 | break; 184 | } 185 | if (!dir && handler->list.prev == &conf->stack) { 186 | break; 187 | } 188 | if (dir) { 189 | handler = list_entry(handler->list.next, struct process_handler, list); 190 | more = (handler->id > preid); 191 | } else { 192 | handler = list_entry(handler->list.prev, struct process_handler, list); 193 | more = (handler->id < preid); 194 | } 195 | if (!more) { 196 | break; 197 | } 198 | } 199 | return ret; 200 | } 201 | 202 | int read_from_console(struct process_handler *obj, struct frame *frame) 203 | { 204 | size_t len = 0; 205 | int fd = obj->conf->con_fd; 206 | struct list_head *tmp; 207 | struct node_info *peer; 208 | char buf[1024]; 209 | 210 | 211 | len = read(fd, buf, 1024); 212 | if (len > 0 && !strncmp(buf, "list", strlen("list"))) { 213 | 214 | list_for_each(tmp, &obj->conf->peers) { 215 | peer = list_entry(tmp, struct node_info, list); 216 | printf("ID:%d online\n", peer->tuple.id); 217 | } 218 | return 1; 219 | } else { 220 | char IDchar[4] = {0}; 221 | int ID = 0; 222 | strncpy(IDchar, buf, 1); 223 | ID = atoi(IDchar); 224 | list_for_each(tmp, &obj->conf->peers) { 225 | peer = list_entry(tmp, struct node_info, list); 226 | if (peer->tuple.id == ID) { 227 | obj->peer = peer; 228 | } 229 | } 230 | if (!obj->peer) { 231 | return -1; 232 | } 233 | len = strlen(buf) - 2; 234 | strncpy(frame->data, buf+2, len); 235 | frame->len = len; 236 | } 237 | 238 | 239 | return 0; 240 | } 241 | 242 | int write_to_console(struct process_handler *obj, struct frame *frame) 243 | { 244 | int ret = 0; 245 | int fd = obj->conf->con_fd; 246 | size_t len; 247 | 248 | printf("From %d:%s\n", frame->sid, frame->data); 249 | 250 | return ret; 251 | } 252 | 253 | static struct process_handler console_handler = { 254 | .send = read_from_console, 255 | .receive = write_to_console, 256 | }; 257 | 258 | int _routing(struct process_handler *obj, struct frame *frame) 259 | { 260 | int ret = 0; 261 | // Nothing todo 262 | return ret; 263 | } 264 | 265 | 266 | int _learning(struct process_handler *obj, struct frame *frame) 267 | { 268 | int ret = 0; 269 | // Nothing todo 270 | return ret; 271 | } 272 | static struct process_handler routing_handler = { 273 | .send = _routing, 274 | .receive = _learning, 275 | }; 276 | 277 | int encode_frame(struct process_handler *obj, struct frame *frame) 278 | { 279 | int ret = 0; 280 | 281 | frame->sid = obj->conf->self->tuple.id; 282 | frame->did = 0; 283 | if (obj->peer) { 284 | frame->did = obj->peer->tuple.id; 285 | } 286 | 287 | return ret; 288 | } 289 | 290 | int decode_frame(struct process_handler *obj, struct frame *frame) 291 | { 292 | int ret = 0; 293 | 294 | if (frame->did != 0 && frame->did != obj->conf->self->tuple.id) { 295 | ret = -1; 296 | } 297 | 298 | return ret; 299 | } 300 | static struct process_handler protocol_handler = { 301 | .send = encode_frame, 302 | .receive = decode_frame, 303 | }; 304 | 305 | int encrypt(struct process_handler *obj, struct frame *frame) 306 | { 307 | int ret = 0; 308 | int i; 309 | char *c = (char *)frame; 310 | int len = sizeof(short)*2 + frame->len; 311 | for (i = 0; i < len; i++) { 312 | c[i] = c[i]+1; 313 | } 314 | return ret; 315 | } 316 | 317 | int decrypt(struct process_handler *obj, struct frame *frame) 318 | { 319 | int ret = 0; 320 | int i; 321 | char *c = (char *)frame; 322 | int len = frame->len; 323 | 324 | for (i = 0; i < len; i++) { 325 | c[i] = c[i]-1; 326 | } 327 | return ret; 328 | } 329 | 330 | static struct process_handler enc_handler = { 331 | .send = encrypt, 332 | .receive = decrypt, 333 | }; 334 | 335 | int send_frame(struct process_handler *obj, struct frame *frame) 336 | { 337 | int ret = 0; 338 | int fd = obj->conf->udp_fd; 339 | size_t len = 0; 340 | struct node_info *peer = obj->peer; 341 | struct config *conf = obj->conf; 342 | struct sockaddr_in addr; 343 | int addr_len = sizeof(struct sockaddr_in); 344 | 345 | bzero(&addr,sizeof(addr)); 346 | addr.sin_family = AF_INET; 347 | if (peer) { 348 | addr.sin_port = htons(peer->tuple.port); 349 | addr.sin_addr.s_addr = inet_addr(peer->tuple.addr); 350 | len = sendto(fd, frame, sizeof(short)*2+frame->len, 0, (struct sockaddr *)&addr, addr_len); 351 | } else { 352 | struct list_head *tmp; 353 | list_for_each(tmp, &conf->peers) { 354 | peer = list_entry(tmp, struct node_info, list); 355 | addr.sin_port = htons(peer->tuple.port); 356 | addr.sin_addr.s_addr = inet_addr(peer->tuple.addr); 357 | len = sendto(fd, frame, sizeof(short)*2+frame->len, 0, (struct sockaddr *)&addr, addr_len); 358 | } 359 | 360 | } 361 | return ret; 362 | } 363 | 364 | int receive_frame(struct process_handler *obj, struct frame *frame) 365 | { 366 | int ret = 0; 367 | size_t len = 0; 368 | struct list_head *tmp; 369 | struct node_info *peer; 370 | struct sockaddr_in addr; 371 | char *saddr; 372 | int port; 373 | int addr_len = sizeof(struct sockaddr_in); 374 | 375 | bzero (&addr, sizeof(addr)); 376 | 377 | len = recvfrom(obj->conf->udp_fd, frame, sizeof(struct frame), 0 , (struct sockaddr *)&addr ,&addr_len); 378 | frame->len = len; 379 | 380 | list_for_each(tmp, &obj->conf->peers) { 381 | peer = list_entry(tmp, struct node_info, list); 382 | saddr = inet_ntoa(addr.sin_addr); 383 | port = ntohs(addr.sin_port); 384 | if (!memcmp(saddr, peer->tuple.addr, strlen(saddr)) && port == peer->tuple.port) { 385 | obj->peer = peer; 386 | } 387 | } 388 | if (!obj->peer) { 389 | ret = -1; 390 | } 391 | 392 | return ret; 393 | } 394 | static struct process_handler udp_handler = { 395 | .send = send_frame, 396 | .receive = receive_frame, 397 | }; 398 | 399 | int register_handler(struct process_handler *handler, struct config *conf) 400 | { 401 | INIT_LIST_HEAD(&handler->list); 402 | handler->conf = conf; 403 | list_add_tail(&handler->list, &conf->stack); 404 | if (conf->first == NULL) { 405 | conf->first = &handler->list; 406 | } 407 | conf->last = &handler->list; 408 | handler->id = conf->num_handlers; 409 | conf->num_handlers++; 410 | return 0; 411 | } 412 | 413 | int unregister_handler(struct process_handler *handler, struct config *conf) 414 | { 415 | //TODO 416 | } 417 | 418 | int init_config(struct config *conf) 419 | { 420 | INIT_LIST_HEAD(&conf->stack); 421 | INIT_LIST_HEAD(&conf->peers); 422 | conf->first = NULL; 423 | conf->last = NULL; 424 | conf->con_fd = -1; 425 | conf->udp_fd = -1; 426 | conf->self = NULL; 427 | conf->num_handlers = 0; 428 | } 429 | 430 | int init_self(struct config *conf, char *addr, unsigned short port) 431 | { 432 | int fd = -1; 433 | struct sockaddr_in saddr; 434 | struct node_info *self; 435 | 436 | self = (struct node_info *)calloc(1, sizeof(struct node_info)); 437 | if (self == NULL) { 438 | exit(-1); 439 | } 440 | conf->self = self; 441 | 442 | strcpy(conf->self->tuple.addr, addr); 443 | conf->self->tuple.port = port; 444 | if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0){ 445 | exit (-1); 446 | } 447 | 448 | bzero(&saddr, sizeof(saddr)); 449 | saddr.sin_family = AF_INET; 450 | saddr.sin_port = htons(conf->self->tuple.port); 451 | saddr.sin_addr.s_addr = inet_addr(conf->self->tuple.addr); 452 | if (bind(fd, (struct sockaddr *)&saddr, sizeof(saddr))<0){ 453 | perror("connect"); 454 | exit(1); 455 | } 456 | 457 | conf->udp_fd = fd; 458 | } 459 | 460 | int init_console(struct config *conf) 461 | { 462 | conf->con_fd = 0; 463 | } 464 | 465 | int init_server_connect(struct config *conf, char *addr, unsigned short port) 466 | { 467 | int ret = 0; 468 | int fd = -1; 469 | struct sockaddr_in srv_addr; 470 | 471 | if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0){ 472 | perror("socket"); 473 | exit (-1); 474 | } 475 | 476 | bzero(&srv_addr, sizeof(srv_addr)); 477 | srv_addr.sin_family = AF_INET; 478 | srv_addr.sin_port = htons(port); 479 | srv_addr.sin_addr.s_addr = inet_addr(addr); 480 | 481 | if (connect(fd, (struct sockaddr*)&srv_addr, sizeof(srv_addr)) == -1) { 482 | perror("connect"); 483 | exit (-1); 484 | } 485 | 486 | conf->ctrl_fd = fd; 487 | return ret; 488 | } 489 | 490 | int main_loop(struct config *conf) 491 | { 492 | int ret = 0; 493 | fd_set rd_set; 494 | int max = conf->con_fd; 495 | 496 | if (conf->ctrl_fd > conf->con_fd) { 497 | max = conf->ctrl_fd; 498 | } 499 | if (max < conf->udp_fd) { 500 | max = conf->udp_fd; 501 | } 502 | 503 | while(1) { 504 | int nfds; 505 | int i; 506 | FD_ZERO(&rd_set); 507 | FD_SET(conf->ctrl_fd, &rd_set); 508 | FD_SET(conf->con_fd, &rd_set); 509 | FD_SET(conf->udp_fd, &rd_set); 510 | 511 | nfds = select(max+1, &rd_set, NULL, NULL, NULL); 512 | 513 | for (i = 0;i < nfds; i++) { 514 | if(FD_ISSET(conf->ctrl_fd, &rd_set)) { 515 | server_msg_read(conf); 516 | } 517 | if(FD_ISSET(conf->con_fd, &rd_set)) { 518 | call_stack(conf, 1); 519 | } 520 | if(FD_ISSET(conf->udp_fd, &rd_set)) { 521 | call_stack(conf, 0); 522 | } 523 | } 524 | } 525 | return ret; 526 | } 527 | 528 | int main(int argc, char **argv) 529 | { 530 | char serverIP[16]; 531 | char localIP[16]; 532 | unsigned short serverPORT; 533 | unsigned short localPORT; 534 | struct config conf; 535 | 536 | if (argc != 5) { 537 | printf("./a.out serverIP serverPORT localIP localPORT\n"); 538 | } 539 | strcpy(serverIP, argv[1]); 540 | serverPORT = atoi(argv[2]); 541 | strcpy(localIP, argv[3]); 542 | localPORT = atoi(argv[4]); 543 | 544 | init_config(&conf); 545 | init_console(&conf); 546 | init_self(&conf, localIP, localPORT); 547 | 548 | register_handler(&console_handler, &conf); 549 | register_handler(&routing_handler, &conf); 550 | register_handler(&protocol_handler, &conf); 551 | register_handler(&enc_handler, &conf); 552 | register_handler(&udp_handler, &conf); 553 | 554 | init_server_connect(&conf, serverIP, serverPORT); 555 | 556 | server_msg_register(&conf); 557 | server_msg_read(&conf); 558 | 559 | main_loop(&conf); 560 | 561 | return 0; 562 | } 563 | -------------------------------------------------------------------------------- /SimpleVPN.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "list.h" 16 | 17 | struct ethernet_header { 18 | unsigned char dest[6]; 19 | unsigned char source[6]; 20 | unsigned short type; 21 | } __attribute__((packed)); 22 | 23 | struct tuple { 24 | char addr[16]; 25 | unsigned short port; 26 | unsigned short id; 27 | unsigned short unused; 28 | } __attribute__((packed)); 29 | 30 | struct ctrl_header { 31 | unsigned short sid; 32 | unsigned short did; 33 | unsigned short num; 34 | unsigned short unused; 35 | } __attribute__((packed)); 36 | 37 | struct node_info { 38 | struct list_head list; 39 | struct tuple tuple; 40 | struct list_head macs; 41 | void *other; 42 | }; 43 | 44 | struct mac_entry { 45 | struct list_head list; 46 | struct list_head node; //struct hlist_node; 47 | char mac[6]; 48 | struct node_info *peer; 49 | }; 50 | 51 | struct frame { 52 | unsigned short sid; 53 | unsigned short did; 54 | char data[1500]; 55 | int len; 56 | } __attribute__((packed)); 57 | 58 | struct control_frame { 59 | struct ctrl_header header; 60 | struct tuple tuple[0]; 61 | } __attribute__((packed)); 62 | 63 | struct config; 64 | struct process_handler { 65 | struct list_head list; 66 | struct node_info *peer; 67 | int (*send)(struct process_handler *this, struct frame *frame); 68 | int (*receive)(struct process_handler *this, struct frame *frame); 69 | struct config *conf; 70 | }; 71 | 72 | struct server { 73 | char addr[16]; 74 | unsigned short port; 75 | void *others; 76 | }; 77 | 78 | struct config { 79 | struct node_info *self; 80 | int tap_fd; 81 | int udp_fd; 82 | int ctrl_fd; 83 | struct server server; 84 | struct list_head macs; 85 | struct list_head peers; 86 | struct list_head stack; 87 | struct list_head *first; 88 | struct list_head *last; 89 | }; 90 | 91 | 92 | int server_msg_read(struct config *conf) 93 | { 94 | int ret = 0; 95 | int i = 0; 96 | size_t len = 0; 97 | struct node_info *peer; 98 | struct sockaddr_in addr; 99 | char *saddr; 100 | int port; 101 | int addr_len = sizeof(struct sockaddr_in); 102 | struct ctrl_header header = {0}; 103 | struct tuple *peers; 104 | 105 | bzero (&addr, sizeof(addr)); 106 | 107 | len = recvfrom(conf->ctrl_fd, &header, sizeof(header), 0 , (struct sockaddr *)&addr ,&addr_len); 108 | if (len <= 0) { 109 | exit(-1); 110 | } 111 | 112 | conf->self->tuple.id = header.did; 113 | if (header.num == 0) { 114 | goto end; 115 | } 116 | peers = (struct tuple *)calloc(header.num, sizeof(struct tuple)); 117 | if (!peers) { 118 | return -1; 119 | } 120 | 121 | len = recvfrom(conf->ctrl_fd, peers, header.num*sizeof(struct tuple), 0 , (struct sockaddr *)&addr ,&addr_len); 122 | for (i = 0; i < header.num; i++) { 123 | struct node_info *peer = (struct node_info *)calloc(1, sizeof(struct node_info)); 124 | memcpy(peer->tuple.addr, peers->addr, 16); 125 | peer->tuple.port = peers->port; 126 | peer->tuple.id = peers->id; 127 | INIT_LIST_HEAD(&peer->list); 128 | INIT_LIST_HEAD(&peer->macs); 129 | list_add_tail(&peer->list, &conf->peers); 130 | peers++; 131 | } 132 | end: 133 | return ret; 134 | } 135 | 136 | int server_msg_register(struct config *conf) 137 | { 138 | int ret = 0; 139 | int i = 0; 140 | size_t len = 0; 141 | struct node_info *peer; 142 | struct sockaddr_in addr; 143 | int addr_len = sizeof(struct sockaddr_in); 144 | struct ctrl_header header = {0}; 145 | 146 | bzero (&addr, sizeof(addr)); 147 | addr.sin_family = AF_INET; 148 | addr.sin_port = htons(conf->server.port); 149 | addr.sin_addr.s_addr = inet_addr(conf->server.addr); 150 | 151 | 152 | len = sendto(conf->ctrl_fd, &conf->self->tuple, sizeof(struct tuple), 0, (struct sockaddr *)&addr, addr_len); 153 | 154 | return ret; 155 | 156 | } 157 | 158 | int call_stack(struct config *conf, int dir) 159 | { 160 | int ret = 0; 161 | struct process_handler *handler; 162 | struct frame frame = {0}; 163 | struct list_head *begin; 164 | struct node_info *tmp_peer; 165 | 166 | dir = !!dir; 167 | if (dir) { 168 | begin = conf->first; 169 | } else { 170 | begin = conf->last; 171 | } 172 | handler = list_entry(begin, struct process_handler, list); 173 | tmp_peer = NULL; 174 | 175 | while(handler) { 176 | int preid = handler->id; 177 | handler->peer = tmp_peer; 178 | if (dir && handler->send) { 179 | ret = handler->send(handler, &frame); 180 | } else if (!dir && handler->receive) { 181 | ret = handler->receive(handler, &frame); 182 | } 183 | if (ret) { 184 | break; 185 | } 186 | tmp_peer = handler->peer; 187 | 188 | if (dir && handler->list.next == &conf->stack) { 189 | break; 190 | } 191 | if (!dir && handler->list.prev == &conf->stack) { 192 | break; 193 | } 194 | if (dir) { 195 | handler = list_entry(handler->list.next, struct process_handler, list); 196 | } else { 197 | handler = list_entry(handler->list.prev, struct process_handler, list); 198 | } 199 | } 200 | return ret; 201 | } 202 | 203 | int read_from_tap(struct process_handler *obj, struct frame *frame) 204 | { 205 | int ret = 0; 206 | int fd = obj->conf->tap_fd; 207 | size_t len; 208 | 209 | len = read(fd, frame->data, sizeof(frame->data)); 210 | frame->len = len; 211 | 212 | return ret; 213 | } 214 | 215 | int write_to_tap(struct process_handler *obj, struct frame *frame) 216 | { 217 | int ret = 0; 218 | int fd = obj->conf->tap_fd; 219 | size_t len; 220 | 221 | len = write(fd, frame->data, sizeof(frame->data)); 222 | 223 | return ret; 224 | } 225 | 226 | static struct process_handler tap_handler = { 227 | .send = read_from_tap, 228 | .receive = write_to_tap, 229 | }; 230 | 231 | int frame_routing(struct process_handler *obj, struct frame *frame) 232 | { 233 | int ret = 0; 234 | struct ethernet_header eh; 235 | struct list_head *tmp; 236 | 237 | memcpy(&eh, frame->data, sizeof(eh)); 238 | list_for_each(tmp, &obj->conf->macs) { 239 | struct mac_entry *tmp_entry = list_entry(tmp, struct mac_entry, node); 240 | if (!memcmp(eh.dest, tmp_entry->mac, 6)) { 241 | obj->peer = tmp_entry->peer; 242 | break; 243 | } 244 | } 245 | return ret; 246 | } 247 | 248 | 249 | int mac_learning(struct process_handler *obj, struct frame *frame) 250 | { 251 | int ret = 0; 252 | struct mac_entry *entry = NULL; 253 | struct ethernet_header eh; 254 | struct list_head *tmp; 255 | 256 | memcpy(&eh, frame->data, sizeof(eh)); 257 | list_for_each(tmp, &obj->conf->macs) { 258 | struct mac_entry *tmp_entry = list_entry(tmp, struct mac_entry, node); 259 | if (!memcmp(eh.source, tmp_entry->mac, 6)) { 260 | entry = tmp_entry; 261 | break; 262 | } 263 | } 264 | 265 | if (entry) { 266 | list_del(&entry->list); 267 | list_del(&entry->node); 268 | } else { 269 | entry = (struct mac_entry *)calloc(1, sizeof(struct mac_entry)); 270 | } 271 | 272 | if (!entry) { 273 | printf("Alloc entry failed\n"); 274 | return -1; 275 | } 276 | 277 | memcpy(entry->mac, eh.source, 6); 278 | entry->peer = obj->peer; 279 | INIT_LIST_HEAD(&entry->list); 280 | list_add(&entry->list, &entry->peer->macs); 281 | INIT_LIST_HEAD(&entry->node); 282 | list_add(&entry->node, &obj->conf->macs); 283 | 284 | return ret; 285 | } 286 | static struct process_handler routing_handler = { 287 | .send = frame_routing, 288 | .receive = mac_learning, 289 | }; 290 | 291 | int encode_frame(struct process_handler *obj, struct frame *frame) 292 | { 293 | int ret = 0; 294 | 295 | frame->sid = obj->conf->self->tuple.id; 296 | frame->did = 0; 297 | if (obj->peer) { 298 | frame->did = obj->peer->tuple.id; 299 | } 300 | printf("####1000 encode_frame :%d %d\n", frame->sid, frame->did); 301 | 302 | return ret; 303 | } 304 | 305 | int decode_frame(struct process_handler *obj, struct frame *frame) 306 | { 307 | int ret = 0; 308 | 309 | printf("#### decode_frame: sid:%d did:%d\n", frame->sid, frame->did); 310 | if (frame->did != 0 && frame->did != obj->conf->self->tuple.id) { 311 | ret = -1; 312 | } 313 | 314 | return ret; 315 | } 316 | static struct process_handler protocol_handler = { 317 | .send = encode_frame, 318 | .receive = decode_frame, 319 | }; 320 | 321 | int encrypt(struct process_handler *obj, struct frame *frame) 322 | { 323 | int ret = 0; 324 | int i; 325 | char *c = (char *)frame; 326 | int len = sizeof(short)*2 + frame->len; 327 | for (i = 0; i < len; i++) { 328 | c[i] = c[i]+1; 329 | } 330 | return ret; 331 | } 332 | 333 | int decrypt(struct process_handler *obj, struct frame *frame) 334 | { 335 | int ret = 0; 336 | int i; 337 | char *c = (char *)frame; 338 | int len = frame->len; 339 | 340 | for (i = 0; i < len; i++) { 341 | c[i] = c[i]-1; 342 | } 343 | return ret; 344 | } 345 | 346 | static struct process_handler enc_handler = { 347 | .send = encrypt, 348 | .receive = decrypt, 349 | }; 350 | 351 | int send_frame(struct process_handler *obj, struct frame *frame) 352 | { 353 | int ret = 0; 354 | int fd = obj->conf->udp_fd; 355 | size_t len = 0; 356 | struct node_info *peer = obj->peer; 357 | struct config *conf = obj->conf; 358 | struct sockaddr_in addr; 359 | int addr_len = sizeof(struct sockaddr_in); 360 | 361 | bzero(&addr,sizeof(addr)); 362 | addr.sin_family = AF_INET; 363 | if (peer) { 364 | addr.sin_port = htons(peer->tuple.port); 365 | addr.sin_addr.s_addr = inet_addr(peer->tuple.addr); 366 | len = sendto(fd, frame, sizeof(short)*2+frame->len, 0, (struct sockaddr *)&addr, addr_len); 367 | } else { 368 | struct list_head *tmp; 369 | list_for_each(tmp, &conf->peers) { 370 | peer = list_entry(tmp, struct node_info, list); 371 | addr.sin_port = htons(peer->tuple.port); 372 | addr.sin_addr.s_addr = inet_addr(peer->tuple.addr); 373 | len = sendto(fd, frame, sizeof(short)*2+frame->len, 0, (struct sockaddr *)&addr, addr_len); 374 | } 375 | 376 | } 377 | return ret; 378 | } 379 | 380 | int receive_frame(struct process_handler *obj, struct frame *frame) 381 | { 382 | int ret = 0; 383 | size_t len = 0; 384 | struct list_head *tmp; 385 | struct node_info *peer; 386 | struct sockaddr_in addr; 387 | char *saddr; 388 | int port; 389 | int addr_len = sizeof(struct sockaddr_in); 390 | 391 | bzero (&addr, sizeof(addr)); 392 | 393 | len = recvfrom(obj->conf->udp_fd, frame, sizeof(struct frame), 0 , (struct sockaddr *)&addr ,&addr_len); 394 | frame->len = len; 395 | 396 | list_for_each(tmp, &obj->conf->peers) { 397 | peer = list_entry(tmp, struct node_info, list); 398 | saddr = inet_ntoa(addr.sin_addr); 399 | port = ntohs(addr.sin_port); 400 | if (!memcmp(saddr, peer->tuple.addr, strlen(saddr)) && port == peer->tuple.port) { 401 | obj->peer = peer; 402 | } 403 | } 404 | if (!obj->peer) { 405 | ret = -1; 406 | } 407 | return ret; 408 | } 409 | static struct process_handler udp_handler = { 410 | .send = send_frame, 411 | .receive = receive_frame, 412 | }; 413 | 414 | int register_handler(struct process_handler *handler, struct config *conf) 415 | { 416 | INIT_LIST_HEAD(&handler->list); 417 | handler->conf = conf; 418 | list_add_tail(&handler->list, &conf->stack); 419 | if (conf->first == NULL) { 420 | conf->first = &handler->list; 421 | } 422 | conf->last = &handler->list; 423 | return 0; 424 | } 425 | 426 | int unregister_handler(struct process_handler *handler, struct config *conf) 427 | { 428 | //TODO 429 | } 430 | 431 | int init_config(struct config *conf) 432 | { 433 | INIT_LIST_HEAD(&conf->stack); 434 | INIT_LIST_HEAD(&conf->peers); 435 | INIT_LIST_HEAD(&conf->macs); 436 | conf->first = NULL; 437 | conf->last = NULL; 438 | conf->tap_fd = -1; 439 | conf->udp_fd = -1; 440 | conf->self = NULL; 441 | } 442 | 443 | int init_self(struct config *conf, char *addr, unsigned short port) 444 | { 445 | int fd = -1; 446 | struct sockaddr_in saddr; 447 | struct node_info *self; 448 | 449 | self = (struct node_info *)calloc(1, sizeof(struct node_info)); 450 | if (self == NULL) { 451 | exit(-1); 452 | } 453 | conf->self = self; 454 | 455 | strcpy(conf->self->tuple.addr, addr); 456 | conf->self->tuple.port = port; 457 | if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0){ 458 | exit (-1); 459 | } 460 | 461 | bzero(&saddr, sizeof(saddr)); 462 | saddr.sin_family = AF_INET; 463 | saddr.sin_port = htons(conf->self->tuple.port); 464 | saddr.sin_addr.s_addr = inet_addr(conf->self->tuple.addr); 465 | if (bind(fd, (struct sockaddr *)&saddr, sizeof(saddr))<0){ 466 | perror("connect"); 467 | exit(1); 468 | } 469 | 470 | conf->udp_fd = fd; 471 | } 472 | 473 | int init_tap(struct config *conf) 474 | { 475 | int fd = -1; 476 | struct ifreq ifr; 477 | 478 | 479 | if( (fd = open("/dev/net/tun", O_RDWR)) < 0) { 480 | exit(-1); 481 | } 482 | memset(&ifr, 0, sizeof(ifr)); 483 | ifr.ifr_flags |= IFF_NO_PI; 484 | ifr.ifr_flags |= IFF_TAP; 485 | snprintf(ifr.ifr_name, IFNAMSIZ, "%s", "tap0"); 486 | ioctl(fd, TUNSETIFF, (void *)&ifr); 487 | 488 | conf->tap_fd = fd; 489 | } 490 | 491 | int init_server_connect(struct config *conf, char *addr, unsigned short port) 492 | { 493 | int ret = 0; 494 | int fd = -1; 495 | struct sockaddr_in srv_addr; 496 | 497 | if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0){ 498 | perror("socket"); 499 | exit (-1); 500 | } 501 | 502 | bzero(&srv_addr, sizeof(srv_addr)); 503 | srv_addr.sin_family = AF_INET; 504 | srv_addr.sin_port = htons(port); 505 | srv_addr.sin_addr.s_addr = inet_addr(addr); 506 | 507 | if (connect(fd, (struct sockaddr*)&srv_addr, sizeof(srv_addr)) == -1) { 508 | perror("connect"); 509 | exit (-1); 510 | } 511 | 512 | conf->ctrl_fd = fd; 513 | return ret; 514 | } 515 | 516 | int main_loop(struct config *conf) 517 | { 518 | int ret = 0; 519 | fd_set rd_set; 520 | int max = conf->tap_fd; 521 | 522 | if (conf->ctrl_fd > conf->tap_fd) { 523 | max = conf->ctrl_fd; 524 | } 525 | if (max < conf->udp_fd) { 526 | max = conf->udp_fd; 527 | } 528 | 529 | while(1) { 530 | int nfds; 531 | int i; 532 | FD_ZERO(&rd_set); 533 | FD_SET(conf->ctrl_fd, &rd_set); 534 | FD_SET(conf->tap_fd, &rd_set); 535 | FD_SET(conf->udp_fd, &rd_set); 536 | 537 | nfds = select(max+1, &rd_set, NULL, NULL, NULL); 538 | 539 | for (i = 0;i < nfds; i++) { 540 | if(FD_ISSET(conf->ctrl_fd, &rd_set)) { 541 | server_msg_read(conf); 542 | } 543 | if(FD_ISSET(conf->tap_fd, &rd_set)) { 544 | call_stack(conf, 1); 545 | } 546 | if(FD_ISSET(conf->udp_fd, &rd_set)) { 547 | call_stack(conf, 0); 548 | } 549 | } 550 | } 551 | return ret; 552 | } 553 | 554 | int main(int argc, char **argv) 555 | { 556 | char serverIP[16]; 557 | char localIP[16]; 558 | unsigned short serverPORT; 559 | unsigned short localPORT; 560 | struct config conf; 561 | 562 | if (argc != 5) { 563 | printf("./a.out serverIP serverPORT localIP localPORT\n"); 564 | } 565 | strcpy(serverIP, argv[1]); 566 | serverPORT = atoi(argv[2]); 567 | strcpy(localIP, argv[3]); 568 | localPORT = atoi(argv[4]); 569 | 570 | init_config(&conf); 571 | init_tap(&conf); 572 | init_self(&conf, localIP, localPORT); 573 | 574 | register_handler(&tap_handler, &conf); 575 | register_handler(&routing_handler, &conf); 576 | register_handler(&protocol_handler, &conf); 577 | register_handler(&enc_handler, &conf); 578 | register_handler(&udp_handler, &conf); 579 | 580 | init_server_connect(&conf, serverIP, serverPORT); 581 | 582 | server_msg_register(&conf); 583 | server_msg_read(&conf); 584 | 585 | main_loop(&conf); 586 | 587 | return 0; 588 | } 589 | -------------------------------------------------------------------------------- /list.h: -------------------------------------------------------------------------------- 1 | // From Linux kernel 2.6.32 2 | 3 | struct list_head; 4 | struct list_head { 5 | struct list_head *next, *prev; 6 | }; 7 | 8 | void __list_add(struct list_head *new, 9 | struct list_head *prev, 10 | struct list_head *next) 11 | { 12 | next->prev = new; 13 | new->next = next; 14 | new->prev = prev; 15 | prev->next = new; 16 | } 17 | 18 | void list_add_tail(struct list_head *new, struct list_head *head) 19 | { 20 | __list_add(new, head->prev, head); 21 | } 22 | 23 | void list_add(struct list_head *new, struct list_head *head) 24 | { 25 | __list_add(new, head, head->next); 26 | } 27 | 28 | void __list_del(struct list_head * prev, struct list_head * next) 29 | { 30 | next->prev = prev; 31 | prev->next = next; 32 | } 33 | 34 | #define LIST_POISON1 ((void *) 0x00100100) 35 | #define LIST_POISON2 ((void *) 0x00200200) 36 | 37 | void list_del(struct list_head *entry) 38 | { 39 | __list_del(entry->prev, entry->next); 40 | entry->next = LIST_POISON1; 41 | entry->prev = LIST_POISON2; 42 | } 43 | 44 | void INIT_LIST_HEAD(struct list_head *list) 45 | { 46 | list->next = list; 47 | list->prev = list; 48 | } 49 | 50 | #define list_for_each(pos, head) \ 51 | for (pos = (head)->next; pos != (head); \ 52 | pos = pos->next) 53 | 54 | #define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER) 55 | 56 | #define container_of(ptr, type, member) ({ \ 57 | const typeof( ((type *)0)->member ) *__mptr = (ptr); \ 58 | (type *)( (char *)__mptr - offsetof(type,member) );}) 59 | 60 | #define list_entry(ptr, type, member) \ 61 | container_of(ptr, type, member) 62 | --------------------------------------------------------------------------------