WIP: Write aligned frames to block device

src/Makefile

@@ -11,7 +11,7 @@ IMAGES     = $(GUIDIR)/images/filebrowser-01.gif $(GUIDIR)/images/filebrowser-bo
              $(GUIDIR)/images/rec_folder.png $(GUIDIR)/images/up_folder.gif $(GUIDIR)/images/play_audio.png $(GUIDIR)/images/hdd.png
 
 
-SRCS = camogm.c camogm_ogm.c camogm_jpeg.c camogm_mov.c camogm_kml.c camogm_read.c index_list.c
+SRCS = camogm.c camogm_ogm.c camogm_jpeg.c camogm_mov.c camogm_kml.c camogm_read.c index_list.c camogm_align.c
 TEST_SRC = camogm_test.c
 OBJS = $(SRCS:.c=.o)
 

src/camogm.c

@@ -514,8 +514,7 @@ int sendImageFrame(camogm_state *state)
 	int port = state->port_num;
 	struct timeval start_time, end_time;
 
-	D6(fprintf(debug_file, "last_error_code = %d\n", state->last_error_code));
-	start_time = get_fpga_time(state->fd_fparmsall[port], port);
+//	start_time = get_fpga_time(state->fd_fparmsall[port], port);
 
 // This is probably needed only for Quicktime (not to exceed already allocated frame index)
 	if (!state->rawdev_op && (state->frameno >= (state->max_frames))) {
@@ -646,12 +645,14 @@ int sendImageFrame(camogm_state *state)
 /* copy from the beginning of the buffer to the end of the frame */
 		state->packetchunks[state->chunk_index  ].bytes = state->jpeg_len - (state->circ_buff_size[port] - state->cirbuf_rp[port]);
 		state->packetchunks[state->chunk_index++].chunk = (unsigned char*)&ccam_dma_buf[state->port_num][0];
+		state->writer_params.segments = 2;
 	} else { // single segment
 		D3(fprintf(debug_file, "_11_"));
 
 /* copy from the beginning of the frame to the end of the frame (no buffer rollovers) */
 		state->packetchunks[state->chunk_index  ].bytes = state->jpeg_len;
 		state->packetchunks[state->chunk_index++].chunk = (unsigned char*)&ccam_dma_buf[state->port_num][state->cirbuf_rp[port] >> 2];
+		state->writer_params.segments = 1;
 	}
 	D3(fprintf(debug_file, "\tcirbuf_rp = 0x%x\t", state->cirbuf_rp[port]));
 	D3(fprintf(debug_file, "_12_"));
@@ -687,16 +688,16 @@ int sendImageFrame(camogm_state *state)
 	}
 	D3(fprintf(debug_file,"cirbuf_rp to next frame = 0x%x\n", state->cirbuf_rp[port]));
 
-	end_time = get_fpga_time(state->fd_fparmsall[port], port);
-	unsigned int mbps; // write speed, MB/s
-	unsigned long long time_diff; // time elapsed, in microseconds
-	time_diff = ((end_time.tv_sec * 1000000 + end_time.tv_usec) - (start_time.tv_sec * 1000000 + start_time.tv_usec));
-	mbps = ((double)state->rawdev.last_jpeg_size / (double)1048576) / ((double)time_diff / (double)1000000);
-	D6(fprintf(debug_file, "Frame start time: %ld:%ld; frame end time: %ld:%ld; last frame size: %lu\n",
-			start_time.tv_sec, start_time.tv_usec,
-			end_time.tv_sec, end_time.tv_usec,
-			state->rawdev.last_jpeg_size));
-	D6(fprintf(debug_file, "Write speed: %d MB/s\n", mbps));
+//	end_time = get_fpga_time(state->fd_fparmsall[port], port);
+//	unsigned int mbps; // write speed, MB/s
+//	unsigned long long time_diff; // time elapsed, in microseconds
+//	time_diff = ((end_time.tv_sec * 1000000 + end_time.tv_usec) - (start_time.tv_sec * 1000000 + start_time.tv_usec));
+//	mbps = ((double)state->rawdev.last_jpeg_size / (double)1048576) / ((double)time_diff / (double)1000000);
+//	D6(fprintf(debug_file, "Frame start time: %ld:%ld; frame end time: %ld:%ld; last frame size: %lu\n",
+//			start_time.tv_sec, start_time.tv_usec,
+//			end_time.tv_sec, end_time.tv_usec,
+//			state->rawdev.last_jpeg_size));
+//	D6(fprintf(debug_file, "Write speed: %d MB/s\n", mbps));
 
 	return 0;
 }
