--- /dev/null
+/*
+ * 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>
+
+//#define GIG_M10x_STUFF_VOICE_DATA
+
+/* check if byte must be stuffed/escaped
+ * I'm not sure which data should be encoded.
+ * Therefore I will go the hard way and decode 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 (modem response)
+ * Return value:
+ * number of processed bytes
+ */
+static inline int cmd_loop(unsigned char c, unsigned char *src, int numbytes,
+ struct inbuf_t *inbuf)
+{
+ struct cardstate *cs = inbuf->cs;
+ unsigned cbytes = cs->cbytes;
+ int inputstate = inbuf->inputstate;
+ int startbytes = numbytes;
+
+ for (;;) {
+ cs->respdata[cbytes] = c;
+ if (c == 10 || c == 13) {
+ gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
+ __func__, cbytes);
+ cs->cbytes = cbytes;
+ gigaset_handle_modem_response(cs); /* can change
+ cs->dle */
+ cbytes = 0;
+
+ if (cs->dle &&
+ !(inputstate & INS_DLE_command)) {
+ inputstate &= ~INS_command;
+ break;
+ }
+ } else {
+ /* advance in line buffer, checking for overflow */
+ if (cbytes < MAX_RESP_SIZE - 1)
+ cbytes++;
+ else
+ dev_warn(cs->dev, "response too large\n");
+ }
+
+ if (!numbytes)
+ break;
+ c = *src++;
+ --numbytes;
+ if (c == DLE_FLAG &&
+ (cs->dle || inputstate & INS_DLE_command)) {
+ inputstate |= INS_DLE_char;
+ break;
+ }
+ }
+
+ cs->cbytes = cbytes;
+ inbuf->inputstate = inputstate;
+
+ return startbytes - numbytes;
+}
+
+/* process a block of received bytes in lock mode (tty i/f)
+ * Return value:
+ * number of processed bytes
+ */
+static inline int lock_loop(unsigned char *src, int numbytes,
+ struct inbuf_t *inbuf)
+{
+ struct cardstate *cs = inbuf->cs;
+
+ gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
+ numbytes, src);
+ gigaset_if_receive(cs, src, numbytes);
+
+ return numbytes;
+}
+
+/* process a block of received bytes in HDLC data mode
+ * 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
+ * numbytes (all bytes processed) on error --FIXME
+ */
+static inline int hdlc_loop(unsigned char c, unsigned char *src, int numbytes,
+ struct inbuf_t *inbuf)
+{
+ struct cardstate *cs = inbuf->cs;
+ struct bc_state *bcs = inbuf->bcs;
+ int inputstate = bcs->inputstate;
+ __u16 fcs = bcs->fcs;
+ struct sk_buff *skb = bcs->skb;
+ unsigned char error;
+ struct sk_buff *compskb;
+ int startbytes = numbytes;
+ int l;
+
+ if (unlikely(inputstate & INS_byte_stuff)) {
+ inputstate &= ~INS_byte_stuff;
+ goto byte_stuff;
+ }
+ for (;;) {
+ if (unlikely(c == PPP_ESCAPE)) {
+ if (unlikely(!numbytes)) {
+ inputstate |= INS_byte_stuff;
+ break;
+ }
+ c = *src++;
+ --numbytes;
+ if (unlikely(c == DLE_FLAG &&
+ (cs->dle ||
+ inbuf->inputstate & INS_DLE_command))) {
+ inbuf->inputstate |= INS_DLE_char;
+ inputstate |= INS_byte_stuff;
+ break;
+ }
+byte_stuff:
+ c ^= PPP_TRANS;
+#ifdef CONFIG_GIGASET_DEBUG
+ if (unlikely(!muststuff(c)))
+ gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
+#endif
+ } else if (unlikely(c == PPP_FLAG)) {
+ if (unlikely(inputstate & INS_skip_frame)) {
+ if (!(inputstate & INS_have_data)) { /* 7E 7E */
+#ifdef CONFIG_GIGASET_DEBUG
+ ++bcs->emptycount;
+#endif
+ } else
+ gig_dbg(DEBUG_HDLC,
+ "7e----------------------------");
+
+ /* end of frame */
+ error = 1;
+ gigaset_rcv_error(NULL, cs, bcs);
+ } else if (!(inputstate & INS_have_data)) { /* 7E 7E */
+#ifdef CONFIG_GIGASET_DEBUG
+ ++bcs->emptycount;
+#endif
+ break;
+ } else {
+ gig_dbg(DEBUG_HDLC,
+ "7e----------------------------");
+
+ /* end of frame */
+ error = 0;
+
+ if (unlikely(fcs != PPP_GOODFCS)) {
+ dev_err(cs->dev,
+ "Packet checksum at %lu failed, "
+ "packet is corrupted (%u bytes)!\n",
+ bcs->rcvbytes, skb->len);
+ compskb = NULL;
+ gigaset_rcv_error(compskb, cs, bcs);
+ error = 1;
+ } else {
+ if (likely((l = skb->len) > 2)) {
+ skb->tail -= 2;
+ skb->len -= 2;
+ } else {
+ dev_kfree_skb(skb);
+ skb = NULL;
+ inputstate |= INS_skip_frame;
+ if (l == 1) {
+ dev_err(cs->dev,
+ "invalid packet size (1)!\n");
+ error = 1;
+ gigaset_rcv_error(NULL,
+ cs, bcs);
+ }
+ }
+ if (likely(!(error ||
+ (inputstate &
+ INS_skip_frame)))) {
+ gigaset_rcv_skb(skb, cs, bcs);
+ }
+ }
+ }
+
+ if (unlikely(error))
+ if (skb)
+ dev_kfree_skb(skb);
+
+ fcs = PPP_INITFCS;
+ inputstate &= ~(INS_have_data | INS_skip_frame);
+ if (unlikely(bcs->ignore)) {
+ inputstate |= INS_skip_frame;
+ skb = NULL;
+ } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)) {
+ skb_reserve(skb, HW_HDR_LEN);
+ } else {
+ dev_warn(cs->dev,
+ "could not allocate new skb\n");
+ inputstate |= INS_skip_frame;
+ }
+
+ break;
+#ifdef CONFIG_GIGASET_DEBUG
+ } else if (unlikely(muststuff(c))) {
+ /* Should not happen. Possible after ZDLE=1<CR><LF>. */
+ gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c);
+#endif
+ }
+
+ /* add character */
+
+#ifdef CONFIG_GIGASET_DEBUG
+ if (unlikely(!(inputstate & INS_have_data))) {
+ gig_dbg(DEBUG_HDLC, "7e (%d x) ================",
+ bcs->emptycount);
+ bcs->emptycount = 0;
+ }
+#endif
+
+ inputstate |= INS_have_data;
+
+ if (likely(!(inputstate & INS_skip_frame))) {
+ if (unlikely(skb->len == SBUFSIZE)) {
+ dev_warn(cs->dev, "received packet too long\n");
+ dev_kfree_skb_any(skb);
+ skb = NULL;
+ inputstate |= INS_skip_frame;
+ break;
+ }
+ *__skb_put(skb, 1) = c;
+ fcs = crc_ccitt_byte(fcs, c);
+ }
+
+ if (unlikely(!numbytes))
+ break;
+ c = *src++;
+ --numbytes;
+ if (unlikely(c == DLE_FLAG &&
+ (cs->dle ||
+ inbuf->inputstate & INS_DLE_command))) {
+ inbuf->inputstate |= INS_DLE_char;
+ break;
+ }
+ }
+ bcs->inputstate = inputstate;
+ bcs->fcs = fcs;
+ bcs->skb = skb;
+ return startbytes - numbytes;
+}
+
+/* process a block of received bytes in transparent data mode
+ * 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
+ * numbytes (all bytes processed) on error --FIXME
+ */
+static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes,
+ struct inbuf_t *inbuf)
+{
+ struct cardstate *cs = inbuf->cs;
+ struct bc_state *bcs = inbuf->bcs;
+ int inputstate = bcs->inputstate;
+ struct sk_buff *skb = bcs->skb;
+ int startbytes = numbytes;
+
+ for (;;) {
+ /* add character */
+ inputstate |= INS_have_data;
+
+ if (likely(!(inputstate & INS_skip_frame))) {
+ if (unlikely(skb->len == SBUFSIZE)) {
+ //FIXME just pass skb up and allocate a new one
+ dev_warn(cs->dev, "received packet too long\n");
+ dev_kfree_skb_any(skb);
+ skb = NULL;
+ inputstate |= INS_skip_frame;
+ break;
+ }
+ *__skb_put(skb, 1) = gigaset_invtab[c];
+ }
+
+ if (unlikely(!numbytes))
+ break;
+ c = *src++;
+ --numbytes;
+ if (unlikely(c == DLE_FLAG &&
+ (cs->dle ||
+ inbuf->inputstate & INS_DLE_command))) {
+ inbuf->inputstate |= INS_DLE_char;
+ break;
+ }
+ }
+
+ /* pass data up */
+ if (likely(inputstate & INS_have_data)) {
+ if (likely(!(inputstate & INS_skip_frame))) {
+ gigaset_rcv_skb(skb, cs, bcs);
+ }
+ inputstate &= ~(INS_have_data | INS_skip_frame);
+ if (unlikely(bcs->ignore)) {
+ inputstate |= INS_skip_frame;
+ skb = NULL;
+ } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN))
+ != NULL)) {
+ skb_reserve(skb, HW_HDR_LEN);
+ } else {
+ dev_warn(cs->dev, "could not allocate new skb\n");
+ inputstate |= INS_skip_frame;
+ }
+ }
+
+ bcs->inputstate = inputstate;
+ bcs->skb = skb;
+ return startbytes - numbytes;
+}
+
+/* process a block of data received from the device
+ */
+void gigaset_m10x_input(struct inbuf_t *inbuf)
+{
+ struct cardstate *cs;
+ unsigned tail, head, numbytes;
+ unsigned char *src, c;
+ int procbytes;
+
+ head = atomic_read(&inbuf->head);
+ tail = atomic_read(&inbuf->tail);
+ gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
+
+ if (head != tail) {
+ cs = inbuf->cs;
+ src = inbuf->data + head;
+ numbytes = (head > tail ? RBUFSIZE : tail) - head;
+ gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
+
+ while (numbytes) {
+ if (atomic_read(&cs->mstate) == MS_LOCKED) {
+ procbytes = lock_loop(src, numbytes, inbuf);
+ src += procbytes;
+ numbytes -= procbytes;
+ } else {
+ c = *src++;
+ --numbytes;
+ if (c == DLE_FLAG && (cs->dle ||
+ inbuf->inputstate & INS_DLE_command)) {
+ if (!(inbuf->inputstate & INS_DLE_char)) {
+ inbuf->inputstate |= INS_DLE_char;
+ goto nextbyte;
+ }
+ /* <DLE> <DLE> => <DLE> in data stream */
+ inbuf->inputstate &= ~INS_DLE_char;
+ }
+
+ if (!(inbuf->inputstate & INS_DLE_char)) {
+
+ /* FIXME use function pointers? */
+ if (inbuf->inputstate & INS_command)
+ procbytes = cmd_loop(c, src, numbytes, inbuf);
+ else if (inbuf->bcs->proto2 == ISDN_PROTO_L2_HDLC)
+ procbytes = hdlc_loop(c, src, numbytes, inbuf);
+ else
+ procbytes = iraw_loop(c, src, numbytes, inbuf);
+
+ src += procbytes;
+ numbytes -= procbytes;
+ } else { /* DLE char */
+ inbuf->inputstate &= ~INS_DLE_char;
+ switch (c) {
+ case 'X': /*begin of command*/
+#ifdef CONFIG_GIGASET_DEBUG
+ if (inbuf->inputstate & INS_command)
+ dev_err(cs->dev,
+ "received <DLE> 'X' in command mode\n");
+#endif
+ inbuf->inputstate |=
+ INS_command | INS_DLE_command;
+ break;
+ case '.': /*end of command*/
+#ifdef CONFIG_GIGASET_DEBUG
+ if (!(inbuf->inputstate & INS_command))
+ dev_err(cs->dev,
+ "received <DLE> '.' in hdlc mode\n");
+#endif
+ inbuf->inputstate &= cs->dle ?
+ ~(INS_DLE_command|INS_command)
+ : ~INS_DLE_command;
+ break;
+ //case DLE_FLAG: /*DLE_FLAG in data stream*/ /* schon oben behandelt! */
+ default:
+ dev_err(cs->dev,
+ "received 0x10 0x%02x!\n",
+ (int) c);
+ /* FIXME: reset driver?? */
+ }
+ }
+ }
+nextbyte:
+ if (!numbytes) {
+ /* end of buffer, check for wrap */
+ if (head > tail) {
+ head = 0;
+ src = inbuf->data;
+ numbytes = tail;
+ } else {
+ head = tail;
+ break;
+ }
+ }
+ }
+
+ gig_dbg(DEBUG_INTR, "setting head to %u", head);
+ atomic_set(&inbuf->head, head);
+ }
+}
+
+
+/* == data output ========================================================== */
+
+/* Encoding of a PPP packet into an octet stuffed HDLC frame
+ * with FCS, opening and closing flags.
+ * parameters:
+ * skb skb containing original packet (freed upon return)
+ * head number of headroom bytes to allocate in result skb
+ * tail number of tailroom bytes to allocate in result skb
+ * Return value:
+ * pointer to newly allocated skb containing the result frame
+ */
+static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail)
+{
+ 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
+ */
+ hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + tail + head);
+ if (!hdlc_skb) {
+ dev_kfree_skb(skb);
+ return NULL;
+ }
+ skb_reserve(hdlc_skb, head);
+
+ /* Copy acknowledge request into new skb */
+ memcpy(hdlc_skb->head, skb->head, 2);
+
+ /* 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(skb);
+ return hdlc_skb;
+}
+
+/* Encoding of a raw packet into an octet stuffed bit inverted frame
+ * parameters:
+ * skb skb containing original packet (freed upon return)
+ * head number of headroom bytes to allocate in result skb
+ * tail number of tailroom bytes to allocate in result skb
+ * Return value:
+ * pointer to newly allocated skb containing the result frame
+ */
+static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
+{
+ struct sk_buff *iraw_skb;
+ unsigned char c;
+ unsigned char *cp;
+ int len;
+
+ /* worst case: every byte must be stuffed */
+ iraw_skb = dev_alloc_skb(2*skb->len + tail + head);
+ if (!iraw_skb) {
+ dev_kfree_skb(skb);
+ return NULL;
+ }
+ skb_reserve(iraw_skb, head);
+
+ cp = skb->data;
+ len = skb->len;
+ while (len--) {
+ c = gigaset_invtab[*cp++];
+ if (c == DLE_FLAG)
+ *(skb_put(iraw_skb, 1)) = c;
+ *(skb_put(iraw_skb, 1)) = c;
+ }
+ dev_kfree_skb(skb);
+ return iraw_skb;
+}
+
+/* gigaset_send_skb
+ * called by common.c to queue an skb for sending
+ * and start transmission if necessary
+ * parameters:
+ * B Channel control structure
+ * skb
+ * Return value:
+ * number of bytes accepted for sending
+ * (skb->len if ok, 0 if out of buffer space)
+ * or error code (< 0, eg. -EINVAL)
+ */
+int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
+{
+ unsigned len = skb->len;
+ unsigned long flags;
+
+ if (bcs->proto2 == ISDN_PROTO_L2_HDLC)
+ skb = HDLC_Encode(skb, HW_HDR_LEN, 0);
+ else
+ skb = iraw_encode(skb, HW_HDR_LEN, 0);
+ if (!skb) {
+ err("unable to allocate memory for encoding!\n");
+ return -ENOMEM;
+ }
+
+ skb_queue_tail(&bcs->squeue, skb);
+ spin_lock_irqsave(&bcs->cs->lock, flags);
+ if (bcs->cs->connected)
+ tasklet_schedule(&bcs->cs->write_tasklet);
+ spin_unlock_irqrestore(&bcs->cs->lock, flags);
+
+ return len; /* ok so far */
+}
git://git.onelab.eu / linux-2.6.git / blobdiff