/* * sunzilog.c * * Driver for Zilog serial chips found on Sun workstations and * servers. This driver could actually be made more generic. * * This is based on the old drivers/sbus/char/zs.c code. A lot * of code has been simply moved over directly from there but * much has been rewritten. Credits therefore go out to Eddie * C. Dost, Pete Zaitcev, Ted Ts'o and Alex Buell for their * work there. * * Copyright (C) 2002 David S. Miller (davem@redhat.com) */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef CONFIG_SERIO #include #endif #include #include #include #ifdef CONFIG_SPARC64 #include #endif #include #if defined(CONFIG_SERIAL_SUNZILOG_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ) #define SUPPORT_SYSRQ #endif #include #include "suncore.h" #include "sunzilog.h" /* On 32-bit sparcs we need to delay after register accesses * to accommodate sun4 systems, but we do not need to flush writes. * On 64-bit sparc we only need to flush single writes to ensure * completion. */ #ifndef CONFIG_SPARC64 #define ZSDELAY() udelay(5) #define ZSDELAY_LONG() udelay(20) #define ZS_WSYNC(channel) do { } while (0) #else #define ZSDELAY() #define ZSDELAY_LONG() #define ZS_WSYNC(__channel) \ sbus_readb(&((__channel)->control)) #endif static int num_sunzilog; #define NUM_SUNZILOG num_sunzilog #define NUM_CHANNELS (NUM_SUNZILOG * 2) #define KEYBOARD_LINE 0x2 #define MOUSE_LINE 0x3 #define ZS_CLOCK 4915200 /* Zilog input clock rate. */ #define ZS_CLOCK_DIVISOR 16 /* Divisor this driver uses. */ /* * We wrap our port structure around the generic uart_port. */ struct uart_sunzilog_port { struct uart_port port; /* IRQ servicing chain. */ struct uart_sunzilog_port *next; /* Current values of Zilog write registers. */ unsigned char curregs[NUM_ZSREGS]; unsigned int flags; #define SUNZILOG_FLAG_CONS_KEYB 0x00000001 #define SUNZILOG_FLAG_CONS_MOUSE 0x00000002 #define SUNZILOG_FLAG_IS_CONS 0x00000004 #define SUNZILOG_FLAG_IS_KGDB 0x00000008 #define SUNZILOG_FLAG_MODEM_STATUS 0x00000010 #define SUNZILOG_FLAG_IS_CHANNEL_A 0x00000020 #define SUNZILOG_FLAG_REGS_HELD 0x00000040 #define SUNZILOG_FLAG_TX_STOPPED 0x00000080 #define SUNZILOG_FLAG_TX_ACTIVE 0x00000100 unsigned int cflag; unsigned char parity_mask; unsigned char prev_status; #ifdef CONFIG_SERIO struct serio *serio; int serio_open; #endif }; #define ZILOG_CHANNEL_FROM_PORT(PORT) ((struct zilog_channel __iomem *)((PORT)->membase)) #define UART_ZILOG(PORT) ((struct uart_sunzilog_port *)(PORT)) #define ZS_IS_KEYB(UP) ((UP)->flags & SUNZILOG_FLAG_CONS_KEYB) #define ZS_IS_MOUSE(UP) ((UP)->flags & SUNZILOG_FLAG_CONS_MOUSE) #define ZS_IS_CONS(UP) ((UP)->flags & SUNZILOG_FLAG_IS_CONS) #define ZS_IS_KGDB(UP) ((UP)->flags & SUNZILOG_FLAG_IS_KGDB) #define ZS_WANTS_MODEM_STATUS(UP) ((UP)->flags & SUNZILOG_FLAG_MODEM_STATUS) #define ZS_IS_CHANNEL_A(UP) ((UP)->flags & SUNZILOG_FLAG_IS_CHANNEL_A) #define ZS_REGS_HELD(UP) ((UP)->flags & SUNZILOG_FLAG_REGS_HELD) #define ZS_TX_STOPPED(UP) ((UP)->flags & SUNZILOG_FLAG_TX_STOPPED) #define ZS_TX_ACTIVE(UP) ((UP)->flags & SUNZILOG_FLAG_TX_ACTIVE) /* Reading and writing Zilog8530 registers. The delays are to make this * driver work on the Sun4 which needs a settling delay after each chip * register access, other machines handle this in hardware via auxiliary * flip-flops which implement the settle time we do in software. * * The port lock must be held and local IRQs must be disabled * when {read,write}_zsreg is invoked. */ static unsigned char read_zsreg(struct zilog_channel __iomem *channel, unsigned char reg) { unsigned char retval; sbus_writeb(reg, &channel->control); ZSDELAY(); retval = sbus_readb(&channel->control); ZSDELAY(); return retval; } static void write_zsreg(struct zilog_channel __iomem *channel, unsigned char reg, unsigned char value) { sbus_writeb(reg, &channel->control); ZSDELAY(); sbus_writeb(value, &channel->control); ZSDELAY(); } static void sunzilog_clear_fifo(struct zilog_channel __iomem *channel) { int i; for (i = 0; i < 32; i++) { unsigned char regval; regval = sbus_readb(&channel->control); ZSDELAY(); if (regval & Rx_CH_AV) break; regval = read_zsreg(channel, R1); sbus_readb(&channel->data); ZSDELAY(); if (regval & (PAR_ERR | Rx_OVR | CRC_ERR)) { sbus_writeb(ERR_RES, &channel->control); ZSDELAY(); ZS_WSYNC(channel); } } } /* This function must only be called when the TX is not busy. The UART * port lock must be held and local interrupts disabled. */ static void __load_zsregs(struct zilog_channel __iomem *channel, unsigned char *regs) { int i; /* Let pending transmits finish. */ for (i = 0; i < 1000; i++) { unsigned char stat = read_zsreg(channel, R1); if (stat & ALL_SNT) break; udelay(100); } sbus_writeb(ERR_RES, &channel->control); ZSDELAY(); ZS_WSYNC(channel); sunzilog_clear_fifo(channel); /* Disable all interrupts. */ write_zsreg(channel, R1, regs[R1] & ~(RxINT_MASK | TxINT_ENAB | EXT_INT_ENAB)); /* Set parity, sync config, stop bits, and clock divisor. */ write_zsreg(channel, R4, regs[R4]); /* Set misc. TX/RX control bits. */ write_zsreg(channel, R10, regs[R10]); /* Set TX/RX controls sans the enable bits. */ write_zsreg(channel, R3, regs[R3] & ~RxENAB); write_zsreg(channel, R5, regs[R5] & ~TxENAB); /* Synchronous mode config. */ write_zsreg(channel, R6, regs[R6]); write_zsreg(channel, R7, regs[R7]); /* Don't mess with the interrupt vector (R2, unused by us) and * master interrupt control (R9). We make sure this is setup * properly at probe time then never touch it again. */ /* Disable baud generator. */ write_zsreg(channel, R14, regs[R14] & ~BRENAB); /* Clock mode control. */ write_zsreg(channel, R11, regs[R11]); /* Lower and upper byte of baud rate generator divisor. */ write_zsreg(channel, R12, regs[R12]); write_zsreg(channel, R13, regs[R13]); /* Now rewrite R14, with BRENAB (if set). */ write_zsreg(channel, R14, regs[R14]); /* External status interrupt control. */ write_zsreg(channel, R15, regs[R15]); /* Reset external status interrupts. */ write_zsreg(channel, R0, RES_EXT_INT); write_zsreg(channel, R0, RES_EXT_INT); /* Rewrite R3/R5, this time without enables masked. */ write_zsreg(channel, R3, regs[R3]); write_zsreg(channel, R5, regs[R5]); /* Rewrite R1, this time without IRQ enabled masked. */ write_zsreg(channel, R1, regs[R1]); } /* Reprogram the Zilog channel HW registers with the copies found in the * software state struct. If the transmitter is busy, we defer this update * until the next TX complete interrupt. Else, we do it right now. * * The UART port lock must be held and local interrupts disabled. */ static void sunzilog_maybe_update_regs(struct uart_sunzilog_port *up, struct zilog_channel __iomem *channel) { if (!ZS_REGS_HELD(up)) { if (ZS_TX_ACTIVE(up)) { up->flags |= SUNZILOG_FLAG_REGS_HELD; } else { __load_zsregs(channel, up->curregs); } } } static void sunzilog_change_mouse_baud(struct uart_sunzilog_port *up) { unsigned int cur_cflag = up->cflag; int brg, new_baud; up->cflag &= ~CBAUD; up->cflag |= suncore_mouse_baud_cflag_next(cur_cflag, &new_baud); brg = BPS_TO_BRG(new_baud, ZS_CLOCK / ZS_CLOCK_DIVISOR); up->curregs[R12] = (brg & 0xff); up->curregs[R13] = (brg >> 8) & 0xff; sunzilog_maybe_update_regs(up, ZILOG_CHANNEL_FROM_PORT(&up->port)); } static void sunzilog_kbdms_receive_chars(struct uart_sunzilog_port *up, unsigned char ch, int is_break, struct pt_regs *regs) { if (ZS_IS_KEYB(up)) { /* Stop-A is handled by drivers/char/keyboard.c now. */ #ifdef CONFIG_SERIO if (up->serio_open) serio_interrupt(up->serio, ch, 0, regs); #endif } else if (ZS_IS_MOUSE(up)) { int ret = suncore_mouse_baud_detection(ch, is_break); switch (ret) { case 2: sunzilog_change_mouse_baud(up); /* fallthru */ case 1: break; case 0: #ifdef CONFIG_SERIO if (up->serio_open) serio_interrupt(up->serio, ch, 0, regs); #endif break; }; } } static struct tty_struct * sunzilog_receive_chars(struct uart_sunzilog_port *up, struct zilog_channel __iomem *channel, struct pt_regs *regs) { struct tty_struct *tty; unsigned char ch, r1, flag; tty = NULL; if (up->port.info != NULL && /* Unopened serial console */ up->port.info->tty != NULL) /* Keyboard || mouse */ tty = up->port.info->tty; for (;;) { r1 = read_zsreg(channel, R1); if (r1 & (PAR_ERR | Rx_OVR | CRC_ERR)) { sbus_writeb(ERR_RES, &channel->control); ZSDELAY(); ZS_WSYNC(channel); } ch = sbus_readb(&channel->control); ZSDELAY(); /* This funny hack depends upon BRK_ABRT not interfering * with the other bits we care about in R1. */ if (ch & BRK_ABRT) r1 |= BRK_ABRT; if (!(ch & Rx_CH_AV)) break; ch = sbus_readb(&channel->data); ZSDELAY(); ch &= up->parity_mask; if (unlikely(ZS_IS_KEYB(up)) || unlikely(ZS_IS_MOUSE(up))) { sunzilog_kbdms_receive_chars(up, ch, 0, regs); continue; } if (tty == NULL) { uart_handle_sysrq_char(&up->port, ch, regs); continue; } /* A real serial line, record the character and status. */ flag = TTY_NORMAL; up->port.icount.rx++; if (r1 & (BRK_ABRT | PAR_ERR | Rx_OVR | CRC_ERR)) { if (r1 & BRK_ABRT) { r1 &= ~(PAR_ERR | CRC_ERR); up->port.icount.brk++; if (uart_handle_break(&up->port)) continue; } else if (r1 & PAR_ERR) up->port.icount.parity++; else if (r1 & CRC_ERR) up->port.icount.frame++; if (r1 & Rx_OVR) up->port.icount.overrun++; r1 &= up->port.read_status_mask; if (r1 & BRK_ABRT) flag = TTY_BREAK; else if (r1 & PAR_ERR) flag = TTY_PARITY; else if (r1 & CRC_ERR) flag = TTY_FRAME; } if (uart_handle_sysrq_char(&up->port, ch, regs)) continue; if (up->port.ignore_status_mask == 0xff || (r1 & up->port.ignore_status_mask) == 0) { tty_insert_flip_char(tty, ch, flag); } if (r1 & Rx_OVR) tty_insert_flip_char(tty, 0, TTY_OVERRUN); } return tty; } static void sunzilog_status_handle(struct uart_sunzilog_port *up, struct zilog_channel __iomem *channel, struct pt_regs *regs) { unsigned char status; status = sbus_readb(&channel->control); ZSDELAY(); sbus_writeb(RES_EXT_INT, &channel->control); ZSDELAY(); ZS_WSYNC(channel); if (status & BRK_ABRT) { if (ZS_IS_MOUSE(up)) sunzilog_kbdms_receive_chars(up, 0, 1, regs); if (ZS_IS_CONS(up)) { /* Wait for BREAK to deassert to avoid potentially * confusing the PROM. */ while (1) { status = sbus_readb(&channel->control); ZSDELAY(); if (!(status & BRK_ABRT)) break; } sun_do_break(); return; } } if (ZS_WANTS_MODEM_STATUS(up)) { if (status & SYNC) up->port.icount.dsr++; /* The Zilog just gives us an interrupt when DCD/CTS/etc. change. * But it does not tell us which bit has changed, we have to keep * track of this ourselves. */ if ((status ^ up->prev_status) ^ DCD) uart_handle_dcd_change(&up->port, (status & DCD)); if ((status ^ up->prev_status) ^ CTS) uart_handle_cts_change(&up->port, (status & CTS)); wake_up_interruptible(&up->port.info->delta_msr_wait); } up->prev_status = status; } static void sunzilog_transmit_chars(struct uart_sunzilog_port *up, struct zilog_channel __iomem *channel) { struct circ_buf *xmit; if (ZS_IS_CONS(up)) { unsigned char status = sbus_readb(&channel->control); ZSDELAY(); /* TX still busy? Just wait for the next TX done interrupt. * * It can occur because of how we do serial console writes. It would * be nice to transmit console writes just like we normally would for * a TTY line. (ie. buffered and TX interrupt driven). That is not * easy because console writes cannot sleep. One solution might be * to poll on enough port->xmit space becomming free. -DaveM */ if (!(status & Tx_BUF_EMP)) return; } up->flags &= ~SUNZILOG_FLAG_TX_ACTIVE; if (ZS_REGS_HELD(up)) { __load_zsregs(channel, up->curregs); up->flags &= ~SUNZILOG_FLAG_REGS_HELD; } if (ZS_TX_STOPPED(up)) { up->flags &= ~SUNZILOG_FLAG_TX_STOPPED; goto ack_tx_int; } if (up->port.x_char) { up->flags |= SUNZILOG_FLAG_TX_ACTIVE; sbus_writeb(up->port.x_char, &channel->data); ZSDELAY(); ZS_WSYNC(channel); up->port.icount.tx++; up->port.x_char = 0; return; } if (up->port.info == NULL) goto ack_tx_int; xmit = &up->port.info->xmit; if (uart_circ_empty(xmit)) goto ack_tx_int; if (uart_tx_stopped(&up->port)) goto ack_tx_int; up->flags |= SUNZILOG_FLAG_TX_ACTIVE; sbus_writeb(xmit->buf[xmit->tail], &channel->data); ZSDELAY(); ZS_WSYNC(channel); xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1); up->port.icount.tx++; if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS) uart_write_wakeup(&up->port); return; ack_tx_int: sbus_writeb(RES_Tx_P, &channel->control); ZSDELAY(); ZS_WSYNC(channel); } static irqreturn_t sunzilog_interrupt(int irq, void *dev_id, struct pt_regs *regs) { struct uart_sunzilog_port *up = dev_id; while (up) { struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(&up->port); struct tty_struct *tty; unsigned char r3; spin_lock(&up->port.lock); r3 = read_zsreg(channel, R3); /* Channel A */ tty = NULL; if (r3 & (CHAEXT | CHATxIP | CHARxIP)) { sbus_writeb(RES_H_IUS, &channel->control); ZSDELAY(); ZS_WSYNC(channel); if (r3 & CHARxIP) tty = sunzilog_receive_chars(up, channel, regs); if (r3 & CHAEXT) sunzilog_status_handle(up, channel, regs); if (r3 & CHATxIP) sunzilog_transmit_chars(up, channel); } spin_unlock(&up->port.lock); if (tty) tty_flip_buffer_push(tty); /* Channel B */ up = up->next; channel = ZILOG_CHANNEL_FROM_PORT(&up->port); spin_lock(&up->port.lock); tty = NULL; if (r3 & (CHBEXT | CHBTxIP | CHBRxIP)) { sbus_writeb(RES_H_IUS, &channel->control); ZSDELAY(); ZS_WSYNC(channel); if (r3 & CHBRxIP) tty = sunzilog_receive_chars(up, channel, regs); if (r3 & CHBEXT) sunzilog_status_handle(up, channel, regs); if (r3 & CHBTxIP) sunzilog_transmit_chars(up, channel); } spin_unlock(&up->port.lock); if (tty) tty_flip_buffer_push(tty); up = up->next; } return IRQ_HANDLED; } /* A convenient way to quickly get R0 status. The caller must _not_ hold the * port lock, it is acquired here. */ static __inline__ unsigned char sunzilog_read_channel_status(struct uart_port *port) { struct zilog_channel __iomem *channel; unsigned char status; channel = ZILOG_CHANNEL_FROM_PORT(port); status = sbus_readb(&channel->control); ZSDELAY(); return status; } /* The port lock is not held. */ static unsigned int sunzilog_tx_empty(struct uart_port *port) { unsigned long flags; unsigned char status; unsigned int ret; spin_lock_irqsave(&port->lock, flags); status = sunzilog_read_channel_status(port); spin_unlock_irqrestore(&port->lock, flags); if (status & Tx_BUF_EMP) ret = TIOCSER_TEMT; else ret = 0; return ret; } /* The port lock is held and interrupts are disabled. */ static unsigned int sunzilog_get_mctrl(struct uart_port *port) { unsigned char status; unsigned int ret; status = sunzilog_read_channel_status(port); ret = 0; if (status & DCD) ret |= TIOCM_CAR; if (status & SYNC) ret |= TIOCM_DSR; if (status & CTS) ret |= TIOCM_CTS; return ret; } /* The port lock is held and interrupts are disabled. */ static void sunzilog_set_mctrl(struct uart_port *port, unsigned int mctrl) { struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port; struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port); unsigned char set_bits, clear_bits; set_bits = clear_bits = 0; if (mctrl & TIOCM_RTS) set_bits |= RTS; else clear_bits |= RTS; if (mctrl & TIOCM_DTR) set_bits |= DTR; else clear_bits |= DTR; /* NOTE: Not subject to 'transmitter active' rule. */ up->curregs[R5] |= set_bits; up->curregs[R5] &= ~clear_bits; write_zsreg(channel, R5, up->curregs[R5]); } /* The port lock is held and interrupts are disabled. */ static void sunzilog_stop_tx(struct uart_port *port) { struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port; up->flags |= SUNZILOG_FLAG_TX_STOPPED; } /* The port lock is held and interrupts are disabled. */ static void sunzilog_start_tx(struct uart_port *port) { struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port; struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port); unsigned char status; up->flags |= SUNZILOG_FLAG_TX_ACTIVE; up->flags &= ~SUNZILOG_FLAG_TX_STOPPED; status = sbus_readb(&channel->control); ZSDELAY(); /* TX busy? Just wait for the TX done interrupt. */ if (!(status & Tx_BUF_EMP)) return; /* Send the first character to jump-start the TX done * IRQ sending engine. */ if (port->x_char) { sbus_writeb(port->x_char, &channel->data); ZSDELAY(); ZS_WSYNC(channel); port->icount.tx++; port->x_char = 0; } else { struct circ_buf *xmit = &port->info->xmit; sbus_writeb(xmit->buf[xmit->tail], &channel->data); ZSDELAY(); ZS_WSYNC(channel); xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1); port->icount.tx++; if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS) uart_write_wakeup(&up->port); } } /* The port lock is held. */ static void sunzilog_stop_rx(struct uart_port *port) { struct uart_sunzilog_port *up = UART_ZILOG(port); struct zilog_channel __iomem *channel; if (ZS_IS_CONS(up)) return; channel = ZILOG_CHANNEL_FROM_PORT(port); /* Disable all RX interrupts. */ up->curregs[R1] &= ~RxINT_MASK; sunzilog_maybe_update_regs(up, channel); } /* The port lock is held. */ static void sunzilog_enable_ms(struct uart_port *port) { struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port; struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port); unsigned char new_reg; new_reg = up->curregs[R15] | (DCDIE | SYNCIE | CTSIE); if (new_reg != up->curregs[R15]) { up->curregs[R15] = new_reg; /* NOTE: Not subject to 'transmitter active' rule. */ write_zsreg(channel, R15, up->curregs[R15]); } } /* The port lock is not held. */ static void sunzilog_break_ctl(struct uart_port *port, int break_state) { struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port; struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port); unsigned char set_bits, clear_bits, new_reg; unsigned long flags; set_bits = clear_bits = 0; if (break_state) set_bits |= SND_BRK; else clear_bits |= SND_BRK; spin_lock_irqsave(&port->lock, flags); new_reg = (up->curregs[R5] | set_bits) & ~clear_bits; if (new_reg != up->curregs[R5]) { up->curregs[R5] = new_reg; /* NOTE: Not subject to 'transmitter active' rule. */ write_zsreg(channel, R5, up->curregs[R5]); } spin_unlock_irqrestore(&port->lock, flags); } static void __sunzilog_startup(struct uart_sunzilog_port *up) { struct zilog_channel __iomem *channel; channel = ZILOG_CHANNEL_FROM_PORT(&up->port); up->prev_status = sbus_readb(&channel->control); /* Enable receiver and transmitter. */ up->curregs[R3] |= RxENAB; up->curregs[R5] |= TxENAB; up->curregs[R1] |= EXT_INT_ENAB | INT_ALL_Rx | TxINT_ENAB; sunzilog_maybe_update_regs(up, channel); } static int sunzilog_startup(struct uart_port *port) { struct uart_sunzilog_port *up = UART_ZILOG(port); unsigned long flags; if (ZS_IS_CONS(up)) return 0; spin_lock_irqsave(&port->lock, flags); __sunzilog_startup(up); spin_unlock_irqrestore(&port->lock, flags); return 0; } /* * The test for ZS_IS_CONS is explained by the following e-mail: ***** * From: Russell King * Date: Sun, 8 Dec 2002 10:18:38 +0000 * * On Sun, Dec 08, 2002 at 02:43:36AM -0500, Pete Zaitcev wrote: * > I boot my 2.5 boxes using "console=ttyS0,9600" argument, * > and I noticed that something is not right with reference * > counting in this case. It seems that when the console * > is open by kernel initially, this is not accounted * > as an open, and uart_startup is not called. * * That is correct. We are unable to call uart_startup when the serial * console is initialised because it may need to allocate memory (as * request_irq does) and the memory allocators may not have been * initialised. * * 1. initialise the port into a state where it can send characters in the * console write method. * * 2. don't do the actual hardware shutdown in your shutdown() method (but * do the normal software shutdown - ie, free irqs etc) ***** */ static void sunzilog_shutdown(struct uart_port *port) { struct uart_sunzilog_port *up = UART_ZILOG(port); struct zilog_channel __iomem *channel; unsigned long flags; if (ZS_IS_CONS(up)) return; spin_lock_irqsave(&port->lock, flags); channel = ZILOG_CHANNEL_FROM_PORT(port); /* Disable receiver and transmitter. */ up->curregs[R3] &= ~RxENAB; up->curregs[R5] &= ~TxENAB; /* Disable all interrupts and BRK assertion. */ up->curregs[R1] &= ~(EXT_INT_ENAB | TxINT_ENAB | RxINT_MASK); up->curregs[R5] &= ~SND_BRK; sunzilog_maybe_update_regs(up, channel); spin_unlock_irqrestore(&port->lock, flags); } /* Shared by TTY driver and serial console setup. The port lock is held * and local interrupts are disabled. */ static void sunzilog_convert_to_zs(struct uart_sunzilog_port *up, unsigned int cflag, unsigned int iflag, int brg) { up->curregs[R10] = NRZ; up->curregs[R11] = TCBR | RCBR; /* Program BAUD and clock source. */ up->curregs[R4] &= ~XCLK_MASK; up->curregs[R4] |= X16CLK; up->curregs[R12] = brg & 0xff; up->curregs[R13] = (brg >> 8) & 0xff; up->curregs[R14] = BRSRC | BRENAB; /* Character size, stop bits, and parity. */ up->curregs[3] &= ~RxN_MASK; up->curregs[5] &= ~TxN_MASK; switch (cflag & CSIZE) { case CS5: up->curregs[3] |= Rx5; up->curregs[5] |= Tx5; up->parity_mask = 0x1f; break; case CS6: up->curregs[3] |= Rx6; up->curregs[5] |= Tx6; up->parity_mask = 0x3f; break; case CS7: up->curregs[3] |= Rx7; up->curregs[5] |= Tx7; up->parity_mask = 0x7f; break; case CS8: default: up->curregs[3] |= Rx8; up->curregs[5] |= Tx8; up->parity_mask = 0xff; break; }; up->curregs[4] &= ~0x0c; if (cflag & CSTOPB) up->curregs[4] |= SB2; else up->curregs[4] |= SB1; if (cflag & PARENB) up->curregs[4] |= PAR_ENAB; else up->curregs[4] &= ~PAR_ENAB; if (!(cflag & PARODD)) up->curregs[4] |= PAR_EVEN; else up->curregs[4] &= ~PAR_EVEN; up->port.read_status_mask = Rx_OVR; if (iflag & INPCK) up->port.read_status_mask |= CRC_ERR | PAR_ERR; if (iflag & (BRKINT | PARMRK)) up->port.read_status_mask |= BRK_ABRT; up->port.ignore_status_mask = 0; if (iflag & IGNPAR) up->port.ignore_status_mask |= CRC_ERR | PAR_ERR; if (iflag & IGNBRK) { up->port.ignore_status_mask |= BRK_ABRT; if (iflag & IGNPAR) up->port.ignore_status_mask |= Rx_OVR; } if ((cflag & CREAD) == 0) up->port.ignore_status_mask = 0xff; } /* The port lock is not held. */ static void sunzilog_set_termios(struct uart_port *port, struct termios *termios, struct termios *old) { struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port; unsigned long flags; int baud, brg; baud = uart_get_baud_rate(port, termios, old, 1200, 76800); spin_lock_irqsave(&up->port.lock, flags); brg = BPS_TO_BRG(baud, ZS_CLOCK / ZS_CLOCK_DIVISOR); sunzilog_convert_to_zs(up, termios->c_cflag, termios->c_iflag, brg); if (UART_ENABLE_MS(&up->port, termios->c_cflag)) up->flags |= SUNZILOG_FLAG_MODEM_STATUS; else up->flags &= ~SUNZILOG_FLAG_MODEM_STATUS; up->cflag = termios->c_cflag; sunzilog_maybe_update_regs(up, ZILOG_CHANNEL_FROM_PORT(port)); uart_update_timeout(port, termios->c_cflag, baud); spin_unlock_irqrestore(&up->port.lock, flags); } static const char *sunzilog_type(struct uart_port *port) { return "SunZilog"; } /* We do not request/release mappings of the registers here, this * happens at early serial probe time. */ static void sunzilog_release_port(struct uart_port *port) { } static int sunzilog_request_port(struct uart_port *port) { return 0; } /* These do not need to do anything interesting either. */ static void sunzilog_config_port(struct uart_port *port, int flags) { } /* We do not support letting the user mess with the divisor, IRQ, etc. */ static int sunzilog_verify_port(struct uart_port *port, struct serial_struct *ser) { return -EINVAL; } static struct uart_ops sunzilog_pops = { .tx_empty = sunzilog_tx_empty, .set_mctrl = sunzilog_set_mctrl, .get_mctrl = sunzilog_get_mctrl, .stop_tx = sunzilog_stop_tx, .start_tx = sunzilog_start_tx, .stop_rx = sunzilog_stop_rx, .enable_ms = sunzilog_enable_ms, .break_ctl = sunzilog_break_ctl, .startup = sunzilog_startup, .shutdown = sunzilog_shutdown, .set_termios = sunzilog_set_termios, .type = sunzilog_type, .release_port = sunzilog_release_port, .request_port = sunzilog_request_port, .config_port = sunzilog_config_port, .verify_port = sunzilog_verify_port, }; static struct uart_sunzilog_port *sunzilog_port_table; static struct zilog_layout __iomem **sunzilog_chip_regs; static struct uart_sunzilog_port *sunzilog_irq_chain; static int zilog_irq = -1; static struct uart_driver sunzilog_reg = { .owner = THIS_MODULE, .driver_name = "ttyS", .devfs_name = "tts/", .dev_name = "ttyS", .major = TTY_MAJOR, }; static void * __init alloc_one_table(unsigned long size) { void *ret; ret = kmalloc(size, GFP_KERNEL); if (ret != NULL) memset(ret, 0, size); return ret; } static void __init sunzilog_alloc_tables(void) { sunzilog_port_table = alloc_one_table(NUM_CHANNELS * sizeof(struct uart_sunzilog_port)); sunzilog_chip_regs = alloc_one_table(NUM_SUNZILOG * sizeof(struct zilog_layout __iomem *)); if (sunzilog_port_table == NULL || sunzilog_chip_regs == NULL) { prom_printf("SunZilog: Cannot allocate tables.\n"); prom_halt(); } } #ifdef CONFIG_SPARC64 /* We used to attempt to use the address property of the Zilog device node * but that totally is not necessary on sparc64. */ static struct zilog_layout __iomem * __init get_zs_sun4u(int chip, int zsnode) { void __iomem *mapped_addr; unsigned int sun4u_ino; struct sbus_bus *sbus = NULL; struct sbus_dev *sdev = NULL; int err; if (central_bus == NULL) { for_each_sbus(sbus) { for_each_sbusdev(sdev, sbus) { if (sdev->prom_node == zsnode) goto found; } } } found: if (sdev == NULL && central_bus == NULL) { prom_printf("SunZilog: sdev&¢ral == NULL for " "Zilog %d in get_zs_sun4u.\n", chip); prom_halt(); } if (central_bus == NULL) { mapped_addr = sbus_ioremap(&sdev->resource[0], 0, PAGE_SIZE, "Zilog Registers"); } else { struct linux_prom_registers zsregs[1]; err = prom_getproperty(zsnode, "reg", (char *) &zsregs[0], sizeof(zsregs)); if (err == -1) { prom_printf("SunZilog: Cannot map " "Zilog %d regs on " "central bus.\n", chip); prom_halt(); } apply_fhc_ranges(central_bus->child, &zsregs[0], 1); apply_central_ranges(central_bus, &zsregs[0], 1); mapped_addr = (void __iomem *) ((((u64)zsregs[0].which_io)<<32UL) | ((u64)zsregs[0].phys_addr)); } if (zilog_irq == -1) { if (central_bus) { unsigned long iclr, imap; iclr = central_bus->child->fhc_regs.uregs + FHC_UREGS_ICLR; imap = central_bus->child->fhc_regs.uregs + FHC_UREGS_IMAP; zilog_irq = build_irq(12, 0, iclr, imap); } else { err = prom_getproperty(zsnode, "interrupts", (char *) &sun4u_ino, sizeof(sun4u_ino)); zilog_irq = sbus_build_irq(sbus_root, sun4u_ino); } } return (struct zilog_layout __iomem *) mapped_addr; } #else /* CONFIG_SPARC64 */ /* * XXX The sun4d case is utterly screwed: it tries to re-walk the tree * (for the 3rd time) in order to find bootbus and cpu. Streamline it. */ static struct zilog_layout __iomem * __init get_zs_sun4cmd(int chip, int node) { struct linux_prom_irqs irq_info[2]; void __iomem *mapped_addr = NULL; int zsnode, cpunode, bbnode; struct linux_prom_registers zsreg[4]; struct resource res; if (sparc_cpu_model == sun4d) { int walk; zsnode = 0; bbnode = 0; cpunode = 0; for (walk = prom_getchild(prom_root_node); (walk = prom_searchsiblings(walk, "cpu-unit")) != 0; walk = prom_getsibling(walk)) { bbnode = prom_getchild(walk); if (bbnode && (bbnode = prom_searchsiblings(bbnode, "bootbus"))) { if ((zsnode = prom_getchild(bbnode)) == node) { cpunode = walk; break; } } } if (!walk) { prom_printf("SunZilog: Cannot find the %d'th bootbus on sun4d.\n", (chip / 2)); prom_halt(); } if (prom_getproperty(zsnode, "reg", (char *) zsreg, sizeof(zsreg)) == -1) { prom_printf("SunZilog: Cannot map Zilog %d\n", chip); prom_halt(); } /* XXX Looks like an off by one? */ prom_apply_generic_ranges(bbnode, cpunode, zsreg, 1); res.start = zsreg[0].phys_addr; res.end = res.start + (8 - 1); res.flags = zsreg[0].which_io | IORESOURCE_IO; mapped_addr = sbus_ioremap(&res, 0, 8, "Zilog Serial"); } else { zsnode = node; #if 0 /* XXX When was this used? */ if (prom_getintdefault(zsnode, "slave", -1) != chipid) { zsnode = prom_getsibling(zsnode); continue; } #endif /* * "address" is only present on ports that OBP opened * (from Mitch Bradley's "Hitchhiker's Guide to OBP"). * We do not use it. */ if (prom_getproperty(zsnode, "reg", (char *) zsreg, sizeof(zsreg)) == -1) { prom_printf("SunZilog: Cannot map Zilog %d\n", chip); prom_halt(); } if (sparc_cpu_model == sun4m) /* Crude. Pass parent. XXX */ prom_apply_obio_ranges(zsreg, 1); res.start = zsreg[0].phys_addr; res.end = res.start + (8 - 1); res.flags = zsreg[0].which_io | IORESOURCE_IO; mapped_addr = sbus_ioremap(&res, 0, 8, "Zilog Serial"); } if (prom_getproperty(zsnode, "intr", (char *) irq_info, sizeof(irq_info)) % sizeof(struct linux_prom_irqs)) { prom_printf("SunZilog: Cannot get IRQ property for Zilog %d.\n", chip); prom_halt(); } if (zilog_irq == -1) { zilog_irq = irq_info[0].pri; } else if (zilog_irq != irq_info[0].pri) { /* XXX. Dumb. Should handle per-chip IRQ, for add-ons. */ prom_printf("SunZilog: Inconsistent IRQ layout for Zilog %d.\n", chip); prom_halt(); } return (struct zilog_layout __iomem *) mapped_addr; } #endif /* !(CONFIG_SPARC64) */ /* Get the address of the registers for SunZilog instance CHIP. */ static struct zilog_layout __iomem * __init get_zs(int chip, int node) { if (chip < 0 || chip >= NUM_SUNZILOG) { prom_printf("SunZilog: Illegal chip number %d in get_zs.\n", chip); prom_halt(); } #ifdef CONFIG_SPARC64 return get_zs_sun4u(chip, node); #else if (sparc_cpu_model == sun4) { struct resource res; /* Not probe-able, hard code it. */ switch (chip) { case 0: res.start = 0xf1000000; break; case 1: res.start = 0xf0000000; break; }; zilog_irq = 12; res.end = (res.start + (8 - 1)); res.flags = IORESOURCE_IO; return sbus_ioremap(&res, 0, 8, "SunZilog"); } return get_zs_sun4cmd(chip, node); #endif } #define ZS_PUT_CHAR_MAX_DELAY 2000 /* 10 ms */ static void sunzilog_putchar(struct uart_port *port, int ch) { struct zilog_channel *channel = ZILOG_CHANNEL_FROM_PORT(port); int loops = ZS_PUT_CHAR_MAX_DELAY; /* This is a timed polling loop so do not switch the explicit * udelay with ZSDELAY as that is a NOP on some platforms. -DaveM */ do { unsigned char val = sbus_readb(&channel->control); if (val & Tx_BUF_EMP) { ZSDELAY(); break; } udelay(5); } while (--loops); sbus_writeb(ch, &channel->data); ZSDELAY(); ZS_WSYNC(channel); } #ifdef CONFIG_SERIO static DEFINE_SPINLOCK(sunzilog_serio_lock); static int sunzilog_serio_write(struct serio *serio, unsigned char ch) { struct uart_sunzilog_port *up = serio->port_data; unsigned long flags; spin_lock_irqsave(&sunzilog_serio_lock, flags); sunzilog_putchar(&up->port, ch); spin_unlock_irqrestore(&sunzilog_serio_lock, flags); return 0; } static int sunzilog_serio_open(struct serio *serio) { struct uart_sunzilog_port *up = serio->port_data; unsigned long flags; int ret; spin_lock_irqsave(&sunzilog_serio_lock, flags); if (!up->serio_open) { up->serio_open = 1; ret = 0; } else ret = -EBUSY; spin_unlock_irqrestore(&sunzilog_serio_lock, flags); return ret; } static void sunzilog_serio_close(struct serio *serio) { struct uart_sunzilog_port *up = serio->port_data; unsigned long flags; spin_lock_irqsave(&sunzilog_serio_lock, flags); up->serio_open = 0; spin_unlock_irqrestore(&sunzilog_serio_lock, flags); } #endif /* CONFIG_SERIO */ #ifdef CONFIG_SERIAL_SUNZILOG_CONSOLE static void sunzilog_console_write(struct console *con, const char *s, unsigned int count) { struct uart_sunzilog_port *up = &sunzilog_port_table[con->index]; unsigned long flags; spin_lock_irqsave(&up->port.lock, flags); uart_console_write(&up->port, s, count, sunzilog_putchar); udelay(2); spin_unlock_irqrestore(&up->port.lock, flags); } static int __init sunzilog_console_setup(struct console *con, char *options) { struct uart_sunzilog_port *up = &sunzilog_port_table[con->index]; unsigned long flags; int baud, brg; printk(KERN_INFO "Console: ttyS%d (SunZilog zs%d)\n", (sunzilog_reg.minor - 64) + con->index, con->index); /* Get firmware console settings. */ sunserial_console_termios(con); /* Firmware console speed is limited to 150-->38400 baud so * this hackish cflag thing is OK. */ switch (con->cflag & CBAUD) { case B150: baud = 150; break; case B300: baud = 300; break; case B600: baud = 600; break; case B1200: baud = 1200; break; case B2400: baud = 2400; break; case B4800: baud = 4800; break; default: case B9600: baud = 9600; break; case B19200: baud = 19200; break; case B38400: baud = 38400; break; }; brg = BPS_TO_BRG(baud, ZS_CLOCK / ZS_CLOCK_DIVISOR); spin_lock_irqsave(&up->port.lock, flags); up->curregs[R15] = BRKIE; sunzilog_convert_to_zs(up, con->cflag, 0, brg); sunzilog_set_mctrl(&up->port, TIOCM_DTR | TIOCM_RTS); __sunzilog_startup(up); spin_unlock_irqrestore(&up->port.lock, flags); return 0; } static struct console sunzilog_console = { .name = "ttyS", .write = sunzilog_console_write, .device = uart_console_device, .setup = sunzilog_console_setup, .flags = CON_PRINTBUFFER, .index = -1, .data = &sunzilog_reg, }; static int __init sunzilog_console_init(void) { int i; if (con_is_present()) return 0; for (i = 0; i < NUM_CHANNELS; i++) { int this_minor = sunzilog_reg.minor + i; if ((this_minor - 64) == (serial_console - 1)) break; } if (i == NUM_CHANNELS) return 0; sunzilog_console.index = i; sunzilog_port_table[i].flags |= SUNZILOG_FLAG_IS_CONS; register_console(&sunzilog_console); return 0; } static inline struct console *SUNZILOG_CONSOLE(void) { int i; if (con_is_present()) return NULL; for (i = 0; i < NUM_CHANNELS; i++) { int this_minor = sunzilog_reg.minor + i; if ((this_minor - 64) == (serial_console - 1)) break; } if (i == NUM_CHANNELS) return NULL; sunzilog_console.index = i; sunzilog_port_table[i].flags |= SUNZILOG_FLAG_IS_CONS; return &sunzilog_console; } #else #define SUNZILOG_CONSOLE() (NULL) #define sunzilog_console_init() do { } while (0) #endif /* * We scan the PROM tree recursively. This is the most reliable way * to find Zilog nodes on various platforms. However, we face an extreme * shortage of kernel stack, so we must be very careful. To that end, * we scan only to a certain depth, and we use a common property buffer * in the scan structure. */ #define ZS_PROPSIZE 128 #define ZS_SCAN_DEPTH 5 struct zs_probe_scan { int depth; void (*scanner)(struct zs_probe_scan *t, int node); int devices; char prop[ZS_PROPSIZE]; }; static int __inline__ sunzilog_node_ok(int node, const char *name, int len) { if (strncmp(name, "zs", len) == 0) return 1; /* Don't fold this procedure just yet. Compare to su_node_ok(). */ return 0; } static void __init sunzilog_scan(struct zs_probe_scan *t, int node) { int len; for (; node != 0; node = prom_getsibling(node)) { len = prom_getproperty(node, "name", t->prop, ZS_PROPSIZE); if (len <= 1) continue; /* Broken PROM node */ if (sunzilog_node_ok(node, t->prop, len)) { (*t->scanner)(t, node); } else { if (t->depth < ZS_SCAN_DEPTH) { t->depth++; sunzilog_scan(t, prom_getchild(node)); --t->depth; } } } } static void __init sunzilog_prepare(void) { struct uart_sunzilog_port *up; struct zilog_layout __iomem *rp; int channel, chip; /* * Temporary fix. */ for (channel = 0; channel < NUM_CHANNELS; channel++) spin_lock_init(&sunzilog_port_table[channel].port.lock); sunzilog_irq_chain = up = &sunzilog_port_table[0]; for (channel = 0; channel < NUM_CHANNELS - 1; channel++) up[channel].next = &up[channel + 1]; up[channel].next = NULL; for (chip = 0; chip < NUM_SUNZILOG; chip++) { rp = sunzilog_chip_regs[chip]; up[(chip * 2) + 0].port.membase = (void __iomem *)&rp->channelA; up[(chip * 2) + 1].port.membase = (void __iomem *)&rp->channelB; /* Channel A */ up[(chip * 2) + 0].port.iotype = UPIO_MEM; up[(chip * 2) + 0].port.irq = zilog_irq; up[(chip * 2) + 0].port.uartclk = ZS_CLOCK; up[(chip * 2) + 0].port.fifosize = 1; up[(chip * 2) + 0].port.ops = &sunzilog_pops; up[(chip * 2) + 0].port.type = PORT_SUNZILOG; up[(chip * 2) + 0].port.flags = 0; up[(chip * 2) + 0].port.line = (chip * 2) + 0; up[(chip * 2) + 0].flags |= SUNZILOG_FLAG_IS_CHANNEL_A; /* Channel B */ up[(chip * 2) + 1].port.iotype = UPIO_MEM; up[(chip * 2) + 1].port.irq = zilog_irq; up[(chip * 2) + 1].port.uartclk = ZS_CLOCK; up[(chip * 2) + 1].port.fifosize = 1; up[(chip * 2) + 1].port.ops = &sunzilog_pops; up[(chip * 2) + 1].port.type = PORT_SUNZILOG; up[(chip * 2) + 1].port.flags = 0; up[(chip * 2) + 1].port.line = (chip * 2) + 1; up[(chip * 2) + 1].flags |= 0; } } static void __init sunzilog_init_kbdms(struct uart_sunzilog_port *up, int channel) { int baud, brg; if (channel == KEYBOARD_LINE) { up->flags |= SUNZILOG_FLAG_CONS_KEYB; up->cflag = B1200 | CS8 | CLOCAL | CREAD; baud = 1200; } else { up->flags |= SUNZILOG_FLAG_CONS_MOUSE; up->cflag = B4800 | CS8 | CLOCAL | CREAD; baud = 4800; } printk(KERN_INFO "zs%d at 0x%p (irq = %s) is a SunZilog\n", channel, up->port.membase, __irq_itoa(zilog_irq)); up->curregs[R15] = BRKIE; brg = BPS_TO_BRG(baud, ZS_CLOCK / ZS_CLOCK_DIVISOR); sunzilog_convert_to_zs(up, up->cflag, 0, brg); sunzilog_set_mctrl(&up->port, TIOCM_DTR | TIOCM_RTS); __sunzilog_startup(up); } #ifdef CONFIG_SERIO static void __init sunzilog_register_serio(struct uart_sunzilog_port *up, int channel) { struct serio *serio; up->serio = serio = kmalloc(sizeof(struct serio), GFP_KERNEL); if (serio) { memset(serio, 0, sizeof(*serio)); serio->port_data = up; serio->id.type = SERIO_RS232; if (channel == KEYBOARD_LINE) { serio->id.proto = SERIO_SUNKBD; strlcpy(serio->name, "zskbd", sizeof(serio->name)); } else { serio->id.proto = SERIO_SUN; serio->id.extra = 1; strlcpy(serio->name, "zsms", sizeof(serio->name)); } strlcpy(serio->phys, (channel == KEYBOARD_LINE ? "zs/serio0" : "zs/serio1"), sizeof(serio->phys)); serio->write = sunzilog_serio_write; serio->open = sunzilog_serio_open; serio->close = sunzilog_serio_close; serio_register_port(serio); } else { printk(KERN_WARNING "zs%d: not enough memory for serio port\n", channel); } } #endif static void __init sunzilog_init_hw(void) { int i; for (i = 0; i < NUM_CHANNELS; i++) { struct uart_sunzilog_port *up = &sunzilog_port_table[i]; struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(&up->port); unsigned long flags; int baud, brg; spin_lock_irqsave(&up->port.lock, flags); if (ZS_IS_CHANNEL_A(up)) { write_zsreg(channel, R9, FHWRES); ZSDELAY_LONG(); (void) read_zsreg(channel, R0); } if (i == KEYBOARD_LINE || i == MOUSE_LINE) { sunzilog_init_kbdms(up, i); up->curregs[R9] |= (NV | MIE); write_zsreg(channel, R9, up->curregs[R9]); } else { /* Normal serial TTY. */ up->parity_mask = 0xff; up->curregs[R1] = EXT_INT_ENAB | INT_ALL_Rx | TxINT_ENAB; up->curregs[R4] = PAR_EVEN | X16CLK | SB1; up->curregs[R3] = RxENAB | Rx8; up->curregs[R5] = TxENAB | Tx8; up->curregs[R9] = NV | MIE; up->curregs[R10] = NRZ; up->curregs[R11] = TCBR | RCBR; baud = 9600; brg = BPS_TO_BRG(baud, ZS_CLOCK / ZS_CLOCK_DIVISOR); up->curregs[R12] = (brg & 0xff); up->curregs[R13] = (brg >> 8) & 0xff; up->curregs[R14] = BRSRC | BRENAB; __load_zsregs(channel, up->curregs); write_zsreg(channel, R9, up->curregs[R9]); } spin_unlock_irqrestore(&up->port.lock, flags); #ifdef CONFIG_SERIO if (i == KEYBOARD_LINE || i == MOUSE_LINE) sunzilog_register_serio(up, i); #endif } } static struct zilog_layout __iomem * __init get_zs(int chip, int node); static void __init sunzilog_scan_probe(struct zs_probe_scan *t, int node) { sunzilog_chip_regs[t->devices] = get_zs(t->devices, node); t->devices++; } static int __init sunzilog_ports_init(void) { struct zs_probe_scan scan; int ret; int uart_count; int i; printk(KERN_DEBUG "SunZilog: %d chips.\n", NUM_SUNZILOG); scan.scanner = sunzilog_scan_probe; scan.depth = 0; scan.devices = 0; sunzilog_scan(&scan, prom_getchild(prom_root_node)); sunzilog_prepare(); if (request_irq(zilog_irq, sunzilog_interrupt, SA_SHIRQ, "SunZilog", sunzilog_irq_chain)) { prom_printf("SunZilog: Unable to register zs interrupt handler.\n"); prom_halt(); } sunzilog_init_hw(); /* We can only init this once we have probed the Zilogs * in the system. Do not count channels assigned to keyboards * or mice when we are deciding how many ports to register. */ uart_count = 0; for (i = 0; i < NUM_CHANNELS; i++) { struct uart_sunzilog_port *up = &sunzilog_port_table[i]; if (ZS_IS_KEYB(up) || ZS_IS_MOUSE(up)) continue; uart_count++; } sunzilog_reg.nr = uart_count; sunzilog_reg.minor = sunserial_current_minor; ret = uart_register_driver(&sunzilog_reg); if (ret == 0) { sunzilog_reg.tty_driver->name_base = sunzilog_reg.minor - 64; sunzilog_reg.cons = SUNZILOG_CONSOLE(); sunserial_current_minor += uart_count; for (i = 0; i < NUM_CHANNELS; i++) { struct uart_sunzilog_port *up = &sunzilog_port_table[i]; if (ZS_IS_KEYB(up) || ZS_IS_MOUSE(up)) continue; if (uart_add_one_port(&sunzilog_reg, &up->port)) { printk(KERN_ERR "SunZilog: failed to add port zs%d\n", i); } } } return ret; } static void __init sunzilog_scan_count(struct zs_probe_scan *t, int node) { t->devices++; } static int __init sunzilog_ports_count(void) { struct zs_probe_scan scan; /* Sun4 Zilog setup is hard coded, no probing to do. */ if (sparc_cpu_model == sun4) return 2; scan.scanner = sunzilog_scan_count; scan.depth = 0; scan.devices = 0; sunzilog_scan(&scan, prom_getchild(prom_root_node)); return scan.devices; } static int __init sunzilog_init(void) { NUM_SUNZILOG = sunzilog_ports_count(); if (NUM_SUNZILOG == 0) return -ENODEV; sunzilog_alloc_tables(); sunzilog_ports_init(); return 0; } static void __exit sunzilog_exit(void) { int i; for (i = 0; i < NUM_CHANNELS; i++) { struct uart_sunzilog_port *up = &sunzilog_port_table[i]; if (ZS_IS_KEYB(up) || ZS_IS_MOUSE(up)) { #ifdef CONFIG_SERIO if (up->serio) { serio_unregister_port(up->serio); up->serio = NULL; } #endif } else uart_remove_one_port(&sunzilog_reg, &up->port); } uart_unregister_driver(&sunzilog_reg); } module_init(sunzilog_init); module_exit(sunzilog_exit); MODULE_AUTHOR("David S. Miller"); MODULE_DESCRIPTION("Sun Zilog serial port driver"); MODULE_LICENSE("GPL"); '>1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418 1419 1420 1421 1422 1423 1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465 1466 1467 1468 1469 1470 1471 1472 1473 1474 1475 1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498 1499 1500 1501 1502 1503 1504 1505 1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520 1521 1522 1523 1524 1525 1526 1527 1528 1529 1530 1531 1532 1533 1534 1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547 1548 1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564 1565 1566 1567 1568 1569 1570 1571 1572 1573 1574 1575 1576 1577 1578 1579 1580 1581 1582 1583 1584 1585 1586 1587 1588 1589 1590 1591 1592 1593 1594 1595 1596 1597 1598 1599 1600 1601 1602 1603 1604 1605 1606 1607 1608 1609 1610 1611 1612 1613 1614 1615 1616 1617 1618 1619 1620 1621 1622 1623 1624 1625 1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637 1638 1639 1640 1641 1642 1643 1644 1645 1646 1647 1648 1649 1650 1651 1652 1653 1654 1655 1656 1657 1658 1659 1660 1661 1662 1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673 1674 1675 1676 1677 1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691 1692 1693 1694 1695 1696 1697 1698 1699 1700 1701 1702 1703 1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721 1722 1723 1724 1725 1726 1727 1728 1729 1730 1731 1732 1733 1734 1735 1736 1737 1738 1739 1740 1741 1742 1743 1744 1745 1746 1747 1748 1749 1750 1751 1752 1753 1754 1755 1756 1757 1758 1759 1760 1761 1762 1763 1764 1765 1766 1767 1768 1769 1770 1771 1772 1773 1774 1775 1776 1777 1778 1779 1780 1781 1782 1783 1784 1785 1786 1787 1788 1789 1790 1791 1792 1793 1794 1795 1796 1797 1798 1799 1800 1801 1802 1803 1804 1805 1806 1807 1808 1809 1810 1811 1812 1813 1814 1815 1816 1817 1818 1819 1820 1821 1822 1823 1824 1825 1826 1827 1828 1829 1830 1831 1832 1833 1834 1835 1836 1837 1838 1839 1840 1841 1842 1843 1844 1845 1846 1847 1848 1849 1850 1851 1852 1853 1854 1855 1856 1857 1858 1859 1860 1861 1862 1863 1864 1865 1866 1867 1868 1869 1870 1871 1872 1873 1874 1875 1876 1877 1878 1879 1880 1881 1882 1883 1884 1885 1886 1887 1888 1889 1890 1891 1892 1893 1894 1895 1896 1897 1898 1899 1900 1901 1902 1903 1904 1905 1906 1907 1908 1909 1910 1911 1912 1913 1914 1915 1916 1917 1918 1919 1920 1921 1922 1923 1924 1925 1926 1927 1928 1929 1930 1931 1932 1933 1934 1935 1936 1937 1938 1939 1940 1941 1942 1943 1944 1945 1946 1947 1948 1949 1950 1951 1952 1953 1954 1955 1956 1957 1958 1959 1960 1961 1962 1963 1964 1965 1966 1967 1968 1969 1970 1971 1972 1973 1974 1975 1976 1977 1978 1979 1980 1981 1982 1983 1984 1985 1986 1987 1988 1989 1990 1991 1992 1993 1994 1995 1996 1997 1998 1999 2000 2001 2002 2003 2004 2005 2006 2007 2008 2009 2010 2011 2012 2013 2014 2015 2016 2017 2018 2019 2020 2021 2022 2023 2024 2025 2026 2027 2028 2029 2030 2031 2032 2033 2034 2035 2036 2037 2038 2039 2040 2041 2042 2043 2044 2045 2046 2047 2048 2049 2050 2051 2052 2053 2054 2055 2056 2057 2058 2059 2060 2061 2062 2063 2064 2065 2066 2067 2068 2069 2070 2071 2072 2073 2074 2075 2076 2077 2078 2079 2080 2081 2082 2083 2084 2085 2086 2087 2088 2089 2090 2091 2092 2093 2094 2095 2096 2097 2098 2099 2100 2101 2102 2103 2104 2105 2106 2107 2108 2109 2110 2111 2112 2113 2114 2115 2116 2117 2118 2119 2120 2121 2122 2123 2124 2125 2126 2127 2128 2129 2130 2131 2132 2133 2134 2135 2136 2137 2138 2139 2140 2141 2142 2143 2144 2145 2146 2147 2148 2149 2150 2151 2152 2153 2154 2155 2156 2157 2158 2159 2160 2161 2162 2163 2164 2165 2166 2167 2168 2169 2170 2171 2172 2173 2174 2175 2176 2177 2178 2179 2180 2181 2182 2183 2184 2185 2186 2187 2188 2189 2190 2191 2192 2193 2194 2195 2196 2197 2198 2199 2200 2201 2202 2203 2204 2205 2206 2207 2208 2209 2210 2211 2212 2213 2214 2215 2216 2217 2218 2219 2220 2221 2222 2223 2224 2225 2226 2227 2228 2229 2230 2231 2232 2233 2234 2235 2236 2237 2238 2239 2240 2241 2242 2243 2244 2245 2246 2247 2248 2249 2250 2251 2252 2253 2254 2255 2256 2257 2258 2259 2260 2261 2262 2263 2264 2265 2266 2267 2268 2269 2270 2271 2272 2273 2274 2275 2276 2277 2278 2279 2280 2281 2282 2283 2284 2285 2286 2287 2288 2289 2290 2291 2292 2293 2294 2295 2296 2297 2298 2299 2300 2301 2302 2303 2304 2305 2306 2307 2308 2309 2310 2311 2312 2313 2314 2315 2316 2317 2318 2319 2320 2321 2322 2323 2324 2325 2326 2327 2328 2329 2330 2331 2332 2333 2334 2335 2336 2337 2338 2339 2340 2341 2342 2343 2344 2345 2346 2347 2348 2349 2350 2351 2352 2353 2354 2355 2356 2357 2358 2359 2360 2361 2362 2363 2364 2365 2366 2367 2368 2369 2370 2371 2372 2373 2374 2375 2376 2377 2378 2379 2380 2381 2382 2383 2384 2385 2386 2387 2388 2389 2390 2391 2392 2393 2394 2395 2396 2397 2398 2399 2400 2401 2402 2403 2404 2405 2406 2407 2408 2409 2410 2411 2412 2413 2414 2415 2416 2417 2418 2419 2420 2421 2422 2423 2424 2425 2426 2427 2428 2429 2430 2431 2432 2433 2434 2435 2436 2437 2438 2439 2440 2441 2442 2443 2444 2445 2446 2447 2448 2449 2450 2451 2452 2453 2454 2455 2456 2457 2458 2459 2460 2461 2462 2463 2464 2465 2466 2467 2468 2469 2470 2471 2472 2473 2474 2475 2476 2477 2478 2479 2480 2481 2482 2483 2484 2485 2486 2487 2488 2489 2490 2491 2492 2493 2494 2495 2496 2497 2498 2499 2500 2501 2502 2503 2504 2505 2506 2507 2508 2509 2510 2511 2512 2513 2514 2515 2516 2517 2518 2519 2520 2521 2522 2523 2524 2525 2526 2527 2528 2529 2530 2531 2532 2533 2534 2535 2536 2537 2538 2539 2540 2541 2542 2543 2544 2545 2546 2547 2548 2549 2550 2551 2552 2553 2554 2555 2556 2557 2558 2559 2560 2561 2562 2563 2564 2565 2566 2567 2568 2569 2570 2571 2572 2573 2574 2575 2576 2577 2578 2579 2580 2581 2582 2583 2584 2585 2586 2587 2588 2589 2590 2591 2592 2593 2594 2595 2596 2597 2598 2599 2600 2601 2602 2603 2604 2605 2606 2607 2608 2609 2610 2611 2612 2613 2614 2615 2616 2617 2618 2619 2620 2621 2622 2623 2624 2625 2626 2627 2628 2629 2630 2631 2632 2633 2634 2635 2636 2637 2638 2639 2640 2641 2642 2643 2644 2645 2646 2647 2648 2649 2650 2651 2652 2653 2654 2655 2656 2657 2658 2659 
/*****************************************************************************
 * Linux PPP over L2TP (PPPoX/PPPoL2TP) Sockets
 *
 * PPPoX    --- Generic PPP encapsulation socket family
 * PPPoL2TP --- PPP over L2TP (RFC 2661)
 *
 * Version:	1.0.0
 *
 * Authors:	Martijn van Oosterhout <kleptog@svana.org>
 *		James Chapman (jchapman@katalix.com)
 * Contributors:
 *		Michal Ostrowski <mostrows@speakeasy.net>
 *		Arnaldo Carvalho de Melo <acme@xconectiva.com.br>
 *		David S. Miller (davem@redhat.com)
 *
 * License:
 *		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.
 *
 */

