marvell/services/tr069/stun.c

#include <pthread.h>

#include "tr069.h"
#include "tr069_uci.h"
#include "tr069_service.h"



#define closesocket(a) close(a)

int  tr069_server_stun_fd;
int  tr069_client_stun_fd;
int  tr069_accept_stun_fd;

void *tcp_direct_connect(void *data);
void *stun_task(void * data);
void enable_stun(void);




struct stun_hdr {
	unsigned short int type;
	unsigned short int len;
	unsigned int id[4];
	unsigned char payload[0];
}__attribute__((packed));

struct stun_attr_hdr {
	unsigned short int type;
	unsigned short int len;
	unsigned char payload[0];
}__attribute__((packed));

struct stun_attr_mapped_addr {
	unsigned char unused;
	unsigned char family;
	unsigned short int port;
	unsigned int address;
}__attribute__((packed));




static unsigned char buf[BUF_SIZE];
static unsigned char * p_stun_msg;
static unsigned short int msglen = 0;

#define RESET_MSG(ADDR) do { \
	msglen = 0; \
	p_stun_msg = ADDR; \
} while (0)

#define GET_MSG_LENGTH() msglen

#define SET_U16_VALUE(VALUE) do { \
	*(p_stun_msg + msglen + 1) = ((VALUE) & 0xff); \
	*(p_stun_msg + msglen + 0) = (((VALUE) >> 8) & 0xff); \
	msglen += 2; \
} while (0)

#define SET_U32_VALUE(VALUE) do { \
	SET_U16_VALUE((unsigned short int)(((VALUE) >> 16) && 0xffff)); \
	SET_U16_VALUE((unsigned short int)((VALUE) && 0xffff)); \
} while (0)

#define SET_STRING_VALUE(STRING) do { \
	msglen += sprintf((char *)(p_stun_msg + msglen), "%s", STRING); \
} while (0)


#define CONNECTION_REQUEST_BINDING_STRING "dslforum.org/TR-111 "
#define DEFAULT_STUN_SERVER "testacs.friendly-tech.com"
//#define DEFAULT_STUN_SERVER "stun.stunprotocol.org"

#define MAPPED_ADDRESS		0x0001
#define RESPONSE_ADDRESS	0x0002
#define CHANGE_REQUEST		0x0003
#define SOURCE_ADDRESS		0x0004
#define CHANGED_ADDRESS		0x0005
#define USERNAME			0x0006
#define PASSWORD			0x0007
#define MESSAGE_INTEGRITY	0x0008
#define ERROR_CODE			0x0009
#define UNKNOWN_ATTRIBUTES	0x000a
#define REFLECTED_FROM		0x000b
#define CONNECTION_REQUEST_BINDING 0xc001
#define BINDING_CHANGE		0xc002

#define BINDING_REQUEST		0x0001
#define BINDING_RESPONSE	0x0101
#define	BINDING_ERR_RESP	0x0111
#define SHARED_SECRET_REQ	0x0002
#define SHARED_SECRET_RESP	0x0102
#define SHARED_SECRET_ERR_RESP	0x0112


static int sent_stun_result(struct tr069_task_command * cmd, int result)
{
	struct tr069_task_command * msg = NULL;
	if (result < 0 && cmd->payload[2] < 5) {
		TR069_DEBUG("%s: Sent stun state to ACS failed, try resent", __func__);
		msg = tr069_malloc(sizeof(struct tr069_task_command) + sizeof(int) * 3);
		if (msg) {
			unsigned int * p = msg->payload;
			memset(msg, 0, sizeof(struct tr069_task_command));
			msg->command_id = TR069_COMMAND_STUN_CHANGE;
			msg->msg = (unsigned int)p;
			p[0] = cmd->payload[0];
			p[1] = cmd->payload[1];
			p[2] = cmd->payload[2] + 1;
			send_tr069_command(msg);
		}
	}
	return 0;
}

