@@ -1418,18 +1418,13 @@ static void gsm_control_modem(struct gsm_mux *gsm, const u8 *data, int clen)
unsigned int modem = 0;
struct gsm_dlci *dlci;
int len = clen;
- int slen;
+ int cl = clen;
const u8 *dp = data;
struct tty_struct *tty;
- while (gsm_read_ea(&addr, *dp++) == 0) {
- len--;
- if (len == 0)
- return;
- }
- /* Must be at least one byte following the EA */
- len--;
- if (len <= 0)
+ len = gsm_read_ea_val(&addr, data, cl);
+
+ if (len < 1)
return;
addr >>= 1;
@@ -1438,15 +1433,21 @@ static void gsm_control_modem(struct gsm_mux *gsm, const u8 *data, int clen)
return;
dlci = gsm->dlci[addr];
- slen = len;
- while (gsm_read_ea(&modem, *dp++) == 0) {
- len--;
- if (len == 0)
- return;
- }
- len--;
+ /* Must be at least one byte following the EA */
+ if ((cl - len) < 1)
+ return;
+
+ dp += len;
+ cl -= len;
+
+ /* get the modem status */
+ len = gsm_read_ea_val(&modem, dp, cl);
+
+ if (len < 1)
+ return;
+
tty = tty_port_tty_get(&dlci->port);
- gsm_process_modem(tty, dlci, modem, slen - len);
+ gsm_process_modem(tty, dlci, modem, cl);
if (tty) {
tty_wakeup(tty);
tty_kref_put(tty);
@@ -1921,8 +1922,7 @@ static void gsm_dlci_data(struct gsm_dlci *dlci, const u8 *data, int clen)
struct tty_port *port = &dlci->port;
struct tty_struct *tty;
unsigned int modem = 0;
- int len = clen;
- int slen = 0;
+ int len;
if (debug & 16)
pr_debug("%d bytes for tty\n", len);
@@ -1933,24 +1933,22 @@ static void gsm_dlci_data(struct gsm_dlci *dlci, const u8 *data, int clen)
case 3: /* Packetised uininterruptible voice/data */
break;
case 2: /* Asynchronous serial with line state in each frame */
- while (gsm_read_ea(&modem, *data++) == 0) {
- len--;
- slen++;
- if (len == 0)
- return;
- }
- len--;
- slen++;
+ len = gsm_read_ea_val(&modem, data, clen);
+ if (len < 1)
+ return;
tty = tty_port_tty_get(port);
if (tty) {
- gsm_process_modem(tty, dlci, modem, slen);
+ gsm_process_modem(tty, dlci, modem, len);
tty_wakeup(tty);
tty_kref_put(tty);
}
+ /* Skip processed modem data */
+ data += len;
+ clen -= len;
fallthrough;
case 1: /* Line state will go via DLCI 0 controls only */
default:
- tty_insert_flip_string(port, data, len);
+ tty_insert_flip_string(port, data, clen);
tty_flip_buffer_push(port);
}
}
@@ -1971,24 +1969,29 @@ static void gsm_dlci_command(struct gsm_dlci *dlci, const u8 *data, int len)
{
/* See what command is involved */
unsigned int command = 0;
- while (len-- > 0) {
- if (gsm_read_ea(&command, *data++) == 1) {
- int clen = *data++;
- len--;
- /* FIXME: this is properly an EA */
- clen >>= 1;
- /* Malformed command ? */
- if (clen > len)
- return;
- if (command & 1)
- gsm_control_message(dlci->gsm, command,
- data, clen);
- else
- gsm_control_response(dlci->gsm, command,
- data, clen);
- return;
- }
- }
+ const u8 *dp = data;
+ int clen = 0;
+ int dlen;
+
+ /* read the command */
+ dlen = gsm_read_ea_val(&command, dp, len);
+ len -= dlen;
+ dp += dlen;
+
+ /* read any control data */
+ dlen = gsm_read_ea_val(&clen, dp, len);
+ len -= dlen;
+ dp += dlen;
+
+ /* Malformed command? */
+ if (clen > len)
+ return;
+
+ if (command & 1)
+ gsm_control_message(dlci->gsm, command, dp, clen);
+ else
+ gsm_control_response(dlci->gsm, command, dp, clen);
+ return;
}
/**