/* This driver handles only L2TP data frames; control frames are handled by a
 * userspace application.
 *
 * To send data in an L2TP session, userspace opens a PPPoL2TP socket and
 * attaches it to a bound UDP socket with local tunnel_id / session_id and
 * peer tunnel_id / session_id set. Data can then be sent or received using
 * regular socket sendmsg() / recvmsg() calls. Kernel parameters of the socket
 * can be read or modified using ioctl() or [gs]etsockopt() calls.
 *
 * When a PPPoL2TP socket is connected with local and peer session_id values
 * zero, the socket is treated as a special tunnel management socket.
 *
 * Here's example userspace code to create a socket for sending/receiving data
 * over an L2TP session:-
 *
 *	struct sockaddr_pppol2tp sax;
 *	int fd;
 *	int session_fd;
 *
 *	fd = socket(AF_PPPOX, SOCK_DGRAM, PX_PROTO_OL2TP);
 *
 *	sax.sa_family = AF_PPPOX;
 *	sax.sa_protocol = PX_PROTO_OL2TP;
 *	sax.pppol2tp.fd = tunnel_fd;	// bound UDP socket
 *	sax.pppol2tp.addr.sin_addr.s_addr = addr->sin_addr.s_addr;
 *	sax.pppol2tp.addr.sin_port = addr->sin_port;
 *	sax.pppol2tp.addr.sin_family = AF_INET;
 *	sax.pppol2tp.s_tunnel  = tunnel_id;
 *	sax.pppol2tp.s_session = session_id;
 *	sax.pppol2tp.d_tunnel  = peer_tunnel_id;
 *	sax.pppol2tp.d_session = peer_session_id;
 *
 *	session_fd = connect(fd, (struct sockaddr *)&sax, sizeof(sax));
 *
 * A pppd plugin that allows PPP traffic to be carried over L2TP using
 * this driver is available from the OpenL2TP project at
 * http://openl2tp.sourceforge.net.
 */

