1 /* 2 * Copyright 2005, Ingo Weinhold <bonefish@cs.tu-berlin.de>. 3 * All rights reserved. Distributed under the terms of the MIT License. 4 */ 5 6 #include <boot/net/UDP.h> 7 8 #include <stdio.h> 9 10 #include <OS.h> 11 12 #include <boot/net/ChainBuffer.h> 13 #include <boot/net/NetStack.h> 14 15 16 //#define TRACE_UDP 17 #ifdef TRACE_UDP 18 # define TRACE(x) dprintf x 19 #else 20 # define TRACE(x) ; 21 #endif 22 23 24 // #pragma mark - UDPPacket 25 26 // constructor 27 UDPPacket::UDPPacket() 28 : fNext(NULL), 29 fData(NULL), 30 fSize(0) 31 { 32 } 33 34 // destructor 35 UDPPacket::~UDPPacket() 36 { 37 free(fData); 38 } 39 40 // SetTo 41 status_t 42 UDPPacket::SetTo(const void *data, size_t size, ip_addr_t sourceAddress, 43 uint16 sourcePort, ip_addr_t destinationAddress, uint16 destinationPort) 44 { 45 if (!data) 46 return B_BAD_VALUE; 47 48 // clone the data 49 fData = malloc(size); 50 if (!fData) 51 return B_NO_MEMORY; 52 memcpy(fData, data, size); 53 54 fSize = size; 55 fSourceAddress = sourceAddress; 56 fDestinationAddress = destinationAddress; 57 fSourcePort = sourcePort; 58 fDestinationPort = destinationPort; 59 60 return B_OK; 61 } 62 63 // Next 64 UDPPacket * 65 UDPPacket::Next() const 66 { 67 return fNext; 68 } 69 70 // SetNext 71 void 72 UDPPacket::SetNext(UDPPacket *next) 73 { 74 fNext = next; 75 } 76 77 // Data 78 const void * 79 UDPPacket::Data() const 80 { 81 return fData; 82 } 83 84 // DataSize 85 size_t 86 UDPPacket::DataSize() const 87 { 88 return fSize; 89 } 90 91 // SourceAddress 92 ip_addr_t 93 UDPPacket::SourceAddress() const 94 { 95 return fSourceAddress; 96 } 97 98 // SourcePort 99 uint16 100 UDPPacket::SourcePort() const 101 { 102 return fSourcePort; 103 } 104 105 // DestinationAddress 106 ip_addr_t 107 UDPPacket::DestinationAddress() const 108 { 109 return fDestinationAddress; 110 } 111 112 // DestinationPort 113 uint16 114 UDPPacket::DestinationPort() const 115 { 116 return fDestinationPort; 117 } 118 119 120 // #pragma mark - UDPSocket 121 122 // constructor 123 UDPSocket::UDPSocket() 124 : fUDPService(NetStack::Default()->GetUDPService()), 125 fFirstPacket(NULL), 126 fLastPacket(NULL), 127 fAddress(INADDR_ANY), 128 fPort(0) 129 { 130 } 131 132 // destructor 133 UDPSocket::~UDPSocket() 134 { 135 if (fPort != 0 && fUDPService) 136 fUDPService->UnbindSocket(this); 137 } 138 139 // Bind 140 status_t 141 UDPSocket::Bind(ip_addr_t address, uint16 port) 142 { 143 if (!fUDPService) { 144 printf("UDPSocket::Bind(): no UDP service\n"); 145 return B_NO_INIT; 146 } 147 148 if (address == INADDR_BROADCAST || port == 0) { 149 printf("UDPSocket::Bind(): broadcast IP or port 0\n"); 150 return B_BAD_VALUE; 151 } 152 153 if (fPort != 0) { 154 printf("UDPSocket::Bind(): already bound\n"); 155 return EALREADY; // correct code? 156 } 157 158 status_t error = fUDPService->BindSocket(this, address, port); 159 if (error != B_OK) { 160 printf("UDPSocket::Bind(): service BindSocket() failed\n"); 161 return error; 162 } 163 164 fAddress = address; 165 fPort = port; 166 167 return B_OK; 168 } 169 170 // Send 171 status_t 172 UDPSocket::Send(ip_addr_t destinationAddress, uint16 destinationPort, 173 ChainBuffer *buffer) 174 { 175 if (!fUDPService) 176 return B_NO_INIT; 177 178 return fUDPService->Send(fPort, destinationAddress, destinationPort, 179 buffer); 180 } 181 182 // Send 183 status_t 184 UDPSocket::Send(ip_addr_t destinationAddress, uint16 destinationPort, 185 const void *data, size_t size) 186 { 187 if (!data) 188 return B_BAD_VALUE; 189 190 ChainBuffer buffer((void*)data, size); 191 return Send(destinationAddress, destinationPort, &buffer); 192 } 193 194 // Receive 195 status_t 196 UDPSocket::Receive(UDPPacket **_packet, bigtime_t timeout) 197 { 198 if (!fUDPService) 199 return B_NO_INIT; 200 201 if (!_packet) 202 return B_BAD_VALUE; 203 204 bigtime_t startTime = system_time(); 205 for (;;) { 206 fUDPService->ProcessIncomingPackets(); 207 if ((*_packet = PopPacket())) 208 return B_OK; 209 210 if (system_time() - startTime > timeout) 211 return (timeout == 0 ? B_WOULD_BLOCK : B_TIMED_OUT); 212 } 213 } 214 215 // PushPacket 216 void 217 UDPSocket::PushPacket(UDPPacket *packet) 218 { 219 if (fLastPacket) 220 fLastPacket->SetNext(packet); 221 else 222 fFirstPacket = packet; 223 224 fLastPacket = packet; 225 packet->SetNext(NULL); 226 } 227 228 // PopPacket 229 UDPPacket * 230 UDPSocket::PopPacket() 231 { 232 if (!fFirstPacket) 233 return NULL; 234 235 UDPPacket *packet = fFirstPacket; 236 fFirstPacket = packet->Next(); 237 238 if (!fFirstPacket) 239 fLastPacket = NULL; 240 241 packet->SetNext(NULL); 242 return packet; 243 } 244 245 246 // #pragma mark - UDPService 247 248 // constructor 249 UDPService::UDPService(IPService *ipService) 250 : IPSubService(kUDPServiceName), 251 fIPService(ipService) 252 { 253 } 254 255 // destructor 256 UDPService::~UDPService() 257 { 258 if (fIPService) 259 fIPService->UnregisterIPSubService(this); 260 } 261 262 // Init 263 status_t 264 UDPService::Init() 265 { 266 if (!fIPService) 267 return B_BAD_VALUE; 268 if (!fIPService->RegisterIPSubService(this)) 269 return B_NO_MEMORY; 270 return B_OK; 271 } 272 273 // IPProtocol 274 uint8 275 UDPService::IPProtocol() const 276 { 277 return IPPROTO_UDP; 278 } 279 280 // HandleIPPacket 281 void 282 UDPService::HandleIPPacket(IPService *ipService, ip_addr_t sourceIP, 283 ip_addr_t destinationIP, const void *data, size_t size) 284 { 285 TRACE(("UDPService::HandleIPPacket(): source: %08lx, destination: %08lx, " 286 "%lu - %lu bytes\n", sourceIP, destinationIP, size, 287 sizeof(udp_header))); 288 289 if (!data || size < sizeof(udp_header)) 290 return; 291 292 // check the header 293 const udp_header *header = (const udp_header*)data; 294 uint16 length = ntohs(header->length); 295 if (length < sizeof(udp_header) || length > size 296 || (header->checksum != 0 // 0 => checksum disabled 297 && _ChecksumData(data, length, sourceIP, destinationIP) != 0)) { 298 TRACE(("UDPService::HandleIPPacket(): dropping packet -- invalid size " 299 "or checksum\n")); 300 return; 301 } 302 303 // find the target socket 304 UDPSocket *socket = _FindSocket(destinationIP, header->destination); 305 if (!socket) 306 return; 307 308 // create a UDPPacket and queue it in the socket 309 UDPPacket *packet = new(nothrow) UDPPacket; 310 if (!packet) 311 return; 312 status_t error = packet->SetTo((uint8*)data + sizeof(udp_header), 313 length - sizeof(udp_header), sourceIP, header->source, destinationIP, 314 header->destination); 315 if (error == B_OK) 316 socket->PushPacket(packet); 317 else 318 delete packet; 319 } 320 321 // Send 322 status_t 323 UDPService::Send(uint16 sourcePort, ip_addr_t destinationAddress, 324 uint16 destinationPort, ChainBuffer *buffer) 325 { 326 TRACE(("UDPService::Send(source port: %hu, to: %08lx:%hu, %lu bytes)\n", 327 sourcePort, destinationAddress, destinationPort, 328 (buffer ? buffer->TotalSize() : 0))); 329 330 if (!fIPService) 331 return B_NO_INIT; 332 333 if (!buffer) 334 return B_BAD_VALUE; 335 336 // prepend the UDP header 337 udp_header header; 338 ChainBuffer headerBuffer(&header, sizeof(header), buffer); 339 header.source = htons(sourcePort); 340 header.destination = htons(destinationPort); 341 header.length = htons(headerBuffer.TotalSize()); 342 343 // compute the checksum 344 header.checksum = 0; 345 header.checksum = htons(_ChecksumBuffer(&headerBuffer, 346 fIPService->IPAddress(), destinationAddress, 347 headerBuffer.TotalSize())); 348 // 0 means checksum disabled; 0xffff is equivalent in this case 349 if (header.checksum == 0) 350 header.checksum = 0xffff; 351 352 return fIPService->Send(destinationAddress, IPPROTO_UDP, &headerBuffer); 353 } 354 355 // ProcessIncomingPackets 356 void 357 UDPService::ProcessIncomingPackets() 358 { 359 if (fIPService) 360 fIPService->ProcessIncomingPackets(); 361 } 362 363 // BindSocket 364 status_t 365 UDPService::BindSocket(UDPSocket *socket, ip_addr_t address, uint16 port) 366 { 367 if (!socket) 368 return B_BAD_VALUE; 369 370 if (_FindSocket(address, port)) { 371 printf("UDPService::BindSocket(): address in use\n"); 372 return EADDRINUSE; 373 } 374 375 return fSockets.Add(socket); 376 } 377 378 // UnbindSocket 379 void 380 UDPService::UnbindSocket(UDPSocket *socket) 381 { 382 fSockets.Remove(socket); 383 } 384 385 // _ChecksumBuffer 386 uint16 387 UDPService::_ChecksumBuffer(ChainBuffer *buffer, ip_addr_t source, 388 ip_addr_t destination, uint16 length) 389 { 390 // The checksum is calculated over a pseudo-header plus the UDP packet. 391 // So we temporarily prepend the pseudo-header. 392 struct pseudo_header { 393 ip_addr_t source; 394 ip_addr_t destination; 395 uint8 pad; 396 uint8 protocol; 397 uint16 length; 398 } __attribute__ ((__packed__)); 399 pseudo_header header = { 400 htonl(source), 401 htonl(destination), 402 0, 403 IPPROTO_UDP, 404 htons(length) 405 }; 406 407 ChainBuffer headerBuffer(&header, sizeof(header), buffer); 408 uint16 checksum = ip_checksum(&headerBuffer); 409 headerBuffer.DetachNext(); 410 return checksum; 411 } 412 413 // _ChecksumData 414 uint16 415 UDPService::_ChecksumData(const void *data, uint16 length, ip_addr_t source, 416 ip_addr_t destination) 417 { 418 ChainBuffer buffer((void*)data, length); 419 return _ChecksumBuffer(&buffer, source, destination, length); 420 } 421 422 // _FindSocket 423 UDPSocket * 424 UDPService::_FindSocket(ip_addr_t address, uint16 port) 425 { 426 int count = fSockets.Count(); 427 for (int i = 0; i < count; i++) { 428 UDPSocket *socket = fSockets.ElementAt(i); 429 if ((address == INADDR_ANY || socket->Address() == INADDR_ANY 430 || socket->Address() == address) 431 && port == socket->Port()) { 432 return socket; 433 } 434 } 435 436 return NULL; 437 } 438