Add ability to adjust fxo gains on wctdm drivers

git-svn-id: http://svn.digium.com/svn/zaptel/branches/1.4@3350 5390a7c7-147a-4af0-8ec9-7488f05a26cb

1 files changed, 59 insertions, 29 deletions

diff --git a/wctdm24xxp/base.c b/wctdm24xxp/base.c
index de6d7cc..b2aff24 100644
--- a/wctdm24xxp/base.c
+++ b/wctdm24xxp/base.c
@@ -2016,6 +2016,51 @@ static void wait_just_a_bit(int foo)
 	while(jiffies < newjiffies);
 }
 
+/*********************************************************************
+ * Set the hwgain on the analog modules
+ *
+ * card = the card position for this module (0-23)
+ * gain = gain in dB x10 (e.g. -3.5dB  would be gain=-35)
+ * tx = (0 for rx; 1 for tx)
+ *
+ *******************************************************************/
+static int wctdm_set_hwgain(struct wctdm *wc, int card, int gain, int tx)
+{
+	if (!(wc->modtype[card] == MOD_TYPE_FXO)) {
+		printk("Cannot adjust gain.  Unsupported module type!\n");
+		return -1;
+	}
+	if (tx) {
+		if (debug)
+			printk("setting FXO tx gain for card=%d to %d\n", card, gain);
+		if (gain >=  -150 && gain <= 0) {
+			wctdm_setreg(wc, card, 38, 16 + (gain/-10));
+			wctdm_setreg(wc, card, 40, 16 + (-gain%10));
+		} else if (gain <= 120 && gain > 0) {
+			wctdm_setreg(wc, card, 38, gain/10);
+			wctdm_setreg(wc, card, 40, (gain%10));
+		} else {
+			printk("FXO tx gain is out of range (%d)\n", gain);
+			return -1;
+		}
+	} else { /* rx */
+		if (debug)
+			printk("setting FXO rx gain for card=%d to %d\n", card, gain);
+		if (gain >=  -150 && gain <= 0) {
+			wctdm_setreg(wc, card, 39, 16+ (gain/-10));
+			wctdm_setreg(wc, card, 41, 16 + (-gain%10));
+		} else if (gain <= 120 && gain > 0) {
+			wctdm_setreg(wc, card, 39, gain/10);
+			wctdm_setreg(wc, card, 41, (gain%10));
+		} else {
+			printk("FXO rx gain is out of range (%d)\n", gain);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
 static int wctdm_init_voicedaa(struct wctdm *wc, int card, int fast, int manual, int sane)
 {
 	unsigned char reg16=0, reg26=0, reg30=0, reg31=0;
@@ -2094,35 +2139,9 @@ static int wctdm_init_voicedaa(struct wctdm *wc, int card, int fast, int manual,
 	wctdm_setreg(wc, card, 5, 0x08);
 	
 	/* Take values for fxotxgain and fxorxgain and apply them to module */
-	if (fxotxgain) {
-		if (fxotxgain >=  -150 && fxotxgain < 0) {
-			wctdm_setreg(wc, card, 38, 16 + (fxotxgain/-10));
-			if(fxotxgain % 10) {
-				wctdm_setreg(wc, card, 40, 16 + (-fxotxgain%10));
-			}
-		}
-		else if (fxotxgain <= 120 && fxotxgain > 0) {
-			wctdm_setreg(wc, card, 38, fxotxgain/10);
-			if(fxotxgain % 10) {
-				wctdm_setreg(wc, card, 40, (fxotxgain%10));
-			}
-		}
-	}
-	if (fxorxgain) {
-		if (fxorxgain >=  -150 && fxorxgain < 0) {
-			wctdm_setreg(wc, card, 39, 16+ (fxorxgain/-10));
-			if(fxotxgain % 10) {
-				wctdm_setreg(wc, card, 41, 16 + (-fxorxgain%10));
-			}
-		}
-		else if (fxorxgain <= 120 && fxorxgain > 0) {
-			wctdm_setreg(wc, card, 39, fxorxgain/10);
-			if(fxorxgain % 10) {
-				wctdm_setreg(wc, card, 41, (fxorxgain%10));
-			}
-		}
-	}
-	
+	wctdm_set_hwgain(wc, card, fxotxgain, 1);
+	wctdm_set_hwgain(wc, card, fxorxgain, 0);
+
 	if(debug)
 		printk("DEBUG fxotxgain:%i.%i fxorxgain:%i.%i\n", (wctdm_getreg(wc, card, 38)/16) ? -(wctdm_getreg(wc, card, 38) - 16) : wctdm_getreg(wc, card, 38), (wctdm_getreg(wc, card, 40)/16) ? -(wctdm_getreg(wc, card, 40) - 16) : wctdm_getreg(wc, card, 40), (wctdm_getreg(wc, card, 39)/16) ? -(wctdm_getreg(wc, card, 39) - 16): wctdm_getreg(wc, card, 39), (wctdm_getreg(wc, card, 41)/16)?-(wctdm_getreg(wc, card, 41) - 16) : wctdm_getreg(wc, card, 41));
 	
@@ -2506,6 +2525,7 @@ static int wctdm_ioctl(struct zt_chan *chan, unsigned int cmd, unsigned long dat
 	struct wctdm_regs regs;
 	struct wctdm_regop regop;
 	struct wctdm_echo_coefs echoregs;
+	struct zt_hwgain hwgain;
 	struct wctdm *wc = chan->pvt;
 	int x;
 	union {
@@ -2631,6 +2651,16 @@ static int wctdm_ioctl(struct zt_chan *chan, unsigned int cmd, unsigned long dat
 
 		}
 		break;
+	case ZT_SET_HWGAIN:
+		if (copy_from_user(&hwgain, (struct zt_hwgain*)data, sizeof(hwgain)))
+			return -EFAULT;
+
+		wctdm_set_hwgain(wc, chan->chanpos-1, hwgain.newgain, hwgain.tx);
+
+		if (debug)
+			printk("Setting hwgain on channel %d to %d for %s direction\n", 
+				chan->chanpos-1, hwgain.newgain, hwgain.tx ? "tx" : "rx");
+		break;
 #ifdef VPM_SUPPORT
 	case ZT_TONEDETECT:
 		if (get_user(x, (int *) data))