RIOT/dist/tools/linux-border_router/multiplex.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <arpa/inet.h>

#include "flowcontrol.h"
#include "multiplex.h"
#include "serial.h"
#include "sixlowdriver.h"

#define END         0xC0
#define ESC         0xDB
#define END_ESC     0xDC
#define ESC_ESC     0xDD

uint8_t serial_out_buf[BUFFER_SIZE];
uint8_t serial_in_buf[BUFFER_SIZE];

uint8_t *get_serial_out_buffer(int offset) {
    if (offset > BUFFER_SIZE) {
        return NULL;
    }
    
    return &(serial_out_buf[offset]);
}

uint8_t *get_serial_in_buffer(int offset) {
    if (offset > BUFFER_SIZE) {
        return NULL;
    }
    
    return &(serial_in_buf[offset]);
}

int init_multiplex(const char *tty_dev) {
    return open_serial_port(tty_dev);
}

uint8_t serial_read_byte() {
    unsigned char c;
    
    read_serial_port(&c, 1);
    
    return (uint8_t)c;
}

int readpacket(uint8_t *packet_buf, size_t size) {
    uint8_t *line_buf_ptr = packet_buf;
    uint8_t byte = END+1;
    uint8_t esc = 0;
    uint8_t translate = 1;
    
    while ((line_buf_ptr - packet_buf) < size-1) {
        byte = serial_read_byte();
        
        if (translate && byte == END) {
            break;
        }
        
        if (line_buf_ptr == packet_buf && byte != 0) {
            translate = 0;
        }
        
        if (line_buf_ptr > packet_buf && !translate && byte == '\n') {
            *line_buf_ptr++ = '\0';
            return line_buf_ptr - packet_buf;
        }
        
        if (translate) {
            if (esc) {
                esc = 0;
                switch (byte) {
                    case(END_ESC):{
                        *line_buf_ptr++ = END;
                        continue;
                    }
                    case(ESC_ESC):{
                        *line_buf_ptr++ = ESC;
                        continue;
                    }
                    default:
                        continue;
                }
            }
                    
            if (byte == ESC) {
                esc = 1;
                continue;
            }
        }
        
        *line_buf_ptr++ = byte;
    }
    
    return (line_buf_ptr - packet_buf);
}



int writepacket(uint8_t *packet_buf, size_t size) {
    uint8_t packet_tmp[2*BUFFER_SIZE];
    uint8_t *byte_ptr = packet_buf;
    uint8_t *tmp_ptr = packet_tmp;
    
    if (2*size + 1 > BUFFER_SIZE) {
        return -1;
    }
    
    while ((byte_ptr - packet_buf) < size) {
        switch (*byte_ptr) {
            case(END):{
                *byte_ptr = END_ESC;
                *tmp_ptr = ESC;
                tmp_ptr++;
                break;
            }
            case(ESC):{
                *byte_ptr = ESC_ESC;
                *tmp_ptr = ESC;
                tmp_ptr++;
                break;
            }
            default:{
                break;
            }
        }
        
        *tmp_ptr = *byte_ptr;
        byte_ptr++;
        tmp_ptr++;
    }
    
    *tmp_ptr++ = END;
    write_serial_port(packet_tmp, tmp_ptr - packet_tmp);
    
    return 0;
}

void demultiplex(const border_packet_t *packet, int len) {
    switch (packet->type) {
        case (BORDER_PACKET_RAW_TYPE):{
            printf("\033[00;33m[via serial interface] %s\033[00m\n",
                    ((unsigned char *)packet) + sizeof (border_packet_t)
                );
            break;
        }
        case (BORDER_PACKET_L3_TYPE):{
            border_l3_header_t *l3_header_buf = (border_l3_header_t *)packet;
            switch (l3_header_buf->ethertype) {
                case (ETHERTYPE_IPV6):{
                    printf("INFO: IPv6-Packet %d received\n", l3_header_buf->seq_num);
                    struct ip6_hdr *ip6_buf = (struct ip6_hdr *)(((unsigned char *)packet) + sizeof (border_l3_header_t));
                    border_send_ipv6_over_tun(get_tun_fd(), ip6_buf);
                    break;
                }
                default:
                    printf("INFO: Unknown ethertype %04x for packet %d\n", l3_header_buf->ethertype, l3_header_buf->seq_num);
                    break;
            }
            break;
        }
        case (BORDER_PACKET_CONF_TYPE):{
            border_conf_header_t *conf_header_buf = (border_conf_header_t *)packet;
            switch (conf_header_buf->conftype) {
                case (BORDER_CONF_SYNACK):{
                    printf("INFO: SYNACK-Packet %d received\n",conf_header_buf->seq_num);
                    signal_connection_established();
                    break;
                }
                case (BORDER_CONF_CONTEXT):{
                    printf("INFO: Context packet (%d) received, "
                            "but nothing is implemented yet for this case.\n",
                            conf_header_buf->seq_num);
                    break;
                }
                case (BORDER_CONF_IPADDR):{
                    char str_addr[IPV6_ADDR_LEN];
                    border_addr_packet_t *addr_packet = (border_addr_packet_t *)packet;
                    
                    printf("INFO: Address packet (%d) received.\n",
                            conf_header_buf->seq_num);
                    inet_ntop(AF_INET6, &addr_packet->addr, str_addr, IPV6_ADDR_LEN);
                    tun_add_addr(str_addr);
                }
                default:
                    printf("INFO: Unknown conftype %02x for packet %d\n", 
                            conf_header_buf->conftype, 
                            conf_header_buf->seq_num);
                    break;
            }
            break;
        }
        default:
            printf("INFO: Unknown border packet type %02x for packet %d\n", packet->type, packet->seq_num);
            //print_packet_hex((unsigned char *)packet,len);
            break;
    }
}

void multiplex_send_context_over_tty(const border_context_t *context) {
    border_context_packet_t *con_packet = (border_context_packet_t *)get_serial_out_buffer(0);
    
    con_packet->empty = 0;
    con_packet->type = BORDER_PACKET_CONF_TYPE;
    con_packet->conftype = BORDER_CONF_CONTEXT;
    memcpy(
            &con_packet->context,
            context, 
            sizeof (border_context_t)
        );
    
    flowcontrol_send_over_tty(
            (border_packet_t *) con_packet,
            sizeof (border_context_packet_t)
        );
}

void multiplex_send_addr_over_tty(struct in6_addr *addr) {
    border_addr_packet_t *packet = (border_addr_packet_t *)get_serial_out_buffer(0);
    
    packet->empty = 0;
    packet->type = BORDER_PACKET_CONF_TYPE;
    packet->conftype = BORDER_CONF_IPADDR;
    memcpy(
            &packet->addr,
            addr, 
            sizeof (struct in6_addr)
        );
    
    flowcontrol_send_over_tty(
            (border_packet_t *) packet,
            sizeof (border_addr_packet_t)
        );
}

void multiplex_send_ipv6_over_tty(const struct ip6_hdr *packet) {
    border_l3_header_t *l3_hdr = (border_l3_header_t *)get_serial_out_buffer(0);
    size_t packet_size = sizeof (struct ip6_hdr) + packet->ip6_plen;
    
    l3_hdr->empty = 0;
    l3_hdr->type = BORDER_PACKET_L3_TYPE;
    l3_hdr->ethertype = ETHERTYPE_IPV6;
    memcpy(
            get_serial_out_buffer(0) + sizeof (border_l3_header_t), 
            packet,
            packet_size
        );
    
    flowcontrol_send_over_tty(
            (border_packet_t *) l3_hdr,
            sizeof (border_l3_header_t) + packet_size
        );
}