WIP: make fl2k_fm use singlechannel mode

Plus some hacky additional locking to cope with the multiple callbacks that happen now.
WIP: add support for palette (single channel) mode
2021-08-05 23:03:52 +02:00 · 2021-08-05 22:54:22 +02:00
3 changed files with 217 additions and 37 deletions
--- a/include/osmo-fl2k.h
+++ b/include/osmo-fl2k.h
@ -39,6 +39,7 @@ enum fl2k_error {
 	FL2K_ERROR_BUSY = -6,
 	FL2K_ERROR_TIMEOUT = -7,
 	FL2K_ERROR_NO_MEM = -11,
+	FL2K_ERROR_OTHER = -9999,
 };

 typedef struct fl2k_data_info {
@ -51,11 +52,17 @@ typedef struct fl2k_data_info {

 	/* filled in by application */
 	int sampletype_signed;		/* are samples signed or unsigned? */
-	char *r_buf;			/* pointer to red buffer */
+	char *r_buf;			/* pointer to red buffer (or singlechan buffer) */
 	char *g_buf;			/* pointer to green buffer */
 	char *b_buf;			/* pointer to blue buffer */
 } fl2k_data_info_t;

+typedef enum fl2k_mode {
+	FL2K_MODE_INVALID,
+	FL2K_MODE_SINGLECHAN,
+	FL2K_MODE_MULTICHAN
+} fl2k_mode_t;
+
 typedef struct fl2k_dev fl2k_dev_t;

 /** The transfer length was chosen by the following criteria:
@ -69,6 +76,8 @@ typedef struct fl2k_dev fl2k_dev_t;
 #define FL2K_BUF_LEN		(1280 * 1024)
 #define FL2K_XFER_LEN		(FL2K_BUF_LEN * 3)

+#define FL2K_PALETTE_SIZE	256
+
 FL2K_API uint32_t fl2k_get_device_count(void);

 FL2K_API const char* fl2k_get_device_name(uint32_t index);
@ -101,6 +110,8 @@ FL2K_API uint32_t fl2k_get_sample_rate(fl2k_dev_t *dev);

 typedef void(*fl2k_tx_cb_t)(fl2k_data_info_t *data_info);

+FL2K_API int fl2k_set_mode(fl2k_dev_t *dev, fl2k_mode_t mode);
+
 /*!
 * Starts the tx thread. This function will block until
 * it is being canceled using fl2k_stop_tx()
--- a/src/fl2k_fm.c
+++ b/src/fl2k_fm.c
@ -56,10 +56,12 @@ fl2k_dev_t *dev = NULL;
 int do_exit = 0;

 pthread_t fm_thread;
-pthread_mutex_t cb_mutex;
-pthread_mutex_t fm_mutex;
-pthread_cond_t cb_cond;
-pthread_cond_t fm_cond;
+pthread_mutex_t cb_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_mutex_t fm_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_mutex_t done_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_cond_t cb_cond = PTHREAD_COND_INITIALIZER;
+pthread_cond_t fm_cond = PTHREAD_COND_INITIALIZER;
+pthread_cond_t done_cond = PTHREAD_COND_INITIALIZER;

 FILE *file;
 int8_t *txbuf = NULL;
@ -110,6 +112,8 @@ sighandler(int signum)
 		fl2k_stop_tx(dev);
 		do_exit = 1;
 		pthread_cond_signal(&fm_cond);
+		pthread_cond_signal(&cb_cond);
+		pthread_cond_signal(&done_cond);
 		return TRUE;
 	}
 	return FALSE;
@ -121,6 +125,8 @@ static void sighandler(int signum)
 	fl2k_stop_tx(dev);
 	do_exit = 1;
 	pthread_cond_signal(&fm_cond);
+	pthread_cond_signal(&cb_cond);
+	pthread_cond_signal(&done_cond);
 }
 #endif

@ -141,7 +147,7 @@ static void sighandler(int signum)
 #define SIN_TABLE_LEN	(1 << SIN_TABLE_ORDER)
 #define ANG_INCR	(0xffffffff / DDS_2PI)

-int8_t sine_table[SIN_TABLE_LEN];
+uint8_t sine_table[SIN_TABLE_LEN];
 int sine_table_init = 0;

 typedef struct {
@ -188,7 +194,7 @@ dds_t dds_init(double sample_freq, double freq, double phase)
 	if (!sine_table_init) {
 		double incr = 1.0 / (double)SIN_TABLE_LEN;
 		for (i = 0; i < SIN_TABLE_LEN; i++)
-			sine_table[i] = sin(incr * i * DDS_2PI) * 127;
+			sine_table[i] = sin(incr * i * DDS_2PI) * 127 + 128;

 		sine_table_init = 1;
 	}
@ -249,6 +255,7 @@ static void *fm_worker(void *arg)
 				tmp_ptr = fmbuf;
 				fmbuf = txbuf;
 				txbuf = tmp_ptr;
+				pthread_cond_signal(&done_cond);
 				pthread_cond_wait(&cb_cond, &cb_mutex);
 			}

@ -424,9 +431,11 @@ void fl2k_callback(fl2k_data_info_t *data_info)
 		pthread_cond_signal(&fm_cond);
 	}

-	pthread_cond_signal(&cb_cond);

-	data_info->sampletype_signed = 1;
+	pthread_cond_signal(&cb_cond);
+	pthread_cond_wait(&done_cond, &done_mutex);
+
+	data_info->sampletype_signed = 0;
 	data_info->r_buf = (char *)txbuf;
 }

@ -542,9 +551,6 @@ int main(int argc, char **argv)

 	fprintf(stderr, "Samplerate:\t%3.2f MHz\n", (double)samp_rate/1000000);
 	fprintf(stderr, "Carrier:\t%3.2f MHz\n", (double)carrier_freq/1000000);
-	fprintf(stderr, "Frequencies:\t%3.2f MHz, %3.2f MHz\n", 
-					(double)((samp_rate - carrier_freq) / 1000000.0),
-					(double)((samp_rate + carrier_freq) / 1000000.0));

 	pthread_mutex_init(&cb_mutex, NULL);
 	pthread_mutex_init(&fm_mutex, NULL);
@ -565,6 +571,11 @@ int main(int argc, char **argv)
 	}

 	pthread_attr_destroy(&attr);
+
+	r = fl2k_set_mode(dev, FL2K_MODE_SINGLECHAN);
+	if (r < 0)
+		fprintf(stderr, "WARNING: Failed to set singlechannel mode\n");
+
 	r = fl2k_start_tx(dev, fl2k_callback, NULL, 0);

 	/* Set the sample rate */
@ -575,6 +586,10 @@ int main(int argc, char **argv)
 	/* read back actual frequency */
 	samp_rate = fl2k_get_sample_rate(dev);

+	//carrier_freq = samp_rate - 97000000;
+	fprintf(stderr, "Frequencies:\t%3.2f MHz, %3.2f MHz\n", 
+					(double)((samp_rate - carrier_freq) / 1000000.0),
+					(double)((samp_rate + carrier_freq) / 1000000.0));
 	/* Calculate needed constants */
 	carrier_per_signal = samp_rate / input_freq;

--- a/src/libosmo-fl2k.c
+++ b/src/libosmo-fl2k.c
@ -89,6 +89,8 @@ struct fl2k_dev {
 	enum fl2k_async_status async_status;
 	int async_cancel;

+	fl2k_mode_t mode;
+	uint8_t enabled_chans;
 	int use_zerocopy;
 	int terminate;

@ -123,9 +125,13 @@ static fl2k_dongle_t known_devices[] = {
 #define CTRL_TIMEOUT	300
 #define BULK_TIMEOUT	0

+#define R_EN		(1 << 0)
+#define G_EN		(1 << 1)
+#define B_EN		(1 << 2)
+
 static int fl2k_read_reg(fl2k_dev_t *dev, uint16_t reg, uint32_t *val)
 {
-	int r;
+	uint32_t r;
 	uint8_t data[4];

 	if (!dev || !val)
@ -134,16 +140,19 @@ static int fl2k_read_reg(fl2k_dev_t *dev, uint16_t reg, uint32_t *val)
 	r = libusb_control_transfer(dev->devh, CTRL_IN, 0x40,
 				    0, reg, data, 4, CTRL_TIMEOUT);

-	if (r < 4)
+	if (r < sizeof(data)) {
 		fprintf(stderr, "Error, short read from register!\n");
+		return FL2K_ERROR_OTHER;
+	}

 	*val = (data[3] << 24) | (data[2] << 16) | (data[1] << 8) | data[0];

-	return r;
+	return FL2K_SUCCESS;
 }

 static int fl2k_write_reg(fl2k_dev_t *dev, uint16_t reg, uint32_t val)
 {
+	uint32_t r;
 	uint8_t data[4];

 	if (!dev)
@ -154,15 +163,72 @@ static int fl2k_write_reg(fl2k_dev_t *dev, uint16_t reg, uint32_t val)
 	data[2] = (val >> 16) & 0xff;
 	data[3] = (val >> 24) & 0xff;

-	return libusb_control_transfer(dev->devh, CTRL_OUT, 0x41,
-				       0, reg, data, 4, CTRL_TIMEOUT);
+	r = libusb_control_transfer(dev->devh, CTRL_OUT, 0x41,
+				    0, reg, data, 4, CTRL_TIMEOUT);
+
+	if (r != sizeof(data))
+		return FL2K_ERROR_OTHER;
+
+	return FL2K_SUCCESS;
+}
+
+int fl2k_load_custom_palette(fl2k_dev_t *dev, uint32_t *palette)
+{
+	int i, r;
+	uint32_t reg = 0;
+
+	if (!dev)
+		return FL2K_ERROR_INVALID_PARAM;
+
+	/* write palette RAM */
+	for (i = 0; i < FL2K_PALETTE_SIZE; i++) {
+		r = fl2k_write_reg(dev, 0x805c, palette[i] << 8 | (i & 0xff));
+		if (r < 0)
+			fprintf(stderr, "Error writing palette entry %d!\n", i);
+	}
+
+	/* verify palette RAM */
+	for (i = 0; i < FL2K_PALETTE_SIZE; i++) {
+		/* for whatever reason there's an address offset of 1
+		 * when reading */
+		r = fl2k_write_reg(dev, 0x8060, (i+1) & 0xff );
+		r |= fl2k_read_reg(dev, 0x805c, &reg);
+
+		if (r < 0)
+			return FL2K_ERROR_OTHER;
+
+		if (reg != palette[i])
+			fprintf(stderr, "Palette entry %d mismatch: 0x%06x, "
+					"expected 0x%06x\n", i, reg, palette[i]);
+	}
+
+	return FL2K_SUCCESS;
+}
+
+int fl2k_set_enabled_channels(fl2k_dev_t *dev, uint8_t chan_mask)
+{
+	uint32_t palette[FL2K_PALETTE_SIZE];
+	uint8_t val;
+
+	/* generate linear 8 bit palette for desired DAC channels */
+	for (int i = 0; i < FL2K_PALETTE_SIZE; i++) {
+		val = i & 0xff;
+		palette[i]  = chan_mask & R_EN ? (i << 16) : 0;
+		palette[i] |= chan_mask & G_EN ? (i << 8) : 0;
+		palette[i] |= chan_mask & B_EN ?  i : 0;
+	}
+
+	return fl2k_load_custom_palette(dev, palette);
 }

 int fl2k_init_device(fl2k_dev_t *dev)
 {
+	int r;
+
 	if (!dev)
 		return FL2K_ERROR_INVALID_PARAM;

+
 	/* initialization */
 	fl2k_write_reg(dev, 0x8020, 0xdf0000cc);

@ -559,6 +625,7 @@ static void LIBUSB_CALL _libusb_callback(struct libusb_transfer *xfer)
 				r = libusb_submit_transfer(xfer);
 				pthread_cond_signal(&dev->buf_cond);
 				dev->underflow_cnt++;
+				fprintf(stderr, "Resubmitted transfer!\n");
 			}
 		}
 	}
@ -798,7 +865,7 @@ static void *fl2k_usb_worker(void *arg)
 	pthread_exit(NULL);
 }

