File content as of revision 48:952dddb74b8b:

#include     "mbed.h"
#include      "log.h"
#include      "net.h"
#include   "action.h"
#include    "icmp6.h"
#include  "udptcp6.h"
#include      "ar6.h"
#include       "nr.h"
#include    "slaac.h"
#include      "eth.h"
#include       "ip.h"
#include      "ip6.h"
#include      "ndp.h"
#include       "io.h"
#include      "ntp.h"
#include      "mac.h"
#include "http-reply.h"

bool Ip6Trace = true;

void Ip6Clear(char* ip)
{
    *ip = 0;
}
bool Ip6IsEmpty(char* ip)
{
    return !*ip; //Just check for the first byte being non zero
}
static void addHexNibble(bool* pAdded, int number, int index, char** pp)
{
    int nibble = number;
    if (index) nibble >>= 4;
    nibble &= 0xF;
    
    if (nibble || *pAdded)
    {
        **pp = nibble < 10 ? nibble + '0' : nibble - 10 + 'a';
        *pp += 1;
        *pAdded = true;
    }
}
int Ip6AddressToString(char* pIp, int size, char* pText)
{
    char* pIpE = pIp + 16;
    char* p = pText;
    while (true)
    {
        bool added = false;
        if (*pIp || *(pIp + 1))
        {
            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 0), 1, &p);
            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 0), 0, &p);
            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 1), 1, &p);
            if (p > pText + size - 2) break;  addHexNibble(&added, *(pIp + 1), 0, &p);
        }
        
        pIp += 2;
        if (pIp >= pIpE) break;
        
        if (p > pText + size - 2) break; *p++ = ':';
    }
    *p = 0;
    return p - pText;
}
static void logHexNibble(bool* pAdded, int number, int index)
{
    int nibble = number;
    if (index) nibble >>= 4;
    nibble &= 0xF;
    
    if (nibble || *pAdded)
    {
        LogPush(nibble < 10 ? nibble + '0' : nibble - 10 + 'a');
        *pAdded = true;
    }
}
int Ip6AddressLog(char* pIp)
{
    int count = 0;
    char* pIpE = pIp + 16;
    while (true)
    {
        bool added = false;
        if (*pIp || *(pIp + 1))
        {
            logHexNibble(&added, *(pIp + 0), 1); if (added) count++;
            logHexNibble(&added, *(pIp + 0), 0); if (added) count++;
            logHexNibble(&added, *(pIp + 1), 1); if (added) count++;
            logHexNibble(&added, *(pIp + 1), 0); if (added) count++;
        }
        
        pIp += 2;
        if (pIp >= pIpE) break;
        
        LogPush(':'); count++;
    }
    return count;
}
static void httpHexNibble(bool* pAdded, int number, int index)
{
    int nibble = number;
    if (index) nibble >>= 4;
    nibble &= 0xF;
    
    if (nibble || *pAdded)
    {
        HttpReplyAddChar(nibble < 10 ? nibble + '0' : nibble - 10 + 'a');
        *pAdded = true;
    }
}
int Ip6AddressHttp(char* pIp)
{
    int count = 0;
    char* pIpE = pIp + 16;
    while (true)
    {
        bool added = false;
        if (*pIp || *(pIp + 1))
        {
            httpHexNibble(&added, *(pIp + 0), 1); if (added) count++;
            httpHexNibble(&added, *(pIp + 0), 0); if (added) count++;
            httpHexNibble(&added, *(pIp + 1), 1); if (added) count++;
            httpHexNibble(&added, *(pIp + 1), 0); if (added) count++;
        }
        
        pIp += 2;
        if (pIp >= pIpE) break;
        
        HttpReplyAddChar(':'); count++;
    }
    return count;
}
bool Ip6IsSame(char* ipA, char* ipB)
{
    return memcmp(ipA, ipB, 16) == 0;
}
void Ip6Copy(char* ipTo, char* ipFrom)
{
    memcpy(ipTo, ipFrom, 16);
}

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 Ip6DstIpFromDest(int dest, char* pDstIp)
{
    switch (dest)
    {
        case   UNICAST:                                        break;
        case   UNICAST_DNS:    Ip6Copy(pDstIp, NdpDnsServer ); break;
        case   UNICAST_NTP:    Ip6Copy(pDstIp, NtpServerIp6 ); break;
        case MULTICAST_NODE:   Ip6Copy(pDstIp, Ip6AllNodes  ); break;
        case MULTICAST_ROUTER: Ip6Copy(pDstIp, Ip6AllRouters); break;
        case MULTICAST_MDNS:   Ip6Copy(pDstIp, Ip6Mdns      ); break;
        case MULTICAST_LLMNR:  Ip6Copy(pDstIp, Ip6Llmnr     ); break;
        default:
            LogTimeF("Ip6DestIpFromDest unknown destination %d\r\n", dest);
            break;           
    }
}
void Ip6SrcIpFromScope(int scope, char* pSrcIp)
{
    if (scope == SCOPE_GLOBAL) Ip6Copy(pSrcIp, SlaacGlobalIp   );
    else                       Ip6Copy(pSrcIp, SlaacLinkLocalIp);
    //Note that scope could be SCOPE_NONE if source was multicast in which case should return the link local ip.
}

