Rewrite TLV parser code

Added the float handling, too,  but not enabled yet.

1 files changed, 293 insertions, 130 deletions

diff --git a/src/control.c b/src/control.c
index 919b08d..95aae82 100644
--- a/src/control.c
+++ b/src/control.c
@@ -27,6 +27,9 @@
 #include <fcntl.h>
 #include <signal.h>
 #include <sys/poll.h>
+#ifdef SALSA_SUPPORT_FLOAT
+#include <math.h>
+#endif
 #include "control.h"
 #include "local.h"
 
@@ -38,7 +41,7 @@
 int snd_ctl_open(snd_ctl_t **ctlp, const char *name, int mode)
 {
 	snd_ctl_t *ctl;
-	char filename[32];
+	char filename[sizeof(SALSA_DEVPATH) + 24];
 	int err, fmode, fd, card, ver;
 
 	*ctlp = NULL;
@@ -188,13 +191,13 @@ static int hw_elem_tlv(snd_ctl_t *ctl, int inum,
 		       unsigned int numid,
 		       unsigned int *tlv, unsigned int tlv_size)
 {
-	struct sndrv_ctl_tlv *xtlv;
+	snd_ctl_tlv_t *xtlv;
 	
 	/* we don't support TLV on protocol ver 2.0.3 or earlier */
 	if (ctl->protocol < SNDRV_PROTOCOL_VERSION(2, 0, 4))
 		return -ENXIO;
 
-	xtlv = malloc(sizeof(struct sndrv_ctl_tlv) + tlv_size);
+	xtlv = malloc(sizeof(snd_ctl_tlv_t) + tlv_size);
 	if (xtlv == NULL)
 		return -ENOMEM; 
 	xtlv->numid = numid;
@@ -354,103 +357,188 @@ int snd_async_add_ctl_handler(snd_async_handler_t **handler, snd_ctl_t *ctl,
 
 /* convert the given raw volume value to a dB gain
  */
-int snd_tlv_convert_to_dB(unsigned int *tlv, long rangemin, long rangemax,
-			  long volume, long *db_gain)
+static int tlv_to_dB_range(unsigned int *tlv, long rangemin, long rangemax,
+			   long volume, long *db_gain)
 {
-	switch (tlv[0]) {
-	case SND_CTL_TLVT_DB_RANGE: {
-		unsigned int pos, len;
-		len = int_index(tlv[1]);
-		if (len > MAX_TLV_RANGE_SIZE)
-			return -EINVAL;
-		pos = 2;
-		while (pos + 4 <= len) {
-			rangemin = (int)tlv[pos];
-			rangemax = (int)tlv[pos + 1];
-			if (volume >= rangemin && volume <= rangemax)
-				return snd_tlv_convert_to_dB(tlv + pos + 2,
+	unsigned int pos, len;
+
+	len = int_index(tlv[1]);
+	if (len > MAX_TLV_RANGE_SIZE)
+		return -EINVAL;
+	pos = 2;
+	while (pos + 4 <= len) {
+		rangemin = (int)tlv[pos++];
+		rangemax = (int)tlv[pos++];
+		if (volume >= rangemin && volume <= rangemax)
+			return snd_tlv_convert_to_dB(tlv + pos,
 						     rangemin, rangemax,
 						     volume, db_gain);
-			pos += int_index(tlv[pos + 3]) + 4;
-		}
-		return -EINVAL;
+		pos++;
+		pos += int_index(tlv[pos]) + 1;
 	}
-	case SND_CTL_TLVT_DB_SCALE: {
-		int min, step, mute;
-		min = tlv[2];
-		step = (tlv[3] & 0xffff);
-		mute = (tlv[3] >> 16) & 1;
-		if (mute && volume == rangemin)
+	return -EINVAL;
+}
+
+static int tlv_to_dB_scale(unsigned int *tlv, long rangemin, long rangemax,
+			  long volume, long *db_gain)
+{
+	int min, step, mute;
+
+	min = tlv[2];
+	step = (tlv[3] & 0xffff);
+	mute = (tlv[3] >> 16) & 1;
+	if (mute && volume == rangemin)
+		*db_gain = SND_CTL_TLV_DB_GAIN_MUTE;
+	else
+		*db_gain = (volume - rangemin) * step + min;
+	return 0;
+}
+
+static int tlv_to_dB_minmax(unsigned int *tlv, long rangemin, long rangemax,
+			   long volume, long *db_gain)
+{
+	int mindb, maxdb;
+
+	mindb = tlv[2];
+	maxdb = tlv[3];
+	if (volume <= rangemin || rangemax <= rangemin) {
+		if (tlv[0] == SND_CTL_TLVT_DB_MINMAX_MUTE)
 			*db_gain = SND_CTL_TLV_DB_GAIN_MUTE;
 		else
-			*db_gain = (volume - rangemin) * step + min;
-		return 0;
+			*db_gain = mindb;
+	} else if (volume >= rangemax)
+		*db_gain = maxdb;
+	else
+		*db_gain = (maxdb - mindb) * (volume - rangemin) /
+			(rangemax - rangemin) + mindb;
+	return 0;
+}
+
+#ifdef SALSA_SUPPORT_FLOAT
+static int tlv_to_dB_linear(unsigned int *tlv, long rangemin, long rangemax,
+			    long volume, long *db_gain)
+{
+	int mindb = tlv[2];
+	int maxdb = tlv[3];
+
+	if (volume <= rangemin || rangemax <= rangemin)
+		*db_gain = mindb;
+	else if (volume >= rangemax)
+		*db_gain = maxdb;
+	else {
+		double val = (double)(volume - rangemin) /
+			(double)(rangemax - rangemin);
+		if (mindb <= SND_CTL_TLV_DB_GAIN_MUTE)
+			*db_gain = (long)(100.0 * 20.0 * log10(val)) + maxdb;
+		else {
+			/* FIXME: precalculate and cache these values */
+			double lmin = pow(10.0, mindb/2000.0);
+			double lmax = pow(10.0, maxdb/2000.0);
+			val = (lmax - lmin) * val + lmin;
+			*db_gain = (long)(100.0 * 20.0 * log10(val));
+		}
 	}
+	return 0;
+}
+#else
+static inline int tlv_to_dB_linear(unsigned int *tlv, long rangemin,
+				   long rangemax, long volume, long *db_gain)
+{
+	return -EINVAL;
+}
+#endif
+
+int snd_tlv_convert_to_dB(unsigned int *tlv, long rangemin, long rangemax,
+			  long volume, long *db_gain)
+{
+	switch (tlv[0]) {
+	case SND_CTL_TLVT_DB_RANGE:
+		return tlv_to_dB_range(tlv, rangemin, rangemax,
+				       volume, db_gain);
+	case SND_CTL_TLVT_DB_SCALE:
+		return tlv_to_dB_scale(tlv, rangemin, rangemax, volume,
+				       db_gain);
 	case SND_CTL_TLVT_DB_MINMAX:
-	case SND_CTL_TLVT_DB_MINMAX_MUTE: {
-		int mindb, maxdb;
-		mindb = tlv[2];
-		maxdb = tlv[3];
-		if (volume <= rangemin || rangemax <= rangemin) {
-			if (tlv[0] == SND_CTL_TLVT_DB_MINMAX_MUTE)
-				*db_gain = SND_CTL_TLV_DB_GAIN_MUTE;
-			else
-				*db_gain = mindb;
-		} else if (volume >= rangemax)
-			*db_gain = maxdb;
-		else
-			*db_gain = (maxdb - mindb) * (volume - rangemin) /
-				(rangemax - rangemin) + mindb;
-		return 0;
-	}
+	case SND_CTL_TLVT_DB_MINMAX_MUTE:
+		return tlv_to_dB_minmax(tlv, rangemin, rangemax, volume,
+					db_gain);
+	case SND_CTL_TLVT_DB_LINEAR:
+		return tlv_to_dB_linear(tlv, rangemin, rangemax, volume,
+					db_gain);
 	}
 	return -EINVAL;
 }
 
 /* Get the dB min/max values
  */
-int snd_tlv_get_dB_range(unsigned int *tlv, long rangemin, long rangemax,
-			 long *min, long *max)
+static int tlv_range_dB_range(unsigned int *tlv, long rangemin, long rangemax,
+			      long *min, long *max)
 {
-	switch (tlv[0]) {
-	case SND_CTL_TLVT_DB_RANGE: {
-		unsigned int pos, len;
-		len = int_index(tlv[1]);
-		if (len > MAX_TLV_RANGE_SIZE)
-			return -EINVAL;
-		pos = 2;
-		while (pos + 4 <= len) {
-			long rmin, rmax;
-			rangemin = (int)tlv[pos];
-			rangemax = (int)tlv[pos + 1];
-			snd_tlv_get_dB_range(tlv + pos + 2, rangemin, rangemax,
-					     &rmin, &rmax);
-			if (pos > 2) {
-				if (rmin < *min)
-					*min = rmin;
-				if (rmax > *max)
-					*max = rmax;
-			} else {
+	unsigned int pos, len;
+	int err;
+
+	len = int_index(tlv[1]);
+	if (len > MAX_TLV_RANGE_SIZE)
+		return -EINVAL;
+	pos = 2;
+	while (pos + 4 <= len) {
+		long rmin, rmax;
+		long submin, submax;
+		submin = (int)tlv[pos++];
+		submax = (int)tlv[pos++];
+		if (rangemax < submax)
+			submax = rangemax;
+		err = snd_tlv_get_dB_range(tlv + pos, submin, submax,
+					   &rmin, &rmax);
+		if (err < 0)
+			return err;
+		if (pos > 4) {
+			if (rmin < *min)
 				*min = rmin;
+			if (rmax > *max)
 				*max = rmax;
-			}
-			pos += int_index(tlv[pos + 3]) + 4;
+		} else {
+			*min = rmin;
+			*max = rmax;
 		}
-		return 0;
+		if (rangemax == submax)
+			return 0;
+		pos++;
+		pos += int_index(tlv[pos]) + 1;
 	}
-	case SND_CTL_TLVT_DB_SCALE: {
-		int step;
+	return 0;
+}
+
+static int tlv_range_dB_scale(unsigned int *tlv, long rangemin, long rangemax,
+			      long *min, long *max)
+{
+	int step;
+	if (tlv[3] & 0x10000)
+		*min = SND_CTL_TLV_DB_GAIN_MUTE;
+	else
 		*min = (int)tlv[2];
-		step = (tlv[3] & 0xffff);
-		*max = *min + (long)(step * (rangemax - rangemin));
-		return 0;
-	}
+	step = (tlv[3] & 0xffff);
+	*max = (int)tlv[2] + step * (rangemax - rangemin);
+	return 0;
+}
+
+int snd_tlv_get_dB_range(unsigned int *tlv, long rangemin, long rangemax,
+			 long *min, long *max)
+{
+	switch (tlv[0]) {
+	case SND_CTL_TLVT_DB_RANGE:
+		return tlv_range_dB_range(tlv, rangemin, rangemax, min, max);
+	case SND_CTL_TLVT_DB_SCALE:
+		return tlv_range_dB_scale(tlv, rangemin, rangemax, min, max);
 	case SND_CTL_TLVT_DB_MINMAX:
-	case SND_CTL_TLVT_DB_MINMAX_MUTE:
 	case SND_CTL_TLVT_DB_LINEAR:
 		*min = (int)tlv[2];
 		*max = (int)tlv[3];
 		return 0;
+	case SND_CTL_TLVT_DB_MINMAX_MUTE:
+		*min = SND_CTL_TLV_DB_GAIN_MUTE;
+		*max = (int)tlv[3];
+		return 0;
 	}
 	return -EINVAL;
 }
@@ -458,69 +546,144 @@ int snd_tlv_get_dB_range(unsigned int *tlv, long rangemin, long rangemax,
 /* Convert from dB gain to the corresponding raw value.
  * The value is round up when xdir > 0.
  */
-int snd_tlv_convert_from_dB(unsigned int *tlv, long rangemin, long rangemax,
-			    long db_gain, long *value, int xdir)
+static int tlv_from_dB_range(unsigned int *tlv, long rangemin, long rangemax,
+			     long db_gain, long *value, int xdir)
 {
-	switch (tlv[0]) {
-	case SND_CTL_TLVT_DB_RANGE: {
-		unsigned int pos, len;
-		len = int_index(tlv[1]);
-		if (len > MAX_TLV_RANGE_SIZE)
-			return -EINVAL;
-		pos = 2;
-		while (pos + 4 <= len) {
-			long dbmin, dbmax;
-			rangemin = (int)tlv[pos];
-			rangemax = (int)tlv[pos + 1];
-			if (!snd_tlv_get_dB_range(tlv + pos + 2,
-						  rangemin, rangemax,
-						  &dbmin, &dbmax) &&
-			    db_gain >= dbmin && db_gain <= dbmax)
-				return snd_tlv_convert_from_dB(tlv + pos + 2,
+	unsigned int pos, len;
+	long dbmin, dbmax, prev_rangemax;
+
+	len = int_index(tlv[1]);
+	if (len > MAX_TLV_RANGE_SIZE)
+		return -EINVAL;
+	if (snd_tlv_get_dB_range(tlv, rangemin, rangemax, &dbmin, &dbmax))
+		return -EINVAL;
+	if (db_gain <= dbmin) {
+		*value = rangemin;
+		return 0;
+	} else if (db_gain >= dbmax) {
+		*value = rangemax;
+		return 0;
+	}
+	pos = 2;
+	prev_rangemax = 0;
+	while (pos + 4 <= len) {
+		rangemin = (int)tlv[pos++];
+		rangemax = (int)tlv[pos++];
+		if (!snd_tlv_get_dB_range(tlv + pos, rangemin, rangemax,
+					  &dbmin, &dbmax) &&
+		    db_gain >= dbmin && db_gain <= dbmax)
+			return snd_tlv_convert_from_dB(tlv + pos,
 						       rangemin, rangemax,
 						       db_gain, value, xdir);
-			pos += int_index(tlv[pos + 3]) + 4;
+		else if (db_gain < dbmin) {
+			*value = xdir ? rangemin : prev_rangemax;
+			return 0;
 		}
-		return -EINVAL;
+		prev_rangemax = rangemax;
+		pos++;
+		pos += int_index(tlv[pos]) + 1;
 	}
-	case SND_CTL_TLVT_DB_SCALE: {
-		int min, step, max;
-		min = tlv[2];
-		step = (tlv[3] & 0xffff);
-		max = min + (int)(step * (rangemax - rangemin));
-		if (db_gain <= min)
+	return -EINVAL;
+}
+
+static void _tlv_from_dB_minmax(long rangemin, long rangemax,
+				long db_gain, long *value, int xdir,
+				int min, int max, int have_mute)
+{
+	if (db_gain <= min) {
+		if (db_gain > SND_CTL_TLV_DB_GAIN_MUTE && xdir > 0 && have_mute)
+			*value = rangemin + 1;
+		else
 			*value = rangemin;
-		else if (db_gain >= max)
-			*value = rangemax;
-		else {
-			long v = (db_gain - min) * (rangemax - rangemin);
-			if (xdir > 0)
-				v += (max - min) - 1;
-			v = v / (max - min) + rangemin;
-			*value = v;
-		}
-		return 0;
+	} else if (db_gain >= max)
+		*value = rangemax;
+	else {
+		long v = (db_gain - min) * (rangemax - rangemin);
+		if (xdir > 0)
+			v += (max - min) - 1;
+		v = v / (max - min) + rangemin;
+		*value = v;
 	}
-	case SND_CTL_TLVT_DB_MINMAX:
-	case SND_CTL_TLVT_DB_MINMAX_MUTE: {
-		int min, max;
-		min = tlv[2];
-		max = tlv[3];
-		if (db_gain <= min)
-			*value = rangemin;
-		else if (db_gain >= max)
-			*value = rangemax;
-		else {
-			long v = (db_gain - min) * (rangemax - rangemin);
-			if (xdir > 0)
-				v += (max - min) - 1;
-			v = v / (max - min) + rangemin;
-			*value = v;
-		}
-		return 0;
+}
+
+static int tlv_from_dB_scale(unsigned int *tlv, long rangemin, long rangemax,
+			     long db_gain, long *value, int xdir)
+{
+	int min, step, max, have_mute;
+
+	min = tlv[2];
+	step = (tlv[3] & 0xffff);
+	max = min + (int)(step * (rangemax - rangemin));
+	have_mute = (tlv[3] & 0x10000);
+	_tlv_from_dB_minmax(rangemin, rangemax, db_gain, value, xdir,
+			    min, max, have_mute);
+	return 0;
+}
+
+static int tlv_from_dB_minmax(unsigned int *tlv, long rangemin, long rangemax,
+			      long db_gain, long *value, int xdir)
+{
+	int min, max, have_mute;
+
+	min = tlv[2];
+	max = tlv[3];
+	have_mute = tlv[0] == SND_CTL_TLVT_DB_MINMAX_MUTE;
+	_tlv_from_dB_minmax(rangemin, rangemax, db_gain, value, xdir,
+			    min, max, have_mute);
+	return 0;
+}
+
+#ifdef SALSA_SUPPORT_FLOAT
+static int tlv_from_dB_linear(unsigned int *tlv, long rangemin, long rangemax,
+			      long db_gain, long *value, int xdir)
+{
+	int min, max;
+	
+	min = tlv[2];
+	max = tlv[3];
+	if (db_gain <= min)
+		*value = rangemin;
+	else if (db_gain >= max)
+		*value = rangemax;
+	else {
+		double vmin, vmax, v;
+		vmin = (min <= SND_CTL_TLV_DB_GAIN_MUTE) ? 0.0 :
+			pow(10.0,  (double)min / 2000.0);
+		vmax = !max ? 1.0 : pow(10.0,  (double)max / 2000.0);
+		v = pow(10.0, (double)db_gain / 2000.0);
+		v = (v - vmin) * (rangemax - rangemin) / (vmax - vmin);
+		if (xdir > 0)
+			v = ceil(v);
+		*value = (long)v + rangemin;
 	}
-	default:
-		break;
+	return 0;
+}
+#else
+static inline int tlv_from_dB_linear(unsigned int *tlv, long rangemin,
+				     long rangemax, long db_gain,
+				     long *value, int xdir)
+{
+	return -EINVAL;
+}
+#endif
+
+int snd_tlv_convert_from_dB(unsigned int *tlv, long rangemin, long rangemax,
+			    long db_gain, long *value, int xdir)
+{
+	switch (tlv[0]) {
+	case SND_CTL_TLVT_DB_RANGE:
+		return tlv_from_dB_range(tlv, rangemin, rangemax,
+					 db_gain, value, xdir);
+	case SND_CTL_TLVT_DB_SCALE:
+		return tlv_from_dB_scale(tlv, rangemin, rangemax,
+					 db_gain, value, xdir);
+	case SND_CTL_TLVT_DB_MINMAX:
+	case SND_CTL_TLVT_DB_MINMAX_MUTE:
+		return tlv_from_dB_minmax(tlv, rangemin, rangemax,
+					 db_gain, value, xdir);
+	case SND_CTL_TLVT_DB_LINEAR:
+		return tlv_from_dB_linear(tlv, rangemin, rangemax,
+					  db_gain, value, xdir);
 	}
 	return -EINVAL;
 }