5 files changed, 262 insertions, 1 deletions

diff --git a/sys/dev/hfa/fore_include.h b/sys/dev/hfa/fore_include.h
index 4ffe4ce424c6..8df95a2330c6 100644
--- a/sys/dev/hfa/fore_include.h
+++ b/sys/dev/hfa/fore_include.h
@@ -39,6 +39,9 @@
 #define _FORE_INCLUDE_H
 
 #include <netatm/kern_include.h>
+#ifdef __FreeBSD__
+#include <sys/sysctl.h>
+#endif
 
 /*
  * If not specified elsewhere, guess which type of bus support we want
diff --git a/sys/dev/hfa/fore_load.c b/sys/dev/hfa/fore_load.c
index 5f136c8e1eb7..7ab34c38afc0 100644
--- a/sys/dev/hfa/fore_load.c
+++ b/sys/dev/hfa/fore_load.c
@@ -36,6 +36,9 @@
  */
 
 #include <dev/hfa/fore_include.h>
+#ifdef __FreeBSD__
+#include <sys/bus.h>
+#endif
 
 #ifndef lint
 __RCSID("@(#) $FreeBSD$");
@@ -828,6 +831,37 @@ fore_attach(devinfo_p)
 
 
 #ifdef __FreeBSD__
+
+SYSCTL_NODE(_hw, OID_AUTO, fore, CTLFLAG_RW, 0, "Fore ATM adapter");
+
+/*
+ * Sysctl handler for the traffic shaping option
+ */
+static int
+fore_sysctl_shape(SYSCTL_HANDLER_ARGS)
+{
+	Fore_unit *fup = arg1;
+	int error;
+	u_int new;
+
+	error = SYSCTL_OUT(req, &fup->fu_shape , sizeof(fup->fu_shape));
+	if (error != 0 || req->newptr == NULL) {
+		return (error);
+	}
+
+	error = SYSCTL_IN(req, &new, sizeof(new));
+	if (error != 0) {
+		return (error);
+	}
+
+	if (new > FUS_SHAPE_ALL) {
+		return (EINVAL);
+	}
+
+	fup->fu_shape = new;
+	return (0);
+}
+
 /*
  * Device probe routine
  * 
@@ -896,6 +930,8 @@ fore_pci_attach(config_id, unit)
 	pcidi_t		device_id;
 	long		val;
 	int		err_count = BOOT_LOOPS;
+	char		ifname[IFNAMSIZ];
+	struct sysctl_oid *oid;
 
 	/*
 	 * Just checking...
@@ -993,6 +1029,23 @@ fore_pci_attach(config_id, unit)
 	}
 
 	/*
+	 * Make the sysctl tree
+	 */
+	snprintf(ifname, sizeof(ifname), "%s%d", FORE_DEV_NAME, unit);
+	fup->sysctl_tree = SYSCTL_ADD_NODE(&fup->sysctl_ctx,
+	    SYSCTL_STATIC_CHILDREN(_hw_fore), OID_AUTO,
+	    ifname, CTLFLAG_RW, 0, "");
+	if (fup->sysctl_tree == NULL)
+		goto failed;
+
+	oid = SYSCTL_ADD_PROC(&fup->sysctl_ctx,
+	    SYSCTL_CHILDREN(fup->sysctl_tree), OID_AUTO,
+	    "shape", CTLFLAG_RW | CTLTYPE_UINT, fup, 0,
+	    fore_sysctl_shape, "IU", "traffic shaping");
+	if (oid == NULL)
+		goto failed;
+
+	/*
 	 * Poke the hardware - boot the CP and prepare it for downloading
 	 */
 	fore_reset(fup);
diff --git a/sys/dev/hfa/fore_output.c b/sys/dev/hfa/fore_output.c
index b0b54362a046..fea1af07042a 100644
--- a/sys/dev/hfa/fore_output.c
+++ b/sys/dev/hfa/fore_output.c
@@ -170,7 +170,7 @@ fore_output(cup, cvp, m)
 	xdp = hxp->hxq_descr;
 	xdp->xd_cell_hdr = ATM_HDR_SET(vcp->vc_vpi, vcp->vc_vci, 0, 0);
 	xdp->xd_spec = XDS_SET_SPEC(0, fvp->fv_aal, nsegs, pdulen);
