Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed

  • uspace/srv/net/inetsrv/pdu.c

    r44c9ef4 r1d24ad3  
    4949#include "pdu.h"
    5050
     51static FIBRIL_MUTEX_INITIALIZE(ip_ident_lock);
     52static uint16_t ip_ident = 0;
     53
    5154/** One's complement addition.
    5255 *
    … …  
    8588}
    8689
    87 /** Encode IPv4 PDU.
     90/** Encode Internet PDU.
    8891 *
    8992 * Encode internet packet into PDU (serialized form). Will encode a
    … …  
    9396 * be set in the header, otherwise the offset will equal @a packet->size.
    9497 *
    95  * @param packet Packet to encode
    96  * @param src    Source address
    97  * @param dest   Destination address
    98  * @param offs   Offset into packet payload (in bytes)
    99  * @param mtu    MTU (Maximum Transmission Unit) in bytes
    100  * @param rdata  Place to store pointer to allocated data buffer
    101  * @param rsize  Place to store size of allocated data buffer
    102  * @param roffs  Place to store offset of remaning data
    103  *
    104  */
    105 int inet_pdu_encode(inet_packet_t *packet, addr32_t src, addr32_t dest,
    106     size_t offs, size_t mtu, void **rdata, size_t *rsize, size_t *roffs)
    107 {
     98 * @param packet        Packet to encode
     99 * @param offs          Offset into packet payload (in bytes)
     100 * @param mtu           MTU (Maximum Transmission Unit) in bytes
     101 * @param rdata         Place to store pointer to allocated data buffer
     102 * @param rsize         Place to store size of allocated data buffer
     103 * @param roffs         Place to store offset of remaning data
     104 */
     105int inet_pdu_encode(inet_packet_t *packet, size_t offs, size_t mtu,
     106    void **rdata, size_t *rsize, size_t *roffs)
     107{
     108        addr32_t src_v4;
     109        addr128_t src_v6;
     110        uint16_t src_af = inet_addr_get(&packet->src, &src_v4, &src_v6);
     111       
     112        addr32_t dest_v4;
     113        addr128_t dest_v6;
     114        uint16_t dest_af = inet_addr_get(&packet->dest, &dest_v4, &dest_v6);
     115       
     116        if (src_af != dest_af)
     117                return EINVAL;
     118       
    108119        /* Upper bound for fragment offset field */
    109120        size_t fragoff_limit = 1 << (FF_FRAGOFF_h - FF_FRAGOFF_l);
    … …  
    113124                return ELIMIT;
    114125       
    115         size_t hdr_size = sizeof(ip_header_t);
    116         if (hdr_size >= mtu)
    117                 return EINVAL;
    118        
    119         assert(hdr_size % 4 == 0);
     126        size_t hdr_size;
     127       
     128        switch (src_af) {
     129        case AF_INET:
     130                hdr_size = sizeof(ip_header_t);
     131                break;
     132        case AF_INET6:
     133                hdr_size = sizeof(ip6_header_t);
     134                break;
     135        default:
     136                assert(false);
     137        }
     138       
     139        size_t data_offs = ROUND_UP(hdr_size, 4);
     140       
    120141        assert(offs % FRAG_OFFS_UNIT == 0);
    121142        assert(offs / FRAG_OFFS_UNIT < fragoff_limit);
    … …  
    123144        /* Value for the fragment offset field */
    124145        uint16_t foff = offs / FRAG_OFFS_UNIT;
     146       
     147        if (hdr_size >= mtu)
     148                return EINVAL;
    125149       
    126150        /* Amount of space in the PDU available for payload */
    … …  
    147171                return ENOMEM;
    148172       
     173        /* Allocate identifier */
     174        fibril_mutex_lock(&ip_ident_lock);
     175        uint16_t ident = ++ip_ident;
     176        fibril_mutex_unlock(&ip_ident_lock);
     177       
    149178        /* Encode header fields */
    150         ip_header_t *hdr = (ip_header_t *) data;
    151        
    152         hdr->ver_ihl =
    153             (4 << VI_VERSION_l) | (hdr_size / sizeof(uint32_t));
    154         hdr->tos = packet->tos;
    155         hdr->tot_len = host2uint16_t_be(size);
    156         hdr->id = host2uint16_t_be(packet->ident);
    157         hdr->flags_foff = host2uint16_t_be(flags_foff);
    158         hdr->ttl = packet->ttl;
    159         hdr->proto = packet->proto;
    160         hdr->chksum = 0;
    161         hdr->src_addr = host2uint32_t_be(src);
    162         hdr->dest_addr = host2uint32_t_be(dest);
    163        
    164         /* Compute checksum */
    165         uint16_t chksum = inet_checksum_calc(INET_CHECKSUM_INIT,
    166             (void *) hdr, hdr_size);
    167         hdr->chksum = host2uint16_t_be(chksum);
     179        ip_header_t *hdr;
     180        ip6_header_t *hdr6;
     181       
     182        switch (src_af) {
     183        case AF_INET:
     184                hdr = (ip_header_t *) data;
     185               
     186                hdr->ver_ihl =
     187                    (4 << VI_VERSION_l) | (hdr_size / sizeof(uint32_t));
     188                hdr->tos = packet->tos;
     189                hdr->tot_len = host2uint16_t_be(size);
     190                hdr->id = host2uint16_t_be(ident);
     191                hdr->flags_foff = host2uint16_t_be(flags_foff);
     192                hdr->ttl = packet->ttl;
     193                hdr->proto = packet->proto;
     194                hdr->chksum = 0;
     195                hdr->src_addr = host2uint32_t_be(src_v4);
     196                hdr->dest_addr = host2uint32_t_be(dest_v4);
     197               
     198                /* Compute checksum */
     199                uint16_t chksum = inet_checksum_calc(INET_CHECKSUM_INIT,
     200                    (void *) hdr, hdr_size);
     201                hdr->chksum = host2uint16_t_be(chksum);
     202               
     203                break;
     204        case AF_INET6:
     205                // TODO FIXME: fragmentation
     206               
     207                hdr6 = (ip6_header_t *) data;
     208               
     209                hdr6->ver_tc = (6 << (VI_VERSION_l));
     210                memset(hdr6->tc_fl, 0, 3);
     211                hdr6->payload_len = host2uint16_t_be(packet->size);
     212                hdr6->next = packet->proto;
     213                hdr6->hop_limit = packet->ttl;
     214               
     215                host2addr128_t_be(src_v6, hdr6->src_addr);
     216                host2addr128_t_be(dest_v6, hdr6->dest_addr);
     217               
     218                break;
     219        default:
     220                assert(false);
     221        }
    168222       
    169223        /* Copy payload */
    170         memcpy((uint8_t *) data + hdr_size, packet->data + offs, xfer_size);
     224        memcpy((uint8_t *) data + data_offs, packet->data + offs, xfer_size);
    171225       
    172226        *rdata = data;
    … …  
    177231}
    178232
    179 /** Encode IPv6 PDU.
    180  *
    181  * Encode internet packet into PDU (serialized form). Will encode a
    182  * fragment of the payload starting at offset @a offs. The resulting
    183  * PDU will have at most @a mtu bytes. @a *roffs will be set to the offset
    184  * of remaining payload. If some data is remaining, the MF flag will
    185  * be set in the header, otherwise the offset will equal @a packet->size.
    186  *
    187  * @param packet Packet to encode
    188  * @param src    Source address
    189  * @param dest   Destination address
    190  * @param offs   Offset into packet payload (in bytes)
    191  * @param mtu    MTU (Maximum Transmission Unit) in bytes
    192  * @param rdata  Place to store pointer to allocated data buffer
    193  * @param rsize  Place to store size of allocated data buffer
    194  * @param roffs  Place to store offset of remaning data
    195  *
    196  */
    197 int inet_pdu_encode6(inet_packet_t *packet, addr128_t src, addr128_t dest,
    198     size_t offs, size_t mtu, void **rdata, size_t *rsize, size_t *roffs)
    199 {
    200         /* IPv6 mandates a minimal MTU of 1280 bytes */
    201         if (mtu < 1280)
    202                 return ELIMIT;
    203        
    204         /* Upper bound for fragment offset field */
    205         size_t fragoff_limit = 1 << (OF_FRAGOFF_h - OF_FRAGOFF_l);
    206        
    207         /* Verify that total size of datagram is within reasonable bounds */
    208         if (offs + packet->size > FRAG_OFFS_UNIT * fragoff_limit)
    209                 return ELIMIT;
    210        
    211         /* Determine whether we need the Fragment extension header */
    212         bool fragment;
    213         if (offs == 0)
    214                 fragment = (packet->size + sizeof(ip6_header_t) > mtu);
    215         else
    216                 fragment = true;
    217        
    218         size_t hdr_size;
    219         if (fragment)
    220                 hdr_size = sizeof(ip6_header_t) + sizeof(ip6_header_fragment_t);
    221         else
    222                 hdr_size = sizeof(ip6_header_t);
    223        
    224         if (hdr_size >= mtu)
    225                 return EINVAL;
    226        
    227         assert(sizeof(ip6_header_t) % 8 == 0);
    228         assert(hdr_size % 8 == 0);
    229         assert(offs % FRAG_OFFS_UNIT == 0);
    230         assert(offs / FRAG_OFFS_UNIT < fragoff_limit);
    231        
    232         /* Value for the fragment offset field */
    233         uint16_t foff = offs / FRAG_OFFS_UNIT;
    234        
    235         /* Amount of space in the PDU available for payload */
    236         size_t spc_avail = mtu - hdr_size;
    237         spc_avail -= (spc_avail % FRAG_OFFS_UNIT);
    238        
    239         /* Amount of data (payload) to transfer */
    240         size_t xfer_size = min(packet->size - offs, spc_avail);
    241        
    242         /* Total PDU size */
    243         size_t size = hdr_size + xfer_size;
    244        
    245         /* Offset of remaining payload */
    246         size_t rem_offs = offs + xfer_size;
    247        
    248         /* Flags */
    249         uint16_t offsmf =
    250             (rem_offs < packet->size ? BIT_V(uint16_t, OF_FLAG_M) : 0) +
    251             (foff << OF_FRAGOFF_l);
    252        
    253         void *data = calloc(size, 1);
    254         if (data == NULL)
    255                 return ENOMEM;
    256        
    257         /* Encode header fields */
    258         ip6_header_t *hdr6 = (ip6_header_t *) data;
    259        
    260         hdr6->ver_tc = (6 << (VI_VERSION_l));
    261         memset(hdr6->tc_fl, 0, 3);
    262         hdr6->hop_limit = packet->ttl;
    263        
    264         host2addr128_t_be(src, hdr6->src_addr);
    265         host2addr128_t_be(dest, hdr6->dest_addr);
    266        
    267         /* Optionally encode Fragment extension header fields */
    268         if (fragment) {
    269                 assert(offsmf != 0);
    270                
    271                 hdr6->payload_len = host2uint16_t_be(packet->size +
    272                     sizeof(ip6_header_fragment_t));
    273                 hdr6->next = IP6_NEXT_FRAGMENT;
    274                
    275                 ip6_header_fragment_t *hdr6f = (ip6_header_fragment_t *)
    276                     (hdr6 + 1);
    277                
    278                 hdr6f->next = packet->proto;
    279                 hdr6f->reserved = 0;
    280                 hdr6f->offsmf = host2uint16_t_be(offsmf);
    281                 hdr6f->id = host2uint32_t_be(packet->ident);
    282         } else {
    283                 assert(offsmf == 0);
    284                
    285                 hdr6->payload_len = host2uint16_t_be(packet->size);
    286                 hdr6->next = packet->proto;
    287         }
    288        
    289         /* Copy payload */
    290         memcpy((uint8_t *) data + hdr_size, packet->data + offs, xfer_size);
    291        
    292         *rdata = data;
    293         *rsize = size;
    294         *roffs = rem_offs;
    295        
    296         return EOK;
    297 }
    298 
    299 /** Decode IPv4 datagram
    300  *
    301  * @param data   Serialized IPv4 datagram
    302  * @param size   Length of serialized IPv4 datagram
    303  * @param packet IP datagram structure to be filled
    304  *
    305  * @return EOK on success
    306  * @return EINVAL if the datagram is invalid or damaged
    307  * @return ENOMEM if not enough memory
    308  *
    309  */
    310233int inet_pdu_decode(void *data, size_t size, inet_packet_t *packet)
    311234{
    … …  
    334257        if (tot_len > size) {
    335258                log_msg(LOG_DEFAULT, LVL_DEBUG, "Total Length = %zu > PDU size = %zu",
    336                     tot_len, size);
     259                        tot_len, size);
    337260                return EINVAL;
    338261        }
    … …  
    371294}
    372295
    373 /** Decode IPv6 datagram
    374  *
    375  * @param data   Serialized IPv6 datagram
    376  * @param size   Length of serialized IPv6 datagram
    377  * @param packet IP datagram structure to be filled
    378  *
    379  * @return EOK on success
    380  * @return EINVAL if the datagram is invalid or damaged
    381  * @return ENOMEM if not enough memory
    382  *
    383  */
    384296int inet_pdu_decode6(void *data, size_t size, inet_packet_t *packet)
    385297{
    386         log_msg(LOG_DEFAULT, LVL_DEBUG, "inet_pdu_decode6()");
    387        
    388         if (size < sizeof(ip6_header_t)) {
    389                 log_msg(LOG_DEFAULT, LVL_DEBUG, "PDU too short (%zu)", size);
    390                 return EINVAL;
    391         }
    392        
    393         ip6_header_t *hdr6 = (ip6_header_t *) data;
    394        
    395         uint8_t version = BIT_RANGE_EXTRACT(uint8_t, VI_VERSION_h,
    396             VI_VERSION_l, hdr6->ver_tc);
    397         if (version != 6) {
    398                 log_msg(LOG_DEFAULT, LVL_DEBUG, "Version (%d) != 6", version);
    399                 return EINVAL;
    400         }
    401        
    402         size_t payload_len = uint16_t_be2host(hdr6->payload_len);
    403         if (payload_len + sizeof(ip6_header_t) > size) {
    404                 log_msg(LOG_DEFAULT, LVL_DEBUG, "Payload Length = %zu > PDU size = %zu",
    405                     payload_len + sizeof(ip6_header_t), size);
    406                 return EINVAL;
    407         }
    408        
    409         uint32_t ident;
    410         uint16_t offsmf;
    411         uint16_t foff;
    412         uint16_t next;
    413         size_t data_offs = sizeof(ip6_header_t);
    414        
    415         /* Fragment extension header */
    416         if (hdr6->next == IP6_NEXT_FRAGMENT) {
    417                 ip6_header_fragment_t *hdr6f = (ip6_header_fragment_t *)
    418                     (hdr6 + 1);
    419                
    420                 ident = uint32_t_be2host(hdr6f->id);
    421                 offsmf = uint16_t_be2host(hdr6f->offsmf);
    422                 foff = BIT_RANGE_EXTRACT(uint16_t, OF_FRAGOFF_h, OF_FRAGOFF_l,
    423                     offsmf);
    424                 next = hdr6f->next;
    425                 data_offs += sizeof(ip6_header_fragment_t);
    426                 payload_len -= sizeof(ip6_header_fragment_t);
    427         } else {
    428                 ident = 0;
    429                 offsmf = 0;
    430                 foff = 0;
    431                 next = hdr6->next;
    432         }
    433        
    434         addr128_t src;
    435         addr128_t dest;
    436        
    437         addr128_t_be2host(hdr6->src_addr, src);
    438         inet_addr_set6(src, &packet->src);
    439        
    440         addr128_t_be2host(hdr6->dest_addr, dest);
    441         inet_addr_set6(dest, &packet->dest);
    442        
    443         packet->tos = 0;
    444         packet->proto = next;
    445         packet->ttl = hdr6->hop_limit;
    446         packet->ident = ident;
    447        
    448         packet->df = 1;
    449         packet->mf = (offsmf & BIT_V(uint16_t, OF_FLAG_M)) != 0;
    450         packet->offs = foff * FRAG_OFFS_UNIT;
    451        
    452         packet->size = payload_len;
    453         packet->data = calloc(packet->size, 1);
    454         if (packet->data == NULL) {
    455                 log_msg(LOG_DEFAULT, LVL_WARN, "Out of memory.");
    456                 return ENOMEM;
    457         }
    458        
    459         memcpy(packet->data, (uint8_t *) data + data_offs, packet->size);
    460        
    461         return EOK;
    462 }
    463 
    464 /** Encode NDP packet
    465  *
    466  * @param ndp   NDP packet structure to be serialized
    467  * @param dgram IPv6 datagram structure to be filled
    468  *
    469  * @return EOK on success
    470  *
    471  */
    472 int ndp_pdu_encode(ndp_packet_t *ndp, inet_dgram_t *dgram)
    473 {
    474         inet_addr_set6(ndp->sender_proto_addr, &dgram->src);
    475         inet_addr_set6(ndp->target_proto_addr, &dgram->dest);
    476         dgram->tos = 0;
    477         dgram->size = sizeof(icmpv6_message_t) + sizeof(ndp_message_t);
    478        
    479         dgram->data = calloc(1, dgram->size);
    480         if (dgram->data == NULL)
    481                 return ENOMEM;
    482        
    483         icmpv6_message_t *icmpv6 = (icmpv6_message_t *) dgram->data;
    484        
    485         icmpv6->type = ndp->opcode;
    486         icmpv6->code = 0;
    487         memset(icmpv6->un.ndp.reserved, 0, 3);
    488        
    489         ndp_message_t *message = (ndp_message_t *) (icmpv6 + 1);
    490        
    491         if (ndp->opcode == ICMPV6_NEIGHBOUR_SOLICITATION) {
    492                 host2addr128_t_be(ndp->solicited_ip, message->target_address);
    493                 message->option = 1;
    494                 icmpv6->un.ndp.flags = 0;
    495         } else {
    496                 host2addr128_t_be(ndp->sender_proto_addr, message->target_address);
    497                 message->option = 2;
    498                 icmpv6->un.ndp.flags = NDP_FLAG_OVERRIDE | NDP_FLAG_SOLICITED;
    499         }
    500        
    501         message->length = 1;
    502         addr48(ndp->sender_hw_addr, message->mac);
    503        
    504         icmpv6_phdr_t phdr;
    505        
    506         host2addr128_t_be(ndp->sender_proto_addr, phdr.src_addr);
    507         host2addr128_t_be(ndp->target_proto_addr, phdr.dest_addr);
    508         phdr.length = host2uint32_t_be(dgram->size);
    509         memset(phdr.zeroes, 0, 3);
    510         phdr.next = IP_PROTO_ICMPV6;
    511        
    512         uint16_t cs_phdr =
    513             inet_checksum_calc(INET_CHECKSUM_INIT, &phdr,
    514             sizeof(icmpv6_phdr_t));
    515        
    516         uint16_t cs_all = inet_checksum_calc(cs_phdr, dgram->data,
    517             dgram->size);
    518        
    519         icmpv6->checksum = host2uint16_t_be(cs_all);
    520        
    521         return EOK;
    522 }
    523 
    524 /** Decode NDP packet
    525  *
    526  * @param dgram Incoming IPv6 datagram encapsulating NDP packet
    527  * @param ndp   NDP packet structure to be filled
    528  *
    529  * @return EOK on success
    530  * @return EINVAL if the Datagram is invalid
    531  *
    532  */
    533 int ndp_pdu_decode(inet_dgram_t *dgram, ndp_packet_t *ndp)
    534 {
    535         uint16_t src_af = inet_addr_get(&dgram->src, NULL,
    536             &ndp->sender_proto_addr);
    537         if (src_af != AF_INET6)
    538                 return EINVAL;
    539        
    540         if (dgram->size < sizeof(icmpv6_message_t) + sizeof(ndp_message_t))
    541                 return EINVAL;
    542        
    543         icmpv6_message_t *icmpv6 = (icmpv6_message_t *) dgram->data;
    544        
    545         ndp->opcode = icmpv6->type;
    546        
    547         ndp_message_t *message = (ndp_message_t *) (icmpv6 + 1);
    548        
    549         addr128_t_be2host(message->target_address, ndp->target_proto_addr);
    550         addr48(message->mac, ndp->sender_hw_addr);
    551        
    552         return EOK;
     298        // FIXME TODO
     299        return ENOTSUP;
    553300}
    554301
Note: See TracChangeset for help on using the changeset viewer.