@@ -880,6 +881,9 @@ void get_disk_info(camogm_state *state)
 	}
 
 	if (get_disk_range(state->rawdev.rawdev_path, &rng) == 0) {
+		state->writer_params.lba_start = rng.from;
+		state->writer_params.lba_end = rng.to;
+		state->writer_params.lba_current = state->writer_params.lba_start;
 		set_disk_range(&rng);
 	} else {
 		D0(fprintf(debug_file, "ERROR: unable to get disk size and starting sector\n"));

src/camogm.h

@@ -27,7 +27,6 @@
 #include <elphel/c313a.h>
 #include <elphel/x393_devices.h>
 
-
 #define CAMOGM_FRAME_NOT_READY    1        ///< frame pointer valid, but not yet acquired
 #define CAMOGM_FRAME_INVALID      2        ///< invalid frame pointer
 #define CAMOGM_FRAME_CHANGED      3        ///< frame parameters have changed
@@ -150,6 +149,16 @@ struct writer_params {
 	int last_ret_val;                                       ///< error value return during last frame recording (if any occurred)
 	bool exit_thread;                                       ///< flag indicating that the writing thread should terminate
 	int state;                                              ///< the state of disk writing thread
+	int segments;                                           ///< the number of segments in frame
+
+	struct iovec *data_chunks;                              ///< a set of vectors pointing to aligned frame data buffers
+	struct iovec prev_rem_vect;                             ///< vector pointing to the remainder of the previous frame
+	unsigned char *rem_buff;                                ///< buffer containing the unaligned remainder of the current frame
+	unsigned char *prev_rem_buff;                           ///< buffer containing the unaligned remainder of the previous frame
+	unsigned char *common_buff;                             ///< buffer for aligned JPEG header
+	uint64_t lba_start;                                     ///< disk starting LBA
+	uint64_t lba_current;                                   ///< current write position in LBAs
+	uint64_t lba_end;                                       ///< disk last LBA
 };
 /**
  * @struct camogm_state

src/camogm_align.c

+/** @file camogm_align.c
+ * @brief Provides frame alignment functions use for recording to block device.
+ * @copyright Copyright (C) 2017 Elphel, Inc.
+ *
+ * @par <b>License</b>
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *  This program 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 General Public License for more details.
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/** @brief This define is needed to use lseek64 and should be set before includes */
+#define _LARGEFILE64_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <string.h>
+#include <sys/uio.h>
+
+#include "camogm_align.h"
+
+static unsigned char app15[ALIGNMENT_SIZE] = {0xff, 0xef};
+
+static inline size_t get_size_from(const struct iovec *vects, int index, size_t offset, int all);
+static inline size_t align_bytes_num(size_t data_len, size_t align_len);
+static inline void vectcpy(struct iovec *dest, void *src, size_t len);
+static inline void vectshrink(struct iovec *vec, size_t len);
+static inline unsigned char *vectrpos(struct iovec *vec, size_t offset);
+static void dev_dbg(const char *prefix, const char *format, ...);
+static void remap_vectors(camogm_state *state, struct iovec *chunks);
+static size_t get_blocks_num(struct iovec *sgl, size_t n_elem);
+/* debug functions */
+static int check_chunks(struct iovec *vects);
+
+/** Replace debug function with the same name from driver code with debug macro to reduce changes */
+static void dev_dbg(const char *prefix, const char *format, ...)
+{
+	va_list args;
+
+	va_start(args, format);
+	D6(vfprintf(debug_file, format, args));
+	va_end(args);
+}
+
+/** Copy @e len bytes from buffer pointed by @e src vector to buffer pointed by @e dest vector */
+static inline void vectcpy(struct iovec *dest, void *src, size_t len)
+{
+	unsigned char *d = (unsigned char *)dest->iov_base;
+
+	memcpy(d + dest->iov_len, src, len);
+	dest->iov_len += len;
+}
+
+/** Shrink vector length by @len bytes */
+static inline void vectshrink(struct iovec *vec, size_t len)
+{
+	if (vec->iov_len >= len) {
+		vec->iov_len -= len;
+	}
+}
+
+/** This helper function is used to position a pointer @e offset bytes from the end
+ * of a buffer. */
+static inline unsigned char *vectrpos(struct iovec *vec, size_t offset)
+{
+	return (unsigned char *)vec->iov_base + (vec->iov_len - offset);
+}
+
+/** Calculate the size of current frame in bytes starting from vector and offset given */
+static inline size_t get_size_from(const struct iovec *vects, int index, size_t offset, int all)
+{
+	int i;
+	size_t total = 0;
+
+	if (index >= MAX_DATA_CHUNKS || offset > vects[index].iov_len) {
+		return 0;
+	}
+
+	for (i = index; i < MAX_DATA_CHUNKS; i++) {
+		if (i == CHUNK_REM && all == EXCLUDE_REM)
+			/* remainder should not be processed */
+			continue;
+		if (i == index)
+			total += vects[i].iov_len - offset;
+		else
+			total += vects[i].iov_len;
+	}
+
+	return total;
+}
+
+/** Return the number of bytes needed to align @e data_len to @e align_len boundary */
+static inline size_t align_bytes_num(size_t data_len, size_t align_len)
+{
+	size_t rem = data_len % align_len;
+	if (rem == 0)
+		return 0;
+	else
+		return align_len - rem;
+}
+
+static void remap_vectors(camogm_state *state, struct iovec *chunks)
+{
+	int chunk_index = 1;
+
+	if (state->exif > 0) {
+		chunks[CHUNK_LEADER].iov_base = (void *)state->packetchunks[chunk_index].chunk;
+		chunks[CHUNK_LEADER].iov_len = state->packetchunks[chunk_index++].bytes;
+		chunks[CHUNK_EXIF].iov_base = (void *)state->packetchunks[chunk_index].chunk;
+		chunks[CHUNK_EXIF].iov_len = state->packetchunks[chunk_index++].bytes;
+		chunks[CHUNK_HEADER].iov_base = (void *)state->packetchunks[chunk_index].chunk;
+		chunks[CHUNK_HEADER].iov_len = state->packetchunks[chunk_index++].bytes;
+	} else {
+		chunks[CHUNK_LEADER].iov_base = (void *)state->packetchunks[chunk_index].chunk;
+		chunks[CHUNK_LEADER].iov_len = JPEG_MARKER_LEN;
+		chunks[CHUNK_HEADER].iov_base = (void *)(state->packetchunks[chunk_index].chunk + JPEG_MARKER_LEN);
+		chunks[CHUNK_HEADER].iov_len = state->packetchunks[chunk_index++].bytes - JPEG_MARKER_LEN;
+	}
+	chunks[CHUNK_DATA_0].iov_base = (void *)state->packetchunks[chunk_index].chunk;
+	chunks[CHUNK_DATA_0].iov_len = state->packetchunks[chunk_index++].bytes;
+	if (state->writer_params.segments == 2) {
+		chunks[CHUNK_DATA_0].iov_base = (void *)state->packetchunks[chunk_index].chunk;
+		chunks[CHUNK_DATA_0].iov_len = state->packetchunks[chunk_index++].bytes;
+	}
+	chunks[CHUNK_TRAILER].iov_base = (void *)state->packetchunks[chunk_index].chunk;
+	chunks[CHUNK_TRAILER].iov_len = state->packetchunks[chunk_index].bytes;
+
+	/* some data may be left from previous frame, copy it to special buffer */
+	if (chunks[CHUNK_REM].iov_len != 0) {
+		vectcpy(&state->writer_params.prev_rem_vect, chunks[CHUNK_REM].iov_base, chunks[CHUNK_REM].iov_len);
+		vectshrink(&chunks[CHUNK_REM], chunks[CHUNK_REM].iov_len);
+	}
+}
+
+/** Debug function, checks frame alignment */
+static int check_chunks(struct iovec *vects)
+{
+	int i;
+	int ret = 0;
+	size_t sz = 0;
+	for (i = 0; i < MAX_DATA_CHUNKS; i++) {
+		if (i != CHUNK_REM) {
+			sz += vects[i].iov_len;
+			if ((vects[i].iov_len % ALIGNMENT_SIZE) != 0) {
+				dev_dbg(NULL, "ERROR: unaligned write from slot %d, length %u\n", i, vects[i].iov_len);
+				ret = -1;
+			}
+		}
+		dev_dbg(NULL, "chunk[%d]: ptr = %p, size = %d\n", i, vects[i].iov_base, vects[i].iov_len);
+	}
+	if ((sz % PHY_BLOCK_SIZE) != 0) {
+		dev_dbg(NULL, "ERROR: total length of the transaction is not aligned to sector boundary, total length %u\n", sz);
+		ret = -1;
+	} else {
+		dev_dbg(NULL, "===== frame is OK =====\n");
+	}
+	return ret;
+}
+
+/** Calculate the number of blocks this frame will occupy. The frame must be aligned to block size */
+static size_t get_blocks_num(struct iovec *sgl, size_t n_elem)
+{
+	int num;
+	size_t total = 0;
+
+	for (num = 0; num < n_elem; num++) {
+		total += sgl[num].iov_len;
+	}
+
+	return total / PHY_BLOCK_SIZE;
+}
+
+int init_align_buffers(camogm_state *state)
+{
+	state->writer_params.data_chunks = (struct iovec *)malloc(MAX_DATA_CHUNKS * sizeof(struct iovec));
+	if (state->writer_params.data_chunks == NULL) {
+		return -1;
+	}
+	state->writer_params.common_buff = (unsigned char *)malloc(COMMON_BUFF_SZ);
+	if (state->writer_params.common_buff == NULL) {
+		deinit_align_buffers(state);
+		return -1;
+	}
+	state->writer_params.rem_buff = (unsigned char *)malloc(REM_BUFF_SZ);
+	if (state->writer_params.rem_buff == NULL) {
+		deinit_align_buffers(state);
+		return -1;
+	}
+	state->writer_params.prev_rem_buff = (unsigned char *)malloc(REM_BUFF_SZ);
+	if (state->writer_params.prev_rem_buff == NULL) {
+		deinit_align_buffers(state);
+		return -1;
+	}
+
+	state->writer_params.data_chunks[CHUNK_COMMON].iov_base = (void *)state->writer_params.common_buff;
+	state->writer_params.data_chunks[CHUNK_COMMON].iov_len = 0;
+
+	state->writer_params.data_chunks[CHUNK_REM].iov_base = (void *)state->writer_params.rem_buff;
+	state->writer_params.data_chunks[CHUNK_REM].iov_len = 0;
+
+	state->writer_params.prev_rem_vect.iov_base = (void *)state->writer_params.prev_rem_buff;
+	state->writer_params.prev_rem_vect.iov_len = 0;
+
+	return 0;
+}
+
+void deinit_align_buffers(camogm_state *state)
+{
+	struct writer_params *params = &state->writer_params;
+
+	if (params->data_chunks) {
+		free(params->data_chunks);
+		params->data_chunks = NULL;
+	}
+	if (params->common_buff) {
+		free(params->common_buff);
+		params->common_buff = NULL;
+	}
+	if (params->rem_buff) {
+		free(params->rem_buff);
+		params->rem_buff = NULL;
+	}
+	if (params->prev_rem_buff) {
+		free(params->prev_rem_buff);
+		params->prev_rem_buff = NULL;
+	}
+}
+
+/** Align current frame to disk sector boundary and each individual buffer to #ALIGNMENT_SIZE boundary */
+void align_frame(camogm_state *state)
+{
+	const char *dev = NULL;
+	unsigned char *src;
+	size_t len, total_sz, data_len;
+	struct iovec *chunks = state->writer_params.data_chunks;
+	struct iovec *cbuff = &chunks[CHUNK_COMMON];
+	struct iovec *rbuff = &state->writer_params.prev_rem_vect;
+
+	remap_vectors(state, chunks);
+
+	total_sz = get_size_from(chunks, 0, 0, INCLUDE_REM) + rbuff->iov_len;
+	if (total_sz < PHY_BLOCK_SIZE) {
+		/* the frame length is less than sector size, delay this frame */
+		if (rbuff->iov_len != 0) {
+			/* some data may be left from previous frame */
+			vectcpy(&chunks[CHUNK_REM], rbuff->iov_base, rbuff->iov_len);
+			vectshrink(rbuff, rbuff->iov_len);
+		}
+		dev_dbg(dev, "frame size is less than sector size: %u bytes; delay recording\n", total_sz);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_LEADER].iov_base, chunks[CHUNK_LEADER].iov_len);
+		vectshrink(&chunks[CHUNK_LEADER], chunks[CHUNK_LEADER].iov_len);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_EXIF].iov_base, chunks[CHUNK_EXIF].iov_len);
+		vectshrink(&chunks[CHUNK_EXIF], chunks[CHUNK_EXIF].iov_len);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_HEADER].iov_base, chunks[CHUNK_HEADER].iov_len);
+		vectshrink(&chunks[CHUNK_HEADER], chunks[CHUNK_HEADER].iov_len);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_DATA_0].iov_base, chunks[CHUNK_DATA_0].iov_len);
+		vectshrink(&chunks[CHUNK_DATA_0], chunks[CHUNK_DATA_0].iov_len);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_DATA_1].iov_base, chunks[CHUNK_DATA_1].iov_len);
+		vectshrink(&chunks[CHUNK_DATA_1], chunks[CHUNK_DATA_1].iov_len);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+		vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+		return;
+	}
+
+	/* copy remainder of previous frame to the beginning of common buffer */
+	if (rbuff->iov_len != 0) {
+		len = rbuff->iov_len;
+		dev_dbg(dev, "copy %u bytes from REM to common buffer\n", len);
+		vectcpy(cbuff, rbuff->iov_base, len);
+		vectshrink(rbuff, rbuff->iov_len);
+	}
+
+	/* copy JPEG marker */
+	len = chunks[CHUNK_LEADER].iov_len;
+	vectcpy(cbuff, chunks[CHUNK_LEADER].iov_base, len);
+	vectshrink(&chunks[CHUNK_LEADER], chunks[CHUNK_LEADER].iov_len);
+
+	/* copy Exif if present */
+	if (chunks[CHUNK_EXIF].iov_len != 0) {
+		len = chunks[CHUNK_EXIF].iov_len;
+		dev_dbg(dev, "copy %u bytes from EXIF to common buffer\n", len);
+		vectcpy(cbuff, chunks[CHUNK_EXIF].iov_base, len);
+		vectshrink(&chunks[CHUNK_EXIF], chunks[CHUNK_EXIF].iov_len);
+	}
+
+	/* align common buffer to ALIGNMENT boundary, APP15 marker should be placed before header data */
+	data_len = cbuff->iov_len + chunks[CHUNK_HEADER].iov_len;
+	len = align_bytes_num(data_len, ALIGNMENT_SIZE);
+	if (len < (JPEG_MARKER_LEN + JPEG_SIZE_LEN) && len != 0) {
+		/* the number of bytes needed for alignment is less than the length of the marker itself, increase the number of stuffing bytes */
+		len += ALIGNMENT_SIZE;
+	}
+	dev_dbg(dev, "total number of stuffing bytes in APP15 marker: %u\n", len);
+	app15[3] = len - JPEG_MARKER_LEN;
+	vectcpy(cbuff, app15, len);
+
+	/* copy JPEG header */
+	len = chunks[CHUNK_HEADER].iov_len;
+	dev_dbg(dev, "copy %u bytes from HEADER to common buffer\n", len);
+	vectcpy(cbuff, chunks[CHUNK_HEADER].iov_base, len);
+	vectshrink(&chunks[CHUNK_HEADER], chunks[CHUNK_HEADER].iov_len);
+
+	/* check if there is enough data to continue - JPEG data length can be too short */
+	len = get_size_from(chunks, CHUNK_DATA_0, 0, EXCLUDE_REM);
+	if (len < PHY_BLOCK_SIZE) {
+		size_t num = align_bytes_num(cbuff->iov_len, PHY_BLOCK_SIZE);
+		dev_dbg(dev, "jpeg data is too short, delay this frame\n");
+		if (len >= num) {
+			/* there is enough data to align common buffer to sector boundary */
+			if (num >= chunks[CHUNK_DATA_0].iov_len) {
+				vectcpy(cbuff, chunks[CHUNK_DATA_0].iov_base, chunks[CHUNK_DATA_0].iov_len);
+				num -= chunks[CHUNK_DATA_0].iov_len;
+				vectshrink(&chunks[CHUNK_DATA_0], chunks[CHUNK_DATA_0].iov_len);
+			} else {
+				src = vectrpos(&chunks[CHUNK_DATA_0], num);
+				vectcpy(cbuff, chunks[CHUNK_DATA_0].iov_base, num);
+				vectshrink(&chunks[CHUNK_DATA_0], num);
+				num = 0;
+			}
+			if (num >= chunks[CHUNK_DATA_1].iov_len) {
+				vectcpy(cbuff, chunks[CHUNK_DATA_1].iov_base, chunks[CHUNK_DATA_1].iov_len);
+				num -= chunks[CHUNK_DATA_1].iov_len;
+				vectshrink(&chunks[CHUNK_DATA_1], chunks[CHUNK_DATA_1].iov_len);
+			} else {
+				src = vectrpos(&chunks[CHUNK_DATA_1], num);
+				vectcpy(cbuff, chunks[CHUNK_DATA_1].iov_base, num);
+				vectshrink(&chunks[CHUNK_DATA_1], num);
+				num = 0;
+			}
+			if (num >= chunks[CHUNK_TRAILER].iov_len) {
+				vectcpy(cbuff, chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+				num -= chunks[CHUNK_TRAILER].iov_len;
+				vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+			} else {
+				src = vectrpos(&chunks[CHUNK_TRAILER], num);
+				vectcpy(cbuff, chunks[CHUNK_TRAILER].iov_base, num);
+				vectshrink(&chunks[CHUNK_TRAILER], num);
+				num = 0;
+			}
+		} else {
+			/* there is not enough data to align common buffer to sector boundary, truncate common buffer */
+			data_len = cbuff->iov_len % PHY_BLOCK_SIZE;
+			src = vectrpos(cbuff, data_len);
+			vectcpy(&chunks[CHUNK_REM], src, data_len);
+			vectshrink(cbuff, data_len);
+		}
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_DATA_0].iov_base, chunks[CHUNK_DATA_0].iov_len);
+		vectshrink(&chunks[CHUNK_DATA_0], chunks[CHUNK_DATA_0].iov_len);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_DATA_1].iov_base, chunks[CHUNK_DATA_1].iov_len);
+		vectshrink(&chunks[CHUNK_DATA_1], chunks[CHUNK_DATA_1].iov_len);
+		vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+		vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+
+		return;
+	}
+
+	/* align frame to sector size boundary; total size could have changed by the moment - recalculate */
+	total_sz = get_size_from(chunks, 0, 0, INCLUDE_REM);
+	len = total_sz % PHY_BLOCK_SIZE;
+	dev_dbg(dev, "number of bytes crossing sector boundary: %u\n", len);
+	if (len != 0) {
+		if (len >= (chunks[CHUNK_DATA_1].iov_len + chunks[CHUNK_TRAILER].iov_len)) {
+			/* current frame is not split or the second part of JPEG data is too short */
+			data_len = len - chunks[CHUNK_DATA_1].iov_len - chunks[CHUNK_TRAILER].iov_len;
+			src = vectrpos(&chunks[CHUNK_DATA_0], data_len);
+			vectcpy(&chunks[CHUNK_REM], src, data_len);
+			vectshrink(&chunks[CHUNK_DATA_0], data_len);
+			vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_DATA_1].iov_base, chunks[CHUNK_DATA_1].iov_len);
+			vectshrink(&chunks[CHUNK_DATA_1], chunks[CHUNK_DATA_1].iov_len);
+			vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+			vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+		} else if (len >= chunks[CHUNK_TRAILER].iov_len) {
+			/* there is enough data in second part to align the frame */
+			data_len = len - chunks[CHUNK_TRAILER].iov_len;
+			src = vectrpos(&chunks[CHUNK_DATA_1], data_len);
+			vectcpy(&chunks[CHUNK_REM], src, data_len);
+			vectshrink(&chunks[CHUNK_DATA_1], data_len);
+			vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+			vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+		} else {
+			/* the trailing marker is split by sector boundary, copy (PHY_BLOCK_SIZE - 1) bytes from
+			 * JPEG data block(s) to remainder buffer and then add trailing marker */
+			data_len = PHY_BLOCK_SIZE - (chunks[CHUNK_TRAILER].iov_len - len);
+			if (data_len >= chunks[CHUNK_DATA_1].iov_len) {
+				size_t cut_len = data_len - chunks[CHUNK_DATA_1].iov_len;
+				src = vectrpos(&chunks[CHUNK_DATA_0], cut_len);
+				vectcpy(&chunks[CHUNK_REM], src, cut_len);
+				vectshrink(&chunks[CHUNK_DATA_0], cut_len);
+				vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_DATA_1].iov_base, chunks[CHUNK_DATA_1].iov_len);
+				vectshrink(&chunks[CHUNK_DATA_1], chunks[CHUNK_DATA_1].iov_len);
+				vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+				vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+			} else {
+				src = vectrpos(&chunks[CHUNK_DATA_1], data_len);
+				vectcpy(&chunks[CHUNK_REM], src, data_len);
+				vectshrink(&chunks[CHUNK_DATA_1], data_len);
+				vectcpy(&chunks[CHUNK_REM], chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+				vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+			}
+		}
+	} else {
+		/* the frame is aligned to sector boundary but some buffers may be not */
+		chunks[CHUNK_ALIGN].iov_base = vectrpos(cbuff, 0);
+//		chunks[CHUNK_ALIGN].iov_dma = cbuff->iov_dma + cbuff->iov_len;
+		chunks[CHUNK_ALIGN].iov_len = 0;
+		if (chunks[CHUNK_DATA_1].iov_len == 0) {
+			data_len = chunks[CHUNK_DATA_0].iov_len % ALIGNMENT_SIZE;
+			src = vectrpos(&chunks[CHUNK_DATA_0], data_len);
+			vectcpy(&chunks[CHUNK_ALIGN], src, data_len);
+			vectshrink(&chunks[CHUNK_DATA_0], data_len);
+		} else {
+			data_len = chunks[CHUNK_DATA_1].iov_len % ALIGNMENT_SIZE;
+			src = vectrpos(&chunks[CHUNK_DATA_1], data_len);
+			vectcpy(&chunks[CHUNK_ALIGN], src, data_len);
+			vectshrink(&chunks[CHUNK_DATA_1], data_len);
+		}
+		vectcpy(&chunks[CHUNK_ALIGN], chunks[CHUNK_TRAILER].iov_base, chunks[CHUNK_TRAILER].iov_len);
+		vectshrink(&chunks[CHUNK_TRAILER], chunks[CHUNK_TRAILER].iov_len);
+	}
+
+	/* debug sanity check, should not happen */
+	if (cbuff->iov_len >= COMMON_BUFF_SZ) {
+		dev_dbg(NULL, "ERROR: the number of bytes copied to common buffer exceeds its size\n");
+	}
+	check_chunks(chunks);
+}
+
+/** Discard buffer pointers which makes the command slot marked as empty */
+void reset_chunks(struct iovec *vects, int all)
+{
+	int i;
+
+	for (i = 0; i < MAX_DATA_CHUNKS; i++) {
+		if (i != CHUNK_REM)
+			vects[i].iov_len = 0;
+	}
+	if (all) {
+		vects[CHUNK_REM].iov_len = 0;
+	}
+}
+
+/** Calculate and update LBA offsets, do not count remainder buffer. Return 1 if file position should be reset to the start */
+int update_lba(camogm_state *state)
+{
+	int ret = 0;
+	size_t total_sz;
+	struct iovec *chunks = state->writer_params.data_chunks;
+
+	total_sz = get_blocks_num(chunks, MAX_DATA_CHUNKS - 1);
+	if (state->writer_params.lba_current + total_sz <= state->writer_params.lba_end) {
+		state->writer_params.lba_current += total_sz;
+	} else {
+		state->writer_params.lba_current = state->writer_params.lba_start;
+		ret = 1;
+	}
+
+	return ret;
+}
+
+/** Go through all data buffers and pick only mapped ones excluding remainder buffer */
+int get_data_buffers(camogm_state *state, struct iovec *mapped, size_t mapped_sz)
+{
+	int ret = 0;
+	struct iovec *all = state->writer_params.data_chunks;
+
+	if (mapped_sz <= 0)
+		return ret;
+
+	for (int i = 0, j = 0; i < MAX_DATA_CHUNKS; i++) {
+		if (i != CHUNK_REM && all[i].iov_len != 0) {
+			if (j < mapped_sz) {
+				mapped[j++] = all[i];
+				ret = j;
+			} else {
+				ret = -1;
+				break;
+			}
+		}
+	}
+
+	return ret;
+}
+
+/** Prepare the last remaining block of data for recording, return the number of bytes ready for recording */
+int prep_last_block(camogm_state *state)
+{
+	int ret = 0;
+	size_t stuff_len;
+	unsigned char *src;
+	struct iovec *cvect = &state->writer_params.data_chunks[CHUNK_COMMON];
+	struct iovec *rvect = &state->writer_params.data_chunks[CHUNK_REM];
+
+	if (rvect->iov_len != 0) {
+		stuff_len = PHY_BLOCK_SIZE - rvect->iov_len;
+		src = vectrpos(rvect, 0);
+		memset(src, 0, stuff_len);
+		rvect->iov_len += stuff_len;
+		ret = rvect->iov_len;
+		vectcpy(cvect, rvect->iov_base, rvect->iov_len);
+		vectshrink(rvect, rvect->iov_len);
+	}
+
+	return ret;
+}
+
+off64_t lba_to_offset(uint64_t lba)
+{
+	return lba * PHY_BLOCK_SIZE;
+}

