Fix round robin frame selector. Add l16 and jpeg profiles. Add timestamp to lrtp_enqueue_frame.

27 files changed, 2351 insertions, 98 deletions

diff --git a/msvc/liblrtp.vcxproj b/msvc/liblrtp.vcxproj
index 6e6b20f..7e7e78b 100644
--- a/msvc/liblrtp.vcxproj
+++ b/msvc/liblrtp.vcxproj
@@ -101,6 +101,8 @@
     <ClCompile Include="..\src\rtp_profile_amrwb.cc" />

     <ClCompile Include="..\src\rtp_profile_opus.cc" />

     <ClCompile Include="..\src\rtp_profile_raw.cc" />

+    <ClCompile Include="..\src\rtp_profile_jpeg.cc" />

+    <ClCompile Include="..\src\rtp_profile_l16.cc" />

     <ClCompile Include="..\src\srtp.cc" />

   </ItemGroup>

   <ItemGroup>

diff --git a/src/Makefile.am b/src/Makefile.am
index 15a201f..feafb93 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -10,7 +10,9 @@ liblrtp_la_SOURCES = \
 	srtp.cc \
 	rtp_profile_amrwb.cc \
 	rtp_profile_opus.cc \
+	rtp_profile_l16.cc \
 	rtp_profile_raw.cc \
+	rtp_profile_jpeg.cc \
 	asc2bin.cc
 
 include_HEADERS = \
@@ -20,7 +22,9 @@ include_HEADERS = \
 EXTRA_DIST = \
 	rtp_profile_amrwb.h \
 	rtp_profile_opus.h \
+	rtp_profile_l16.h \
 	rtp_profile_raw.h \
+	rtp_profile_jpeg.h \
 	rtp.h \
 	srtp.h \
 	asc2bin.h
diff --git a/src/lrtp.cc b/src/lrtp.cc
index da02083..69b9efe 100644
--- a/src/lrtp.cc
+++ b/src/lrtp.cc
@@ -36,6 +36,8 @@
 #include "rtp_profile_amrwb.h"
 #include "rtp_profile_opus.h"
 #include "rtp_profile_raw.h"
+#include "rtp_profile_l16.h"
+#include "rtp_profile_jpeg.h"
 
 #include "srtp.h"
 
@@ -68,6 +70,8 @@ static const profile_class_t registered_profiles[] = {
   { PROFILE_AMRWB, profile_amrwb_create },
   { PROFILE_OPUS, profile_opus_create },
   { PROFILE_RAW, profile_raw_create },
+  { PROFILE_L16, profile_raw_create },
+  { PROFILE_JPEG, profile_jpeg_create },
   { }
 };
 
@@ -91,16 +95,16 @@ void lrtp_close(struct lrtp_t *lrtp)
 }
 
 EXPORT
-struct lrtp_profile_t *lrtp_create_profile(struct lrtp_t *lrtp,
-                                           lrtp_profile_id_t profile_id,
-                                           unsigned int csrc, ...)
+int lrtp_create_profile(struct lrtp_t *lrtp,
+                        lrtp_profile_id_t profile_id,
+                        unsigned int csrc, ...)
 {
   struct lrtp_profile_t *profile = NULL;
 
   if(lrtp->profiles.find(csrc) != lrtp->profiles.end()) {
     // TODO: CSRC already active
     printf("ERROR: CSRC already active\n");
-    return NULL;
+    return 1;
   }
 
   va_list ap;
@@ -118,15 +122,18 @@ struct lrtp_profile_t *lrtp_create_profile(struct lrtp_t *lrtp,
 
   va_end(ap);
 
-  if(profile) {
-    profile->id = PROFILE_RAW;
-    profile->lrtp = lrtp;
-    profile->csrc = csrc;
-
-    lrtp->profiles[csrc] = profile;
+  if(!profile) {
+    printf("ERROR: Could not find profile [%d]\n", profile_id);
+    return 1;
   }
 
-  return profile;
+  profile->id = profile_id;
+  profile->lrtp = lrtp;
+  profile->csrc = csrc;
+
+  lrtp->profiles[csrc] = profile;
+  
+  return 0;
 }
 
 EXPORT
@@ -144,13 +151,23 @@ void lrtp_destroy_profile(struct lrtp_t *lrtp, unsigned int csrc)
 }
 
 EXPORT
-int lrtp_enqueue_frame(struct lrtp_profile_t *profile,
-                       const char *data, size_t size)
+int lrtp_enqueue_frame(struct lrtp_t *lrtp, unsigned int csrc,
+                       const char *data, size_t size,
+                       unsigned long int timestamp)
 {
+  if(lrtp->profiles.find(csrc) == lrtp->profiles.end()) {
+    // TODO: CSRC not found
+    printf("ERROR: CSRC not found\n");
+    return 1;
+  }
+
+  struct lrtp_profile_t *profile = lrtp->profiles[csrc];
+
   inputframe_t *frame = new inputframe_t();
   frame->data = data;
   frame->size = size;
   frame->offset = 0;
+  frame->timestamp = timestamp;
 
   //printf("lrtp_enqueue_frame: frame->size: %d\n", frame->size);
 
@@ -159,31 +176,46 @@ int lrtp_enqueue_frame(struct lrtp_profile_t *profile,
   return 0;
 }
 
-static lrtp_profile_t *get_next_profile(struct lrtp_t *lrtp)
+// Assume we have at least one csrc in the list
+static csrc_t get_next_csrc(struct lrtp_t *lrtp)
 {
-  // TODO: This function /should/ return the next profile containing frame data,
-  // not just the next profile in the list (regardless of it's framequeue being
-  // empty!).
-
-  if(lrtp->profiles.size() == 0) return NULL; // No profiles
-
   profile_map_t::iterator i = lrtp->profiles.find(lrtp->next_csrc);
 
   if(i == lrtp->profiles.end()) {
+    // If we haven't got a valid csrc start from the beginning.
     i = lrtp->profiles.begin();
+  } else {
+    // Otherwise increase the iterator, potentially wrapping.
+    i++;
+    if(i == lrtp->profiles.end()) i = lrtp->profiles.begin();
   }
 
-  struct lrtp_profile_t *profile = i->second;
+  lrtp->next_csrc = i->second->csrc;
+
+  return lrtp->next_csrc;
+}
 
-  i++;
+static lrtp_profile_t *get_next_profile(struct lrtp_t *lrtp)
+{
+  if(lrtp->profiles.size() == 0) return NULL; // No profiles
 
-  if(i == lrtp->profiles.end()) {
-    i = lrtp->profiles.begin();
-  }
-  
-  lrtp->next_csrc = i->second->csrc;
+  csrc_t start = get_next_csrc(lrtp);
+  csrc_t cur = start;
+  do {
+    if(lrtp->profiles.find(cur) == lrtp->profiles.end()) {
+      printf("ERROR: This shouldn't happen."
+             " Selected profile is not in the list\n");
+      break;
+    }
 
-  return profile;
+    struct lrtp_profile_t *profile = lrtp->profiles.find(cur)->second;
+    if(profile->framelist.size()) return profile;
+
+    cur = get_next_csrc(lrtp);
+
+  } while(cur != start);
+
+  return NULL;
 }
 
 EXPORT
@@ -201,7 +233,7 @@ int lrtp_pack(struct lrtp_t *lrtp, char *packet, size_t maxsize)
 
   RTP rtp;
   rtp.setSSrc(lrtp->ssrc);
-  //rtp.setTimestamp(ts); // TODO...
+  rtp.setTimestamp(frame->timestamp);
   rtp.setSeq(lrtp->seq);
   rtp.addCSrc(profile->csrc);
 
@@ -219,7 +251,6 @@ int lrtp_pack(struct lrtp_t *lrtp, char *packet, size_t maxsize)
       profile->process_finished(profile, frame->data,
                                 profile->process_finished_ptr);
     }
-
     profile->framelist.pop_front();
     delete frame;
   }
diff --git a/src/lrtp.h b/src/lrtp.h
index ce6b0e2..d4abbc1 100644
--- a/src/lrtp.h
+++ b/src/lrtp.h
@@ -56,15 +56,13 @@ struct lrtp_t *lrtp_init(const char *key, unsigned int ssrc);
 EXPORT
 void lrtp_close(struct lrtp_t *lrtp);
 
-struct lrtp_profile_t;
-
 /**
- * @param ...
+ * @return 0 on success, 1 on error.
  */
 EXPORT
-struct lrtp_profile_t *lrtp_create_profile(struct lrtp_t *lrtp,
-                                           lrtp_profile_id_t profile_id,
-                                           unsigned int csrc, ...);
+int lrtp_create_profile(struct lrtp_t *lrtp,
+                        lrtp_profile_id_t profile_id,
+                        unsigned int csrc, ...);
 
 EXPORT
 void lrtp_destroy_profile(struct lrtp_t *lrtp, unsigned int csrc);
@@ -75,7 +73,7 @@ typedef enum {
   PACK_BUFFER_TOO_SMALL = 1001, // Packet buffer needs to be bigger.
   PACK_UNKNOWN_PROFILE  = 1002, // Illegal profile id
   PACK_MISSING_PROFILE  = 1003, // Profile pointer NULL or not valid.
-} lrtp_pack_status_t;
+} lrtp_pack_status_t;f
 
 /**
  * Enqueue a media frame for the packetiser.
@@ -83,15 +81,16 @@ typedef enum {
  * @param framedate The frame data that needs encapsulation.
  * @param framesize The size in bytes of the frame data.
  * @return 0 on success, or a negative error code on error.
- * NOTE: The frame pointer cannot be freed or overwritten until all frame data
- * has been handled by lrtp_pack(). Either call lrtp_pack() until it returns 0
- * after each call to lrtp_enqueue_frame or use the
+ * WARNING: The frame pointer cannot be freed or overwritten until all frame
+ * data has been handled by lrtp_pack(). Either call lrtp_pack() until it
+ * returns 0 after each call to lrtp_enqueue_frame or use the
  * OPTION_SET_PROCESS_FINISHED_HANDLER option to set a process finished handler.
  * See lrtp_profiles.h for further details.
  */
 EXPORT
-int lrtp_enqueue_frame(struct lrtp_profile_t *profile,
-                       const char *framedate, size_t framesize);
+int lrtp_enqueue_frame(struct lrtp_t *lrtp, unsigned int csrc,
+                       const char *framedate, size_t framesize,
+                       unsigned long int timestamp);
 
 /**
  * Handle frame data from the frame queue and create at most a single sRTP
diff --git a/src/lrtp_profiles.h b/src/lrtp_profiles.h
index a3d6db1..d17625d 100644
--- a/src/lrtp_profiles.h
+++ b/src/lrtp_profiles.h
@@ -58,8 +58,25 @@ typedef enum {
                                  // Number of bytes per rtp packet.
                                  // Default is 100
 
+// L16 (Linear 16 bit PCM) profile options:
+#define OPTION_L16_SAMPLES_PER_CHANNEL_PER_PACKET 3000 // Integer argument.
+                                                       // Maximum number of
+                                                       // samples per channel
+                                                       // in each packet.
+                                                       // Default is 1024.
+
+#define OPTION_L16_CHANNELS 3001 // Integer argument.
+                                 // Number of channels.
+                                 // Default is 1 (mono)
+
+#define OPTION_L16_LITTLE_ENDIAN 3002 // Boolean argument
+                                      // If true, samples are converted to big
+                                      // endian (network order) before
+                                      // transmission.
+                                      // Default: false
+
 // AMR-WB profile options:
-#define OPTION_AMRWB_FRAME_TYPE_INDEX 3000 // Integer argument.
+#define OPTION_AMRWB_FRAME_TYPE_INDEX 4000 // Integer argument.
                                            // Frame type index according to
                                            // Table 1a in "3GPP TS 26.201"
                                            // Default is 8: AMR-WB 23.85 kbit/s
diff --git a/src/rtp_profile.h b/src/rtp_profile.h
index 96dc9eb..c38f824 100644
--- a/src/rtp_profile.h
+++ b/src/rtp_profile.h
@@ -39,6 +39,7 @@ typedef struct {
   const char *data;
   size_t size;
   size_t offset;
+  unsigned long int timestamp;
 } inputframe_t;
 
 typedef struct {
diff --git a/src/rtp_profile_amrwb.cc b/src/rtp_profile_amrwb.cc
index b376cb9..94084cf 100644
--- a/src/rtp_profile_amrwb.cc
+++ b/src/rtp_profile_amrwb.cc
@@ -94,7 +94,7 @@ static int frame_type_index(const char *frame_header)
   return (*frame_header >> 3) & 0xf;
 }
 
-static int set_frame_type_index(char *frame_header, int index)
+static void set_frame_type_index(char *frame_header, int index)
 {
   *frame_header = (*frame_header & 0x87) | ( (index & 0xf) << 3);
 }
@@ -105,12 +105,13 @@ static int set_frame_type_index(char *frame_header, int index)
  *     set the RX_TYPE (see [6]) to either SPEECH_BAD or SID_BAD
  *     depending on the frame type (FT).
  */
