Index: soft/giet_vm/memo/src/pseg.cpp
===================================================================
--- soft/giet_vm/memo/src/pseg.cpp	(revision 238)
+++ soft/giet_vm/memo/src/pseg.cpp	(revision 255)
@@ -35,6 +35,11 @@
 #include <iomanip>
 
+#include <cstdio>
+
 #include "pseg.h"
 #include "exception.h"
+
+
+#define max(x, y) (((x) > (y)) ? (x) : (y))
 
 
@@ -44,41 +49,35 @@
 
 //////////////////////////////////////
-const std::string & VSeg::name() const
-{
+const std::string & VSeg::name() const {
 	return m_name;
 }
 
 //////////////////////////////////////
-const std::string & VSeg::file() const
-{
+const std::string & VSeg::file() const {
 	return m_file;
 }
 
 ///////////////////////////
-uintptr_t VSeg::vma() const
-{
+uintptr_t VSeg::vma() const {
 	return m_vma;
 }
 
 /////////////////////////
-paddr_t VSeg::lma() const
-{
+paddr_t VSeg::lma() const {
 	return m_lma;
 }
 
 ///////////////////////////
-size_t VSeg::length() const
-{
+size_t VSeg::length() const {
 	return m_length;
 }
 
 /////////////////////////
-size_t VSeg::type() const
-{
+size_t VSeg::type() const {
 	return m_type;
 }
 
 /////////////////////////////////////////
-void VSeg::print( std::ostream &o ) const
+void VSeg::print(std::ostream &o) const
 {
 	o << std::hex << std::noshowbase 
@@ -89,21 +88,19 @@
       << m_lma << ", size: 0x"
       << std::setw (8) << std::setfill('0') 
-      << m_length  << ",ident: " 
-      << (m_ident ? "yes" : "no") << ", in(file): "
-      << m_file << ", name: " << m_name << ">";
+          << m_length  << ",ident: " 
+          << (m_ident ? "yes" : "no") << ", in(file): "
+          << m_file << ", name: " << m_name << ">";
 }
 
 /////////////
-VSeg::~VSeg()
-{
-}
+VSeg::~VSeg() {}
 
 /////////////////////////////////////////
-VSeg & VSeg::operator=( const VSeg &ref )
-{
-    if ( &ref == this )
+VSeg & VSeg::operator=(const VSeg & ref)
+{
+    if (&ref == this)
         return *this;
 
-    m_name = ref.m_name,
+    m_name = ref.m_name;
     m_file = ref.m_file;
     m_vma = ref.m_vma;
@@ -111,5 +108,5 @@
     m_length = ref.m_length;
     m_ident = ref.m_ident;
-	return *this;
+    return *this;
 }
 
@@ -117,9 +114,9 @@
 VSeg::VSeg()
     : m_name("No Name"),
-      m_file("Empty section"),
-      m_vma(0),
-      m_length(0),
-      m_loadable(false),
-      m_ident(0)
+    m_file("Empty section"),
+    m_vma(0),
+    m_length(0),
+    m_loadable(false),
+    m_ident(0)
 {
 }
@@ -127,26 +124,26 @@
 ////////////////////////////////////
 VSeg::VSeg(std::string&  binaryName, 
-           std::string&  name, 
-           uintptr_t     vma, 
-           size_t        length, 
-           bool          loadable, 
-           bool          ident)
-    : m_name(name),
-      m_file(binaryName),
-      m_vma(vma),
-      m_length(length),
-      m_loadable(loadable),
-      m_ident(ident)
+        std::string&  name, 
+        uintptr_t     vma, 
+        size_t        length, 
+        bool          loadable, 
+        bool          ident)
+: m_name(name),
+    m_file(binaryName),
+    m_vma(vma),
+    m_length(length),
+    m_loadable(loadable),
+    m_ident(ident)
 {
 }
 
 /////////////////////////////
-VSeg::VSeg( const VSeg &ref )
-    : m_name("To be copied"),
-      m_file("Empty"),
-      m_vma(0),
-      m_length(0),
-      m_loadable(false),
-      m_ident(0)
+VSeg::VSeg(const VSeg &ref):
+    m_name(ref.m_name),
+    m_file(ref.m_file),
+    m_vma(ref.m_vma),
+    m_length(ref.m_length),
+    m_loadable(ref.m_loadable),
+    m_ident(ref.m_ident)
 {
     (*this) = ref;
@@ -159,37 +156,21 @@
 
 /////////////////////////
-paddr_t PSeg::lma() const
-{
-	return m_lma;
+paddr_t PSeg::lma() const {
+    return m_lma;
 }
 
 ///////////////////////////
-paddr_t PSeg::limit() const
-{
-	return m_limit;
-}
-
-/////////////////////////////
-paddr_t PSeg::length() const
-{
-	return m_length;
+paddr_t PSeg::length() const {
+    return m_length;
 }
 
 /////////////////////////
-size_t PSeg::type() const
-{
-	return m_type;
-}
-
-/////////////////////////////
-paddr_t PSeg::nextLma() const
-{
-	return m_nextLma;
+size_t PSeg::type() const {
+    return m_type;
 }
 
 //////////////////////////////////////
-const std::string & PSeg::name() const
-{
-	return m_name;
+const std::string & PSeg::name() const {
+    return m_name;
 }
 
@@ -198,5 +179,5 @@
 {
 
-    if(this->m_type == PSEG_TYPE_PERI)
+    if (this->m_type == PSEG_TYPE_PERI)
         return;
 
@@ -204,25 +185,24 @@
     size_t    size = m_vsegs.size();
     paddr_t   used[size][2];          // lma, lma+length
-    size_t    i,j,error=0;
-    
-    for(it = m_vsegs.begin(), i= 0 ; it < m_vsegs.end() ; it++, i++)
+    size_t    i, j, error = 0;
+
+    for (it = m_vsegs.begin(), i = 0; it < m_vsegs.end(); it++, i++)
     {
         paddr_t it_limit = (*it).lma() + (*it).length();
-        for(j=0; j< i; j++)
+        for(j = 0; j < i; j++)
         {
-            if( used[j][0] == (*it).lma() ) //not the same lma ,
-            {
-                error = 1;
-                std::cout << "ok \n";
-            }
-            if( used[j][1] == it_limit )  // and not the same limit
+            if (used[j][0] == (*it).lma()) //not the same lma ,
+                {
+                    error = 1;
+                }
+            if (used[j][1] == it_limit)  // and not the same limit
             {
                 error = 2;
             }
-            if( (used[j][0] < (*it).lma()) and ((*it).lma() < used[j][1]) ) // lma  within
+            if ((used[j][0] < (*it).lma()) and ((*it).lma() < used[j][1])) // lma  within
             {
                 error = 3;
             }
-            if(  ((used[j][0] < it_limit) and (it_limit < used[j][1] )) ) // limit no within
+            if (((used[j][0] < it_limit) and (it_limit < used[j][1]))) // limit no within
             {
                 error = 4;
@@ -232,8 +212,8 @@
                 std::cout << "used[j][1]: " << std::hex << used[j][1] << std::endl;
             }
-            if(error)
+            if (error)
             {
                 std::ostringstream err;
-                err << " Error" << error << " ,ovelapping Buffers:" << std::endl 
+                err << " Error" << error << ", ovelapping Buffers:" << std::endl 
                     << *it << std::endl << m_vsegs[j] << std::endl; 
                 throw exception::RunTimeError( err.str().c_str() );
@@ -246,128 +226,138 @@
 }
 
-//////////////////////////////////////
-void PSeg::setName(std::string& name )
-{
-    m_name = name;
-}
 
 /////////////////////////////////////////////////////////
-paddr_t PSeg::align( paddr_t toAlign, unsigned alignPow2)
-{
+paddr_t PSeg::align(paddr_t toAlign, unsigned alignPow2) {
     return ((toAlign + (1 << alignPow2) - 1 ) >> alignPow2) << alignPow2; 
 }
 
 //////////////////////////////////////////
-paddr_t PSeg::pageAlign( paddr_t toAlign )
-{
+paddr_t PSeg::pageAlign(paddr_t toAlign) {
     size_t pgs = pageSize();
     size_t pageSizePow2 = __builtin_ctz(pgs);
-    
+
     return align(toAlign, pageSizePow2); 
 }
 
-////////////////////////////////
-void PSeg::setLma( paddr_t lma )
-{
-    m_lma = lma;
-    
-    m_nextLma = pageAlign(lma);
-
-    m_pageLimit = pageAlign(m_lma+m_length); 
-
-    m_limit = (m_lma + m_length);
-}
-
-/////////////////////////////////////
-void PSeg::setLength( paddr_t length )
-{
-    m_length = length;
-
-    m_pageLimit = pageAlign(m_lma+m_length); 
-
-    m_limit = (m_lma + m_length);
-}
+
 
 ////////////////////////////
-void PSeg::add( VSeg& vseg )
-{
-    vseg.m_lma = m_nextLma;
-//    incNextLma(vseg.length());   //for the next vseg
+void PSeg::add(VSeg& vseg) {
+    std::vector<VSeg>::iterator it;
+    int nb_elems = m_vsegs.size();
+    int i = 0;
+    bool mapped = false;
+    paddr_t prev_base = m_lma;
+    paddr_t prev_length = 0x0;
+    paddr_t curr_base = 0x0;
+    paddr_t curr_length = 0x0;
+    paddr_t next_base = 0x0;
+    paddr_t next_length = 0x0;
+
+    const int alignment = max(vseg.m_align, __builtin_ctz(pageSize())); // 12
+
+    if (vseg.length() == 0) {
+        std::cout << "*** Error: Adding a vseg of size 0 (base " << vseg.vma() << ")" << std::endl;
+        exit(1);
+    }
+
+    if (nb_elems == 0) {
+        if (vseg.length() <= m_length) {
+            vseg.m_lma = m_lma;
+            m_vsegs.push_back(vseg);
+            return;
+        }
+        else {
+            std::cout << "*** Error: Not enough space to map first VSeg (base = "
+                      << std::hex << vseg.vma() << " - Size = " << vseg.length() << ")" << std::endl;
+            std::cout << "    PSeg too small! (base = " << m_lma << " - size = "
+                      << m_length << ")" << std::endl;
+            exit(1);
+        }
+    }
+
+    curr_base = m_vsegs[0].lma(); // Initialisation avant recherche du min
+    curr_length = m_vsegs[0].length();
+    for (it = m_vsegs.begin(); it != m_vsegs.end(); it++) {
+        if ((*it).lma() < curr_base) {
+            curr_base = (*it).lma();
+            curr_length = (*it).length();
+        }
+    }
+
+    while (i < nb_elems) {
+        if (align(prev_base + prev_length, alignment) + vseg.length() <= curr_base) {
+            vseg.m_lma = align(prev_base + prev_length, alignment);
+            mapped = true;
+            break;
+        }
+        else if (i < nb_elems - 1) {
+            // Searching for the vseg already mapped with lowest paddr > curr_base
+            next_base = 0;
+            bool found = false;
+            for (it = m_vsegs.begin(); it != m_vsegs.end(); it++) {
+                if ((!found || (*it).lma() < next_base) && (*it).lma() > curr_base) {
+                    found = true;
+                    next_base = (*it).lma();
+                    next_length = (*it).length();
+                }
+            }
+            assert(found);
+
+            prev_base = curr_base;
+            prev_length = curr_length;
+
+            curr_base = next_base;
+            curr_length = next_length;
+        }
+        else {
+            if (align(curr_base + curr_length, alignment) + vseg.length() <= m_lma + m_length) {
+                vseg.m_lma = align(curr_base + curr_length, alignment);
+                mapped = true;
+            }
+        }
+        i++;
+    }
+
+    if (!mapped) {
+        std::cout << "*** Error: Not enough space to map VSeg (base = " << std::hex << vseg.vma() << " - Size = " << vseg.length() << ")" << std::endl;
+        exit(1);
+    }
+
     m_vsegs.push_back(vseg); 
 }
 
+
 /////////////////////////////////
-void PSeg::addIdent( VSeg& vseg )
-{
+void PSeg::addIdent(VSeg& vseg) {
+    std::vector<VSeg>::iterator it;
+
+    for (it = m_vsegs.begin(); it != m_vsegs.end(); it++) {
+        if ((vseg.vma() == (*it).lma()) ||
+            ((vseg.vma() < (*it).lma()) && (vseg.vma() + vseg.length() > (*it).lma())) ||
+            ((vseg.vma() > (*it).lma()) && ((*it).lma() + (*it).length() > vseg.vma()))) {
+            std::cout << "*** Error: Identity VSeg overlaps another segment:" << std::endl
+                      << "Added Segment Base : " << std::hex << vseg.vma()
+                      << " - Size : " << vseg.length() << std::endl;
+            std::cout << "Existing Segment Base : " << (*it).lma()
+                      << " - size : " << (*it).length() << std::endl;
+            exit(1);
+        }
+    }
+
     vseg.m_lma = vseg.m_vma;
     m_vsegs.push_back(vseg); 
 }
 
+
 /////////////////////////////////////////
-void PSeg::setNextLma( paddr_t nextLma)
-{
-    m_nextLma = nextLma;
-    confNextLma();
-}
-
-//////////////////////////////////
-void PSeg::incNextLma( size_t inc)
-{
-    m_nextLma += inc;
-    confNextLma();
-}
-
-////////////////////////
-void PSeg::confNextLma()
-{
-    if(m_nextLma > m_limit)
-    {
-        std::cerr << "Erreur pseg overflow... nextLma: "
-                  << std::hex << m_nextLma << ", limit: " 
-                  << m_limit << std::endl;
-        exit(1); 
-    }
-
-    m_nextLma = pageAlign( m_nextLma );
-
-    if(m_nextLma > m_pageLimit)
-    {
-        std::cerr << "Erreur pseg page overflow... nextLma: "
-                  << std::hex << m_nextLma << ", limit: " 
-                  << m_pageLimit << std::endl;
-        exit(1); 
-    }
-}
-
-/////////////////////////////////
-void PSeg::setPageSize(size_t pg)
-{
-    if( pg == 0)
-    {
-        std::cerr << "PageSize must be positive" << std::endl;
-        return;
-    }
-    pageSize() = pg;
-}
-
-////////////////////////
-size_t& PSeg::pageSize()
-{
-    static size_t m_pageSize;
-    return m_pageSize;
-}
-
-/////////////////////////////////////////
-PSeg & PSeg::operator=( const PSeg &ref )
-{
-    if ( &ref == this )
+PSeg & PSeg::operator=(const PSeg &ref) {
+    if (&ref == this) {
         return *this;
+    }
 
     m_name = ref.m_name;
     m_length = ref.m_length;
-    m_limit = ref.m_limit;
-    m_pageLimit = ref.m_pageLimit;
     m_lma = ref.m_lma;
-    m_nextLma = ref.m_nextLma;
     m_vsegs = ref.m_vsegs;
     m_type = ref.m_type;
@@ -377,12 +367,11 @@
 
 //////////////////////////////////////////
-void PSeg::print( std::ostream &o ) const
+void PSeg::print(std::ostream &o) const
 {
 	o << "<Physical segment "
 	  << std::showbase << m_name
 	  << ", from: " << std::hex 
-      << m_lma << " to " << m_limit 
+      << m_lma
       << ", size : "  << m_length 
-      << ", filled to: "  << m_nextLma 
       << ", type : "  << m_type 
       << ", containing: "<< std::endl;
@@ -403,54 +392,27 @@
     m_length = length;
     m_type = type;
-
-    setLma(lma);
+    m_lma = lma;
 }
 
 ////////////////////////////////////
-PSeg::PSeg( const std::string &name):
+PSeg::PSeg(const std::string &name):
       m_lma(0),
-      m_length(0),
-      m_nextLma(0),
-      m_limit(0)
-{
+      m_length(0) {
     m_name = name;
 }
 
-////////////////////////
-PSeg::PSeg( paddr_t lma):
-      m_name("No name"),
-      m_lma(0),
-      m_length(0),
-      m_nextLma(0),
-      m_limit(0)
-{
-    setLma(lma);
-}
-
-////////////
-PSeg::PSeg()
-    :
-      m_name("Empty section"),
-      m_lma(0),
-      m_length(0),
-      m_nextLma(0),
-      m_limit(0)
-{
-}
-
-/////////////////////////////
-PSeg::PSeg( const PSeg &ref )
-    : m_name("To be copied"),
-      m_lma(0),
-      m_length(0),
-      m_nextLma(0),
-      m_limit(0)
-{
-    (*this) = ref;
-}
-
-PSeg::~PSeg()
-{
-}
+
+////////////////////////////////////
+PSeg::PSeg(const PSeg& pseg) {
+    m_name = pseg.m_name;
+    m_length = pseg.m_length;
+    m_lma = pseg.m_lma;
+    m_vsegs = pseg.m_vsegs;
+    m_type = pseg.m_type;
+
+    (*this) = pseg;
+}
+
+PSeg::~PSeg() {}
 
 
