File content as of revision 11:c051adb70c5a:

#include    "mbed.h"
#include     "log.h"
#include     "net.h"
#include   "icmp6.h"
#include "udptcp6.h"
#include      "ar.h"
#include   "slaac.h"
#include     "eth.h"
#include     "ip6.h"
#include     "ndp.h"

#define DEBUG false

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};

void Ip6DestIpFromAction(int action, char* pDstIp)
{
    switch (action)
    {
        case   UNICAST:                                            break;
        case   UNICAST_DNS:    memcpy(pDstIp, NdpDnsServer,  16);  break;
        case MULTICAST_NODE:   memcpy(pDstIp, Ip6AllNodes,   16);  break;
        case MULTICAST_ROUTER: memcpy(pDstIp, Ip6AllRouters, 16);  break;
        case MULTICAST_MDNS:   memcpy(pDstIp, Ip6Mdns,       16);  break;
        case MULTICAST_LLMNR:  memcpy(pDstIp, Ip6Llmnr,      16);  break;
        default:
            LogTimeF("Ip6 DestIpFromAction unknown action %d\r\n", action);
            break;           
    }
}

#define HEADER_LENGTH 40
__packed struct header
{
    uint32_t versionTrafficFlow;
    uint16_t dataLength;
    uint8_t  protocol;
    uint8_t  hoplimit;
    char     src[16];
    char     dst[16];
};

static uint8_t     version;
static int      dataLength;
static uint8_t    protocol;
static uint8_t    hoplimit;
static char     pSrcIp[16];
static char     pDstIp[16];
static void*         pData;

static void readHeader(struct header * pHeader)
{
    version    =            (pHeader->versionTrafficFlow >> 4) & 0xF;
    dataLength = NetToHost16(pHeader->dataLength);
    protocol   =             pHeader->protocol;
    hoplimit   =             pHeader->hoplimit;
              memcpy(pSrcIp, pHeader->src, 16);
              memcpy(pDstIp, pHeader->dst, 16);
    pData      = (char*)pHeader + HEADER_LENGTH;
}
static void writeHeader(struct header * pHeader)
{
           pHeader->versionTrafficFlow  = version << 4;
           pHeader->protocol            = protocol;
           pHeader->hoplimit            = 255;
    memcpy(pHeader->dst, pDstIp, 16);
    memcpy(pHeader->src, pSrcIp, 16);
           pHeader->dataLength          = NetToHost16(dataLength);
}

static void logHeader(char* title)
{
    char text[100];
    LogTimeF("%s\r\n", title);
    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);
}

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;
    readHeader(pHeader);
    
    bool isMe        = getIsSame(pDstIp, SlaacLinkLocalIp);
    bool isMulticast = pDstIp[0] == 0xFF;
    bool isSolicited = getIsSolicited(pDstIp);
    bool isGroup     = isSolicited && getIsSameGroup(pDstIp, SlaacLinkLocalIp);
    
    bool doIt = isMe || (isMulticast && !isSolicited) || isGroup;
    
    if (!doIt) return DO_NOTHING;
    
    ArAdd6(pSrcMac, pSrcIp);
    
    if (DEBUG) logHeader("IP6 packet received");

    int action = DO_NOTHING;
    switch (protocol)
    {
        case ICMP6: action = Icmp6HandleReceivedPacket(pSrcIp, pDstIp, &dataLength, pData); break;
        case UDP:   action =  Udp6HandleReceivedPacket(pSrcIp, pDstIp, &dataLength, pData); break;
        case TCP:   action =  Tcp6HandleReceivedPacket(pSrcIp, pDstIp, &dataLength, pData); break;        
        default:
            logHeader("IP6 packet unhandled");
            return DO_NOTHING;
    }
    if (!action) return DO_NOTHING;
    
    memcpy(pDstMac, pSrcMac, 6);
    
    if (DEBUG) logHeader("IP6 packet replied to");

    writeHeader(pHeader);
      
    *pSize = HEADER_LENGTH + dataLength;
    
    return action;
}
int Ip6PollForPacketToSend(void* pPacket, int* pSize, char* pDstMac)
{    
    pData      = (char*)pPacket + HEADER_LENGTH;
    dataLength = 0;
    version    = 6;
    hoplimit   = 255;
    
    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) return DO_NOTHING;
    switch (action)
    {
        case UNICAST:
        case UNICAST_DNS:
        case UNICAST_DHCP:
            ArRev6(pDstIp, pDstMac);             //Make the remote MAC from NP
            break;
    }
        
    if (DEBUG) logHeader("IP6 polled packet sent");

    writeHeader((header*)pPacket);

    *pSize = HEADER_LENGTH + dataLength;
    
    return action;
}