File content as of revision 10:f0854784e960:

#include  "mbed.h"
#include   "log.h"
#include   "net.h"
#include "icmp6.h"
#include  "udp6.h"
#include  "tcp6.h"
#include    "ar.h"
#include  "dhcp.h"
#include "slaac.h"
#include   "eth.h"
#include   "ip6.h"

#define HEADER_LENGTH 40

char Ip6AllNodes[] = {  0xff, 0x02, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x01};

char Ip6AllRouters[] = {0xff, 0x02, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x02};

char Ip6Mdns[] = {      0xff, 0x02, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0xfb};

char Ip6Llmnr[] = {     0xff, 0x02, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00,
                        0x00, 0x01, 0x00, 0x03};

__packed struct header
{
    uint32_t versionTrafficFlow;
    uint16_t dataLength;
    uint8_t  protocol;
    uint8_t  hoplimit;
    char     src[16];
    char     dst[16];
};
static bool getIsSolicited(char* p)
{
    if (*p++ != 0xff) return false;
    if (*p++ != 0x02) return false;
    
    if (*p++ != 0x00) return false;
    if (*p++ != 0x00) return false;
    
    if (*p++ != 0x00) return false;
    if (*p++ != 0x00) return false;
    
    if (*p++ != 0x00) return false;
    if (*p++ != 0x00) return false;
    
    if (*p++ != 0x00) return false;
    if (*p++ != 0x00) return false;
    
    if (*p++ != 0x00) return false;
    if (*p++ != 0x01) return false;
    
    if (*p++ != 0xff) return false;
    
    return true;
}
static bool getIsSame(char* pA, char* pB)
{
    return memcmp(pA, pB, 16) == 0;
}
static bool getIsSameGroup(char* pA, char* pB)
{
    pA += 13;
    pB += 13;
    if (*pA++ != *pB++) return false;
    if (*pA++ != *pB++) return false;
    return *pA == *pB;
}
int Ip6HandleReceivedPacket(char* pSrcMac, void* pPacket, int* pSize, char* pDstMac)
{
    struct header * pHeader = (header*)pPacket;
    
    uint8_t     version  =            (pHeader->versionTrafficFlow >> 4) & 0xF;
    int      dataLength  = NetToHost16(pHeader->dataLength);
    uint8_t    protocol  =             pHeader->protocol;
    uint8_t    hoplimit  =             pHeader->hoplimit;
    char        src[16];   memcpy(src, pHeader->src, 16);
    char        dst[16];   memcpy(dst, pHeader->dst, 16);
    void*          pData = (char*)pPacket + HEADER_LENGTH;
    
    bool isMe        = getIsSame(dst, SlaacLinkLocalIp);
    bool isMulticast = dst[0] == 0xFF;
    bool isSolicited = getIsSolicited(dst);
    bool isGroup     = isSolicited && getIsSameGroup(dst, SlaacLinkLocalIp);
    
    bool doIt = isMe || (isMulticast && !isSolicited) || isGroup;
    
    if (!doIt) return DO_NOTHING;
    
    ArAdd6(pSrcMac, src);
    int action = DO_NOTHING;
    switch (protocol)
    {
        case ICMP6: action = Icmp6HandleReceivedPacket(src, dst, &dataLength, pData); break;
        case UDP:   action =  Udp6HandleReceivedPacket(src, dst, &dataLength, pData); break;
        case TCP:   action =  Tcp6HandleReceivedPacket(src, dst, &dataLength, pData); break;        
        default:
            char text[100];
            LogTimeF("IP6 packet unhandled\r\n");
            LogF("  Size              %d\r\n", *pSize);
            LogF("  Version           %d\r\n", version);
            LogF("  Payload length    %d\r\n", dataLength);
            LogF("  Hop limit         %d\r\n", hoplimit);
            NetProtocolToString(protocol, sizeof(text), text);
            LogF("  Protocol          %s\r\n", text);
            NetIp6AddressToString(src, sizeof(text), text);
            LogF("  Source IP         %s\r\n", text);
            NetIp6AddressToString(dst, sizeof(text), text);
            LogF("  Destination IP    %s\r\n", text);
            return DO_NOTHING;
    }
    switch (action)
    {
        case DO_NOTHING:
            return DO_NOTHING;
        case UNICAST:
            memcpy(pDstMac, pSrcMac, 6);
            break;
        case MULTICAST_ROUTER:
            break;
        default:
            LogTimeF("Ip6 unknown action %d\r\n", action);
            return DO_NOTHING;
    }

    memcpy(pHeader->dst, dst, 16);
    memcpy(pHeader->src, src, 16);
    pHeader->dataLength   = NetToHost16(dataLength);
        
    *pSize = HEADER_LENGTH + dataLength;
    
    return action;
}
int Ip6PollForPacketToSend(void* pPacket, int* pSize, char* pDstMac)
{    
    void* pData = (char*)pPacket + HEADER_LENGTH;
    int dataLength = 0;
    
    char pSrcIp[16];
    char pDstIp[16];
    uint8_t protocol = 0;
    int action = DO_NOTHING;
    if (action == DO_NOTHING)
    {
        action = Icmp6PollForPacketToSend(pData, &dataLength, pSrcIp, pDstIp);
        protocol = ICMP6;
    }
    
    if (action == DO_NOTHING)
    {
        action = Udp6PollForPacketToSend(pData, &dataLength, pSrcIp, pDstIp);
        protocol = UDP;
    }
    if (action == DO_NOTHING) return DO_NOTHING;
    
    ArRev6(pDstIp, pDstMac);             //Make the remote MAC from ARP

    uint8_t version  = 6;
    uint8_t hoplimit = 255;
    
    struct header * pHeader = (header*)pPacket;
    pHeader->versionTrafficFlow  = version << 4;
    pHeader->protocol            = protocol;
    pHeader->hoplimit            = 255;
    memcpy(pHeader->dst, pDstIp, 16);
    memcpy(pHeader->src, pSrcIp, 16);
    pHeader->dataLength   = NetToHost16(dataLength);
    
    *pSize = HEADER_LENGTH + dataLength;
    
    LogTimeF("Ip6 sending waiting packet\r\n");
    char text[100];
    LogF("  Size              %d\r\n", *pSize);
    LogF("  Version           %d\r\n", version);
    LogF("  Payload length    %d\r\n", dataLength);
    LogF("  Hop limit         %d\r\n", hoplimit);
    NetProtocolToString(protocol, sizeof(text), text);
    LogF("  Protocol          %s\r\n", text);
    NetIp6AddressToString(pSrcIp, sizeof(text), text);
    LogF("  Source IP         %s\r\n", text);
    NetIp6AddressToString(pDstIp, sizeof(text), text);
    LogF("  Destination IP    %s\r\n", text);
    
    return action;
}