src/camogm_align.h

+/** @file camogm_align.h
+ * @brief Provides frame alignment functions use for recording to block device.
+ * @copyright Copyright (C) 2017 Elphel, Inc.
+ *
+ * @par <b>License</b>
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *  This program 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 General Public License for more details.
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _CAMOGM_ALIGN_H
+#define _CAMOGM_ALIGN_H
+
+#include <unistd.h>
+#include <sys/types.h>
+
+#include "camogm.h"
+
+#define PHY_BLOCK_SIZE            512            ///< Physical disk block size
+#define JPEG_MARKER_LEN           2              ///< The size in bytes of JPEG marker
+#define JPEG_SIZE_LEN             2              ///< The size in bytes of JPEG marker length field
+#define INCLUDE_REM               1              ///< Include REM buffer to total size calculation
+#define EXCLUDE_REM               0              ///< Exclude REM buffer from total size calculation
+#define MAX_DATA_CHUNKS           9              ///< An array or JPEG frame chunks contains pointers to JPEG leading marker,
+                                                 ///< JPEG header, Exif data if present, stuffing bytes chunk which aligns
+                                                 ///< the frame size to disk sector boundary, JPEG data which
+                                                 ///< can be split into two chunks, align buffers, JPEG
+                                                 ///< trailing marker, and pointer to a buffer containing the remainder of a
+                                                 ///< frame. Nine chunks of data in total.
+#define ALIGNMENT_SIZE            32             ///< Align buffers length to this amount of bytes
+/** Common buffer should be large enough to contain JPEG header, Exif, some alignment bytes and remainder from previous frame */
+#define COMMON_BUFF_SZ            MAX_EXIF_SIZE + JPEG_HEADER_MAXSIZE + ALIGNMENT_SIZE + 2 * PHY_BLOCK_SIZE
+#define REM_BUFF_SZ               2 * PHY_BLOCK_SIZE
+
+///** This structure holds raw device buffer pointers */
+//struct drv_pointers {
+//	uint64_t lba_start;                          ///< raw buffer starting LBA
+//	uint64_t lba_end;                            ///< raw buffer ending LBA
+//	uint64_t lba_write;                          ///< current write pointer inside raw buffer
+//	uint16_t wr_count;                           ///< the number of LBA to write next time
+//};
+
+/** Container structure for frame buffers */
+//struct frame_buffers {
+//	struct fvec exif_buff;                       ///< Exif buffer
+//	struct fvec jpheader_buff;                   ///< JPEG header buffer
+//	struct fvec trailer_buff;                    ///< buffer for trailing marker
+//	struct fvec common_buff;                     ///< common buffer where other parts are combined
+//	struct fvec rem_buff;                        ///< remainder from previous frame
+//};
+
+/** Symbolic names for slots in buffer pointers. Buffer alignment function relies on the order of these names, so
+ * new names can be added but the overall order should not be changed */
+enum {
+	CHUNK_LEADER,                                ///< pointer to JPEG leading marker
+	CHUNK_EXIF,                                  ///< pointer to Exif buffer
+	CHUNK_HEADER,                                ///< pointer to JPEG header data excluding leading marker
+	CHUNK_COMMON,                                ///< pointer to common buffer
+	CHUNK_DATA_0,                                ///< pointer to JPEG data
+	CHUNK_DATA_1,                                ///< pointer to the second half of JPEG data if a frame crosses circbuf boundary
+	CHUNK_TRAILER,                               ///< pointer to JPEG trailing marker
+	CHUNK_ALIGN,                                 ///< pointer to buffer where the second part of JPEG data should be aligned
+	CHUNK_REM                                    ///< pointer to buffer containing the remainder of current frame. It will be recorded during next transaction
+};
+
+int init_align_buffers(camogm_state *state);
+void deinit_align_buffers(camogm_state *state);
+void align_frame(camogm_state *state);
+void reset_chunks(struct iovec *vects, int all);
+int update_lba(camogm_state *state);
+int get_data_buffers(camogm_state *state, struct iovec *mapped, size_t all_sz);
+int prep_last_block(camogm_state *state);
+off64_t lba_to_offset(uint64_t lba);
+
+#endif /* _CAMOGM_ALIGN_H */