#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      srcIp[16];
static char      dstIp[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;
              Ip6Copy(srcIp, pHeader->src);
              Ip6Copy(dstIp, pHeader->dst);
    pData      = (char*)pHeader + HEADER_LENGTH;
}
static void writeHeader(struct header * pHeader)
{
            pHeader->versionTrafficFlow  = version << 4;
            pHeader->protocol            = protocol;
            pHeader->hoplimit            = hoplimit;
    Ip6Copy(pHeader->dst, dstIp);
    Ip6Copy(pHeader->src, srcIp);
            pHeader->dataLength          = NetToHost16(dataLength);
}

static void logHeader()
{
    if (NetTraceVerbose)
    {
        Log("IP6 header\r\n");
        LogF("  Version           %d\r\n", version);
        LogF("  Payload length    %d\r\n", dataLength);
        LogF("  Hop limit         %d\r\n", hoplimit);
        LogF("  Protocol          "); IpProtocolLog(protocol); Log("\r\n");
        Log ("  Source IP         "); Ip6AddressLog(srcIp);    Log("\r\n");
        Log ("  Destination IP    "); Ip6AddressLog(dstIp);    Log("\r\n");
    }
    else
    {
        Log("IP6   header ");
        IpProtocolLog(protocol);
        Log(" ");
        Ip6AddressLog(srcIp);
        Log(" >>> ");
        Ip6AddressLog(dstIp);
        Log("\r\n");
    }
}

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 getIsSameGroup(char* pA, char* pB)
{
    pA += 13;
    pB += 13;
    if (*pA++ != *pB++) return false;
    if (*pA++ != *pB++) return false;
    return *pA == *pB;
}
static void (*pTraceBack)(void);
static void trace()
{
    pTraceBack();
    logHeader();
}
int Ip6HandleReceivedPacket(void (*traceback)(void), char* pSrcMac, void* pPacket, int* pSize, char* pDstMac)
{
    pTraceBack = traceback;
    
    struct header * pHeader = (header*)pPacket;
    readHeader(pHeader);
    
    int  scope       = SlaacScope(dstIp);
    bool isMulticast = dstIp[0] == 0xFF;
    bool isSolicited = getIsSolicited(dstIp);
    bool isGroup     = getIsSameGroup(dstIp, SlaacLinkLocalIp);
    
    bool doIt = scope || (isMulticast && !isSolicited) || (isGroup && isSolicited);
    
    if (!doIt)
    {
        if (Ip6Trace)
        {
            LogTime("IP6 filtered out ip ");
            Ip6AddressLog(dstIp);
            LogF(" from ");
            Ip6AddressLog(srcIp);
            Log("\r\n");
        }
        return DO_NOTHING;
    }
    
    Ar6AddIpRecord(trace, pSrcMac, srcIp);
    NrMakeRequestForNameFromIp6(srcIp);

    int action = DO_NOTHING;
    switch (protocol)
    {
        case HOPOPT: action = DO_NOTHING;                                                                break;
        case ICMP6:  action = Icmp6HandleReceivedPacket(trace, scope, srcIp, dstIp, &dataLength, pData); break;
        case UDP:    action =  Udp6HandleReceivedPacket(trace, scope, srcIp, dstIp, &dataLength, pData); break;
        case TCP:    action =  Tcp6HandleReceivedPacket(trace, scope, srcIp, dstIp, &dataLength, pData); break;        
        default:
            LogTimeF("IP6 protocol %d unhandled\r\n", protocol);
            return DO_NOTHING;
    }
    if (!action) return DO_NOTHING;
    
    if (NdpIpNeedsToBeRouted(dstIp)) MacCopy(pDstMac, NdpRouterMac); //Send to the router MAC
    else                             MacCopy(pDstMac,      pSrcMac); //Send back to the source

    writeHeader(pHeader);
      
    *pSize = HEADER_LENGTH + dataLength;
    
    if (ActionGetTracePart(action)) logHeader();

    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)
    {
        action = Icmp6PollForPacketToSend(pData, &dataLength, srcIp, dstIp);
        protocol = ICMP6;
    }
    
    if (!action)
    {
        action = Udp6PollForPacketToSend(pData, &dataLength, srcIp, dstIp);
        protocol = UDP;
    }
    if (!action) return DO_NOTHING;
    
    int dest = ActionGetDestPart(action);
    switch (dest)
    {
        case UNICAST:
        case UNICAST_DNS:
        case UNICAST_DHCP:
        case UNICAST_NTP:
            if (NdpIpNeedsToBeRouted(dstIp)) MacCopy   (pDstMac, NdpRouterMac); //Send to the router MAC
            else                             Ar6IpToMac(dstIp,        pDstMac); //Make the remote MAC from NP
            break;
        case MULTICAST_NODE:
        case MULTICAST_ROUTER:
        case MULTICAST_MDNS:
        case MULTICAST_LLMNR:
        case SOLICITED_NODE:
            break;
        default:
            LogTimeF("Ip6PollForPacketToSend - undefined destination %d\r\n", dest);
            break;
    }

    writeHeader((header*)pPacket);

    *pSize = HEADER_LENGTH + dataLength;
    
    if (ActionGetTracePart(action)) logHeader();

    return action;
}