-	xdp->xd_rate = FORE_DEF_RATE;
+	xdp->xd_rate = fvp->rate;
 
 	/*
 	 * Everything is ready to go, so officially claim the host queue
diff --git a/sys/dev/hfa/fore_var.h b/sys/dev/hfa/fore_var.h
index 62a52f3d87e6..03efd9ae661c 100644
--- a/sys/dev/hfa/fore_var.h
+++ b/sys/dev/hfa/fore_var.h
@@ -47,6 +47,7 @@
 struct fore_vcc {
         struct cmn_vcc  fv_cmn;		/* Common VCC stuff */
 	Fore_aal	fv_aal;		/* CP version of AAL */
+	uint32_t	rate;		/* Rate control (data/idle cell ratio) */
 };
 typedef struct fore_vcc  Fore_vcc;
 
@@ -235,6 +236,16 @@ struct fore_unit {
 #endif
 	struct callout_handle fu_thandle;	/* Timer handle */
 	int		fu_ft4;		/* Running ForeThought 4 firmware */
+
+	/* shaping enable */
+	u_int		fu_shape;
+	u_int		fu_num_shaped;	/* number of shaped VCCs */
+
+#ifdef __FreeBSD__
+	/* sysctl support */
+	struct sysctl_ctx_list	sysctl_ctx;
+	struct sysctl_oid	*sysctl_tree;
+#endif
 };
 typedef struct fore_unit	Fore_unit;
 
@@ -260,6 +271,12 @@ typedef struct fore_unit	Fore_unit;
  */
 #define	FUF_STATCMD	0x80		/* Statistics request in progress */
 
