tty: n_gsm: Save dlci address open status when config requester

[ Upstream commit 0b91b5332368f2fb0c3e5cfebc6aff9e167acd8b ]

When n_gsm config "initiator=0",as requester ,receive SABM frame,n_gsm
register gsmtty dev,and save dlci open address status,if receive DLC0
DISC or CLD frame,it can unregister the gsmtty dev by saving dlci address.

Signed-off-by: Zhenguo Zhao <Zhenguo.Zhao1@unisoc.com>
Link: https://lore.kernel.org/r/1629461872-26965-8-git-send-email-zhenguo6858@gmail.com
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>
Signed-off-by: Sasha Levin <sashal@kernel.org>

diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c
index 91ce8e6..3038e56 100644 (file)
--- a/drivers/tty/n_gsm.c
+++ b/drivers/tty/n_gsm.c
@@ -274,6 +274,10 @@ static DEFINE_SPINLOCK(gsm_mux_lock);
 
 static struct tty_driver *gsm_tty_driver;
 
+/* Save dlci open address */
+static int addr_open[256] = { 0 };
+/* Save dlci open count */
+static int addr_cnt;
 /*
  *     This section of the driver logic implements the GSM encodings
  *     both the basic and the 'advanced'. Reliable transport is not
@@ -1191,6 +1195,7 @@ static void gsm_control_rls(struct gsm_mux *gsm, const u8 *data, int clen)
 }
 
 static void gsm_dlci_begin_close(struct gsm_dlci *dlci);
+static void gsm_dlci_close(struct gsm_dlci *dlci);
 
 /**
  *     gsm_control_message     -       DLCI 0 control processing
@@ -1209,15 +1214,28 @@ static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
 {
        u8 buf[1];
        unsigned long flags;
+       struct gsm_dlci *dlci;
+       int i;
+       int address;
 
        switch (command) {
        case CMD_CLD: {
-               struct gsm_dlci *dlci = gsm->dlci[0];
+               if (addr_cnt > 0) {
+                       for (i = 0; i < addr_cnt; i++) {
+                               address = addr_open[i];
+                               dlci = gsm->dlci[address];
+                               gsm_dlci_close(dlci);
+                               addr_open[i] = 0;
+                       }
+               }
                /* Modem wishes to close down */
+               dlci = gsm->dlci[0];
                if (dlci) {
                        dlci->dead = true;
                        gsm->dead = true;
-                       gsm_dlci_begin_close(dlci);
+                       gsm_dlci_close(dlci);
+                       addr_cnt = 0;
+                       gsm_response(gsm, 0, UA|PF);
                }
                }
                break;
@@ -1780,6 +1798,7 @@ static void gsm_queue(struct gsm_mux *gsm)
        struct gsm_dlci *dlci;
        u8 cr;
        int address;
+       int i, j, k, address_tmp;
        /* We have to sneak a look at the packet body to do the FCS.
           A somewhat layering violation in the spec */
 
@@ -1822,6 +1841,11 @@ static void gsm_queue(struct gsm_mux *gsm)
                else {
                        gsm_response(gsm, address, UA|PF);
                        gsm_dlci_open(dlci);
+                       /* Save dlci open address */
+                       if (address) {
+                               addr_open[addr_cnt] = address;
+                               addr_cnt++;
+                       }
                }
                break;
        case DISC|PF:
@@ -1832,8 +1856,33 @@ static void gsm_queue(struct gsm_mux *gsm)
                        return;
                }
                /* Real close complete */
-               gsm_response(gsm, address, UA|PF);
-               gsm_dlci_close(dlci);
+               if (!address) {
+                       if (addr_cnt > 0) {
+                               for (i = 0; i < addr_cnt; i++) {
+                                       address = addr_open[i];
+                                       dlci = gsm->dlci[address];
+                                       gsm_dlci_close(dlci);
+                                       addr_open[i] = 0;
+                               }
+                       }
+                       dlci = gsm->dlci[0];
+                       gsm_dlci_close(dlci);
+                       addr_cnt = 0;
+                       gsm_response(gsm, 0, UA|PF);
+               } else {
+                       gsm_response(gsm, address, UA|PF);
+                       gsm_dlci_close(dlci);
+                       /* clear dlci address */
+                       for (j = 0; j < addr_cnt; j++) {
+                               address_tmp = addr_open[j];
+                               if (address_tmp == address) {
+                                       for (k = j; k < addr_cnt; k++)
+                                               addr_open[k] = addr_open[k+1];
+                               addr_cnt--;
+                               break;
+                               }
+                       }
+               }
                break;
        case UA|PF:
                if (cr == 0 || dlci == NULL)