/* * 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> /* 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. * 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 */ gig_dbg(DEBUG_TRANSCMD, "%s: End of Message (%d Bytes)", __func__, cbytes); if (cbytes >= MAX_RESP_SIZE) { dev_warn(cs->dev, "response too large (%d)\n", cbytes); cbytes = MAX_RESP_SIZE; } cs->cbytes = cbytes; 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; } /* set up next receive skb for data mode */ static void new_rcv_skb(struct bc_state *bcs) { struct cardstate *cs = bcs->cs; unsigned short hw_hdr_len = cs->hw_hdr_len; if (bcs->ignore) { bcs->skb = NULL; return; } bcs->skb = dev_alloc_skb(SBUFSIZE + hw_hdr_len); if (bcs->skb == NULL) { dev_warn(cs->dev, "could not allocate new skb\n"); return; } skb_reserve(bcs->skb, hw_hdr_len); } /* 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->fcs; struct sk_buff *skb = bcs->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; new_rcv_skb(bcs); skb = bcs->skb; } else { /* empty frame (7E 7E) */ #ifdef CONFIG_GIGASET_DEBUG ++bcs->emptycount; #endif if (!skb) { /* skipped (?) */ gigaset_isdn_rcv_err(bcs); new_rcv_skb(bcs); skb = bcs->skb; } } 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 == SBUFSIZE) { dev_warn(cs->dev, "received packet too long\n"); dev_kfree_skb_any(skb); /* skip remainder of packet */ bcs->skb = skb = NULL; } else { *__skb_put(skb, 1) = c; fcs = crc_ccitt_byte(fcs, c); } } } bcs->inputstate = inputstate; bcs->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->skb; unsigned char *src = inbuf->data + inbuf->head; unsigned procbytes = 0; unsigned char c; if (!skb) { /* skip this block */ new_rcv_skb(bcs); return numbytes; } while (procbytes < numbytes && skb->len < SBUFSIZE) { 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; new_rcv_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);