+/*
+ * Shaping values
+ */
+#define FUS_NO_SHAPING	0
+#define FUS_SHAPE_ONE	1
+#define FUS_SHAPE_ALL	2
 
 /*
  * Macros to access CP memory
diff --git a/sys/dev/hfa/fore_vcm.c b/sys/dev/hfa/fore_vcm.c
index e2bc530ef5e3..8a618449921e 100644
--- a/sys/dev/hfa/fore_vcm.c
+++ b/sys/dev/hfa/fore_vcm.c
@@ -98,6 +98,173 @@ fore_instvcc(cup, cvp)
 	return (0);
 }
 
+static const u_int rate_tab[255] = {
+    353207, /*   0 */
+    312501, /*   1 */ 312501, /*   2 */
+    312501, /*   3 */ 312501, /*   4 */
+    312501, /*   5 */ 312501, /*   6 */
+    312501, /*   7 */ 312501, /*   8 */
+    312501, /*   9 */ 312501, /*  10 */
+    312501, /*  11 */ 312501, /*  12 */
+    312501, /*  13 */ 312501, /*  14 */
+    312501, /*  15 */ 312501, /*  16 */
+    312501, /*  17 */ 284091, /*  18 */
+    284091, /*  19 */ 284091, /*  20 */
+    284091, /*  21 */ 284091, /*  22 */
+    284091, /*  23 */ 284091, /*  24 */
+    284091, /*  25 */ 284091, /*  26 */
+    284091, /*  27 */ 284091, /*  28 */
+    284091, /*  29 */ 284091, /*  30 */
+    284091, /*  31 */ 284091, /*  32 */
+    284091, /*  33 */ 284091, /*  34 */
+    284091, /*  35 */ 284091, /*  36 */
+    284091, /*  37 */ 284091, /*  38 */
+    260417, /*  39 */ 260417, /*  40 */
+    260417, /*  41 */ 260417, /*  42 */
+    260417, /*  43 */ 260417, /*  44 */
+    260417, /*  45 */ 260417, /*  46 */
+    260417, /*  47 */ 260417, /*  48 */
+    260417, /*  49 */ 260417, /*  50 */
+    260417, /*  51 */ 260417, /*  52 */
+    260417, /*  53 */ 260417, /*  54 */
+    260417, /*  55 */ 240385, /*  56 */
+    240385, /*  57 */ 240385, /*  58 */
+    240385, /*  59 */ 240385, /*  60 */
+    240385, /*  61 */ 240385, /*  62 */
+    240385, /*  63 */ 240385, /*  64 */
+    240385, /*  65 */ 240385, /*  66 */
+    240385, /*  67 */ 240385, /*  68 */
+    240385, /*  69 */ 240385, /*  70 */
+    223215, /*  71 */ 223215, /*  72 */
+    223215, /*  73 */ 223215, /*  74 */
+    223215, /*  75 */ 223215, /*  76 */
+    223215, /*  77 */ 223215, /*  78 */
+    223215, /*  79 */ 223215, /*  80 */
+    223215, /*  81 */ 223215, /*  82 */
+    223215, /*  83 */ 208334, /*  84 */
+    208334, /*  85 */ 208334, /*  86 */
+    208334, /*  87 */ 208334, /*  88 */
+    208334, /*  89 */ 208334, /*  90 */
+    208334, /*  91 */ 208334, /*  92 */
+    208334, /*  93 */ 208334, /*  94 */
+    195313, /*  95 */ 195313, /*  96 */
+    195313, /*  97 */ 195313, /*  98 */
+    195313, /* 101 */ 195313, /* 102 */
+    195313, /* 103 */ 183824, /* 104 */
+    183824, /* 105 */ 183824, /* 106 */
+    183824, /* 107 */ 183824, /* 108 */
+    183824, /* 109 */ 183824, /* 110 */
+    183824, /* 111 */ 183824, /* 112 */
+    173612, /* 113 */ 173612, /* 114 */
+    173612, /* 115 */ 173612, /* 116 */
+    173612, /* 117 */ 173612, /* 118 */
+    173612, /* 119 */ 173612, /* 120 */
+    164474, /* 121 */ 164474, /* 122 */
+    164474, /* 123 */ 164474, /* 124 */
+    164474, /* 125 */ 164474, /* 126 */
+    164474, /* 127 */ 156250, /* 128 */
+    156250, /* 129 */ 156250, /* 130 */
+    156250, /* 131 */ 156250, /* 132 */
+    156250, /* 133 */ 148810, /* 134 */
+    148810, /* 135 */ 148810, /* 136 */
+    148810, /* 137 */ 148810, /* 138 */
+    148810, /* 139 */ 142046, /* 140 */
+    142046, /* 141 */ 142046, /* 142 */
+    142046, /* 143 */ 142046, /* 144 */
+    135870, /* 145 */ 135870, /* 146 */
+    135870, /* 147 */ 135870, /* 148 */
+    130209, /* 149 */ 130209, /* 150 */
+    130209, /* 151 */ 130209, /* 152 */
+    130209, /* 153 */ 125000, /* 154 */
+    125000, /* 155 */ 125000, /* 156 */
+    125000, /* 157 */ 120193, /* 158 */
+    120193, /* 159 */ 120193, /* 160 */
+    115741, /* 161 */ 115741, /* 162 */
+    115741, /* 163 */ 115741, /* 164 */
+    111608, /* 165 */ 111608, /* 166 */
+    111608, /* 167 */ 107759, /* 168 */
+    107759, /* 169 */ 107759, /* 170 */
+    104167, /* 171 */ 104167, /* 172 */
+    104167, /* 173 */ 100807, /* 174 */
+    100807, /* 175 */  97657, /* 176 */
+     97657, /* 177 */  97657, /* 178 */
+     94697, /* 179 */  94697, /* 180 */
+     91912, /* 181 */  91912, /* 182 */
+     89286, /* 183 */  89286, /* 184 */
+     86806, /* 185 */  86806, /* 186 */
+     84460, /* 187 */  84460, /* 188 */
+     82237, /* 189 */  82237, /* 190 */
+     80129, /* 191 */  78125, /* 192 */
+     78126, /* 193 */  76220, /* 194 */
+     74405, /* 195 */  74405, /* 196 */
+     72675, /* 197 */  71023, /* 198 */
+     69445, /* 199 */  69445, /* 200 */
+     67935, /* 201 */  66490, /* 202 */
+     65105, /* 203 */  63776, /* 204 */
+     62500, /* 205 */  61275, /* 206 */
+     60097, /* 207 */  58963, /* 208 */
+     57871, /* 209 */  56819, /* 210 */
+     54825, /* 211 */  53880, /* 212 */
+     52967, /* 213 */  51230, /* 214 */
+     50404, /* 215 */  48829, /* 216 */
+     47349, /* 217 */  46642, /* 218 */
+     45290, /* 219 */  44015, /* 220 */
+     42809, /* 221 */  41119, /* 222 */
+     40065, /* 223 */  39063, /* 224 */
+     37651, /* 225 */  36338, /* 226 */
+     35113, /* 227 */  33968, /* 228 */
+     32553, /* 229 */  31250, /* 230 */
+     30049, /* 231 */  28936, /* 232 */
+     27655, /* 233 */  26261, /* 234 */
+     25000, /* 235 */  23855, /* 236 */
+     22645, /* 237 */  21259, /* 238 */
+     20033, /* 239 */  18826, /* 240 */
+     17557, /* 241 */  16277, /* 242 */
+     15025, /* 243 */  13767, /* 244 */
+     12551, /* 245 */  11282, /* 246 */
+     10017, /* 247 */   8779, /* 248 */
+      7531, /* 249 */   6263, /* 250 */
+      5017, /* 251 */   3761, /* 252 */
+      2509, /* 253 */   1254, /* 254 */
+};
+
+/*
+ * Find the best match of the high part of the Rate Control Information
+ * 
+ * This function is called when a VC is opened in order to help
+ * in converting Fore's rate to PCR.
+ * The Fore's Rate Control Information is encoded as 32-bit field
+ * comprised of two 16-bit subfields.
+ *
+ * Arguments:
+ *	*pcr		Peak Cell Rate, will be updated with actual value
+ *
+ * Returns:
+ *	descr		the rate descriptor
+ *
+ */
+static uint32_t
+pcr2rate(int32_t *pcr)
+{
+	u_int i;
+
+	if (*pcr >= rate_tab[0]) {
+		/* special case link rate */
+		*pcr = rate_tab[0];
+		return (0);
+	}
+
+	for (i = 0; i < sizeof(rate_tab) / sizeof(rate_tab[0]); i++)
+		if (*pcr >= rate_tab[i])
+			break;
+	if (i == sizeof(rate_tab) / sizeof(rate_tab[0])) {
+		/* smaller than smallest */
+		i--;
+	}
+	/* update with the actual value */
+	*pcr = rate_tab[i];
+	return ((255 - i) << 16) | i;
+}
 
 /*
  * Open a VCC
@@ -145,6 +312,24 @@ fore_openvcc(cup, cvp)
 	}
 
 	/*
+	 * Compute the PCR (but only for outgoing VCCs)
+	 */
+	fvp->rate = FORE_DEF_RATE;
+	if ((vcp->vc_type & VCC_OUT) && cvp->cv_connvc) {
+		Atm_attributes *attr = &cvp->cv_connvc->cvc_attr;
+
+		if (attr && attr->traffic.v.forward.PCR_all_traffic > 0 &&
+		    attr->traffic.v.forward.PCR_all_traffic < rate_tab[0] &&
+		    (fup->fu_shape == FUS_SHAPE_ALL ||
+		    (fup->fu_shape == FUS_SHAPE_ONE &&
+		    fup->fu_num_shaped == 0))) {
+			fvp->rate = pcr2rate(&attr->traffic.v.forward.
+			    PCR_all_traffic);
+			fup->fu_num_shaped++;
+		}
+	}
+
+	/*
 	 * Only need to tell the CP about incoming VCCs
 	 */
 	if ((vcp->vc_type & VCC_IN) == 0) {
@@ -314,6 +499,9 @@ fore_closevcc(cup, cvp)
 	if (fvp->fv_state == CVS_ACTIVE)
 		fup->fu_open_vcc--;
 
+	if (fvp->rate != 0)
+		fup->fu_num_shaped--;
+
 	DEVICE_UNLOCK((Cmn_unit *)fup);
 
 	return (err);