static int handle_binding_response(const unsigned char * response, int total)
{
	struct stun_attr_hdr * attr;
	unsigned short int type, length;
	TR069_DEBUG("Total remain %d", total);
	while (total > 0) {
		attr = (struct stun_attr_hdr *)response;
		type = htons(attr->type);
		length = htons(attr->len);
		total -= 4;
		response += 4;
		switch (type) {
			case MAPPED_ADDRESS: {
				struct stun_attr_mapped_addr * mapped = (struct stun_attr_mapped_addr *)&attr->payload[0];
				static struct in_addr addr_backup = {0};
				struct in_addr addr_in;
				addr_in.s_addr = mapped->address;
				TR069_DEBUG("Handle mapped address: port %d, address %s", mapped->port, inet_ntoa(addr_in));
				if (memcmp(&addr_backup, &addr_in, sizeof(struct in_addr))) {
					struct tr069_task_command * msg;
					TR069_DEBUG("Handle new puplic addreess");
					addr_backup = addr_in;
					// Send command to tr069 thread
					msg = tr069_malloc(sizeof(struct tr069_task_command) + sizeof(int) * 3);
					if (msg) {
						unsigned int * p = msg->payload;
						memset(msg, 0, sizeof(struct tr069_task_command) + sizeof(int) * 3);
						msg->command_id = TR069_COMMAND_STUN_CHANGE;
						msg->tsk_cmd_cb = sent_stun_result;
						msg->msg = (unsigned int)p;
						p[0] = addr_backup.s_addr;
						p[1] = htons(mapped->port);
						p[2] = 0; // Trace re-send cnt
						send_tr069_command(msg);
					} else {
						TR069_ERR("Cannot malloc memory!(%s:%d)", __func__, __LINE__);
					}

					char * ip_buf = tr069_malloc(16);
					memset(ip_buf,0,16);
					tr069_get_host_ip("ccinet0",ip_buf);
					inet_aton(ip_buf,&addr_backup);
					tr069_free(ip_buf);

				}
				break;
			}
			default:
				TR069_DEBUG("Handle unkonwn attribute 0x%04x", type);
				break;
		}
		response += length;
		total -= length;
		TR069_DEBUG("After process remain %d", total);
	}
	return 0;
}

#define PRINT_BUF_SIZE 65
static char print_buf[PRINT_BUF_SIZE];
static void print_string(const char * string)
{
	int len = strlen(string);
	int printed = 0;

	while (printed != len) {
		if ((len - printed) > (PRINT_BUF_SIZE - 1)) {
			memcpy(print_buf, string + printed, (PRINT_BUF_SIZE - 1));
			printed += (PRINT_BUF_SIZE - 1);
			print_buf[PRINT_BUF_SIZE - 1] = '\0';
		} else {
			sprintf(print_buf, "%s", string + printed);
			printed = len;
		}
		TR069_DEBUG("[stun-value]%s", print_buf);
	}
}

static char * get_line(char ** buf)
{
	char * string = NULL;
	char * start;
	
    if (buf == NULL) return NULL;
    
	string = *buf;
	
	if (string == NULL) return NULL;
	while ((*string == '\r') || (*string == '\n')) {
		// Skipped special char from the beginning
		string++;
	}
	if (*string == '\0') {
		// String reach end
		*buf = NULL;
		return NULL;
	}
	start = string;
	while ((*string != '\r') && (*string != '\n') && (*string != '\0')) {
		string++;
	}
	if (*string != '\0') {
		*string++ = '\0'; // Mark one line end
		*buf = string;
	} else {
		*buf = string;
	}
	return start;
}

static int handle_udp_request(char * request, int total, int udptcp)
{
	int ret = 200;
	char * string =  request;
	char * line = NULL;
	//char * token = NULL;
	const char * method = NULL;

	print_string(string);
	if (udptcp == 0) {
		if (!strncmp(string, "GET", 3)) {
			goto send_cmd;
		}
	}

	if ((string[total - 1] != '\n') || (string[total - 2] != '\r') ||
		(string[total - 3] != '\n' || (string[total - 4] != '\r'))) {
    		TR069_ERR("HTTP request not complete");
		ret = 401;
		goto done;
	}
	if (!strncmp(string, "GET", 3)) {
		method = "GET";
	} else if (!strncmp(string, "POST", 4)) {
		method = "POST";
	} else {
    		TR069_ERR("Unknow method");
		ret = 401;
		goto done;
	}

	if (cpe.conn_req_username[0] == '\0') {
    		TR069_ERR("Not have user name");
		ret = 200;
		goto send_cmd;
	}

	while (NULL != (line = get_line(&string))) {
		#define AUTHOR_HEAD "Authorization: Digest "
		TR069_DEBUG("Get line: %s", line);
		if (!strncmp(line, AUTHOR_HEAD, strlen(AUTHOR_HEAD))) {
			struct digestdata *digest = NULL;
			line += strlen(AUTHOR_HEAD);
			digest_input(line, &digest);
			if (digest) {
				ret = digest_authenticate(cpe.conn_req_username, cpe.conn_req_password, method, digest);
				if (ret == 0) {
					ret = 200;
					goto send_cmd;
				}
				clear_digestdata(digest);
				digest = NULL;
			}
			break;
		}
	}

	// Here should be no digest message or digest failed
	ret = 401;
	goto done;
	
	// Send message to tr069 main task, establish ACS connection
send_cmd:
	{
		struct tr069_task_command * msg =
			tr069_malloc(sizeof(struct tr069_task_command));
		if (msg) {
			memset(msg, 0, sizeof(struct tr069_task_command));
			msg->command_id = TR069_COMMAND_REQUEST_CONNECT;
			send_tr069_command(msg);
		}
	}
done:
	return ret;
}

