--- rp-pppoe-3.8/src/pppoe-server.c	2010-11-12 12:44:27.222006988 +0300
+++ rp-pppoe-3.8/src/pppoe-server.c	2010-11-12 12:46:46.560755947 +0300
@@ -98,6 +98,7 @@
 static int NumServiceNames = 0;
 static char const *ServiceNames[MAX_SERVICE_NAMES];
 static int ReactionOnNoServiceName = 1;
+#define PPPOE_PADI_ONLY_REQUESTED_SERVICE_NAME
 
 PppoeSessionFunctionTable DefaultSessionFunctionTable = {
     PppoeStopSession,
@@ -646,15 +647,52 @@
 	cursor += TAG_HDR_SIZE;
 	plen += TAG_HDR_SIZE;
     } else {
-	for (i=0; i<NumServiceNames; i++) {
-	    int slen = strlen(ServiceNames[i]);
-	    servname.length = htons(slen);
-	    CHECK_ROOM(cursor, pado.payload, TAG_HDR_SIZE+slen);
-	    memcpy(cursor, &servname, TAG_HDR_SIZE);
-	    memcpy(cursor+TAG_HDR_SIZE, ServiceNames[i], slen);
-	    cursor += TAG_HDR_SIZE+slen;
-	    plen += TAG_HDR_SIZE+slen;
+        /* If there was a specific service-name requested, only put it
+           (D-Link routers fail otherwise) */
+	int rs_slen = 0;
+	if (requestedService.type) {
+	    rs_slen = ntohs(requestedService.length);
+	    if (rs_slen) {
+		servname.length = requestedService.length;
+		CHECK_ROOM(cursor, pado.payload, TAG_HDR_SIZE+rs_slen);
+		memcpy(cursor, &servname, TAG_HDR_SIZE);
+		memcpy(cursor+TAG_HDR_SIZE, &requestedService.payload, rs_slen);
+		cursor += TAG_HDR_SIZE+rs_slen;
+		plen += TAG_HDR_SIZE+rs_slen;
+	    }
+	}
+        /* If PPPOE_PADI_ONLY_REQUESTED_SERVICE_NAME is not defined put the
+           rest of the service-names.
+	   Or, unconditionally, supply all of them if nothing specific
+           was requested. */
+#ifdef PPPOE_PADI_ONLY_REQUESTED_SERVICE_NAME
+        if (!rs_slen) {
+#endif
+	    for (i=0; i<NumServiceNames; i++) {
+		int slen = strlen(ServiceNames[i]);
+#ifndef PPPOE_PADI_ONLY_REQUESTED_SERVICE_NAME
+		int skip = 0;
+		if (rs_slen) {
+		    if (slen == rs_slen &&
+			!memcmp(ServiceNames[i], &requestedService.payload, slen)) {
+			skip = 1;
+		    }
+		}
+		if (!skip) {
+#endif
+		    servname.length = htons(slen);
+		    CHECK_ROOM(cursor, pado.payload, TAG_HDR_SIZE+slen);
+		    memcpy(cursor, &servname, TAG_HDR_SIZE);
+		    memcpy(cursor+TAG_HDR_SIZE, ServiceNames[i], slen);
+		    cursor += TAG_HDR_SIZE+slen;
+		    plen += TAG_HDR_SIZE+slen;
+#ifndef PPPOE_PADI_ONLY_REQUESTED_SERVICE_NAME
+		}
+#endif
+	    }
+#ifdef PPPOE_PADI_ONLY_REQUESTED_SERVICE_NAME
 	}
+#endif
     }
 
     CHECK_ROOM(cursor, pado.payload, TAG_HDR_SIZE + COOKIE_LEN);