/*
  SFBHeap.cpp - The ancient K&R malloc, tweaked for packets
  Copyright (C) 2009 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA

  $Id$
*/

#include "SFBHeap.h"
#include "SFBAssert.h"

u32 SFBHeap::headers(u32 packetBytes) {
  return
    (packetBytes+sizeof(HeapHeader)-1)/sizeof(HeapHeader)+1;
}

void * SFBHeap::initUsed(HeapHeader * h, u32 words) {
  h->f.size = words;
  h->f.checkBytes = USED_CHECK_BYTES;
  h->f.s.ph.f[PacketHeader::SOURCE] = WMEM;      // Default to 'working memory' source unless overridden
  h->f.s.ph.f[PacketHeader::FLAGS] = 0;
  h->f.s.ph.f[PacketHeader::CURSOR] = 0;
  h->f.s.ph.f[PacketHeader::LENGTH] = 0;
  return (void*) (h+1);
}

bool SFBHeap::putPacketByte(u8 * packet, const u8 byte) {
  HeapHeader * h = ((HeapHeader*) packet)-1;
  if (packetLength(packet) >=  dataSize(h)) {
    h->f.s.ph.f[PacketHeader::FLAGS] |= PK_BUFFER;            
    return false;
  }
  packet[h->f.s.ph.f[PacketHeader::LENGTH]++] = byte;
  return true;
}

void SFBHeap::terminatePacket(u8 * packet) {
  HeapHeader * h = ((HeapHeader*) packet)-1;
  packet[h->f.s.ph.f[PacketHeader::LENGTH]] = 0;
}


#ifdef TEST_SFBHEAP
#include <stdlib.h> /* for exit, random */
void _apiError_(u32 code,const char * file, int lineno) { exit(code); }
#include <stdio.h>
#include <stdlib.h>

int die(const char * message) {
  printf("%s\n",message);
  exit(1);
}
#define ASSERT(condition) ((condition)?0:die(#condition))
#endif /* TEST_SFBHEAP */

u8 * SFBHeap::makePacket(u32 size) {
  return (u8*) SFBHeap::malloc(size);
}

u32 SFBHeap::dataSize(HeapHeader * h) {
  return (h->f.size-1)*sizeof(HeapHeader);
}

SFBHeap::SFBHeap() {
  HeapHeader * h = asHeader(0);

  base.f.size = 0;                // An empty free block, never to be given out
  base.f.checkBytes = FREE_CHECK_BYTES;
  base.f.s.nextFree = h;
  freePtr = &base;

  h->f.size = HEAP_SIZE_HEADERS;
  h->f.checkBytes = FREE_CHECK_BYTES;
  h->f.s.nextFree = &base;
}

void * SFBHeap::malloc(u32 sizeWanted) {
  u32 nunits = headers(sizeWanted);
  HeapHeader *prevp = freePtr;
  
  for (HeapHeader * p = prevp->f.s.nextFree; ; prevp = p, p = p->f.s.nextFree) {
    if (p->f.size >= nunits) {
      if (p->f.size == nunits) 
        prevp->f.s.nextFree = p->f.s.nextFree;
      else {
        p->f.size -= nunits;
        p += p->f.size;
        p->f.size = nunits;
      }
      freePtr = prevp;
      return initUsed(p,nunits);
    }
    if (p == freePtr) // Wrapped, no win :(
      return 0;
  }
}

void SFBHeap::free(void * toFree) {
  if (!toFree) return;

  HeapHeader * bp = ((HeapHeader*) toFree)-1;
  API_ASSERT(bp->f.checkBytes == USED_CHECK_BYTES,E_API_BAD_FREE_ARG);

  bp->f.checkBytes = FREE_CHECK_BYTES;

  HeapHeader * p;
  for (p = freePtr; !(bp > p && bp < p->f.s.nextFree); p = p->f.s.nextFree)
    if (p >= p->f.s.nextFree && (bp > p || bp < p->f.s.nextFree))
      break;

  if (bp+bp->f.size == p->f.s.nextFree) {  // Merge with next
    bp->f.size += p->f.s.nextFree->f.size; 
    bp->f.s.nextFree = p->f.s.nextFree->f.s.nextFree;
  } else 
    bp->f.s.nextFree = p->f.s.nextFree;    // Link in

  if (p + p->f.size == bp) {            // Merge with prev
    p->f.size += bp->f.size;
    p->f.s.nextFree = bp->f.s.nextFree;
  } else
    p->f.s.nextFree = bp;

  freePtr = p;                  // Keep freePtr valid, start here
}