#include <linux/module.h>
#include <linux/string.h>
#include <linux/list.h>
#include <asm/uaccess.h>

#include <linux/kernel.h>
#include <linux/spinlock.h>
#include <linux/kthread.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/errno.h>
#include <linux/jiffies.h>

#include <linux/netdevice.h>
#include <linux/net.h>
#include <linux/inetdevice.h>
#include <linux/skbuff.h>
#include <linux/init.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include <linux/if_pppox.h>
#include <linux/if_pppol2tp.h>
#include <net/sock.h>
#include <linux/ppp_channel.h>
#include <linux/ppp_defs.h>
#include <linux/if_ppp.h>
#include <linux/file.h>
#include <linux/hash.h>
#include <linux/sort.h>
#include <linux/proc_fs.h>
#include <net/net_namespace.h>
#include <net/dst.h>
#include <net/ip.h>
#include <net/udp.h>
#include <net/xfrm.h>

#include <asm/byteorder.h>
#include <asm/atomic.h>


#define PPPOL2TP_DRV_VERSION	"V1.0"

/* L2TP header constants */
#define L2TP_HDRFLAG_T	   0x8000
#define L2TP_HDRFLAG_L	   0x4000
#define L2TP_HDRFLAG_S	   0x0800
#define L2TP_HDRFLAG_O	   0x0200
#define L2TP_HDRFLAG_P	   0x0100