void *tcp_direct_connect(void *data)
{
	UNUSED(data);

	tr069_client_stun_fd = usock(USOCK_UNIX, TR069_INNER_STUN_CONN_SOCKET, NULL);
	if (tr069_client_stun_fd < 0) {
		TR069_ERR("%s,Failed to connect to server\n",__FUNCTION__);
		return NULL;
	}
	TR069_INFO("%s,client fd %d", __FUNCTION__,tr069_client_stun_fd);
	
	

	struct sockaddr_in ip = {0};
	int fd = -1;
	int ret;
	int optval;
	char * buf = NULL;
	for (;;) {
		struct sockaddr_in in = {0};
		int client;
		int size = sizeof(in);
		//OSATaskSleep(20 * 1000 / 5);
		if (fd < 0) {
			TR069_INFO("[tcp]Try build tcp socket..");
			fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
			if (fd < 0) {
				TR069_ERR("[tcp]Build tcp socket faield..");
				continue;
			}
			
			
			bzero(&ip, sizeof(ip));
			ip.sin_family = AF_INET;
			ip.sin_port = htons(8080);
			ip.sin_addr.s_addr=htonl(INADDR_ANY);

			
			ret = bind(fd, (const struct sockaddr *)&ip, sizeof(struct sockaddr));
			if (ret < 0) {
				TR069_ERR("[tcp]Bind to port failed..");
				closesocket(fd);
				fd = -1;
				continue;
			}
			TR069_INFO("[tcp]bind success..");
			ret = listen(fd, 2);
			if (ret < 0) {
				TR069_ERR("[tcp]Listen port failed..");
				closesocket(fd);
				fd = -1;
				continue;
			}
			TR069_INFO("[tcp]listen success..");
		}
		client = accept(fd, (struct sockaddr *)&in, (socklen_t *)&size);
		if (client < 0) {
			TR069_ERR("[tcp]Accept bad socket handle..");
			continue;
		}
		TR069_DEBUG("[tcp]Accept success..");
		optval = 1;
		ret = setsockopt(client, IPPROTO_TCP, TCP_NODELAY, &optval, sizeof(int));
		optval = 8 * 1000;
		
		struct timeval timeout={8,0};
		ret = setsockopt(client, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(struct timeval));
		if (ret < 0) {
			TR069_ERR("build_new_socket set tcp recv timeout failed(%d)", ret);
		}
		#undef BUFFER_SIZE
		#define BUFFER_SIZE 2048
		buf = tr069_malloc(BUFFER_SIZE+1);
		if (buf == NULL) {
			TR069_ERR("[tcp]malloc failed");
			goto done;
		}
		memset(buf,0,BUFFER_SIZE+1);
		ret = recv(client, buf, BUFFER_SIZE, 0);
		if (ret >= 0) {
			print_string(buf);
			ret = handle_udp_request(buf, ret, 1);
			memset(buf, 0, BUFFER_SIZE+1);
			if (401 == ret) {
				#undef HTTP_RESPONSE
				#define HTTP_RESPONSE "HTTP/1.1 401 Unauthorized\r\nContent-Length: 0\r\n"\
															"WWW-Authenticate: Digest realm=\"%s\", nonce=\"%08x\", qop=\"auth\", algorithm=MD5\r\n\r\n"
				ret = snprintf(buf, BUFFER_SIZE, HTTP_RESPONSE, "tr069", (unsigned int)buf);
			} else {
				#undef HTTP_RESPONSE
				#define HTTP_RESPONSE "HTTP/1.1 200 OK\r\nContent-Length: 0\r\n\r\n"
				ret = snprintf(buf, BUFFER_SIZE, "%s", HTTP_RESPONSE);
			}
			send(client, buf, ret, 0);
		}

		tr069_free(buf);
		buf = NULL;
done:
		closesocket(client);
		client = -1;
	}
}

