+/* try to decode a socks5 header */
+#define SSH_SOCKS5_AUTHDONE 0x1000
+#define SSH_SOCKS5_NOAUTH 0x00
+#define SSH_SOCKS5_IPV4 0x01
+#define SSH_SOCKS5_DOMAIN 0x03
+#define SSH_SOCKS5_IPV6 0x04
+#define SSH_SOCKS5_CONNECT 0x01
+#define SSH_SOCKS5_SUCCESS 0x00
+
+static int
+channel_decode_socks5(Channel *c, fd_set * readset, fd_set * writeset)
+{
+ struct {
+ u_int8_t version;
+ u_int8_t command;
+ u_int8_t reserved;
+ u_int8_t atyp;
+ } s5_req, s5_rsp;
+ u_int16_t dest_port;
+ u_char *p, dest_addr[255+1];
+ int i, have, found, nmethods, addrlen, af;
+
+ debug2("channel %d: decode socks5", c->self);
+ p = buffer_ptr(&c->input);
+ if (p[0] != 0x05)
+ return -1;
+ have = buffer_len(&c->input);
+ if (!(c->flags & SSH_SOCKS5_AUTHDONE)) {
+ /* format: ver | nmethods | methods */
+ if (have < 2)
+ return 0;
+ nmethods = p[1];
+ if (have < nmethods + 2)
+ return 0;
+ /* look for method: "NO AUTHENTICATION REQUIRED" */
+ for (found = 0, i = 2 ; i < nmethods + 2; i++) {
+ if (p[i] == SSH_SOCKS5_NOAUTH ) {
+ found = 1;
+ break;
+ }
+ }
+ if (!found) {
+ debug("channel %d: method SSH_SOCKS5_NOAUTH not found",
+ c->self);
+ return -1;
+ }
+ buffer_consume(&c->input, nmethods + 2);
+ buffer_put_char(&c->output, 0x05); /* version */
+ buffer_put_char(&c->output, SSH_SOCKS5_NOAUTH); /* method */
+ FD_SET(c->sock, writeset);
+ c->flags |= SSH_SOCKS5_AUTHDONE;
+ debug2("channel %d: socks5 auth done", c->self);
+ return 0; /* need more */
+ }
+ debug2("channel %d: socks5 post auth", c->self);
+ if (have < sizeof(s5_req)+1)
+ return 0; /* need more */
+ memcpy((char *)&s5_req, p, sizeof(s5_req));
+ if (s5_req.version != 0x05 ||
+ s5_req.command != SSH_SOCKS5_CONNECT ||
+ s5_req.reserved != 0x00) {
+ debug2("channel %d: only socks5 connect supported", c->self);
+ return -1;
+ }
+ switch (s5_req.atyp){
+ case SSH_SOCKS5_IPV4:
+ addrlen = 4;
+ af = AF_INET;
+ break;
+ case SSH_SOCKS5_DOMAIN:
+ addrlen = p[sizeof(s5_req)];
+ af = -1;
+ break;
+ case SSH_SOCKS5_IPV6:
+ addrlen = 16;
+ af = AF_INET6;
+ break;
+ default:
+ debug2("channel %d: bad socks5 atyp %d", c->self, s5_req.atyp);
+ return -1;
+ }
+ if (have < 4 + addrlen + 2)
+ return 0;
+ buffer_consume(&c->input, sizeof(s5_req));
+ if (s5_req.atyp == SSH_SOCKS5_DOMAIN)
+ buffer_consume(&c->input, 1); /* host string length */
+ buffer_get(&c->input, (char *)&dest_addr, addrlen);
+ buffer_get(&c->input, (char *)&dest_port, 2);
+ dest_addr[addrlen] = '\0';
+ if (s5_req.atyp == SSH_SOCKS5_DOMAIN)
+ strlcpy(c->path, (char *)dest_addr, sizeof(c->path));
+ else if (inet_ntop(af, dest_addr, c->path, sizeof(c->path)) == NULL)
+ return -1;
+ c->host_port = ntohs(dest_port);
+
+ debug2("channel %d: dynamic request: socks5 host %s port %u command %u",
+ c->self, c->path, c->host_port, s5_req.command);
+
+ s5_rsp.version = 0x05;
+ s5_rsp.command = SSH_SOCKS5_SUCCESS;
+ s5_rsp.reserved = 0; /* ignored */
+ s5_rsp.atyp = SSH_SOCKS5_IPV4;
+ ((struct in_addr *)&dest_addr)->s_addr = INADDR_ANY;
+ dest_port = 0; /* ignored */
+
+ buffer_append(&c->output, (char *)&s5_rsp, sizeof(s5_rsp));
+ buffer_append(&c->output, (char *)&dest_addr, sizeof(struct in_addr));
+ buffer_append(&c->output, (char *)&dest_port, sizeof(dest_port));
+ return 1;
+}
+