#define L2TP_HDR_VER_MASK  0x000F
#define L2TP_HDR_VER	   0x0002

/* Space for UDP, L2TP and PPP headers */
#define PPPOL2TP_HEADER_OVERHEAD	40

/* Just some random numbers */
#define L2TP_TUNNEL_MAGIC	0x42114DDA
#define L2TP_SESSION_MAGIC	0x0C04EB7D

#define PPPOL2TP_HASH_BITS	4
#define PPPOL2TP_HASH_SIZE	(1 << PPPOL2TP_HASH_BITS)

/* Default trace flags */
#define PPPOL2TP_DEFAULT_DEBUG_FLAGS	0

#define PRINTK(_mask, _type, _lvl, _fmt, args...)			\
	do {								\
		if ((_mask) & (_type))					\
			printk(_lvl "PPPOL2TP: " _fmt, ##args);		\
	} while(0)

/* Number of bytes to build transmit L2TP headers.
 * Unfortunately the size is different depending on whether sequence numbers
 * are enabled.
 */
#define PPPOL2TP_L2TP_HDR_SIZE_SEQ		10
#define PPPOL2TP_L2TP_HDR_SIZE_NOSEQ		6

struct pppol2tp_tunnel;

/* Describes a session. It is the sk_user_data field in the PPPoL2TP
 * socket. Contains information to determine incoming packets and transmit
 * outgoing ones.
 */
struct pppol2tp_session
{
	int			magic;		/* should be
						 * L2TP_SESSION_MAGIC */
	int			owner;		/* pid that opened the socket */

	struct sock		*sock;		/* Pointer to the session
						 * PPPoX socket */
	struct sock		*tunnel_sock;	/* Pointer to the tunnel UDP
						 * socket */

	struct pppol2tp_addr	tunnel_addr;	/* Description of tunnel */

	struct pppol2tp_tunnel	*tunnel;	/* back pointer to tunnel
						 * context */

	char			name[20];	/* "sess xxxxx/yyyyy", where
						 * x=tunnel_id, y=session_id */
	int			mtu;
	int			mru;
	int			flags;		/* accessed by PPPIOCGFLAGS.
						 * Unused. */
	unsigned		recv_seq:1;	/* expect receive packets with
						 * sequence numbers? */
	unsigned		send_seq:1;	/* send packets with sequence
						 * numbers? */
	unsigned		lns_mode:1;	/* behave as LNS? LAC enables
						 * sequence numbers under
						 * control of LNS. */
	int			debug;		/* bitmask of debug message
						 * categories */
	int			reorder_timeout; /* configured reorder timeout
						  * (in jiffies) */
	u16			nr;		/* session NR state (receive) */
	u16			ns;		/* session NR state (send) */
	struct sk_buff_head	reorder_q;	/* receive reorder queue */
	struct pppol2tp_ioc_stats stats;
	struct hlist_node	hlist;		/* Hash list node */
};

/* The sk_user_data field of the tunnel's UDP socket. It contains info to track
 * all the associated sessions so incoming packets can be sorted out
 */
struct pppol2tp_tunnel
{
	int			magic;		/* Should be L2TP_TUNNEL_MAGIC */
	rwlock_t		hlist_lock;	/* protect session_hlist */
	struct hlist_head	session_hlist[PPPOL2TP_HASH_SIZE];
						/* hashed list of sessions,
						 * hashed by id */
	int			debug;		/* bitmask of debug message
						 * categories */
	char			name[12];	/* "tunl xxxxx" */
	struct pppol2tp_ioc_stats stats;

	void (*old_sk_destruct)(struct sock *);

	struct sock		*sock;		/* Parent socket */
	struct list_head	list;		/* Keep a list of all open
						 * prepared sockets */

	atomic_t		ref_count;
};

/* Private data stored for received packets in the skb.
 */
struct pppol2tp_skb_cb {
	u16			ns;
	u16			nr;
	u16			has_seq;
	u16			length;
	unsigned long		expires;
};

#define PPPOL2TP_SKB_CB(skb)	((struct pppol2tp_skb_cb *) &skb->cb[sizeof(struct inet_skb_parm)])

static int pppol2tp_xmit(struct ppp_channel *chan, struct sk_buff *skb);
static void pppol2tp_tunnel_free(struct pppol2tp_tunnel *tunnel);

static atomic_t pppol2tp_tunnel_count;
static atomic_t pppol2tp_session_count;
static struct ppp_channel_ops pppol2tp_chan_ops = { pppol2tp_xmit , NULL };
static struct proto_ops pppol2tp_ops;
static LIST_HEAD(pppol2tp_tunnel_list);
static DEFINE_RWLOCK(pppol2tp_tunnel_list_lock);

/* Helpers to obtain tunnel/session contexts from sockets.
 */
static inline struct pppol2tp_session *pppol2tp_sock_to_session(struct sock *sk)
{
	struct pppol2tp_session *session;

	if (sk == NULL)
		return NULL;

	sock_hold(sk);
	session = (struct pppol2tp_session *)(sk->sk_user_data);
	if (session == NULL) {
		sock_put(sk);
		goto out;
	}

	BUG_ON(session->magic != L2TP_SESSION_MAGIC);
out:
	return session;
}

static inline struct pppol2tp_tunnel *pppol2tp_sock_to_tunnel(struct sock *sk)
{
	struct pppol2tp_tunnel *tunnel;

	if (sk == NULL)
		return NULL;

	sock_hold(sk);
	tunnel = (struct pppol2tp_tunnel *)(sk->sk_user_data);
	if (tunnel == NULL) {
		sock_put(sk);
		goto out;
	}

	BUG_ON(tunnel->magic != L2TP_TUNNEL_MAGIC);
out:
	return tunnel;
}

/* Tunnel reference counts. Incremented per session that is added to
 * the tunnel.
 */
static inline void pppol2tp_tunnel_inc_refcount(struct pppol2tp_tunnel *tunnel)
{
	atomic_inc(&tunnel->ref_count);
}

static inline void pppol2tp_tunnel_dec_refcount(struct pppol2tp_tunnel *tunnel)
{
	if (atomic_dec_and_test(&tunnel->ref_count))
		pppol2tp_tunnel_free(tunnel);
}

/* Session hash list.
 * The session_id SHOULD be random according to RFC2661, but several
 * L2TP implementations (Cisco and Microsoft) use incrementing
 * session_ids.  So we do a real hash on the session_id, rather than a
 * simple bitmask.
 */
static inline struct hlist_head *
pppol2tp_session_id_hash(struct pppol2tp_tunnel *tunnel, u16 session_id)
{
	unsigned long hash_val = (unsigned long) session_id;
	return &tunnel->session_hlist[hash_long(hash_val, PPPOL2TP_HASH_BITS)];
}

/* Lookup a session by id
 */
static struct pppol2tp_session *
pppol2tp_session_find(struct pppol2tp_tunnel *tunnel, u16 session_id)
{
	struct hlist_head *session_list =
		pppol2tp_session_id_hash(tunnel, session_id);
	struct pppol2tp_session *session;
	struct hlist_node *walk;

	read_lock_bh(&tunnel->hlist_lock);
	hlist_for_each_entry(session, walk, session_list, hlist) {
		if (session->tunnel_addr.s_session == session_id) {
			read_unlock_bh(&tunnel->hlist_lock);
			return session;
		}
	}
	read_unlock_bh(&tunnel->hlist_lock);

	return NULL;
}

/* Lookup a tunnel by id
 */
static struct pppol2tp_tunnel *pppol2tp_tunnel_find(u16 tunnel_id)
{
	struct pppol2tp_tunnel *tunnel = NULL;

	read_lock_bh(&pppol2tp_tunnel_list_lock);
	list_for_each_entry(tunnel, &pppol2tp_tunnel_list, list) {
		if (tunnel->stats.tunnel_id == tunnel_id) {
			read_unlock_bh(&pppol2tp_tunnel_list_lock);
			return tunnel;
		}
	}
	read_unlock_bh(&pppol2tp_tunnel_list_lock);

	return NULL;
}

/*****************************************************************************
 * Receive data handling
 *****************************************************************************/

/* Queue a skb in order. We come here only if the skb has an L2TP sequence
 * number.
 */
static void pppol2tp_recv_queue_skb(struct pppol2tp_session *session, struct sk_buff *skb)
{
	struct sk_buff *skbp;
	struct sk_buff *tmp;
	u16 ns = PPPOL2TP_SKB_CB(skb)->ns;

	spin_lock_bh(&session->reorder_q.lock);
	skb_queue_walk_safe(&session->reorder_q, skbp, tmp) {
		if (PPPOL2TP_SKB_CB(skbp)->ns > ns) {
			__skb_queue_before(&session->reorder_q, skbp, skb);
			PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG,
			       "%s: pkt %hu, inserted before %hu, reorder_q len=%d\n",
			       session->name, ns, PPPOL2TP_SKB_CB(skbp)->ns,
			       skb_queue_len(&session->reorder_q));
			session->stats.rx_oos_packets++;
			goto out;
		}
	}

	__skb_queue_tail(&session->reorder_q, skb);

out:
	spin_unlock_bh(&session->reorder_q.lock);
}