+/*
 static int quality_indicator(const char *frame_header)
 {
   return (*frame_header >> 2) & 0x1;
 }
-
-static int set_quality_indicator(char *frame_header, int quality)
+*/
+static void set_quality_indicator(char *frame_header, int quality)
 {
   *frame_header = (*frame_header & 0xf8) | ( (quality & 0x1) << 2);
 }
@@ -223,10 +224,13 @@ int profile_amrwb_unpack(struct lrtp_profile_t *profile,
                          const RTP &rtp,
                          std::list<outputframe_t *> &framelist)
 {
-  struct lrtp_profile_amrwb_t *p = (struct lrtp_profile_amrwb_t *)profile;
+  //struct lrtp_profile_amrwb_t *p = (struct lrtp_profile_amrwb_t *)profile;
 
-  size_t size = rtp.payloadSize();
+  //size_t size = rtp.payloadSize();
   const char *payload = rtp.payloadData();
+
+  unsigned int timestamp = rtp.timestamp();
+
   /*
   printf("---------------------------------------- New packet:\n");
   printf("payload (unpack):\n");
@@ -265,6 +269,7 @@ int profile_amrwb_unpack(struct lrtp_profile_t *profile,
     size_t frame_size = wb_frame_size[frame_size_idx];
 
     outputframe_t *of = new outputframe_t();
+    of->ts = timestamp;
     of->size = frame_size;
     char *buf = (char*)malloc(of->size);
     memcpy(buf, frame_data, frame_size);
@@ -278,6 +283,9 @@ int profile_amrwb_unpack(struct lrtp_profile_t *profile,
     frame_toc += frameheader_size;
     frame_data += frame_size;
 
+    int num_samples = 1; // TODO: Figure out how many samples were in this package
+    timestamp += num_samples;
+
     frame_num--;
   }
 
diff --git a/src/rtp_profile_jpeg.cc b/src/rtp_profile_jpeg.cc
new file mode 100644
index 0000000..7aeacc4
--- /dev/null
+++ b/src/rtp_profile_jpeg.cc
@@ -0,0 +1,1642 @@
+/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/* vim: set et sw=2 ts=2: */
+/***************************************************************************
+ *            rtp_profile_jpeg.cc
+ *
+ *  Wed Dec 11 08:09:16 CET 2013
+ *  Copyright 2013 Bent Bisballe Nyeng
+ *  deva@aasimon.org
+ ****************************************************************************/
+/*
+ *  This file is part of lrtp.
+ *
+ * lrtp 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.
+ *
+ * lrtp 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 Lesser General Public
+ * License along with lrtp; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+/*
+ * Large portions of this file have been taken from the gstreamer jpeg rtp
+ * pay/depay implementation version 1.1.3:
+ *  gst-plugins-good-1.1.3/gst/rtp/gstrtpjpegpay.c
+ * and
+ *  gst-plugins-good-1.1.3/gst/rtp/gstrtpjpegdepay.c
+ * Copyright (C) 2008 Wim Taymans <wim.taymans@gmail.com>
+ * Copyright (C) 2008 Axis Communications <dev-gstreamer@axis.com>
+ * @author Bjorn Ostby <bjorn.ostby@axis.com>
+ * @author Peter Kjellerstedt <peter.kjellerstedt@axis.com>
+ */
+#include "rtp_profile_jpeg.h"
+
+#include "rtp_profile.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#include <list>
+#include <map>
+#include <string>
+
+// For htons
+#include <arpa/inet.h>
+
+#if 0
+#define GST_LOG(fmt...) printf("\nLOG\t"fmt)
+#define GST_WARNING(fmt...) printf("\nWARN:\t"fmt)
+#define GST_ELEMENT_WARNING(fmt...) printf("\nWARN\t"fmt)
+#define GST_WARNING_OBJECT(fmt...) printf("\nWARN\t"fmt)
+#define GST_LOG_OBJECT(fmt...) printf("\nLOG:\t"fmt)
+#define GST_DEBUG_OBJECT(fmt...) printf("\nDEBUG:\t"fmt)
+#else
+#define GST_LOG(fmt...)
+#define GST_WARNING(fmt...)
+#define GST_ELEMENT_WARNING(fmt...)
+#define GST_WARNING_OBJECT(fmt...)
+#define GST_LOG_OBJECT(fmt...)
+#define GST_DEBUG_OBJECT(fmt...)
+#endif
+
+#define GST_ROUND_UP_8(num)  (((num)+7)&~7)
+
+#define G_LIKELY(x) x
+#define G_UNLIKELY(x) x
+
+#define g_htons(x) htons(x)
+
+#define TRUE true
+#define FALSE false
+
+typedef unsigned char guint8;
+typedef unsigned int guint;
+typedef int gint;
+typedef unsigned long gulong;
+typedef bool gboolean;
+typedef unsigned long long GstClockTime;
+typedef unsigned short guint16;
+
+typedef enum {
+  /* custom success starts here */
+  GST_FLOW_CUSTOM_SUCCESS_2 = 102,
+  GST_FLOW_CUSTOM_SUCCESS_1 = 101,
+  GST_FLOW_CUSTOM_SUCCESS = 100,
+
+  /* core predefined */
+  GST_FLOW_OK		  =  0,
+  /* expected failures */
+  GST_FLOW_NOT_LINKED     = -1,
+  GST_FLOW_FLUSHING       = -2,
+  /* error cases */
+  GST_FLOW_EOS            = -3,
+  GST_FLOW_NOT_NEGOTIATED = -4,
+  GST_FLOW_ERROR	  = -5,
+  GST_FLOW_NOT_SUPPORTED  = -6,
+
+  /* custom error starts here */
+  GST_FLOW_CUSTOM_ERROR   = -100,
+  GST_FLOW_CUSTOM_ERROR_1 = -101,
+  GST_FLOW_CUSTOM_ERROR_2 = -102
+} GstFlowReturn;
+
+///--------------------------------------------------------------------------
+
+////////////////////////////////////////////////////
+/// Begin if pay
+////////////////////////////////////////////////////
+
+
+
+/*
+ * QUANT_PREFIX_LEN:
+ *
+ * Prefix length in the header before the quantization tables:
+ * Two size bytes and one byte for precision
+ */
+#define QUANT_PREFIX_LEN     3
+
+/*
+ * RtpJpegMarker:
+ * @JPEG_MARKER: Prefix for JPEG marker
+ * @JPEG_MARKER_SOI: Start of Image marker
+ * @JPEG_MARKER_JFIF: JFIF marker
+ * @JPEG_MARKER_CMT: Comment marker
+ * @JPEG_MARKER_DQT: Define Quantization Table marker
+ * @JPEG_MARKER_SOF: Start of Frame marker
+ * @JPEG_MARKER_DHT: Define Huffman Table marker
+ * @JPEG_MARKER_SOS: Start of Scan marker
+ * @JPEG_MARKER_EOI: End of Image marker
+ * @JPEG_MARKER_DRI: Define Restart Interval marker
+ * @JPEG_MARKER_H264: H264 marker
+ *
+ * Identifers for markers in JPEG header
+ */
+typedef enum {
+  JPEG_MARKER = 0xFF,
+  JPEG_MARKER_SOI = 0xD8,
+  JPEG_MARKER_JFIF = 0xE0,
+  JPEG_MARKER_CMT = 0xFE,
+  JPEG_MARKER_DQT = 0xDB,
+  JPEG_MARKER_SOF = 0xC0,
+  JPEG_MARKER_DHT = 0xC4,
+  JPEG_MARKER_SOS = 0xDA,
+  JPEG_MARKER_EOI = 0xD9,
+  JPEG_MARKER_DRI = 0xDD,
+  JPEG_MARKER_H264 = 0xE4
+} RtpJpegMarker;
+
+#define DEFAULT_JPEG_QUANT    255
+
+#define DEFAULT_JPEG_QUALITY  255
+#define DEFAULT_JPEG_TYPE     1
+
+enum
+{
+  PROP_0,
+  PROP_JPEG_QUALITY,
+  PROP_JPEG_TYPE,
+  PROP_LAST
+};
+
+enum
+{
+  Q_TABLE_0 = 0,
+  Q_TABLE_1,
+  Q_TABLE_MAX                   /* only support for two tables at the moment */
+};
+
+typedef struct _RtpJpegHeader RtpJpegHeader;
+
+/*
+ * RtpJpegHeader:
+ * @type_spec: type specific
+ * @offset: fragment offset
+ * @type: type field
+ * @q: quantization table for this frame
+ * @width: width of image in 8-pixel multiples
+ * @height: height of image in 8-pixel multiples
+ *
+ * 0                   1                   2                   3
+ * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * | Type-specific |              Fragment Offset                  |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |      Type     |       Q       |     Width     |     Height    |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct _RtpJpegHeader
+{
+  unsigned int type_spec:8;
+  unsigned int offset:24;
+  unsigned char type;
+  unsigned char q;
+  unsigned char width;
+  unsigned char height;
+};
+
+/*
+ * RtpQuantHeader
+ * @mbz: must be zero
+ * @precision: specify size of quantization tables
+ * @length: length of quantization data
+ *
+ * 0                   1                   2                   3
+ * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |      MBZ      |   Precision   |             Length            |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                    Quantization Table Data                    |
+ * |                              ...                              |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+typedef struct
+{
+  unsigned char mbz;
+  unsigned char precision;
+  unsigned short length;
+} RtpQuantHeader;
+
+typedef struct
+{
+  unsigned char size;
+  const unsigned char *data;
+} RtpQuantTable;
+
+/*
+ * RtpRestartMarkerHeader:
+ * @restartInterval: number of MCUs that appear between restart markers
+ * @restartFirstLastCount: a combination of the first packet mark in the chunk
+ *                         last packet mark in the chunk and the position of the
+ *                         first restart interval in the current "chunk"
+ *
+ *    0                   1                   2                   3
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |       Restart Interval        |F|L|       Restart Count       |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ *  The restart marker header is implemented according to the following
+ *  methodology specified in section 3.1.7 of rfc2435.txt.
+ *
+ *  "If the restart intervals in a frame are not guaranteed to be aligned
+ *  with packet boundaries, the F (first) and L (last) bits MUST be set
+ *  to 1 and the Restart Count MUST be set to 0x3FFF.  This indicates
+ *  that a receiver MUST reassemble the entire frame before decoding it."
+ *
+ */
+
+typedef struct
+{
+  unsigned short restart_interval;
+  unsigned short restart_count;
+} RtpRestartMarkerHeader;
+
+typedef struct
+{
+  unsigned char id;
+  unsigned char samp;
+  unsigned char qt;
+} CompInfo;
+
+typedef struct {
+  //GstRTPBasePayload payload;
+
+  unsigned char quality;
+  unsigned char type;
+
+  int height;
+  int width;
+
+  unsigned char quant;
+} GstRtpJPEGPay;
+
+//typedef char GstRTPBasePayload;
+typedef struct {
+  const unsigned char *data;
+  size_t size;
+  unsigned long long timestamp;
+} GstBuffer;
+
+#include <vector>
+
+typedef std::vector<GstBuffer> GstBufferList;
+
+#define QUANT_TABLE_SIZE 15
+
+static unsigned int gst_rtp_jpeg_pay_header_size(const unsigned char *data,
+                                                 unsigned int offset)
+{
+  return data[offset] << 8 | data[offset + 1];
+}
+
+static unsigned int gst_rtp_jpeg_pay_read_quant_table(const unsigned char *data,
+                                                      unsigned int size,
+                                                      unsigned int offset,
+                                                      RtpQuantTable tables[])
+{
+  unsigned int quant_size, tab_size;
+  unsigned char prec;
+  unsigned char id;
+
+  if (offset + 2 > size)
+    goto too_small;
+
+  quant_size = gst_rtp_jpeg_pay_header_size (data, offset);
+  if (quant_size < 2)
+    goto small_quant_size;
+
+  /* clamp to available data */
+  if (offset + quant_size > size)
+    quant_size = size - offset;
+
+  offset += 2;
+  quant_size -= 2;
+
+  while (quant_size > 0) {
+    /* not enough to read the id */
+    if (offset + 1 > size)
+      break;
+
+    id = data[offset] & 0x0f;
+    if (id == 15)
+      /* invalid id received - corrupt data */
+      goto invalid_id;
+
+    prec = (data[offset] & 0xf0) >> 4;
+    if (prec)
+      tab_size = 128;
+    else
+      tab_size = 64;
+
+    /* there is not enough for the table */
+    if (quant_size < tab_size + 1)
+      goto no_table;
+
+    GST_LOG("read quant table %d, tab_size %d, prec %02x", id, tab_size, prec);
+
+    tables[id].size = tab_size;
+    tables[id].data = &data[offset + 1];
+
+    tab_size += 1;
+    quant_size -= tab_size;
+    offset += tab_size;
+  }
+done:
+  return offset + quant_size;
+
+  /* ERRORS */
+too_small:
+  {
+    GST_WARNING ("not enough data");
+    return size;
+  }
+small_quant_size:
+  {
+    GST_WARNING ("quant_size too small (%u < 2)", quant_size);
+    return size;
+  }
+invalid_id:
+  {
+    GST_WARNING ("invalid id");
+    goto done;
+  }
+no_table:
+  {
+    GST_WARNING ("not enough data for table (%u < %u)", quant_size,
+        tab_size + 1);
+    goto done;
+  }
+}
+
+static bool
+gst_rtp_jpeg_pay_read_sof (GstRtpJPEGPay * pay, const guint8 * data,
+    guint size, guint * offset, CompInfo info[], RtpQuantTable tables[],
+    gulong tables_elements)
+{
+  guint sof_size, off;
+  guint width, height, infolen;
+  CompInfo elem;
+  gint i, j;
+
+  off = *offset;
+
+  /* we need at least 17 bytes for the SOF */
+  if (off + 17 > size)
+    goto wrong_size;
+
+  sof_size = gst_rtp_jpeg_pay_header_size (data, off);
+  if (sof_size < 17)
+    goto wrong_length;
+
+  *offset += sof_size;
+
+  /* skip size */
+  off += 2;
+
+  /* precision should be 8 */
+  if (data[off++] != 8)
+    goto bad_precision;
+
+  /* read dimensions */
+  height = data[off] << 8 | data[off + 1];
+  width = data[off + 2] << 8 | data[off + 3];
+  off += 4;
+
+  GST_LOG_OBJECT("got dimensions %ux%u", height, width);
+
+  if (height == 0) {
+    goto invalid_dimension;
+  }
+  if (height > 2040) {
+    height = 0;
+  }
+  if (width == 0) {
+    goto invalid_dimension;
+  }
+  if (width > 2040) {
+    width = 0;
+  }
+
+  if (height == 0 || width == 0) {
+    pay->height = 0;
+    pay->width = 0;
+  } else {
+    pay->height = GST_ROUND_UP_8 (height) / 8;
+    pay->width = GST_ROUND_UP_8 (width) / 8;
+  }
+
+  /* we only support 3 components */
+  if (data[off++] != 3)
+    goto bad_components;
+
+  infolen = 0;
+  for (i = 0; i < 3; i++) {
+    elem.id = data[off++];
+    elem.samp = data[off++];
+    elem.qt = data[off++];
+    GST_LOG_OBJECT("got comp %d, samp %02x, qt %d",elem.id, elem.samp, elem.qt);
+    /* insertion sort from the last element to the first */
+    for (j = infolen; j > 1; j--) {
+      if (G_LIKELY (info[j - 1].id < elem.id))
+        break;
+      info[j] = info[j - 1];
+    }
+    info[j] = elem;
+    infolen++;
+  }
+
+  /* see that the components are supported */
+  if (info[0].samp == 0x21)
+    pay->type = 0;
+  else if (info[0].samp == 0x22)
+    pay->type = 1;
+  else {
+    GST_LOG_OBJECT("info[0].samp := %02x\n", (unsigned char)info[0].samp);
+    goto invalid_comp;
+  }
+
+  if (!(info[1].samp == 0x11)) {
+    GST_LOG_OBJECT("!(info[1].samp == 0x11)\n");
+    goto invalid_comp;
+  }
+
+  if (!(info[2].samp == 0x11)) {
+    GST_LOG_OBJECT("!(info[2].samp == 0x11)\n");
+    goto invalid_comp;
+  }
+
+  /* the other components are free to use any quant table but they have to
+   * have the same table id */
+  if (info[1].qt != info[2].qt) {
+    /* Some MJPG (like the one from the Logitech C-920 camera) uses different
+     * quant tables for component 1 and 2 but both tables contain the exact
+     * same data, so we could consider them as being the same tables */
+    if (!(info[1].qt < tables_elements &&
+            info[2].qt < tables_elements &&
+            tables[info[1].qt].size > 0 &&
+            tables[info[1].qt].size == tables[info[2].qt].size &&
+            memcmp (tables[info[1].qt].data, tables[info[2].qt].data,
+                tables[info[1].qt].size) == 0))
+      goto invalid_comp;
+  }
+
+  return TRUE;
+
+  /* ERRORS */
+wrong_size:
+  {
+    GST_ELEMENT_WARNING("Wrong size %u (needed %u).", size, off + 17);
+    return FALSE;
+  }
+wrong_length:
+  {
+    GST_ELEMENT_WARNING("Wrong SOF length %u.", sof_size);
+    return FALSE;
+  }
+bad_precision:
+  {
+    GST_ELEMENT_WARNING("Wrong precision, expecting 8.");
+    return FALSE;
+  }
+invalid_dimension:
+  {
+    GST_ELEMENT_WARNING("Wrong dimension, size %ux%u", width, height);
+    return FALSE;
+  }
+bad_components:
+  {
+    GST_ELEMENT_WARNING("Wrong number of components");
+    return FALSE;
+  }
+invalid_comp:
+  {
+    GST_ELEMENT_WARNING("Invalid component");
+    return FALSE;
+  }
+}
+
+static gboolean
+gst_rtp_jpeg_pay_read_dri (GstRtpJPEGPay * pay, const guint8 * data,
+    guint size, guint * offset, RtpRestartMarkerHeader * dri)
+{
+  guint dri_size, off;
+
+  off = *offset;
+
+  /* we need at least 4 bytes for the DRI */
+  if (off + 4 > size)
+    goto wrong_size;
+
+  dri_size = gst_rtp_jpeg_pay_header_size (data, off);
+  if (dri_size < 4)
+    goto wrong_length;
+
+  *offset += dri_size;
+  off += 2;
+
+  dri->restart_interval = g_htons ((data[off] << 8) | (data[off + 1]));
+  dri->restart_count = g_htons (0xFFFF);
+
+  return dri->restart_interval > 0;
+
+wrong_size:
+  {
+    GST_WARNING ("not enough data for DRI");
+    *offset = size;
+    return FALSE;
+  }
+wrong_length:
+  {
+    GST_WARNING ("DRI size too small (%u)", dri_size);
+    *offset += dri_size;
+    return FALSE;
+  }
+}
+
+static RtpJpegMarker
+gst_rtp_jpeg_pay_scan_marker (const guint8 * data, guint size, guint * offset)
+{
+  while ((data[(*offset)++] != JPEG_MARKER) && ((*offset) < size));
+
+  if (G_UNLIKELY ((*offset) >= size)) {
+    GST_LOG ("found EOI marker");
+    return JPEG_MARKER_EOI;
+  } else {
+    guint8 marker;
+
+    marker = data[*offset];
+    GST_LOG ("found 0x%02x marker at offset %u", marker, *offset);
+    (*offset)++;
+    return (RtpJpegMarker)marker;
+  }
+}
+
+static GstFlowReturn
+gst_rtp_jpeg_pay_handle_buffer(GstRtpJPEGPay *basepayload,
+                               GstBuffer *buffer, std::list<RTP> &rtplist)
+{
+  GstRtpJPEGPay *pay;
+  GstClockTime timestamp;
+  GstFlowReturn ret = GST_FLOW_OK;//ERROR;
+  RtpJpegHeader jpeg_header;
+  RtpQuantHeader quant_header;
+  RtpRestartMarkerHeader restart_marker_header;
+  RtpQuantTable tables[QUANT_TABLE_SIZE] = { {0, NULL}, };
+  CompInfo info[3] = { {0,}, };
+  guint quant_data_size;
+  //GstMapInfo map;
+  const guint8 *data;
+  size_t size;
+  guint mtu;
+  guint bytes_left;
+  guint jpeg_header_size = 0;
+  guint offset;
+  gboolean frame_done;
+  gboolean sos_found, sof_found, dqt_found, dri_found;
+  gint i;
+  //GstBufferList *list = NULL;
+
+  pay = basepayload;
+
+  // Max packet size (std. 1400)
+  mtu = 1400;// basepayload->mtu;//GST_RTP_BASE_PAYLOAD_MTU (pay);
+
+  //gst_buffer_map (buffer, &map, GST_MAP_READ);
+  data = buffer->data;
+  size = buffer->size;
+  timestamp = buffer->timestamp;//GST_BUFFER_TIMESTAMP (buffer);
+  offset = 0;
+
+  GST_LOG_OBJECT("got buffer size %d timestamp %d", (int)size, (int)timestamp);
+
+  /* parse the jpeg header for 'start of scan' and read quant tables if needed */
+  sos_found = FALSE;
+  dqt_found = FALSE;
+  sof_found = FALSE;
+  dri_found = FALSE;
+
+  while (!sos_found && (offset < size)) {
+    GST_LOG_OBJECT("checking from offset %u", offset);
+    int marker = gst_rtp_jpeg_pay_scan_marker (data, size, &offset);
+    GST_LOG_OBJECT("Marker: 0x%02x", marker);
+    switch (marker) {
+      case JPEG_MARKER_JFIF:
+        GST_LOG_OBJECT("skipping marker (JFIF)");
+        offset += gst_rtp_jpeg_pay_header_size (data, offset);
+        break;
+      case JPEG_MARKER_CMT:
+        GST_LOG_OBJECT("skipping marker (CMT)");
+        offset += gst_rtp_jpeg_pay_header_size (data, offset);
+        break;
+      case JPEG_MARKER_DHT:
+        GST_LOG_OBJECT("skipping marker (DHT: define Huffman table)");
+        offset += gst_rtp_jpeg_pay_header_size (data, offset);
+        break;
+      case JPEG_MARKER_H264:
+        GST_LOG_OBJECT("skipping marker (H264)");
+        offset += gst_rtp_jpeg_pay_header_size (data, offset);
+        break;
+      case JPEG_MARKER_SOF:
+        if (!gst_rtp_jpeg_pay_read_sof(pay, data, size, &offset, info, tables,
+                                       QUANT_TABLE_SIZE))
+          goto invalid_format;
+        sof_found = TRUE;
+        break;
+      case JPEG_MARKER_DQT:
+        GST_LOG ("DQT found");
+        offset = gst_rtp_jpeg_pay_read_quant_table (data, size, offset, tables);
+        dqt_found = TRUE;
+        break;
+      case JPEG_MARKER_SOS:
+        sos_found = TRUE;
+        GST_LOG_OBJECT("SOS found");
+        jpeg_header_size = offset + gst_rtp_jpeg_pay_header_size (data, offset);
+        break;
+      case JPEG_MARKER_EOI:
+        GST_WARNING_OBJECT("EOI reached before SOS!");
+        break;
+      case JPEG_MARKER_SOI:
+        GST_LOG_OBJECT("SOI found");
+        break;
+      case JPEG_MARKER_DRI:
+        GST_LOG_OBJECT("DRI found");
+        if (gst_rtp_jpeg_pay_read_dri (pay, data, size, &offset,
+                &restart_marker_header))
+          dri_found = TRUE;
+        break;
+      default:
+        break;
+    }
+  }
+  if (!dqt_found || !sof_found)
+    goto unsupported_jpeg;
+
+  /* by now we should either have negotiated the width/height or the SOF header
+   * should have filled us in */
+  if (pay->width < 0 || pay->height < 0) {
+    goto no_dimension;
+  }
+
+  GST_LOG_OBJECT("header size %u", jpeg_header_size);
+
+  size -= jpeg_header_size;
+  data += jpeg_header_size;
+  offset = 0;
+
+  if (dri_found)
+    pay->type += 64;
+
+  /* prepare stuff for the jpeg header */
+  jpeg_header.type_spec = 0;
+  jpeg_header.type = pay->type;
+  jpeg_header.q = pay->quant;
+  jpeg_header.width = pay->width;
+  jpeg_header.height = pay->height;
+
+  /* collect the quant headers sizes */
+  quant_header.mbz = 0;
+  quant_header.precision = 0;
+  quant_header.length = 0;
+  quant_data_size = 0;
+
+  if (pay->quant > 127) {
+    /* for the Y and U component, look up the quant table and its size. quant
+     * tables for U and V should be the same */
+    for (i = 0; i < 2; i++) {
+      guint qsize;
+      guint qt;
+
+      qt = info[i].qt;
+      if (qt >= QUANT_TABLE_SIZE)
+        goto invalid_quant;
+
+      qsize = tables[qt].size;
+      if (qsize == 0)
+        goto invalid_quant;
+
+      quant_header.precision |= (qsize == 64 ? 0 : (1 << i));
+      quant_data_size += qsize;
+    }
+    quant_header.length = g_htons (quant_data_size);
+    quant_data_size += sizeof (quant_header);
+  }
+
+  GST_LOG_OBJECT("quant_data size %u", quant_data_size);
+
+  //list = gst_buffer_list_new ();
+
+  bytes_left = sizeof (jpeg_header) + quant_data_size + size;
+
+  if (dri_found)
+    bytes_left += sizeof (restart_marker_header);
+
+  frame_done = FALSE;
+  do {
+    //GstBuffer *outbuf;
+    //char *outbuf;
+    guint8 *payload;
+    guint payload_size = (bytes_left < mtu ? bytes_left : mtu);
+    guint header_size;
+    //GstBuffer *paybuf;
+    //GstRTPBuffer rtp = { NULL };
+    RTP rtp;
+
+    header_size = sizeof (jpeg_header) + quant_data_size;
+    if (dri_found)
+      header_size += sizeof (restart_marker_header);
+
+
+    //outbuf = gst_rtp_buffer_new_allocate (header_size, 0, 0);
+    //outbuf = malloc(header_size);
+
+    //gst_rtp_buffer_map (outbuf, GST_MAP_WRITE, &rtp);
+
+    if (payload_size == bytes_left) {
+      GST_LOG_OBJECT("last packet of frame");
+      frame_done = TRUE;
+      //gst_rtp_buffer_set_marker (&rtp, 1);
+      rtp.setMarker(true);
+    }
+
+
+    //gst_rtp_buffer_get_payload (&rtp);
+    GST_LOG_OBJECT("-------- %d\n", header_size);
+    unsigned char *payload_base =
+      (unsigned char*)malloc(/*header_size*/16*1024);
+    payload = payload_base;
+    
+    
+    /* update offset */
+#if (G_BYTE_ORDER == G_LITTLE_ENDIAN)
+    jpeg_header.offset = ((offset & 0x0000FF) << 16) |
+        ((offset & 0xFF0000) >> 16) | (offset & 0x00FF00);
+#else
+    jpeg_header.offset = offset;
+#endif
+    memcpy (payload, &jpeg_header, sizeof (jpeg_header));
+    payload += sizeof (jpeg_header);
+    payload_size -= sizeof (jpeg_header);
+
+    if (dri_found) {
+      memcpy (payload, &restart_marker_header, sizeof (restart_marker_header));
+      payload += sizeof (restart_marker_header);
+      payload_size -= sizeof (restart_marker_header);
+    }
+
+    /* only send quant table with first packet */
+    if (G_UNLIKELY (quant_data_size > 0)) {
+      memcpy (payload, &quant_header, sizeof (quant_header));
+      payload += sizeof (quant_header);
+
+      /* copy the quant tables for luma and chrominance */
+      for (i = 0; i < 2; i++) {
+        guint qsize;
+        guint qt;
+
+        qt = info[i].qt;
+        qsize = tables[qt].size;
+        memcpy (payload, tables[qt].data, qsize);
+
+        GST_LOG_OBJECT("component %d using quant %d, size %d", i, qt, qsize);
+
+        payload += qsize;
+      }
+      payload_size -= quant_data_size;
+      bytes_left -= quant_data_size;
+      quant_data_size = 0;
+    }
+    GST_LOG_OBJECT("sending payload size %d", payload_size);
+    //gst_rtp_buffer_unmap (&rtp);
+    
+    /* create a new buf to hold the payload */
+    //paybuf = gst_buffer_copy_region (buffer, GST_BUFFER_COPY_MEMORY,
+    //                                 jpeg_header_size + offset, payload_size);
+
+    /* join memory parts */
+    //outbuf = gst_buffer_append (outbuf, paybuf);
+    memcpy(payload, buffer->data + jpeg_header_size + offset, payload_size);
+    payload += payload_size;
+
+    rtp.setPayload((char*)payload_base, payload - payload_base);
+    free(payload_base);
+
+    //GST_BUFFER_TIMESTAMP (outbuf) = timestamp;
+    rtp.setTimestamp(timestamp);
+
+    /* and add to list */
+    //gst_buffer_list_insert (list, -1, outbuf);
+    rtplist.push_back(rtp);
+
+    bytes_left -= payload_size;
+    offset += payload_size;
+    data += payload_size;
+  }
+  while (!frame_done);
+
+  /* push the whole buffer list at once */
+  //ret = gst_rtp_base_payload_push_list (basepayload, list);
+
+  //  gst_buffer_unmap (buffer, &map);
+  //  gst_buffer_unref (buffer);
+
+  return ret;
+
+  /* ERRORS */
+unsupported_jpeg:
+  {
+    GST_ELEMENT_WARNING("Unsupported JPEG");
+    //    gst_buffer_unmap (buffer, &map);
+    //    gst_buffer_unref (buffer);
+    return GST_FLOW_ERROR;
+  }
+no_dimension:
+  {
+    GST_ELEMENT_WARNING("No size given");
+    //    gst_buffer_unmap (buffer, &map);
+    //    gst_buffer_unref (buffer);
+    return GST_FLOW_ERROR;
+  }
+invalid_format:
+  {
+    /* error was posted */
+    //    gst_buffer_unmap (buffer, &map);
+    //    gst_buffer_unref (buffer);
+    return GST_FLOW_ERROR;
+  }
+invalid_quant:
+  {
+    GST_ELEMENT_WARNING("Invalid quant tables");
+    //    gst_buffer_unmap (buffer, &map);
+    //    gst_buffer_unref (buffer);
+    return GST_FLOW_ERROR;
+  }
+}
+
+static void
+gst_rtp_jpeg_pay_init (GstRtpJPEGPay * pay)
+{
+  pay->quality = DEFAULT_JPEG_QUALITY;
+  pay->quant = DEFAULT_JPEG_QUANT;
+  pay->type = DEFAULT_JPEG_TYPE;
+  pay->width = -1;
+  pay->height = -1;
+}
+
+
+////////////////////////////////////////////////////
+/// End of pay
+////////////////////////////////////////////////////
+
+///--------------------------------------------------------------------------
+
+////////////////////////////////////////////////////
+/// Begin of depay
+////////////////////////////////////////////////////
+
+
+// TODO
+typedef std::string GstAdapter;
+
+typedef struct {
+  //  GstRTPBaseDepayload depayload;
+
+  GstAdapter adapter;
+  gboolean    discont;
+
+  /* cached quant tables */
+  //guint8 * qtables[255];
+  std::map<int, guint8 *> qtables;
+  gint frate_num;
+  gint frate_denom;
+  gint media_width;
+  gint media_height;
+  gint width, height;
+} GstRtpJPEGDepay;
+
+
+
+
+
+
+
+static void
+gst_rtp_jpeg_depay_init (GstRtpJPEGDepay * rtpjpegdepay)
+{
+  //  rtpjpegdepay->adapter = 0;//TODO: gst_adapter_new ();
+}
+
+/*
+static void
+gst_rtp_jpeg_depay_reset (GstRtpJPEGDepay * depay)
+{
+  gint i;
+
+  depay->width = 0;
+  depay->height = 0;
+  depay->media_width = 0;
+  depay->media_height = 0;
+  depay->frate_num = 0;
+  depay->frate_denom = 1;
+  depay->discont = TRUE;
+
+  for (i = 0; i < 255; i++) {
+    free (depay->qtables[i]);
+    depay->qtables[i] = NULL;
+  }
+
+  //TODO: gst_adapter_clear (depay->adapter);
+}
+*/
+
+static const int zigzag[] = {
+  0, 1, 8, 16, 9, 2, 3, 10,
+  17, 24, 32, 25, 18, 11, 4, 5,
+  12, 19, 26, 33, 40, 48, 41, 34,
+  27, 20, 13, 6, 7, 14, 21, 28,
+  35, 42, 49, 56, 57, 50, 43, 36,
+  29, 22, 15, 23, 30, 37, 44, 51,
+  58, 59, 52, 45, 38, 31, 39, 46,
+  53, 60, 61, 54, 47, 55, 62, 63
+};
+
+/*
+ * Table K.1 from JPEG spec.
+ */
+static const int jpeg_luma_quantizer[64] = {
+  16, 11, 10, 16, 24, 40, 51, 61,
+  12, 12, 14, 19, 26, 58, 60, 55,
+  14, 13, 16, 24, 40, 57, 69, 56,
+  14, 17, 22, 29, 51, 87, 80, 62,
+  18, 22, 37, 56, 68, 109, 103, 77,
+  24, 35, 55, 64, 81, 104, 113, 92,
+  49, 64, 78, 87, 103, 121, 120, 101,
+  72, 92, 95, 98, 112, 100, 103, 99
+};
+
+/*
+ * Table K.2 from JPEG spec.
+ */
+static const int jpeg_chroma_quantizer[64] = {
+  17, 18, 24, 47, 99, 99, 99, 99,
+  18, 21, 26, 66, 99, 99, 99, 99,
+  24, 26, 56, 99, 99, 99, 99, 99,
+  47, 66, 99, 99, 99, 99, 99, 99,
+  99, 99, 99, 99, 99, 99, 99, 99,
+  99, 99, 99, 99, 99, 99, 99, 99,
+  99, 99, 99, 99, 99, 99, 99, 99,
+  99, 99, 99, 99, 99, 99, 99, 99
+};
+
+#define CLAMP(x, l, u) (x<l?l:(x>u)?u:x)
+
+/* Call MakeTables with the Q factor and a guint8[128] return array
+ */
+static void
+MakeTables (GstRtpJPEGDepay * rtpjpegdepay, gint Q, guint8 qtable[128])
+{
+  gint i;
+  guint factor;
+
+  factor = CLAMP (Q, 1, 99);
+
+  if (Q < 50)
+    Q = 5000 / factor;
+  else
+    Q = 200 - factor * 2;
+
+  for (i = 0; i < 64; i++) {
+    gint lq = (jpeg_luma_quantizer[zigzag[i]] * Q + 50) / 100;
+    gint cq = (jpeg_chroma_quantizer[zigzag[i]] * Q + 50) / 100;
+
+    /* Limit the quantizers to 1 <= q <= 255 */
+    qtable[i] = CLAMP (lq, 1, 255);
+    qtable[i + 64] = CLAMP (cq, 1, 255);
+  }
+}
+
+static const guint8 lum_dc_codelens[] = {
+  0, 1, 5, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0
+};
+
+static const guint8 lum_dc_symbols[] = {
+  0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
+};
+
+static const guint8 lum_ac_codelens[] = {
+  0, 2, 1, 3, 3, 2, 4, 3, 5, 5, 4, 4, 0, 0, 1, 0x7d
+};
+
+static const guint8 lum_ac_symbols[] = {
+  0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
+  0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07,
+  0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
+  0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0,
+  0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16,
+  0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
+  0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
+  0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
+  0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
+  0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
+  0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79,
+  0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
+  0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98,
+  0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
+  0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
+  0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5,
+  0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
+  0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
+  0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
+  0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
+  0xf9, 0xfa
+};
+
+static const guint8 chm_dc_codelens[] = {
+  0, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0
+};
+
+static const guint8 chm_dc_symbols[] = {
+  0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
+};
+
+static const guint8 chm_ac_codelens[] = {
+  0, 2, 1, 2, 4, 4, 3, 4, 7, 5, 4, 4, 0, 1, 2, 0x77
+};
+
+static const guint8 chm_ac_symbols[] = {
+  0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21,
+  0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71,
+  0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
+  0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33, 0x52, 0xf0,
+  0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34,
+  0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26,
+  0x27, 0x28, 0x29, 0x2a, 0x35, 0x36, 0x37, 0x38,
+  0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48,
+  0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
+  0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
+  0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78,
+  0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+  0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96,
+  0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5,
+  0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
+  0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3,
+  0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2,
+  0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
+  0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9,
+  0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
+  0xf9, 0xfa
+};
+
+static guint8 *
+MakeQuantHeader (guint8 * p, const guint8 * qt, gint size, gint tableNo)
+{
+  *p++ = 0xff;
+  *p++ = 0xdb;                  /* DQT */
+  *p++ = 0;                     /* length msb */
+  *p++ = size + 3;              /* length lsb */
+  *p++ = tableNo;
+  memcpy (p, qt, size);
+
+  return (p + size);
+}
+
+static guint8 *
+MakeHuffmanHeader (guint8 * p, const guint8 * codelens, int ncodes,
+    const guint8 * symbols, int nsymbols, int tableNo, int tableClass)
+{
+  *p++ = 0xff;
+  *p++ = 0xc4;                  /* DHT */
+  *p++ = 0;                     /* length msb */
+  *p++ = 3 + ncodes + nsymbols; /* length lsb */
+  *p++ = (tableClass << 4) | tableNo;
+  memcpy (p, codelens, ncodes);
+  p += ncodes;
+  memcpy (p, symbols, nsymbols);
+  p += nsymbols;
+
+  return (p);
+}
+
+static guint8 *
+MakeDRIHeader (guint8 * p, guint16 dri)
+{
+  *p++ = 0xff;
+  *p++ = 0xdd;                  /* DRI */
+  *p++ = 0x0;                   /* length msb */
+  *p++ = 4;                     /* length lsb */
+  *p++ = dri >> 8;              /* dri msb */
+  *p++ = dri & 0xff;            /* dri lsb */
+
+  return (p);
+}
+
+/*
+ *  Arguments:
+ *    type, width, height: as supplied in RTP/JPEG header
+ *    qt: quantization tables as either derived from
+ *        the Q field using MakeTables() or as specified
+ *        in section 4.2.
+ *    dri: restart interval in MCUs, or 0 if no restarts.
+ *
+ *    p: pointer to return area
+ *
+ *  Return value:
+ *    The length of the generated headers.
+ *
+ *    Generate a frame and scan headers that can be prepended to the
+ *    RTP/JPEG data payload to produce a JPEG compressed image in
+ *    interchange format (except for possible trailing garbage and
+ *    absence of an EOI marker to terminate the scan).
+ */
+static guint
+MakeHeaders (guint8 * p, int type, int width, int height, const guint8 * qt,
+    guint precision, guint16 dri)
+{
+  guint8 *start = p;
+  gint size;
+
+  *p++ = 0xff;
+  *p++ = 0xd8;                  /* SOI */
+
+  size = ((precision & 1) ? 128 : 64);
+  p = MakeQuantHeader (p, qt, size, 0);
+  qt += size;
+
+  size = ((precision & 2) ? 128 : 64);
+  p = MakeQuantHeader (p, qt, size, 1);
+  qt += size;
+
+  if (dri != 0)
+    p = MakeDRIHeader (p, dri);
+
+  *p++ = 0xff;
+  *p++ = 0xc0;                  /* SOF */
+  *p++ = 0;                     /* length msb */
+  *p++ = 17;                    /* length lsb */
+  *p++ = 8;                     /* 8-bit precision */
+  *p++ = height >> 8;           /* height msb */
+  *p++ = height;                /* height lsb */
+  *p++ = width >> 8;            /* width msb */
+  *p++ = width;                 /* width lsb */
+  *p++ = 3;                     /* number of components */
+  *p++ = 0;                     /* comp 0 */
+  if ((type & 0x3f) == 0)
+    *p++ = 0x21;                /* hsamp = 2, vsamp = 1 */
+  else
+    *p++ = 0x22;                /* hsamp = 2, vsamp = 2 */
+  *p++ = 0;                     /* quant table 0 */
+  *p++ = 1;                     /* comp 1 */
+  *p++ = 0x11;                  /* hsamp = 1, vsamp = 1 */
+  *p++ = 1;                     /* quant table 1 */
+  *p++ = 2;                     /* comp 2 */
+  *p++ = 0x11;                  /* hsamp = 1, vsamp = 1 */
+  *p++ = 1;                     /* quant table 1 */
+
+  p = MakeHuffmanHeader (p, lum_dc_codelens,
+      sizeof (lum_dc_codelens), lum_dc_symbols, sizeof (lum_dc_symbols), 0, 0);
+  p = MakeHuffmanHeader (p, lum_ac_codelens,
+      sizeof (lum_ac_codelens), lum_ac_symbols, sizeof (lum_ac_symbols), 0, 1);
+  p = MakeHuffmanHeader (p, chm_dc_codelens,
+      sizeof (chm_dc_codelens), chm_dc_symbols, sizeof (chm_dc_symbols), 1, 0);
+  p = MakeHuffmanHeader (p, chm_ac_codelens,
+      sizeof (chm_ac_codelens), chm_ac_symbols, sizeof (chm_ac_symbols), 1, 1);
+
+  *p++ = 0xff;
+  *p++ = 0xda;                  /* SOS */
+  *p++ = 0;                     /* length msb */
+  *p++ = 12;                    /* length lsb */
+  *p++ = 3;                     /* 3 components */
+  *p++ = 0;                     /* comp 0 */
+  *p++ = 0;                     /* huffman table 0 */
+  *p++ = 1;                     /* comp 1 */
+  *p++ = 0x11;                  /* huffman table 1 */
+  *p++ = 2;                     /* comp 2 */
+  *p++ = 0x11;                  /* huffman table 1 */
+  *p++ = 0;                     /* first DCT coeff */
+  *p++ = 63;                    /* last DCT coeff */
+  *p++ = 0;                     /* sucessive approx. */
+
+  return (p - start);
+};
+
+//static GstBuffer *
+static int
+gst_rtp_jpeg_depay_process(GstRtpJPEGDepay *rtpjpegdepay,
+                           const RTP &rtp)
+{
+  //TODO:  GstBuffer *outbuf;
+  gint payload_len, header_len;
+  const guint8 *payload;
+  guint frag_offset;
+  gint Q;
+  gint type, width, height;
+  guint16 dri, precision, length;
+  const guint8 *qtable;
+  // TODO: GstRTPBuffer _rtp = { NULL };
+
+  /* // TODO:
+  if (GST_BUFFER_IS_DISCONT (buf)) {
+    gst_adapter_clear (rtpjpegdepay->adapter);
+    rtpjpegdepay->discont = TRUE;
+  }
+  */
+
+  //gst_rtp_buffer_map(buf, GST_MAP_READ, &_rtp);
+  payload_len = rtp.payloadSize();//gst_rtp_buffer_get_payload_len (&_rtp);
+
+  if (payload_len < 8)
+    goto empty_packet;
+
+  //payload = gst_rtp_buffer_get_payload(&_rtp);
+  payload = (const guint8 *)rtp.payloadData();
+  header_len = 0;
+
+  /*  0                   1                   2                   3
+   *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+   * | Type-specific |              Fragment Offset                  |
+   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+   * |      Type     |       Q       |     Width     |     Height    |
+   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+   */
+  frag_offset = (payload[1] << 16) | (payload[2] << 8) | payload[3];
+  type = payload[4];
+  Q = payload[5];
+  width = payload[6] * 8;
+  height = payload[7] * 8;
+
+  /* allow frame dimensions > 2040, passed in SDP session or media attributes
+   * from gstrtspsrc.c (gst_rtspsrc_sdp_attributes_to_caps), or in caps */
+  /* // No!
+  if (!width)
+    width = rtpjpegdepay->media_width;
+
+  if (!height)
+    height = rtpjpegdepay->media_height;
+  */
+
+  if (width == 0 || height == 0)
+    goto invalid_dimension;
+
+  GST_DEBUG_OBJECT("frag %u, type %u, Q %d, width %u, height %u",
+                   frag_offset, type, Q, width, height);
+
+  header_len += 8;
+  payload += 8;
+  payload_len -= 8;
+
+  dri = 0;
+  if (type > 63) {
+    if (payload_len < 4)
+      goto empty_packet;
+
+    /*  0                   1                   2                   3
+     *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     * |       Restart Interval        |F|L|       Restart Count       |
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     */
+    dri = (payload[0] << 8) | payload[1];
+
+    GST_DEBUG_OBJECT("DRI %d", dri);
+
+    payload += 4;
+    header_len += 4;
+    payload_len -= 4;
+  }
+
+  if (Q >= 128 && frag_offset == 0) {
+    if (payload_len < 4)
+      goto empty_packet;
+
+    /*  0                   1                   2                   3
+     *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     * |      MBZ      |   Precision   |             Length            |
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     * |                    Quantization Table Data                    |
+     * |                              ...                              |
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     */
+    precision = payload[1];
+    length = (payload[2] << 8) | payload[3];
+
+    GST_DEBUG_OBJECT("precision %04x, length %d", precision, length);
+
+    if (Q == 255 && length == 0)
+      goto empty_packet;
+
+    payload += 4;
+    header_len += 4;
+    payload_len -= 4;
+
+    if (length > payload_len)
+      goto empty_packet;
+
+    if (length > 0) {
+      GST_LOG_OBJECT("--- Use qtable from payload %d bytes", length);
+      qtable = (guint8*)payload;
+    } else {
+      if(rtpjpegdepay->qtables.find(Q) == rtpjpegdepay->qtables.end()) {
+        GST_LOG_OBJECT("Ooops! Missing qtable for Q: %d", Q);
+      }
+      GST_LOG_OBJECT("--- %d qtables\n", rtpjpegdepay->qtables.size());
+      qtable = rtpjpegdepay->qtables[Q];
+    }
+
+    payload += length;
+    header_len += length;
+    payload_len -= length;
+  } else {
+    length = 0;
+    qtable = NULL;
+    precision = 0;
+  }
+
+  if (frag_offset == 0) {
+    //    GstMapInfo map;
+    guint size;
+
+    if (rtpjpegdepay->width != width || rtpjpegdepay->height != height) {
+      /*
+      GstCaps *outcaps;
+
+      outcaps =
+          gst_caps_new_simple ("image/jpeg", "framerate", GST_TYPE_FRACTION,
+          rtpjpegdepay->frate_num, rtpjpegdepay->frate_denom, "width",
+          G_TYPE_INT, width, "height", G_TYPE_INT, height, NULL);
+      gst_pad_set_caps (depayload->srcpad, outcaps);
+      gst_caps_unref (outcaps);
+      */
+      rtpjpegdepay->width = width;
+      rtpjpegdepay->height = height;
+    }
+
+    GST_LOG_OBJECT("first packet, length %d", length);
+
+    /* first packet */
+    if (length == 0) {
+      if (Q < 128) {
+        /* no quant table, see if we have one cached */
+        qtable = rtpjpegdepay->qtables[Q];
+        if (!qtable) {
+          GST_DEBUG_OBJECT("making Q %d table", Q);
+          /* make and cache the table */
+          guint8 *_qtable = (guint8*)malloc(sizeof(guint8) * 128);
+          MakeTables(rtpjpegdepay, Q, _qtable);
+          rtpjpegdepay->qtables[Q] = _qtable;
+        } else {
+          GST_DEBUG_OBJECT("using cached table for Q %d", Q);
+        }
+        /* all 8 bit quantizers */
+        precision = 0;
+      } else {
+        if (!qtable)
+          goto no_qtable;
+      }
+    }
+    /* max header length, should be big enough */
+    /*
+    outbuf = gst_buffer_new_and_alloc (1000);
+    gst_buffer_map (outbuf, &map, GST_MAP_WRITE);
+    size = MakeHeaders (map.data, type, width, height, qtable, precision, dri);
+    gst_buffer_unmap (outbuf, &map);
+    gst_buffer_resize (outbuf, 0, size);
+    */
+    guint8 *_outbuf = (guint8*)malloc(1000);
+    size = MakeHeaders(_outbuf, type, width, height, qtable, precision, dri);
+    //    _outbuf = (guint8*)realloc(_outbuf, size);
+
+    GST_DEBUG_OBJECT("pushing %u bytes of header", size);
+
+    // TODO: gst_adapter_push (rtpjpegdepay->adapter, _outbuf);
+    rtpjpegdepay->adapter.append((char*)_outbuf, size);
+
+    free(_outbuf);
+  }
+
+  /* take JPEG data, push in the adapter */
+  GST_DEBUG_OBJECT("pushing data at offset %d", header_len);
+  /*
+  outbuf = gst_rtp_buffer_get_payload_subbuffer (&_rtp, header_len, -1);
+  gst_adapter_push (rtpjpegdepay->adapter, outbuf);
+  outbuf = NULL;
+  */
+  rtpjpegdepay->adapter.append(rtp.payloadData() + header_len,
+                               rtp.payloadSize() - header_len);
+
+  //  if (gst_rtp_buffer_get_marker (&_rtp)) {
+  if(rtp.marker()) {
+    guint avail;
+    guint8 end[2];
+    //GstMapInfo map;
+
+    /* last buffer take all data out of the adapter */
+    // TODO: avail = gst_adapter_available (rtpjpegdepay->adapter);
+    avail = rtpjpegdepay->adapter.size();
+
+    GST_DEBUG_OBJECT("marker set, last buffer");
+
+    if (avail < 2)
+      goto invalid_packet;
+
+    /* take the last bytes of the jpeg data to see if there is an EOI
+     * marker */
+    //TODO: gst_adapter_copy (rtpjpegdepay->adapter, end, avail - 2, 2);
+    end[0] = rtpjpegdepay->adapter[avail - 2];
+    end[1] = rtpjpegdepay->adapter[avail - 1];
+
+    if (end[0] != 0xff && end[1] != 0xd9) {
+      GST_DEBUG_OBJECT("no EOI marker, adding one");
+
+      /* no EOI marker, add one */
+      /*
+      outbuf = gst_buffer_new_and_alloc (2);
+      gst_buffer_map (outbuf, &map, GST_MAP_WRITE);
+      map.data[0] = 0xff;
+      map.data[1] = 0xd9;
+      gst_buffer_unmap (outbuf, &map);
+      */
+      char buf[] = { 0xff, 0xd9 };
+
+      // TODO: gst_adapter_push (rtpjpegdepay->adapter, buf, sizeof(buf));
+      rtpjpegdepay->adapter.append(buf, sizeof(buf));
+
+      avail += 2;
+    }
+
+    // TODO: outbuf = gst_adapter_take_buffer (rtpjpegdepay->adapter, avail);
+
+    if (rtpjpegdepay->discont) {
+      // TODO: GST_BUFFER_FLAG_SET(outbuf, GST_BUFFER_FLAG_DISCONT);
+      rtpjpegdepay->discont = FALSE;
+    }
+
+    GST_DEBUG_OBJECT("returning %u bytes", avail);
+    return avail;
+  }
+
+  //gst_rtp_buffer_unmap (&_rtp);
+
+  return 0; //TODO: outbuf;
+
+  /* ERRORS */
+empty_packet:
+  {
+    GST_ELEMENT_WARNING("Empty Payload.");
+    //gst_rtp_buffer_unmap (&_rtp);
+    return 0;
+  }
+invalid_dimension:
+  {
+    GST_ELEMENT_WARNING("Invalid Dimension %dx%d.", width, height);
+    //gst_rtp_buffer_unmap (&_rtp);
+    return 0;
+  }
+no_qtable:
+  {
+    GST_WARNING_OBJECT("no qtable");
+    //gst_rtp_buffer_unmap (&_rtp);
+    return 0;
+  }
+invalid_packet:
+  {
+    GST_WARNING_OBJECT("invalid packet");
+    // TODO: gst_adapter_flush (rtpjpegdepay->adapter, gst_adapter_available (rtpjpegdepay->adapter));
+    //gst_rtp_buffer_unmap (&_rtp);
+    return 0;
+  }
+}
+
+
+
+
+
+
+////////////////////////////////////////////////////
+/// End of depay
+////////////////////////////////////////////////////
+
+///--------------------------------------------------------------------------
+
+struct lrtp_profile_jpeg_t {
+  struct lrtp_profile_t profile; // 'Inherit' lrtp_profile_t
+
+  std::list<RTP> rtplist;
+  GstRtpJPEGDepay depay;
+  uint32_t timestamp;
+};
+
+
+int profile_jpeg_pack(struct lrtp_profile_t *profile,
+                      const char *frame, size_t framesize,
+                      RTP &rtp)
+{
+  struct lrtp_profile_jpeg_t *p = (struct lrtp_profile_jpeg_t *)profile;
+
+  // We assume that all frames in the same stream have unique timestamps...
+  if(rtp.timestamp() != p->timestamp) {
+
+    // TODO: If p->rtplist is not empty, clear it and issue a warning...
+
+    GstBuffer buffer;
+    buffer.data = (const unsigned char*)frame;
+    buffer.size = framesize;
+
+    GstRtpJPEGPay basepayload;
+    gst_rtp_jpeg_pay_init(&basepayload);
+    int ret = gst_rtp_jpeg_pay_handle_buffer(&basepayload, &buffer, p->rtplist);
+
+    p->timestamp = rtp.timestamp();
+
+    GST_LOG_OBJECT("ret: %d - list: %d\n", ret, p->rtplist.size());
+
+    if(ret != GST_FLOW_OK) {
+      rtp.setValid(false);
+      return -1;
+    }
+  }
+
+  if(p->rtplist.size()) {
+    RTP r = p->rtplist.front();
+    p->rtplist.pop_front();
+    rtp.setMarker(r.marker());
+    rtp.setPayload(r.payloadData(), r.payloadSize());
+
+    if(p->rtplist.size()) return 1; // signal more packets
+    else return framesize; // signal 'all data used'
+  }
+
+  rtp.setValid(false);
+
+  return framesize;
+}
+
+int profile_jpeg_unpack(struct lrtp_profile_t *profile,
+                        const RTP &rtp,
+                        std::list<outputframe_t *> &framelist)
+{
+  struct lrtp_profile_jpeg_t *p = (struct lrtp_profile_jpeg_t *)profile;
+
+  {
+    int ret = gst_rtp_jpeg_depay_process(&p->depay, rtp);
+    if(ret > 0) {
+      outputframe_t *of = new outputframe_t();
+
+      of->size = ret;
+      char *buf = (char*)malloc(of->size);
+
+      memcpy(buf, p->depay.adapter.data(), ret);
+      p->depay.adapter.clear();
+
+      of->data = buf;
+      of->ts = rtp.timestamp();
+
+      framelist.push_back(of);
+    }
+  }
+
+  return 0;
+}
+
+void profile_jpeg_destroy(struct lrtp_profile_t *profile)
+{
+  struct lrtp_profile_jpeg_t *p = (struct lrtp_profile_jpeg_t *)profile;
+  delete p;
+}
+
+struct lrtp_profile_t *profile_jpeg_create(struct lrtp_t *lrtp,
+                                           unsigned int csrc,
+                                           va_list vp)
+{
+  struct lrtp_profile_jpeg_t *p = new struct lrtp_profile_jpeg_t;
+
+  p->profile.pack = profile_jpeg_pack;
+  p->profile.unpack = profile_jpeg_unpack;
+  p->profile.destroy = profile_jpeg_destroy;
+
+  p->timestamp = 0xffffffff;
+
+  gst_rtp_jpeg_depay_init(&p->depay);
+
+  while(true) {
+    int type = va_arg(vp, int);
+    if(type == OPTION_END) break;
+
+    switch(type) {
+    default:
+      // TODO: Unknown arg type
+      break;
+    }
+  }
+
+  return &p->profile;
+}
+
diff --git a/src/rtp_profile_jpeg.h b/src/rtp_profile_jpeg.h
new file mode 100644
index 0000000..a8c21e7
--- /dev/null
+++ b/src/rtp_profile_jpeg.h
@@ -0,0 +1,39 @@
+/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/* vim: set et sw=2 ts=2: */
+/***************************************************************************
+ *            rtp_profile_jpeg.h
+ *
+ *  Wed Dec 11 08:09:16 CET 2013
+ *  Copyright 2013 Bent Bisballe Nyeng
+ *  deva@aasimon.org
+ ****************************************************************************/
+
+/*
+ *  This file is part of lrtp.
+ *
+ * lrtp 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.
+ *
+ * lrtp 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 Lesser General Public
+ * License along with lrtp; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+#ifndef __LRTP_RTP_PROFILE_JPEG_H__
+#define __LRTP_RTP_PROFILE_JPEG_H__
+
+#include <stdarg.h>
+
+#include "lrtp.h"
+
+struct lrtp_profile_t *profile_jpeg_create(struct lrtp_t *lrtp,
+                                           unsigned int csrc,
+                                           va_list ap);
+
+#endif/*__LRTP_RTP_PROFILE_JPEG_H__*/
diff --git a/src/rtp_profile_l16.cc b/src/rtp_profile_l16.cc
new file mode 100644
index 0000000..379df1c
--- /dev/null
+++ b/src/rtp_profile_l16.cc
@@ -0,0 +1,148 @@
+/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/* vim: set et sw=2 ts=2: */
+/***************************************************************************
+ *            rtp_profile_l16.cc
+ *
+ *  Mon Dec 23 14:36:50 CET 2013
+ *  Copyright 2013 Bent Bisballe Nyeng
+ *  deva@aasimon.org
+ ****************************************************************************/
+
+/*
+ *  This file is part of lrtp.
+ *
+ * lrtp 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.
+ *
+ * lrtp 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 Lesser General Public
+ * License along with lrtp; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+#include "rtp_profile_l16.h"
+
+#include "rtp_profile.h"
+
+#include <stdio.h>
+#include <string.h>
+
+// For ntoh et al.
+#ifdef WIN32
+#include <winsock2.h>
+#else
+#include <arpa/inet.h>
+#endif
+
+struct lrtp_profile_l16_t {
+  struct lrtp_profile_t profile; // 'Inherit' lrtp_profile_t
+
+  unsigned short int pkg_size;
+  unsigned int num_channels;
+  bool little_endian;
+};
+
+int profile_l16_pack(struct lrtp_profile_t *profile,
+                     const char *frame, size_t framesize,
+                     RTP &rtp)
+{
+  struct lrtp_profile_l16_t *p = (struct lrtp_profile_l16_t *)profile;
+
+  size_t cpsamples = p->pkg_size;
+
+  // Calculate complete sample-channel tuples in frame.
+  size_t num_samples = framesize / (sizeof(short) * p->num_channels);
+
+  if(cpsamples > num_samples) cpsamples = num_samples;
+
+  // Convert to bytes
+  size_t cpsz = cpsamples * sizeof(short) * p->num_channels;
+
+  if(p->little_endian) {
+    short *buf = (short*)malloc(cpsz / sizeof(short));
+    for(size_t i = 0; i < cpsz / sizeof(short); i++) {
+      buf[i] = htons(((const short*)frame)[i]);
+    }
+    rtp.setPayload((char*)buf, cpsz);
+    free(buf);
+  } else {
+    rtp.setPayload(frame, cpsz);
+  }
+
+  return cpsz;
+}
+
+int profile_l16_unpack(struct lrtp_profile_t *profile,
+                       const RTP &rtp,
+                       std::list<outputframe_t *> &framelist)
+{
+  struct lrtp_profile_l16_t *p = (struct lrtp_profile_l16_t *)profile;
+
+  outputframe_t *of = new outputframe_t();
+  of->size = rtp.payloadSize();
+  char *buf = (char*)malloc(of->size);
+
+  if(p->little_endian) {
+    const short *frame = (const short *)rtp.payloadData();
+    for(size_t i = 0; i < of->size / sizeof(short); i++) {
+      ((short*)buf)[i] = ntohs(frame[i]);
+    }
+  } else {
+    of->size = rtp.payload(buf, of->size);
+  }
+
+  of->data = buf;
+
+  framelist.push_back(of);
+
+  return 0;
+}
+
+void profile_l16_destroy(struct lrtp_profile_t *profile)
+{
+  struct lrtp_profile_l16_t *p = (struct lrtp_profile_l16_t *)profile;
+  delete p;
+}
+
+struct lrtp_profile_t *profile_l16_create(struct lrtp_t *lrtp,
+                                          unsigned int csrc,
+                                          va_list vp)
+{
+  struct lrtp_profile_l16_t *p = new struct lrtp_profile_l16_t;
+
+  p->profile.pack = profile_l16_pack;
+  p->profile.unpack = profile_l16_unpack;
+  p->profile.destroy = profile_l16_destroy;
+
+  p->pkg_size = 1024;
+  p->little_endian = false;
+  p->num_channels = 1;
+  
+  while(true) {
+    int type = va_arg(vp, int);
+    if(type == OPTION_END) break;
+
+    switch(type) {
+    case OPTION_L16_SAMPLES_PER_CHANNEL_PER_PACKET:
+      p->pkg_size = va_arg(vp, int);
+      break;
+    case OPTION_L16_CHANNELS:
+      p->num_channels = va_arg(vp, int);
+      break;
+    case OPTION_L16_LITTLE_ENDIAN:
+      p->little_endian = va_arg(vp, int);
+      break;
+    default:
+      // TODO: Unknown arg type
+      break;
+    }
+  }
+
+  return &p->profile;
+}
+
diff --git a/src/rtp_profile_l16.h b/src/rtp_profile_l16.h
new file mode 100644
index 0000000..9f5f423
--- /dev/null
+++ b/src/rtp_profile_l16.h
@@ -0,0 +1,39 @@
+/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/* vim: set et sw=2 ts=2: */
+/***************************************************************************
+ *            rtp_profile_l16.h
+ *
+ *  Mon Dec 23 14:36:50 CET 2013
+ *  Copyright 2013 Bent Bisballe Nyeng
+ *  deva@aasimon.org
+ ****************************************************************************/
+
+/*
+ *  This file is part of lrtp.
+ *
+ * lrtp 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.
+ *
+ * lrtp 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 Lesser General Public
+ * License along with lrtp; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+#ifndef __LRTP_RTP_PROFILE_L16_H__
+#define __LRTP_RTP_PROFILE_L16_H__
+
+#include <stdarg.h>
+
+#include "lrtp.h"
+
+struct lrtp_profile_t *profile_l16_create(struct lrtp_t *lrtp,
+                                          unsigned int csrc,
+                                          va_list ap);
+
+#endif/*__LRTP_RTP_PROFILE_L16_H__*/
diff --git a/src/rtp_profile_opus.cc b/src/rtp_profile_opus.cc
index 102d175..3fc0de9 100644
--- a/src/rtp_profile_opus.cc
+++ b/src/rtp_profile_opus.cc
@@ -144,13 +144,14 @@ int profile_opus_unpack(struct lrtp_profile_t *profile,
                         const RTP &rtp,
                         std::list<outputframe_t *> &framelist)
 {
-  struct lrtp_profile_opus_t *p = (struct lrtp_profile_opus_t *)profile;
+  //struct lrtp_profile_opus_t *p = (struct lrtp_profile_opus_t *)profile;
 
   outputframe_t *of = new outputframe_t();
   of->size = rtp.payloadSize();
   char *buf = (char*)malloc(of->size);
   of->size = rtp.payload(buf, of->size);
   of->data = buf;
+  of->ts = rtp.timestamp();
 
   framelist.push_back(of);
 
diff --git a/src/rtp_profile_raw.cc b/src/rtp_profile_raw.cc
index 2b27501..25cc297 100644
--- a/src/rtp_profile_raw.cc
+++ b/src/rtp_profile_raw.cc
@@ -57,7 +57,7 @@ int profile_raw_unpack(struct lrtp_profile_t *profile,
                        const RTP &rtp,
                        std::list<outputframe_t *> &framelist)
 {
-  struct lrtp_profile_raw_t *p = (struct lrtp_profile_raw_t *)profile;
+  //struct lrtp_profile_raw_t *p = (struct lrtp_profile_raw_t *)profile;
 
   outputframe_t *of = new outputframe_t();
   of->size = rtp.payloadSize();
diff --git a/test/Makefile.am b/test/Makefile.am
index b735c3c..00dc707 100644
--- a/test/Makefile.am
+++ b/test/Makefile.am
@@ -1,7 +1,11 @@
-TESTS = amrwb opus init rtp srtp raw asc2bin
+TESTS = jpeg amrwb opus init rtp srtp raw l16 asc2bin framelist
 
 check_PROGRAMS = $(TESTS)
 
+jpeg_CXXFLAGS = $(CPPUNIT_CFLAGS) -DOUTPUT=\"jpeg\" -I../src
+jpeg_LDFLAGS = $(CPPUNIT_LIBS) -L../src/.libs/ -llrtp
+jpeg_SOURCES = test.cc test_jpeg.cc
+
 amrwb_CXXFLAGS = $(CPPUNIT_CFLAGS) -DOUTPUT=\"amrwb\" -I../src
 amrwb_LDFLAGS = $(CPPUNIT_LIBS) -L../src/.libs/ -llrtp
 amrwb_SOURCES = test.cc test_amrwb.cc
@@ -26,9 +30,17 @@ raw_CXXFLAGS = $(CPPUNIT_CFLAGS) -DOUTPUT=\"raw\" -I../src
 raw_LDFLAGS = $(CPPUNIT_LIBS) -L../src/.libs/ -llrtp
 raw_SOURCES = test.cc test_raw.cc
 
+l16_CXXFLAGS = $(CPPUNIT_CFLAGS) -DOUTPUT=\"l16\" -I../src
+l16_LDFLAGS = $(CPPUNIT_LIBS) -L../src/.libs/ -llrtp
+l16_SOURCES = test.cc test_l16.cc
+
 asc2bin_CXXFLAGS = $(CPPUNIT_CFLAGS) -DOUTPUT=\"asc2bin\" -I../src
 asc2bin_LDFLAGS = $(CPPUNIT_LIBS)
 asc2bin_SOURCES = test.cc test_asc2bin.cc
 
+framelist_CXXFLAGS = $(CPPUNIT_CFLAGS) -DOUTPUT=\"framelist\" -I../src
+framelist_LDFLAGS = $(CPPUNIT_LIBS) -L../src/.libs/ -llrtp
+framelist_SOURCES = test.cc test_framelist.cc
+
 clean-local:
 	-rm -f result_*.xml
diff --git a/test/test1.jpg b/test/test1.jpg
new file mode 100644
index 0000000..96071a2
--- /dev/null
+++ b/test/test1.jpg
diff --git a/test/test2.jpg b/test/test2.jpg
new file mode 100644
index 0000000..fda292b
--- /dev/null
+++ b/test/test2.jpg
diff --git a/test/test3.jpg b/test/test3.jpg
new file mode 100644
index 0000000..ad9ad2d
--- /dev/null
+++ b/test/test3.jpg
diff --git a/test/test4.jpg b/test/test4.jpg
new file mode 100644
index 0000000..cc86495
--- /dev/null
+++ b/test/test4.jpg
diff --git a/test/test_amrwb.cc b/test/test_amrwb.cc
index adefda1..d6a0731 100644
--- a/test/test_amrwb.cc
+++ b/test/test_amrwb.cc
@@ -68,7 +68,7 @@ public:
     std::vector<std::string> packets;
     unsigned int csrc = 42;
   
-    int sent = 0;
+    // int sent = 0;
 
     size_t num_frames = NUM_PKGS;
     size_t framesize = wb_frame_size[frame_type_index] * num_frames;
@@ -76,24 +76,25 @@ public:
     { // Encode
       struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
 
-      struct lrtp_profile_t *profile =
+      int x =
         lrtp_create_profile(lrtp, PROFILE_AMRWB, csrc,
                             OPTION_AMRWB_FRAME_TYPE_INDEX, frame_type_index,
                             OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
 
       char packet[PKG_SIZE];
-      size_t packetsize = sizeof(packet);
+      //size_t packetsize = sizeof(packet);
 
       char cnt = 0;
     
+      int timestamp = 0;
+
       for(int i = 0; i < NUM_PKGS; i++) {
         char frame[wb_frame_size[frame_type_index] * num_frames];
 
-        for(int i = 0; i < framesize; i++) frame[i] = cnt++;
-
-        int timestamp = 0;
+        for(size_t j = 0; j < framesize; j++) frame[j] = cnt++;
 
-        int ret = lrtp_enqueue_frame(profile, frame, framesize);
+        int ret = lrtp_enqueue_frame(lrtp, csrc, frame, framesize, timestamp++);
         while( (ret = lrtp_pack(lrtp, packet, sizeof(packet))) != 0) {
           std::string p;
           p.append(packet, ret);
@@ -108,10 +109,11 @@ public:
     { // Decode
       struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
 
-      struct lrtp_profile_t *profile =
+      int x =
         lrtp_create_profile(lrtp, PROFILE_AMRWB, csrc,
                             OPTION_AMRWB_FRAME_TYPE_INDEX, frame_type_index,
                             OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
 
       char frame[16*1024];
     
diff --git a/test/test_asc2bin.cc b/test/test_asc2bin.cc
index 1663c70..f214a5a 100644
--- a/test/test_asc2bin.cc
+++ b/test/test_asc2bin.cc
@@ -30,8 +30,6 @@
 #include "../src/asc2bin.h"
 #include "../src/asc2bin.cc"
 
-static int g_i = -1;
-
 class test_asc2bin_class : public CppUnit::TestFixture
 {
   CPPUNIT_TEST_SUITE(test_asc2bin_class);
diff --git a/test/test_init.cc b/test/test_init.cc
index 3281183..13ca874 100644
--- a/test/test_init.cc
+++ b/test/test_init.cc
@@ -50,12 +50,10 @@ public:
     CPPUNIT_ASSERT(lrtp);
 
     unsigned int csrc = 42;
-    struct lrtp_profile_t *profile =
-      lrtp_create_profile(lrtp, PROFILE_RAW, csrc,
-                          OPTION_RAW_PKG_SIZE, 100,
-                          OPTION_END);
-
-    CPPUNIT_ASSERT(profile);
+    int x = lrtp_create_profile(lrtp, PROFILE_RAW, csrc,
+                                OPTION_RAW_PKG_SIZE, 100,
+                                OPTION_END);
+    CPPUNIT_ASSERT_EQUAL(0, x);
 
     lrtp_destroy_profile(lrtp, csrc);
 
diff --git a/test/test_jpeg.cc b/test/test_jpeg.cc
new file mode 100644
index 0000000..894ae3a
--- /dev/null
+++ b/test/test_jpeg.cc
@@ -0,0 +1,165 @@
+/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/* vim: set et sw=2 ts=2: */
+/***************************************************************************
+ *            test_jpeg.cc
+ *
+ *  Wed Dec 18 08:59:12 CET 2013
+ *  Copyright 2013 Bent Bisballe Nyeng
+ *  deva@aasimon.org
+ ****************************************************************************/
+
+/*
+ *  This file is part of lrtp.
+ *
+ * lrtp 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.
+ *
+ * lrtp 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 Lesser General Public
+ * License along with lrtp; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+#include <cppunit/extensions/HelperMacros.h>
+
+#include <lrtp.h>
+
+#include <stdio.h>
+
+#include <string>
+#include <vector>
+
+#define KEY "123456789012345678901234567890123456789012345678901234567890"
+#define SSRC 1234567890
+
+class test_jpeg_class : public CppUnit::TestFixture
+{
+  CPPUNIT_TEST_SUITE(test_jpeg_class);
+	CPPUNIT_TEST(test_jpeg);
+	CPPUNIT_TEST_SUITE_END();
+
+public:
+	void setUp() {}
+	void tearDown() {}
+
+	void test_jpeg() {
+
+    std::vector<std::string> packets;
+    unsigned int csrc = 42;
+
+    { // Encode
+      struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
+      
+      int x = lrtp_create_profile(lrtp, PROFILE_JPEG, csrc,
+                                  OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
+      //char num = 0;
+
+      char packet[16*1024];
+
+
+      for(int n = 1; n < 5; n++) {
+        char filename[32];
+        sprintf(filename, "test%d.jpg", n);
+
+        //printf("----------- Image %d -----------\n", n);
+        
+        FILE *fp = fopen(filename, "r");
+        CPPUNIT_ASSERT(fp);
+        fseek(fp, 0, SEEK_END);
+        int imagesize = ftell(fp);
+        fseek(fp, 0, SEEK_SET);
+        char *image = (char*)malloc(imagesize);
+        fread(image, imagesize, 1, fp);
+        fclose(fp);
+
+        int ret = 0;
+        ret = lrtp_enqueue_frame(lrtp, csrc, image, imagesize, n);
+        CPPUNIT_ASSERT_EQUAL(0, ret);
+
+        while( (ret = lrtp_pack(lrtp, packet, sizeof(packet))) != 0) {
+          std::string p;
+          p.append(packet, ret);
+          packets.push_back(p);
+          //dump("pkg", packet, ret);
+        }
+
+        free(image);
+      }
+      
+      lrtp_destroy_profile(lrtp, csrc);
+      
+      lrtp_close(lrtp);
+    }
+
+    //printf("\nPackets: %d\n", packets.size());
+
+    { // Decode
+      //FILE *fp = fopen("output.jpg", "w");
+
+      // Write SIO and JFIF from original image:
+      //      fwrite(image, 2 + 16, 1, fp);
+
+      struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
+      
+      int x = lrtp_create_profile(lrtp, PROFILE_JPEG, csrc,
+                                  OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
+
+      char frame[16*1024];
+      size_t framesize = sizeof(frame);
+      
+      //int cnt = 0;
+      
+      //      int num = 2 + 16; // Skip SOI and JFIF in comparison
+
+      std::vector<std::string>::iterator i = packets.begin();
+      while(i != packets.end()) {
+        size_t packetsize = i->size();
+        // printf("unpack sz: %d\n", packetsize);
+        const char *packet = i->data();
+        unsigned int ts;
+        
+        framesize = sizeof(frame);
+        
+        lrtp_unpack(lrtp, packet, packetsize);
+        int ret;
+        while((ret = lrtp_dequeue_frame(lrtp, frame, framesize, &csrc, &ts))
+              != 0) {
+          //printf("Got %d bytes, csrc %d, ts: %d\n", ret, csrc, ts);
+          
+          //fwrite(frame, ret, 1, fp);
+          /*
+          int err = 0;
+          for(int i = 0; i < ret; i++) {
+            err += abs(frame[i] - image[num++]);
+          }
+          CPPUNIT_ASSERT_EQUAL(0, err);
+          */
+          //dump("pkg", frame, ret);
+        }
+        
+        i++;
+      }
+      
+      //      CPPUNIT_ASSERT_EQUAL((int)imagesize, num);
+
+      lrtp_destroy_profile(lrtp, csrc);
+      lrtp_close(lrtp);
+
+      //fclose(fp);
+    }
+
+    //free(image);
+	}
+};
+
+// Registers the fixture into the 'registry'
+CPPUNIT_TEST_SUITE_REGISTRATION(test_jpeg_class);
+
+
diff --git a/test/test_l16.cc b/test/test_l16.cc
new file mode 100644
index 0000000..0cdb4b2
--- /dev/null
+++ b/test/test_l16.cc
@@ -0,0 +1,148 @@
+/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/* vim: set et sw=2 ts=2: */
+/***************************************************************************
+ *            test_l16.cc
+ *
+ *  Mon Sep  2 14:02:16 CEST 2013
+ *  Copyright 2013 Bent Bisballe Nyeng
+ *  deva@aasimon.org
+ ****************************************************************************/
+
+/*
+ *  This file is part of lrtp.
+ *
+ * lrtp 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.
+ *
+ * lrtp 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 Lesser General Public
+ * License along with lrtp; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+#include <cppunit/extensions/HelperMacros.h>
+
+#include <lrtp.h>
+
+#include <stdio.h>
+
+#include <string>
+#include <vector>
+
+#define KEY "123456789012345678901234567890123456789012345678901234567890"
+#define SSRC 1234567890
+
+class test_conn_class : public CppUnit::TestFixture
+{
+  CPPUNIT_TEST_SUITE(test_conn_class);
+	CPPUNIT_TEST(test_l16);
+	CPPUNIT_TEST_SUITE_END();
+
+public:
+	void setUp() {}
+	void tearDown() {}
+
+	void test_l16() {
+    char frame[5];
+    int pkg_size = 4;
+
+    std::vector<std::string> packets;
+    unsigned int csrc = 42;
+
+    CPPUNIT_ASSERT(false); // We need to actually test the profile options here...
+    
+    { // Encode
+      struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
+      
+      /*
+       OPTION_L16_SAMPLES_PER_CHANNEL_PER_PACKET
+       OPTION_L16_CHANNELS
+       OPTION_L16_LITTLE_ENDIAN
+      */
+
+      int x =
+        lrtp_create_profile(lrtp, PROFILE_L16, csrc,
+                            OPTION_L16_SAMPLES_PER_CHANNEL_PER_PACKET, pkg_size,
+                            OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
+
+      char num = 0;
+
+      char packet[16*1024];
+      //size_t size = sizeof(packet);
+      
+      for(unsigned int ts = 0; ts < 8; ts++) {
+
+        for(size_t i = 0; i < sizeof(frame); i++) frame[i] = num++;
+
+        int ret = 0;
+        ret = lrtp_enqueue_frame(lrtp, csrc, frame, sizeof(frame), ts);
+        while( (ret = lrtp_pack(lrtp, packet, sizeof(packet))) != 0) {
+          std::string p;
+          p.append(packet, ret);
+          packets.push_back(p);
+          //dump("pkg", packet, ret);
+        }
+      }
+      
+      lrtp_destroy_profile(lrtp, csrc);
+      
+      lrtp_close(lrtp);
+    }
+    
+    { // Decode
+      struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
+
+      int x =
+        lrtp_create_profile(lrtp, PROFILE_L16, csrc,
+                            OPTION_L16_SAMPLES_PER_CHANNEL_PER_PACKET, pkg_size,
+                            OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
+
+      char frame[16*1024];
+      size_t framesize = sizeof(frame);
+      
+      //int cnt = 0;
+      
+      char num = 0;
+
+      std::vector<std::string>::iterator i = packets.begin();
+      while(i != packets.end()) {
+        size_t packetsize = i->size();
+        // printf("unpack sz: %d\n", packetsize);
+        const char *packet = i->data();
+        unsigned int ts;
+        
+        framesize = sizeof(frame);
+        
+        lrtp_unpack(lrtp, packet, packetsize);
+        int ret;
+        while((ret = lrtp_dequeue_frame(lrtp, frame, framesize, &csrc, &ts))
+              != 0) {
+          //printf("Got %d bytes, csrc %d, ts: %d\n", ret, csrc, ts);
+
+          int err = 0;
+          for(int i = 0; i < ret; i++) {
+            err += abs(frame[i] - num++);
+          }
+          CPPUNIT_ASSERT_EQUAL(0, err);
+
+          //dump("pkg", frame, ret);
+        }
+        
+        i++;
+      }
+      
+      lrtp_destroy_profile(lrtp, csrc);
+      lrtp_close(lrtp);
+    }
+	}
+};
+
+// Registers the fixture into the 'registry'
+CPPUNIT_TEST_SUITE_REGISTRATION(test_conn_class);
diff --git a/test/test_opus.cc b/test/test_opus.cc
index ea6a5b4..380e7b9 100644
--- a/test/test_opus.cc
+++ b/test/test_opus.cc
@@ -81,20 +81,19 @@ public:
 
     int32_t lookahead;
   
-    size_t ts = 0;
+    //size_t ts = 0;
 
     int sent = 0;
 
     { // Encode
       struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
 
-      struct lrtp_profile_t *profile =
-        lrtp_create_profile(lrtp, PROFILE_OPUS, csrc,
-                            //OPTION_RAW_PKG_SIZE, pkg_size,
-                            OPTION_END);
+      int x = lrtp_create_profile(lrtp, PROFILE_OPUS, csrc,
+                                  OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
 
       char packet[16*1024];
-      size_t packetsize = sizeof(packet);
+      //size_t packetsize = sizeof(packet);
     
       int err;
       OpusEncoder *opus = opus_encoder_create(FS, channels,
@@ -108,14 +107,14 @@ public:
 
       long long int sin_x = 0;
 
-      int cnt = 0;
+      //int cnt = 0;
       size_t timestamp = 0;
       size_t idx = 0;
       for(unsigned int ts = 0; ts < FS / 10; ts++) {
         size_t pcmsize = ms[idx] / (48000.0 / FS); // Number of samples pr channel
         sent += pcmsize;
         short *pcm = new short[100000/*pcmsize * channels*/];
-        for(int i = 0 ; i < pcmsize; i++) {
+        for(size_t i = 0 ; i < pcmsize; i++) {
           sin_x++;
 
           if((int)sin_x % FS == 0) {
@@ -138,7 +137,7 @@ public:
           printf("Opus error: %s\n", opus_strerror(framesize));	
         }
 
-        int ret = lrtp_enqueue_frame(profile, frame, framesize);
+        int ret = lrtp_enqueue_frame(lrtp, csrc, frame, framesize, timestamp);
         while( (ret = lrtp_pack(lrtp, packet, sizeof(packet))) != 0) {
           std::string p;
           p.append(packet, ret);
@@ -156,15 +155,15 @@ public:
     { // Decode
       struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
 
-      struct lrtp_profile_t *profile =
-        lrtp_create_profile(lrtp, PROFILE_OPUS, csrc,
-                            OPTION_END);
+      int x = lrtp_create_profile(lrtp, PROFILE_OPUS, csrc,
+                                  OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
 
       int err;
       OpusDecoder *opus = opus_decoder_create(FS, channels, &err);
       CPPUNIT_ASSERT_EQUAL(0, err);
 
-      int idx = (sizeof(ms)/sizeof(size_t)) - 1;
+      //int idx = (sizeof(ms)/sizeof(size_t)) - 1;
 
       char frame[16*1024];
 
@@ -173,7 +172,7 @@ public:
       long long int errl = 0;
       long long int errr = 0;
   
-      int cnt = 0;
+      //int cnt = 0;
       std::vector<std::string>::iterator i = packets.begin();
       while(i != packets.end()) {
         size_t packetsize = i->size();
diff --git a/test/test_raw.cc b/test/test_raw.cc
index d3910d3..e219ffa 100644
--- a/test/test_raw.cc
+++ b/test/test_raw.cc
@@ -1,7 +1,7 @@
 /* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
 /* vim: set et sw=2 ts=2: */
 /***************************************************************************
- *            test_init.cc
+ *            test_raw.cc
  *
  *  Mon Sep  2 14:02:16 CEST 2013
  *  Copyright 2013 Bent Bisballe Nyeng
@@ -57,22 +57,22 @@ public:
     { // Encode
       struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
       
-      struct lrtp_profile_t *profile =
-        lrtp_create_profile(lrtp, PROFILE_RAW, csrc,
-                            OPTION_RAW_PKG_SIZE, pkg_size,
-                            OPTION_END);
-      
+      int x = lrtp_create_profile(lrtp, PROFILE_RAW, csrc,
+                                  OPTION_RAW_PKG_SIZE, pkg_size,
+                                  OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
+
       char num = 0;
 
       char packet[16*1024];
-      size_t size = sizeof(packet);
+      //size_t size = sizeof(packet);
       
       for(unsigned int ts = 0; ts < 8; ts++) {
 
-        for(int i = 0; i < sizeof(frame); i++) frame[i] = num++;
+        for(size_t i = 0; i < sizeof(frame); i++) frame[i] = num++;
 
         int ret = 0;
-        ret = lrtp_enqueue_frame(profile, frame, sizeof(frame));
+        ret = lrtp_enqueue_frame(lrtp, csrc, frame, sizeof(frame), ts);
         while( (ret = lrtp_pack(lrtp, packet, sizeof(packet))) != 0) {
           std::string p;
           p.append(packet, ret);
@@ -89,15 +89,15 @@ public:
     { // Decode
       struct lrtp_t *lrtp = lrtp_init(KEY, SSRC);
       
-      struct lrtp_profile_t *profile =
-        lrtp_create_profile(lrtp, PROFILE_RAW, csrc,
-                            OPTION_RAW_PKG_SIZE, pkg_size,
-                            OPTION_END);
-      
+      int x = lrtp_create_profile(lrtp, PROFILE_RAW, csrc,
+                                  OPTION_RAW_PKG_SIZE, pkg_size,
+                                  OPTION_END);
+      CPPUNIT_ASSERT_EQUAL(0, x);
+
       char frame[16*1024];
       size_t framesize = sizeof(frame);
       
-      int cnt = 0;
+      //int cnt = 0;
       
       char num = 0;
 
diff --git a/test/test_rtp.cc b/test/test_rtp.cc
index 489cd42..4703740 100644
--- a/test/test_rtp.cc
+++ b/test/test_rtp.cc
@@ -105,7 +105,7 @@ public:
     CPPUNIT_ASSERT_EQUAL(sz, sz2);
 
     int err = 0;
-    for(int i = 0; i < sz; i++) {
+    for(size_t i = 0; i < sz; i++) {
       err += abs(buf[i] - buf2[i]);
     }
     CPPUNIT_ASSERT_EQUAL(0, err);
diff --git a/test/test_srtp.cc b/test/test_srtp.cc
index c7b82e5..7c0dc60 100644
--- a/test/test_srtp.cc
+++ b/test/test_srtp.cc
@@ -78,7 +78,7 @@ public:
     CPPUNIT_ASSERT_EQUAL(sz, sz0);
 
     int err = 0;
-    for(int i = 0; i < sz0; i++) {
+    for(size_t i = 0; i < sz0; i++) {
       err += abs(buf0[i] - buf[i]);
     }