src/camogm_jpeg.c

@@ -32,12 +32,15 @@
 
 #include "camogm_jpeg.h"
 #include "camogm_read.h"
+#include "camogm_align.h"
 
 /** State file record format. It includes device path in /dev, starting, current and ending LBAs */
 #define STATE_FILE_FORMAT         "%s\t%llu\t%llu\t%llu\n"
 
 /* forward declarations */
 static void *jpeg_writer(void *thread_args);
+static int save_state_file(const rawdev_buffer *rawdev, uint64_t current_pos);
+static int open_state_file(const rawdev_buffer *rawdev, uint64_t *current_pos);
 
 /** Get starting and endign LBAs of the partition specified as raw device buffer */
 static int get_disk_range(struct range *range)
@@ -96,12 +99,12 @@ static int find_state(FILE *f, uint64_t *pos, const rawdev_buffer *rawdev)
 }
 
 /** Read state from file and restore disk write pointer */
-static int open_state_file(const rawdev_buffer *rawdev)
+static int open_state_file(const rawdev_buffer *rawdev, uint64_t *current_pos)
 {
 	int fd, len;
 	FILE *f;
 	int ret = 0;
-	uint64_t curr_pos;
+	uint64_t lba_pos;
 	char buff[SMALL_BUFF_LEN] = {0};
 
 	if (strlen(rawdev->state_path) == 0) {
@@ -110,15 +113,9 @@ static int open_state_file(const rawdev_buffer *rawdev)
 
 	f = fopen(rawdev->state_path, "r");
 	if (f != NULL) {
-		if (find_state(f, &curr_pos, rawdev) != -1) {
-			fd = open(SYSFS_AHCI_LBA_CURRENT, O_WRONLY);
-			if (fd >= 0) {
-				len = snprintf(buff, SMALL_BUFF_LEN, "%llu", curr_pos);
-				write(fd, buff, len + 1);
-				close(fd);
-			} else {
-				ret = -1;
-			}
+		if (find_state(f, &lba_pos, rawdev) != -1) {
+			*current_pos = lba_pos;
+			D0(fprintf(debug_file, "Got starting LBA from state file: %llu\n", lba_pos));
 		}
 		fclose(f);
 	} else {
@@ -129,12 +126,11 @@ static int open_state_file(const rawdev_buffer *rawdev)
 }
 
 /** Save current position of the disk write pointer */
-static int save_state_file(const rawdev_buffer *rawdev)
+static int save_state_file(const rawdev_buffer *rawdev, uint64_t current_pos)
 {
 	int ret = 0;
 	FILE *f;
 	struct range range;
-	uint64_t curr_pos;
 
 	if (strlen(rawdev->state_path) == 0) {
 		return ret;
@@ -143,21 +139,13 @@ static int save_state_file(const rawdev_buffer *rawdev)
 		return -1;
 	}
 
-	// get raw device buffer current postion on disk, this position indicates where recording has stopped
-	f = fopen(SYSFS_AHCI_LBA_CURRENT, "r");
-	if (f == NULL) {
-		return -1;
-	}
-	fscanf(f, "%llu\n", &curr_pos);
-	fclose(f);
-
 	// save pointers to a regular file
 	f = fopen(rawdev->state_path, "w");
 	if (f == NULL) {
 		return -1;
 	}
 	fprintf(f, "Device\t\tStart LBA\tCurrent LBA\tEnd LBA\n");
-	fprintf(f, STATE_FILE_FORMAT, rawdev->rawdev_path, range.from, curr_pos, range.to);
+	fprintf(f, STATE_FILE_FORMAT, rawdev->rawdev_path, range.from, current_pos, range.to);
 	fflush(f);
 	fsync(fileno(f));
 	fclose(f);
@@ -198,6 +186,12 @@ int camogm_init_jpeg(camogm_state *state)
 			D0(fprintf(debug_file, "Can not start writer thread: %s\n", strerror(ret_val)));
 			ret = -1;
 		}
+
+		ret_val = init_align_buffers(state);
+		if (ret_val != 0) {
+			D0(fprintf(debug_file, "Can not initialize alignment buffers\n"));
+			ret = -1;
+		}
 	}
 
 	return ret;
@@ -221,6 +215,8 @@ void camogm_free_jpeg(camogm_state *state)
 	pthread_mutex_unlock(&state->writer_params.writer_mutex);
 	pthread_join(state->writer_params.writer_thread, NULL);
 	state->writer_params.exit_thread = false;
+
+	deinit_align_buffers(state);
 }
 
 /** Calculate the total length of current frame */
@@ -247,6 +243,7 @@ int camogm_start_jpeg(camogm_state *state)
 {
 	char * slash;
 	int rslt;
+	off64_t offset;
 
 	if (!state->rawdev_op) {
 		strcpy(state->path, state->path_prefix);   // make state->path a directory name (will be replaced when the frames will be written)
@@ -264,16 +261,18 @@ int camogm_start_jpeg(camogm_state *state)
 			}
 		}
 	} else {
-//		if (open_state_file(&state->rawdev) != 0) {
-//			D0(fprintf(debug_file, "Could not set write pointer via sysfs, recording will start from the beginning of partition: "
-//					"%s\n", state->rawdev.rawdev_path));
-//		}
+		if (open_state_file(&state->rawdev, &state->writer_params.lba_current) != 0) {
+			D0(fprintf(debug_file, "Could not get write pointer from state file, recording will start from the beginning of partition: "
+					"%s\n", state->rawdev.rawdev_path));
+		}
 		state->writer_params.blockdev_fd = open(state->rawdev.rawdev_path, O_WRONLY);
 		if (state->writer_params.blockdev_fd < 0) {
 			D0(fprintf(debug_file, "Error opening block device: %s\n", state->rawdev.rawdev_path));
 			return -CAMOGM_FRAME_FILE_ERR;
 		}
-		D6(fprintf(debug_file, "Open block device: %s\n", state->rawdev.rawdev_path));
+		offset = lba_to_offset(state->writer_params.lba_current - state->writer_params.lba_start);
+		lseek64(state->writer_params.blockdev_fd, offset, SEEK_SET);
+		D6(fprintf(debug_file, "Open block device: %s, offset in bytes: %llu\n", state->rawdev.rawdev_path, offset));
 	}
 
 	return 0;