/* Dequeue a single skb.
 */
static void pppol2tp_recv_dequeue_skb(struct pppol2tp_session *session, struct sk_buff *skb)
{
	struct pppol2tp_tunnel *tunnel = session->tunnel;
	int length = PPPOL2TP_SKB_CB(skb)->length;
	struct sock *session_sock = NULL;

	/* We're about to requeue the skb, so return resources
	 * to its current owner (a socket receive buffer).
	 */
	skb_orphan(skb);

	tunnel->stats.rx_packets++;
	tunnel->stats.rx_bytes += length;
	session->stats.rx_packets++;
	session->stats.rx_bytes += length;

	if (PPPOL2TP_SKB_CB(skb)->has_seq) {
		/* Bump our Nr */
		session->nr++;
		PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG,
		       "%s: updated nr to %hu\n", session->name, session->nr);
	}

	/* If the socket is bound, send it in to PPP's input queue. Otherwise
	 * queue it on the session socket.
	 */
	session_sock = session->sock;
	if (session_sock->sk_state & PPPOX_BOUND) {
		struct pppox_sock *po;
		PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG,
		       "%s: recv %d byte data frame, passing to ppp\n",
		       session->name, length);

		/* We need to forget all info related to the L2TP packet
		 * gathered in the skb as we are going to reuse the same
		 * skb for the inner packet.
		 * Namely we need to:
		 * - reset xfrm (IPSec) information as it applies to
		 *   the outer L2TP packet and not to the inner one
		 * - release the dst to force a route lookup on the inner
		 *   IP packet since skb->dst currently points to the dst
		 *   of the UDP tunnel
		 * - reset netfilter information as it doesn't apply
		 *   to the inner packet either
		 */
		secpath_reset(skb);
		dst_release(skb->dst);
		skb->dst = NULL;
		nf_reset(skb);

		po = pppox_sk(session_sock);
		ppp_input(&po->chan, skb);
	} else {
		PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_INFO,
		       "%s: socket not bound\n", session->name);

		/* Not bound. Nothing we can do, so discard. */
		session->stats.rx_errors++;
		kfree_skb(skb);
	}

	sock_put(session->sock);
}