u8 * SFBHeap::remember(const u8 * packet,bool unreadOnly, u8 sourceOverride) {
  if (!packet) return 0;
  API_ASSERT_VALID_PACKET(packet);
  u32 start = unreadOnly?packetCursor(packet):0;
  u32 size = packetLength(packet)-start;
  u8 * ret = (u8*) SFBHeap::malloc(size+1);
  if (!ret)
    return 0;

  HeapHeader * h = ((HeapHeader*) ret)-1;
  h->f.s.ph.f[PacketHeader::SOURCE] = 
    (sourceOverride < MAX_FACE_INDEX)? sourceOverride : packetSource(packet);
    
  for (u32 i = 0; i < size; ++i) {
    putPacketByte(ret,packet[i+start]);
  }
  terminatePacket(ret);
  return ret;
}

u8 * SFBHeap::rememberBytes(const u8 * bytes, u32 size) {
  if (!bytes) return 0;
  u8 * ret = (u8*) SFBHeap::malloc(size+1);
  if (!ret)
    return 0;

  for (u32 i = 0; i < size; ++i) {
    putPacketByte(ret,bytes[i]);
  }
  terminatePacket(ret);
  return ret;
}

void SFBHeap::forget(u8 * packet) {
  if (!packet) return;
  API_ASSERT_VALID_PACKET(packet);
  SFBHeap::free(packet);
}

#ifdef TEST_SFBHEAP

/* Compilation for testing, e.g.:
g++ -DTEST_SFBHEAP -O0 -g3 -Wall -pedantic -Werror -Wundef    -I../../../src/include  -DHOST_MODE -MMD -MP -MF"../../../targets/host/build/SFBHeap.d" -MT"../../../targets/host/build/SFBHeap.d" -o"./sfbheaptest" "SFBHeap.cpp" SFBPacketIO.cpp SFBReactor.cpp;valgrind ./sfbheaptest

 */

class TestSFBHeap {
  static void printHeap(SFBHeap * h) {
    int i;
    printf("base %p s=%3d c=0x%04x %p freePtr=%p\n",(void*)&h->base,h->base.f.size,h->base.f.checkBytes,(void*) h->base.f.s.nextFree,(void*)h->freePtr);
    for (i = 0; i < SFBHeap::HEAP_SIZE_HEADERS; i += h->asHeader(i)->f.size) {
      SFBHeap::HeapHeader * hh = h->asHeader(i);
      printf("%4d %p s=%3d c=0x%04x %p\n",i,(void*)hh,hh->f.size,hh->f.checkBytes,(void*) hh->f.s.nextFree);
    }
  }

  void heapcheck(SFBHeap & h) {
    ASSERT(h.freePtr);
    ASSERT(h.freePtr->f.checkBytes==SFBHeap::FREE_CHECK_BYTES);

    for (int i = 0; i < SFBHeap::HEAP_SIZE_HEADERS; i += h.asHeader(i)->f.size) {
      SFBHeap::HeapHeader * hh = h.asHeader(i);
      ASSERT(hh->f.size <= SFBHeap::HEAP_SIZE_HEADERS-i);
      ASSERT(hh->f.checkBytes==SFBHeap::FREE_CHECK_BYTES || hh->f.checkBytes==SFBHeap::USED_CHECK_BYTES);
    }
    bool foundFreePtr = h.freePtr==&h.base;
    SFBHeap::HeapHeader * l = 0, * f;
    for (f = h.base.f.s.nextFree; f != &h.base; l = f, f = f->f.s.nextFree) {
      if (f==h.freePtr)
        foundFreePtr = true;
      ASSERT(f > l);
      ASSERT(f->f.checkBytes==SFBHeap::FREE_CHECK_BYTES);
      ASSERT(f->f.size <= SFBHeap::HEAP_SIZE_HEADERS); 
    }
    ASSERT(foundFreePtr);
  }

