diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c index 271efa7ae793..56a3466acfc6 100644 --- a/drivers/tty/n_gsm.c +++ b/drivers/tty/n_gsm.c @@ -425,6 +425,27 @@ static int gsm_read_ea(unsigned int *val, u8 c) return c & EA; } +/** + * gsm_read_ea_val - read a value until EA + * @val: variable holding value + * @data: buffer of data + * @dlen: length of data + * + * Processes an EA value. Updates the passed variable and + * returns the processed data length. + */ +static unsigned int gsm_read_ea_val(unsigned int *val, const u8 *data, int dlen) +{ + unsigned int len = 0; + + for (; dlen > 0; dlen--) { + len++; + if (gsm_read_ea(val, *data++)) + break; + } + return len; +} + /** * gsm_encode_modem - encode modem data bits * @dlci: DLCI to encode from @@ -746,6 +767,37 @@ static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len, return m; } +/** + * gsm_is_flow_ctrl_msg - checks if flow control message + * @msg: message to check + * + * Returns true if the given message is a flow control command of the + * control channel. False is returned in any other case. + */ +static bool gsm_is_flow_ctrl_msg(struct gsm_msg *msg) +{ + unsigned int cmd; + + if (msg->addr > 0) + return false; + + switch (msg->ctrl & ~PF) { + case UI: + case UIH: + cmd = 0; + if (gsm_read_ea_val(&cmd, msg->data + 2, msg->len - 2) < 1) + break; + switch (cmd & ~PF) { + case CMD_FCOFF: + case CMD_FCON: + return true; + } + break; + } + + return false; +} + /** * gsm_data_kick - poke the queue * @gsm: GSM Mux @@ -765,7 +817,7 @@ static void gsm_data_kick(struct gsm_mux *gsm, struct gsm_dlci *dlci) int len; list_for_each_entry_safe(msg, nmsg, &gsm->tx_list, list) { - if (gsm->constipated && msg->addr) + if (gsm->constipated && !gsm_is_flow_ctrl_msg(msg)) continue; if (gsm->encoding != 0) { gsm->txframe[0] = GSM1_SOF;