├── .gitignore ├── LICENSE ├── Makefile ├── README.md ├── Vagrantfile ├── apps └── tcp_echo.c ├── arp.c ├── arp.h ├── ethernet.c ├── ethernet.h ├── ip.c ├── ip.h ├── net.c ├── net.h ├── raw.c ├── raw.h ├── raw ├── soc.c ├── soc.h ├── tap.h └── tap_linux.c ├── tcp.c ├── tcp.h ├── test ├── ethernet_test.c ├── ip_test.c ├── mask_test.c ├── queue_test.c ├── raw_soc_test.c ├── raw_tap_test.c ├── raw_test.c ├── tcp_listen_test.c └── tcp_test.c ├── util.c └── util.h /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | .vagrant/ 3 | .ssh-config 4 | *.o 5 | 6 | test/*_test 7 | apps/tcp_echo 8 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2019 Kawamura Shintaro 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWARE. 20 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | APPS = apps/tcp_echo 2 | TEST = test/raw_test test/ethernet_test test/ip_test test/mask_test \ 3 | test/tcp_test test/tcp_listen_test test/queue_test 4 | OBJS = raw.o util.o ethernet.o net.o ip.o arp.o tcp.o 5 | CFLAGS := $(CFLAGS) -g -W -Wall -Wno-unused-parameter -I . -DTCP_DEBUG -g 6 | 7 | ifeq ($(shell uname), Linux) 8 | OBJS := $(OBJS) raw/soc.o raw/tap_linux.o 9 | TEST := $(TEST) test/raw_soc_test test/raw_tap_test 10 | CFLAGS := $(CFLAGS) -pthread -DHAVE_PF_PACKET -DHAVE_TAP 11 | endif 12 | 13 | .PHONY: all clean 14 | 15 | all: $(TEST) $(APPS) 16 | 17 | $(APPS): % : %.o $(OBJS) 18 | $(CC) $(CFLAGS) -o $@ $^ 19 | 20 | $(TEST): % : %.o $(OBJS) 21 | $(CC) $(CFLAGS) -o $@ $^ 22 | 23 | .c.o: 24 | $(CC) $(CFLAGS) -c $< -o $@ 25 | 26 | clean: 27 | rm -rf $(APPS) $(APPS:=.o) $(TEST) $(TEST:=.o) $(OBJS) 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TCP/IP stack 2 | 3 | Create TCP/IP stack from scratch. 4 | 5 | This is based on [pandax381/microps](https://github.com/pandax381/microps). 6 | 7 | `tcp.c` is based on [RFC 793 - Transmission Control Protocol](https://tools.ietf.org/html/rfc793#page-52). 8 | 9 | ## How to use 10 | 11 | Use tap device on Linux on VirtualBox 12 | 13 | ```bash 14 | # use brctl to create bridge 15 | $ sudo apt install -y bridge-utils 16 | 17 | # create br0 and connect it to eth1 18 | $ sudo brctl addbr br0 19 | $ sudo brctl addif br0 eth1 20 | $ sudo ip link set br0 up 21 | $ sudo ip addr add dev br0 192.168.33.12/24 22 | 23 | # create apps/tcp_echo and test binary at /vagrant 24 | $ cd /vagrant 25 | $ make clean && make 26 | 27 | # start tcp echo server 28 | $ sudo apps/tcp_echo 29 | 30 | # connect created tap1 device to br0 31 | $ sudo brctl addif br0 tap2 && sudo ip link set tap2 up && sudo arp -d 192.168.33.13 32 | 33 | # connect using telnet 34 | $ telnet 192.168.33.13 20000 35 | ``` 36 | 37 | ## Features 38 | 39 | - [x] Raw Device 40 | - [x] tap device on Linux 41 | - [x] PF_PACKET socket on Linux 42 | - [ ] tap device on BSD 43 | - [ ] BFP on BSD 44 | - [x] Ethernet 45 | - [x] ARP 46 | - [x] IP 47 | - [x] ip_tx 48 | - [x] ip_rx 49 | - [x] Fragmentation 50 | - [x] Checksum 51 | - [ ] Routing 52 | - [ ] Packet Forwarding 53 | - [ ] Dynamic network device selection by IP Address 54 | - [ ] ICMP 55 | - [ ] DHCP 56 | - [x] TCP 57 | - [x] Socket API (open, connect, bind, listen, accept, send, recv) 58 | - [ ] Blocking I/O API 59 | - [ ] Non-blocing I/O API 60 | - [ ] Event Driven Architecture API (like select, poll, epoll, kqueue...) 61 | - [x] Timeout 62 | - [x] User Timeout 63 | - [x] Retransmission Timeout 64 | - [x] TIME WAIT Timeout 65 | - [x] Connection Control 66 | - [x] Passive Open 67 | - [x] Backlog 68 | - [ ] SYN-Backlog 69 | - [x] Active Open 70 | - [x] Close Connection 71 | - [x] Send Data 72 | - [x] Data Segmentation by MTU 73 | - [x] Retransmission 74 | - [x] Flow Control 75 | - [ ] Congestion Control 76 | - [x] Receive Data 77 | - [x] Reply ACK 78 | - [ ] Partial ACK 79 | - [ ] Sequential ID Rotation 80 | - [ ] URG Pointer 81 | - [ ] Precedence and Security 82 | - [ ] UDP 83 | - [ ] Payload Passing over layers with Zero Copy 84 | 85 | ## Development on VSCode 86 | 87 | I use macOS and create Ubuntu 18.04 VM using vagrant. 88 | I use VSCode as editor and Remote SSH plugin. 89 | 90 | ```bash 91 | $ vagrant up 92 | $ vagrant ssh-config > .ssh-config 93 | ``` 94 | 95 | Setup VSCode Remote SSH with `.ssh-config` and login. 96 | Then re-install VSCode plugins in remote VSCode. 97 | 98 | ## LICENSE 99 | 100 | MIT 101 | -------------------------------------------------------------------------------- /Vagrantfile: -------------------------------------------------------------------------------- 1 | # -*- mode: ruby -*- 2 | # vi: set ft=ruby : 3 | 4 | # All Vagrant configuration is done below. The "2" in Vagrant.configure 5 | # configures the configuration version (we support older styles for 6 | # backwards compatibility). Please don't change it unless you know what 7 | # you're doing. 8 | Vagrant.configure("2") do |config| 9 | # The most common configuration options are documented and commented below. 10 | # For a complete reference, please see the online documentation at 11 | # https://docs.vagrantup.com. 12 | 13 | # Every Vagrant development environment requires a box. You can search for 14 | # boxes at https://vagrantcloud.com/search. 15 | config.vm.box = "bento/ubuntu-18.04" 16 | 17 | # Disable automatic box update checking. If you disable this, then 18 | # boxes will only be checked for updates when the user runs 19 | # `vagrant box outdated`. This is not recommended. 20 | # config.vm.box_check_update = false 21 | 22 | # Create a forwarded port mapping which allows access to a specific port 23 | # within the machine from a port on the host machine. In the example below, 24 | # accessing "localhost:8080" will access port 80 on the guest machine. 25 | # NOTE: This will enable public access to the opened port 26 | # config.vm.network "forwarded_port", guest: 80, host: 8080 27 | 28 | # Create a forwarded port mapping which allows access to a specific port 29 | # within the machine from a port on the host machine and only allow access 30 | # via 127.0.0.1 to disable public access 31 | # config.vm.network "forwarded_port", guest: 80, host: 8080, host_ip: "127.0.0.1" 32 | 33 | # Create a private network, which allows host-only access to the machine 34 | # using a specific IP. 35 | # config.vm.network "private_network", ip: "192.168.33.10" 36 | config.vm.network "private_network", ip: "192.168.33.10" 37 | 38 | # Create a public network, which generally matched to bridged network. 39 | # Bridged networks make the machine appear as another physical device on 40 | # your network. 41 | # config.vm.network "public_network" 42 | 43 | # Share an additional folder to the guest VM. The first argument is 44 | # the path on the host to the actual folder. The second argument is 45 | # the path on the guest to mount the folder. And the optional third 46 | # argument is a set of non-required options. 47 | # config.vm.synced_folder "../data", "/vagrant_data" 48 | 49 | # Provider-specific configuration so you can fine-tune various 50 | # backing providers for Vagrant. These expose provider-specific options. 51 | # Example for VirtualBox: 52 | # 53 | # config.vm.provider "virtualbox" do |vb| 54 | # # Display the VirtualBox GUI when booting the machine 55 | # vb.gui = true 56 | # 57 | # # Customize the amount of memory on the VM: 58 | # vb.memory = "1024" 59 | # end 60 | # 61 | # View the documentation for the provider you are using for more 62 | # information on available options. 63 | config.vm.provider "virtualbox" do |vb| 64 | vb.customize ["modifyvm", :id, "--nicpromisc2", "allow-all"] 65 | end 66 | 67 | # Enable provisioning with a shell script. Additional provisioners such as 68 | # Puppet, Chef, Ansible, Salt, and Docker are also available. Please see the 69 | # documentation for more information about their specific syntax and use. 70 | # config.vm.provision "shell", inline: <<-SHELL 71 | # apt-get update 72 | # apt-get install -y apache2 73 | # SHELL 74 | config.vm.provision "shell", inline: <<-SHELL 75 | apt-get update 76 | apt-get install -y git build-essential gdb 77 | SHELL 78 | end 79 | -------------------------------------------------------------------------------- /apps/tcp_echo.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "arp.h" 6 | #include "ethernet.h" 7 | #include "ip.h" 8 | #include "net.h" 9 | #include "raw.h" 10 | #include "tcp.h" 11 | #include "util.h" 12 | 13 | static int setup(void) { 14 | if (ethernet_init() == -1) { 15 | fprintf(stderr, "ethernet_init(): failure\n"); 16 | return -1; 17 | } else if (ip_init() == -1) { 18 | fprintf(stderr, "ip_init(): failure\n"); 19 | return -1; 20 | } else if (arp_init() == -1) { 21 | fprintf(stderr, "arp_init(): failure\n"); 22 | return -1; 23 | } else if (tcp_init() == -1) { 24 | fprintf(stderr, "tcp_init(): failure\n"); 25 | return -1; 26 | } 27 | return 0; 28 | } 29 | 30 | static void *handler(void *arg) { 31 | int soc = arg, n; 32 | uint8_t buf[1024]; 33 | 34 | while (1) { 35 | n = tcp_api_recv(soc, buf, 1024); 36 | if (n == -1) { 37 | fprintf(stderr, "tcp_api_recv: failed\n"); 38 | break; 39 | } else if (n == 0) { 40 | fprintf(stderr, "tcp_api_recv: size is 0\n"); 41 | } else { 42 | fprintf(stderr, ">>> recv data <<<\n"); 43 | hexdump(stderr, buf, n); 44 | } 45 | 46 | n = tcp_api_send(soc, buf, n); 47 | if (n == -1) { 48 | fprintf(stderr, "tcp_api_send: failed\n"); 49 | break; 50 | } else { 51 | fprintf(stderr, ">>> send data success <<<\n"); 52 | } 53 | if (n >= 4 && strncmp("quit", buf, 4) == 0) { 54 | break; 55 | } 56 | } 57 | 58 | if (tcp_api_close(soc) == -1) { 59 | fprintf(stderr, "tcp_api_close: failed\n"); 60 | } else { 61 | fprintf(stderr, "tcp_api_close: success\n"); 62 | } 63 | 64 | return NULL; 65 | } 66 | 67 | static void *accept_handler(void *arg) { 68 | int listener = arg, soc; 69 | pthread_t th; 70 | 71 | while (1) { 72 | soc = tcp_api_accept(listener); 73 | if (soc == -1) { 74 | fprintf(stderr, "tcp_api_accept: failed\n"); 75 | return NULL; 76 | } 77 | fprintf(stderr, "tcp_api_accept success: soc : %d\n", soc); 78 | 79 | pthread_create(&th, NULL, handler, soc); 80 | } 81 | 82 | return NULL; 83 | } 84 | 85 | int main(int argc, char const *argv[]) { 86 | sigset_t sigset; 87 | int signo; 88 | struct netdev *dev; 89 | struct netif *netif; 90 | char *name = "tap2", *ipaddr = "192.168.33.13", *netmask = "255.255.0.0"; 91 | int listener; 92 | uint8_t buf[1024]; 93 | size_t n; 94 | pthread_t th; 95 | 96 | sigemptyset(&sigset); 97 | sigaddset(&sigset, SIGINT); 98 | sigprocmask(SIG_BLOCK, &sigset, NULL); 99 | if (setup() == -1) { 100 | return -1; 101 | } 102 | 103 | // setup 104 | dev = netdev_alloc(NETDEV_TYPE_ETHERNET); 105 | if (!dev) { 106 | fprintf(stderr, "netdev_alloc() : failed\n"); 107 | return -1; 108 | } 109 | strncpy(dev->name, name, sizeof(dev->name) - 1); 110 | if (dev->ops->open(dev, RAWDEV_TYPE_AUTO) == -1) { 111 | fprintf(stderr, "failed to open raw device\n"); 112 | return -1; 113 | } 114 | 115 | // set netif 116 | netif = ip_netif_register(dev, ipaddr, netmask, NULL); 117 | 118 | dev->ops->run(dev); 119 | 120 | listener = tcp_api_open(); 121 | if (listener == -1) { 122 | fprintf(stderr, "tcp_api_open: failed\n"); 123 | return -1; 124 | } 125 | 126 | if (tcp_api_bind(listener, 20000) == -1) { 127 | fprintf(stderr, "tcp_api_bind: failed\n"); 128 | return -1; 129 | } 130 | if (tcp_api_listen(listener) == -1) { 131 | fprintf(stderr, "tcp_api_listen: failed\n"); 132 | return -1; 133 | } 134 | 135 | fprintf(stderr, "tcp_api_listen success\n"); 136 | 137 | pthread_create(&th, NULL, accept_handler, listener); 138 | 139 | while (1) { 140 | sigwait(&sigset, &signo); 141 | if (signo == SIGINT) { 142 | break; 143 | } 144 | } 145 | 146 | if (tcp_api_close(listener) == -1) { 147 | fprintf(stderr, "tcp_api_close: failed\n"); 148 | return -1; 149 | } 150 | 151 | if (dev->ops->close) { 152 | dev->ops->close(dev); 153 | } 154 | fprintf(stderr, "closed\n"); 155 | return 0; 156 | } 157 | -------------------------------------------------------------------------------- /arp.c: -------------------------------------------------------------------------------- 1 | #include "arp.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "ethernet.h" 10 | #include "ip.h" 11 | #include "net.h" 12 | #include "util.h" 13 | 14 | #define ARP_HRD_ETHERNET 0x0001 15 | 16 | #define ARP_OP_REQUEST 1 17 | #define ARP_OP_REPLY 2 18 | 19 | #define ARP_TABLE_SIZE 4096 20 | #define ARP_TABLE_TIMEOUT_SEC 300 21 | 22 | struct arp_hdr { 23 | uint16_t hrd; 24 | uint16_t pro; 25 | uint8_t hln; 26 | uint8_t pln; 27 | uint16_t op; 28 | }; 29 | 30 | struct arp_ethernet { 31 | struct arp_hdr hdr; 32 | uint8_t sha[ETHERNET_ADDR_LEN]; 33 | ip_addr_t spa; 34 | uint8_t tha[ETHERNET_ADDR_LEN]; 35 | ip_addr_t tpa; 36 | } __attribute__((packed)); 37 | 38 | struct arp_entry { 39 | unsigned char used; 40 | ip_addr_t pa; 41 | uint8_t ha[ETHERNET_ADDR_LEN]; 42 | time_t timestamp; 43 | pthread_cond_t cond; 44 | void *data; 45 | size_t len; 46 | struct netif *netif; 47 | }; 48 | 49 | static struct arp_entry arp_table[ARP_TABLE_SIZE]; 50 | static time_t timestamp; 51 | static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; 52 | 53 | static char *arp_opcode_ntop(uint16_t opcode) { 54 | switch (ntoh16(opcode)) { 55 | case ARP_OP_REQUEST: 56 | return "REQUEST"; 57 | case ARP_OP_REPLY: 58 | return "REPLY"; 59 | default: 60 | return "UNKNOWN"; 61 | } 62 | } 63 | 64 | static void arp_dump(uint8_t *packet, size_t plen) { 65 | struct arp_ethernet *message; 66 | char addr[128]; 67 | 68 | message = (struct arp_ethernet *)packet; 69 | fprintf(stderr, " hrd: 0x%04x\n", ntoh16(message->hdr.hrd)); 70 | fprintf(stderr, " pro: 0x%04x\n", ntoh16(message->hdr.pro)); 71 | fprintf(stderr, " hln: %u\n", message->hdr.hln); 72 | fprintf(stderr, " pln: %u\n", message->hdr.pln); 73 | fprintf(stderr, " op: %u (%s)\n", ntoh16(message->hdr.op), 74 | arp_opcode_ntop(message->hdr.op)); 75 | fprintf(stderr, " sha: %s\n", 76 | ethernet_addr_ntop(message->sha, addr, sizeof(addr))); 77 | fprintf(stderr, " spa: %s\n", 78 | ip_addr_ntop(&message->spa, addr, sizeof(addr))); 79 | fprintf(stderr, " tha: %s\n", 80 | ethernet_addr_ntop(message->tha, addr, sizeof(addr))); 81 | fprintf(stderr, " tpa: %s\n", 82 | ip_addr_ntop(&message->tpa, addr, sizeof(addr))); 83 | } 84 | 85 | /* 86 | * CONTROL ARP TABLE ENTRY 87 | */ 88 | 89 | static struct arp_entry *arp_table_freespace(void) { 90 | struct arp_entry *entry; 91 | int i; 92 | 93 | for (i = 0; i < ARP_TABLE_SIZE; i++) { 94 | entry = &arp_table[i]; 95 | if (!entry->used) { 96 | return entry; 97 | } 98 | } 99 | // TODO: remove unused entry with LRU 100 | return NULL; 101 | } 102 | 103 | static int arp_table_insert(const ip_addr_t *pa, const uint8_t *ha) { 104 | struct arp_entry *entry; 105 | 106 | entry = arp_table_freespace(); 107 | if (!entry) { 108 | return -1; 109 | } 110 | entry->used = 1; 111 | entry->pa = *pa; 112 | memcpy(entry->ha, ha, ETHERNET_ADDR_LEN); 113 | time(&entry->timestamp); 114 | pthread_cond_broadcast(&entry->cond); 115 | return 0; 116 | } 117 | 118 | static struct arp_entry *arp_table_select(const ip_addr_t *pa) { 119 | struct arp_entry *entry; 120 | int i; 121 | 122 | for (i = 0; i < ARP_TABLE_SIZE; i++) { 123 | entry = &arp_table[i]; 124 | if (entry->used && entry->pa == *pa) { 125 | return entry; 126 | } 127 | } 128 | return NULL; 129 | } 130 | 131 | static int arp_table_update(struct netdev *dev, const ip_addr_t *pa, 132 | const uint8_t *ha) { 133 | struct arp_entry *entry; 134 | 135 | // find entry 136 | entry = arp_table_select(pa); 137 | if (!entry) { 138 | return -1; 139 | } 140 | 141 | // set resolved 142 | memcpy(entry->ha, ha, ETHERNET_ADDR_LEN); 143 | time(&entry->timestamp); 144 | 145 | // send saved packet data with resolved hardware address 146 | if (entry->data) { 147 | if (entry->netif->dev != dev) { 148 | fprintf(stderr, "[warning] receive response from unintended device\n"); 149 | dev = entry->netif->dev; 150 | } 151 | dev->ops->tx(dev, ETHERNET_TYPE_IP, (uint8_t *)entry->data, entry->len, 152 | entry->ha); 153 | free(entry->data); 154 | entry->data = NULL; 155 | entry->len = 0; 156 | } 157 | 158 | pthread_cond_broadcast(&entry->cond); 159 | return 0; 160 | } 161 | 162 | static void arp_entry_clear(struct arp_entry *entry) { 163 | entry->used = 0; 164 | entry->pa = 0; 165 | memset(entry->ha, 0, ETHERNET_ADDR_LEN); 166 | entry->timestamp = 0; 167 | if (entry->data) { 168 | free(entry->data); 169 | entry->data = NULL; 170 | entry->len = 0; 171 | } 172 | entry->netif = NULL; 173 | } 174 | 175 | /* 176 | * ARP COMMUNICATION 177 | */ 178 | 179 | static int arp_send_request(struct netif *netif, const ip_addr_t *tpa) { 180 | struct arp_ethernet request; 181 | 182 | if (!tpa) { 183 | return -1; 184 | } 185 | // set arp reqeust parameters 186 | request.hdr.hrd = hton16(ARP_HRD_ETHERNET); 187 | request.hdr.pro = hton16(ETHERNET_TYPE_IP); 188 | request.hdr.hln = ETHERNET_ADDR_LEN; 189 | request.hdr.pln = IP_ADDR_LEN; 190 | request.hdr.op = hton16(ARP_OP_REQUEST); 191 | memcpy(request.sha, netif->dev->addr, ETHERNET_ADDR_LEN); 192 | request.spa = ((struct netif_ip *)netif)->unicast; 193 | memset(request.tha, 0, ETHERNET_ADDR_LEN); 194 | request.tpa = *tpa; 195 | 196 | #ifdef DEBUG 197 | fprintf(stderr, ">>> arp_send_request <<<\n"); 198 | arp_dump((uint8_t *)&request, sizeof(request)); 199 | #endif 200 | 201 | if (netif->dev->ops->tx(netif->dev, ETHERNET_TYPE_ARP, (uint8_t *)&request, 202 | sizeof(request), ETHERNET_ADDR_BROADCAST) == -1) { 203 | return -1; 204 | } 205 | return 0; 206 | } 207 | 208 | static int arp_send_reply(struct netif *netif, const uint8_t *tha, 209 | const ip_addr_t *tpa, const uint8_t *dst) { 210 | struct arp_ethernet reply; 211 | 212 | if (!tha || !tpa) { 213 | return -1; 214 | } 215 | reply.hdr.hrd = hton16(ARP_HRD_ETHERNET); 216 | reply.hdr.pro = hton16(ETHERNET_TYPE_IP); 217 | reply.hdr.hln = ETHERNET_ADDR_LEN; 218 | reply.hdr.pln = IP_ADDR_LEN; 219 | reply.hdr.op = hton16(ARP_OP_REPLY); 220 | memcpy(reply.sha, netif->dev->addr, ETHERNET_ADDR_LEN); 221 | reply.spa = ((struct netif_ip *)netif)->unicast; 222 | memcpy(reply.tha, tha, ETHERNET_ADDR_LEN); 223 | reply.tpa = *tpa; 224 | 225 | #ifdef DEBUG 226 | fprintf(stderr, ">>> arp_send_reply <<<\n"); 227 | arp_dump((uint8_t *)&reply, sizeof(reply)); 228 | #endif 229 | 230 | if (netif->dev->ops->tx(netif->dev, ETHERNET_TYPE_ARP, (uint8_t *)&reply, 231 | sizeof(reply), dst) < 0) { 232 | return -1; 233 | } 234 | return 0; 235 | } 236 | 237 | /* 238 | * ARP INTERFACES 239 | */ 240 | 241 | static void arp_table_patrol(void) { 242 | struct arp_entry *entry; 243 | int i; 244 | 245 | for (i = 0; i < ARP_TABLE_SIZE; i++) { 246 | entry = &arp_table[i]; 247 | if (entry->used && timestamp - entry->timestamp > ARP_TABLE_TIMEOUT_SEC) { 248 | arp_entry_clear(entry); 249 | pthread_cond_broadcast(&entry->cond); 250 | } 251 | } 252 | } 253 | 254 | static void arp_rx(uint8_t *packet, size_t plen, struct netdev *dev) { 255 | struct arp_ethernet *message; 256 | time_t now; 257 | int marge = 0; 258 | struct netif *netif; 259 | 260 | // validate length 261 | if (plen < sizeof(struct arp_ethernet)) { 262 | return; 263 | } 264 | 265 | // validate ARP message 266 | message = (struct arp_ethernet *)packet; 267 | if (ntoh16(message->hdr.hrd) != ARP_HRD_ETHERNET) { 268 | return; 269 | } 270 | if (ntoh16(message->hdr.pro) != ETHERNET_TYPE_IP) { 271 | return; 272 | } 273 | if (message->hdr.hln != ETHERNET_ADDR_LEN) { 274 | return; 275 | } 276 | if (message->hdr.pln != IP_ADDR_LEN) { 277 | return; 278 | } 279 | 280 | #ifdef DEBUG 281 | fprintf(stderr, ">>> arp_rx <<<\n"); 282 | arp_dump(packet, plen); 283 | #endif 284 | 285 | pthread_mutex_lock(&mutex); 286 | time(&now); 287 | if (now - timestamp > 10) { 288 | timestamp = now; 289 | arp_table_patrol(); 290 | } 291 | 292 | // update arp table entry 293 | marge = (arp_table_update(dev, &message->spa, message->sha) == 0) ? 1 : 0; 294 | pthread_mutex_unlock(&mutex); 295 | 296 | // save arp message if target is this machine 297 | netif = netdev_get_netif(dev, NETIF_FAMILY_IPV4); 298 | if (netif && ((struct netif_ip *)netif)->unicast == message->tpa) { 299 | if (!marge) { 300 | pthread_mutex_lock(&mutex); 301 | // TODO: if there is no space 302 | // TODO: Resilient for DoS attack 303 | arp_table_insert(&message->spa, message->sha); 304 | pthread_mutex_unlock(&mutex); 305 | } 306 | if (ntoh16(message->hdr.op) == ARP_OP_REQUEST) { 307 | arp_send_reply(netif, message->sha, &message->spa, message->sha); 308 | } 309 | } 310 | return; 311 | } 312 | 313 | int arp_resolve(struct netif *netif, const ip_addr_t *pa, uint8_t *ha, 314 | const void *data, size_t len) { 315 | struct timeval now; 316 | struct timespec timeout; 317 | struct arp_entry *entry; 318 | int ret; 319 | 320 | pthread_mutex_lock(&mutex); 321 | 322 | // set timeout 323 | gettimeofday(&now, NULL); 324 | timeout.tv_sec = now.tv_sec + 1; 325 | timeout.tv_nsec = now.tv_usec * 1000; 326 | 327 | entry = arp_table_select(pa); 328 | if (entry) { 329 | if (memcmp(entry->ha, ETHERNET_ADDR_ANY, ETHERNET_ADDR_LEN) == 0) { 330 | // apr request has already sent. wait for reply 331 | // resend arp request for the case packet loss 332 | arp_send_request(netif, pa); 333 | do { 334 | // wait until reply come with timeout 335 | ret = pthread_cond_timedwait(&entry->cond, &mutex, &timeout); 336 | } while (ret == EINTR); 337 | if (!entry->used || ret == ETIMEDOUT) { 338 | if (entry->used) { 339 | arp_entry_clear(entry); 340 | } 341 | pthread_mutex_unlock(&mutex); 342 | return ARP_RESOLVE_ERROR; 343 | } 344 | } 345 | memcpy(ha, entry->ha, ETHERNET_ADDR_LEN); 346 | pthread_mutex_unlock(&mutex); 347 | return ARP_RESOLVE_FOUND; 348 | } 349 | 350 | // create arp table entry 351 | entry = arp_table_freespace(); 352 | if (!entry) { 353 | pthread_mutex_unlock(&mutex); 354 | return ARP_RESOLVE_ERROR; 355 | } 356 | 357 | // save data 358 | if (data) { 359 | entry->data = malloc(len); 360 | if (!entry->data) { 361 | pthread_mutex_unlock(&mutex); 362 | return ARP_RESOLVE_ERROR; 363 | } 364 | memcpy(entry->data, data, len); 365 | entry->len = len; 366 | } 367 | 368 | // set arp entry 369 | entry->used = 1; 370 | entry->pa = *pa; 371 | time(&entry->timestamp); 372 | entry->netif = netif; 373 | 374 | // send arp query request 375 | arp_send_request(netif, pa); 376 | 377 | pthread_mutex_unlock(&mutex); 378 | return ARP_RESOLVE_QUERY; 379 | } 380 | 381 | int arp_init(void) { 382 | int i; 383 | 384 | time(×tamp); 385 | for (i = 0; i < ARP_TABLE_SIZE; i++) { 386 | pthread_cond_init(&arp_table[i].cond, NULL); 387 | } 388 | netdev_proto_register(NETDEV_PROTO_ARP, arp_rx); 389 | return 0; 390 | } 391 | -------------------------------------------------------------------------------- /arp.h: -------------------------------------------------------------------------------- 1 | #ifndef _ARP_H_ 2 | #define _ARP_H_ 3 | 4 | #include 5 | #include "ip.h" 6 | #include "net.h" 7 | 8 | #define ARP_RESOLVE_ERROR -1 9 | #define ARP_RESOLVE_QUERY 0 10 | #define ARP_RESOLVE_FOUND 1 11 | 12 | int arp_init(void); 13 | int arp_resolve(struct netif *netif, const ip_addr_t *pa, uint8_t *ha, 14 | const void *data, size_t len); 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /ethernet.c: -------------------------------------------------------------------------------- 1 | #include "ethernet.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "net.h" 8 | #include "raw.h" 9 | #include "util.h" 10 | 11 | struct ethernet_hdr { 12 | uint8_t dst[ETHERNET_ADDR_LEN]; 13 | uint8_t src[ETHERNET_ADDR_LEN]; 14 | uint16_t type; 15 | }; 16 | 17 | struct ethernet_priv { 18 | struct netdev *dev; 19 | struct rawdev *raw; 20 | pthread_t thread; 21 | int terminate; 22 | }; 23 | 24 | const uint8_t ETHERNET_ADDR_ANY[ETHERNET_ADDR_LEN] = { 25 | "\x00\x00\x00\x00\x00\x00"}; 26 | const uint8_t ETHERNET_ADDR_BROADCAST[ETHERNET_ADDR_LEN] = { 27 | "\xff\xff\xff\xff\xff\xff"}; 28 | 29 | int ethernet_addr_pton(const char *p, uint8_t *n) { 30 | int index; 31 | char *ep; 32 | long val; 33 | 34 | if (!p || !n) { 35 | return -1; 36 | } 37 | 38 | // decode MAC address 39 | for (index = 0; index < ETHERNET_ADDR_LEN; index++) { 40 | val = strtol(p, &ep, 16); 41 | if (ep == p || val < 0 || val > 0xff || 42 | (index < ETHERNET_ADDR_LEN - 1 && *ep != ':')) { 43 | break; 44 | } 45 | n[index] = (uint8_t)val; 46 | // skip ":" 47 | p = ep + 1; 48 | } 49 | if (index != ETHERNET_ADDR_LEN || *ep != '\0') { 50 | return -1; 51 | } 52 | return 0; 53 | } 54 | 55 | char *ethernet_addr_ntop(const uint8_t *n, char *p, size_t size) { 56 | if (!n || !p) { 57 | return NULL; 58 | } 59 | snprintf(p, size, "%02x:%02x:%02x:%02x:%02x:%02x", n[0], n[1], n[2], n[3], 60 | n[4], n[5]); 61 | return p; 62 | } 63 | 64 | int ethernet_open(struct netdev *dev, int opt) { 65 | struct ethernet_priv *priv; 66 | struct rawdev *raw; 67 | 68 | priv = malloc(sizeof(struct ethernet_priv)); 69 | if (!priv) { 70 | return -1; 71 | } 72 | raw = rawdev_alloc(opt, dev->name); 73 | if (!raw) { 74 | free(priv); 75 | return -1; 76 | } 77 | if (raw->ops->open(raw) == -1) { 78 | free(raw); 79 | free(priv); 80 | return -1; 81 | } 82 | priv->raw = raw; 83 | priv->thread = pthread_self(); 84 | priv->terminate = 0; 85 | priv->dev = dev; 86 | dev->priv = priv; 87 | if (memcmp(dev->addr, ETHERNET_ADDR_ANY, ETHERNET_ADDR_LEN) == 0) { 88 | raw->ops->addr(raw, dev->addr, ETHERNET_ADDR_LEN); 89 | } 90 | memcpy(dev->broadcast, ETHERNET_ADDR_BROADCAST, ETHERNET_ADDR_LEN); 91 | return 0; 92 | } 93 | 94 | int ethernet_close(struct netdev *dev) { 95 | struct ethernet_priv *priv; 96 | 97 | if (!dev || !dev->priv) { 98 | return 1; 99 | } 100 | priv = dev->priv; 101 | if (!pthread_equal(priv->thread, pthread_self())) { 102 | priv->terminate = 1; 103 | pthread_join(priv->thread, NULL); 104 | } 105 | if (priv->raw) { 106 | priv->raw->ops->close(priv->raw); 107 | priv->raw = NULL; 108 | } 109 | free(priv); 110 | dev->priv = NULL; 111 | 112 | return 0; 113 | } 114 | 115 | static void ethernet_dump(struct netdev *dev, uint8_t *frame, size_t flen) { 116 | struct ethernet_hdr *hdr; 117 | char addr[ETHERNET_ADDR_STR_LEN]; 118 | 119 | hdr = (struct ethernet_hdr *)frame; 120 | fprintf(stderr, " dev: %s (%s)\n", dev->name, 121 | ethernet_addr_ntop(dev->addr, addr, sizeof(addr))); 122 | fprintf(stderr, " src: %s\n", 123 | ethernet_addr_ntop(hdr->src, addr, sizeof(addr))); 124 | fprintf(stderr, " dst: %s\n", 125 | ethernet_addr_ntop(hdr->dst, addr, sizeof(addr))); 126 | fprintf(stderr, " type: 0x%04x (%s)\n", ntoh16(hdr->type), 127 | ethernet_addr_ntop(hdr->src, addr, sizeof(addr))); 128 | fprintf(stderr, " len: %zu octets\n", flen); 129 | hexdump(stderr, frame, flen); 130 | } 131 | 132 | static void ethernet_rx(uint8_t *frame, size_t flen, void *arg) { 133 | struct netdev *dev; 134 | struct ethernet_hdr *hdr; 135 | uint8_t *payload; 136 | size_t plen; 137 | 138 | dev = (struct netdev *)arg; 139 | if (flen < sizeof(struct ethernet_hdr)) { 140 | fprintf(stderr, "ethernet frame size is shorter than ethernet_hdr\n"); 141 | return; 142 | } 143 | hdr = (struct ethernet_hdr *)frame; 144 | // check the frame is for self NIC. 145 | if (memcmp(dev->addr, hdr->dst, ETHERNET_ADDR_LEN) != 0) { 146 | if (memcmp(ETHERNET_ADDR_BROADCAST, hdr->dst, ETHERNET_ADDR_LEN) != 0) { 147 | return; 148 | } 149 | } 150 | 151 | #ifdef DEBUG 152 | fprintf(stderr, ">>> ethernet_rx <<<\n"); 153 | ethernet_dump(dev, frame, flen); 154 | #endif 155 | 156 | payload = (uint8_t *)(hdr + 1); 157 | plen = flen - sizeof(struct ethernet_hdr); 158 | dev->rx_handler(dev, hdr->type, payload, plen); 159 | } 160 | 161 | static void *ethernet_rx_thread(void *arg) { 162 | struct netdev *dev; 163 | struct ethernet_priv *priv; 164 | 165 | dev = (struct netdev *)arg; 166 | priv = (struct ethernet_priv *)dev->priv; 167 | while (!priv->terminate) { 168 | priv->raw->ops->rx(priv->raw, ethernet_rx, dev, 1000); 169 | } 170 | return NULL; 171 | } 172 | 173 | int ethernet_run(struct netdev *dev) { 174 | struct ethernet_priv *priv; 175 | int err; 176 | priv = (struct ethernet_priv *)dev->priv; 177 | if ((err = pthread_create(&priv->thread, NULL, ethernet_rx_thread, dev)) != 178 | 0) { 179 | fprintf(stderr, "pthread_create: error, code=%d\n", err); 180 | return -1; 181 | } 182 | return 0; 183 | } 184 | 185 | int ethernet_stop(struct netdev *dev) { 186 | struct ethernet_priv *priv; 187 | 188 | priv = dev->priv; 189 | priv->terminate = 1; 190 | pthread_join(priv->thread, NULL); 191 | priv->thread = pthread_self(); 192 | priv->terminate = 0; 193 | return 0; 194 | } 195 | 196 | ssize_t ethernet_tx(struct netdev *dev, uint16_t type, uint8_t *payload, 197 | size_t plen, const void *dst) { 198 | struct ethernet_priv *priv; 199 | uint8_t frame[ETHERNET_FRAME_SIZE_MAX]; 200 | struct ethernet_hdr *hdr; 201 | size_t flen; 202 | 203 | priv = (struct ethernet_priv *)dev->priv; 204 | if (!payload || plen > ETHERNET_PAYLOAD_SIZE_MAX || !dst) { 205 | return -1; 206 | } 207 | memset(frame, 0, sizeof(frame)); 208 | hdr = (struct ethernet_hdr *)frame; 209 | memcpy(hdr->dst, dst, ETHERNET_ADDR_LEN); 210 | memcpy(hdr->src, dev->addr, ETHERNET_ADDR_LEN); 211 | hdr->type = hton16(type); 212 | memcpy(hdr + 1, payload, plen); 213 | flen = sizeof(struct ethernet_hdr) + 214 | (plen < ETHERNET_PAYLOAD_SIZE_MIN ? ETHERNET_PAYLOAD_SIZE_MIN : plen); 215 | 216 | #ifdef DEBUG 217 | fprintf(stderr, ">>> ethernet_tx <<<\n"); 218 | ethernet_dump(dev, frame, flen); 219 | #endif 220 | 221 | return priv->raw->ops->tx(priv->raw, frame, flen) == (ssize_t)flen 222 | ? (ssize_t)plen 223 | : -1; 224 | } 225 | 226 | struct netdev_ops ethernet_ops = { 227 | .open = ethernet_open, 228 | .close = ethernet_close, 229 | .run = ethernet_run, 230 | .stop = ethernet_stop, 231 | .tx = ethernet_tx, 232 | }; 233 | 234 | struct netdev_def ethernet_def = { 235 | .type = NETDEV_TYPE_ETHERNET, 236 | .mtu = ETHERNET_PAYLOAD_SIZE_MAX, 237 | .flags = NETDEV_FLAG_BROADCAST, 238 | .hlen = ETHERNET_HDR_SIZE, 239 | .alen = ETHERNET_ADDR_LEN, 240 | .ops = ðernet_ops, 241 | }; 242 | 243 | int ethernet_init(void) { 244 | if (netdev_driver_register(ðernet_def) == -1) { 245 | return -1; 246 | } 247 | return 0; 248 | } 249 | -------------------------------------------------------------------------------- /ethernet.h: -------------------------------------------------------------------------------- 1 | #ifndef ETHERNET_H 2 | #define ETHERNET_H 3 | 4 | #include 5 | #include 6 | 7 | #define ETHERNET_ADDR_LEN 6 8 | #define ETHERNET_ADDR_STR_LEN 18 /* "xx:xx:xx:xx:xx:xx\0" */ 9 | 10 | #define ETHERNET_HDR_SIZE 14 11 | #define ETHERNET_TRL_SIZE 4 12 | #define ETHERNET_FRAME_SIZE_MIN 64 13 | #define ETHERNET_FRAME_SIZE_MAX 1518 14 | #define ETHERNET_PAYLOAD_SIZE_MIN \ 15 | (ETHERNET_FRAME_SIZE_MIN - (ETHERNET_HDR_SIZE + ETHERNET_TRL_SIZE)) 16 | #define ETHERNET_PAYLOAD_SIZE_MAX \ 17 | (ETHERNET_FRAME_SIZE_MAX - (ETHERNET_HDR_SIZE + ETHERNET_TRL_SIZE)) 18 | 19 | #define ETHERNET_TYPE_IP (0x0800) 20 | #define ETHERNET_TYPE_ARP (0x0806) 21 | #define ETHERNET_TYPE_IPV6 (0x86dd) 22 | 23 | extern const uint8_t ETHERNET_ADDR_ANY[ETHERNET_ADDR_LEN]; 24 | extern const uint8_t ETHERNET_ADDR_BROADCAST[ETHERNET_ADDR_LEN]; 25 | 26 | int ethernet_addr_pton(const char *p, uint8_t *n); 27 | char *ethernet_addr_ntop(const uint8_t *n, char *p, size_t size); 28 | int ethernet_init(void); 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /ip.c: -------------------------------------------------------------------------------- 1 | #include "ip.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "arp.h" 10 | #include "net.h" 11 | #include "util.h" 12 | 13 | #define IP_FRAGMENT_TIMEOUT_SEC 30 14 | #define IP_FRAGMENT_NUM_MAX 8 15 | 16 | struct ip_route { 17 | uint8_t used; 18 | ip_addr_t network; 19 | ip_addr_t netmask; 20 | ip_addr_t nexthop; 21 | struct netif *netif; 22 | }; 23 | 24 | struct ip_fragment { 25 | struct ip_fragment *next; 26 | ip_addr_t src; 27 | ip_addr_t dst; 28 | uint16_t id; 29 | uint16_t protocol; 30 | uint16_t len; 31 | uint8_t data[65535]; 32 | uint32_t mask[2048]; 33 | time_t timestamp; 34 | }; 35 | 36 | struct ip_protocol { 37 | struct ip_protocol *next; 38 | uint8_t type; 39 | void (*handler)(uint8_t *payload, size_t len, ip_addr_t *src, ip_addr_t *dst, 40 | struct netif *netif); 41 | }; 42 | 43 | static struct netif *default_netif = NULL; 44 | static struct ip_protocol *protocols = NULL; 45 | static struct ip_fragment *fragments = NULL; 46 | static int ip_forwarding = 0; 47 | 48 | const ip_addr_t IP_ADDR_ANY = 0x00000000; 49 | const ip_addr_t IPADDR_BROADCAST = 0xffffffff; 50 | 51 | // convert string ip address (p) to ip_addr_t (n) 52 | int ip_addr_pton(const char *p, ip_addr_t *n) { 53 | char *sp, *ep; 54 | int idx; 55 | long ret; 56 | 57 | sp = (char *)p; 58 | for (idx = 0; idx < 4; idx++) { 59 | ret = strtol(sp, &ep, 10); 60 | if (ret < 0 || ret > 255) { 61 | return -1; 62 | } else if (ep == sp) { 63 | // not change 64 | return -1; 65 | } else if ((idx == 3 && *ep != '\0') || (idx != 3 && *ep != '.')) { 66 | // separater or end of string 67 | return -1; 68 | } 69 | ((uint8_t *)n)[idx] = ret; 70 | sp = ep + 1; 71 | } 72 | return 0; 73 | } 74 | 75 | char *ip_addr_ntop(const ip_addr_t *n, char *p, size_t size) { 76 | uint8_t *ptr; 77 | 78 | ptr = (uint8_t *)n; 79 | snprintf(p, size, "%d.%d.%d.%d", ptr[0], ptr[1], ptr[2], ptr[3]); 80 | return p; 81 | } 82 | 83 | void ip_dump(struct netif *netif, struct ip_hdr *hdr, uint8_t *packet, 84 | size_t plen) { 85 | struct netif_ip *iface; 86 | char addr[IP_ADDR_STR_LEN]; 87 | 88 | iface = (struct netif_ip *)netif; 89 | fprintf(stderr, " dev: %s (%s)\n", netif->dev->name, 90 | ip_addr_ntop(&iface->unicast, addr, sizeof(addr))); 91 | fprintf(stderr, " src: %s\n", 92 | ip_addr_ntop(&hdr->src, addr, sizeof(addr))); 93 | fprintf(stderr, " dst: %s\n", 94 | ip_addr_ntop(&hdr->dst, addr, sizeof(addr))); 95 | fprintf(stderr, " protocol: 0x%02x\n", hdr->protocol); 96 | fprintf(stderr, " len: %zu octets\n", plen); 97 | hexdump(stderr, packet, plen); 98 | } 99 | 100 | /* 101 | * IP FRAGMENT 102 | */ 103 | 104 | static struct ip_fragment *ip_fragment_alloc(struct ip_hdr *hdr) { 105 | struct ip_fragment *new_fragment; 106 | 107 | new_fragment = malloc(sizeof(struct ip_fragment)); 108 | if (!new_fragment) { 109 | return NULL; 110 | } 111 | new_fragment->next = fragments; 112 | new_fragment->src = hdr->src; 113 | new_fragment->dst = hdr->dst; 114 | new_fragment->id = hdr->id; 115 | new_fragment->protocol = hdr->protocol; 116 | new_fragment->len = 0; 117 | memset(new_fragment->data, 0, sizeof(new_fragment->data)); 118 | maskclr(new_fragment->mask, sizeof(new_fragment->mask)); 119 | fragments = new_fragment; 120 | return new_fragment; 121 | } 122 | 123 | static void ip_fragment_free(struct ip_fragment *fragment) { free(fragment); } 124 | 125 | static struct ip_fragment *ip_fragment_detach(struct ip_fragment *fragment) { 126 | struct ip_fragment *entry, *prev = NULL; 127 | 128 | for (entry = fragments; entry; entry = entry->next) { 129 | if (entry == fragment) { 130 | if (prev) { 131 | prev->next = fragment->next; 132 | } else { 133 | fragments = fragment->next; 134 | } 135 | fragment->next = NULL; 136 | return fragment; 137 | } 138 | prev = entry; 139 | } 140 | return NULL; 141 | } 142 | 143 | static struct ip_fragment *ip_fragment_search(struct ip_hdr *hdr) { 144 | struct ip_fragment *entry; 145 | 146 | for (entry = fragments; entry; entry = entry->next) { 147 | if (entry->src == hdr->src && entry->dst == hdr->dst && 148 | entry->id == hdr->id && entry->protocol == hdr->protocol) { 149 | return entry; 150 | } 151 | } 152 | return NULL; 153 | } 154 | 155 | static int ip_fragment_patrol(void) { 156 | time_t now; 157 | struct ip_fragment *entry, *prev = NULL; 158 | int count = 0; 159 | 160 | now = time(NULL); 161 | entry = fragments; 162 | while (entry) { 163 | if (now - entry->timestamp > IP_FRAGMENT_TIMEOUT_SEC) { 164 | if (prev) { 165 | entry = prev->next = entry->next; 166 | } else { 167 | entry = fragments = entry->next; 168 | } 169 | free(entry); 170 | count++; 171 | continue; 172 | } 173 | prev = entry; 174 | entry = entry->next; 175 | } 176 | return count; 177 | } 178 | 179 | static struct ip_fragment *ip_fragment_process(struct ip_hdr *hdr, 180 | uint8_t *payload, size_t plen) { 181 | struct ip_fragment *fragment; 182 | uint16_t off; 183 | static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; 184 | static time_t timestamp = 0; 185 | static size_t count = 0; 186 | 187 | pthread_mutex_lock(&mutex); 188 | 189 | // check fragment timeout 190 | if (time(NULL) - timestamp > 10) { 191 | ip_fragment_patrol(); 192 | } 193 | 194 | // find or create fragment object 195 | fragment = ip_fragment_search(hdr); 196 | if (!fragment) { 197 | if (count >= IP_FRAGMENT_NUM_MAX) { 198 | // too many fragments 199 | pthread_mutex_unlock(&mutex); 200 | return NULL; 201 | } 202 | // create new fragment object 203 | fragment = ip_fragment_alloc(hdr); 204 | if (!fragment) { 205 | // failed to allocate fragment object 206 | pthread_mutex_unlock(&mutex); 207 | return NULL; 208 | } 209 | count++; 210 | } 211 | 212 | // copy data to fragment object 213 | off = (ntoh16(hdr->offset) & 0x1fff) << 3; 214 | memcpy(fragment->data + off, payload, plen); 215 | maskset(fragment->mask, sizeof(fragment->mask), off, plen); 216 | if ((ntoh16(hdr->offset) & 0x2000) == 0) { 217 | fragment->len = off + plen; 218 | } 219 | fragment->timestamp = time(NULL); 220 | 221 | // check fragment is completed 222 | if (!fragment->len) { 223 | // don't know total fragment length yet 224 | pthread_mutex_unlock(&mutex); 225 | return NULL; 226 | } 227 | if (!maskchk(fragment->mask, sizeof(fragment->mask), 0, fragment->len)) { 228 | // imcomplete fragments 229 | pthread_mutex_unlock(&mutex); 230 | return NULL; 231 | } 232 | 233 | // detach fragment object 234 | ip_fragment_detach(fragment); 235 | count--; 236 | pthread_mutex_unlock(&mutex); 237 | return fragment; 238 | } 239 | 240 | /* 241 | * IP INTERFACE 242 | */ 243 | 244 | struct netif *ip_netif_register(struct netdev *dev, const char *addr, 245 | const char *netmask, const char *gateway) { 246 | struct netif_ip *iface; 247 | 248 | iface = malloc(sizeof(struct netif_ip)); 249 | if (!iface) { 250 | return NULL; 251 | } 252 | 253 | // set netif_ip parameters 254 | ((struct netif *)iface)->next = NULL; 255 | ((struct netif *)iface)->family = NETIF_FAMILY_IPV4; 256 | ((struct netif *)iface)->dev = NULL; 257 | if (ip_addr_pton(addr, &iface->unicast) == -1) { 258 | goto ERR_SETUP_NETIF; 259 | } 260 | if (ip_addr_pton(netmask, &iface->netmask) == -1) { 261 | goto ERR_SETUP_NETIF; 262 | } 263 | iface->network = iface->unicast & iface->netmask; 264 | iface->broadcast = iface->network | ~iface->netmask; 265 | 266 | // TODO : register to route table 267 | default_netif = (struct netif *)iface; 268 | // TODO : set gateway 269 | 270 | // register netdev 271 | if (netdev_add_netif(dev, (struct netif *)iface) == -1) { 272 | goto ERR_SETUP_NETIF; 273 | } 274 | 275 | return (struct netif *)iface; 276 | 277 | ERR_SETUP_NETIF: 278 | free(iface); 279 | return NULL; 280 | } 281 | 282 | struct netif *ip_netif_by_addr(ip_addr_t *addr) { 283 | struct netdev *dev; 284 | struct netif *entry; 285 | 286 | for (dev = netdev_root(); dev; dev = dev->next) { 287 | for (entry = dev->ifs; entry; entry = entry->next) { 288 | if (entry->family == NETIF_FAMILY_IPV4 && 289 | ((struct netif_ip *)entry)->unicast == *addr) { 290 | return entry; 291 | } 292 | } 293 | } 294 | return NULL; 295 | } 296 | 297 | struct netif *ip_netif_by_peer(ip_addr_t *peer) { 298 | // TODO: find netif from routing table 299 | 300 | return default_netif; 301 | } 302 | 303 | /* 304 | * IP CORE 305 | */ 306 | 307 | static void ip_rx(uint8_t *dgram, size_t dlen, struct netdev *dev) { 308 | struct ip_hdr *hdr; 309 | uint16_t hlen, offset; 310 | struct netif_ip *iface; 311 | uint8_t *payload; 312 | size_t plen; 313 | struct ip_fragment *fragment = NULL; 314 | struct ip_protocol *protocol; 315 | 316 | // get ip header 317 | if (dlen < sizeof(struct ip_hdr)) { 318 | fprintf(stderr, "too short dgram for ip header\n"); 319 | return; 320 | } 321 | hdr = (struct ip_hdr *)dgram; 322 | if ((hdr->vhl) >> 4 != IP_VERSION_IPV4) { 323 | fprintf(stderr, "not ipv4 packet.\n"); 324 | return; 325 | } 326 | 327 | // validate ip header 328 | hlen = (hdr->vhl & 0x0f) << 2; 329 | if (dlen < hlen || dlen < ntoh16(hdr->len)) { 330 | fprintf(stderr, "ip packet length error.\n"); 331 | return; 332 | } 333 | if (cksum16((uint16_t *)hdr, hlen, 0) != 0) { 334 | fprintf(stderr, "ip packet checksum error.\n"); 335 | return; 336 | } 337 | if (!hdr->ttl) { 338 | fprintf(stderr, "ip packet was dead (TTL=0).\n"); 339 | return; 340 | } 341 | 342 | iface = (struct netif_ip *)netdev_get_netif(dev, NETIF_FAMILY_IPV4); 343 | if (!iface) { 344 | fprintf(stderr, "ip unknown interface.\n"); 345 | return; 346 | } 347 | if (hdr->dst != iface->unicast) { 348 | if (hdr->dst != iface->broadcast && hdr->dst != IPADDR_BROADCAST) { 349 | // for other host 350 | if (ip_forwarding) { 351 | // TOOD: ip_forward_process 352 | } 353 | return; 354 | } 355 | } 356 | 357 | #ifdef DEBUG 358 | fprintf(stderr, ">>> ip_rx <<<\n"); 359 | ip_dump((struct netif *)iface, hdr, dgram, dlen); 360 | #endif 361 | 362 | payload = (uint8_t *)hdr + hlen; 363 | plen = ntoh16(hdr->len) - hlen; 364 | offset = ntoh16(hdr->offset); 365 | if (offset & 0x2000 || offset & 0x1fff) { 366 | fragment = ip_fragment_process(hdr, payload, plen); 367 | if (!fragment) { 368 | return; 369 | } 370 | // completed fragment 371 | payload = fragment->data; 372 | plen = fragment->len; 373 | } 374 | for (protocol = protocols; protocol; protocol = protocol->next) { 375 | if (protocol->type == hdr->protocol) { 376 | protocol->handler(payload, plen, &hdr->src, &hdr->dst, 377 | (struct netif *)iface); 378 | break; 379 | } 380 | } 381 | if (fragment) { 382 | ip_fragment_free(fragment); 383 | } 384 | } 385 | 386 | static int ip_tx_netdev(struct netif *netif, uint8_t *packet, size_t plen, 387 | const ip_addr_t *dst) { 388 | ssize_t ret; 389 | uint8_t ha[128] = {}; 390 | 391 | if (!(netif->dev->flags & NETDEV_FLAG_NOARP)) { 392 | if (dst) { 393 | ret = arp_resolve(netif, dst, (void *)ha, packet, plen); 394 | if (ret != ARP_RESOLVE_FOUND) { 395 | // ARP_RESOLVE_ERROR then error 396 | // ARP_RESOLVE_QUERY then wait and send packet in arp layer after arp 397 | // reply come 398 | return ret; 399 | } 400 | } else { 401 | memcpy(ha, netif->dev->broadcast, netif->dev->alen); 402 | } 403 | } 404 | if (netif->dev->ops->tx(netif->dev, ETHERNET_TYPE_IP, packet, plen, 405 | (void *)ha) != (ssize_t)plen) { 406 | return -1; 407 | } 408 | return 1; 409 | } 410 | 411 | static int ip_tx_core(struct netif *netif, uint8_t protocol, const uint8_t *buf, 412 | size_t len, const ip_addr_t *src, const ip_addr_t *dst, 413 | const ip_addr_t *nexthop, uint16_t id, uint16_t offset) { 414 | uint8_t packet[4096]; 415 | struct ip_hdr *hdr; 416 | uint16_t hlen; 417 | 418 | // set header 419 | hdr = (struct ip_hdr *)packet; 420 | hlen = sizeof(struct ip_hdr); 421 | hdr->vhl = (IP_VERSION_IPV4 << 4) | (hlen >> 2); 422 | hdr->tos = 0; 423 | hdr->len = hton16(hlen + len); 424 | hdr->id = hton16(id); 425 | hdr->offset = hton16(offset); 426 | hdr->ttl = 0xff; 427 | hdr->protocol = protocol; 428 | hdr->sum = 0; 429 | hdr->src = src ? *src : ((struct netif_ip *)netif)->unicast; 430 | hdr->dst = *dst; 431 | hdr->sum = cksum16((uint16_t *)hdr, hlen, 0); 432 | 433 | // copy payload 434 | memcpy(hdr + 1, buf, len); 435 | 436 | #ifdef DEBUG 437 | fprintf(stderr, ">>> ip_tx_core <<<\n"); 438 | ip_dump(netif, hdr, (uint8_t *)packet, hlen + len); 439 | #endif 440 | 441 | return ip_tx_netdev(netif, (uint8_t *)packet, hlen + len, nexthop); 442 | } 443 | 444 | static uint16_t ip_generate_id(void) { 445 | static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; 446 | static uint16_t id = 128; 447 | uint16_t ret; 448 | 449 | // TODO: multi netdev 450 | // TODO: rotate id 451 | pthread_mutex_lock(&mutex); 452 | ret = id++; 453 | pthread_mutex_unlock(&mutex); 454 | return ret; 455 | } 456 | 457 | ssize_t ip_tx(struct netif *netif, uint8_t protocol, const uint8_t *buf, 458 | size_t len, const ip_addr_t *dst) { 459 | ip_addr_t *nexthop = NULL, *src = NULL; 460 | uint16_t id, flag, offset; 461 | size_t done, slen; 462 | 463 | // determine nexthop 464 | if (netif && *dst == IPADDR_BROADCAST) { 465 | nexthop = NULL; 466 | } else { 467 | // TODO: find route 468 | if (netif) { 469 | src = &((struct netif_ip *)netif)->unicast; 470 | } 471 | // TODO: use route to determin nexthop 472 | nexthop = dst; 473 | } 474 | id = ip_generate_id(); 475 | 476 | // send ip packet (if fragmented then sometimes) 477 | for (done = 0; done < len; done += slen) { 478 | slen = MIN((len - done), (size_t)(netif->dev->mtu - IP_HDR_SIZE_MIN)); 479 | flag = ((done + slen) < len) ? 0x2000 : 0x0000; 480 | offset = flag | ((done >> 3) & 0x1fff); 481 | if (ip_tx_core(netif, protocol, buf + done, slen, src, dst, nexthop, id, 482 | offset) == -1) { 483 | return -1; 484 | } 485 | } 486 | return len; 487 | } 488 | 489 | int ip_add_protocol(uint8_t protocol, 490 | void (*handler)(uint8_t *, size_t, ip_addr_t *, ip_addr_t *, 491 | struct netif *)) { 492 | struct ip_protocol *p; 493 | 494 | // check protocol is already registered 495 | for (p = protocols; p; p = p->next) { 496 | if (p->type == protocol) { 497 | return -1; 498 | } 499 | } 500 | 501 | // register new protocol 502 | p = malloc(sizeof(struct ip_protocol)); 503 | p->next = protocols; 504 | p->type = protocol; 505 | p->handler = handler; 506 | protocols = p; 507 | return 0; 508 | } 509 | 510 | int ip_init(void) { return netdev_proto_register(NETDEV_PROTO_IP, ip_rx); } 511 | -------------------------------------------------------------------------------- /ip.h: -------------------------------------------------------------------------------- 1 | #ifndef IP_H 2 | #define IP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "net.h" 8 | 9 | #define IP_VERSION_IPV4 4 10 | 11 | #define IP_PROTOCOL_ICMP 1 12 | #define IP_PROTOCOL_TCP 6 13 | #define IP_PROTOCOL_UDP 17 14 | #define IP_PROTOCOL_RAW 255 15 | 16 | #define IP_HDR_SIZE_MIN 20 17 | #define IP_HDR_SIZE_MAX 60 18 | 19 | #define IP_PAYLOAD_SIZE_MAX (65535 - IP_HDR_SIZE_MIN) 20 | 21 | #define IP_ADDR_LEN 4 22 | #define IP_ADDR_STR_LEN 16 /* "ddd.ddd.ddd.ddd\0" */ 23 | 24 | typedef uint32_t ip_addr_t; 25 | 26 | struct ip_hdr { 27 | uint8_t vhl; 28 | uint8_t tos; 29 | uint16_t len; 30 | uint16_t id; 31 | uint16_t offset; 32 | uint8_t ttl; 33 | uint8_t protocol; 34 | uint16_t sum; 35 | ip_addr_t src; 36 | ip_addr_t dst; 37 | uint8_t options[0]; 38 | }; 39 | 40 | struct netif_ip { 41 | struct netif netif; 42 | ip_addr_t unicast; 43 | ip_addr_t netmask; 44 | ip_addr_t network; 45 | ip_addr_t broadcast; 46 | ip_addr_t gateway; 47 | }; 48 | 49 | extern const ip_addr_t IP_ADDR_ANY; 50 | extern const ip_addr_t IPADDR_BROADCAST; 51 | 52 | int ip_addr_pton(const char *p, ip_addr_t *n); 53 | char *ip_addr_ntop(const ip_addr_t *n, char *p, size_t size); 54 | 55 | struct netif *ip_netif_register(struct netdev *dev, const char *addr, 56 | const char *netmask, const char *gateway); 57 | struct netif *ip_netif_by_addr(ip_addr_t *addr); 58 | struct netif *ip_netif_by_peer(ip_addr_t *peer); 59 | 60 | ssize_t ip_tx(struct netif *netif, uint8_t protocol, const uint8_t *buf, 61 | size_t len, const ip_addr_t *dst); 62 | int ip_add_protocol(uint8_t protocol, 63 | void (*handler)(uint8_t *, size_t, ip_addr_t *, ip_addr_t *, 64 | struct netif *)); 65 | int ip_init(void); 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /net.c: -------------------------------------------------------------------------------- 1 | #include "net.h" 2 | #include 3 | #include 4 | #include "util.h" 5 | 6 | struct netdev_driver { 7 | struct netdev_driver *next; 8 | uint16_t type; 9 | uint16_t mtu; 10 | uint16_t flags; 11 | uint16_t hlen; 12 | uint16_t alen; 13 | struct netdev_ops *ops; 14 | }; 15 | 16 | struct netdev_proto { 17 | struct netdev_proto *next; 18 | uint16_t type; 19 | void (*handler)(uint8_t *packet, size_t plen, struct netdev *dev); 20 | }; 21 | 22 | static struct netdev_driver *drivers = NULL; 23 | static struct netdev_proto *protos = NULL; 24 | static struct netdev *devices = NULL; 25 | 26 | int netdev_driver_register(struct netdev_def *def) { 27 | struct netdev_driver *entry; 28 | 29 | for (entry = drivers; entry; entry = entry->next) { 30 | if (entry->type == def->type) { 31 | return -1; 32 | } 33 | } 34 | entry = malloc(sizeof(struct netdev_driver)); 35 | if (!entry) { 36 | return -1; 37 | } 38 | entry->next = drivers; 39 | entry->type = def->type; 40 | entry->mtu = def->mtu; 41 | entry->flags = def->flags; 42 | entry->hlen = def->hlen; 43 | entry->alen = def->alen; 44 | entry->ops = def->ops; 45 | drivers = entry; 46 | return 0; 47 | } 48 | 49 | int netdev_proto_register(unsigned short type, 50 | void (*handler)(uint8_t *packet, size_t plen, 51 | struct netdev *dev)) { 52 | struct netdev_proto *entry; 53 | 54 | // check this proto is already registered 55 | for (entry = protos; entry; entry = entry->next) { 56 | if (entry->type == type) { 57 | return -1; 58 | } 59 | } 60 | 61 | entry = malloc(sizeof(struct netdev_proto)); 62 | if (!entry) { 63 | return -1; 64 | } 65 | entry->next = protos; 66 | entry->type = type; 67 | entry->handler = handler; 68 | protos = entry; 69 | return 0; 70 | } 71 | 72 | static void netdev_rx_handler(struct netdev *dev, uint16_t type, 73 | uint8_t *packet, size_t plen) { 74 | struct netdev_proto *entry; 75 | 76 | for (entry = protos; entry; entry = entry->next) { 77 | if (hton16(entry->type) == type) { 78 | entry->handler(packet, plen, dev); 79 | } 80 | } 81 | } 82 | 83 | struct netdev *netdev_root(void) { 84 | return devices; 85 | } 86 | 87 | struct netdev *netdev_alloc(uint16_t type) { 88 | struct netdev_driver *driver; 89 | struct netdev *dev; 90 | 91 | // find driver matches type 92 | for (driver = drivers; driver; driver = driver->next) { 93 | if (driver->type == type) { 94 | break; 95 | } 96 | } 97 | if (!driver) { 98 | return NULL; 99 | } 100 | 101 | dev = malloc(sizeof(struct netdev)); 102 | if (!dev) { 103 | return NULL; 104 | } 105 | dev->next = devices; 106 | dev->ifs = NULL; 107 | dev->type = driver->type; 108 | dev->mtu = driver->mtu; 109 | dev->flags = driver->flags; 110 | dev->hlen = driver->hlen; 111 | dev->alen = driver->alen; 112 | dev->rx_handler = netdev_rx_handler; 113 | dev->ops = driver->ops; 114 | devices = dev; 115 | return dev; 116 | } 117 | 118 | int netdev_add_netif(struct netdev *dev, struct netif *netif) { 119 | struct netif *entry; 120 | 121 | // check netif is already registered to this netdev 122 | for (entry = dev->ifs; entry; entry = entry->next) { 123 | if (entry->family == netif->family) { 124 | return -1; 125 | } 126 | } 127 | 128 | // set netif 129 | netif->next = dev->ifs; 130 | netif->dev = dev; 131 | dev->ifs = netif; 132 | return 0; 133 | } 134 | 135 | struct netif *netdev_get_netif(struct netdev *dev, int family) { 136 | struct netif *entry; 137 | 138 | for (entry = dev->ifs; entry; entry = entry->next) { 139 | if (entry->family == family) { 140 | return entry; 141 | } 142 | } 143 | return NULL; 144 | } 145 | -------------------------------------------------------------------------------- /net.h: -------------------------------------------------------------------------------- 1 | #ifndef NET_H 2 | #define NET_H 3 | 4 | #include 5 | #include 6 | 7 | #define NETDEV_TYPE_ETHERNET (0x0001) 8 | 9 | #define NETDEV_FLAG_BROADCAST (0x0001) 10 | #define NETDEV_FLAG_MULTICAST (0x0002) 11 | #define NETDEV_FLAG_P2P (0x0004) 12 | #define NETDEV_FLAG_LOOPBACK (0x0008) 13 | #define NETDEV_FLAG_NOARP (0x0010) 14 | #define NETDEV_FLAG_PROMISC (0x0020) 15 | #define NETDEV_FLAG_RUNNING (0x0040) 16 | #define NETDEV_FLAG_UP (0x0080) 17 | 18 | #include "ethernet.h" 19 | #define NETDEV_PROTO_IP ETHERNET_TYPE_IP 20 | #define NETDEV_PROTO_ARP ETHERNET_TYPE_ARP 21 | #define NETDEV_PROTO_IPV6 ETHERNET_TYPE_IPV6 22 | 23 | #define NETIF_FAMILY_IPV4 (0x02) 24 | 25 | #ifndef IFNAMSIZ 26 | #define IFNAMSIZ (16) 27 | #endif 28 | 29 | struct netdev; 30 | 31 | struct netif { 32 | struct netif *next; 33 | uint8_t family; 34 | struct netdev *dev; 35 | // Depends on implementation of protocols. 36 | }; 37 | 38 | struct netdev_ops { 39 | int (*open)(struct netdev *dev, int opt); 40 | int (*close)(struct netdev *dev); 41 | int (*run)(struct netdev *dev); 42 | int (*stop)(struct netdev *dev); 43 | ssize_t (*tx)(struct netdev *dev, uint16_t type, uint8_t *packet, size_t size, 44 | const void *dst); 45 | }; 46 | 47 | struct netdev_def { 48 | uint16_t type; 49 | uint16_t mtu; 50 | uint16_t flags; 51 | uint16_t hlen; 52 | uint16_t alen; 53 | struct netdev_ops *ops; 54 | }; 55 | 56 | struct netdev { 57 | struct netdev *next; 58 | struct netif *ifs; 59 | char name[IFNAMSIZ]; 60 | uint16_t type; 61 | uint16_t mtu; 62 | uint16_t flags; 63 | uint16_t hlen; 64 | uint16_t alen; 65 | uint8_t addr[16]; 66 | uint8_t peer[16]; 67 | uint8_t broadcast[16]; 68 | void (*rx_handler)(struct netdev *dev, uint16_t type, uint8_t *packet, 69 | size_t plen); 70 | struct netdev_ops *ops; 71 | void *priv; 72 | }; 73 | 74 | int netdev_driver_register(struct netdev_def *def); 75 | int netdev_proto_register(unsigned short type, 76 | void (*handler)(uint8_t *packet, size_t plen, 77 | struct netdev *dev)); 78 | 79 | struct netdev *netdev_root(void); 80 | struct netdev *netdev_alloc(uint16_t type); 81 | int netdev_add_netif(struct netdev *dev, struct netif *netif); 82 | struct netif *netdev_get_netif(struct netdev *dev, int family); 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /raw.c: -------------------------------------------------------------------------------- 1 | #include "raw.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "raw.h" 7 | 8 | #ifdef HAVE_TAP 9 | #include "raw/tap.h" 10 | extern struct rawdev_ops tap_dev_ops; 11 | #endif 12 | 13 | #ifdef HAVE_PF_PACKET 14 | #include "raw/soc.h" 15 | #define RAWDEV_TYPE_DEFAULT RAWDEV_TYPE_SOCKET 16 | extern struct rawdev_ops soc_dev_ops; 17 | #endif 18 | 19 | static uint8_t rawdev_detect_type(char *name) { 20 | if (strncmp(name, "tap", 3) == 0) { 21 | return RAWDEV_TYPE_TAP; 22 | } 23 | return RAWDEV_TYPE_DEFAULT; 24 | } 25 | 26 | struct rawdev *rawdev_alloc(uint8_t type, char *name) { 27 | struct rawdev *dev; 28 | struct rawdev_ops *ops; 29 | 30 | if (type == RAWDEV_TYPE_AUTO) { 31 | type = rawdev_detect_type(name); 32 | } 33 | 34 | // set ops 35 | switch (type) { 36 | #ifdef HAVE_TAP 37 | case RAWDEV_TYPE_TAP: 38 | ops = &tap_dev_ops; 39 | break; 40 | #endif 41 | 42 | #ifdef HAVE_PF_PACKET 43 | case RAWDEV_TYPE_SOCKET: 44 | ops = &soc_dev_ops; 45 | break; 46 | #endif 47 | 48 | default: 49 | fprintf(stderr, "unsupported raw device type (%u)\n", type); 50 | return NULL; 51 | } 52 | 53 | dev = malloc(sizeof(struct rawdev)); 54 | if (!dev) { 55 | fprintf(stderr, "malloc rawdev: failure\n"); 56 | return NULL; 57 | } 58 | dev->type = type; 59 | dev->name = name; 60 | dev->ops = ops; 61 | dev->priv = NULL; 62 | return dev; 63 | } 64 | -------------------------------------------------------------------------------- /raw.h: -------------------------------------------------------------------------------- 1 | #ifndef RAW_H 2 | #define RAW_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define RAWDEV_TYPE_AUTO 0 9 | #define RAWDEV_TYPE_TAP 1 10 | #define RAWDEV_TYPE_SOCKET 2 11 | 12 | struct rawdev; 13 | 14 | struct rawdev_ops { 15 | int (*open)(struct rawdev *dev); 16 | void (*close)(struct rawdev *dev); 17 | void (*rx)(struct rawdev *dev, void (*callback)(uint8_t *, size_t, void *), 18 | void *arg, int timeout); 19 | ssize_t (*tx)(struct rawdev *dev, const uint8_t *buf, size_t len); 20 | int (*addr)(struct rawdev *dev, uint8_t *dst, size_t size); 21 | }; 22 | 23 | struct rawdev { 24 | uint8_t type; 25 | char *name; 26 | struct rawdev_ops *ops; 27 | void *priv; 28 | }; 29 | 30 | struct rawdev *rawdev_alloc(uint8_t type, char *name); 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /raw/soc.c: -------------------------------------------------------------------------------- 1 | #include "soc.h" 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 | struct soc_dev { 16 | int fd; 17 | }; 18 | 19 | struct soc_dev *soc_dev_open(char *name) { 20 | struct soc_dev *dev; 21 | struct ifreq ifr; 22 | struct sockaddr_ll sockaddr; 23 | 24 | dev = malloc(sizeof(struct soc_dev)); 25 | dev->fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); 26 | if (dev->fd == -1) { 27 | perror("socket()"); 28 | goto ERROR; 29 | } 30 | 31 | // find device interface index 32 | strncpy(ifr.ifr_name, name, sizeof(ifr.ifr_name) - 1); 33 | if (ioctl(dev->fd, SIOCGIFINDEX, &ifr) == -1) { 34 | perror("ioctl [SIOCGIFINDEX]"); 35 | goto ERROR; 36 | } 37 | 38 | // bind device interface 39 | memset(&sockaddr, 0, sizeof(sockaddr)); 40 | sockaddr.sll_family = AF_PACKET; 41 | sockaddr.sll_protocol = htons(ETH_P_ALL); 42 | sockaddr.sll_ifindex = ifr.ifr_ifindex; 43 | if (bind(dev->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) == -1) { 44 | perror("bind"); 45 | goto ERROR; 46 | } 47 | 48 | // set IFF_PROMISC flag 49 | if (ioctl(dev->fd, SIOCGIFFLAGS, &ifr) == -1) { 50 | perror("ioctl [SIOCGIFFLAGS]"); 51 | goto ERROR; 52 | } 53 | ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC; 54 | if (ioctl(dev->fd, SIOCSIFFLAGS, &ifr) == -1) { 55 | perror("ioctl [SIOCSIFFLAGS]"); 56 | goto ERROR; 57 | } 58 | 59 | return dev; 60 | 61 | ERROR: 62 | if (dev) { 63 | soc_dev_close(dev); 64 | } 65 | return NULL; 66 | } 67 | 68 | void soc_dev_close(struct soc_dev *dev) { 69 | if (dev->fd != -1) { 70 | close(dev->fd); 71 | } 72 | free(dev); 73 | } 74 | 75 | void soc_dev_rx(struct soc_dev *dev, 76 | void (*callback)(uint8_t *, size_t, void *), void *arg, 77 | int timeout) { 78 | struct pollfd pfd; 79 | int ret; 80 | ssize_t len; 81 | uint8_t buf[2048]; 82 | 83 | // wait until packet arrives 84 | pfd.fd = dev->fd; 85 | pfd.events = POLLIN; 86 | ret = poll(&pfd, 1, timeout); 87 | switch (ret) { 88 | case -1: 89 | if (errno != EINTR) { 90 | perror("poll"); 91 | } 92 | case 0: /* timeout */ 93 | return; 94 | } 95 | 96 | len = read(dev->fd, buf, sizeof(buf)); 97 | switch (len) { 98 | case -1: 99 | perror("read"); 100 | 101 | case 0: /* EOF */ 102 | return; 103 | } 104 | callback(buf, len, arg); 105 | } 106 | 107 | ssize_t soc_dev_tx(struct soc_dev *dev, const uint8_t *buf, size_t len) { 108 | return write(dev->fd, buf, len); 109 | } 110 | 111 | int soc_dev_addr(char *name, uint8_t *dst, size_t size) { 112 | int fd; 113 | struct ifreq ifr; 114 | 115 | fd = socket(AF_INET, SOCK_DGRAM, 0); 116 | if (fd == -1) { 117 | perror("socket"); 118 | return -1; 119 | } 120 | ifr.ifr_addr.sa_family = AF_INET; 121 | strncpy(ifr.ifr_name, name, sizeof(ifr.ifr_name) - 1); 122 | if (ioctl(fd, SIOCGIFHWADDR, &ifr) == -1) { 123 | perror("ioctl [SIOCGIFHWADDR]"); 124 | close(fd); 125 | return -1; 126 | } 127 | memcpy(dst, ifr.ifr_hwaddr.sa_data, size); 128 | close(fd); 129 | return 0; 130 | } 131 | 132 | #include "raw.h" 133 | 134 | static int soc_dev_open_wrap(struct rawdev *dev) { 135 | dev->priv = soc_dev_open(dev->name); 136 | return dev->priv ? 0 : -1; 137 | } 138 | 139 | static void soc_dev_close_wrap(struct rawdev *dev) { soc_dev_close(dev->priv); } 140 | 141 | static void soc_dev_rx_wrap(struct rawdev *dev, 142 | void (*callback)(uint8_t *, size_t, void *), 143 | void *arg, int timeout) { 144 | soc_dev_rx(dev->priv, callback, arg, timeout); 145 | } 146 | 147 | static ssize_t soc_dev_tx_wrap(struct rawdev *dev, const uint8_t *buf, 148 | size_t len) { 149 | return soc_dev_tx(dev->priv, buf, len); 150 | } 151 | 152 | static int soc_dev_addr_wrap(struct rawdev *dev, uint8_t *dst, size_t size) { 153 | return soc_dev_addr(dev->name, dst, size); 154 | } 155 | 156 | struct rawdev_ops soc_dev_ops = { 157 | .open = soc_dev_open_wrap, 158 | .close = soc_dev_close_wrap, 159 | .rx = soc_dev_rx_wrap, 160 | .tx = soc_dev_tx_wrap, 161 | .addr = soc_dev_addr_wrap, 162 | }; 163 | -------------------------------------------------------------------------------- /raw/soc.h: -------------------------------------------------------------------------------- 1 | #ifndef SOC_DEV_H 2 | #define SOC_DEV_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | struct soc_dev; 9 | 10 | struct soc_dev *soc_dev_open(char *name); 11 | void soc_dev_close(struct soc_dev *dev); 12 | void soc_dev_rx(struct soc_dev *dev, 13 | void (*callback)(uint8_t *, size_t, void *), void *arg, 14 | int timeout); 15 | ssize_t soc_dev_tx(struct soc_dev *dev, const uint8_t *buf, size_t len); 16 | int soc_dev_addr(char *name, uint8_t *dst, size_t size); 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /raw/tap.h: -------------------------------------------------------------------------------- 1 | #ifndef TAP_DEV_H 2 | #define TAP_DEV_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | struct tap_dev; 9 | 10 | struct tap_dev *tap_dev_open(char *name); 11 | void tap_dev_close(struct tap_dev *dev); 12 | void tap_dev_rx(struct tap_dev *dev, 13 | void (*callback)(uint8_t *, size_t, void *), void *arg, 14 | int timeout); 15 | ssize_t tap_dev_tx(struct tap_dev *dev, const uint8_t *buf, size_t len); 16 | int tap_dev_addr(char *name, uint8_t *dst, size_t size); 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /raw/tap_linux.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 "raw/tap.h" 14 | 15 | #define CLONE_DEVICE "/dev/net/tun" 16 | 17 | struct tap_dev { 18 | int fd; 19 | }; 20 | 21 | struct tap_dev *tap_dev_open(char *name) { 22 | struct tap_dev *dev; 23 | struct ifreq ifr; 24 | 25 | dev = malloc(sizeof(struct tap_dev)); 26 | if (!dev) { 27 | fprintf(stderr, "malloc: failure\n"); 28 | goto ERROR; 29 | } 30 | dev->fd = open(CLONE_DEVICE, O_RDWR); 31 | if (dev->fd == -1) { 32 | perror("open"); 33 | goto ERROR; 34 | } 35 | 36 | // setup tap device 37 | strncpy(ifr.ifr_name, name, sizeof(ifr.ifr_name) - 1); 38 | ifr.ifr_flags = IFF_TAP | IFF_NO_PI; 39 | if (ioctl(dev->fd, TUNSETIFF, &ifr) == -1) { 40 | perror("ioctl [TUNSETIFF]"); 41 | goto ERROR; 42 | } 43 | return dev; 44 | 45 | ERROR: 46 | if (dev) { 47 | tap_dev_close(dev); 48 | } 49 | return NULL; 50 | } 51 | 52 | void tap_dev_close(struct tap_dev *dev) { 53 | if (dev->fd != -1) { 54 | close(dev->fd); 55 | } 56 | free(dev); 57 | } 58 | 59 | void tap_dev_rx(struct tap_dev *dev, 60 | void (*callback)(uint8_t *, size_t, void *), void *arg, 61 | int timeout) { 62 | struct pollfd pfd; 63 | int ret; 64 | ssize_t len; 65 | uint8_t buf[2048]; 66 | 67 | // wait until packet arrives 68 | pfd.fd = dev->fd; 69 | pfd.events = POLLIN; 70 | ret = poll(&pfd, 1, timeout); 71 | switch (ret) { 72 | case -1: 73 | if (errno != EINTR) { 74 | perror("poll"); 75 | } 76 | case 0: /* timeout */ 77 | return; 78 | } 79 | 80 | len = read(dev->fd, buf, sizeof(buf)); 81 | switch (len) { 82 | case -1: 83 | perror("read"); 84 | 85 | case 0: /* EOF */ 86 | return; 87 | } 88 | callback(buf, len, arg); 89 | } 90 | 91 | ssize_t tap_dev_tx(struct tap_dev *dev, const uint8_t *buf, size_t len) { 92 | return write(dev->fd, buf, len); 93 | } 94 | 95 | int tap_dev_addr(char *name, uint8_t *dst, size_t size) { 96 | int fd; 97 | struct ifreq ifr; 98 | 99 | fd = socket(AF_INET, SOCK_DGRAM, 0); 100 | if (fd == -1) { 101 | perror("socket"); 102 | return -1; 103 | } 104 | ifr.ifr_addr.sa_family = AF_INET; 105 | strncpy(ifr.ifr_name, name, sizeof(ifr.ifr_name) - 1); 106 | if (ioctl(fd, SIOCGIFHWADDR, &ifr) == -1) { 107 | perror("ioctl [SIOCGIFHWADDR]"); 108 | close(fd); 109 | return -1; 110 | } 111 | memcpy(dst, ifr.ifr_hwaddr.sa_data, size); 112 | close(fd); 113 | return 0; 114 | } 115 | 116 | #include "raw.h" 117 | 118 | static int tap_dev_open_wrap(struct rawdev *dev) { 119 | dev->priv = tap_dev_open(dev->name); 120 | return dev->priv ? 0 : -1; 121 | } 122 | 123 | static void tap_dev_close_wrap(struct rawdev *dev) { tap_dev_close(dev->priv); } 124 | 125 | static void tap_dev_rx_wrap(struct rawdev *dev, 126 | void (*callback)(uint8_t *, size_t, void *), 127 | void *arg, int timeout) { 128 | tap_dev_rx(dev->priv, callback, arg, timeout); 129 | } 130 | 131 | static ssize_t tap_dev_tx_wrap(struct rawdev *dev, const uint8_t *buf, 132 | size_t len) { 133 | return tap_dev_tx(dev->priv, buf, len); 134 | } 135 | 136 | static int tap_dev_addr_wrap(struct rawdev *dev, uint8_t *dst, size_t size) { 137 | return tap_dev_addr(dev->name, dst, size); 138 | } 139 | 140 | struct rawdev_ops tap_dev_ops = { 141 | .open = tap_dev_open_wrap, 142 | .close = tap_dev_close_wrap, 143 | .rx = tap_dev_rx_wrap, 144 | .tx = tap_dev_tx_wrap, 145 | .addr = tap_dev_addr_wrap, 146 | }; 147 | -------------------------------------------------------------------------------- /tcp.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "ip.h" 9 | #include "util.h" 10 | 11 | // TODO: user timeout should set by user 12 | #define USER_TIMEOUT (20) /* user timeout (seconds) */ 13 | #define TIME_WAIT_TIMEOUT (2 * 10) /* TIME_WAIT timeout (seconds) */ 14 | #define TCP_SND_BUF_SIZE (10 * 1024) 15 | 16 | #define TCP_CB_TABLE_SIZE 128 17 | #define TCP_SOURCE_PORT_MIN 49152 18 | #define TCP_SOURCE_PORT_MAX 65535 19 | 20 | #define TCP_CB_STATE_CLOSED 0 21 | #define TCP_CB_STATE_LISTEN 1 22 | #define TCP_CB_STATE_SYN_SENT 2 23 | #define TCP_CB_STATE_SYN_RCVD 3 24 | #define TCP_CB_STATE_ESTABLISHED 4 25 | #define TCP_CB_STATE_FIN_WAIT1 5 26 | #define TCP_CB_STATE_FIN_WAIT2 6 27 | #define TCP_CB_STATE_CLOSING 7 28 | #define TCP_CB_STATE_TIME_WAIT 8 29 | #define TCP_CB_STATE_CLOSE_WAIT 9 30 | #define TCP_CB_STATE_LAST_ACK 10 31 | 32 | #define TCP_FLG_FIN 0x01 33 | #define TCP_FLG_SYN 0x02 34 | #define TCP_FLG_RST 0x04 35 | #define TCP_FLG_PSH 0x08 36 | #define TCP_FLG_ACK 0x10 37 | #define TCP_FLG_URG 0x20 38 | 39 | #define TCP_FLG_IS(x, y) (((x)&0x3f) == (y)) 40 | #define TCP_FLG_ISSET(x, y) (((x)&0x3f) & (y)) 41 | 42 | #define TCP_HDR_LEN(hdr) (((hdr)->off >> 4) << 2) 43 | #define TCP_DATA_LEN(hdr, len) ((len)-TCP_HDR_LEN(hdr)) 44 | 45 | #define IS_FREE_CB(cb) (!(cb)->used && (cb)->state == TCP_CB_STATE_CLOSED) 46 | 47 | #define TCP_SOCKET_INVALID(x) ((x) < 0 || (x) >= TCP_CB_TABLE_SIZE) 48 | 49 | #ifndef TCP_DEBUG 50 | #ifdef DEBUG 51 | #define TCP_DEBUG 1 52 | #endif 53 | #endif 54 | 55 | struct tcp_hdr { 56 | uint16_t src; 57 | uint16_t dst; 58 | uint32_t seq; 59 | uint32_t ack; 60 | uint8_t off; 61 | uint8_t flg; 62 | uint16_t win; 63 | uint16_t sum; 64 | uint16_t urg; 65 | }; 66 | 67 | struct tcp_txq_entry { 68 | struct tcp_hdr *segment; 69 | uint16_t len; 70 | struct timeval timestamp; 71 | struct tcp_txq_entry *next; 72 | }; 73 | 74 | struct tcp_txq_head { 75 | struct tcp_txq_entry *head; 76 | struct tcp_txq_entry *tail; 77 | uint16_t snt; 78 | }; 79 | 80 | struct tcp_cb { 81 | uint8_t used; 82 | uint8_t state; 83 | struct netif *iface; 84 | uint16_t port; // network byte order 85 | struct { 86 | ip_addr_t addr; 87 | uint16_t port; // network byte order 88 | } peer; 89 | struct { 90 | uint32_t nxt; 91 | uint32_t una; 92 | uint16_t up; 93 | uint32_t wl1; 94 | uint32_t wl2; 95 | uint16_t wnd; 96 | } snd; 97 | uint32_t iss; 98 | struct { 99 | uint32_t nxt; 100 | uint16_t up; 101 | uint16_t wnd; 102 | } rcv; 103 | uint32_t irs; 104 | struct tcp_txq_head txq; 105 | uint8_t window[65535]; 106 | struct tcp_cb *parent; 107 | struct queue_head backlog; 108 | pthread_cond_t cond; 109 | long timeout; 110 | }; 111 | 112 | static struct tcp_cb cb_table[TCP_CB_TABLE_SIZE]; 113 | static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; 114 | static pthread_t timer_thread; 115 | static pthread_cond_t timer_cond; 116 | 117 | static ssize_t tcp_tx(struct tcp_cb *cb, uint32_t seq, uint32_t ack, 118 | uint8_t flg, struct timeval *now, uint8_t *buf, 119 | size_t len); 120 | 121 | static char *tcp_flg_ntop(uint8_t flg, char *buf, int len) { 122 | int i = 0; 123 | if (TCP_FLG_ISSET(flg, TCP_FLG_FIN)) { 124 | buf[i++] = 'F'; 125 | } 126 | if (TCP_FLG_ISSET(flg, TCP_FLG_SYN)) { 127 | buf[i++] = 'S'; 128 | } 129 | if (TCP_FLG_ISSET(flg, TCP_FLG_RST)) { 130 | buf[i++] = 'R'; 131 | } 132 | if (TCP_FLG_ISSET(flg, TCP_FLG_PSH)) { 133 | buf[i++] = 'P'; 134 | } 135 | if (TCP_FLG_ISSET(flg, TCP_FLG_ACK)) { 136 | buf[i++] = 'A'; 137 | } 138 | if (TCP_FLG_ISSET(flg, TCP_FLG_URG)) { 139 | buf[i++] = 'U'; 140 | } 141 | buf[i] = 0; 142 | return buf; 143 | } 144 | 145 | static char *tcp_state_ntop(uint8_t state) { 146 | switch (state) { 147 | case TCP_CB_STATE_CLOSED: 148 | return "CLOSED"; 149 | case TCP_CB_STATE_LISTEN: 150 | return "LISTEN"; 151 | case TCP_CB_STATE_SYN_SENT: 152 | return "SYN_SENT"; 153 | case TCP_CB_STATE_SYN_RCVD: 154 | return "SYN_RCVD"; 155 | case TCP_CB_STATE_ESTABLISHED: 156 | return "ESTABLISHED"; 157 | case TCP_CB_STATE_FIN_WAIT1: 158 | return "FIN_WAIT1"; 159 | case TCP_CB_STATE_FIN_WAIT2: 160 | return "FIN_WAIT2"; 161 | case TCP_CB_STATE_CLOSING: 162 | return "CLOSING"; 163 | case TCP_CB_STATE_TIME_WAIT: 164 | return "TIME_WAIT"; 165 | case TCP_CB_STATE_CLOSE_WAIT: 166 | return "CLOSE_WAIT"; 167 | case TCP_CB_STATE_LAST_ACK: 168 | return "LAST_ACK"; 169 | 170 | default: 171 | return "UNKNOWN"; 172 | } 173 | } 174 | 175 | static void tcp_state_dump(struct tcp_cb *cb) { 176 | char buf[64]; 177 | 178 | fprintf(stderr, " used: %d\n", cb->used); 179 | fprintf(stderr, " state: %s\n", tcp_state_ntop(cb->state)); 180 | fprintf(stderr, " self.port: %u\n", ntoh16(cb->port)); 181 | fprintf(stderr, " peer.addr: %s\n", ip_addr_ntop(&cb->peer.addr, buf, 64)); 182 | fprintf(stderr, " peer.port: %u\n", ntoh16(cb->peer.port)); 183 | fprintf(stderr, " snd.nxt: %u\n", cb->snd.nxt); 184 | fprintf(stderr, " snd.una: %u\n", cb->snd.una); 185 | fprintf(stderr, " snd.wnd: %u\n", cb->snd.wnd); 186 | fprintf(stderr, " txq.snt: %u\n", cb->txq.snt); 187 | fprintf(stderr, " rcv.nxt: %u\n", cb->rcv.nxt); 188 | fprintf(stderr, " rcv.wnd: %u\n", cb->rcv.wnd); 189 | fprintf(stderr, " n_backlog: %u\n", cb->backlog.num); 190 | fprintf(stderr, " timeout: %ld\n", cb->timeout); 191 | } 192 | 193 | static void tcp_dump(struct tcp_cb *cb, struct tcp_hdr *hdr, size_t plen) { 194 | char buf[64]; 195 | 196 | tcp_state_dump(cb); 197 | fprintf(stderr, " len: %lu\n", plen); 198 | fprintf(stderr, " src: %u\n", ntoh16(hdr->src)); 199 | fprintf(stderr, " dst: %u\n", ntoh16(hdr->dst)); 200 | fprintf(stderr, " seq: %u\n", ntoh32(hdr->seq)); 201 | fprintf(stderr, " ack: %u\n", ntoh32(hdr->ack)); 202 | fprintf(stderr, " off: %u\n", hdr->off); 203 | fprintf(stderr, " flg: [%s]\n", tcp_flg_ntop(hdr->flg, buf, 64)); 204 | fprintf(stderr, " win: %u\n", ntoh16(hdr->win)); 205 | fprintf(stderr, " sum: %u\n", ntoh16(hdr->sum)); 206 | fprintf(stderr, " urg: %u\n", ntoh16(hdr->urg)); 207 | } 208 | 209 | static uint16_t tcp_checksum(ip_addr_t self, ip_addr_t peer, uint8_t *segment, 210 | size_t len) { 211 | uint32_t pseudo = 0; 212 | 213 | pseudo += (self >> 16) & 0xffff; 214 | pseudo += self & 0xffff; 215 | pseudo += (peer >> 16) & 0xffff; 216 | pseudo += self & 0xffff; 217 | pseudo += hton16((uint16_t)IP_PROTOCOL_TCP); 218 | pseudo += hton16(len); 219 | return cksum16((uint16_t *)segment, len, pseudo); 220 | } 221 | 222 | /* 223 | * Segment Queue 224 | */ 225 | 226 | static struct tcp_txq_entry *tcp_txq_add(struct tcp_cb *cb, struct tcp_hdr *hdr, 227 | size_t len) { 228 | struct tcp_txq_entry *txq; 229 | 230 | txq = malloc(sizeof(struct tcp_txq_entry)); 231 | if (!txq) { 232 | return NULL; 233 | } 234 | txq->segment = hdr; 235 | txq->len = len; 236 | // clear timestamp 237 | memset(&txq->timestamp, 0, sizeof(txq->timestamp)); 238 | txq->next = NULL; 239 | // set txq to next of tail entry 240 | if (cb->txq.head == NULL) { 241 | cb->txq.head = txq; 242 | } else { 243 | cb->txq.tail->next = txq; 244 | } 245 | // update tail entry 246 | cb->txq.tail = txq; 247 | 248 | return txq; 249 | } 250 | 251 | static void tcp_txq_clear_all(struct tcp_cb *cb) { 252 | struct tcp_txq_entry *txq = cb->txq.head, *next; 253 | while (txq) { 254 | next = txq->next; 255 | free(txq->segment); 256 | free(txq); 257 | txq = next; 258 | } 259 | cb->txq.head = cb->txq.tail = NULL; 260 | } 261 | 262 | /* 263 | * EVENT PROCESSING 264 | * https://tools.ietf.org/html/rfc793#section-3.9 265 | */ 266 | 267 | static void tcp_close_cb(struct tcp_cb *cb) { 268 | cb->state = TCP_CB_STATE_CLOSED; 269 | memset(&cb->snd, 0, sizeof(cb->snd)); 270 | cb->iss = 0; 271 | memset(&cb->rcv, 0, sizeof(cb->rcv)); 272 | cb->irs = 0; 273 | tcp_txq_clear_all(cb); 274 | return; 275 | } 276 | 277 | // SEGMENT ARRIVES 278 | // https://tools.ietf.org/html/rfc793#page-65 279 | static void tcp_event_segment_arrives(struct tcp_cb *cb, struct tcp_hdr *hdr, 280 | size_t len) { 281 | size_t plen; 282 | int acceptable = 0; 283 | struct timeval now; 284 | 285 | plen = TCP_DATA_LEN(hdr, len); 286 | if (gettimeofday(&now, NULL) == -1) { 287 | perror("gettimeofday"); 288 | return; 289 | } 290 | 291 | switch (cb->state) { 292 | case TCP_CB_STATE_CLOSED: 293 | if (!TCP_FLG_ISSET(hdr->flg, TCP_FLG_RST)) { 294 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_ACK)) { 295 | tcp_tx(cb, ntoh32(hdr->ack), 0, TCP_FLG_RST, &now, NULL, 0); 296 | } else { 297 | tcp_tx(cb, 0, ntoh32(hdr->seq) + plen, TCP_FLG_RST | TCP_FLG_ACK, 298 | &now, NULL, 0); 299 | } 300 | } 301 | return; 302 | 303 | case TCP_CB_STATE_LISTEN: 304 | // first check for an RST 305 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_RST)) { 306 | // incoming RST is ignored 307 | goto ERROR_RX_LISTEN; 308 | } 309 | 310 | // second check for an ACK 311 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_ACK)) { 312 | tcp_tx(cb, ntoh32(hdr->ack), 0, TCP_FLG_RST, &now, NULL, 0); 313 | goto ERROR_RX_LISTEN; 314 | } 315 | 316 | // third check for a SYN 317 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_SYN)) { 318 | // TODO: check the security 319 | 320 | // TODO: If the SEG.PRC is greater than the TCB.PRC 321 | 322 | // else 323 | cb->rcv.wnd = sizeof(cb->window); 324 | cb->rcv.nxt = ntoh32(hdr->seq) + 1; 325 | cb->irs = ntoh32(hdr->seq); 326 | cb->iss = (uint32_t)random(); 327 | tcp_tx(cb, cb->iss, cb->rcv.nxt, TCP_FLG_SYN | TCP_FLG_ACK, &now, NULL, 328 | 0); 329 | cb->snd.nxt = cb->iss + 1; 330 | cb->snd.una = cb->iss; 331 | cb->timeout = now.tv_sec + USER_TIMEOUT; 332 | cb->state = TCP_CB_STATE_SYN_RCVD; 333 | 334 | // TODO: ? queue to backlog ? 335 | // TODO: increment hdr->seq for save text 336 | goto CHECK_URG; 337 | } 338 | 339 | // no packet should come here. drop segment 340 | ERROR_RX_LISTEN: 341 | // return state to CLOSED 342 | tcp_close_cb(cb); 343 | pthread_cond_broadcast(&cb->cond); 344 | cb->parent = NULL; 345 | return; 346 | 347 | case TCP_CB_STATE_SYN_SENT: 348 | // first check the ACK bit 349 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_ACK)) { 350 | if (ntoh32(hdr->ack) <= cb->iss || ntoh32(hdr->ack) > cb->snd.nxt) { 351 | tcp_tx(cb, ntoh32(hdr->ack), 0, TCP_FLG_RST, &now, NULL, 0); 352 | return; 353 | } 354 | if (cb->snd.una <= ntoh32(hdr->ack) && 355 | ntoh32(hdr->ack) <= cb->snd.nxt) { 356 | acceptable = 1; 357 | } else { 358 | // drop invalid ack 359 | return; 360 | } 361 | } 362 | 363 | // second check the RST bit 364 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_RST)) { 365 | if (!acceptable) { 366 | // drop segment 367 | return; 368 | } 369 | fprintf(stderr, "error: connection reset\n"); 370 | tcp_close_cb(cb); 371 | pthread_cond_signal(&cb->cond); 372 | return; 373 | } 374 | 375 | // TODO: third check the security and precedence 376 | 377 | // fourth check the SYN bit 378 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_SYN)) { 379 | cb->rcv.nxt = ntoh32(hdr->seq) + 1; 380 | cb->irs = ntoh32(hdr->seq); 381 | // TODO: ? if there is an ACK ? 382 | if (cb->snd.una < ntoh32(hdr->ack)) { 383 | // update snd.una and user timeout 384 | cb->snd.una = ntoh32(hdr->ack); 385 | cb->timeout = now.tv_sec + USER_TIMEOUT; 386 | pthread_cond_signal(&timer_cond); 387 | } 388 | 389 | // TODO: clear all retransmission queue 390 | 391 | if (cb->snd.una > cb->iss) { 392 | // our SYN has been ACKed 393 | cb->state = TCP_CB_STATE_ESTABLISHED; 394 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_ACK, &now, NULL, 0); 395 | pthread_cond_signal(&cb->cond); 396 | if (plen > 0 || TCP_FLG_ISSET(hdr->flg, TCP_FLG_URG)) { 397 | goto CHECK_URG; 398 | } 399 | return; 400 | } else { 401 | cb->state = TCP_CB_STATE_SYN_RCVD; 402 | tcp_tx(cb, cb->iss, cb->rcv.nxt, TCP_FLG_SYN | TCP_FLG_ACK, &now, 403 | NULL, 0); 404 | pthread_cond_signal(&cb->cond); 405 | // TODO: If there are other controls or text in the segment, queue 406 | // them for processing after the ESTABLISHED state has been reached, 407 | return; 408 | } 409 | } 410 | 411 | // fifth, if neither of the SYN or RST bits is set 412 | if (!TCP_FLG_ISSET(hdr->flg, TCP_FLG_ACK | TCP_FLG_RST)) { 413 | // drop segment 414 | return; 415 | } 416 | 417 | return; 418 | 419 | case TCP_CB_STATE_SYN_RCVD: 420 | case TCP_CB_STATE_ESTABLISHED: 421 | case TCP_CB_STATE_FIN_WAIT1: 422 | case TCP_CB_STATE_FIN_WAIT2: 423 | case TCP_CB_STATE_CLOSING: 424 | case TCP_CB_STATE_TIME_WAIT: 425 | case TCP_CB_STATE_CLOSE_WAIT: 426 | case TCP_CB_STATE_LAST_ACK: 427 | break; 428 | 429 | default: 430 | fprintf(stderr, ">>> segment recv : not implement tcp state (%d) <<<\n", 431 | cb->state); 432 | return; 433 | } 434 | 435 | // first check sequence number 436 | if (plen > 0) { 437 | if (cb->rcv.wnd > 0) { 438 | acceptable = (cb->rcv.nxt <= ntoh32(hdr->seq) && 439 | ntoh32(hdr->seq) < cb->rcv.nxt + cb->rcv.wnd) || 440 | (cb->rcv.nxt <= ntoh32(hdr->seq) && 441 | ntoh32(hdr->seq) + plen - 1 < cb->rcv.nxt + cb->rcv.wnd); 442 | } else { 443 | acceptable = 0; 444 | } 445 | } else { 446 | if (cb->rcv.wnd > 0) { 447 | acceptable = (cb->rcv.nxt <= ntoh32(hdr->seq) && 448 | ntoh32(hdr->seq) < cb->rcv.nxt + cb->rcv.wnd); 449 | } else { 450 | acceptable = ntoh32(hdr->seq) == cb->rcv.nxt; 451 | } 452 | } 453 | 454 | if (!acceptable) { 455 | if (!TCP_FLG_ISSET(hdr->flg, TCP_FLG_RST)) { 456 | fprintf(stderr, "is not acceptable !!!\n"); 457 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_ACK, &now, NULL, 0); 458 | } 459 | // drop segment 460 | return; 461 | } 462 | 463 | // second check the RST bit 464 | switch (cb->state) { 465 | case TCP_CB_STATE_SYN_RCVD: 466 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_RST)) { 467 | // close connection 468 | tcp_close_cb(cb); 469 | pthread_cond_signal(&cb->cond); 470 | return; 471 | } 472 | break; 473 | 474 | case TCP_CB_STATE_ESTABLISHED: 475 | case TCP_CB_STATE_FIN_WAIT1: 476 | case TCP_CB_STATE_FIN_WAIT2: 477 | case TCP_CB_STATE_CLOSE_WAIT: 478 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_RST)) { 479 | // TODO: signal to SEND, RECEIVE waiting thread. 480 | tcp_close_cb(cb); 481 | pthread_cond_broadcast(&cb->cond); 482 | return; 483 | } 484 | break; 485 | 486 | case TCP_CB_STATE_CLOSING: 487 | case TCP_CB_STATE_TIME_WAIT: 488 | case TCP_CB_STATE_LAST_ACK: 489 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_RST)) { 490 | // close connection 491 | tcp_close_cb(cb); 492 | pthread_cond_broadcast(&cb->cond); 493 | return; 494 | } 495 | break; 496 | 497 | default: 498 | fprintf(stderr, ">>> seg recv (2) : not implement tcp state (%d) <<<\n", 499 | cb->state); 500 | break; 501 | } 502 | 503 | // TODO: third check security and precedence 504 | 505 | // fourth, check the SYN bit 506 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_SYN)) { 507 | tcp_tx(cb, 0, cb->rcv.nxt, TCP_FLG_RST, &now, NULL, 0); 508 | tcp_close_cb(cb); 509 | pthread_cond_broadcast(&cb->cond); 510 | return; 511 | } 512 | // TODO: ? If the SYN is not in the window this step would not be reached and 513 | // an ack would have been sent in the first step (sequence number check). ? 514 | 515 | // fifth check the ACK field 516 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_ACK)) { 517 | switch (cb->state) { 518 | case TCP_CB_STATE_SYN_RCVD: 519 | if (!(cb->snd.una <= ntoh32(hdr->ack) && 520 | ntoh32(hdr->ack) <= cb->snd.nxt)) { 521 | // hdr->ack is not acceptable 522 | tcp_tx(cb, ntoh32(hdr->ack), cb->rcv.nxt, TCP_FLG_RST, &now, NULL, 0); 523 | // The connection remains in the same state after send RST 524 | break; 525 | } 526 | cb->state = TCP_CB_STATE_ESTABLISHED; 527 | if (cb->parent) { 528 | // add cb to backlog 529 | queue_push(&cb->parent->backlog, cb, sizeof(*cb)); 530 | pthread_cond_signal(&cb->parent->cond); 531 | } else { 532 | // parent == NULL means cb is created by user and first state was 533 | // SYN_SENT 534 | pthread_cond_signal(&cb->cond); 535 | } 536 | 537 | // enter ESTABLISHED state and continue processing 538 | case TCP_CB_STATE_ESTABLISHED: 539 | case TCP_CB_STATE_FIN_WAIT1: 540 | case TCP_CB_STATE_FIN_WAIT2: 541 | case TCP_CB_STATE_CLOSE_WAIT: 542 | case TCP_CB_STATE_CLOSING: 543 | if (cb->snd.una <= ntoh32(hdr->ack) && 544 | ntoh32(hdr->ack) <= cb->snd.nxt) { 545 | if (cb->snd.una < ntoh32(hdr->ack)) { 546 | // update snd.una and user timeout 547 | cb->snd.una = ntoh32(hdr->ack); 548 | cb->timeout = now.tv_sec + USER_TIMEOUT; 549 | pthread_cond_signal(&timer_cond); 550 | } 551 | // TODO: retransmission queue send 552 | pthread_cond_broadcast(&cb->cond); 553 | 554 | if ((cb->snd.wl1 < ntoh32(hdr->seq)) || 555 | (cb->snd.wl1 == ntoh32(hdr->seq) && 556 | cb->snd.wl2 <= ntoh32(hdr->ack))) { 557 | cb->snd.wnd = ntoh16(hdr->win); 558 | cb->snd.wl1 = ntoh32(hdr->seq); 559 | cb->snd.wl2 = ntoh32(hdr->ack); 560 | } 561 | } else if (ntoh32(hdr->ack) > cb->snd.nxt) { 562 | fprintf(stderr, "recv ack but ack is advanced to snd.nxt\n"); 563 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_ACK, &now, NULL, 0); 564 | // drop the segment 565 | return; 566 | } 567 | // else if SEG.ACK < SND.UNA then it can be ignored 568 | 569 | if (cb->state == TCP_CB_STATE_FIN_WAIT1) { 570 | // if this ACK is for sent FIN 571 | if (ntoh32(hdr->ack) + 1 == cb->snd.nxt) { 572 | cb->state = TCP_CB_STATE_FIN_WAIT2; 573 | } 574 | } else if (cb->state == TCP_CB_STATE_FIN_WAIT2) { 575 | // TODO: if the retransmission queue is empty, the user's CLOSE can be 576 | // acknowledged ("ok") 577 | } else if (cb->state == TCP_CB_STATE_CLOSING) { 578 | // if this ACK is for sent FIN 579 | if (ntoh32(hdr->ack) + 1 == cb->snd.nxt) { 580 | cb->state = TCP_CB_STATE_TIME_WAIT; 581 | } 582 | } 583 | 584 | break; 585 | 586 | case TCP_CB_STATE_LAST_ACK: 587 | // if this ACK is for sent FIN 588 | if (ntoh32(hdr->ack) == cb->snd.nxt) { 589 | tcp_close_cb(cb); 590 | pthread_cond_broadcast(&cb->cond); 591 | return; 592 | } 593 | break; 594 | 595 | case TCP_CB_STATE_TIME_WAIT: 596 | // TODO: restart the 2 MSL timeout 597 | break; 598 | 599 | default: 600 | fprintf(stderr, ">>> seg recv (5) : not implement tcp state (%d) <<<\n", 601 | cb->state); 602 | break; 603 | } 604 | } 605 | 606 | CHECK_URG: 607 | // sixth, check the URG bit 608 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_URG)) { 609 | switch (cb->state) { 610 | case TCP_CB_STATE_ESTABLISHED: 611 | case TCP_CB_STATE_FIN_WAIT1: 612 | case TCP_CB_STATE_FIN_WAIT2: 613 | cb->rcv.up = MAX(cb->rcv.up, ntoh16(hdr->urg)); 614 | // TODO: signal 615 | 616 | break; 617 | 618 | case TCP_CB_STATE_CLOSING: 619 | case TCP_CB_STATE_TIME_WAIT: 620 | case TCP_CB_STATE_CLOSE_WAIT: 621 | case TCP_CB_STATE_LAST_ACK: 622 | // this should not occur. ignore the urg 623 | break; 624 | 625 | case TCP_CB_STATE_SYN_RCVD: 626 | // do nothing 627 | break; 628 | 629 | default: 630 | fprintf(stderr, ">>> seg recv (6) : not implement tcp state (%d) <<<\n", 631 | cb->state); 632 | break; 633 | } 634 | } 635 | 636 | // seventh, process the segment text 637 | switch (cb->state) { 638 | case TCP_CB_STATE_ESTABLISHED: 639 | case TCP_CB_STATE_FIN_WAIT1: 640 | case TCP_CB_STATE_FIN_WAIT2: 641 | // TODO: accept not ordered packet 642 | if (plen > 0 && cb->rcv.nxt == ntoh32(hdr->seq)) { 643 | // copy segment to receive buffer 644 | memcpy(cb->window + (sizeof(cb->window) - cb->rcv.wnd), 645 | (uint8_t *)hdr + TCP_HDR_LEN(hdr), plen); 646 | cb->rcv.nxt = ntoh32(hdr->seq) + plen; 647 | cb->rcv.wnd -= plen; 648 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_ACK, &now, NULL, 0); 649 | pthread_cond_broadcast(&cb->cond); 650 | } else if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_PSH)) { 651 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_ACK, &now, NULL, 0); 652 | pthread_cond_broadcast(&cb->cond); 653 | } 654 | break; 655 | 656 | case TCP_CB_STATE_CLOSING: 657 | case TCP_CB_STATE_TIME_WAIT: 658 | case TCP_CB_STATE_CLOSE_WAIT: 659 | case TCP_CB_STATE_LAST_ACK: 660 | // this should not occur. ignore the text 661 | break; 662 | 663 | case TCP_CB_STATE_SYN_RCVD: 664 | // do nothing 665 | break; 666 | 667 | default: 668 | fprintf(stderr, ">>> seg recv (7) : not implement tcp state (%d) <<<\n", 669 | cb->state); 670 | break; 671 | } 672 | 673 | // eighth, check the FIN bit 674 | if (TCP_FLG_ISSET(hdr->flg, TCP_FLG_FIN)) { 675 | cb->rcv.nxt = ntoh32(hdr->seq) + 1; 676 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_ACK, &now, NULL, 0); 677 | switch (cb->state) { 678 | case TCP_CB_STATE_SYN_RCVD: 679 | case TCP_CB_STATE_ESTABLISHED: 680 | cb->state = TCP_CB_STATE_CLOSE_WAIT; 681 | break; 682 | 683 | case TCP_CB_STATE_FIN_WAIT1: 684 | cb->state = TCP_CB_STATE_CLOSING; 685 | break; 686 | 687 | case TCP_CB_STATE_FIN_WAIT2: 688 | cb->state = TCP_CB_STATE_TIME_WAIT; 689 | // start time-wait timer 690 | cb->timeout = now.tv_sec + TIME_WAIT_TIMEOUT; 691 | // TODO: turn off other timers 692 | break; 693 | 694 | case TCP_CB_STATE_CLOSING: 695 | case TCP_CB_STATE_CLOSE_WAIT: 696 | case TCP_CB_STATE_LAST_ACK: 697 | // remain state 698 | break; 699 | 700 | case TCP_CB_STATE_TIME_WAIT: 701 | // remain state 702 | // restart the 2MSL timeout 703 | cb->timeout = now.tv_sec + TIME_WAIT_TIMEOUT; 704 | break; 705 | 706 | default: 707 | fprintf(stderr, ">>> seg recv (8) : not implement tcp state (%d) <<<\n", 708 | cb->state); 709 | break; 710 | } 711 | // signal "connection closing" 712 | pthread_cond_broadcast(&cb->cond); 713 | } 714 | return; 715 | } 716 | 717 | /* 718 | * TCP APPLICATION CONTROLLER 719 | */ 720 | 721 | // TODO: where to get current time. 722 | static ssize_t tcp_tx(struct tcp_cb *cb, uint32_t seq, uint32_t ack, 723 | uint8_t flg, struct timeval *now, uint8_t *buf, 724 | size_t len) { 725 | uint8_t *segment; 726 | struct tcp_hdr *hdr; 727 | ip_addr_t self, peer; 728 | struct tcp_txq_entry *txq = NULL; 729 | int have_unsent; 730 | 731 | // allocate segment 732 | segment = malloc(sizeof(struct tcp_hdr) + len); 733 | if (!segment) { 734 | return -1; 735 | } 736 | 737 | // set header params 738 | hdr = (struct tcp_hdr *)segment; 739 | memset(hdr, 0, sizeof(struct tcp_hdr)); 740 | hdr->src = cb->port; 741 | hdr->dst = cb->peer.port; 742 | hdr->seq = hton32(seq); 743 | hdr->ack = hton32(ack); 744 | hdr->off = (sizeof(struct tcp_hdr) >> 2) << 4; 745 | hdr->flg = flg; 746 | hdr->win = hton16(cb->rcv.wnd); 747 | hdr->sum = 0; 748 | hdr->urg = 0; 749 | 750 | // copy data 751 | memcpy(hdr + 1, buf, len); 752 | 753 | if (len > 0 || flg & (TCP_FLG_SYN | TCP_FLG_FIN)) { 754 | // add txq list only packets which have ack reply 755 | 756 | // check unsent segment exists in cb->txq 757 | have_unsent = cb->txq.tail && cb->txq.tail->timestamp.tv_sec == 0; 758 | 759 | // add txq 760 | txq = tcp_txq_add(cb, hdr, sizeof(struct tcp_hdr) + len); 761 | if (!txq) { 762 | free(segment); 763 | return -1; 764 | } 765 | 766 | // flow control 767 | // TODO: cb->txq.snt is not exactly the sliding window size 768 | if (have_unsent || (!TCP_FLG_ISSET(hdr->flg, TCP_FLG_SYN) && 769 | cb->txq.snt + len > cb->snd.wnd)) { 770 | // before established snd.wnd is 0. so SYN does not care snd.wnd 771 | // segment is stored in txq and send later in timer_thread 772 | fprintf(stderr, ">>> not send havesent %d %d<<<\n", have_unsent, 773 | cb->txq.tail); 774 | tcp_dump(cb, hdr, len); 775 | return 0; 776 | } 777 | } 778 | 779 | // calculate checksum 780 | self = ((struct netif_ip *)cb->iface)->unicast; 781 | peer = cb->peer.addr; 782 | hdr->sum = 783 | tcp_checksum(self, peer, (uint8_t *)hdr, sizeof(struct tcp_hdr) + len); 784 | 785 | #ifdef TCP_DEBUG 786 | fprintf(stderr, ">>> tcp_tx <<<\n"); 787 | tcp_dump(cb, hdr, len); 788 | #endif 789 | 790 | // send packet 791 | if (ip_tx(cb->iface, IP_PROTOCOL_TCP, (uint8_t *)hdr, 792 | sizeof(struct tcp_hdr) + len, &peer) == -1) { 793 | // failed to send ip packet 794 | if (!txq) { 795 | // this packet does not expect reply so not queued into txq list 796 | free(segment); 797 | } 798 | return -1; 799 | } 800 | 801 | if (txq) { 802 | // set timestamp 803 | txq->timestamp = *now; 804 | cb->txq.snt += len; 805 | } else { 806 | // this packet does not expect reply so not queued into txq list 807 | free(segment); 808 | } 809 | 810 | return len; 811 | } 812 | 813 | static void tcp_rx(uint8_t *segment, size_t len, ip_addr_t *src, ip_addr_t *dst, 814 | struct netif *iface) { 815 | struct tcp_hdr *hdr; 816 | int i; 817 | struct tcp_cb *cb, *fcb = NULL, *lcb = NULL; 818 | 819 | // validate tcp packet 820 | if (*dst != ((struct netif_ip *)iface)->unicast) { 821 | return; 822 | } 823 | if (len < sizeof(struct tcp_hdr)) { 824 | return; 825 | } 826 | 827 | // validate checksum 828 | hdr = (struct tcp_hdr *)segment; 829 | if (tcp_checksum(*src, *dst, segment, len) != 0) { 830 | fprintf(stderr, "tcp checksum error\n"); 831 | return; 832 | } 833 | 834 | pthread_mutex_lock(&mutex); 835 | 836 | // find connection cb or listener cb 837 | for (i = 0; i < TCP_CB_TABLE_SIZE; i++) { 838 | cb = &cb_table[i]; 839 | if (IS_FREE_CB(cb)) { 840 | // cache cb for the case SYN message and no cb is connected to this peer 841 | if (fcb == NULL) { 842 | fcb = cb; 843 | } 844 | } else if ((!cb->iface || cb->iface == iface) && cb->port == hdr->dst) { 845 | // if ip address and dst port number matches 846 | if (cb->peer.addr == *src && cb->peer.port == hdr->src) { 847 | // this cb is connection for this tcp packet 848 | break; 849 | } else if (cb->state == TCP_CB_STATE_LISTEN && !lcb) { 850 | // listener socket is found 851 | lcb = cb; 852 | } 853 | } 854 | } 855 | 856 | // cb that matches this tcp packet is not found. 857 | // create socket if listener socket exists and packet is SYN packet. 858 | if (i == TCP_CB_TABLE_SIZE) { 859 | if (!fcb) { 860 | // cb resource is run out 861 | // TODO: send RST 862 | pthread_mutex_unlock(&mutex); 863 | return; 864 | } 865 | 866 | // create accept socket 867 | cb = fcb; 868 | cb->iface = iface; 869 | if (lcb) { 870 | // TODO: ? if SYN is not set ? 871 | cb->state = lcb->state; 872 | cb->port = lcb->port; 873 | cb->parent = lcb; 874 | } else { 875 | // this port is not listened. 876 | // this packet is invalid. no connection is found. 877 | cb->port = 0; 878 | } 879 | cb->peer.addr = *src; 880 | cb->peer.port = hdr->src; 881 | } 882 | // else cb that matches this tcp packet is found. 883 | 884 | #ifdef TCP_DEBUG 885 | fprintf(stderr, ">>> tcp_rx <<<\n"); 886 | tcp_dump(cb, hdr, TCP_DATA_LEN(hdr, len)); 887 | #endif 888 | 889 | // handle message 890 | tcp_event_segment_arrives(cb, hdr, len); 891 | pthread_mutex_unlock(&mutex); 892 | return; 893 | } 894 | 895 | static void *tcp_timer_thread(void *arg) { 896 | struct timeval timestamp, diff; 897 | struct timespec timeout; 898 | struct tcp_cb *cb; 899 | struct tcp_txq_entry *txq, *prev, *tmp; 900 | ip_addr_t self, peer; 901 | size_t sum = 0; 902 | int i; 903 | 904 | diff.tv_sec = 0; 905 | diff.tv_usec = 100 * 1000; 906 | 907 | pthread_mutex_lock(&mutex); 908 | while (1) { 909 | gettimeofday(×tamp, NULL); 910 | for (i = 0; i < TCP_CB_TABLE_SIZE; i++) { 911 | cb = &cb_table[i]; 912 | 913 | if (cb->state == TCP_CB_STATE_CLOSED) { 914 | // skip check timeout 915 | continue; 916 | } 917 | 918 | // check user timeout 919 | if ((cb->snd.una != cb->snd.nxt || cb->state == TCP_CB_STATE_TIME_WAIT) && 920 | cb->timeout < timestamp.tv_sec) { 921 | // force close connection because of ack timeout or TIME_WAIT timeout 922 | 923 | #ifdef TCP_DEBUG 924 | fprintf(stderr, ">>> find user timeout (%ld - %ld) <<<\n", 925 | timestamp.tv_sec, cb->timeout); 926 | tcp_state_dump(cb); 927 | #endif 928 | tcp_close_cb(cb); 929 | pthread_cond_broadcast(&cb->cond); 930 | continue; 931 | } 932 | 933 | // check retransmission timeout and vacuum acked segment entry 934 | prev = NULL; 935 | txq = cb->txq.head; 936 | sum = 0; 937 | if (cb->txq.head) { 938 | self = ((struct netif_ip *)cb->iface)->unicast; 939 | peer = cb->peer.addr; 940 | } 941 | while (txq) { 942 | if (ntoh32(txq->segment->seq) >= cb->snd.una) { 943 | // TODO: check sum + datalen should compare with snd.wnd. but 944 | // txq->segment does not support splitting. so sum add later 945 | /* sum += TCP_DATA_LEN(txq->segment, txq->len); */ 946 | 947 | if (sum < cb->snd.wnd) { 948 | // this txq is in sliding send window 949 | 950 | if (txq->timestamp.tv_sec == 0) { 951 | // this txq is not sent 952 | 953 | // re-calc checksum 954 | txq->segment->ack = hton32(cb->rcv.nxt); 955 | txq->segment->sum = 0; 956 | txq->segment->sum = 957 | tcp_checksum(self, peer, txq->segment, txq->len); 958 | 959 | #ifdef TCP_DEBUG 960 | fprintf(stderr, ">>> tcp_tx in timer_thread <<<\n"); 961 | tcp_dump(cb, txq->segment, TCP_DATA_LEN(txq->segment, txq->len)); 962 | #endif 963 | 964 | // send packet 965 | ip_tx(cb->iface, IP_PROTOCOL_TCP, (uint8_t *)txq->segment, 966 | txq->len, &cb->peer.addr); 967 | txq->timestamp = timestamp; 968 | } else if (timestamp.tv_sec - txq->timestamp.tv_sec > 3) { 969 | // if retransmission timeout (3 seconds) then resend 970 | 971 | // re-calc checksum 972 | txq->segment->ack = hton32(cb->rcv.nxt); 973 | txq->segment->sum = 0; 974 | txq->segment->sum = 975 | tcp_checksum(self, peer, txq->segment, txq->len); 976 | 977 | #ifdef TCP_DEBUG 978 | fprintf(stderr, 979 | ">>> find retransmission timeout (%ld - %ld) <<<\n", 980 | timestamp.tv_sec, txq->timestamp.tv_sec); 981 | tcp_dump(cb, txq->segment, TCP_DATA_LEN(txq->segment, txq->len)); 982 | #endif 983 | 984 | // resend packet 985 | ip_tx(cb->iface, IP_PROTOCOL_TCP, (uint8_t *)txq->segment, 986 | txq->len, &cb->peer.addr); 987 | txq->timestamp = timestamp; 988 | } 989 | } 990 | 991 | // FIXME: temporary sum add here. no support splitting 992 | sum += TCP_DATA_LEN(txq->segment, txq->len); 993 | 994 | // update previous tcp_txq_entry 995 | prev = txq; 996 | txq = txq->next; 997 | } else { 998 | // remove tcp_txq_entry from list 999 | cb->txq.snt -= TCP_DATA_LEN(txq->segment, txq->len); 1000 | 1001 | // swap tail tcp_txq_entry 1002 | if (!txq->next) { 1003 | // txq is tail entry 1004 | cb->txq.tail = prev; 1005 | } 1006 | // swap previous tcp_txq_entry 1007 | if (prev) { 1008 | prev->next = txq->next; 1009 | } else { 1010 | cb->txq.head = txq->next; 1011 | } 1012 | 1013 | // free tcp_txq_entry 1014 | tmp = txq->next; 1015 | free(txq->segment); 1016 | free(txq); 1017 | // check next entry 1018 | txq = tmp; 1019 | } 1020 | } 1021 | } 1022 | // sleep 100 ms 1023 | timeradd(×tamp, &diff, ×tamp); 1024 | timeout.tv_sec = timestamp.tv_sec; 1025 | timeout.tv_nsec = timestamp.tv_usec * 1000; 1026 | pthread_cond_timedwait(&timer_cond, &mutex, &timeout); 1027 | } 1028 | pthread_mutex_unlock(&mutex); 1029 | 1030 | return NULL; 1031 | } 1032 | 1033 | /* 1034 | * TCP APPLICATION INTERFACE 1035 | */ 1036 | 1037 | int tcp_api_open(void) { 1038 | struct tcp_cb *cb; 1039 | int i; 1040 | 1041 | pthread_mutex_lock(&mutex); 1042 | for (i = 0; i < TCP_CB_TABLE_SIZE; i++) { 1043 | cb = &cb_table[i]; 1044 | if (IS_FREE_CB(cb)) { 1045 | cb->used = 1; 1046 | pthread_mutex_unlock(&mutex); 1047 | return i; 1048 | } 1049 | } 1050 | pthread_mutex_unlock(&mutex); 1051 | fprintf(stderr, "error: insufficient resources\n"); 1052 | return -1; 1053 | } 1054 | 1055 | int tcp_close(struct tcp_cb *cb) { 1056 | struct tcp_cb *backlog = NULL; 1057 | size_t size; 1058 | struct timeval now; 1059 | if (!cb->used) { 1060 | fprintf(stderr, "error: connection illegal for this process\n"); 1061 | return -1; 1062 | } 1063 | 1064 | cb->used = 0; 1065 | if (gettimeofday(&now, NULL) == -1) { 1066 | return -1; 1067 | } 1068 | 1069 | switch (cb->state) { 1070 | case TCP_CB_STATE_CLOSED: 1071 | break; 1072 | 1073 | case TCP_CB_STATE_LISTEN: 1074 | // close all cb in backlog 1075 | while (queue_pop(&cb->backlog, (void **)&backlog, &size) != -1) { 1076 | tcp_close(backlog); 1077 | } 1078 | case TCP_CB_STATE_SYN_SENT: 1079 | // close socket 1080 | tcp_close_cb(cb); 1081 | pthread_cond_broadcast(&cb->cond); 1082 | break; 1083 | 1084 | case TCP_CB_STATE_SYN_RCVD: 1085 | // if send buffer is empty 1086 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_FIN | TCP_FLG_ACK, &now, 1087 | NULL, 0); 1088 | cb->snd.nxt++; 1089 | cb->state = TCP_CB_STATE_FIN_WAIT1; 1090 | // TODO: else then wait change to ESTABLISHED state 1091 | break; 1092 | 1093 | case TCP_CB_STATE_ESTABLISHED: 1094 | // if send buffer is empty 1095 | // TODO: else then wait send all data in send buffer 1096 | // TODO: ? linux tcp need ack with fin ? 1097 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_FIN | TCP_FLG_ACK, &now, 1098 | NULL, 0); 1099 | cb->snd.nxt++; 1100 | cb->state = TCP_CB_STATE_FIN_WAIT1; 1101 | break; 1102 | 1103 | case TCP_CB_STATE_FIN_WAIT1: 1104 | case TCP_CB_STATE_FIN_WAIT2: 1105 | // "ok" response would be acceptable 1106 | case TCP_CB_STATE_CLOSING: 1107 | case TCP_CB_STATE_TIME_WAIT: 1108 | case TCP_CB_STATE_LAST_ACK: 1109 | fprintf(stderr, "error: connection closing\n"); 1110 | return -1; 1111 | 1112 | case TCP_CB_STATE_CLOSE_WAIT: 1113 | // wait send all data in send buffer 1114 | tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_FIN | TCP_FLG_ACK, &now, 1115 | NULL, 0); 1116 | cb->snd.nxt++; 1117 | cb->state = TCP_CB_STATE_CLOSING; 1118 | break; 1119 | } 1120 | 1121 | return 0; 1122 | } 1123 | 1124 | int tcp_api_close(int soc) { 1125 | int ret; 1126 | 1127 | // validate soc id 1128 | if (TCP_SOCKET_INVALID(soc)) { 1129 | return -1; 1130 | } 1131 | 1132 | pthread_mutex_lock(&mutex); 1133 | ret = tcp_close(&cb_table[soc]); 1134 | pthread_mutex_unlock(&mutex); 1135 | 1136 | return ret; 1137 | } 1138 | 1139 | int tcp_api_connect(int soc, ip_addr_t *addr, uint16_t port) { 1140 | struct tcp_cb *cb, *tmp; 1141 | struct timeval now; 1142 | int i, j; 1143 | int offset; 1144 | 1145 | // validate soc id 1146 | if (TCP_SOCKET_INVALID(soc)) { 1147 | return -1; 1148 | } 1149 | 1150 | pthread_mutex_lock(&mutex); 1151 | cb = &cb_table[soc]; 1152 | 1153 | // check cb state 1154 | if (!cb->used || cb->state != TCP_CB_STATE_CLOSED) { 1155 | pthread_mutex_unlock(&mutex); 1156 | return -1; 1157 | } 1158 | 1159 | if (gettimeofday(&now, NULL) == -1) { 1160 | perror("gettimeofday"); 1161 | return -1; 1162 | } 1163 | 1164 | // if port number is not specified then generate nice port 1165 | if (!cb->port) { 1166 | offset = time(NULL) % 1024; 1167 | // find port number which is not used between TCP_SOURCE_PORT_MIN and 1168 | // TCP_SOURCE_PORT_MAX 1169 | for (i = TCP_SOURCE_PORT_MIN + offset; i <= TCP_SOURCE_PORT_MAX; i++) { 1170 | for (j = 0; j < TCP_CB_TABLE_SIZE; j++) { 1171 | tmp = &cb_table[j]; 1172 | if (!IS_FREE_CB(tmp) && tmp->port == hton16((uint16_t)i)) { 1173 | break; 1174 | } 1175 | } 1176 | if (j == TCP_CB_TABLE_SIZE) { 1177 | // port number (i) is not used 1178 | cb->port = hton16((uint16_t)i); 1179 | break; 1180 | } 1181 | } 1182 | if (!cb->port) { 1183 | // could not find unused port number 1184 | pthread_mutex_unlock(&mutex); 1185 | return -1; 1186 | } 1187 | } 1188 | 1189 | // initalize cb 1190 | cb->peer.addr = *addr; 1191 | cb->peer.port = hton16(port); 1192 | cb->iface = ip_netif_by_peer(&cb->peer.addr); 1193 | if (!cb->iface) { 1194 | pthread_mutex_unlock(&mutex); 1195 | return -1; 1196 | } 1197 | cb->rcv.wnd = sizeof(cb->window); 1198 | cb->iss = (uint32_t)random(); 1199 | 1200 | // send SYN packet 1201 | if (tcp_tx(cb, cb->iss, 0, TCP_FLG_SYN, &now, NULL, 0) == -1) { 1202 | pthread_mutex_unlock(&mutex); 1203 | return -1; 1204 | } 1205 | cb->snd.una = cb->iss; 1206 | cb->snd.nxt = cb->iss + 1; 1207 | cb->timeout = now.tv_sec + USER_TIMEOUT; 1208 | cb->state = TCP_CB_STATE_SYN_SENT; 1209 | 1210 | // wait until state change 1211 | while (cb->state == TCP_CB_STATE_SYN_SENT) { 1212 | pthread_cond_wait(&cb_table[soc].cond, &mutex); 1213 | } 1214 | 1215 | pthread_mutex_unlock(&mutex); 1216 | return 0; 1217 | } 1218 | 1219 | int tcp_api_bind(int soc, uint16_t port) { 1220 | struct tcp_cb *cb; 1221 | int i; 1222 | 1223 | // validate soc id 1224 | if (TCP_SOCKET_INVALID(soc)) { 1225 | return -1; 1226 | } 1227 | 1228 | pthread_mutex_lock(&mutex); 1229 | 1230 | // check port is already used 1231 | for (i = 0; i < TCP_CB_TABLE_SIZE; i++) { 1232 | if (cb_table[i].port == hton16(port)) { 1233 | pthread_mutex_unlock(&mutex); 1234 | fprintf(stderr, "error: port is already used\n"); 1235 | return -1; 1236 | } 1237 | } 1238 | 1239 | // check cb is closed 1240 | cb = &cb_table[soc]; 1241 | if (!cb->used || cb->state != TCP_CB_STATE_CLOSED) { 1242 | pthread_mutex_unlock(&mutex); 1243 | return -1; 1244 | } 1245 | 1246 | // TODO: bind ip address 1247 | 1248 | // set port number 1249 | cb->port = hton16(port); 1250 | pthread_mutex_unlock(&mutex); 1251 | return 0; 1252 | } 1253 | 1254 | int tcp_api_listen(int soc) { 1255 | struct tcp_cb *cb; 1256 | 1257 | // validate soc id 1258 | if (TCP_SOCKET_INVALID(soc)) { 1259 | return -1; 1260 | } 1261 | 1262 | pthread_mutex_lock(&mutex); 1263 | cb = &cb_table[soc]; 1264 | if (!cb->used || cb->state != TCP_CB_STATE_CLOSED || !cb->port) { 1265 | pthread_mutex_unlock(&mutex); 1266 | return -1; 1267 | } 1268 | cb->state = TCP_CB_STATE_LISTEN; 1269 | pthread_mutex_unlock(&mutex); 1270 | return 0; 1271 | } 1272 | 1273 | int tcp_api_accept(int soc) { 1274 | struct tcp_cb *cb, *backlog = NULL; 1275 | size_t size; 1276 | 1277 | // validate soc id 1278 | if (TCP_SOCKET_INVALID(soc)) { 1279 | return -1; 1280 | } 1281 | 1282 | pthread_mutex_lock(&mutex); 1283 | cb = &cb_table[soc]; 1284 | if (!cb->used || cb->state != TCP_CB_STATE_LISTEN) { 1285 | pthread_mutex_unlock(&mutex); 1286 | return -1; 1287 | } 1288 | 1289 | while (cb->state == TCP_CB_STATE_LISTEN && 1290 | queue_pop(&cb->backlog, (void **)&backlog, &size) == -1) { 1291 | pthread_cond_wait(&cb->cond, &mutex); 1292 | } 1293 | 1294 | if (!backlog) { 1295 | pthread_mutex_unlock(&mutex); 1296 | return -1; 1297 | } 1298 | 1299 | backlog->used = 1; 1300 | pthread_mutex_unlock(&mutex); 1301 | 1302 | return array_offset(cb_table, backlog); 1303 | } 1304 | 1305 | ssize_t tcp_api_recv(int soc, uint8_t *buf, size_t size) { 1306 | struct tcp_cb *cb; 1307 | size_t total, len; 1308 | char *err; 1309 | 1310 | // validate soc id 1311 | if (TCP_SOCKET_INVALID(soc)) { 1312 | return -1; 1313 | } 1314 | 1315 | pthread_mutex_lock(&mutex); 1316 | cb = &cb_table[soc]; 1317 | if (!cb->used) { 1318 | pthread_mutex_unlock(&mutex); 1319 | return -1; 1320 | } 1321 | 1322 | TCP_RECEIVE_RETRY: 1323 | switch (cb->state) { 1324 | case TCP_CB_STATE_CLOSED: 1325 | err = "error: connection illegal for this process\n"; 1326 | goto ERROR_RECEIVE; 1327 | 1328 | case TCP_CB_STATE_LISTEN: 1329 | case TCP_CB_STATE_SYN_SENT: 1330 | case TCP_CB_STATE_SYN_RCVD: 1331 | // TODO: wait change to ESTABLISHED 1332 | err = "error: connection illegal for this process\n"; 1333 | goto ERROR_RECEIVE; 1334 | 1335 | case TCP_CB_STATE_CLOSE_WAIT: 1336 | case TCP_CB_STATE_ESTABLISHED: 1337 | case TCP_CB_STATE_FIN_WAIT1: 1338 | case TCP_CB_STATE_FIN_WAIT2: 1339 | total = sizeof(cb->window) - cb->rcv.wnd; 1340 | if (total == 0) { 1341 | if (cb->state == TCP_CB_STATE_CLOSE_WAIT) { 1342 | err = "error: connection closing\n"; 1343 | goto ERROR_RECEIVE; 1344 | } 1345 | 1346 | // wait and retry to read rcv buffer 1347 | pthread_cond_wait(&cb->cond, &mutex); 1348 | goto TCP_RECEIVE_RETRY; 1349 | } 1350 | len = total > size ? size : total; 1351 | memcpy(buf, cb->window, len); 1352 | memmove(cb->window, cb->window + len, total - len); 1353 | cb->rcv.wnd += len; 1354 | pthread_mutex_unlock(&mutex); 1355 | return len; 1356 | 1357 | case TCP_CB_STATE_CLOSING: 1358 | case TCP_CB_STATE_TIME_WAIT: 1359 | case TCP_CB_STATE_LAST_ACK: 1360 | err = "error: connection closing\n"; 1361 | goto ERROR_RECEIVE; 1362 | 1363 | default: 1364 | pthread_mutex_unlock(&mutex); 1365 | return -1; 1366 | } 1367 | 1368 | ERROR_RECEIVE: 1369 | pthread_mutex_unlock(&mutex); 1370 | fprintf(stderr, err); 1371 | return -1; 1372 | } 1373 | 1374 | ssize_t tcp_api_send(int soc, uint8_t *buf, size_t len) { 1375 | struct tcp_cb *cb; 1376 | struct timeval now; 1377 | size_t snt = 0, size; 1378 | uint16_t wnd; 1379 | char *err; 1380 | 1381 | // validate soc id 1382 | if (TCP_SOCKET_INVALID(soc)) { 1383 | return -1; 1384 | } 1385 | 1386 | pthread_mutex_lock(&mutex); 1387 | cb = &cb_table[soc]; 1388 | if (!cb->used) { 1389 | pthread_mutex_unlock(&mutex); 1390 | return -1; 1391 | } 1392 | 1393 | TCP_API_SEND_NEXT: 1394 | switch (cb->state) { 1395 | case TCP_CB_STATE_CLOSED: 1396 | err = "error: connection illegal for this process\n"; 1397 | goto ERROR_SEND; 1398 | 1399 | case TCP_CB_STATE_LISTEN: 1400 | // TODO: change to active mode if foreign socket is specified 1401 | case TCP_CB_STATE_SYN_SENT: 1402 | case TCP_CB_STATE_SYN_RCVD: 1403 | // TODO: wait change to ESTABLISHED 1404 | err = "error: connection illegal for this process\n"; 1405 | goto ERROR_SEND; 1406 | 1407 | case TCP_CB_STATE_CLOSE_WAIT: 1408 | case TCP_CB_STATE_ESTABLISHED: 1409 | // send buf 1410 | break; 1411 | 1412 | case TCP_CB_STATE_FIN_WAIT1: 1413 | case TCP_CB_STATE_FIN_WAIT2: 1414 | case TCP_CB_STATE_CLOSING: 1415 | case TCP_CB_STATE_TIME_WAIT: 1416 | case TCP_CB_STATE_LAST_ACK: 1417 | err = "error: connection closing\n"; 1418 | goto ERROR_SEND; 1419 | 1420 | default: 1421 | pthread_mutex_unlock(&mutex); 1422 | return -1; 1423 | } 1424 | 1425 | if (gettimeofday(&now, NULL) == -1) { 1426 | perror("gettimeofday"); 1427 | pthread_mutex_unlock(&mutex); 1428 | return (snt == 0) ? -1 : ((ssize_t)snt); 1429 | } 1430 | 1431 | if (len > 0) { 1432 | // mtu may changes, so calc size each time 1433 | size = cb->iface->dev->mtu - IP_HDR_SIZE_MAX - sizeof(struct tcp_hdr); 1434 | 1435 | // check data size 1436 | if (len < size) { 1437 | size = len; 1438 | } 1439 | 1440 | // check send buffer size 1441 | wnd = TCP_SND_BUF_SIZE - (cb->snd.nxt - cb->snd.una); 1442 | if (wnd < size) { 1443 | size = wnd; 1444 | if (size <= 0) { 1445 | // wait until some data ack 1446 | fprintf(stderr, 1447 | ">>> send : wait for ack snd_buf_size: %d, snd.nxt: %u, " 1448 | "snd.una: %u <<<\n", 1449 | TCP_SND_BUF_SIZE, cb->snd.nxt, cb->snd.una); 1450 | pthread_cond_wait(&cb->cond, &mutex); 1451 | // retry 1452 | goto TCP_API_SEND_NEXT; 1453 | } 1454 | } 1455 | 1456 | // send segment. if send window is not enough then stored into txq in tcp_tx 1457 | if (tcp_tx(cb, cb->snd.nxt, cb->rcv.nxt, TCP_FLG_PSH | TCP_FLG_ACK, &now, 1458 | buf + snt, size) == -1) { 1459 | // TODO: memory allocation error or ip_tx error 1460 | return snt; 1461 | } 1462 | cb->timeout = now.tv_sec + USER_TIMEOUT; 1463 | cb->snd.nxt += size; 1464 | snt += size; 1465 | len -= size; 1466 | 1467 | if (len > 0) { 1468 | // send next segment 1469 | goto TCP_API_SEND_NEXT; 1470 | } 1471 | } 1472 | 1473 | pthread_mutex_unlock(&mutex); 1474 | // TODO: support urg pointer 1475 | return snt; 1476 | 1477 | ERROR_SEND: 1478 | pthread_mutex_unlock(&mutex); 1479 | fprintf(stderr, err); 1480 | return -1; 1481 | } 1482 | 1483 | int tcp_init(void) { 1484 | int i; 1485 | 1486 | // initialize mutex and condition variables 1487 | for (i = 0; i < TCP_CB_TABLE_SIZE; i++) { 1488 | pthread_cond_init(&cb_table[i].cond, NULL); 1489 | } 1490 | pthread_mutex_init(&mutex, NULL); 1491 | pthread_cond_init(&timer_cond, NULL); 1492 | 1493 | if (ip_add_protocol(IP_PROTOCOL_TCP, tcp_rx) == -1) { 1494 | return -1; 1495 | } 1496 | if (pthread_create(&timer_thread, NULL, tcp_timer_thread, NULL) == -1) { 1497 | return -1; 1498 | } 1499 | return 0; 1500 | } 1501 | -------------------------------------------------------------------------------- /tcp.h: -------------------------------------------------------------------------------- 1 | #ifndef _TCP_H_ 2 | #define _TCP_H_ 3 | 4 | #include 5 | #include 6 | #include "ip.h" 7 | 8 | int tcp_init(void); 9 | int tcp_api_open(void); 10 | int tcp_api_close(int soc); 11 | int tcp_api_connect(int soc, ip_addr_t *addr, uint16_t port); 12 | int tcp_api_bind(int soc, uint16_t port); 13 | int tcp_api_listen(int soc); 14 | int tcp_api_accept(int soc); 15 | ssize_t tcp_api_recv(int soc, uint8_t *buf, size_t size); 16 | ssize_t tcp_api_send(int soc, uint8_t *buf, size_t len); 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /test/ethernet_test.c: -------------------------------------------------------------------------------- 1 | #include "ethernet.h" 2 | #include 3 | #include 4 | #include 5 | #include "net.h" 6 | #include "raw.h" 7 | 8 | static int setup(void) { 9 | if (ethernet_init() == -1) { 10 | fprintf(stderr, "ethernet_init(): failure\n"); 11 | return -1; 12 | } 13 | return 0; 14 | } 15 | 16 | int main(int argc, char const *argv[]) { 17 | sigset_t sigset; 18 | int signo; 19 | struct netdev *dev; 20 | char *name = "tap1"; 21 | 22 | sigemptyset(&sigset); 23 | sigaddset(&sigset, SIGINT); 24 | sigprocmask(SIG_BLOCK, &sigset, NULL); 25 | if (setup() == -1) { 26 | return -1; 27 | } 28 | 29 | // setup 30 | dev = netdev_alloc(NETDEV_TYPE_ETHERNET); 31 | if (!dev) { 32 | fprintf(stderr, "netdev_alloc() : failed\n"); 33 | return -1; 34 | } 35 | strncpy(dev->name, name, sizeof(dev->name) - 1); 36 | if (dev->ops->open(dev, RAWDEV_TYPE_AUTO) == -1) { 37 | fprintf(stderr, "failed to open raw device\n"); 38 | return -1; 39 | } 40 | dev->ops->run(dev); 41 | 42 | while (1) { 43 | sigwait(&sigset, &signo); 44 | if (signo == SIGINT) { 45 | break; 46 | } 47 | } 48 | if (dev->ops->close) { 49 | dev->ops->close(dev); 50 | } 51 | fprintf(stderr, "closed\n"); 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /test/ip_test.c: -------------------------------------------------------------------------------- 1 | #include "ip.h" 2 | #include 3 | #include 4 | #include 5 | #include "arp.h" 6 | #include "ethernet.h" 7 | #include "net.h" 8 | #include "raw.h" 9 | 10 | static int setup(void) { 11 | if (ethernet_init() == -1) { 12 | fprintf(stderr, "ethernet_init(): failure\n"); 13 | return -1; 14 | } else if (ip_init() == -1) { 15 | fprintf(stderr, "ip_init(): failure\n"); 16 | return -1; 17 | } else if (arp_init() == -1) { 18 | fprintf(stderr, "arp_init(): failure\n"); 19 | return -1; 20 | } 21 | return 0; 22 | } 23 | 24 | int main(int argc, char const *argv[]) { 25 | sigset_t sigset; 26 | int signo; 27 | struct netdev *dev; 28 | char *name = "tap1", *ipaddr = "192.168.33.11"; 29 | struct netif_ip iface = {}; 30 | 31 | sigemptyset(&sigset); 32 | sigaddset(&sigset, SIGINT); 33 | sigprocmask(SIG_BLOCK, &sigset, NULL); 34 | if (setup() == -1) { 35 | return -1; 36 | } 37 | 38 | // setup 39 | dev = netdev_alloc(NETDEV_TYPE_ETHERNET); 40 | if (!dev) { 41 | fprintf(stderr, "netdev_alloc() : failed\n"); 42 | return -1; 43 | } 44 | strncpy(dev->name, name, sizeof(dev->name) - 1); 45 | if (dev->ops->open(dev, RAWDEV_TYPE_AUTO) == -1) { 46 | fprintf(stderr, "failed to open raw device\n"); 47 | return -1; 48 | } 49 | 50 | // set netif 51 | iface.netif.family = NETIF_FAMILY_IPV4; 52 | ip_addr_pton(ipaddr, &iface.unicast); 53 | netdev_add_netif(dev, (struct netif *)&iface); 54 | 55 | dev->ops->run(dev); 56 | 57 | char *data = "hello world"; 58 | ip_addr_t dst; 59 | if (ip_addr_pton("192.168.33.10", &dst) != 0) { 60 | fprintf(stderr, "ip_addr_pton: failed\n"); 61 | return -1; 62 | } 63 | if (ip_tx((struct netif *)&iface, IP_PROTOCOL_RAW, (uint8_t *)data, 12, 64 | &dst) == -1) { 65 | fprintf(stderr, "ip_tx: failed\n"); 66 | return -1; 67 | } 68 | 69 | while (1) { 70 | sigwait(&sigset, &signo); 71 | if (signo == SIGINT) { 72 | break; 73 | } 74 | } 75 | if (dev->ops->close) { 76 | dev->ops->close(dev); 77 | } 78 | fprintf(stderr, "closed\n"); 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /test/mask_test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "util.h" 4 | 5 | #define MASK_SIZE 4 6 | 7 | int main(int argc, char const *argv[]) { 8 | uint32_t mask[MASK_SIZE]; 9 | 10 | fprintf(stderr, ">>> clr all <<<\n"); 11 | maskclr(mask, MASK_SIZE); 12 | maskdbg(mask, MASK_SIZE); 13 | 14 | if (maskchk(mask, MASK_SIZE, 0, 1)) { 15 | fprintf(stderr, "check failed\n"); 16 | } 17 | fprintf(stderr, ">>> set 31-65 <<<\n"); 18 | maskset(mask, MASK_SIZE, 31, 34); 19 | maskdbg(mask, MASK_SIZE); 20 | 21 | if (maskchk(mask, MASK_SIZE, 30, 3)) { 22 | fprintf(stderr, "check failed : 1\n"); 23 | } 24 | if (maskchk(mask, MASK_SIZE, 31, 35)) { 25 | fprintf(stderr, "check failed : 2\n"); 26 | } 27 | if (!maskchk(mask, MASK_SIZE, 31, 34)) { 28 | fprintf(stderr, "check failed : 3\n"); 29 | } 30 | if (!maskchk(mask, MASK_SIZE, 32, 2)) { 31 | fprintf(stderr, "check failed : 4\n"); 32 | } 33 | 34 | fprintf(stderr, ">>> set all <<<\n"); 35 | maskset(mask, MASK_SIZE, 0, MASK_SIZE * 32); 36 | maskdbg(mask, MASK_SIZE); 37 | 38 | if (!maskchk(mask, MASK_SIZE, 0, 1)) { 39 | fprintf(stderr, "check failed : first\n"); 40 | } 41 | if (!maskchk(mask, MASK_SIZE, 0, MASK_SIZE * 32)) { 42 | fprintf(stderr, "check failed : all\n"); 43 | } 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /test/queue_test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "util.h" 5 | 6 | int main(int argc, char const *argv[]) { 7 | int failed = 0; 8 | void *data; 9 | size_t size; 10 | uint64_t data1 = (uint64_t)random(); 11 | uint16_t data2 = (uint16_t)random(); 12 | uint8_t data3 = (uint8_t)random(); 13 | 14 | struct queue_head queue = {}; 15 | 16 | if (queue_pop(&queue, &data, &size) != -1) { 17 | fprintf(stderr, "check failed : pop from empty queue\n"); 18 | failed++; 19 | } 20 | 21 | if (queue_push(&queue, &data1, sizeof(uint64_t)) != 0) { 22 | fprintf(stderr, "check failed : push 1\n"); 23 | failed++; 24 | } 25 | 26 | if (queue_push(&queue, &data2, sizeof(uint16_t)) != 0) { 27 | fprintf(stderr, "check failed : push 2\n"); 28 | failed++; 29 | } 30 | 31 | if (queue_pop(&queue, &data, &size) != 0) { 32 | fprintf(stderr, "check failed : pop data1\n"); 33 | failed++; 34 | } else if (size != sizeof(uint64_t)) { 35 | fprintf(stderr, "check failed : poped data1 size\n"); 36 | failed++; 37 | } else if (*(uint64_t *)data != data1) { 38 | fprintf(stderr, "check failed : poped data1\n"); 39 | failed++; 40 | } 41 | 42 | if (queue_push(&queue, &data3, sizeof(uint8_t)) != 0) { 43 | fprintf(stderr, "check failed : push 3\n"); 44 | failed++; 45 | } 46 | 47 | if (queue_pop(&queue, &data, &size) != 0) { 48 | fprintf(stderr, "check failed : pop data2\n"); 49 | failed++; 50 | } else if (size != sizeof(uint16_t)) { 51 | fprintf(stderr, "check failed : poped data2 size\n"); 52 | failed++; 53 | } else if (*(uint16_t *)data != data2) { 54 | fprintf(stderr, "check failed : poped data2\n"); 55 | failed++; 56 | } 57 | 58 | if (queue_pop(&queue, &data, &size) != 0) { 59 | fprintf(stderr, "check failed : pop data3\n"); 60 | failed++; 61 | } else if (size != sizeof(uint8_t)) { 62 | fprintf(stderr, "check failed : poped data3 size\n"); 63 | failed++; 64 | } else if (*(uint8_t *)data != data3) { 65 | fprintf(stderr, "check failed : poped data3\n"); 66 | failed++; 67 | } 68 | 69 | if (queue_pop(&queue, &data, &size) != -1) { 70 | fprintf(stderr, "check failed : pop from all poped empty queue\n"); 71 | failed++; 72 | } 73 | 74 | if (!failed) { 75 | fprintf(stderr, "TEST SUCCESS!\n"); 76 | return 0; 77 | } else { 78 | fprintf(stderr, "TEST FAILED : %d errors\n", failed); 79 | return 1; 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /test/raw_soc_test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "raw/soc.h" 4 | 5 | volatile sig_atomic_t terminate; 6 | 7 | static void on_signal(int s) { terminate = 1; } 8 | 9 | static void rx_handler(uint8_t *frame, size_t len, void *arg) { 10 | fprintf(stderr, "receive %zu octets\n", len); 11 | } 12 | 13 | int main(int argc, char const *argv[]) { 14 | char *name = "eth1"; 15 | struct soc_dev *dev; 16 | uint8_t addr[6]; 17 | 18 | signal(SIGINT, on_signal); 19 | 20 | dev = soc_dev_open(name); 21 | if (dev == NULL) { 22 | return -1; 23 | } 24 | soc_dev_addr(name, addr, sizeof(addr)); 25 | fprintf(stderr, "[%s] %02x:%02x:%02x:%02x:%02x:%02x\n", name, addr[0], 26 | addr[1], addr[2], addr[3], addr[4], addr[5]); 27 | 28 | while (!terminate) { 29 | soc_dev_rx(dev, rx_handler, dev, 1000); 30 | } 31 | 32 | soc_dev_close(dev); 33 | 34 | printf("closed"); 35 | 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /test/raw_tap_test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "raw/tap.h" 4 | 5 | volatile sig_atomic_t terminate; 6 | 7 | static void on_signal(int s) { terminate = 1; } 8 | 9 | static void rx_handler(uint8_t *frame, size_t len, void *arg) { 10 | fprintf(stderr, "receive %zu octets\n", len); 11 | } 12 | 13 | int main(int argc, char const *argv[]) { 14 | char *name = "tap1"; 15 | struct tap_dev *dev; 16 | uint8_t addr[6]; 17 | 18 | signal(SIGINT, on_signal); 19 | 20 | dev = tap_dev_open(name); 21 | if (dev == NULL) { 22 | return -1; 23 | } 24 | tap_dev_addr(name, addr, sizeof(addr)); 25 | fprintf(stderr, "[%s] %02x:%02x:%02x:%02x:%02x:%02x\n", name, addr[0], 26 | addr[1], addr[2], addr[3], addr[4], addr[5]); 27 | 28 | while (!terminate) { 29 | tap_dev_rx(dev, rx_handler, dev, 1000); 30 | } 31 | 32 | tap_dev_close(dev); 33 | 34 | printf("closed"); 35 | 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /test/raw_test.c: -------------------------------------------------------------------------------- 1 | #include "raw.h" 2 | #include 3 | #include 4 | #include "util.h" 5 | 6 | volatile sig_atomic_t terminate; 7 | 8 | static void on_signal(int s) { terminate = 1; } 9 | 10 | static void dump(uint8_t *frame, size_t len, void *arg) { 11 | fprintf(stderr, "%s: receive %zu octets\n", (char *)arg, len); 12 | hexdump(stderr, frame, len); 13 | } 14 | 15 | int main(int argc, char const *argv[]) { 16 | char *name = "tap1"; 17 | struct rawdev *dev; 18 | uint8_t addr[6]; 19 | 20 | signal(SIGINT, on_signal); 21 | 22 | dev = rawdev_alloc(RAWDEV_TYPE_AUTO, name); 23 | if (dev == NULL) { 24 | fprintf(stderr, "rawdev_alloc(): error\n"); 25 | return -1; 26 | } 27 | if (dev->ops->open(dev) == -1) { 28 | fprintf(stderr, "dev->ops->open(): failure - (%s)\n", dev->name); 29 | return -1; 30 | } 31 | dev->ops->addr(dev, addr, sizeof(addr)); 32 | fprintf(stderr, "[%s] %02x:%02x:%02x:%02x:%02x:%02x\n", name, addr[0], 33 | addr[1], addr[2], addr[3], addr[4], addr[5]); 34 | while (!terminate) { 35 | dev->ops->rx(dev, dump, dev, 1000); 36 | } 37 | 38 | dev->ops->close(dev); 39 | 40 | printf("closed"); 41 | 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /test/tcp_listen_test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "arp.h" 5 | #include "ethernet.h" 6 | #include "ip.h" 7 | #include "net.h" 8 | #include "raw.h" 9 | #include "tcp.h" 10 | #include "util.h" 11 | 12 | static int setup(void) { 13 | if (ethernet_init() == -1) { 14 | fprintf(stderr, "ethernet_init(): failure\n"); 15 | return -1; 16 | } else if (ip_init() == -1) { 17 | fprintf(stderr, "ip_init(): failure\n"); 18 | return -1; 19 | } else if (arp_init() == -1) { 20 | fprintf(stderr, "arp_init(): failure\n"); 21 | return -1; 22 | } else if (tcp_init() == -1) { 23 | fprintf(stderr, "tcp_init(): failure\n"); 24 | return -1; 25 | } 26 | return 0; 27 | } 28 | 29 | #define BUF_SIZE (100 * 1024) 30 | 31 | int main(int argc, char const *argv[]) { 32 | sigset_t sigset; 33 | int signo; 34 | struct netdev *dev; 35 | struct netif *netif; 36 | char *name = "tap2", *ipaddr = "192.168.33.13", *netmask = "255.255.0.0"; 37 | int soc, listener; 38 | uint8_t buf[BUF_SIZE]; 39 | size_t n; 40 | 41 | memset(buf, 'a', BUF_SIZE); 42 | 43 | // sigemptyset(&sigset); 44 | // sigaddset(&sigset, SIGINT); 45 | // sigprocmask(SIG_BLOCK, &sigset, NULL); 46 | if (setup() == -1) { 47 | return -1; 48 | } 49 | 50 | // setup 51 | dev = netdev_alloc(NETDEV_TYPE_ETHERNET); 52 | if (!dev) { 53 | fprintf(stderr, "netdev_alloc() : failed\n"); 54 | return -1; 55 | } 56 | strncpy(dev->name, name, sizeof(dev->name) - 1); 57 | if (dev->ops->open(dev, RAWDEV_TYPE_AUTO) == -1) { 58 | fprintf(stderr, "failed to open raw device\n"); 59 | return -1; 60 | } 61 | 62 | // set netif 63 | netif = ip_netif_register(dev, ipaddr, netmask, NULL); 64 | 65 | dev->ops->run(dev); 66 | 67 | listener = tcp_api_open(); 68 | if (listener == -1) { 69 | fprintf(stderr, "tcp_api_open: failed\n"); 70 | return -1; 71 | } 72 | 73 | if (tcp_api_bind(listener, 20000) == -1) { 74 | fprintf(stderr, "tcp_api_bind: failed\n"); 75 | return -1; 76 | } 77 | if (tcp_api_listen(listener) == -1) { 78 | fprintf(stderr, "tcp_api_listen: failed\n"); 79 | return -1; 80 | } 81 | 82 | fprintf(stderr, "tcp_api_listen success\n"); 83 | 84 | while (1) { 85 | soc = tcp_api_accept(listener); 86 | if (!soc) { 87 | fprintf(stderr, "tcp_api_accept: failed\n"); 88 | return -1; 89 | } 90 | fprintf(stderr, "tcp_api_accept success: soc : %d\n", soc); 91 | 92 | n = tcp_api_send(soc, buf, BUF_SIZE); 93 | if (n == -1) { 94 | fprintf(stderr, "tcp_api_send: failed\n"); 95 | } else if (n == BUF_SIZE) { 96 | fprintf(stderr, ">>> send data success <<<\n"); 97 | } else { 98 | fprintf(stderr, "tcp_api_send: partial success : %d\n", n); 99 | } 100 | 101 | n = tcp_api_recv(soc, buf, BUF_SIZE); 102 | if (n == -1) { 103 | fprintf(stderr, "tcp_api_recv: failed\n"); 104 | } else if (n == 0) { 105 | fprintf(stderr, "tcp_api_recv: size is 0\n"); 106 | } else { 107 | fprintf(stderr, ">>> recv data <<<\n"); 108 | hexdump(stderr, buf, n); 109 | } 110 | 111 | if (tcp_api_close(soc) == -1) { 112 | fprintf(stderr, "tcp_api_close: failed\n"); 113 | } else { 114 | fprintf(stderr, "tcp_api_close: success\n"); 115 | } 116 | // sigwait(&sigset, &signo); 117 | // if (signo == SIGINT) { 118 | // break; 119 | // } 120 | } 121 | 122 | if (tcp_api_close(listener) == -1) { 123 | fprintf(stderr, "tcp_api_close: failed\n"); 124 | return -1; 125 | } 126 | 127 | if (dev->ops->close) { 128 | dev->ops->close(dev); 129 | } 130 | fprintf(stderr, "closed\n"); 131 | return 0; 132 | } 133 | -------------------------------------------------------------------------------- /test/tcp_test.c: -------------------------------------------------------------------------------- 1 | #include "tcp.h" 2 | #include 3 | #include 4 | #include 5 | #include "arp.h" 6 | #include "ethernet.h" 7 | #include "ip.h" 8 | #include "net.h" 9 | #include "raw.h" 10 | 11 | static int setup(void) { 12 | if (ethernet_init() == -1) { 13 | fprintf(stderr, "ethernet_init(): failure\n"); 14 | return -1; 15 | } else if (ip_init() == -1) { 16 | fprintf(stderr, "ip_init(): failure\n"); 17 | return -1; 18 | } else if (arp_init() == -1) { 19 | fprintf(stderr, "arp_init(): failure\n"); 20 | return -1; 21 | } else if (tcp_init() == -1) { 22 | fprintf(stderr, "tcp_init(): failure\n"); 23 | return -1; 24 | } 25 | return 0; 26 | } 27 | 28 | int main(int argc, char const *argv[]) { 29 | sigset_t sigset; 30 | int signo; 31 | struct netdev *dev; 32 | struct netif *netif; 33 | char *name = "tap1", *ipaddr = "192.168.33.11", *netmask = "255.255.0.0"; 34 | int soc; 35 | 36 | sigemptyset(&sigset); 37 | sigaddset(&sigset, SIGINT); 38 | sigprocmask(SIG_BLOCK, &sigset, NULL); 39 | if (setup() == -1) { 40 | return -1; 41 | } 42 | 43 | // setup 44 | dev = netdev_alloc(NETDEV_TYPE_ETHERNET); 45 | if (!dev) { 46 | fprintf(stderr, "netdev_alloc() : failed\n"); 47 | return -1; 48 | } 49 | strncpy(dev->name, name, sizeof(dev->name) - 1); 50 | if (dev->ops->open(dev, RAWDEV_TYPE_AUTO) == -1) { 51 | fprintf(stderr, "failed to open raw device\n"); 52 | return -1; 53 | } 54 | 55 | // set netif 56 | netif = ip_netif_register(dev, ipaddr, netmask, NULL); 57 | 58 | dev->ops->run(dev); 59 | 60 | ip_addr_t dst; 61 | if (ip_addr_pton("192.168.33.10", &dst) != 0) { 62 | fprintf(stderr, "ip_addr_pton: failed\n"); 63 | return -1; 64 | } 65 | 66 | soc = tcp_api_open(); 67 | if (soc == -1) { 68 | fprintf(stderr, "tcp_api_open: failed\n"); 69 | return -1; 70 | } 71 | 72 | fprintf(stderr, "sleep\n"); 73 | sleep(10); 74 | fprintf(stderr, "start\n"); 75 | 76 | if (tcp_api_connect(soc, &dst, 19999) == -1) { 77 | fprintf(stderr, "tcp_api_connect: failed\n"); 78 | return -1; 79 | } 80 | 81 | fprintf(stderr, "tcp_api_connect success\n"); 82 | 83 | while (1) { 84 | sigwait(&sigset, &signo); 85 | if (signo == SIGINT) { 86 | break; 87 | } 88 | } 89 | 90 | if (tcp_api_close(soc) == -1) { 91 | fprintf(stderr, "tcp_api_close: failed\n"); 92 | return -1; 93 | } 94 | 95 | if (dev->ops->close) { 96 | dev->ops->close(dev); 97 | } 98 | fprintf(stderr, "closed\n"); 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /util.c: -------------------------------------------------------------------------------- 1 | #include "util.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | void hexdump(FILE *fp, void *data, size_t size) { 10 | int offset, index; 11 | unsigned char *src; 12 | 13 | src = (unsigned char *)data; 14 | fprintf(fp, 15 | "+------+-------------------------------------------------+----------" 16 | "--------+\n"); 17 | for (offset = 0; offset < (int)size; offset += 16) { 18 | fprintf(fp, "| %04x | ", offset); 19 | for (index = 0; index < 16; index++) { 20 | if (offset + index < (int)size) { 21 | fprintf(fp, "%02x ", 0xff & src[offset + index]); 22 | } else { 23 | fprintf(fp, " "); 24 | } 25 | } 26 | fprintf(fp, "| "); 27 | for (index = 0; index < 16; index++) { 28 | if (offset + index < (int)size) { 29 | if (isascii(src[offset + index]) && isprint(src[offset + index])) { 30 | fprintf(fp, "%c", src[offset + index]); 31 | } else { 32 | fprintf(fp, "."); 33 | } 34 | } else { 35 | fprintf(fp, " "); 36 | } 37 | } 38 | fprintf(fp, " |\n"); 39 | } 40 | fprintf(fp, 41 | "+------+-------------------------------------------------+----------" 42 | "--------+\n"); 43 | } 44 | 45 | /* 46 | * QUEUE OPERATIONS 47 | */ 48 | 49 | int queue_push(struct queue_head *queue, void *data, size_t size) { 50 | struct queue_entry *entry; 51 | if (!queue || !data) { 52 | return -1; 53 | } 54 | entry = malloc(sizeof(struct queue_entry)); 55 | if (!entry) { 56 | return -1; 57 | } 58 | entry->data = data; 59 | entry->size = size; 60 | entry->next = NULL; 61 | if (queue->tail) { 62 | queue->tail->next = entry; 63 | } 64 | queue->tail = entry; 65 | if (!queue->next) { 66 | queue->next = entry; 67 | } 68 | queue->num++; 69 | return 0; 70 | } 71 | 72 | int queue_pop(struct queue_head *queue, void **data, size_t *size) { 73 | struct queue_entry *entry; 74 | if (!queue || !queue->next) { 75 | return -1; 76 | } 77 | entry = queue->next; 78 | queue->next = entry->next; 79 | if (!queue->next) { 80 | queue->tail = NULL; 81 | } 82 | queue->num--; 83 | *data = entry->data; 84 | *size = entry->size; 85 | free(entry); 86 | return 0; 87 | } 88 | 89 | uint16_t cksum16(uint16_t *data, uint16_t size, uint32_t init) { 90 | uint32_t sum; 91 | 92 | sum = init; 93 | while (size > 1) { 94 | sum += *(data++); 95 | size -= 2; 96 | } 97 | if (size) { 98 | sum += *(uint8_t *)data; 99 | } 100 | sum = (sum & 0xffff) + (sum >> 16); 101 | sum = (sum & 0xffff) + (sum >> 16); 102 | return ~(uint16_t)sum; 103 | } 104 | 105 | // is not portable 106 | #ifndef __BIG_ENDIAN 107 | #define __BIG_ENDIAN 4321 108 | #endif 109 | #ifndef __LITTLE_ENDIAN 110 | #define __LITTLE_ENDIAN 1234 111 | #endif 112 | 113 | static int endian = 0; 114 | 115 | static int byteorder(void) { 116 | uint32_t x = 0x00000001; 117 | 118 | return *(uint8_t *)&x ? __LITTLE_ENDIAN : __BIG_ENDIAN; 119 | } 120 | 121 | static uint16_t byteswap16(uint16_t v) { 122 | return (v & 0x00ff) << 8 | (v & 0xff00) >> 8; 123 | } 124 | 125 | static uint32_t byteswap32(uint32_t v) { 126 | return (v & 0x000000ff) << 24 | (v & 0x0000ff00) << 8 | 127 | (v & 0x00ff0000) >> 8 | (v & 0xff000000) >> 24; 128 | } 129 | 130 | uint16_t hton16(uint16_t h) { 131 | if (!endian) { 132 | endian = byteorder(); 133 | } 134 | return endian == __LITTLE_ENDIAN ? byteswap16(h) : h; 135 | } 136 | 137 | uint16_t ntoh16(uint16_t n) { 138 | if (!endian) { 139 | endian = byteorder(); 140 | } 141 | return endian == __LITTLE_ENDIAN ? byteswap16(n) : n; 142 | } 143 | 144 | uint32_t hton32(uint32_t h) { 145 | if (!endian) { 146 | endian = byteorder(); 147 | } 148 | return endian == __LITTLE_ENDIAN ? byteswap32(h) : h; 149 | } 150 | 151 | uint32_t ntoh32(uint32_t n) { 152 | if (!endian) { 153 | endian = byteorder(); 154 | } 155 | return endian == __LITTLE_ENDIAN ? byteswap32(n) : n; 156 | } 157 | 158 | void maskset(uint32_t *mask, size_t size, size_t offset, size_t len) { 159 | size_t idx, so, sb, bl; 160 | 161 | so = offset / 32; 162 | sb = offset % 32; 163 | bl = (len > 32 - sb) ? 32 - sb : len; 164 | mask[so] |= (0xffffffff >> (32 - bl)) << sb; 165 | len -= bl; 166 | for (idx = so; idx < so + (len / 32); idx++) { 167 | mask[idx + 1] = 0xffffffff; 168 | } 169 | len -= (32 * (idx - so)); 170 | if (len) { 171 | mask[idx + 1] |= (0xffffffff >> (32 - len)); 172 | } 173 | } 174 | 175 | int maskchk(uint32_t *mask, size_t size, size_t offset, size_t len) { 176 | size_t idx, so, sb, bl; 177 | 178 | so = offset / 32; 179 | sb = offset % 32; 180 | bl = (len > 32 - sb) ? 32 - sb : len; 181 | if ((mask[offset / 32] & ((0xffffffff >> (32 - bl)) << sb)) ^ 182 | ((0xffffffff >> (32 - bl)) << sb)) { 183 | return 0; 184 | } 185 | len -= bl; 186 | for (idx = so; idx < so + (len / 32); idx++) { 187 | if (mask[idx + 1] ^ 0xffffffff) { 188 | return 0; 189 | } 190 | } 191 | len -= (32 * (idx - so)); 192 | if (len) { 193 | if ((mask[idx + 1] & (0xffffffff >> (32 - len))) ^ 194 | (0xffffffff >> (32 - len))) { 195 | return 0; 196 | } 197 | } 198 | return 1; 199 | } 200 | 201 | void maskclr(uint32_t *mask, size_t size) { 202 | memset(mask, 0, sizeof(*mask) * size); 203 | } 204 | 205 | #define ISBIT(x) ((x) ? 1 : 0) 206 | 207 | void maskdbg(uint32_t *mask, size_t size) { 208 | uint8_t *ptr; 209 | 210 | for (ptr = (uint8_t *)mask; ptr < (uint8_t *)(mask + size); ptr++) { 211 | fprintf(stderr, "%d%d%d%d %d%d%d%d\n", ISBIT(*ptr & 0x01), 212 | ISBIT(*ptr & 0x02), ISBIT(*ptr & 0x04), ISBIT(*ptr & 0x08), 213 | ISBIT(*ptr & 0x10), ISBIT(*ptr & 0x20), ISBIT(*ptr & 0x40), 214 | ISBIT(*ptr & 0x80)); 215 | } 216 | } 217 | -------------------------------------------------------------------------------- /util.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTIL_H_ 2 | #define _UTIL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #ifndef MAX 8 | #define MAX(x, y) (((x) > (y)) ? (x) : (y)) 9 | #endif 10 | #ifndef MIN 11 | #define MIN(x, y) (((x) < (y)) ? (x) : (y)) 12 | #endif 13 | 14 | #define array_offset(array, x) \ 15 | (((uintptr_t)(x) - (uintptr_t)(array)) / sizeof(*x)) 16 | 17 | struct queue_entry { 18 | void *data; 19 | size_t size; 20 | struct queue_entry *next; 21 | }; 22 | 23 | struct queue_head { 24 | struct queue_entry *next; 25 | struct queue_entry *tail; 26 | unsigned int num; 27 | }; 28 | 29 | void hexdump(FILE *fp, void *data, size_t size); 30 | 31 | int queue_push(struct queue_head *queue, void *data, size_t size); 32 | int queue_pop(struct queue_head *queue, void **data, size_t *size); 33 | 34 | uint16_t cksum16(uint16_t *data, uint16_t size, uint32_t init); 35 | uint16_t hton16(uint16_t); 36 | uint16_t ntoh16(uint16_t); 37 | uint32_t hton32(uint32_t); 38 | uint32_t ntoh32(uint32_t); 39 | 40 | void maskset(uint32_t *mask, size_t size, size_t offset, size_t len); 41 | int maskchk(uint32_t *mask, size_t size, size_t offset, size_t len); 42 | void maskclr(uint32_t *mask, size_t size); 43 | void maskdbg(uint32_t *mask, size_t size); 44 | 45 | #endif 46 | --------------------------------------------------------------------------------