@@ -325,6 +324,15 @@ int camogm_frame_jpeg(camogm_state *state)
 		while (state->writer_params.data_ready)
 			pthread_cond_wait(&state->writer_params.main_cond, &state->writer_params.writer_mutex);
 		D6(fprintf(debug_file, "_13a_"));
+
+		D6(fprintf(debug_file, "\n"));
+		align_frame(state);
+		if (update_lba(state) == 1) {
+			D0(fprintf(debug_file, "The end of block device reached, continue recording from start\n"));
+		}
+		D6(fprintf(debug_file, "Block device positions: start = %llu, current = %llu, end = %llu\n",
+				state->writer_params.lba_start, state->writer_params.lba_current, state->writer_params.lba_end));
+
 		// proceed if last frame was recorded without errors
 		if (state->writer_params.last_ret_val == 0) {
 			state->writer_params.data_ready = true;
@@ -336,8 +344,8 @@ int camogm_frame_jpeg(camogm_state *state)
 		}
 
 		// update statistics
-		state->rawdev.last_jpeg_size = camogm_get_jpeg_size(state);
-		state->rawdev.total_rec_len += state->rawdev.last_jpeg_size;
+//		state->rawdev.last_jpeg_size = camogm_get_jpeg_size(state);
+//		state->rawdev.total_rec_len += state->rawdev.last_jpeg_size;
 	}
 
 	return 0;
