From 68b9412776d11cdc3f8d5987409df27c38a0b115 Mon Sep 17 00:00:00 2001
From: Laurent Deru <laurent.deru@cetic.be>
Date: Mon, 2 Sep 2013 16:19:08 +0200
Subject: [PATCH] Drop packet if there are not enough free buffers to perform
 fragmentation

---
 core/lib/memb.c            | 13 +++++++++++++
 core/lib/memb.h            |  1 +
 core/net/ipv6/sicslowpan.c |  7 +++++++
 core/net/queuebuf.c        | 10 ++++++++++
 core/net/queuebuf.h        |  4 +++-
 5 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/core/lib/memb.c b/core/lib/memb.c
index f07414435..b4056dd9f 100644
--- a/core/lib/memb.c
+++ b/core/lib/memb.c
@@ -107,5 +107,18 @@ memb_inmemb(struct memb *m, void *ptr)
     (char *)ptr < (char *)m->mem + (m->num * m->size);
 }
 /*---------------------------------------------------------------------------*/
+int
+memb_numfree(struct memb *m)
+{
+  int i;
+  int num_free = 0;
 
+  for(i = 0; i < m->num; ++i) {
+    if(m->count[i] == 0) {
+      ++num_free;
+    }
+  }
+
+  return num_free;
+}
 /** @} */
diff --git a/core/lib/memb.h b/core/lib/memb.h
index f5e654cc1..d66e5a419 100644
--- a/core/lib/memb.h
+++ b/core/lib/memb.h
@@ -130,6 +130,7 @@ char  memb_free(struct memb *m, void *ptr);
 
 int memb_inmemb(struct memb *m, void *ptr);
 
+int  memb_numfree(struct memb *m);
 
 /** @} */
 /** @} */
diff --git a/core/net/ipv6/sicslowpan.c b/core/net/ipv6/sicslowpan.c
index e69bdd88b..8e139d3ce 100644
--- a/core/net/ipv6/sicslowpan.c
+++ b/core/net/ipv6/sicslowpan.c
@@ -1454,6 +1454,13 @@ output(const uip_lladdr_t *localdest)
      * IPv6/HC1/HC06/HC_UDP dispatchs/headers.
      * The following fragments contain only the fragn dispatch.
      */
+    int estimated_fragments = ((int)uip_len) / ((int)MAC_MAX_PAYLOAD - SICSLOWPAN_FRAGN_HDR_LEN) + 1;
+    int freebuf = queuebuf_numfree() - 1;
+    PRINTFO("uip_len: %d, fragments: %d, free bufs: %d\n", uip_len, estimated_fragments, freebuf);
+    if(freebuf < estimated_fragments) {
+      PRINTFO("Dropping packet, not enough free bufs\n");
+      return 0;
+    }
 
     PRINTFO("Fragmentation sending packet len %d\n", uip_len);
 
diff --git a/core/net/queuebuf.c b/core/net/queuebuf.c
index 8cb8708b3..ca49b68fc 100644
--- a/core/net/queuebuf.c
+++ b/core/net/queuebuf.c
@@ -307,6 +307,16 @@ queuebuf_init(void)
 #endif /* QUEUEBUF_STATS */
 }
 /*---------------------------------------------------------------------------*/
+int
+queuebuf_numfree(void)
+{
+  if(packetbuf_is_reference()) {
+    return memb_numfree(&refbufmem);
+  } else {
+    return memb_numfree(&bufmem);
+  }
+}
+/*---------------------------------------------------------------------------*/
 #if QUEUEBUF_DEBUG
 struct queuebuf *
 queuebuf_new_from_packetbuf_debug(const char *file, int line)
diff --git a/core/net/queuebuf.h b/core/net/queuebuf.h
index a1727342d..cfa33a34b 100644
--- a/core/net/queuebuf.h
+++ b/core/net/queuebuf.h
@@ -108,7 +108,9 @@ packetbuf_attr_t queuebuf_attr(struct queuebuf *b, uint8_t type);
 
 void queuebuf_debug_print(void);
 
-#endif /* QUEUEBUF_H_ */
+int queuebuf_numfree(void);
+
+#endif /* __QUEUEBUF_H__ */
 
 /** @} */
 /** @} */