First version of Multiple Tunneling Protocol Daemon (mtpd) for Android.
Currently it only supports L2TP without secrets.
diff --git a/l2tp.c b/l2tp.c
new file mode 100644
index 0000000..367ca28
--- /dev/null
+++ b/l2tp.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright (C) 2009 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/* A simple implementation of L2TP Access Concentrator (RFC 2661) which only
+ * creates a single session. The following code only handles control packets.
+ * Data packets are handled by PPPoLAC driver which can be found in Android
+ * kernel tree. */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <linux/if_pppolac.h>
+
+#include "mtpd.h"
+
+/* TODO: Support secrets. */
+
+/* To avoid unnecessary endianness conversions, tunnels, sessions, attributes,
+ * and values are all accessed in network order. */
+
+/* 0 is reserved. We put ACK here just for convenience. */
+enum l2tp_message {
+ ACK = 0,
+ SCCRQ = 1,
+ SCCRP = 2,
+ SCCCN = 3,
+ STOPCCN = 4,
+ HELLO = 6,
+ OCRQ = 7,
+ OCRP = 8,
+ OCCN = 9,
+ ICRQ = 10,
+ ICRP = 11,
+ ICCN = 12,
+ CDN = 14,
+ WEN = 15,
+ SLI = 16,
+ MESSAGE_MAX = 16,
+};
+
+static char *messages[] = {
+ "ACK", "SCCRQ", "SCCRP", "SCCCN", "STOPCCN", NULL, "HELLO", "OCRQ",
+ "OCRP", "OCCN", "ICRQ", "ICRP", "ICCN", NULL, "CDN", "WEN", "SLI",
+};
+
+/* This is incomplete. Only those we used are listed here. */
+#define RESULT_CODE htons(1)
+#define PROTOCOL_VERSION htons(2)
+#define FRAMING_CAPABILITIES htons(3)
+#define HOST_NAME htons(7)
+#define ASSIGNED_TUNNEL htons(9)
+#define WINDOW_SIZE htons(10)
+#define ASSIGNED_SESSION htons(14)
+#define CALL_SERIAL_NUMBER htons(15)
+#define FRAMING_TYPE htons(19)
+#define CONNECT_SPEED htons(24)
+
+#define MESSAGE_FLAG 0xC802
+#define MESSAGE_MASK 0xCB0F
+#define ATTRIBUTE_FLAG(length) (0x8006 + (length))
+#define ATTRIBUTE_LENGTH(flag) (0x03FF & (flag))
+#define ATTRIBUTE_HIDDEN(flag) (0x4000 & (flag))
+
+#define ACK_SIZE 12
+#define MESSAGE_HEADER_SIZE 20
+#define ATTRIBUTE_HEADER_SIZE 6
+
+static uint16_t local_tunnel;
+static uint16_t local_session;
+static uint16_t local_sequence;
+static uint16_t remote_tunnel;
+static uint16_t remote_session;
+static uint16_t remote_sequence;
+
+static uint16_t state;
+static int acknowledged;
+
+/* According to RFC 2661 page 46, an exponential backoff strategy is required
+ * for retransmission. However, it might waste too much time waiting for IPsec
+ * negotiation. Here we use the same interval to keep things simple. */
+#define TIMEOUT_INTERVAL 2000
+
+#define MAX_PACKET_LENGTH 2048
+
+static struct packet {
+ int message;
+ int length;
+ uint8_t buffer[MAX_PACKET_LENGTH] __attribute__((aligned));
+} incoming, outgoing;
+
+struct attribute {
+ uint16_t flag;
+ uint16_t vendor;
+ uint16_t type;
+ uint8_t value[1];
+} __attribute__((packed));
+
+static void set_message(uint16_t session, uint16_t message)
+{
+ uint16_t *p = (uint16_t *)outgoing.buffer;
+ p[0] = htons(MESSAGE_FLAG);
+ /* p[1] will be filled in send_packet(). */
+ p[2] = remote_tunnel;
+ p[3] = session;
+ p[4] = htons(local_sequence);
+ p[5] = htons(remote_sequence);
+ p[6] = htons(ATTRIBUTE_FLAG(2));
+ p[7] = 0;
+ p[8] = 0;
+ p[9] = htons(message);
+ outgoing.message = message;
+ outgoing.length = MESSAGE_HEADER_SIZE;
+ ++local_sequence;
+}
+
+static void add_attribute_raw(uint16_t type, void *value, int size)
+{
+ struct attribute *p = (struct attribute *)&outgoing.buffer[outgoing.length];
+ p->flag = htons(ATTRIBUTE_FLAG(size));
+ p->vendor = 0;
+ p->type = type;
+ memcpy(&p->value, value, size);
+ outgoing.length += ATTRIBUTE_HEADER_SIZE + size;
+}
+
+static void add_attribute_u16(uint16_t attribute, uint16_t value)
+{
+ add_attribute_raw(attribute, &value, sizeof(uint16_t));
+}
+
+static void add_attribute_u32(uint16_t attribute, uint32_t value)
+{
+ add_attribute_raw(attribute, &value, sizeof(uint32_t));
+}
+
+static void send_packet()
+{
+ uint16_t *p = (uint16_t *)outgoing.buffer;
+ p[1] = htons(outgoing.length);
+ send(the_socket, outgoing.buffer, outgoing.length, 0);
+ acknowledged = 0;
+}
+
+static void send_ack()
+{
+ uint16_t buffer[6] = {
+ htons(MESSAGE_FLAG), htons(ACK_SIZE), remote_tunnel, 0,
+ htons(local_sequence), htons(remote_sequence),
+ };
+ send(the_socket, buffer, ACK_SIZE, 0);
+}
+
+static int recv_packet(uint16_t *session)
+{
+ uint16_t *p = (uint16_t *)incoming.buffer;
+
+ incoming.length = recv(the_socket, incoming.buffer, MAX_PACKET_LENGTH, 0);
+ if (incoming.length == -1 && errno != EINTR) {
+ log_print(FATAL, "Recv() %s", strerror(errno));
+ exit(NETWORK_ERROR);
+ }
+
+ /* We only handle packets in our tunnel. */
+ if ((incoming.length != ACK_SIZE && incoming.length < MESSAGE_HEADER_SIZE)
+ || (p[0] & htons(MESSAGE_MASK)) != htons(MESSAGE_FLAG)
+ || p[1] > htons(incoming.length) || p[2] != local_tunnel) {
+ return 0;
+ }
+
+ if (incoming.length == ACK_SIZE) {
+ incoming.message = ACK;
+ } else if (p[6] == htons(ATTRIBUTE_FLAG(2)) && !p[7] && !p[8]) {
+ incoming.message = ntohs(p[9]);
+ } else {
+ return 0;
+ }
+
+ /* Check if the packet is duplicated and send ACK if necessary. */
+ if ((uint16_t)(ntohs(p[4]) - remote_sequence) > 32767) {
+ if (incoming.message != ACK) {
+ send_ack();
+ }
+ return 0;
+ }
+
+ if (ntohs(p[5]) == local_sequence) {
+ acknowledged = 1;
+ }
+
+ /* Our sending and receiving window sizes are both 1. Thus we only handle
+ * this packet if it is their next one and they received our last one. */
+ if (ntohs(p[4]) != remote_sequence || !acknowledged) {
+ return 0;
+ }
+ *session = p[3];
+ if (incoming.message != ACK) {
+ ++remote_sequence;
+ }
+ return 1;
+}
+
+static int get_attribute_raw(uint16_t type, void *value, int size)
+{
+ int offset = MESSAGE_HEADER_SIZE;
+ while (incoming.length >= offset + ATTRIBUTE_HEADER_SIZE) {
+ struct attribute *p = (struct attribute *)&incoming.buffer[offset];
+ uint16_t flag = ntohs(p->flag);
+ int length = ATTRIBUTE_LENGTH(flag);
+
+ offset += length;
+ length -= ATTRIBUTE_HEADER_SIZE;
+ if (length < 0 || offset > incoming.length) {
+ break;
+ }
+
+ /* Currently we do not support hidden attributes. */
+ if (!ATTRIBUTE_HIDDEN(flag) && !p->vendor && p->type == type) {
+ if (size > length) {
+ size = length;
+ }
+ memcpy(value, p->value, size);
+ return size;
+ }
+ }
+ return 0;
+}
+
+static int get_attribute_u16(uint16_t type, uint16_t *value)
+{
+ return get_attribute_raw(type, value, sizeof(uint16_t)) == sizeof(uint16_t);
+}
+
+static int l2tp_connect(int argc, char **argv)
+{
+ if (argc < 2) {
+ return -USAGE_ERROR;
+ }
+ create_socket(AF_INET, SOCK_DGRAM, argv[0], argv[1]);
+
+ while (!local_tunnel) {
+ local_tunnel = random();
+ }
+
+ log_print(DEBUG, "Sending SCCRQ (local_tunnel = %d)", local_tunnel);
+ state = SCCRQ;
+ set_message(0, SCCRQ);
+ add_attribute_u16(PROTOCOL_VERSION, htons(0x0100));
+ add_attribute_raw(HOST_NAME, "anonymous", 9);
+ add_attribute_u32(FRAMING_CAPABILITIES, htonl(3));
+ add_attribute_u16(ASSIGNED_TUNNEL, local_tunnel);
+ add_attribute_u16(WINDOW_SIZE, htons(1));
+ send_packet();
+ return TIMEOUT_INTERVAL;
+}
+
+static int create_pppox()
+{
+ int pppox;
+ log_print(INFO, "Creating PPPoX socket");
+ pppox = socket(AF_PPPOX, SOCK_DGRAM, PX_PROTO_OLAC);
+
+ if (pppox == -1) {
+ log_print(FATAL, "Socket() %s", strerror(errno));
+ exit(SYSTEM_ERROR);
+ } else {
+ struct sockaddr_pppolac address = {
+ .sa_family = AF_PPPOX,
+ .sa_protocol = PX_PROTO_OLAC,
+ .udp_socket = the_socket,
+ .local = {.tunnel = local_tunnel, .session = local_session},
+ .remote = {.tunnel = remote_tunnel, .session = remote_session},
+ };
+ if (connect(pppox, (struct sockaddr *)&address, sizeof(address)) != 0) {
+ log_print(FATAL, "Connect() %s", strerror(errno));
+ exit(SYSTEM_ERROR);
+ }
+ }
+ return pppox;
+}
+
+static int l2tp_process()
+{
+ uint16_t sequence = local_sequence;
+ uint16_t tunnel;
+ uint16_t session;
+
+ if (!recv_packet(&session)) {
+ return acknowledged ? 0 : TIMEOUT_INTERVAL;
+ }
+
+ /* Here is the fun part. We always try to protect our tunnel and session
+ * from being closed even if we received unexpected messages. */
+ switch(incoming.message) {
+ case SCCRP:
+ if (state == SCCRQ) {
+ if (get_attribute_u16(ASSIGNED_TUNNEL, &tunnel) && tunnel) {
+ remote_tunnel = tunnel;
+ log_print(DEBUG, "Received SCCRP (remote_tunnel = %d) -> "
+ "Sending SCCCN", remote_tunnel);
+ state = SCCCN;
+ set_message(0, SCCCN);
+ break;
+ }
+ log_print(DEBUG, "Received SCCRP without assigned tunnel");
+ log_print(ERROR, "Protocol error");
+ return -PROTOCOL_ERROR;
+ }
+ break;
+
+ case ICRP:
+ if (state == ICRQ && session == local_session) {
+ if (get_attribute_u16(ASSIGNED_SESSION, &session) && session) {
+ remote_session = session;
+ log_print(DEBUG, "Received ICRP (remote_session = %d) -> "
+ "Sending ICCN", remote_session);
+ state = ICCN;
+ set_message(remote_session, ICCN);
+ add_attribute_u32(CONNECT_SPEED, htonl(100000000));
+ add_attribute_u32(FRAMING_TYPE, htonl(3));
+ break;
+ }
+ log_print(DEBUG, "Received ICRP without assigned session");
+ log_print(ERROR, "Protocol error");
+ return -PROTOCOL_ERROR;
+ }
+ break;
+
+ case STOPCCN:
+ log_print(DEBUG, "Received STOPCCN");
+ log_print(INFO, "Remote server hung up");
+ state = STOPCCN;
+ return -REMOTE_REQUESTED;
+
+ case CDN:
+ if (session && session == local_session) {
+ log_print(DEBUG, "Received CDN (local_session = %d)",
+ local_session);
+ log_print(INFO, "Remote server hung up");
+ return -REMOTE_REQUESTED;
+ }
+ break;
+
+ case ACK:
+ case HELLO:
+ case WEN:
+ case SLI:
+ /* These are harmless, so we just treat them the same way. */
+ if (state == SCCCN) {
+ while (!local_session) {
+ local_session = random();
+ }
+ log_print(DEBUG, "Received %s -> Sending ICRQ (local_session = "
+ "%d)", messages[incoming.message], local_session);
+ log_print(INFO, "Tunnel established");
+ state = ICRQ;
+ set_message(0, ICRQ);
+ add_attribute_u16(ASSIGNED_SESSION, local_session);
+ add_attribute_u32(CALL_SERIAL_NUMBER, random());
+ break;
+ }
+
+ if (incoming.message == ACK) {
+ log_print(DEBUG, "Received ACK");
+ } else {
+ log_print(DEBUG, "Received %s -> Sending ACK",
+ messages[incoming.message]);
+ send_ack();
+ }
+
+ if (state == ICCN) {
+ log_print(INFO, "Session established");
+ state = ACK;
+ start_pppd(create_pppox());
+ }
+ return 0;
+
+ case ICRQ:
+ case OCRQ:
+ /* Since we run pppd as a client, it does not makes sense to
+ * accept ICRQ or OCRQ. Always send CDN with a proper error. */
+ if (get_attribute_u16(ASSIGNED_SESSION, &session) && session) {
+ log_print(DEBUG, "Received %s (remote_session = %d) -> "
+ "Sending CDN", messages[incoming.message], session);
+ set_message(session, CDN);
+ add_attribute_u32(RESULT_CODE, htonl(0x00020006));
+ add_attribute_u16(ASSIGNED_SESSION, 0);
+ }
+ break;
+ }
+
+ if (sequence != local_sequence) {
+ send_packet();
+ return TIMEOUT_INTERVAL;
+ }
+
+ /* We reach here if we got an unexpected message. Log it and send ACK. */
+ if (incoming.message > MESSAGE_MAX || !messages[incoming.message]) {
+ log_print(DEBUG, "Received UNKNOWN %d -> Sending ACK anyway",
+ incoming.message);
+ } else {
+ log_print(DEBUG, "Received UNEXPECTED %s -> Sending ACK anyway",
+ messages[incoming.message]);
+ }
+ send_ack();
+ return 0;
+}
+
+static int l2tp_timeout()
+{
+ if (acknowledged) {
+ return 0;
+ }
+ log_print(DEBUG, "Timeout -> Sending %s", messages[outgoing.message]);
+ send(the_socket, outgoing.buffer, outgoing.length, 0);
+ return TIMEOUT_INTERVAL;
+}
+
+static void l2tp_shutdown()
+{
+ if (state != STOPCCN) {
+ log_print(DEBUG, "Sending STOPCCN");
+ set_message(0, STOPCCN);
+ add_attribute_u16(ASSIGNED_TUNNEL, local_tunnel);
+ add_attribute_u16(RESULT_CODE, htons(6));
+ send_packet();
+ }
+}
+
+struct protocol l2tp = {
+ .name = "l2tp",
+ .usage = "<server> <port>",
+ .connect = l2tp_connect,
+ .process = l2tp_process,
+ .timeout = l2tp_timeout,
+ .shutdown = l2tp_shutdown,
+};