/* Dequeue skbs from the session's reorder_q, subject to packet order.
 * Skbs that have been in the queue for too long are simply discarded.
 */
static void pppol2tp_recv_dequeue(struct pppol2tp_session *session)
{
	struct sk_buff *skb;
	struct sk_buff *tmp;

	/* If the pkt at the head of the queue has the nr that we
	 * expect to send up next, dequeue it and any other
	 * in-sequence packets behind it.
	 */
	spin_lock_bh(&session->reorder_q.lock);
	skb_queue_walk_safe(&session->reorder_q, skb, tmp) {
		if (time_after(jiffies, PPPOL2TP_SKB_CB(skb)->expires)) {
			session->stats.rx_seq_discards++;
			session->stats.rx_errors++;
			PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG,
			       "%s: oos pkt %hu len %d discarded (too old), "
			       "waiting for %hu, reorder_q_len=%d\n",
			       session->name, PPPOL2TP_SKB_CB(skb)->ns,
			       PPPOL2TP_SKB_CB(skb)->length, session->nr,
			       skb_queue_len(&session->reorder_q));
			__skb_unlink(skb, &session->reorder_q);
			kfree_skb(skb);
			sock_put(session->sock);
			continue;
		}

		if (PPPOL2TP_SKB_CB(skb)->has_seq) {
			if (PPPOL2TP_SKB_CB(skb)->ns != session->nr) {
				PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG,
				       "%s: holding oos pkt %hu len %d, "
				       "waiting for %hu, reorder_q_len=%d\n",
				       session->name, PPPOL2TP_SKB_CB(skb)->ns,
				       PPPOL2TP_SKB_CB(skb)->length, session->nr,
				       skb_queue_len(&session->reorder_q));
				goto out;
			}
		}
		__skb_unlink(skb, &session->reorder_q);

		/* Process the skb. We release the queue lock while we
		 * do so to let other contexts process the queue.
		 */
		spin_unlock_bh(&session->reorder_q.lock);
		pppol2tp_recv_dequeue_skb(session, skb);
		spin_lock_bh(&session->reorder_q.lock);
	}

out:
	spin_unlock_bh(&session->reorder_q.lock);
}

static inline int pppol2tp_verify_udp_checksum(struct sock *sk,
					       struct sk_buff *skb)
{
	struct udphdr *uh = udp_hdr(skb);
	u16 ulen = ntohs(uh->len);
	struct inet_sock *inet;
	__wsum psum;

	if (sk->sk_no_check || skb_csum_unnecessary(skb) || !uh->check)
		return 0;

	inet = inet_sk(sk);
	psum = csum_tcpudp_nofold(inet->saddr, inet->daddr, ulen,
				  IPPROTO_UDP, 0);

	if ((skb->ip_summed == CHECKSUM_COMPLETE) &&
	    !csum_fold(csum_add(psum, skb->csum)))
		return 0;

	skb->csum = psum;

	return __skb_checksum_complete(skb);
}

/* Internal receive frame. Do the real work of receiving an L2TP data frame
 * here. The skb is not on a list when we get here.
 * Returns 0 if the packet was a data packet and was successfully passed on.
 * Returns 1 if the packet was not a good data packet and could not be
 * forwarded.  All such packets are passed up to userspace to deal with.
 */
static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb)
{
	struct pppol2tp_session *session = NULL;
	struct pppol2tp_tunnel *tunnel;
	unsigned char *ptr, *optr;
	u16 hdrflags;
	u16 tunnel_id, session_id;
	int length;
	int offset;

	tunnel = pppol2tp_sock_to_tunnel(sock);
	if (tunnel == NULL)
		goto no_tunnel;

	if (tunnel->sock && pppol2tp_verify_udp_checksum(tunnel->sock, skb))
		goto discard_bad_csum;

	/* UDP always verifies the packet length. */
	__skb_pull(skb, sizeof(struct udphdr));

	/* Short packet? */
	if (!pskb_may_pull(skb, 12)) {
		PRINTK(tunnel->debug, PPPOL2TP_MSG_DATA, KERN_INFO,
		       "%s: recv short packet (len=%d)\n", tunnel->name, skb->len);
		goto error;
	}

	/* Point to L2TP header */
	optr = ptr = skb->data;

	/* Get L2TP header flags */
	hdrflags = ntohs(*(__be16*)ptr);

	/* Trace packet contents, if enabled */
	if (tunnel->debug & PPPOL2TP_MSG_DATA) {
		length = min(16u, skb->len);
		if (!pskb_may_pull(skb, length))
			goto error;

		printk(KERN_DEBUG "%s: recv: ", tunnel->name);

		offset = 0;
		do {
			printk(" %02X", ptr[offset]);
		} while (++offset < length);

		printk("\n");
	}

	/* Get length of L2TP packet */
	length = skb->len;

	/* If type is control packet, it is handled by userspace. */
	if (hdrflags & L2TP_HDRFLAG_T) {
		PRINTK(tunnel->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG,
		       "%s: recv control packet, len=%d\n", tunnel->name, length);
		goto error;
	}

	/* Skip flags */
	ptr += 2;

	/* If length is present, skip it */
	if (hdrflags & L2TP_HDRFLAG_L)
		ptr += 2;

	/* Extract tunnel and session ID */
	tunnel_id = ntohs(*(__be16 *) ptr);
	ptr += 2;
	session_id = ntohs(*(__be16 *) ptr);
	ptr += 2;

	/* Find the session context */
	session = pppol2tp_session_find(tunnel, session_id);
	if (!session) {
		/* Not found? Pass to userspace to deal with */
		PRINTK(tunnel->debug, PPPOL2TP_MSG_DATA, KERN_INFO,
		       "%s: no socket found (%hu/%hu). Passing up.\n",
		       tunnel->name, tunnel_id, session_id);
		goto error;
	}
	sock_hold(session->sock);

	/* The ref count on the socket was increased by the above call since
	 * we now hold a pointer to the session. Take care to do sock_put()
	 * when exiting this function from now on...
	 */

	/* Handle the optional sequence numbers.  If we are the LAC,
	 * enable/disable sequence numbers under the control of the LNS.  If
	 * no sequence numbers present but we were expecting them, discard
	 * frame.
	 */
	if (hdrflags & L2TP_HDRFLAG_S) {
		u16 ns, nr;
		ns = ntohs(*(__be16 *) ptr);
		ptr += 2;
		nr = ntohs(*(__be16 *) ptr);
		ptr += 2;

		/* Received a packet with sequence numbers. If we're the LNS,
		 * check if we sre sending sequence numbers and if not,
		 * configure it so.
		 */
		if ((!session->lns_mode) && (!session->send_seq)) {
			PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_INFO,
			       "%s: requested to enable seq numbers by LNS\n",
			       session->name);
			session->send_seq = -1;
		}

		/* Store L2TP info in the skb */
		PPPOL2TP_SKB_CB(skb)->ns = ns;
		PPPOL2TP_SKB_CB(skb)->nr = nr;
		PPPOL2TP_SKB_CB(skb)->has_seq = 1;

		PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG,
		       "%s: recv data ns=%hu, nr=%hu, session nr=%hu\n",
		       session->name, ns, nr, session->nr);
	} else {
		/* No sequence numbers.
		 * If user has configured mandatory sequence numbers, discard.
		 */
		if (session->recv_seq) {
			PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_WARNING,
			       "%s: recv data has no seq numbers when required. "
			       "Discarding\n", session->name);
			session->stats.rx_seq_discards++;
			goto discard;
		}

		/* If we're the LAC and we're sending sequence numbers, the
		 * LNS has requested that we no longer send sequence numbers.
		 * If we're the LNS and we're sending sequence numbers, the
		 * LAC is broken. Discard the frame.
		 */
		if ((!session->lns_mode) && (session->send_seq)) {
			PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_INFO,
			       "%s: requested to disable seq numbers by LNS\n",
			       session->name);
			session->send_seq = 0;
		} else if (session->send_seq) {
			PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_WARNING,
			       "%s: recv data has no seq numbers when required. "
			       "Discarding\n", session->name);
			session->stats.rx_seq_discards++;
			goto discard;
		}

		/* Store L2TP info in the skb */
		PPPOL2TP_SKB_CB(skb)->has_seq = 0;
	}

	/* If offset bit set, skip it. */
	if (hdrflags & L2TP_HDRFLAG_O) {
		offset = ntohs(*(__be16 *)ptr);
		ptr += 2 + offset;
	}

	offset = ptr - optr;
	if (!pskb_may_pull(skb, offset))
		goto discard;

	__skb_pull(skb, offset);

	/* Skip PPP header, if present.	 In testing, Microsoft L2TP clients
	 * don't send the PPP header (PPP header compression enabled), but
	 * other clients can include the header. So we cope with both cases
	 * here. The PPP header is always FF03 when using L2TP.
	 *
	 * Note that skb->data[] isn't dereferenced from a u16 ptr here since
	 * the field may be unaligned.
	 */
	if (!pskb_may_pull(skb, 2))
		goto discard;

	if ((skb->data[0] == 0xff) && (skb->data[1] == 0x03))
		skb_pull(skb, 2);

	/* Prepare skb for adding to the session's reorder_q.  Hold
	 * packets for max reorder_timeout or 1 second if not
	 * reordering.
	 */
	PPPOL2TP_SKB_CB(skb)->length = length;
	PPPOL2TP_SKB_CB(skb)->expires = jiffies +
		(session->reorder_timeout ? session->reorder_timeout : HZ);

	/* Add packet to the session's receive queue. Reordering is done here, if
	 * enabled. Saved L2TP protocol info is stored in skb->sb[].
	 */
	if (PPPOL2TP_SKB_CB(skb)->has_seq) {
		if (session->reorder_timeout != 0) {
			/* Packet reordering enabled. Add skb to session's
			 * reorder queue, in order of ns.
			 */
			pppol2tp_recv_queue_skb(session, skb);
		} else {
			/* Packet reordering disabled. Discard out-of-sequence
			 * packets
			 */
			if (PPPOL2TP_SKB_CB(skb)->ns != session->nr) {
				session->stats.rx_seq_discards++;
				PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG,
				       "%s: oos pkt %hu len %d discarded, "
				       "waiting for %hu, reorder_q_len=%d\n",
				       session->name, PPPOL2TP_SKB_CB(skb)->ns,
				       PPPOL2TP_SKB_CB(skb)->length, session->nr,
				       skb_queue_len(&session->reorder_q));
				goto discard;
			}
			skb_queue_tail(&session->reorder_q, skb);
		}
	} else {
		/* No sequence numbers. Add the skb to the tail of the
		 * reorder queue. This ensures that it will be
		 * delivered after all previous sequenced skbs.
		 */
		skb_queue_tail(&session->reorder_q, skb);
	}

	/* Try to dequeue as many skbs from reorder_q as we can. */
	pppol2tp_recv_dequeue(session);

	return 0;