@@ -353,15 +361,36 @@ int camogm_frame_jpeg(camogm_state *state)
 int camogm_end_jpeg(camogm_state *state)
 {
 	int ret = 0;
+	int bytes;
+	ssize_t iovlen;
 	struct frame_data fdata = {0};
 
 	if (state->rawdev_op) {
+		// write any remaining data, do not use writer thread as there can be only one block left CHUNK_REM buffer
+		pthread_mutex_lock(&state->writer_params.writer_mutex);
+		bytes = prep_last_block(state);
+		if (bytes > 0) {
+			D6(fprintf(debug_file, "Write last block of data, size = %d\n", bytes));
+			// the remaining data block is placed in CHUNK_COMMON buffer, write just this buffer
+			iovlen = writev(state->writer_params.blockdev_fd, &state->writer_params.data_chunks[CHUNK_COMMON], 1);
+			if (iovlen < bytes) {
+				D0(fprintf(debug_file, "writev error: %s (returned %i, expected %i)\n", strerror(errno), iovlen, bytes));
+				state->writer_params.last_ret_val = -CAMOGM_FRAME_FILE_ERR;
+			} else {
+				// update statistic, just one block written
+				state->writer_params.lba_current += 1;
+				state->rawdev.total_rec_len += bytes;
+			}
+			reset_chunks(state->writer_params.data_chunks, 1);
+		}
+		pthread_mutex_unlock(&state->writer_params.writer_mutex);
+
 		D6(fprintf(debug_file, "Closing block device %s\n", state->rawdev.rawdev_path));
 		ret = close(state->writer_params.blockdev_fd);
 		if (ret == -1)
 			D0(fprintf(debug_file, "Error: %s\n", strerror(errno)));
 
-//		save_state_file(&state->rawdev);
+		save_state_file(&state->rawdev, state->writer_params.lba_current);
 	}
 	return ret;
 }
