Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r1d94e21 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);
     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        }
    116138       
    117139        size_t data_offs = ROUND_UP(hdr_size, 4);
    … …  
    149171                return ENOMEM;
    150172       
     173        /* Allocate identifier */
     174        fibril_mutex_lock(&ip_ident_lock);
     175        uint16_t ident = ++ip_ident;
     176        fibril_mutex_unlock(&ip_ident_lock);
     177       
    151178        /* Encode header fields */
    152         ip_header_t *hdr = (ip_header_t *) data;
    153        
    154         hdr->ver_ihl =
    155             (4 << VI_VERSION_l) | (hdr_size / sizeof(uint32_t));
    156         hdr->tos = packet->tos;
    157         hdr->tot_len = host2uint16_t_be(size);
    158         hdr->id = host2uint16_t_be(packet->ident);
    159         hdr->flags_foff = host2uint16_t_be(flags_foff);
    160         hdr->ttl = packet->ttl;
    161         hdr->proto = packet->proto;
    162         hdr->chksum = 0;
    163         hdr->src_addr = host2uint32_t_be(src);
    164         hdr->dest_addr = host2uint32_t_be(dest);
    165        
    166         /* Compute checksum */
    167         uint16_t chksum = inet_checksum_calc(INET_CHECKSUM_INIT,
    168             (void *) hdr, hdr_size);
    169         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        }
    170222       
    171223        /* Copy payload */
    … …  
    179231}
    180232
    181 /** Encode IPv6 PDU.
    182  *
    183  * Encode internet packet into PDU (serialized form). Will encode a
    184  * fragment of the payload starting at offset @a offs. The resulting
    185  * PDU will have at most @a mtu bytes. @a *roffs will be set to the offset
    186  * of remaining payload. If some data is remaining, the MF flag will
    187  * be set in the header, otherwise the offset will equal @a packet->size.
    188  *
    189  * @param packet Packet to encode
    190  * @param src    Source address
    191  * @param dest   Destination address
    192  * @param offs   Offset into packet payload (in bytes)
    193  * @param mtu    MTU (Maximum Transmission Unit) in bytes
    194  * @param rdata  Place to store pointer to allocated data buffer
    195  * @param rsize  Place to store size of allocated data buffer
    196  * @param roffs Place to store offset of remaning data
    197  *
    198  */
    199 int inet_pdu_encode6(inet_packet_t *packet, addr128_t src, addr128_t dest,
    200     size_t offs, size_t mtu, void **rdata, size_t *rsize, size_t *roffs)
    201 {
    202         /* Upper bound for fragment offset field */
    203         size_t fragoff_limit = 1 << (FF_FRAGOFF_h - FF_FRAGOFF_l);
    204        
    205         /* Verify that total size of datagram is within reasonable bounds */
    206         if (offs + packet->size > FRAG_OFFS_UNIT * fragoff_limit)
    207                 return ELIMIT;
    208        
    209         size_t hdr_size = sizeof(ip6_header_t);
    210        
    211         size_t data_offs = ROUND_UP(hdr_size, 4);
    212        
    213         assert(offs % FRAG_OFFS_UNIT == 0);
    214         assert(offs / FRAG_OFFS_UNIT < fragoff_limit);
    215        
    216 #if 0
    217         // FIXME TODO fragmentation
    218        
    219         /* Value for the fragment offset field */
    220         uint16_t foff = offs / FRAG_OFFS_UNIT;
    221 #endif
    222        
    223         if (hdr_size >= mtu)
    224                 return EINVAL;
    225        
    226         /* Amount of space in the PDU available for payload */
    227         size_t spc_avail = mtu - hdr_size;
    228         spc_avail -= (spc_avail % FRAG_OFFS_UNIT);
    229        
    230         /* Amount of data (payload) to transfer */
    231         size_t xfer_size = min(packet->size - offs, spc_avail);
    232        
    233         /* Total PDU size */
    234         size_t size = hdr_size + xfer_size;
    235        
    236         /* Offset of remaining payload */
    237         size_t rem_offs = offs + xfer_size;
    238        
    239 #if 0
    240         // FIXME TODO fragmentation
    241        
    242         /* Flags */
    243         uint16_t flags_foff =
    244             (packet->df ? BIT_V(uint16_t, FF_FLAG_DF) : 0) +
    245             (rem_offs < packet->size ? BIT_V(uint16_t, FF_FLAG_MF) : 0) +
    246             (foff << FF_FRAGOFF_l);
    247 #endif
    248        
    249         void *data = calloc(size, 1);
    250         if (data == NULL)
    251                 return ENOMEM;
    252        
    253         /* Encode header fields */
    254         ip6_header_t *hdr6 = (ip6_header_t *) data;
    255        
    256         hdr6->ver_tc = (6 << (VI_VERSION_l));
    257         memset(hdr6->tc_fl, 0, 3);
    258         hdr6->payload_len = host2uint16_t_be(packet->size);
    259         hdr6->next = packet->proto;
    260         hdr6->hop_limit = packet->ttl;
    261        
    262         host2addr128_t_be(src, hdr6->src_addr);
    263         host2addr128_t_be(dest, hdr6->dest_addr);
    264        
    265         /* Copy payload */
    266         memcpy((uint8_t *) data + data_offs, packet->data + offs, xfer_size);
    267        
    268         *rdata = data;
    269         *rsize = size;
    270         *roffs = rem_offs;
    271        
    272         return EOK;
    273 }
    274 
    275 /** Decode IPv4 datagram
    276  *
    277  * @param data   Serialized IPv4 datagram
    278  * @param size   Length of serialized IPv4 datagram
    279  * @param packet IP datagram structure to be filled
    280  *
    281  * @return EOK on success
    282  * @return EINVAL if the datagram is invalid or damaged
    283  * @return ENOMEM if not enough memory
    284  *
    285  */
    286233int inet_pdu_decode(void *data, size_t size, inet_packet_t *packet)
    287234{
    … …  
    310257        if (tot_len > size) {
    311258                log_msg(LOG_DEFAULT, LVL_DEBUG, "Total Length = %zu > PDU size = %zu",
    312                     tot_len, size);
     259                        tot_len, size);
    313260                return EINVAL;
    314261        }
    … …  
    347294}
    348295
    349 /** Decode IPv6 datagram
    350  *
    351  * @param data   Serialized IPv6 datagram
    352  * @param size   Length of serialized IPv6 datagram
    353  * @param packet IP datagram structure to be filled
    354  *
    355  * @return EOK on success
    356  * @return EINVAL if the datagram is invalid or damaged
    357  * @return ENOMEM if not enough memory
    358  *
    359  */
    360296int inet_pdu_decode6(void *data, size_t size, inet_packet_t *packet)
    361297{
    362         log_msg(LOG_DEFAULT, LVL_DEBUG, "inet_pdu_decode6()");
    363        
    364         if (size < sizeof(ip6_header_t)) {
    365                 log_msg(LOG_DEFAULT, LVL_DEBUG, "PDU too short (%zu)", size);
    366                 return EINVAL;
    367         }
    368        
    369         ip6_header_t *hdr6 = (ip6_header_t *) data;
    370        
    371         uint8_t version = BIT_RANGE_EXTRACT(uint8_t, VI_VERSION_h,
    372             VI_VERSION_l, hdr6->ver_tc);
    373         if (version != 6) {
    374                 log_msg(LOG_DEFAULT, LVL_DEBUG, "Version (%d) != 6", version);
    375                 return EINVAL;
    376         }
    377        
    378         size_t payload_len = uint16_t_be2host(hdr6->payload_len);
    379         if (payload_len + sizeof(ip6_header_t) > size) {
    380                 log_msg(LOG_DEFAULT, LVL_DEBUG, "Total Length = %zu > PDU size = %zu",
    381                     payload_len + sizeof(ip6_header_t), size);
    382                 return EINVAL;
    383         }
    384        
    385 #if 0
    386         // FIXME TODO fragmentation
    387        
    388         uint16_t ident = uint16_t_be2host(hdr->id);
    389         uint16_t flags_foff = uint16_t_be2host(hdr->flags_foff);
    390         uint16_t foff = BIT_RANGE_EXTRACT(uint16_t, FF_FRAGOFF_h, FF_FRAGOFF_l,
    391             flags_foff);
    392 #endif
    393        
    394         /* XXX Checksum */
    395        
    396         addr128_t src;
    397         addr128_t dest;
    398        
    399         addr128_t_be2host(hdr6->src_addr, src);
    400         inet_addr_set6(src, &packet->src);
    401        
    402         addr128_t_be2host(hdr6->dest_addr, dest);
    403         inet_addr_set6(dest, &packet->dest);
    404        
    405         packet->tos = 0;
    406         packet->proto = hdr6->next;
    407         packet->ttl = hdr6->hop_limit;
    408        
    409 #if 0
    410         // FIXME TODO fragmentation
    411        
    412         packet->ident = ident;
    413         packet->df = (flags_foff & BIT_V(uint16_t, FF_FLAG_DF)) != 0;
    414         packet->mf = (flags_foff & BIT_V(uint16_t, FF_FLAG_MF)) != 0;
    415         packet->offs = foff * FRAG_OFFS_UNIT;
    416        
    417         /* XXX IP options */
    418         size_t data_offs = sizeof(uint32_t) *
    419             BIT_RANGE_EXTRACT(uint8_t, VI_IHL_h, VI_IHL_l, hdr->ver_ihl);
    420 #endif
    421        
    422         packet->ident = 0;
    423         packet->df = 0;
    424         packet->mf = 0;
    425         packet->offs = 0;
    426        
    427         packet->size = payload_len;
    428         packet->data = calloc(packet->size, 1);
    429         if (packet->data == NULL) {
    430                 log_msg(LOG_DEFAULT, LVL_WARN, "Out of memory.");
    431                 return ENOMEM;
    432         }
    433        
    434         memcpy(packet->data, (uint8_t *) data + sizeof(ip6_header_t), packet->size);
    435        
    436         return EOK;
    437 }
    438 
    439 /** Encode NDP packet
    440  *
    441  * @param ndp   NDP packet structure to be serialized
    442  * @param dgram IPv6 datagram structure to be filled
    443  *
    444  * @return EOK on success
    445  *
    446  */
    447 int ndp_pdu_encode(ndp_packet_t *ndp, inet_dgram_t *dgram)
    448 {
    449         inet_addr_set6(ndp->sender_proto_addr, &dgram->src);
    450         inet_addr_set6(ndp->target_proto_addr, &dgram->dest);
    451         dgram->tos = 0;
    452         dgram->size = sizeof(icmpv6_message_t) + sizeof(ndp_message_t);
    453        
    454         dgram->data = calloc(1, dgram->size);
    455         if (dgram->data == NULL)
    456                 return ENOMEM;
    457        
    458         icmpv6_message_t *icmpv6 = (icmpv6_message_t *) dgram->data;
    459        
    460         icmpv6->type = ndp->opcode;
    461         icmpv6->code = 0;
    462         memset(icmpv6->un.ndp.reserved, 0, 3);
    463        
    464         ndp_message_t *message = (ndp_message_t *) (icmpv6 + 1);
    465        
    466         if (ndp->opcode == ICMPV6_NEIGHBOUR_SOLICITATION) {
    467                 host2addr128_t_be(ndp->solicited_ip, message->target_address);
    468                 message->option = 1;
    469                 icmpv6->un.ndp.flags = 0;
    470         } else {
    471                 host2addr128_t_be(ndp->sender_proto_addr, message->target_address);
    472                 message->option = 2;
    473                 icmpv6->un.ndp.flags = NDP_FLAG_OVERRIDE | NDP_FLAG_SOLICITED;
    474         }
    475        
    476         message->length = 1;
    477         addr48(ndp->sender_hw_addr, message->mac);
    478        
    479         icmpv6_phdr_t phdr;
    480        
    481         host2addr128_t_be(ndp->sender_proto_addr, phdr.src_addr);
    482         host2addr128_t_be(ndp->target_proto_addr, phdr.dest_addr);
    483         phdr.length = host2uint32_t_be(dgram->size);
    484         memset(phdr.zeroes, 0, 3);
    485         phdr.next = IP_PROTO_ICMPV6;
    486        
    487         uint16_t cs_phdr =
    488             inet_checksum_calc(INET_CHECKSUM_INIT, &phdr,
    489             sizeof(icmpv6_phdr_t));
    490        
    491         uint16_t cs_all = inet_checksum_calc(cs_phdr, dgram->data,
    492             dgram->size);
    493        
    494         icmpv6->checksum = host2uint16_t_be(cs_all);
    495        
    496         return EOK;
    497 }
    498 
    499 /** Decode NDP packet
    500  *
    501  * @param dgram Incoming IPv6 datagram encapsulating NDP packet
    502  * @param ndp   NDP packet structure to be filled
    503  *
    504  * @return EOK on success
    505  * @return EINVAL if the Datagram is invalid
    506  *
    507  */
    508 int ndp_pdu_decode(inet_dgram_t *dgram, ndp_packet_t *ndp)
    509 {
    510         uint16_t src_af = inet_addr_get(&dgram->src, NULL,
    511             &ndp->sender_proto_addr);
    512         if (src_af != AF_INET6)
    513                 return EINVAL;
    514        
    515         if (dgram->size < sizeof(icmpv6_message_t) + sizeof(ndp_message_t))
    516                 return EINVAL;
    517        
    518         icmpv6_message_t *icmpv6 = (icmpv6_message_t *) dgram->data;
    519        
    520         ndp->opcode = icmpv6->type;
    521        
    522         ndp_message_t *message = (ndp_message_t *) (icmpv6 + 1);
    523        
    524         addr128_t_be2host(message->target_address, ndp->target_proto_addr);
    525         addr48(message->mac, ndp->sender_hw_addr);
    526        
    527         return EOK;
     298        // FIXME TODO
     299        return ENOTSUP;
    528300}
    529301
Note: See TracChangeset for help on using the changeset viewer.