#define ADJUEST_SOCKET_TIMEOUT 1
#define MAX_STUN_MSG_SIZE (sizeof(void *))
#define MAX_STUN_MSGQ_SIZE 16

struct stun_command {
	int command_id;
	unsigned int payload[0];
};

void enable_stun(void)
{
	struct stun_command * command = tr069_malloc(sizeof(struct stun_command));
	if (!command) {
		TR069_ERR("%s: failed", __func__);
		return;
	}
	command->command_id = 1;
	write(tr069_client_stun_fd,(void *)&command,MAX_STUN_MSG_SIZE);
	return;
}

static pthread_t s_tid_StunTcpTask;



void *stun_task(void * data)
{
	UNUSED(data);
	struct hostent * host_entry;
	struct stun_command * cmd = NULL;
	int ret;
	int fd = -1;
	struct sockaddr_in ip = {0};
	struct stun_hdr * hdr;
	struct timeval timeout_value ={0,0};
	//int stun_timeout = 20, sent_stun_cmd_state = 2,
	int sent_stun_cmd_state = 2;
	int	wait_connect_timeout = cpe.stun_max_period ;
	
	int connected_fd = -1;
	unlink(TR069_INNER_STUN_CONN_SOCKET);

	tr069_server_stun_fd = usock(USOCK_UNIX | USOCK_SERVER | USOCK_NONBLOCK, TR069_INNER_STUN_CONN_SOCKET, NULL);
	if (tr069_server_stun_fd < 0) {
		TR069_ERR("usock failed\n");
		return NULL;
	}

	ret = 0;
	ret = pthread_create(&s_tid_StunTcpTask, NULL, tcp_direct_connect, NULL);
	if(ret < 0){
		TR069_ERR("Failed to create stun_tcp thread, ret:%d",ret);
		return NULL;
	}

	while(1) {
		connected_fd = accept(tr069_server_stun_fd, NULL, 0);
		if (connected_fd < 0) {
			TR069_ERR("accept socket failed\n");
			usleep(1000);
		}
		else
			break;
	}
	tr069_accept_stun_fd = connected_fd;
	
	// Task main loop
	for (;;) {
		if (!cpe.stun_enable) {

			int len = read(tr069_accept_stun_fd,(void *)&cmd,MAX_STUN_MSG_SIZE);
			TR069_DEBUG("STUN re-enable...[%d]",len);
			if (cmd) tr069_free(cmd);
			cmd = NULL;
		} else {
			// Clear message if have
			//unsigned int timeout = 0;
			read(tr069_accept_stun_fd,(void *)&cmd,MAX_STUN_MSG_SIZE);
			if (cmd) tr069_free(cmd);
			cmd = NULL;
		}

		if (fd < 0) {
			fd = socket(AF_INET6, SOCK_DGRAM, 0);
			if (fd < 0) {
				TR069_ERR("Create socket failed(%d)", fd);
				continue;
			}

			timeout_value.tv_sec = wait_connect_timeout;
			timeout_value.tv_usec = 0;
			//timeout_value={wait_connect_timeout,0};
			
			ret = setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout_value, sizeof(struct timeval));
			if (ret < 0) {
				TR069_ERR("Set socket recv timeout failed(%d)", ret);
			}
		}

#ifdef ADJUEST_SOCKET_TIMEOUT
		if (fd > 0 && wait_connect_timeout != cpe.stun_max_period) {
			wait_connect_timeout = cpe.stun_max_period ;

			timeout_value.tv_sec = wait_connect_timeout;
			timeout_value.tv_usec = 0;
			
			ret = setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout_value, sizeof(struct timeval));
			if (ret < 0) {
				TR069_ERR("Set socket recv timeout failed(%d)", ret);
			}
		}
#else
		if (fd > 0 && wait_connect_timeout != cpe.stun_max_period / 2) {
			stun_timeout = wait_connect_timeout = cpe.stun_max_period / 2;
			
			timeout_value.tv_sec = wait_connect_timeout;
			timeout_value.tv_usec = 0;
			
			ret = setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout_value, sizeof(struct timeval));
			if (ret < 0) {
				TR069_ERR("Set socket recv timeout failed(%d)", ret);
			}
		}
