diff --git a/sys/rpicamsrc/RaspiCapture.c b/sys/rpicamsrc/RaspiCapture.c
index f8b357643d..489fcd970c 100644
--- a/sys/rpicamsrc/RaspiCapture.c
+++ b/sys/rpicamsrc/RaspiCapture.c
@@ -911,18 +911,48 @@ static void encoder_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buf
 }
 
 GstFlowReturn
-raspi_capture_fill_buffer(RASPIVID_STATE *state, GstBuffer **bufp)
+raspi_capture_fill_buffer(RASPIVID_STATE *state, GstBuffer **bufp,
+    GstClock *clock, GstClockTime base_time)
 {
   GstBuffer *buf;
   MMAL_BUFFER_HEADER_T *buffer;
   GstFlowReturn ret = GST_FLOW_ERROR;
+  /* No timestamps if no clockm or invalid PTS */
+  GstClockTime gst_pts = GST_CLOCK_TIME_NONE;
 
   /* FIXME: Use our own interruptible cond wait: */
   buffer = mmal_queue_wait(state->encoded_buffer_q);
 
+  
+  if (G_LIKELY (clock)) {
+    MMAL_PARAMETER_INT64_T param;
+    GstClockTime runtime;
+
+    runtime = gst_clock_get_time (clock) - base_time;
+
+    param.hdr.id = MMAL_PARAMETER_SYSTEM_TIME;
+    param.hdr.size = sizeof(param);
+    param.value = -1;
+
+    mmal_port_parameter_get(state->encoder_output_port, &param.hdr);
+
+    if (param.value != -1 && param.value >= buffer->pts) {
+      GstClockTime offset = param.value - buffer->pts;
+      if (runtime >= offset)
+        gst_pts = runtime - offset;
+    }
+    GST_LOG ("Buf PTS %" G_GINT64_FORMAT " DTS %" G_GINT64_FORMAT
+        " STC %" G_GINT64_FORMAT " TS %" GST_TIME_FORMAT,
+        buffer->pts, buffer->dts, param.value,
+        GST_TIME_ARGS (gst_pts));
+  }
+
+
   mmal_buffer_header_mem_lock(buffer);
   buf = gst_buffer_new_allocate(NULL, buffer->length, NULL);
   if (buf) {
+    /* FIXME: Can we avoid copies and give MMAL our own buffers to fill? */
+    GST_BUFFER_PTS(buf) = gst_pts;
     gst_buffer_fill(buf, 0, buffer->data, buffer->length);
     ret = GST_FLOW_OK;
   }
@@ -1056,7 +1086,7 @@ raspi_capture_set_format_and_start(RASPIVID_STATE *state)
       .num_preview_video_frames = 3,
       .stills_capture_circular_buffer_height = 0,
       .fast_preview_resume = 0,
-      .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC
+      .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
    };
 
    camera = state->camera_component;
diff --git a/sys/rpicamsrc/RaspiCapture.h b/sys/rpicamsrc/RaspiCapture.h
index f4f0bf9048..5a72195a2e 100644
--- a/sys/rpicamsrc/RaspiCapture.h
+++ b/sys/rpicamsrc/RaspiCapture.h
@@ -101,7 +101,8 @@ void raspicapture_init();
 void raspicapture_default_config(RASPIVID_CONFIG *config);
 RASPIVID_STATE *raspi_capture_setup(RASPIVID_CONFIG *config);
 gboolean raspi_capture_start(RASPIVID_STATE *state);
-GstFlowReturn raspi_capture_fill_buffer(RASPIVID_STATE *state, GstBuffer **buf);
+GstFlowReturn raspi_capture_fill_buffer(RASPIVID_STATE *state, GstBuffer **buf,
+    GstClock *clock, GstClockTime base_time);
 void raspi_capture_stop(RASPIVID_STATE *state);
 void raspi_capture_free(RASPIVID_STATE *state);
 gboolean raspi_capture_request_i_frame(RASPIVID_STATE *state);
diff --git a/sys/rpicamsrc/gstrpicamsrc.c b/sys/rpicamsrc/gstrpicamsrc.c
index cf5c6bdc05..4a6afe08f8 100644
--- a/sys/rpicamsrc/gstrpicamsrc.c
+++ b/sys/rpicamsrc/gstrpicamsrc.c
@@ -435,8 +435,9 @@ gst_rpi_cam_src_init (GstRpiCamSrc * src)
   raspicapture_default_config (&src->capture_config);
   src->capture_config.intraperiod = KEYFRAME_INTERVAL_DEFAULT;
   src->capture_config.verbose = 1;
-  /* do-timestamping by default for now. FIXME: Implement proper timestamping */
-  gst_base_src_set_do_timestamp (GST_BASE_SRC (src), TRUE);
+  /* Don't let basesrc set timestamps, we'll do it using
+   * buffer PTS and system times */
+  gst_base_src_set_do_timestamp (GST_BASE_SRC (src), FALSE);
 }
 
 static void
@@ -862,17 +863,29 @@ gst_rpi_cam_src_create (GstPushSrc * parent, GstBuffer ** buf)
 {
   GstRpiCamSrc *src = GST_RPICAMSRC (parent);
   GstFlowReturn ret;
+  GstClock *clock = NULL;
+  GstClockTime base_time;
+
   if (!src->started) {
     if (!raspi_capture_start (src->capture_state))
       return GST_FLOW_ERROR;
     src->started = TRUE;
   }
 
+  GST_OBJECT_LOCK (src);
+  if ((clock = GST_ELEMENT_CLOCK (src)) != NULL)
+    gst_object_ref (clock);
+  base_time = GST_ELEMENT_CAST (src)->base_time;
+  GST_OBJECT_UNLOCK (src);
+
   /* FIXME: Use custom allocator */
-  ret = raspi_capture_fill_buffer (src->capture_state, buf);
+  ret = raspi_capture_fill_buffer (src->capture_state, buf, clock, base_time);
   if (*buf)
     GST_LOG_OBJECT (src, "Made buffer of size %" G_GSIZE_FORMAT,
         gst_buffer_get_size (*buf));
+
+  if (clock)
+    gst_object_unref (clock);
   return ret;
 }