  void checkEmpty(SFBHeap &h) {  // Ensure nothing there and all coalesced
    ASSERT(h.base.f.s.nextFree);
    ASSERT(h.base.f.size==0);
    ASSERT(h.base.f.s.nextFree->f.size==SFBHeap::HEAP_SIZE_HEADERS);
    ASSERT(h.base.f.s.nextFree->f.s.nextFree==&h.base);

  }
  void testAlignment() {
    ASSERT(SFBHeap::headers(0)==1);
    ASSERT(SFBHeap::headers(1)==2);
    ASSERT(SFBHeap::headers(2)==2);
    ASSERT(SFBHeap::headers(3)==2);
    ASSERT(SFBHeap::headers(4)==2);
    ASSERT(SFBHeap::headers(5)==2);
    ASSERT(SFBHeap::headers(6)==2);
    ASSERT(SFBHeap::headers(7)==2);
    ASSERT(SFBHeap::headers(8)==2);
    ASSERT(SFBHeap::headers(9)==3);
    ASSERT(SFBHeap::headers(21)==4);
  }

  void testOne(SFBHeap &h) {
    heapcheck(h);
    checkEmpty(h);
    printHeap(&h);
    u8 * ret = (u8*) h.malloc(0);
    heapcheck(h);
    printHeap(&h);
    ASSERT(ret!=0);
    ASSERT(packetLength(ret)==0);
    ASSERT(packetFlags(ret)==0);
    h.free(ret);
    heapcheck(h);
    checkEmpty(h);
    printHeap(&h);
  }

  void testTwo(SFBHeap &h) {
    heapcheck(h);
    u8 * ptrs[100];
    int inuse = 0;
    while ((ptrs[inuse] = (u8*) h.malloc(inuse))) {
      ++inuse;
      heapcheck(h);
    }
    int i = 0;
    while (i < inuse) {
      if (i&1) 
        h.free(ptrs[i--]);
      else
        h.free(ptrs[--inuse]);
      heapcheck(h);
    }
    heapcheck(h);
    checkEmpty(h);
  }

  void testRandoms(SFBHeap &h) {
    srandom(1);
    heapcheck(h);
    const int max_ptrs = 100;
    u8 * ptrs[max_ptrs];
    int inuse = 0;
    int fillOdds = 50;
    for (int i = 0; i < 1000000; ++i) {
      //printf("i=%3d\n",i);
      //printHeap(&h);
      if (inuse==0) {
        checkEmpty(h);
        ++fillOdds;
      } else if (inuse >= max_ptrs) {
        --fillOdds;
      }
      if (inuse < max_ptrs && random()%100 < fillOdds) {
        int len = random()%(random()%MAX_PACKET_LENGTH+2);
        u8 * ret = (u8*) h.malloc(len);
        if (ret != 0) {
          ptrs[inuse++] = ret;
          //printf("malloc %p=%d\n",(void*)(ret-8),len);
        }
      } else if (inuse > 0 && random()%100 >= fillOdds) {
        int idx = random()%inuse;
        //printf("free %p\n",(void*)(ptrs[idx]-8));
        h.free(ptrs[idx]);
        ptrs[idx] = ptrs[--inuse];
      }
      heapcheck(h);
    }
  }

  SFBHeap & heapToTest;

public:
  TestSFBHeap(SFBHeap & h) : heapToTest(h) { }

  void runTests() {
    testAlignment();
    testOne(heapToTest);
    testTwo(heapToTest);
    testRandoms(heapToTest);
  }
};

int main() {
  SFBHeap heap;
  TestSFBHeap test(heap);
  test.runTests();
  return 0;
}
#endif