@@ -395,25 +424,40 @@ void *jpeg_writer(void *thread_args)
 		if (params->data_ready) {
 			l = 0;
 			state->writer_params.last_ret_val = 0;
-			for (int i = 0; i < (state->chunk_index) - 1; i++) {
-				chunks_iovec[i].iov_base = state->packetchunks[i + 1].chunk;
-				chunks_iovec[i].iov_len = state->packetchunks[i + 1].bytes;
-				l += chunks_iovec[i].iov_len;
-			}
-			chunk_index = state->chunk_index;
-			iovlen = writev(state->writer_params.blockdev_fd, chunks_iovec, chunk_index - 1);
-			if (iovlen < l) {
-				D0(fprintf(debug_file, "writev error: %s (returned %li, expected %li)\n", strerror(errno), iovlen, l));
+//			for (int i = 0; i < (state->chunk_index) - 1; i++) {
+//				chunks_iovec[i].iov_base = state->packetchunks[i + 1].chunk;
+//				chunks_iovec[i].iov_len = state->packetchunks[i + 1].bytes;
+//				l += chunks_iovec[i].iov_len;
+//			}
+//			chunk_index = state->chunk_index;
+			chunk_index = get_data_buffers(state, &chunks_iovec, FILE_CHUNKS_NUM);
+			if (chunk_index > 0) {
+				for (int i = 0; i < chunk_index; i++)
+					l += chunks_iovec[i].iov_len;
+				iovlen = writev(state->writer_params.blockdev_fd, chunks_iovec, chunk_index);
+				if (iovlen < l) {
+					D0(fprintf(debug_file, "writev error: %s (returned %i, expected %i)\n", strerror(errno), iovlen, l));
+					state->writer_params.last_ret_val = -CAMOGM_FRAME_FILE_ERR;
+				} else {
+					// update statistic
+					state->rawdev.last_jpeg_size = l;
+					state->rawdev.total_rec_len += state->rawdev.last_jpeg_size;
+					D6(fprintf(debug_file, "Current position in block device: %lld\n", lseek64(state->writer_params.blockdev_fd, 0, SEEK_CUR)));
+				}
+			} else {
+				D0(fprintf(debug_file, "data vector mapping error: %d)\n", chunk_index));
 				state->writer_params.last_ret_val = -CAMOGM_FRAME_FILE_ERR;
 			}
 
 			// release main thread
+			reset_chunks(state->writer_params.data_chunks, 0);
 			params->data_ready = false;
 			pthread_cond_signal(&params->main_cond);
 		}
 	}
 	params->state = STATE_STOPPED;
 	pthread_mutex_unlock(&state->writer_params.writer_mutex);
+	D5(fprintf(debug_file, "Exit from recording thread\n"));
 
 	return NULL;
 }