isdn/gigaset: handle Supplementary Service Listen

Add minimal handling for the non-optional CAPI FACILITY_REQ
Supplementary Service function Listen.

Impact: bugfix
Signed-off-by: Tilman Schmidt <tilman@imap.cc>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/isdn/gigaset/capi.c b/drivers/isdn/gigaset/capi.c
index 68194b5027312a598e74f14b6f9cc7803196e323..3ef149fe48c816d71c85e9dd366ab31da188cac2 100644 (file)
--- a/drivers/isdn/gigaset/capi.c
+++ b/drivers/isdn/gigaset/capi.c
@@ -45,6 +45,7 @@
 #define CAPI_FACILITY_LI       0x0005
 
 #define CAPI_SUPPSVC_GETSUPPORTED      0x0000
+#define CAPI_SUPPSVC_LISTEN            0x0001
 
 /* missing from capiutil.h */
 #define CAPIMSG_PLCI_PART(m)   CAPIMSG_U8(m, 9)
@@ -1151,7 +1152,7 @@ static void do_facility_req(struct gigaset_capi_ctr *iif,
        case CAPI_FACILITY_SUPPSVC:
                /* decode Function parameter */
                pparam = cmsg->FacilityRequestParameter;
-               if (pparam == NULL || *pparam < 2) {
+               if (pparam == NULL || pparam[0] < 2) {
                        dev_notice(cs->dev, "%s: %s missing\n", "FACILITY_REQ",
                                   "Facility Request Parameter");
                        send_conf(iif, ap, skb, CapiIllMessageParmCoding);
@@ -1168,8 +1169,32 @@ static void do_facility_req(struct gigaset_capi_ctr *iif,
                        /* Supported Services: none */
                        capimsg_setu32(confparam, 6, 0);
                        break;
+               case CAPI_SUPPSVC_LISTEN:
+                       if (pparam[0] < 7 || pparam[3] < 4) {
+                               dev_notice(cs->dev, "%s: %s missing\n",
+                                          "FACILITY_REQ", "Notification Mask");
+                               send_conf(iif, ap, skb,
+                                         CapiIllMessageParmCoding);
+                               return;
+                       }
+                       if (CAPIMSG_U32(pparam, 4) != 0) {
+                               dev_notice(cs->dev,
+       "%s: unsupported supplementary service notification mask 0x%x\n",
+                                  "FACILITY_REQ", CAPIMSG_U32(pparam, 4));
+                               info = CapiFacilitySpecificFunctionNotSupported;
+                               confparam[3] = 2;       /* length */
+                               capimsg_setu16(confparam, 4,
+                                       CapiSupplementaryServiceNotSupported);
+                       }
+                       info = CapiSuccess;
+                       confparam[3] = 2;       /* length */
+                       capimsg_setu16(confparam, 4, CapiSuccess);
+                       break;
                /* ToDo: add supported services */
                default:
+                       dev_notice(cs->dev,
+               "%s: unsupported supplementary service function 0x%04x\n",
+                                  "FACILITY_REQ", function);
                        info = CapiFacilitySpecificFunctionNotSupported;
                        /* Supplementary Service specific parameter */
                        confparam[3] = 2;       /* length */