discard:
	session->stats.rx_errors++;
	kfree_skb(skb);
	sock_put(session->sock);
	sock_put(sock);

	return 0;

discard_bad_csum:
	LIMIT_NETDEBUG("%s: UDP: bad checksum\n", tunnel->name);
	UDP_INC_STATS_USER(&init_net, UDP_MIB_INERRORS, 0);
	tunnel->stats.rx_errors++;
	kfree_skb(skb);

	return 0;

error:
	/* Put UDP header back */
	__skb_push(skb, sizeof(struct udphdr));
	sock_put(sock);

no_tunnel:
	return 1;
}

/* UDP encapsulation receive handler. See net/ipv4/udp.c.
 * Return codes:
 * 0 : success.
 * <0: error
 * >0: skb should be passed up to userspace as UDP.
 */
static int pppol2tp_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
{
	struct pppol2tp_tunnel *tunnel;

	tunnel = pppol2tp_sock_to_tunnel(sk);
	if (tunnel == NULL)
		goto pass_up;

	PRINTK(tunnel->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG,
	       "%s: received %d bytes\n", tunnel->name, skb->len);

	if (pppol2tp_recv_core(sk, skb))
		goto pass_up_put;

	sock_put(sk);
	return 0;

pass_up_put:
	sock_put(sk);
pass_up:
	return 1;
}

/* Receive message. This is the recvmsg for the PPPoL2TP socket.
 */
static int pppol2tp_recvmsg(struct kiocb *iocb, struct socket *sock,
			    struct msghdr *msg, size_t len,
			    int flags)
{
	int err;
	struct sk_buff *skb;
	struct sock *sk = sock->sk;

	err = -EIO;
	if (sk->sk_state & PPPOX_BOUND)
		goto end;

	msg->msg_namelen = 0;

	err = 0;
	skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT,
				flags & MSG_DONTWAIT, &err);
	if (!skb)
		goto end;

	if (len > skb->len)
		len = skb->len;
	else if (len < skb->len)
		msg->msg_flags |= MSG_TRUNC;

	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len);
	if (likely(err == 0))
		err = len;

	kfree_skb(skb);
end:
	return err;
}

/************************************************************************
 * Transmit handling
 ***********************************************************************/

/* Tell how big L2TP headers are for a particular session. This
 * depends on whether sequence numbers are being used.
 */
static inline int pppol2tp_l2tp_header_len(struct pppol2tp_session *session)
{
	if (session->send_seq)
		return PPPOL2TP_L2TP_HDR_SIZE_SEQ;

	return PPPOL2TP_L2TP_HDR_SIZE_NOSEQ;
}

/* Build an L2TP header for the session into the buffer provided.
 */
static void pppol2tp_build_l2tp_header(struct pppol2tp_session *session,
				       void *buf)
{
	__be16 *bufp = buf;
	u16 flags = L2TP_HDR_VER;

	if (session->send_seq)
		flags |= L2TP_HDRFLAG_S;

	/* Setup L2TP header.
	 * FIXME: Can this ever be unaligned? Is direct dereferencing of
	 * 16-bit header fields safe here for all architectures?
	 */
	*bufp++ = htons(flags);
	*bufp++ = htons(session->tunnel_addr.d_tunnel);
	*bufp++ = htons(session->tunnel_addr.d_session);
	if (session->send_seq) {
		*bufp++ = htons(session->ns);
		*bufp++ = 0;
		session->ns++;
		PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG,
		       "%s: updated ns to %hu\n", session->name, session->ns);
	}
}

/* This is the sendmsg for the PPPoL2TP pppol2tp_session socket.  We come here
 * when a user application does a sendmsg() on the session socket. L2TP and
 * PPP headers must be inserted into the user's data.
 */
static int pppol2tp_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *m,
			    size_t total_len)
{
	static const unsigned char ppph[2] = { 0xff, 0x03 };
	struct sock *sk = sock->sk;
	struct inet_sock *inet;
	__wsum csum;
	struct sk_buff *skb;
	int error;
	int hdr_len;
	struct pppol2tp_session *session;
	struct pppol2tp_tunnel *tunnel;
	struct udphdr *uh;
	unsigned int len;
	struct sock *sk_tun;
	u16 udp_len;

	error = -ENOTCONN;
	if (sock_flag(sk, SOCK_DEAD) || !(sk->sk_state & PPPOX_CONNECTED))
		goto error;

	/* Get session and tunnel contexts */
	error = -EBADF;
	session = pppol2tp_sock_to_session(sk);
	if (session == NULL)
		goto error;

	sk_tun = session->tunnel_sock;
	tunnel = pppol2tp_sock_to_tunnel(sk_tun);
	if (tunnel == NULL)
		goto error_put_sess;

	/* What header length is configured for this session? */
	hdr_len = pppol2tp_l2tp_header_len(session);

	/* Allocate a socket buffer */
	error = -ENOMEM;
	skb = sock_wmalloc(sk, NET_SKB_PAD + sizeof(struct iphdr) +
			   sizeof(struct udphdr) + hdr_len +
			   sizeof(ppph) + total_len,
			   0, GFP_KERNEL);
	if (!skb)
		goto error_put_sess_tun;

	/* Reserve space for headers. */
	skb_reserve(skb, NET_SKB_PAD);
	skb_reset_network_header(skb);
	skb_reserve(skb, sizeof(struct iphdr));
	skb_reset_transport_header(skb);

	/* Build UDP header */
	inet = inet_sk(sk_tun);
	udp_len = hdr_len + sizeof(ppph) + total_len;
	uh = (struct udphdr *) skb->data;
	uh->source = inet->sport;
	uh->dest = inet->dport;
	uh->len = htons(udp_len);
	uh->check = 0;
	skb_put(skb, sizeof(struct udphdr));

	/* Build L2TP header */
	pppol2tp_build_l2tp_header(session, skb->data);
	skb_put(skb, hdr_len);

	/* Add PPP header */
	skb->data[0] = ppph[0];
	skb->data[1] = ppph[1];
	skb_put(skb, 2);

	/* Copy user data into skb */
	error = memcpy_fromiovec(skb->data, m->msg_iov, total_len);
	if (error < 0) {
		kfree_skb(skb);
		goto error_put_sess_tun;
	}
	skb_put(skb, total_len);

	/* Calculate UDP checksum if configured to do so */
	if (sk_tun->sk_no_check == UDP_CSUM_NOXMIT)
		skb->ip_summed = CHECKSUM_NONE;
	else if (!(skb->dst->dev->features & NETIF_F_V4_CSUM)) {
		skb->ip_summed = CHECKSUM_COMPLETE;
		csum = skb_checksum(skb, 0, udp_len, 0);
		uh->check = csum_tcpudp_magic(inet->saddr, inet->daddr,
					      udp_len, IPPROTO_UDP, csum);
		if (uh->check == 0)
			uh->check = CSUM_MANGLED_0;
	} else {
		skb->ip_summed = CHECKSUM_PARTIAL;
		skb->csum_start = skb_transport_header(skb) - skb->head;
		skb->csum_offset = offsetof(struct udphdr, check);
		uh->check = ~csum_tcpudp_magic(inet->saddr, inet->daddr,
					       udp_len, IPPROTO_UDP, 0);
	}

	/* Debug */
	if (session->send_seq)
		PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG,
		       "%s: send %Zd bytes, ns=%hu\n", session->name,
		       total_len, session->ns - 1);
	else
		PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG,
		       "%s: send %Zd bytes\n", session->name, total_len);

	if (session->debug & PPPOL2TP_MSG_DATA) {
		int i;
		unsigned char *datap = skb->data;

		printk(KERN_DEBUG "%s: xmit:", session->name);
		for (i = 0; i < total_len; i++) {
			printk(" %02X", *datap++);
			if (i == 15) {
				printk(" ...");
				break;
			}
		}
		printk("\n");
	}

	/* Queue the packet to IP for output */
	len = skb->len;
	error = ip_queue_xmit(skb, 1);

	/* Update stats */
	if (error >= 0) {
		tunnel->stats.tx_packets++;
		tunnel->stats.tx_bytes += len;
		session->stats.tx_packets++;
		session->stats.tx_bytes += len;
	} else {
		tunnel->stats.tx_errors++;
		session->stats.tx_errors++;
	}

	return error;

error_put_sess_tun:
	sock_put(session->tunnel_sock);
error_put_sess:
	sock_put(sk);
error:
	return error;
}

/* Automatically called when the skb is freed.
 */
static void pppol2tp_sock_wfree(struct sk_buff *skb)
{
	sock_put(skb->sk);
}

/* For data skbs that we transmit, we associate with the tunnel socket
 * but don't do accounting.
 */
static inline void pppol2tp_skb_set_owner_w(struct sk_buff *skb, struct sock *sk)
{
	sock_hold(sk);
	skb->sk = sk;
	skb->destructor = pppol2tp_sock_wfree;
}

/* Transmit function called by generic PPP driver.  Sends PPP frame
 * over PPPoL2TP socket.
 *
 * This is almost the same as pppol2tp_sendmsg(), but rather than
 * being called with a msghdr from userspace, it is called with a skb
 * from the kernel.
 *
 * The supplied skb from ppp doesn't have enough headroom for the
 * insertion of L2TP, UDP and IP headers so we need to allocate more
 * headroom in the skb. This will create a cloned skb. But we must be
 * careful in the error case because the caller will expect to free
 * the skb it supplied, not our cloned skb. So we take care to always
 * leave the original skb unfreed if we return an error.
 */
static int pppol2tp_xmit(struct ppp_channel *chan, struct sk_buff *skb)
{
	static const u8 ppph[2] = { 0xff, 0x03 };
	struct sock *sk = (struct sock *) chan->private;
	struct sock *sk_tun;
	int hdr_len;
	u16 udp_len;
	struct pppol2tp_session *session;
	struct pppol2tp_tunnel *tunnel;
	int rc;
	int headroom;
	int data_len = skb->len;
	struct inet_sock *inet;
	__wsum csum;
	struct udphdr *uh;
	unsigned int len;
	int old_headroom;
	int new_headroom;

	if (sock_flag(sk, SOCK_DEAD) || !(sk->sk_state & PPPOX_CONNECTED))
		goto abort;

	/* Get session and tunnel contexts from the socket */
	session = pppol2tp_sock_to_session(sk);
	if (session == NULL)
		goto abort;

	sk_tun = session->tunnel_sock;
	if (sk_tun == NULL)
		goto abort_put_sess;
	tunnel = pppol2tp_sock_to_tunnel(sk_tun);
	if (tunnel == NULL)
		goto abort_put_sess;

	/* What header length is configured for this session? */
	hdr_len = pppol2tp_l2tp_header_len(session);

	/* Check that there's enough headroom in the skb to insert IP,
	 * UDP and L2TP and PPP headers. If not enough, expand it to
	 * make room. Adjust truesize.
	 */
	headroom = NET_SKB_PAD + sizeof(struct iphdr) +
		sizeof(struct udphdr) + hdr_len + sizeof(ppph);
	old_headroom = skb_headroom(skb);
	if (skb_cow_head(skb, headroom))
		goto abort_put_sess_tun;

	new_headroom = skb_headroom(skb);
	skb_orphan(skb);
	skb->truesize += new_headroom - old_headroom;

	/* Setup PPP header */
	__skb_push(skb, sizeof(ppph));
	skb->data[0] = ppph[0];
	skb->data[1] = ppph[1];

	/* Setup L2TP header */
	pppol2tp_build_l2tp_header(session, __skb_push(skb, hdr_len));

	udp_len = sizeof(struct udphdr) + hdr_len + sizeof(ppph) + data_len;

	/* Setup UDP header */
	inet = inet_sk(sk_tun);
	__skb_push(skb, sizeof(*uh));
	skb_reset_transport_header(skb);
	uh = udp_hdr(skb);
	uh->source = inet->sport;
	uh->dest = inet->dport;
	uh->len = htons(udp_len);
	uh->check = 0;

	/* Debug */
	if (session->send_seq)
		PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG,
		       "%s: send %d bytes, ns=%hu\n", session->name,
		       data_len, session->ns - 1);
	else
		PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG,
		       "%s: send %d bytes\n", session->name, data_len);

	if (session->debug & PPPOL2TP_MSG_DATA) {
		int i;
		unsigned char *datap = skb->data;

		printk(KERN_DEBUG "%s: xmit:", session->name);
		for (i = 0; i < data_len; i++) {
			printk(" %02X", *datap++);
			if (i == 31) {
				printk(" ...");
				break;
			}
		}
		printk("\n");
	}

	memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt));
	IPCB(skb)->flags &= ~(IPSKB_XFRM_TUNNEL_SIZE | IPSKB_XFRM_TRANSFORMED |
			      IPSKB_REROUTED);
	nf_reset(skb);

	/* Get routing info from the tunnel socket */
	dst_release(skb->dst);
	skb->dst = dst_clone(__sk_dst_get(sk_tun));
	pppol2tp_skb_set_owner_w(skb, sk_tun);

	/* Calculate UDP checksum if configured to do so */
	if (sk_tun->sk_no_check == UDP_CSUM_NOXMIT)
		skb->ip_summed = CHECKSUM_NONE;
	else if (!(skb->dst->dev->features & NETIF_F_V4_CSUM)) {
		skb->ip_summed = CHECKSUM_COMPLETE;
		csum = skb_checksum(skb, 0, udp_len, 0);
		uh->check = csum_tcpudp_magic(inet->saddr, inet->daddr,
					      udp_len, IPPROTO_UDP, csum);
		if (uh->check == 0)
			uh->check = CSUM_MANGLED_0;
	} else {
		skb->ip_summed = CHECKSUM_PARTIAL;
		skb->csum_start = skb_transport_header(skb) - skb->head;
		skb->csum_offset = offsetof(struct udphdr, check);
		uh->check = ~csum_tcpudp_magic(inet->saddr, inet->daddr,
					       udp_len, IPPROTO_UDP, 0);
	}

	/* Queue the packet to IP for output */
	len = skb->len;
	rc = ip_queue_xmit(skb, 1);

	/* Update stats */
	if (rc >= 0) {
		tunnel->stats.tx_packets++;
		tunnel->stats.tx_bytes += len;
		session->stats.tx_packets++;
		session->stats.tx_bytes += len;
	} else {
		tunnel->stats.tx_errors++;
		session->stats.tx_errors++;
	}

	sock_put(sk_tun);
	sock_put(sk);
	return 1;

