10.0.0.21 Git - firmerware/blob - src/net/reply.c

   1 /* netreply.c -- Directly responding to parsed packets
   2  *
   3  * This file is part of 0cpm Firmerware.
   4  *
   5  * 0cpm Firmerware is Copyright (c)2011 Rick van Rein, OpenFortress.
   6  *
   7  * 0cpm Firmerware is free software: you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License as
   9  * published by the Free Software Foundation, version 3.
  10  *
  11  * 0cpm Firmerware is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with 0cpm Firmerware.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20
  21 #include <stdlib.h>
  22 #include <stdint.h>
  23 #include <stdbool.h>
  24 #include <stdarg.h>
  25
  26 // #include <netinet/ip.h>
  27 // #include <netinet/udp.h>
  28 // #include <netinet/ip6.h>
  29 // #include <netinet/ip_icmp.h>
  30 // #include <netinet/icmp6.h>
  31 // #include <netinet/if_ether.h>
  32
  33 #include <config.h>
  34
  35 #include <0cpm/cpu.h>
  36 #include <0cpm/irq.h>
  37 #include <0cpm/timer.h>
  38 #include <0cpm/netinet.h>
  39 #include <0cpm/netfun.h>
  40 #include <0cpm/netdb.h>
  41 #include <0cpm/cons.h>
  42
  43
  44 /* A few well-known addresses to look for
  45  */
  46 extern uint8_t ether_broadcast [ETHER_ADDR_LEN];
  47 uint8_t ether_zero [ETHER_ADDR_LEN] = { 0, 0, 0, 0, 0, 0 };
  48 uint8_t prefix_6bed4 [8] = {
  49                         0x20, 0x01, 0xab, 0xba, 0x6b, 0xed, 0x00, 0x04 };
  50 extern uint32_t ip4_6bed4;
  51 uint8_t ip4_mine [4] = { 192, 168, 3, 13 };
  52
  53 extern uint8_t linklocal_mine [];
  54
  55 //TODO:MAC_FROM_BOTTOM// uint8_t ether_mine [ETHER_ADDR_LEN] = { 0x11, 0x22, 0x33, 0x44, 0x55, 0x66 };
  56 uint8_t ether_mine [ETHER_ADDR_LEN] = { 0x00, 0x0b, 0x82, 0x19, 0xa0, 0xf4 };
  57
  58 uint16_t bootsecs = 0;
  59
  60
  61 /******************* HEADERVEL CREATION UTILITIES *******************/
  62
  63
  64 /* Create an Ethernet header.
  65  * Some fields are filled later: h_proto
  66  */
  67 uint8_t *netreply_ether (uint8_t *pout, intptr_t *mem) {
  68         struct ethhdr *ethin  = (struct ethhdr *) mem [MEM_ETHER_HEAD];
  69         struct ethhdr *ethout = (struct ethhdr *) pout;
  70         if (mem [MEM_ETHER_DST] != 0) {
  71                 memcpy (ethout->h_dest, (void *) mem [MEM_ETHER_DST], ETHER_ADDR_LEN);
  72         } else if (memcmp (ethin->h_source, ether_zero, ETHER_ADDR_LEN) != 0) {
  73                 memcpy (ethout->h_dest, ethin->h_source, ETHER_ADDR_LEN);
  74         } else {
  75                 memset (ethout->h_dest, 0xff, ETHER_ADDR_LEN);  // Broadcast
  76         }
  77         memcpy (ethout->h_source, ether_mine, ETHER_ADDR_LEN);
  78         mem [MEM_ETHER_HEAD] = (intptr_t) ethout;
  79         return &pout [sizeof (struct ethhdr)];
  80 }
  81
  82 /* Create an IPv4 header.
  83  * Some IPv4 fields are filled later: protocol, tot_len, check.
  84  */
  85 uint8_t *netreply_ip4 (uint8_t *pout, intptr_t *mem) {
  86         struct iphdr *ip4in;
  87         struct iphdr *ip4out;
  88         pout = netreply_ether (pout, mem);
  89         ip4in  = (struct iphdr *) mem [MEM_IP4_HEAD];
  90         ip4out = (struct iphdr *) pout;
  91         bzero (ip4out, sizeof (struct iphdr));
  92         netset8  (ip4out->version_ihl, 0x45);
  93         netset8  (ip4out->ttl, 64);
  94         netset16 (ip4out->frag_off, 0x4000);    // Don't fragment
  95         memcpy (&ip4out->saddr, &ip4in->daddr, 4);
  96         memcpy (&ip4out->daddr, &ip4in->saddr, 4);
  97         mem [MEM_IP4_HEAD] = (uintptr_t) ip4out;
  98         return &pout [sizeof (struct iphdr)];
  99 }
 100
 101 /* Create an UDPv4 header.
 102  * Some fields are filled later: len, check.
 103  */
 104 uint8_t *netreply_udp4 (uint8_t *pout, intptr_t *mem) {
 105         struct udphdr *udp;
 106         pout = netreply_ip4 (pout, mem);
 107         udp = (struct udphdr *) pout;
 108         netset16 (udp->source, mem [MEM_UDP4_DST_PORT]);
 109         netset16 (udp->dest  , mem [MEM_UDP4_SRC_PORT]);
 110         mem [MEM_UDP4_HEAD] = (intptr_t) udp;
 111         return &pout [sizeof (struct udphdr)];
 112 }
 113
 114 /* Create an UDPv4 header for 6bed4.
 115  * Some fields are filled later: len, check.
 116  */
 117 uint8_t *netreply_udp4_6bed4 (uint8_t *pout, intptr_t *mem) {
 118         struct udphdr *udp;
 119         mem [MEM_IP4_SRC] = htonl (ip4_6bed4);  // TODO -- netset16?
 120         pout = netreply_ip4 (pout, mem);
 121         udp = (struct udphdr *) pout;
 122         netset16 (udp->source, mem [MEM_UDP4_DST_PORT]);
 123         netset16 (udp->dest  , 3653);
 124         mem [MEM_6BED4_PLOAD] =
 125         mem [MEM_UDP4_HEAD] = (intptr_t) udp;
 126         return &pout [sizeof (struct udphdr)];
 127 }
 128
 129 /* Create an IPv6 header.
 130  * Some fields are filled later: plen, nxt.
 131  */
 132 uint8_t *netreply_ip6 (uint8_t *pout, intptr_t *mem) {
 133         struct ip6_hdr *in6 = (struct ip6_hdr *) mem [MEM_IP6_HEAD];
 134         struct ip6_hdr *ip6;
 135         if (mem [MEM_6BED4_PLOAD] != 0) {
 136                 // Use 6bed4 for destination address
 137                 pout = netreply_udp4_6bed4 (pout, mem);
 138         } else {
 139                 // Use LAN for destination address
 140                 pout = netreply_ether (pout, mem);
 141         }
 142         ip6 = (struct ip6_hdr *) pout;
 143         netset32 (ip6->ip6_flow, 0x60000000);
 144         netset8  (ip6->ip6_hlim, 64);
 145         memcpy (&ip6->ip6_src, &in6->ip6_dst, 16);
 146         memcpy (&ip6->ip6_dst, &in6->ip6_src, 16);
 147         mem [MEM_IP6_HEAD] = (intptr_t) ip6;
 148         return &pout [sizeof (struct ip6_hdr)];
 149 }
 150
 151 /* Create an UDPv6 header.
 152  * Some fields are filled later: len, check.
 153  */
 154 uint8_t *netreply_udp6 (uint8_t *pout, intptr_t *mem) {
 155         struct udphdr *udp;
 156         pout = netreply_ip6 (pout, mem);
 157         udp = (struct udphdr *) pout;
 158         netset16 (udp->source, mem [MEM_UDP6_DST_PORT]);
 159         netset16 (udp->dest  , mem [MEM_UDP6_SRC_PORT]);
 160         mem [MEM_UDP6_HEAD] = (intptr_t) udp;
 161         return &pout [sizeof (struct udphdr)];
 162 }
 163
 164
 165 /******************* APPLICATION LEVEL ROUTINES *******************/
 166
 167
 168 /* Create an ARP Reply packet to respond to an ARP Query
 169  * There are no checksum or length fields in ARP.
 170  */
 171 uint8_t *netreply_arp_query (uint8_t *pout, intptr_t *mem) {
 172         struct ether_arp *arpout;
 173         struct ether_arp *arpin;
 174         bottom_printf ("Received an ARP Request; replying\n");
 175         pout = netreply_ether (pout, mem);
 176         arpout = (struct ether_arp *) pout;
 177         arpin  = (struct ether_arp *) mem [MEM_ARP_HEAD];
 178         //TODO:OLD// if (memcmp (arpin->arp_tpa, ip4_mine, 4) == 0) {
 179         // TODO: Real binding for IPv4
 180         if (netget32 (*(nint32_t *)arpin->arp_tpa) == ip4binding [0].ip4addr) {
 181                 // arpout->ea_hdr.ar_hrd = htons (ARPHRD_IEEE802);
 182                 netset16 (arpout->ea_hdr.ar_hrd, ARPHRD_ETHER);
 183                 netset16 (arpout->ea_hdr.ar_pro, ETHERTYPE_IP);
 184                 netset8  (arpout->ea_hdr.ar_hln, ETHER_ADDR_LEN);
 185                 netset8  (arpout->ea_hdr.ar_pln, 4);    // IPv4 len
 186                 netset16 (arpout->ea_hdr.ar_op,  2);    // ARP Reply
 187                 memcpy (arpout->arp_sha, ether_mine, ETHER_ADDR_LEN);
 188                 netset32 (*(nint32_t *)arpout->arp_spa, mem [MEM_IP4_DST]);
 189                 memcpy (arpout->arp_tha, arpin->arp_sha, ETHER_ADDR_LEN);
 190                 memcpy (arpout->arp_tpa, arpin->arp_spa, 4);
 191                 return pout + sizeof (struct ether_arp);
 192         } else {
 193                 return NULL;
 194         }
 195 }
 196
 197 /* Create an ICMPv4 Echo Reply packet to respond to Echo Request
 198  * Some fields are filled later: icmp_cksum.
 199  */
 200 uint8_t *netreply_icmp4_echo_req (uint8_t *pout, intptr_t *mem) {
 201         struct icmphdr *icmp4out;
 202         struct icmphdr *icmp4in;
 203         uint32_t alen;
 204         bottom_printf ("Received an ICMPv4 Echo Request; replying\n");
 205         pout = netreply_ip4 (pout, mem);
 206         icmp4out = (struct icmphdr *) pout;
 207         icmp4in  = (struct icmphdr *) mem [MEM_ICMP4_HEAD];
 208         netset8  (icmp4out->type, ICMP_ECHOREPLY);
 209         netset8  (icmp4out->code, 0);
 210         memcpy (&icmp4out->un.echo, &icmp4in->un.echo, 4);
 211         pout = pout + sizeof (struct icmphdr);
 212         alen = mem [MEM_ALL_DONE] - mem [MEM_ICMP4_HEAD] - sizeof (struct icmphdr);
 213         if ((alen > 0) && (alen < 128)) {
 214                 memcpy (pout, &icmp4in [1], alen);
 215                 pout += alen;
 216         }
 217         mem [MEM_ICMP4_HEAD] = (intptr_t) icmp4out;
 218         return pout;
 219 }
 220
 221 /* Create an ICMPv6 Echo Reply packet to respond to Echo Request
 222  * Some fields are filled later: icmp6_cksum.
 223  */
 224 uint8_t *netreply_icmp6_echo_req (uint8_t *pout, intptr_t *mem) {
 225         struct icmp6_hdr *icmp6;
 226         uint16_t len;
 227         bottom_printf ("Received an ICMPv6 Echo Request; replying\n");
 228         pout = netreply_ip6 (pout, mem);
 229         icmp6 = (struct icmp6_hdr *) pout;
 230         netset8 (icmp6->icmp6_type, ICMP6_ECHO_REPLY);
 231         netset8 (icmp6->icmp6_code, 0);
 232         len = mem [MEM_ALL_DONE] - mem [MEM_ICMP6_HEAD];
 233         memcpy (&icmp6->icmp6_data8,
 234                         (void *) (mem [MEM_ICMP6_HEAD] + 4),
 235                         len - 4);
 236         mem [MEM_ICMP6_HEAD] = (intptr_t) icmp6;
 237         return pout + len;
 238 }
 239
 240 /* Create a Neighbour Advertisement packet to respond to Neighbour Discovery.
 241  * This will also respond to ff02::1:ffxx:xxxx packets that end in the last
 242  * 3 bytes of the ethernet address.
 243  *
 244  * Some fields are filled later: icmp6_cksum.
 245  */
 246 uint8_t *netreply_icmp6_ngb_disc (uint8_t *pout, intptr_t *mem) {
 247         int bndidx;
 248         uint8_t *addr;
 249         uint16_t flgs;
 250         struct ip6_hdr *ip6;
 251         struct icmp6_hdr *icmp6;
 252         bottom_printf ("Received an ICMPv6 Neighbour Discovery; replying\n");
 253         // The first comparison is against the link local address fe80::...
 254         bndidx = IP6BINDING_COUNT;
 255         addr = linklocal_mine;
 256         flgs = I6B_DEFEND_ME;
 257         do {
 258                 if ((flgs & (I6B_EXPIRED | I6B_DEFEND_ME)) == I6B_DEFEND_ME) {
 259                         if (memcmp ((void *) (mem [MEM_ICMP6_HEAD] + 8), addr, 16) == 0) {
 260                                 bndidx++;
 261                                 break;
 262                         }
 263                 }
 264                 addr = ip6binding [bndidx].ip6addr;
 265                 flgs = ip6binding [bndidx].flags;
 266         } while (bndidx-- >= 0);
 267         if (bndidx < 0) {
 268                 return NULL;
 269         }
 270         pout = netreply_ip6 (pout, mem);
 271         ip6 = (struct ip6_hdr *) mem [MEM_IP6_HEAD];
 272         memcpy (&ip6->ip6_src, addr, 16);
 273         netset8 (ip6->ip6_hlim, 255);
 274         icmp6 = (struct icmp6_hdr *) pout;
 275         netset8 (icmp6->icmp6_type, ND_NEIGHBOR_ADVERT);
 276         netset8 (icmp6->icmp6_code, 0);
 277         netset32 (icmp6->icmp6_data32 [0], 0x60000000);
 278         memcpy (&icmp6->icmp6_data32 [1], addr, 16);
 279         netset8 (icmp6->icmp6_data8 [20], ND_OPT_TARGET_LINKADDR);
 280         netset8 (icmp6->icmp6_data8 [21], 1);   // 1x 8 bytes
 281         memcpy (icmp6->icmp6_data8 + 22, ether_mine, ETHER_ADDR_LEN);
 282         mem [MEM_ICMP6_HEAD] = (intptr_t) icmp6;
 283         return pout + 8 + 16 + 8;
 284 }
 285
 286 /* Respond to a DHCPv4 OFFER with a REQUEST.  An address is being
 287  * offered in the yiaddr field (offset 16) of the DHCP packet, but
 288  * that will also be repeated in the future DHCP ACK.
 289  */
 290 uint8_t *netreply_dhcp4_offer (uint8_t *pout, intptr_t *mem) {
 291         uint8_t *yiaddrptr = (uint8_t *) (mem [MEM_DHCP4_HEAD] + 16);
 292         uint8_t *popt;
 293         static const uint8_t dhcp4_options [] = {
 294                 99, 130, 83, 99,        // Magic cookie, RFC 1497
 295                 53, 1, 3,               // DHCP message type REQUEST
 296                 50, 4, 0, 0, 0, 0,      // Requested IP @ 4 + 3 + 2
 297                 // 55, 4, 1, 3, 42, 2,  // Param Request List:
 298                                         // mask, router, ntp?, time offset?.
 299                 255                     // End Option
 300         };
 301         bottom_printf ("DHCPv4 offer for %d.%d.%d.%d received -- requesting its activation\n", (int) yiaddrptr [0], (int) yiaddrptr [1], (int) yiaddrptr [2], (int) yiaddrptr [3]);
 302         // TODO: Validate offer to be mine
 303         mem [MEM_ETHER_DST] = (intptr_t) ether_broadcast;
 304         pout = netreply_udp4 (pout, mem);
 305         netset32 (((struct iphdr *) mem [MEM_IP4_HEAD])->daddr, 0xffffffff);
 306         bzero (pout, 576);      // erase the acceptable package size
 307         pout [0] = 1;           // bootrequest
 308         pout [1] = 1;           // ARP hardware address type
 309         pout [2] = 6;           // ARP hardware address length
 310         pout [3] = 0;           // hops
 311         memcpy (pout + 4, ether_mine + 2, 4);           // client-randomiser
 312         *(uint16_t *) (pout +  8) = htons (bootsecs++); // 0 or seconds trying -- TODO:netset16
 313         // flags=0, no broadcast reply needed
 314         // ciaddr [4] is 0.0.0.0, the initial client address
 315         // yiaddr [4] is 0.0.0.0, the "your" address
 316         // siaddr [4] is 0.0.0.0, the server address is returned
 317         // giaddr [4] is 0.0.0.0, gateway address is not set by client
 318         memcpy (pout + 28, ether_mine, 6);      // client hw addr
 319         // sname [64], the server hostname is empty
 320         // file [128], the boot filename, is empty
 321         // options
 322         popt = pout + 236;
 323         memcpy (popt, dhcp4_options, sizeof (dhcp4_options));
 324         memcpy (popt + 4 + 3 + 2, yiaddrptr, 4);
 325         // return popt + sizeof (dhcp4_options);
 326         return popt + sizeof (dhcp4_options);
 327 }
 328
 329
 330 /* Respond to a DHCPv6 advertisement with a request.
 331  *
 332  * Note that the phone is supportive of multi-homing, and wants to
 333  * be reachable over as many IPv6 addresses as are available, so it
 334  * will actually take hold of all the IPv6 space that it can get.
 335  */
 336 uint8_t *netreply_dhcp6_advertise (uint8_t *pout, intptr_t *mem) {
 337         uint32_t adlen;
 338         struct udphdr *udp;
 339         nint16_t *options;
 340         bottom_printf ("DHCPv6 advertisement\n");
 341         // TODO: Validate offer to be mine
 342         adlen = mem [MEM_ALL_DONE] - mem [MEM_DHCP6_HEAD];
 343         if (adlen > 1024) {
 344                 return NULL;    // Suspicuously long options
 345         }
 346         pout = netreply_udp6 (pout, mem);
 347         udp = (struct udphdr *) mem [MEM_UDP6_HEAD];
 348         netset16 (udp->dest, 547);
 349         memcpy (pout, (void *) mem [MEM_DHCP6_HEAD], adlen);
 350         mem [MEM_DHCP6_HEAD] = (intptr_t) pout;
 351         netdb_dhcp6_recurse_options ((nint16_t *) (pout + 4), adlen - 4);
 352         pout [0] = 3;
 353         return pout + adlen;
 354 }
 355