asyncdata.c
/*
* Common data handling layer for ser_gigaset and usb_gigaset
*
* Copyright (c) 2005 by Tilman Schmidt <tilman@imap.cc>,
* Hansjoerg Lipp <hjlipp@web.de>,
* Stefan Eilers.
*
* =====================================================================
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License as
* published by the Free Software Foundation; either version 2 of
* the License, or (at your option) any later version.
* =====================================================================
*/
#include "gigaset.h"
#include <linux/crc-ccitt.h>
#include <linux/bitrev.h>
#include <linux/export.h>
/* check if byte must be stuffed/escaped
* I'm not sure which data should be encoded.
* Therefore I will go the hard way and encode every value
* less than 0x20, the flag sequence and the control escape char.
*/
static inline int muststuff(unsigned char c)
{
if (c < PPP_TRANS) return 1;
if (c == PPP_FLAG) return 1;
if (c == PPP_ESCAPE) return 1;
/* other possible candidates: */
/* 0x91: XON with parity set */
/* 0x93: XOFF with parity set */
return 0;
}
/* == data input =========================================================== */
/* process a block of received bytes in command mode
* (mstate != MS_LOCKED && (inputstate & INS_command))
* Append received bytes to the command response buffer and forward them
* line by line to the response handler. Exit whenever a mode/state change
* might have occurred.
* Note: Received lines may be terminated by CR, LF, or CR LF, which will be
* removed before passing the line to the response handler.
* Return value:
* number of processed bytes
*/
static unsigned cmd_loop(unsigned numbytes, struct inbuf_t *inbuf)
{
unsigned char *src = inbuf->data + inbuf->head;
struct cardstate *cs = inbuf->cs;
unsigned cbytes = cs->cbytes;
unsigned procbytes = 0;
unsigned char c;
while (procbytes < numbytes) {
c = *src++;
procbytes++;
switch (c) {
case '\n':
if (cbytes == 0 && cs->respdata[0] == '\r') {
/* collapse LF with preceding CR */
cs->respdata[0] = 0;
break;
}
/* --v-- fall through --v-- */
case '\r':
/* end of message line, pass to response handler */
if (cbytes >= MAX_RESP_SIZE) {
dev_warn(cs->dev, "response too large (%d)\n",
cbytes);
cbytes = MAX_RESP_SIZE;
}
cs->cbytes = cbytes;
gigaset_dbg_buffer(DEBUG_TRANSCMD, "received response",
cbytes, cs->respdata);
gigaset_handle_modem_response(cs);
cbytes = 0;
/* store EOL byte for CRLF collapsing */
cs->respdata[0] = c;
/* cs->dle may have changed */
if (cs->dle && !(inbuf->inputstate & INS_DLE_command))
inbuf->inputstate &= ~INS_command;
/* return for reevaluating state */
goto exit;
case DLE_FLAG:
if (inbuf->inputstate & INS_DLE_char) {
/* quoted DLE: clear quote flag */
inbuf->inputstate &= ~INS_DLE_char;
} else if (cs->dle ||
(inbuf->inputstate & INS_DLE_command)) {
/* DLE escape, pass up for handling */
inbuf->inputstate |= INS_DLE_char;
goto exit;
}
/* quoted or not in DLE mode: treat as regular data */
/* --v-- fall through --v-- */
default:
/* append to line buffer if possible */
if (cbytes < MAX_RESP_SIZE)
cs->respdata[cbytes] = c;
cbytes++;
}
}
exit:
cs->cbytes = cbytes;
return procbytes;
}
/* process a block of received bytes in lock mode
* All received bytes are passed unmodified to the tty i/f.
* Return value:
* number of processed bytes
*/
static unsigned lock_loop(unsigned numbytes, struct inbuf_t *inbuf)
{
unsigned char *src = inbuf->data + inbuf->head;
gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response", numbytes, src);
gigaset_if_receive(inbuf->cs, src, numbytes);
return numbytes;
}
/* process a block of received bytes in HDLC data mode
* (mstate != MS_LOCKED && !(inputstate & INS_command) && proto2 == L2_HDLC)
* Collect HDLC frames, undoing byte stuffing and watching for DLE escapes.
* When a frame is complete, check the FCS and pass valid frames to the LL.
* If DLE is encountered, return immediately to let the caller handle it.
* Return value:
* number of processed bytes
*/
static unsigned hdlc_loop(unsigned numbytes, struct inbuf_t *inbuf)
{
struct cardstate *cs = inbuf->cs;
struct bc_state *bcs = cs->bcs;
int inputstate = bcs->inputstate;
__u16 fcs = bcs->rx_fcs;
struct sk_buff *skb = bcs->rx_skb;
unsigned char *src = inbuf->data + inbuf->head;
unsigned procbytes = 0;
unsigned char c;
if (inputstate & INS_byte_stuff) {
if (!numbytes)
return 0;
inputstate &= ~INS_byte_stuff;
goto byte_stuff;
}
while (procbytes < numbytes) {
c = *src++;
procbytes++;
if (c == DLE_FLAG) {
if (inputstate & INS_DLE_char) {
/* quoted DLE: clear quote flag */
inputstate &= ~INS_DLE_char;
} else if (cs->dle || (inputstate & INS_DLE_command)) {
/* DLE escape, pass up for handling */
inputstate |= INS_DLE_char;
break;
}
}
if (c == PPP_ESCAPE) {
/* byte stuffing indicator: pull in next byte */
if (procbytes >= numbytes) {
/* end of buffer, save for later processing */
inputstate |= INS_byte_stuff;
break;
}
byte_stuff:
c = *src++;
procbytes++;
if (c == DLE_FLAG) {
if (inputstate & INS_DLE_char) {
/* quoted DLE: clear quote flag */
inputstate &= ~INS_DLE_char;
} else if (cs->dle ||
(inputstate & INS_DLE_command)) {
/* DLE escape, pass up for handling */
inputstate |=
INS_DLE_char | INS_byte_stuff;
break;
}
}
c ^= PPP_TRANS;
#ifdef CONFIG_GIGASET_DEBUG
if (!muststuff(c))
gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
#endif
} else if (c == PPP_FLAG) {
/* end of frame: process content if any */
if (inputstate & INS_have_data) {
gig_dbg(DEBUG_HDLC,
"7e----------------------------");
/* check and pass received frame */
if (!skb) {
/* skipped frame */
gigaset_isdn_rcv_err(bcs);
} else if (skb->len < 2) {
/* frame too short for FCS */
dev_warn(cs->dev,
"short frame (%d)\n",
skb->len);
gigaset_isdn_rcv_err(bcs);
dev_kfree_skb_any(skb);
} else if (fcs != PPP_GOODFCS) {
/* frame check error */
dev_err(cs->dev,
"Checksum failed, %u bytes corrupted!\n",
skb->len);
gigaset_isdn_rcv_err(bcs);
dev_kfree_skb_any(skb);
} else {
/* good frame */
__skb_trim(skb, skb->len - 2);
gigaset_skb_rcvd(bcs, skb);
}
/* prepare reception of next frame */
inputstate &= ~INS_have_data;
skb = gigaset_new_rx_skb(bcs);
} else {
/* empty frame (7E 7E) */
#ifdef CONFIG_GIGASET_DEBUG
++bcs->emptycount;
#endif
if (!skb) {
/* skipped (?) */
gigaset_isdn_rcv_err(bcs);
skb = gigaset_new_rx_skb(bcs);
}
}
fcs = PPP_INITFCS;
continue;
#ifdef CONFIG_GIGASET_DEBUG
} else if (muststuff(c)) {
/* Should not happen. Possible after ZDLE=1<CR><LF>. */
gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c);
#endif
}
/* regular data byte, append to skb */
#ifdef CONFIG_GIGASET_DEBUG
if (!(inputstate & INS_have_data)) {
gig_dbg(DEBUG_HDLC, "7e (%d x) ================",
bcs->emptycount);
bcs->emptycount = 0;
}
#endif
inputstate |= INS_have_data;
if (skb) {
if (skb->len >= bcs->rx_bufsize) {
dev_warn(cs->dev, "received packet too long\n");
dev_kfree_skb_any(skb);
/* skip remainder of packet */
bcs->rx_skb = skb = NULL;
} else {
*__skb_put(skb, 1) = c;
fcs = crc_ccitt_byte(fcs, c);
}
}
}
bcs->inputstate = inputstate;
bcs->rx_fcs = fcs;
return procbytes;
}
/* process a block of received bytes in transparent data mode
* (mstate != MS_LOCKED && !(inputstate & INS_command) && proto2 != L2_HDLC)
* Invert bytes, undoing byte stuffing and watching for DLE escapes.
* If DLE is encountered, return immediately to let the caller handle it.
* Return value:
* number of processed bytes
*/
static unsigned iraw_loop(unsigned numbytes, struct inbuf_t *inbuf)
{
struct cardstate *cs = inbuf->cs;
struct bc_state *bcs = cs->bcs;
int inputstate = bcs->inputstate;
struct sk_buff *skb = bcs->rx_skb;
unsigned char *src = inbuf->data + inbuf->head;
unsigned procbytes = 0;
unsigned char c;
if (!skb) {
/* skip this block */
gigaset_new_rx_skb(bcs);
return numbytes;
}
while (procbytes < numbytes && skb->len < bcs->rx_bufsize) {
c = *src++;
procbytes++;
if (c == DLE_FLAG) {
if (inputstate & INS_DLE_char) {
/* quoted DLE: clear quote flag */
inputstate &= ~INS_DLE_char;
} else if (cs->dle || (inputstate & INS_DLE_command)) {
/* DLE escape, pass up for handling */
inputstate |= INS_DLE_char;
break;
}
}
/* regular data byte: append to current skb */
inputstate |= INS_have_data;
*__skb_put(skb, 1) = bitrev8(c);
}
/* pass data up */
if (inputstate & INS_have_data) {
gigaset_skb_rcvd(bcs, skb);
inputstate &= ~INS_have_data;
gigaset_new_rx_skb(bcs);
}
bcs->inputstate = inputstate;
return procbytes;
}
/* process DLE escapes
* Called whenever a DLE sequence might be encountered in the input stream.
* Either processes the entire DLE sequence or, if that isn't possible,
* notes the fact that an initial DLE has been received in the INS_DLE_char
* inputstate flag and resumes processing of the sequence on the next call.
*/
static void handle_dle(struct inbuf_t *inbuf)
{
struct cardstate *cs = inbuf->cs;
if (cs->mstate == MS_LOCKED)
return; /* no DLE processing in lock mode */
if (!(inbuf->inputstate & INS_DLE_char)) {
/* no DLE pending */
if (inbuf->data[inbuf->head] == DLE_FLAG &&
(cs->dle || inbuf->inputstate & INS_DLE_command)) {
/* start of DLE sequence */
inbuf->head++;
if (inbuf->head == inbuf->tail ||
inbuf->head == RBUFSIZE) {
/* end of buffer, save for later processing */
inbuf->inputstate |= INS_DLE_char;
return;
}
} else {
/* regular data byte */
return;
}
}
/* consume pending DLE */
inbuf->inputstate &= ~INS_DLE_char;
switch (inbuf->data[inbuf->head]) {
case 'X': /* begin of event message */
if (inbuf->inputstate & INS_command)
dev_notice(cs->dev,
"received <DLE>X in command mode\n");
inbuf->inputstate |= INS_command | INS_DLE_command;
inbuf->head++; /* byte consumed */
break;
case '.': /* end of event message */
if (!(inbuf->inputstate & INS_DLE_command))
dev_notice(cs->dev,
"received <DLE>. without <DLE>X\n");
inbuf->inputstate &= ~INS_DLE_command;
/* return to data mode if in DLE mode */
if (cs->dle)
inbuf->inputstate &= ~INS_command;
inbuf->head++; /* byte consumed */
break;
case DLE_FLAG: /* DLE in data stream */
/* mark as quoted */
inbuf->inputstate |= INS_DLE_char;
if (!(cs->dle || inbuf->inputstate & INS_DLE_command))
dev_notice(cs->dev,
"received <DLE><DLE> not in DLE mode\n");
break; /* quoted byte left in buffer */
default:
dev_notice(cs->dev, "received <DLE><%02x>\n",
inbuf->data[inbuf->head]);
/* quoted byte left in buffer */
}
}
/**
* gigaset_m10x_input() - process a block of data received from the device
* @inbuf: received data and device descriptor structure.
*
* Called by hardware module {ser,usb}_gigaset with a block of received
* bytes. Separates the bytes received over the serial data channel into
* user data and command replies (locked/unlocked) according to the
* current state of the interface.
*/
void gigaset_m10x_input(struct inbuf_t *inbuf)
{
struct cardstate *cs = inbuf->cs;
unsigned numbytes, procbytes;
gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", inbuf->head, inbuf->tail);
while (inbuf->head != inbuf->tail) {
/* check for DLE escape */
handle_dle(inbuf);
/* process a contiguous block of bytes */
numbytes = (inbuf->head > inbuf->tail ?
RBUFSIZE : inbuf->tail) - inbuf->head;
gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
/*
* numbytes may be 0 if handle_dle() ate the last byte.
* This does no harm, *_loop() will just return 0 immediately.
*/
if (cs->mstate == MS_LOCKED)
procbytes = lock_loop(numbytes, inbuf);
else if (inbuf->inputstate & INS_command)
procbytes = cmd_loop(numbytes, inbuf);
else if (cs->bcs->proto2 == L2_HDLC)
procbytes = hdlc_loop(numbytes, inbuf);
else
procbytes = iraw_loop(numbytes, inbuf);
inbuf->head += procbytes;
/* check for buffer wraparound */
if (inbuf->head >= RBUFSIZE)
inbuf->head = 0;
gig_dbg(DEBUG_INTR, "head set to %u", inbuf->head);
}
}
EXPORT_SYMBOL_GPL(gigaset_m10x_input);
/* == data output ========================================================== */
/*
* Encode a data packet into an octet stuffed HDLC frame with FCS,
* opening and closing flags, preserving headroom data.
* parameters:
* skb skb containing original packet (freed upon return)
* Return value:
* pointer to newly allocated skb containing the result frame
* and the original link layer header, NULL on error
*/
static struct sk_buff *HDLC_Encode(struct sk_buff *skb)
{
struct sk_buff *hdlc_skb;
__u16 fcs;
unsigned char c;
unsigned char *cp;
int len;
unsigned int stuf_cnt;
stuf_cnt = 0;
fcs = PPP_INITFCS;
cp = skb->data;
len = skb->len;
while (len--) {
if (muststuff(*cp))
stuf_cnt++;
fcs = crc_ccitt_byte(fcs, *cp++);
}
fcs ^= 0xffff; /* complement */
/* size of new buffer: original size + number of stuffing bytes
* + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes
* + room for link layer header
*/
hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + skb->mac_len);
if (!hdlc_skb) {
dev_kfree_skb_any(skb);
return NULL;
}
/* Copy link layer header into new skb */
skb_reset_mac_header(hdlc_skb);
skb_reserve(hdlc_skb, skb->mac_len);
memcpy(skb_mac_header(hdlc_skb), skb_mac_header(skb), skb->mac_len);
hdlc_skb->mac_len = skb->mac_len;
/* Add flag sequence in front of everything.. */
*(skb_put(hdlc_skb, 1)) = PPP_FLAG;
/* Perform byte stuffing while copying data. */
while (skb->len--) {
if (muststuff(*skb->data)) {
*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
*(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS;
} else
*(skb_put(hdlc_skb, 1)) = *skb->data++;
}
/* Finally add FCS (byte stuffed) and flag sequence */
c = (fcs & 0x00ff); /* least significant byte first */
if (muststuff(c)) {
*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
c ^= PPP_TRANS;
}
*(skb_put(hdlc_skb, 1)) = c;
c = ((fcs >> 8) & 0x00ff);
if (muststuff(c)) {
*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
c ^= PPP_TRANS;
}
*(skb_put(hdlc_skb, 1)) = c;
*(skb_put(hdlc_skb, 1)) = PPP_FLAG;
dev_kfree_skb_any(skb);
return hdlc_skb;
}
/*
* Encode a data packet into an octet stuffed raw bit inverted frame,
* preserving headroom data.
* parameters:
* skb skb containing original packet (freed upon return)
* Return value:
* pointer to newly allocated skb containing the result frame
* and the original link layer header, NULL on error
*/
static struct sk_buff *iraw_encode(struct sk_buff *skb)
{
struct sk_buff *iraw_skb;
unsigned char c;
unsigned char *cp;
int len;
/* size of new buffer (worst case = every byte must be stuffed):
* 2 * original size + room for link layer header
*/
iraw_skb = dev_alloc_skb(2 * skb->len + skb->mac_len);
if (!iraw_skb) {
dev_kfree_skb_any(skb);
return NULL;
}
/* copy link layer header into new skb */
skb_reset_mac_header(iraw_skb);
skb_reserve(iraw_skb, skb->mac_len);
memcpy(skb_mac_header(iraw_skb), skb_mac_header(skb), skb->mac_len);
iraw_skb->mac_len = skb->mac_len;
/* copy and stuff data */
cp = skb->data;
len = skb->len;
while (len--) {
c = bitrev8(*cp++);
if (c == DLE_FLAG)
*(skb_put(iraw_skb, 1)) = c;
*(skb_put(iraw_skb, 1)) = c;
}
dev_kfree_skb_any(skb);
return iraw_skb;
}
/**
* gigaset_m10x_send_skb() - queue an skb for sending
* @bcs: B channel descriptor structure.
* @skb: data to send.
*
* Called by LL to encode and queue an skb for sending, and start
* transmission if necessary.
* Once the payload data has been transmitted completely, gigaset_skb_sent()
* will be called with the skb's link layer header preserved.
*
* Return value:
* number of bytes accepted for sending (skb->len) if ok,
* error code < 0 (eg. -ENOMEM) on error
*/
int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
{
struct cardstate *cs = bcs->cs;
unsigned len = skb->len;
unsigned long flags;
if (bcs->proto2 == L2_HDLC)
skb = HDLC_Encode(skb);
else
skb = iraw_encode(skb);
if (!skb) {
dev_err(cs->dev,
"unable to allocate memory for encoding!\n");
return -ENOMEM;
}
skb_queue_tail(&bcs->squeue, skb);
spin_lock_irqsave(&cs->lock, flags);
if (cs->connected)
tasklet_schedule(&cs->write_tasklet);
spin_unlock_irqrestore(&cs->lock, flags);
return len; /* ok so far */
}
EXPORT_SYMBOL_GPL(gigaset_m10x_send_skb);