-/* Buffer format conversion functions for R, G, B DACs */
+/* Buffer format conversion functions for R, G, B DACs (multichannel mode) */
 static inline void fl2k_convert_r(char *out,
 				  char *in,
 				  uint32_t len,
@ -865,24 +932,59 @@ static inline void fl2k_convert_b(char *out,
 	}
 }

+/* Conversion function for single channel (256-color) mode */
+static inline void fl2k_convert_singlechan(char *out,
+					   char *in,
+					   uint32_t len,
+					   uint8_t offset)
+{
+	unsigned int i;
+
+	if (!in || !out)
+		return;
+
+	/* swap 32 bit words */
+	for (i = 0; i < len; i += 8) {
+		out[i+0] = in[i+4] + offset;
+		out[i+1] = in[i+5] + offset;
+		out[i+2] = in[i+6] + offset;
+		out[i+3] = in[i+7] + offset;
+		out[i+4] = in[i+0] + offset;
+		out[i+5] = in[i+1] + offset;
+		out[i+6] = in[i+2] + offset;
+		out[i+7] = in[i+3] + offset;
+	}
+}
+
 static void *fl2k_sample_worker(void *arg)
 {
 	int r = 0;
-	unsigned int i, j;
+	unsigned int i, callback_cnt;
 	fl2k_dev_t *dev = (fl2k_dev_t *)arg;
 	fl2k_xfer_info_t *xfer_info = NULL;
 	struct libusb_transfer *xfer = NULL;
 	char *out_buf = NULL;
-	fl2k_data_info_t data_info;
+	fl2k_data_info_t data_info[3];
 	uint32_t underflows = 0;
 	uint64_t buf_cnt = 0;

 	while (FL2K_RUNNING == dev->async_status) {
-		memset(&data_info, 0, sizeof(fl2k_data_info_t));
+		/* for backward API compatibility, the buffer size should
+		 * not change between single- and multichannel mode, thus
+		 * we call the callback 3 times for singlechannel mode to
+		 * get the required amount of data */
+		callback_cnt = (dev->mode == FL2K_MODE_SINGLECHAN) ? 3 : 1;

-		data_info.len = FL2K_BUF_LEN;
-		data_info.underflow_cnt = dev->underflow_cnt;
-		data_info.ctx = dev->cb_ctx;
+		for (i = 0; i < callback_cnt; i++) {
+			memset(&data_info[i], 0, sizeof(fl2k_data_info_t));
+			data_info[i].len = FL2K_BUF_LEN;
+			data_info[i].underflow_cnt = dev->underflow_cnt;
+			data_info[i].ctx = dev->cb_ctx;
+		}
+
+		/* call application callback to get samples */
+		if (dev->cb)
+			dev->cb(&data_info[0]);

 		if (dev->underflow_cnt > underflows) {
 			fprintf(stderr, "Underflow! Skipped %d buffers\n",
@ -890,10 +992,6 @@ static void *fl2k_sample_worker(void *arg)
 			underflows = dev->underflow_cnt;
 		}

-		/* call application callback to get samples */
-		if (dev->cb)
-			dev->cb(&data_info);
-
 		xfer = fl2k_get_next_xfer(dev, BUF_EMPTY);

 		if (!xfer) {
@ -914,15 +1012,29 @@ static void *fl2k_sample_worker(void *arg)
 		xfer_info = (fl2k_xfer_info_t *)xfer->user_data;
 		out_buf = (char *)xfer->buffer;

-		/* Re-arrange and copy bytes in buffer for DACs */
-		fl2k_convert_r(out_buf, data_info.r_buf, dev->xfer_buf_len,
-			       data_info.sampletype_signed ? 128 : 0);
+		if (dev->mode == FL2K_MODE_SINGLECHAN) {
+			/* Convert a buffer to the singlechannel DAC format */
+			for (i = 0; i < callback_cnt; i++) {
+				fl2k_convert_singlechan(&out_buf[i*FL2K_BUF_LEN], data_info[i].r_buf,
+							FL2K_BUF_LEN,
+							data_info[i].sampletype_signed ? 128 : 0);

-		fl2k_convert_g(out_buf, data_info.g_buf, dev->xfer_buf_len,
-			       data_info.sampletype_signed ? 128 : 0);
+				/* We need to fetch two more buffers to get
+				 * the same amount of data as in multichannel mode */
+				if (dev->cb && (i < 2))
+					dev->cb(&data_info[i+1]);
+			}
+		} else {
+			/* Re-arrange and copy bytes in buffer for DACs */
+			fl2k_convert_r(out_buf, data_info[0].r_buf, dev->xfer_buf_len,
+				       data_info[0].sampletype_signed ? 128 : 0);

-		fl2k_convert_b(out_buf, data_info.b_buf, dev->xfer_buf_len,
-			       data_info.sampletype_signed ? 128 : 0);
+			fl2k_convert_g(out_buf, data_info[0].g_buf, dev->xfer_buf_len,
+				       data_info[0].sampletype_signed ? 128 : 0);
+
+			fl2k_convert_b(out_buf, data_info[0].b_buf, dev->xfer_buf_len,
+				       data_info[0].sampletype_signed ? 128 : 0);
+		}

 		xfer_info->seq = buf_cnt++;
 		xfer_info->state = BUF_FILLED;
@ -930,13 +1042,55 @@ static void *fl2k_sample_worker(void *arg)

 	/* notify application if we've lost the device */
 	if (dev->dev_lost && dev->cb) {
-		data_info.device_error = 1;
-		dev->cb(&data_info);
+		data_info[0].device_error = 1;
+		dev->cb(&data_info[0]);
 	}

 	pthread_exit(NULL);
 }

+int fl2k_set_mode(fl2k_dev_t *dev, fl2k_mode_t mode)
+{
+	uint32_t reg;
+	int r = 0;
+
+	if (!dev)
+		return FL2K_ERROR_INVALID_PARAM;
+
+	if (FL2K_RUNNING == dev->async_status)
+		return FL2K_ERROR_BUSY;
+
+	if (dev->mode == mode)
+		return FL2K_SUCCESS;
+
+	r = fl2k_read_reg(dev, 0x8004, &reg);
+
+	if (r < 0)
+		return FL2K_ERROR_OTHER;
+
+	if (mode == FL2K_MODE_SINGLECHAN) {
+		/* enable 256 color palette mode */
+		reg |= (1 << 25) | (1 << 26);
+		r |= fl2k_set_enabled_channels(dev, R_EN);
+	} else if (mode == FL2K_MODE_MULTICHAN) {
+		reg &= ~((1 << 25) | (1 << 26));
+	} else {
+		return FL2K_ERROR_INVALID_PARAM;
+	}
+
+	r = fl2k_write_reg(dev, 0x8004, reg );
+
+	if (!r)
+		dev->mode = mode;
+
+	return r;
+}
+
+fl2k_mode_t fl2k_get_mode(fl2k_dev_t *dev)
+{
+	if (dev)
+		return dev->mode;
+}

 int fl2k_start_tx(fl2k_dev_t *dev, fl2k_tx_cb_t cb, void *ctx,
 		  uint32_t buf_num)