#endif

		// Do DNS if need
		if (ip.sin_port == 0) {
			struct sockaddr_in6 addr;
			host_entry = gethostbyname(DEFAULT_STUN_SERVER);
			if (host_entry == 0) {
				TR069_ERR("DNS failed!");
				sleep(4);
				continue;
			}
			TR069_DEBUG("Get IP %d.%d.%d.%d", host_entry->h_addr_list[0][0] & 0xff,
										 host_entry->h_addr_list[0][1] & 0xff,
										 host_entry->h_addr_list[0][2] & 0xff,
										 host_entry->h_addr_list[0][3] & 0xff);
			ip.sin_addr = * (struct in_addr *) host_entry->h_addr_list[0];
			ip.sin_family = AF_INET;
			ip.sin_port = htons(3478);

			memset(&addr, 0, sizeof(addr));
			addr.sin6_family = AF_INET6;
    		addr.sin6_port = htons(234);
			ret = bind(fd, (struct sockaddr *)&addr, sizeof(struct sockaddr_in));
			if (ret < 0) {
				TR069_ERR("Bind port failed(%d)",ret);
				memset(&ip, 0, sizeof(ip));
				continue;
			}
		}

		// Build and send STUN message
		hdr = (struct stun_hdr *)buf;
		hdr->type = htons(BINDING_REQUEST);
		hdr->id[0] = 0xdf;
		RESET_MSG(&hdr->payload[0]);
		SET_U16_VALUE(CHANGE_REQUEST);
		SET_U16_VALUE(4);
		SET_U32_VALUE(0);

		SET_U16_VALUE(CONNECTION_REQUEST_BINDING);
		SET_U16_VALUE(((unsigned short int)strlen(CONNECTION_REQUEST_BINDING_STRING)));
		SET_STRING_VALUE(CONNECTION_REQUEST_BINDING_STRING);
		hdr->len = htons(GET_MSG_LENGTH());

		TR069_DEBUG("Send request to STUN server");
		ret = sendto(fd, buf, msglen + sizeof(struct stun_hdr), 0, (const struct sockaddr *)&ip, sizeof(ip));
		if (ret <= 0) {
			TR069_ERR("Socket send failed(%d)", ret);
			sleep(4);
			// If STUN request send failed, we continue into recv process
			// The recv process can help us delay and send request again
		}

		sent_stun_cmd_state = 2;
#ifdef ADJUEST_SOCKET_TIMEOUT
/*
		ret = setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &stun_timeout, sizeof(int));
		if (ret < 0) {
			TR069_ERR("Set socket recv timeout failed(%d)", ret);
		}
*/
#endif

		// 1) Recieve STUN binding response after our send request
		// 2) Wait the solicited request from ACS. The wait timeout is 'stun_max_period', it also have delay function
		while (sent_stun_cmd_state) {
			memset(buf, 0, BUF_SIZE);
			ret = recv(fd, buf, BUF_SIZE-1, 0);
			TR069_DEBUG("Socket recv %d", ret);
			do {
				struct stun_hdr * response = (struct stun_hdr *)buf;
				if (ret < 0) break;
				if ((unsigned int)ret < sizeof(struct stun_hdr)) {
					TR069_ERR("Recv STUN message too short(%d)", ret);
					break;
				}

				switch (htons(response->type)) {
				case BINDING_REQUEST:
					TR069_DEBUG("Handle binding request...");
					break;
				case BINDING_RESPONSE:
					TR069_DEBUG("Handle binding response...");
					if (htons(response->len) + sizeof(struct stun_hdr) < (unsigned int)ret) {
						TR069_ERR("Recv STUN message not complete");
						break;
					}
					handle_binding_response(&response->payload[0], (int)htons(response->len));
					break;
				case BINDING_ERR_RESP:
					TR069_DEBUG("Handle binding error response...");
					break;
				case SHARED_SECRET_REQ:
					TR069_DEBUG("Handle shared secret request...");
					break;
				case SHARED_SECRET_RESP:
					TR069_DEBUG("Handle shared secret response...");
					break;
				case SHARED_SECRET_ERR_RESP:
					TR069_DEBUG("Handle shared secret error response...");
					break;
				default:
					// Here is UDP connection request
					TR069_DEBUG("Handle ACS UDP connection request...");
					handle_udp_request((char *)buf, ret, 0);
					break;
				}
			} while (0);
#ifdef ADJUEST_SOCKET_TIMEOUT
			timeout_value.tv_sec = wait_connect_timeout;
			timeout_value.tv_usec = 0;


			ret = setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout_value, sizeof(struct timeval));
			if (ret < 0) {
				TR069_ERR("Set socket recv timeout failed(%d)", ret);
			}
#endif
			sent_stun_cmd_state--;
		}
	}
}