abort_put_sess_tun:
	sock_put(sk_tun);
abort_put_sess:
	sock_put(sk);
abort:
	/* Free the original skb */
	kfree_skb(skb);
	return 1;
}

/*****************************************************************************
 * Session (and tunnel control) socket create/destroy.
 *****************************************************************************/

/* When the tunnel UDP socket is closed, all the attached sockets need to go
 * too.
 */
static void pppol2tp_tunnel_closeall(struct pppol2tp_tunnel *tunnel)
{
	int hash;
	struct hlist_node *walk;
	struct hlist_node *tmp;
	struct pppol2tp_session *session;
	struct sock *sk;

	if (tunnel == NULL)
		BUG();

	PRINTK(tunnel->debug, PPPOL2TP_MSG_CONTROL, KERN_INFO,
	       "%s: closing all sessions...\n", tunnel->name);

	write_lock_bh(&tunnel->hlist_lock);
	for (hash = 0; hash < PPPOL2TP_HASH_SIZE; hash++) {
again:
		hlist_for_each_safe(walk, tmp, &tunnel->session_hlist[hash]) {
			struct sk_buff *skb;

			session = hlist_entry(walk, struct pppol2tp_session, hlist);

			sk = session->sock;

			PRINTK(session->debug, PPPOL2TP_MSG_CONTROL, KERN_INFO,
			       "%s: closing session\n", session->name);

			hlist_del_init(&session->hlist);

			/* Since we should hold the sock lock while
			 * doing any unbinding, we need to release the
			 * lock we're holding before taking that lock.
			 * Hold a reference to the sock so it doesn't
			 * disappear as we're jumping between locks.
			 */
			sock_hold(sk);
			write_unlock_bh(&tunnel->hlist_lock);
			lock_sock(sk);

			if (sk->sk_state & (PPPOX_CONNECTED | PPPOX_BOUND)) {
				pppox_unbind_sock(sk);
				sk->sk_state = PPPOX_DEAD;
				sk->sk_state_change(sk);
			}

			/* Purge any queued data */
			skb_queue_purge(&sk->sk_receive_queue);
			skb_queue_purge(&sk->sk_write_queue);
			while ((skb = skb_dequeue(&session->reorder_q))) {
				kfree_skb(skb);
				sock_put(sk);
			}

			release_sock(sk);
			sock_put(sk);

			/* Now restart from the beginning of this hash
			 * chain.  We always remove a session from the
			 * list so we are guaranteed to make forward
			 * progress.
			 */
			write_lock_bh(&tunnel->hlist_lock);
			goto again;
		}
	}
	write_unlock_bh(&tunnel->hlist_lock);
}

/* Really kill the tunnel.
 * Come here only when all sessions have been cleared from the tunnel.
 */
static void pppol2tp_tunnel_free(struct pppol2tp_tunnel *tunnel)
{
	/* Remove from socket list */
	write_lock_bh(&pppol2tp_tunnel_list_lock);
	list_del_init(&tunnel->list);
	write_unlock_bh(&pppol2tp_tunnel_list_lock);

	atomic_dec(&pppol2tp_tunnel_count);
	kfree(tunnel);
}

/* Tunnel UDP socket destruct hook.
 * The tunnel context is deleted only when all session sockets have been
 * closed.
 */
static void pppol2tp_tunnel_destruct(struct sock *sk)
{
	struct pppol2tp_tunnel *tunnel;

	tunnel = sk->sk_user_data;
	if (tunnel == NULL)
		goto end;

	PRINTK(tunnel->debug, PPPOL2TP_MSG_CONTROL, KERN_INFO,
	       "%s: closing...\n", tunnel->name);

	/* Close all sessions */
	pppol2tp_tunnel_closeall(tunnel);

	/* No longer an encapsulation socket. See net/ipv4/udp.c */
	(udp_sk(sk))->encap_type = 0;
	(udp_sk(sk))->encap_rcv = NULL;

	/* Remove hooks into tunnel socket */
	tunnel->sock = NULL;
	sk->sk_destruct = tunnel->old_sk_destruct;
	sk->sk_user_data = NULL;

	/* Call original (UDP) socket descructor */
	if (sk->sk_destruct != NULL)
		(*sk->sk_destruct)(sk);

	pppol2tp_tunnel_dec_refcount(tunnel);

end:
	return;
}

/* Really kill the session socket. (Called from sock_put() if
 * refcnt == 0.)
 */
static void pppol2tp_session_destruct(struct sock *sk)
{
	struct pppol2tp_session *session = NULL;

	if (sk->sk_user_data != NULL) {
		struct pppol2tp_tunnel *tunnel;

		session = sk->sk_user_data;
		if (session == NULL)
			goto out;

		BUG_ON(session->magic != L2TP_SESSION_MAGIC);

		/* Don't use pppol2tp_sock_to_tunnel() here to
		 * get the tunnel context because the tunnel
		 * socket might have already been closed (its
		 * sk->sk_user_data will be NULL) so use the
		 * session's private tunnel ptr instead.
		 */
		tunnel = session->tunnel;
		if (tunnel != NULL) {
			BUG_ON(tunnel->magic != L2TP_TUNNEL_MAGIC);

			/* If session_id is zero, this is a null
			 * session context, which was created for a
			 * socket that is being used only to manage
			 * tunnels.
			 */
			if (session->tunnel_addr.s_session != 0) {
				/* Delete the session socket from the
				 * hash
				 */
				write_lock_bh(&tunnel->hlist_lock);
				hlist_del_init(&session->hlist);
				write_unlock_bh(&tunnel->hlist_lock);

				atomic_dec(&pppol2tp_session_count);
			}

			/* This will delete the tunnel context if this
			 * is the last session on the tunnel.
			 */
			session->tunnel = NULL;
			session->tunnel_sock = NULL;
			pppol2tp_tunnel_dec_refcount(tunnel);
		}
	}

	kfree(session);
out:
	return;
}

/* Called when the PPPoX socket (session) is closed.
 */
static int pppol2tp_release(struct socket *sock)
{
	struct sock *sk = sock->sk;
	struct pppol2tp_session *session;
	int error;

	if (!sk)
		return 0;

	error = -EBADF;
	lock_sock(sk);
	if (sock_flag(sk, SOCK_DEAD) != 0)
		goto error;

	pppox_unbind_sock(sk);

	/* Signal the death of the socket. */
	sk->sk_state = PPPOX_DEAD;
	sock_orphan(sk);
	sock->sk = NULL;

	session = pppol2tp_sock_to_session(sk);

	/* Purge any queued data */
	skb_queue_purge(&sk->sk_receive_queue);
	skb_queue_purge(&sk->sk_write_queue);
	if (session != NULL) {
		struct sk_buff *skb;
		while ((skb = skb_dequeue(&session->reorder_q))) {
			kfree_skb(skb);
			sock_put(sk);
		}
		sock_put(sk);
	}

	release_sock(sk);

	/* This will delete the session context via
	 * pppol2tp_session_destruct() if the socket's refcnt drops to
	 * zero.
	 */
	sock_put(sk);

	return 0;

error:
	release_sock(sk);
	return error;
}

/* Internal function to prepare a tunnel (UDP) socket to have PPPoX
 * sockets attached to it.
 */
static struct sock *pppol2tp_prepare_tunnel_socket(int fd, u16 tunnel_id,
						   int *error)
{
	int err;
	struct socket *sock = NULL;
	struct sock *sk;
	struct pppol2tp_tunnel *tunnel;
	struct sock *ret = NULL;

	/* Get the tunnel UDP socket from the fd, which was opened by
	 * the userspace L2TP daemon.
	 */
	err = -EBADF;
	sock = sockfd_lookup(fd, &err);
	if (!sock) {
		PRINTK(-1, PPPOL2TP_MSG_CONTROL, KERN_ERR,
		       "tunl %hu: sockfd_lookup(fd=%d) returned %d\n",
		       tunnel_id, fd, err);
		goto err;
	}

	sk = sock->sk;

	/* Quick sanity checks */
	err = -EPROTONOSUPPORT;
	if (sk->sk_protocol != IPPROTO_UDP) {
		PRINTK(-1, PPPOL2TP_MSG_CONTROL, KERN_ERR,
		       "tunl %hu: fd %d wrong protocol, got %d, expected %d\n",
		       tunnel_id, fd, sk->sk_protocol, IPPROTO_UDP);
		goto err;
	}
	err = -EAFNOSUPPORT;
	if (sock->ops->family != AF_INET) {
		PRINTK(-1, PPPOL2TP_MSG_CONTROL, KERN_ERR,
		       "tunl %hu: fd %d wrong family, got %d, expected %d\n",
		       tunnel_id, fd, sock->ops->family, AF_INET);
		goto err;
	}

	err = -ENOTCONN;

	/* Check if this socket has already been prepped */
	tunnel = (struct pppol2tp_tunnel *)sk->sk_user_data;
	if (tunnel != NULL) {
		/* User-data field already set */
		err = -EBUSY;
		BUG_ON(tunnel->magic != L2TP_TUNNEL_MAGIC);

		/* This socket has already been prepped */
		ret = tunnel->sock;
		goto out;
	}

	/* This socket is available and needs prepping. Create a new tunnel
	 * context and init it.
	 */
	sk->sk_user_data = tunnel = kzalloc(sizeof(struct pppol2tp_tunnel), GFP_KERNEL);
	if (sk->sk_user_data == NULL) {
		err = -ENOMEM;
		goto err;
	}

	tunnel->magic = L2TP_TUNNEL_MAGIC;
	sprintf(&tunnel->name[0], "tunl %hu", tunnel_id);

	tunnel->stats.tunnel_id = tunnel_id;
	tunnel->debug = PPPOL2TP_DEFAULT_DEBUG_FLAGS;

	/* Hook on the tunnel socket destructor so that we can cleanup
	 * if the tunnel socket goes away.
	 */
	tunnel->old_sk_destruct = sk->sk_destruct;
	sk->sk_destruct = &pppol2tp_tunnel_destruct;

	tunnel->sock = sk;
	sk->sk_allocation = GFP_ATOMIC;

	/* Misc init */
	rwlock_init(&tunnel->hlist_lock);

	/* Add tunnel to our list */
	INIT_LIST_HEAD(&tunnel->list);
	write_lock_bh(&pppol2tp_tunnel_list_lock);
	list_add(&tunnel->list, &pppol2tp_tunnel_list);
	write_unlock_bh(&pppol2tp_tunnel_list_lock);
	atomic_inc(&pppol2tp_tunnel_count);

	/* Bump the reference count. The tunnel context is deleted
	 * only when this drops to zero.
	 */
	pppol2tp_tunnel_inc_refcount(tunnel);

	/* Mark socket as an encapsulation socket. See net/ipv4/udp.c */
	(udp_sk(sk))->encap_type = UDP_ENCAP_L2TPINUDP;
	(udp_sk(sk))->encap_rcv = pppol2tp_udp_encap_recv;

	ret = tunnel->sock;

	*error = 0;
out:
	if (sock)
		sockfd_put(sock);

	return ret;

err:
	*error = err;
	goto out;
}

static struct proto pppol2tp_sk_proto = {
	.name	  = "PPPOL2TP",
	.owner	  = THIS_MODULE,
	.obj_size = sizeof(struct pppox_sock),
};

/* socket() handler. Initialize a new struct sock.
 */
static int pppol2tp_create(struct net *net, struct socket *sock)
{
	int error = -ENOMEM;
	struct sock *sk;

	sk = sk_alloc(net, PF_PPPOX, GFP_KERNEL, &pppol2tp_sk_proto);
	if (!sk)
		goto out;

	sock_init_data(sock, sk);

	sock->state  = SS_UNCONNECTED;
	sock->ops    = &pppol2tp_ops;

	sk->sk_backlog_rcv = pppol2tp_recv_core;
	sk->sk_protocol	   = PX_PROTO_OL2TP;
	sk->sk_family	   = PF_PPPOX;
	sk->sk_state	   = PPPOX_NONE;
	sk->sk_type	   = SOCK_STREAM;
	sk->sk_destruct	   = pppol2tp_session_destruct;

	error = 0;

out:
	return error;
}

/* connect() handler. Attach a PPPoX socket to a tunnel UDP socket
 */
static int pppol2tp_connect(struct socket *sock, struct sockaddr *uservaddr,
			    int sockaddr_len, int flags)
{
	struct sock *sk = sock->sk;
	struct sockaddr_pppol2tp *sp = (struct sockaddr_pppol2tp *) uservaddr;
	struct pppox_sock *po = pppox_sk(sk);
	struct sock *tunnel_sock = NULL;
	struct pppol2tp_session *session = NULL;
	struct pppol2tp_tunnel *tunnel;
	struct dst_entry *dst;
	int error = 0;

	lock_sock(sk);

	error = -EINVAL;
	if (sp->sa_protocol != PX_PROTO_OL2TP)
		goto end;

	/* Check for already bound sockets */
	error = -EBUSY;