__be32 *xdr_encode_opaque_fixed(__be32 *p, const void *ptr, unsigned int nbytes)
{
	if (likely(nbytes != 0)) {
		unsigned int quadlen = XDR_QUADLEN(nbytes);
		unsigned int padding = (quadlen << 2) - nbytes;

		if (ptr != NULL)
			memcpy(p, ptr, nbytes);
		if (padding != 0)
			memset((char *)p + nbytes, 0, padding);
		p += quadlen;
	}
	return p;
}


static void xdr_buf_tail_copy_right(const struct xdr_buf *buf,
				    unsigned int base, unsigned int len,
				    unsigned int shift)
{
	const struct kvec *tail = buf->tail;
	unsigned int to = base + shift;

	if (to >= tail->iov_len)
		return;
	if (len + to > tail->iov_len)
		len = tail->iov_len - to;
	memmove(tail->iov_base + to, tail->iov_base + base, len);
}

sunrpc: Avoid a KASAN slab-out-of-bounds bug in xdr_set_page_base()

This seems to happen fairly easily during READ_PLUS testing on NFS v4.2.
I found that we could end up accessing xdr->buf->pages[pgnr] with a pgnr
greater than the number of pages in the array. So let's just return
early if we're setting base to a point at the end of the page data and
let xdr_set_tail_base() handle setting up the buffer pointers instead.

Signed-off-by: Anna Schumaker <Anna.Schumaker@Netapp.com>
Fixes: 8d86e373b0ef ("SUNRPC: Clean up helpers xdr_set_iov() and xdr_set_page_base()")
Signed-off-by: Trond Myklebust <trond.myklebust@hammerspace.com>

static void _copy_to_pages(struct page **, size_t, const char *, size_t);

static const struct kvec empty_iov = {.iov_base = NULL, .iov_len = 0};


_copy_to_pages(struct page **pages, size_t pgbase, const char *p, size_t len)
{
	struct page **pgto;
	char *vto;
	size_t copy;

	if (!len)
		return;

	pgto = pages + (pgbase >> PAGE_SHIFT);
	pgbase &= ~PAGE_MASK;

	for (;;) {
		copy = PAGE_SIZE - pgbase;
		if (copy > len)
			copy = len;

		vto = kmap_atomic(*pgto);
		memcpy(vto + pgbase, p, copy);
		kunmap_atomic(vto);

		len -= copy;
		if (len == 0)
			break;

		pgbase += copy;
		if (pgbase == PAGE_SIZE) {
			flush_dcache_page(*pgto);
			pgbase = 0;
			pgto++;
		}
		p += copy;
	}
	flush_dcache_page(*pgto);
}


static bool xdr_set_next_buffer(struct xdr_stream *xdr)
{
	if (xdr->page_ptr != NULL)
		xdr_set_next_page(xdr);
	else if (xdr->iov == xdr->buf->head)
		xdr_set_page(xdr, 0, xdr_stream_remaining(xdr));
	return xdr->p != xdr->end;
}


static unsigned int xdr_set_iov(struct xdr_stream *xdr, struct kvec *iov,
				unsigned int base, unsigned int len)
{
	if (len > iov->iov_len)
		len = iov->iov_len;
	if (unlikely(base > len))
		base = len;
	xdr->p = (__be32*)(iov->iov_base + base);
	xdr->end = (__be32*)(iov->iov_base + len);
	xdr->iov = iov;
	xdr->page_ptr = NULL;
	return len - base;
}


size_t xdr_buf_pagecount(const struct xdr_buf *buf)
{
	if (!buf->page_len)
		return 0;
	return (buf->page_base + buf->page_len + PAGE_SIZE - 1) >> PAGE_SHIFT;
}


int xdr_process_buf(const struct xdr_buf *buf, unsigned int offset,
		    unsigned int len,
		    int (*actor)(struct scatterlist *, void *), void *data)
{
	int i, ret = 0;
	unsigned int page_len, thislen, page_offset;
	struct scatterlist      sg[1];

	sg_init_table(sg, 1);

	if (offset >= buf->head[0].iov_len) {
		offset -= buf->head[0].iov_len;
	} else {
		thislen = buf->head[0].iov_len - offset;
		if (thislen > len)
			thislen = len;
		sg_set_buf(sg, buf->head[0].iov_base + offset, thislen);
		ret = actor(sg, data);
		if (ret)
			goto out;
		offset = 0;
		len -= thislen;
	}
	if (len == 0)
		goto out;

	if (offset >= buf->page_len) {
		offset -= buf->page_len;
	} else {
		page_len = buf->page_len - offset;
		if (page_len > len)
			page_len = len;
		len -= page_len;
		page_offset = (offset + buf->page_base) & (PAGE_SIZE - 1);
		i = (offset + buf->page_base) >> PAGE_SHIFT;
		thislen = PAGE_SIZE - page_offset;
		do {
			if (thislen > page_len)
				thislen = page_len;
			sg_set_page(sg, buf->pages[i], thislen, page_offset);
			ret = actor(sg, data);
			if (ret)
				goto out;
			page_len -= thislen;
			i++;
			page_offset = 0;
			thislen = PAGE_SIZE;
		} while (page_len != 0);
		offset = 0;
	}
	if (len == 0)
		goto out;
	if (offset < buf->tail[0].iov_len) {
		thislen = buf->tail[0].iov_len - offset;
		if (thislen > len)
			thislen = len;
		sg_set_buf(sg, buf->tail[0].iov_base + offset, thislen);
		ret = actor(sg, data);
		len -= thislen;
	}
	if (len != 0)
		ret = -EINVAL;
out:
	return ret;
}


ssize_t xdr_stream_decode_opaque_dup(struct xdr_stream *xdr, void **ptr,
		size_t maxlen, gfp_t gfp_flags)
{
	ssize_t ret;
	void *p;

	ret = xdr_stream_decode_opaque_inline(xdr, &p, maxlen);
	if (ret > 0) {
		*ptr = kmemdup(p, ret, gfp_flags);
		if (*ptr != NULL)
			return ret;
		ret = -ENOMEM;
	}
	*ptr = NULL;
	return ret;
}


_shift_data_right_pages(struct page **pages, size_t pgto_base,
		size_t pgfrom_base, size_t len)
{
	struct page **pgfrom, **pgto;
	char *vfrom, *vto;
	size_t copy;

	BUG_ON(pgto_base <= pgfrom_base);

	if (!len)
		return;

	pgto_base += len;
	pgfrom_base += len;

	pgto = pages + (pgto_base >> PAGE_SHIFT);
	pgfrom = pages + (pgfrom_base >> PAGE_SHIFT);

	pgto_base &= ~PAGE_MASK;
	pgfrom_base &= ~PAGE_MASK;

	do {
		/* Are any pointers crossing a page boundary? */
		if (pgto_base == 0) {
			pgto_base = PAGE_SIZE;
			pgto--;
		}
		if (pgfrom_base == 0) {
			pgfrom_base = PAGE_SIZE;
			pgfrom--;
		}

		copy = len;
		if (copy > pgto_base)
			copy = pgto_base;
		if (copy > pgfrom_base)
			copy = pgfrom_base;
		pgto_base -= copy;
		pgfrom_base -= copy;

		vto = kmap_atomic(*pgto);
		if (*pgto != *pgfrom) {
			vfrom = kmap_atomic(*pgfrom);
			memcpy(vto + pgto_base, vfrom + pgfrom_base, copy);
			kunmap_atomic(vfrom);
		} else
			memmove(vto + pgto_base, vto + pgfrom_base, copy);
		flush_dcache_page(*pgto);
		kunmap_atomic(vto);

	} while ((len -= copy) != 0);
}


bool xdr_stream_subsegment(struct xdr_stream *xdr, struct xdr_buf *subbuf,
			   unsigned int nbytes)
{
	unsigned int remaining, offset, len;

	if (xdr_buf_subsegment(xdr->buf, subbuf, xdr_stream_pos(xdr), nbytes))
		return false;

	if (subbuf->head[0].iov_len)
		if (!__xdr_inline_decode(xdr, subbuf->head[0].iov_len))
			return false;

	remaining = subbuf->page_len;
	offset = subbuf->page_base;
	while (remaining) {
		len = min_t(unsigned int, remaining, PAGE_SIZE) - offset;

		if (xdr->p == xdr->end && !xdr_set_next_buffer(xdr))
			return false;
		if (!__xdr_inline_decode(xdr, len))
			return false;

		remaining -= len;
		offset = 0;
	}

	return true;
}


xdr_decode_netobj(__be32 *p, struct xdr_netobj *obj)
{
	unsigned int	len;

	if ((len = be32_to_cpu(*p++)) > XDR_MAX_NETOBJ)
		return NULL;
	obj->len  = len;
	obj->data = (u8 *) p;
	return p + XDR_QUADLEN(len);
}


static void xdr_realign_pages(struct xdr_stream *xdr)
{
	struct xdr_buf *buf = xdr->buf;
	struct kvec *iov = buf->head;
	unsigned int cur = xdr_stream_pos(xdr);
	unsigned int copied;

	/* Realign pages to current pointer position */
	if (iov->iov_len > cur) {
		copied = xdr_shrink_bufhead(buf, cur);
		trace_rpc_xdr_alignment(xdr, cur, copied);
		xdr_set_page(xdr, 0, buf->page_len);
	}
}


static void xdr_buf_pages_copy_left(const struct xdr_buf *buf,
				    unsigned int base, unsigned int len,
				    unsigned int shift)
{
	unsigned int pgto;

	if (base >= buf->page_len)
		return;
	if (len > buf->page_len - base)
		len = buf->page_len - base;
	/* Shift data into head */
	if (shift > base) {
		const struct kvec *head = buf->head;
		unsigned int hdto = head->iov_len + base - shift;
		unsigned int hdlen = len;

		if (WARN_ONCE(shift > head->iov_len + base,
			      "SUNRPC: Misaligned data.\n"))
			return;
		if (hdto + hdlen > head->iov_len)
			hdlen = head->iov_len - hdto;
		_copy_from_pages(head->iov_base + hdto, buf->pages,
				 buf->page_base + base, hdlen);
		base += hdlen;
		len -= hdlen;
		if (!len)
			return;
	}
	pgto = base - shift;
	_shift_data_left_pages(buf->pages, buf->page_base + pgto,
			       buf->page_base + base, len);
}


xdr_encode_string(__be32 *p, const char *string)
{
	return xdr_encode_array(p, string, strlen(string));
}


int write_bytes_to_xdr_buf(const struct xdr_buf *buf, unsigned int base,
			   void *obj, unsigned int len)
{
	struct xdr_buf subbuf;
	int status;

	status = xdr_buf_subsegment(buf, &subbuf, base, len);
	if (status != 0)
		return status;
	__write_bytes_to_xdr_buf(&subbuf, obj, len);
	return 0;
}


static void xdr_buf_pages_shift_left(const struct xdr_buf *buf,
				     unsigned int base, unsigned int len,
				     unsigned int shift)
{
	if (!shift || !len)
		return;
	if (base >= buf->page_len) {
		xdr_buf_tail_shift_left(buf, base - buf->page_len, len, shift);
		return;
	}
	xdr_buf_pages_copy_left(buf, base, len, shift);
	len += base;
	if (len <= buf->page_len)
		return;
	xdr_buf_tail_copy_left(buf, 0, len - buf->page_len, shift);
}


int xdr_reserve_space_vec(struct xdr_stream *xdr, struct kvec *vec, size_t nbytes)
{
	int thislen;
	int v = 0;
	__be32 *p;

	/*
	 * svcrdma requires every READ payload to start somewhere
	 * in xdr->pages.
	 */
	if (xdr->iov == xdr->buf->head) {
		xdr->iov = NULL;
		xdr->end = xdr->p;
	}

	while (nbytes) {
		thislen = xdr->buf->page_len % PAGE_SIZE;
		thislen = min_t(size_t, nbytes, PAGE_SIZE - thislen);

		p = xdr_reserve_space(xdr, thislen);
		if (!p)
			return -EIO;

		vec[v].iov_base = p;
		vec[v].iov_len = thislen;
		v++;
		nbytes -= thislen;
	}

	return v;
}


static void __write_bytes_to_xdr_buf(const struct xdr_buf *subbuf,
				     void *obj, unsigned int len)
{
	unsigned int this_len;

	this_len = min_t(unsigned int, len, subbuf->head[0].iov_len);
	memcpy(subbuf->head[0].iov_base, obj, this_len);
	len -= this_len;
	obj += this_len;
	this_len = min_t(unsigned int, len, subbuf->page_len);
	_copy_to_pages(subbuf->pages, subbuf->page_base, obj, this_len);
	len -= this_len;
	obj += this_len;
	this_len = min_t(unsigned int, len, subbuf->tail[0].iov_len);
	memcpy(subbuf->tail[0].iov_base, obj, this_len);
}


__be32 * xdr_inline_decode(struct xdr_stream *xdr, size_t nbytes)
{
	__be32 *p;

	if (unlikely(nbytes == 0))
		return xdr->p;
	if (xdr->p == xdr->end && !xdr_set_next_buffer(xdr))
		goto out_overflow;
	p = __xdr_inline_decode(xdr, nbytes);
	if (p != NULL)
		return p;
	return xdr_copy_to_scratch(xdr, nbytes);
out_overflow:
	trace_rpc_xdr_overflow(xdr, nbytes);
	return NULL;
}


int xdr_encode_array2(const struct xdr_buf *buf, unsigned int base,
		      struct xdr_array2_desc *desc)
{
	if ((unsigned long) base + 4 + desc->array_len * desc->elem_size >
	    buf->head->iov_len + buf->page_len + buf->tail->iov_len)
		return -EINVAL;

	return xdr_xcode_array2(buf, base, desc, 1);
}


unsigned int xdr_read_pages(struct xdr_stream *xdr, unsigned int len)
{
	unsigned int nwords = XDR_QUADLEN(len);
	unsigned int base, end, pglen;

	pglen = xdr_align_pages(xdr, nwords << 2);
	if (pglen == 0)
		return 0;

	base = (nwords << 2) - pglen;
	end = xdr_stream_remaining(xdr) - pglen;

	xdr_set_tail_base(xdr, base, end);
	return len <= pglen ? len : pglen;
}


int xdr_restrict_buflen(struct xdr_stream *xdr, int newbuflen)
{
	struct xdr_buf *buf = xdr->buf;
	int left_in_this_buf = (void *)xdr->end - (void *)xdr->p;
	int end_offset = buf->len + left_in_this_buf;

	if (newbuflen < 0 || newbuflen < buf->len)
		return -1;
	if (newbuflen > buf->buflen)
		return 0;
	if (newbuflen < end_offset)
		xdr->end = (void *)xdr->end + newbuflen - end_offset;
	buf->buflen = newbuflen;
	return 0;
}


static void xdr_set_next_page(struct xdr_stream *xdr)
{
	unsigned int newbase;

	newbase = (1 + xdr->page_ptr - xdr->buf->pages) << PAGE_SHIFT;
	newbase -= xdr->buf->page_base;
	if (newbase < xdr->buf->page_len)
		xdr_set_page_base(xdr, newbase, xdr_stream_remaining(xdr));
	else
		xdr_set_tail_base(xdr, 0, xdr_stream_remaining(xdr));
}


unsigned int xdr_stream_pos(const struct xdr_stream *xdr)
{
	return (unsigned int)(XDR_QUADLEN(xdr->buf->len) - xdr->nwords) << 2;
}


int read_bytes_from_xdr_buf(const struct xdr_buf *buf, unsigned int base,
			    void *obj, unsigned int len)
{
	struct xdr_buf subbuf;
	int status;

	status = xdr_buf_subsegment(buf, &subbuf, base, len);
	if (status != 0)
		return status;
	__read_bytes_from_xdr_buf(&subbuf, obj, len);
	return 0;
}


unsigned int xdr_align_data(struct xdr_stream *xdr, unsigned int offset,
			    unsigned int length)
{
	struct xdr_buf *buf = xdr->buf;
	unsigned int from, bytes, len;
	unsigned int shift;

	xdr_realign_pages(xdr);
	from = xdr_page_pos(xdr);

	if (from >= buf->page_len + buf->tail->iov_len)
		return 0;
	if (from + buf->head->iov_len >= buf->len)
		return 0;

	len = buf->len - buf->head->iov_len;

	/* We only shift data left! */
	if (WARN_ONCE(from < offset, "SUNRPC: misaligned data src=%u dst=%u\n",
		      from, offset))
		return 0;
	if (WARN_ONCE(offset > buf->page_len,
		      "SUNRPC: buffer overflow. offset=%u, page_len=%u\n",
		      offset, buf->page_len))
		return 0;

	/* Move page data to the left */
	shift = from - offset;
	xdr_buf_pages_shift_left(buf, from, len, shift);

	bytes = xdr_stream_remaining(xdr);
	if (length > bytes)
		length = bytes;
	bytes -= length;

	xdr->buf->len -= shift;
	xdr_set_page(xdr, offset + length, bytes);
	return length;
}


void xdr_init_encode(struct xdr_stream *xdr, struct xdr_buf *buf, __be32 *p,
		     struct rpc_rqst *rqst)
{
	struct kvec *iov = buf->head;
	int scratch_len = buf->buflen - buf->page_len - buf->tail[0].iov_len;

	xdr_reset_scratch_buffer(xdr);
	BUG_ON(scratch_len < 0);
	xdr->buf = buf;
	xdr->iov = iov;
	xdr->p = (__be32 *)((char *)iov->iov_base + iov->iov_len);
	xdr->end = (__be32 *)((char *)iov->iov_base + scratch_len);
	BUG_ON(iov->iov_len > scratch_len);

	if (p != xdr->p && p != NULL) {
		size_t len;

		BUG_ON(p < xdr->p || p > xdr->end);
		len = (char *)p - (char *)xdr->p;
		xdr->p = p;
		buf->len += len;
		iov->iov_len += len;
	}
	xdr->rqst = rqst;
}


xdr_decode_string_inplace(__be32 *p, char **sp,
			  unsigned int *lenp, unsigned int maxlen)
{
	u32 len;

	len = be32_to_cpu(*p++);
	if (len > maxlen)
		return NULL;
	*lenp = len;
	*sp = (char *) p;
	return p + XDR_QUADLEN(len);
}


static void xdr_set_page(struct xdr_stream *xdr, unsigned int base,
			 unsigned int len)
{
	if (xdr_set_page_base(xdr, base, len) == 0) {
		base -= xdr->buf->page_len;
		xdr_set_tail_base(xdr, base, len);
	}
}


static void xdr_buf_pages_copy_right(const struct xdr_buf *buf,
				     unsigned int base, unsigned int len,
				     unsigned int shift)
{
	const struct kvec *tail = buf->tail;
	unsigned int to = base + shift;
	unsigned int pglen = 0;
	unsigned int talen = 0, tato = 0;

	if (base >= buf->page_len)
		return;
	if (len > buf->page_len - base)
		len = buf->page_len - base;
	if (to >= buf->page_len) {
		tato = to - buf->page_len;
		if (tail->iov_len >= len + tato)
			talen = len;
		else if (tail->iov_len > tato)
			talen = tail->iov_len - tato;
	} else if (len + to >= buf->page_len) {
		pglen = buf->page_len - to;
		talen = len - pglen;
		if (talen > tail->iov_len)
			talen = tail->iov_len;
	} else
		pglen = len;

	_copy_from_pages(tail->iov_base + tato, buf->pages,
			 buf->page_base + base + pglen, talen);
	_shift_data_right_pages(buf->pages, buf->page_base + to,
				buf->page_base + base, pglen);
}


xdr_alloc_bvec(struct xdr_buf *buf, gfp_t gfp)
{
	size_t i, n = xdr_buf_pagecount(buf);

	if (n != 0 && buf->bvec == NULL) {
		buf->bvec = kmalloc_array(n, sizeof(buf->bvec[0]), gfp);
		if (!buf->bvec)
			return -ENOMEM;
		for (i = 0; i < n; i++) {
			buf->bvec[i].bv_page = buf->pages[i];
			buf->bvec[i].bv_len = PAGE_SIZE;
			buf->bvec[i].bv_offset = 0;
		}
	}
	return 0;
}


unsigned int xdr_expand_hole(struct xdr_stream *xdr, unsigned int offset,
			     unsigned int length)
{
	struct xdr_buf *buf = xdr->buf;
	unsigned int from, to, shift;

	xdr_realign_pages(xdr);
	from = xdr_page_pos(xdr);
	to = xdr_align_size(offset + length);

	/* Could the hole be behind us? */
	if (to > from) {
		unsigned int buflen = buf->len - buf->head->iov_len;
		shift = to - from;
		xdr_buf_try_expand(buf, shift);
		xdr_buf_pages_shift_right(buf, from, buflen, shift);
		xdr_set_page(xdr, to, xdr_stream_remaining(xdr));
	} else if (to != from)
		xdr_align_data(xdr, to, 0);
	xdr_buf_pages_zero(buf, offset, length);

	return length;
}


static void xdr_buf_tail_shift_right(const struct xdr_buf *buf,
				     unsigned int base, unsigned int len,
				     unsigned int shift)
{
	const struct kvec *tail = buf->tail;

	if (base >= tail->iov_len || !shift || !len)
		return;
	xdr_buf_tail_copy_right(buf, base, len, shift);
}


static int xdr_xcode_array2(const struct xdr_buf *buf, unsigned int base,
			    struct xdr_array2_desc *desc, int encode)
{
	char *elem = NULL, *c;
	unsigned int copied = 0, todo, avail_here;
	struct page **ppages = NULL;
	int err;

	if (encode) {
		if (xdr_encode_word(buf, base, desc->array_len) != 0)
			return -EINVAL;
	} else {
		if (xdr_decode_word(buf, base, &desc->array_len) != 0 ||
		    desc->array_len > desc->array_maxlen ||
		    (unsigned long) base + 4 + desc->array_len *
				    desc->elem_size > buf->len)
			return -EINVAL;
	}
	base += 4;

	if (!desc->xcode)
		return 0;

	todo = desc->array_len * desc->elem_size;

	/* process head */
	if (todo && base < buf->head->iov_len) {
		c = buf->head->iov_base + base;
		avail_here = min_t(unsigned int, todo,
				   buf->head->iov_len - base);
		todo -= avail_here;

		while (avail_here >= desc->elem_size) {
			err = desc->xcode(desc, c);
			if (err)
				goto out;
			c += desc->elem_size;
			avail_here -= desc->elem_size;
		}
		if (avail_here) {
			if (!elem) {
				elem = kmalloc(desc->elem_size, GFP_KERNEL);
				err = -ENOMEM;
				if (!elem)
					goto out;
			}
			if (encode) {
				err = desc->xcode(desc, elem);
				if (err)
					goto out;
				memcpy(c, elem, avail_here);
			} else
				memcpy(elem, c, avail_here);
			copied = avail_here;
		}
		base = buf->head->iov_len;  /* align to start of pages */
	}

	/* process pages array */
	base -= buf->head->iov_len;
	if (todo && base < buf->page_len) {
		unsigned int avail_page;

		avail_here = min(todo, buf->page_len - base);
		todo -= avail_here;

		base += buf->page_base;
		ppages = buf->pages + (base >> PAGE_SHIFT);
		base &= ~PAGE_MASK;
		avail_page = min_t(unsigned int, PAGE_SIZE - base,
					avail_here);
		c = kmap(*ppages) + base;

		while (avail_here) {
			avail_here -= avail_page;
			if (copied || avail_page < desc->elem_size) {
				unsigned int l = min(avail_page,
					desc->elem_size - copied);
				if (!elem) {
					elem = kmalloc(desc->elem_size,
						       GFP_KERNEL);
					err = -ENOMEM;
					if (!elem)
						goto out;
				}
				if (encode) {
					if (!copied) {
						err = desc->xcode(desc, elem);
						if (err)
							goto out;
					}
					memcpy(c, elem + copied, l);
					copied += l;
					if (copied == desc->elem_size)
						copied = 0;
				} else {
					memcpy(elem + copied, c, l);
					copied += l;
					if (copied == desc->elem_size) {
						err = desc->xcode(desc, elem);
						if (err)
							goto out;
						copied = 0;
					}
				}
				avail_page -= l;
				c += l;
			}
			while (avail_page >= desc->elem_size) {
				err = desc->xcode(desc, c);
				if (err)
					goto out;
				c += desc->elem_size;
				avail_page -= desc->elem_size;
			}
			if (avail_page) {
				unsigned int l = min(avail_page,
					    desc->elem_size - copied);
				if (!elem) {
					elem = kmalloc(desc->elem_size,
						       GFP_KERNEL);
					err = -ENOMEM;
					if (!elem)
						goto out;
				}
				if (encode) {
					if (!copied) {
						err = desc->xcode(desc, elem);
						if (err)
							goto out;
					}
					memcpy(c, elem + copied, l);
					copied += l;
					if (copied == desc->elem_size)
						copied = 0;
				} else {
					memcpy(elem + copied, c, l);
					copied += l;
					if (copied == desc->elem_size) {
						err = desc->xcode(desc, elem);
						if (err)
							goto out;
						copied = 0;
					}
				}
			}
			if (avail_here) {
				kunmap(*ppages);
				ppages++;
				c = kmap(*ppages);
			}

			avail_page = min(avail_here,
				 (unsigned int) PAGE_SIZE);
		}
		base = buf->page_len;  /* align to start of tail */
	}

	/* process tail */
	base -= buf->page_len;
	if (todo) {
		c = buf->tail->iov_base + base;
		if (copied) {
			unsigned int l = desc->elem_size - copied;

			if (encode)
				memcpy(c, elem + copied, l);
			else {
				memcpy(elem + copied, c, l);
				err = desc->xcode(desc, elem);
				if (err)
					goto out;
			}
			todo -= l;
			c += l;
		}
		while (todo) {
			err = desc->xcode(desc, c);
			if (err)
				goto out;
			c += desc->elem_size;
			todo -= desc->elem_size;
		}
	}
	err = 0;

out:
	kfree(elem);
	if (ppages)
		kunmap(*ppages);
	return err;
}


void xdr_enter_page(struct xdr_stream *xdr, unsigned int len)
{
	len = xdr_align_pages(xdr, len);
	/*
	 * Position current pointer at beginning of tail, and
	 * set remaining message length.
	 */
	if (len != 0)
		xdr_set_page_base(xdr, 0, len);
}


xdr_encode_netobj(__be32 *p, const struct xdr_netobj *obj)
{
	unsigned int	quadlen = XDR_QUADLEN(obj->len);

	p[quadlen] = 0;		/* zero trailing bytes */
	*p++ = cpu_to_be32(obj->len);
	memcpy(p, obj->data, obj->len);
	return p + XDR_QUADLEN(obj->len);
}


unsigned int xdr_page_pos(const struct xdr_stream *xdr)
{
	unsigned int pos = xdr_stream_pos(xdr);

	WARN_ON(pos < xdr->buf->head[0].iov_len);
	return pos - xdr->buf->head[0].iov_len;
}


static void xdr_buf_tail_copy_left(const struct xdr_buf *buf, unsigned int base,
				   unsigned int len, unsigned int shift)
{
	const struct kvec *tail = buf->tail;

	if (base >= tail->iov_len)
		return;
	if (len > tail->iov_len - base)
		len = tail->iov_len - base;
	/* Shift data into head */
	if (shift > buf->page_len + base) {
		const struct kvec *head = buf->head;
		unsigned int hdto =
			head->iov_len + buf->page_len + base - shift;
		unsigned int hdlen = len;

		if (WARN_ONCE(shift > head->iov_len + buf->page_len + base,
			      "SUNRPC: Misaligned data.\n"))
			return;
		if (hdto + hdlen > head->iov_len)
			hdlen = head->iov_len - hdto;
		memcpy(head->iov_base + hdto, tail->iov_base + base, hdlen);
		base += hdlen;
		len -= hdlen;
		if (!len)
			return;
	}
	/* Shift data into pages */
	if (shift > base) {
		unsigned int pgto = buf->page_len + base - shift;
		unsigned int pglen = len;

		if (pgto + pglen > buf->page_len)
			pglen = buf->page_len - pgto;
		_copy_to_pages(buf->pages, buf->page_base + pgto,
			       tail->iov_base + base, pglen);
		base += pglen;
		len -= pglen;
		if (!len)
			return;
	}
	memmove(tail->iov_base + base - shift, tail->iov_base + base, len);
}


static void xdr_buf_pages_zero(const struct xdr_buf *buf, unsigned int pgbase,
			       unsigned int len)
{
	struct page **pages = buf->pages;
	struct page **page;
	char *vpage;
	unsigned int zero;

	if (!len)
		return;
	if (pgbase >= buf->page_len) {
		xdr_buf_iov_zero(buf->tail, pgbase - buf->page_len, len);
		return;
	}
	if (pgbase + len > buf->page_len) {
		xdr_buf_iov_zero(buf->tail, 0, pgbase + len - buf->page_len);
		len = buf->page_len - pgbase;
	}

	pgbase += buf->page_base;

	page = pages + (pgbase >> PAGE_SHIFT);
	pgbase &= ~PAGE_MASK;

	do {
		zero = PAGE_SIZE - pgbase;
		if (zero > len)
			zero = len;

		vpage = kmap_atomic(*page);
		memset(vpage + pgbase, 0, zero);
		kunmap_atomic(vpage);

		flush_dcache_page(*page);
		pgbase = 0;
		page++;

	} while ((len -= zero) != 0);
}


void xdr_init_decode_pages(struct xdr_stream *xdr, struct xdr_buf *buf,
			   struct page **pages, unsigned int len)
{
	memset(buf, 0, sizeof(*buf));
	buf->pages =  pages;
	buf->page_len =  len;
	buf->buflen =  len;
	buf->len = len;
	xdr_init_decode(xdr, buf, NULL, NULL);
}


static __be32 * __xdr_inline_decode(struct xdr_stream *xdr, size_t nbytes)
{
	unsigned int nwords = XDR_QUADLEN(nbytes);
	__be32 *p = xdr->p;
	__be32 *q = p + nwords;

	if (unlikely(nwords > xdr->nwords || q > xdr->end || q < p))
		return NULL;
	xdr->p = q;
	xdr->nwords -= nwords;
	return p;
}


ssize_t xdr_stream_decode_string_dup(struct xdr_stream *xdr, char **str,
		size_t maxlen, gfp_t gfp_flags)
{
	void *p;
	ssize_t ret;

	ret = xdr_stream_decode_opaque_inline(xdr, &p, maxlen);
	if (ret > 0) {
		char *s = kmemdup_nul(p, ret, gfp_flags);
		if (s != NULL) {
			*str = s;
			return strlen(s);
		}
		ret = -ENOMEM;
	}
	*str = NULL;
	return ret;
}


static unsigned int xdr_buf_pages_fill_sparse(const struct xdr_buf *buf,
					      unsigned int buflen, gfp_t gfp)
{
	unsigned int i, npages, pagelen;

	if (!(buf->flags & XDRBUF_SPARSE_PAGES))
		return buflen;
	if (buflen <= buf->head->iov_len)
		return buflen;
	pagelen = buflen - buf->head->iov_len;
	if (pagelen > buf->page_len)
		pagelen = buf->page_len;
	npages = (pagelen + buf->page_base + PAGE_SIZE - 1) >> PAGE_SHIFT;
	for (i = 0; i < npages; i++) {
		if (!buf->pages[i])
			continue;
		buf->pages[i] = alloc_page(gfp);
		if (likely(buf->pages[i]))
			continue;
		buflen -= pagelen;
		pagelen = i << PAGE_SHIFT;
		if (pagelen > buf->page_base)
			buflen += pagelen - buf->page_base;
		break;
	}
	return buflen;
}


static void __read_bytes_from_xdr_buf(const struct xdr_buf *subbuf,
				      void *obj, unsigned int len)
{
	unsigned int this_len;

	this_len = min_t(unsigned int, len, subbuf->head[0].iov_len);
	memcpy(obj, subbuf->head[0].iov_base, this_len);
	len -= this_len;
	obj += this_len;
	this_len = min_t(unsigned int, len, subbuf->page_len);
	_copy_from_pages(obj, subbuf->pages, subbuf->page_base, this_len);
	len -= this_len;
	obj += this_len;
	this_len = min_t(unsigned int, len, subbuf->tail[0].iov_len);
	memcpy(obj, subbuf->tail[0].iov_base, this_len);
}


static void printoption(struct Curl_easy *data,
                        const char *direction, int cmd, int option)
{
  if(data->set.verbose) {
    if(cmd == CURL_IAC) {
      if(CURL_TELCMD_OK(option))
        infof(data, "%s IAC %s\n", direction, CURL_TELCMD(option));
      else
        infof(data, "%s IAC %d\n", direction, option);
    }
    else {
      const char *fmt = (cmd == CURL_WILL) ? "WILL" :
                        (cmd == CURL_WONT) ? "WONT" :
                        (cmd == CURL_DO) ? "DO" :
                        (cmd == CURL_DONT) ? "DONT" : 0;
      if(fmt) {
        const char *opt;
        if(CURL_TELOPT_OK(option))
          opt = CURL_TELOPT(option);
        else if(option == CURL_TELOPT_EXOPL)
          opt = "EXOPL";
        else
          opt = NULL;

        if(opt)
          infof(data, "%s %s %s\n", direction, fmt, opt);
        else
          infof(data, "%s %s %d\n", direction, fmt, option);
      }
      else
        infof(data, "%s %d %d\n", direction, cmd, option);
    }
  }
}

telnet: check sscanf() for correct number of matches

CVE-2021-22898

Bug: https://curl.se/docs/CVE-2021-22898.html

#define printoption(a,b,c,d)  Curl_nop_stmt

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}

const struct Curl_handler Curl_handler_telnet = {
  "TELNET",                             /* scheme */
  ZERO_NULL,                            /* setup_connection */
  telnet_do,                            /* do_it */
  telnet_done,                          /* done */
  ZERO_NULL,                            /* do_more */
  ZERO_NULL,                            /* connect_it */
  ZERO_NULL,                            /* connecting */
  ZERO_NULL,                            /* doing */
  ZERO_NULL,                            /* proto_getsock */
  ZERO_NULL,                            /* doing_getsock */
  ZERO_NULL,                            /* domore_getsock */
  ZERO_NULL,                            /* perform_getsock */
  ZERO_NULL,                            /* disconnect */
  ZERO_NULL,                            /* readwrite */
  ZERO_NULL,                            /* connection_check */
  ZERO_NULL,                            /* attach connection */
  PORT_TELNET,                          /* defport */
  CURLPROTO_TELNET,                     /* protocol */
  CURLPROTO_TELNET,                     /* family */
  PROTOPT_NONE | PROTOPT_NOURLQUERY     /* flags */
};


static CURLcode telnet_do(struct Curl_easy *data, bool *done)
{
  CURLcode result;
  struct connectdata *conn = data->conn;
  curl_socket_t sockfd = conn->sock[FIRSTSOCKET];
#ifdef USE_WINSOCK
  WSAEVENT event_handle;
  WSANETWORKEVENTS events;
  HANDLE stdin_handle;
  HANDLE objs[2];
  DWORD  obj_count;
  DWORD  wait_timeout;
  DWORD readfile_read;
  int err;
#else
  timediff_t interval_ms;
  struct pollfd pfd[2];
  int poll_cnt;
  curl_off_t total_dl = 0;
  curl_off_t total_ul = 0;
#endif
  ssize_t nread;
  struct curltime now;
  bool keepon = TRUE;
  char *buf = data->state.buffer;
  struct TELNET *tn;

  *done = TRUE; /* unconditionally */

  result = init_telnet(data);
  if(result)
    return result;

  tn = data->req.p.telnet;

  result = check_telnet_options(data);
  if(result)
    return result;

#ifdef USE_WINSOCK
  /* We want to wait for both stdin and the socket. Since
  ** the select() function in winsock only works on sockets
  ** we have to use the WaitForMultipleObjects() call.
  */

  /* First, create a sockets event object */
  event_handle = WSACreateEvent();
  if(event_handle == WSA_INVALID_EVENT) {
    failf(data, "WSACreateEvent failed (%d)", SOCKERRNO);
    return CURLE_FAILED_INIT;
  }

  /* Tell winsock what events we want to listen to */
  if(WSAEventSelect(sockfd, event_handle, FD_READ|FD_CLOSE) == SOCKET_ERROR) {
    WSACloseEvent(event_handle);
    return CURLE_OK;
  }

  /* The get the Windows file handle for stdin */
  stdin_handle = GetStdHandle(STD_INPUT_HANDLE);

  /* Create the list of objects to wait for */
  objs[0] = event_handle;
  objs[1] = stdin_handle;

  /* If stdin_handle is a pipe, use PeekNamedPipe() method to check it,
     else use the old WaitForMultipleObjects() way */
  if(GetFileType(stdin_handle) == FILE_TYPE_PIPE ||
     data->set.is_fread_set) {
    /* Don't wait for stdin_handle, just wait for event_handle */
    obj_count = 1;
    /* Check stdin_handle per 100 milliseconds */
    wait_timeout = 100;
  }
  else {
    obj_count = 2;
    wait_timeout = 1000;
  }

  /* Keep on listening and act on events */
  while(keepon) {
    const DWORD buf_size = (DWORD)data->set.buffer_size;
    DWORD waitret = WaitForMultipleObjects(obj_count, objs,
                                           FALSE, wait_timeout);
    switch(waitret) {

    case WAIT_TIMEOUT:
    {
      for(;;) {
        if(data->set.is_fread_set) {
          size_t n;
          /* read from user-supplied method */
          n = data->state.fread_func(buf, 1, buf_size, data->state.in);
          if(n == CURL_READFUNC_ABORT) {
            keepon = FALSE;
            result = CURLE_READ_ERROR;
            break;
          }

          if(n == CURL_READFUNC_PAUSE)
            break;

          if(n == 0)                        /* no bytes */
            break;

          /* fall through with number of bytes read */
          readfile_read = (DWORD)n;
        }
        else {
          /* read from stdin */
          if(!PeekNamedPipe(stdin_handle, NULL, 0, NULL,
                            &readfile_read, NULL)) {
            keepon = FALSE;
            result = CURLE_READ_ERROR;
            break;
          }

          if(!readfile_read)
            break;

          if(!ReadFile(stdin_handle, buf, buf_size,
                       &readfile_read, NULL)) {
            keepon = FALSE;
            result = CURLE_READ_ERROR;
            break;
          }
        }

        result = send_telnet_data(data, buf, readfile_read);
        if(result) {
          keepon = FALSE;
          break;
        }
      }
    }
    break;

    case WAIT_OBJECT_0 + 1:
    {
      if(!ReadFile(stdin_handle, buf, buf_size,
                   &readfile_read, NULL)) {
        keepon = FALSE;
        result = CURLE_READ_ERROR;
        break;
      }

      result = send_telnet_data(data, buf, readfile_read);
      if(result) {
        keepon = FALSE;
        break;
      }
    }
    break;

    case WAIT_OBJECT_0:
    {
      events.lNetworkEvents = 0;
      if(WSAEnumNetworkEvents(sockfd, event_handle, &events) == SOCKET_ERROR) {
        err = SOCKERRNO;
        if(err != EINPROGRESS) {
          infof(data, "WSAEnumNetworkEvents failed (%d)", err);
          keepon = FALSE;
          result = CURLE_READ_ERROR;
        }
        break;
      }
      if(events.lNetworkEvents & FD_READ) {
        /* read data from network */
        result = Curl_read(data, sockfd, buf, data->set.buffer_size, &nread);
        /* read would've blocked. Loop again */
        if(result == CURLE_AGAIN)
          break;
        /* returned not-zero, this an error */
        else if(result) {
          keepon = FALSE;
          break;
        }
        /* returned zero but actually received 0 or less here,
           the server closed the connection and we bail out */
        else if(nread <= 0) {
          keepon = FALSE;
          break;
        }

        result = telrcv(data, (unsigned char *) buf, nread);
        if(result) {
          keepon = FALSE;
          break;
        }

        /* Negotiate if the peer has started negotiating,
           otherwise don't. We don't want to speak telnet with
           non-telnet servers, like POP or SMTP. */
        if(tn->please_negotiate && !tn->already_negotiated) {
          negotiate(data);
          tn->already_negotiated = 1;
        }
      }
      if(events.lNetworkEvents & FD_CLOSE) {
        keepon = FALSE;
      }
    }
    break;

    }

    if(data->set.timeout) {
      now = Curl_now();
      if(Curl_timediff(now, conn->created) >= data->set.timeout) {
        failf(data, "Time-out");
        result = CURLE_OPERATION_TIMEDOUT;
        keepon = FALSE;
      }
    }
  }

  /* We called WSACreateEvent, so call WSACloseEvent */
  if(!WSACloseEvent(event_handle)) {
    infof(data, "WSACloseEvent failed (%d)", SOCKERRNO);
  }
#else
  pfd[0].fd = sockfd;
  pfd[0].events = POLLIN;

  if(data->set.is_fread_set) {
    poll_cnt = 1;
    interval_ms = 100; /* poll user-supplied read function */
  }
  else {
    /* really using fread, so infile is a FILE* */
    pfd[1].fd = fileno((FILE *)data->state.in);
    pfd[1].events = POLLIN;
    poll_cnt = 2;
    interval_ms = 1 * 1000;
  }

  while(keepon) {
    switch(Curl_poll(pfd, poll_cnt, interval_ms)) {
    case -1:                    /* error, stop reading */
      keepon = FALSE;
      continue;
    case 0:                     /* timeout */
      pfd[0].revents = 0;
      pfd[1].revents = 0;
      /* FALLTHROUGH */
    default:                    /* read! */
      if(pfd[0].revents & POLLIN) {
        /* read data from network */
        result = Curl_read(data, sockfd, buf, data->set.buffer_size, &nread);
        /* read would've blocked. Loop again */
        if(result == CURLE_AGAIN)
          break;
        /* returned not-zero, this an error */
        if(result) {
          keepon = FALSE;
          break;
        }
        /* returned zero but actually received 0 or less here,
           the server closed the connection and we bail out */
        else if(nread <= 0) {
          keepon = FALSE;
          break;
        }

        total_dl += nread;
        Curl_pgrsSetDownloadCounter(data, total_dl);
        result = telrcv(data, (unsigned char *)buf, nread);
        if(result) {
          keepon = FALSE;
          break;
        }

        /* Negotiate if the peer has started negotiating,
           otherwise don't. We don't want to speak telnet with
           non-telnet servers, like POP or SMTP. */
        if(tn->please_negotiate && !tn->already_negotiated) {
          negotiate(data);
          tn->already_negotiated = 1;
        }
      }

      nread = 0;
      if(poll_cnt == 2) {
        if(pfd[1].revents & POLLIN) { /* read from in file */
          nread = read(pfd[1].fd, buf, data->set.buffer_size);
        }
      }
      else {
        /* read from user-supplied method */
        nread = (int)data->state.fread_func(buf, 1, data->set.buffer_size,
                                            data->state.in);
        if(nread == CURL_READFUNC_ABORT) {
          keepon = FALSE;
          break;
        }
        if(nread == CURL_READFUNC_PAUSE)
          break;
      }

      if(nread > 0) {
        result = send_telnet_data(data, buf, nread);
        if(result) {
          keepon = FALSE;
          break;
        }
        total_ul += nread;
        Curl_pgrsSetUploadCounter(data, total_ul);
      }
      else if(nread < 0)
        keepon = FALSE;

      break;
    } /* poll switch statement */

    if(data->set.timeout) {
      now = Curl_now();
      if(Curl_timediff(now, conn->created) >= data->set.timeout) {
        failf(data, "Time-out");
        result = CURLE_OPERATION_TIMEDOUT;
        keepon = FALSE;
      }
    }

    if(Curl_pgrsUpdate(data)) {
      result = CURLE_ABORTED_BY_CALLBACK;
      break;
    }
  }
#endif
  /* mark this as "no further transfer wanted" */
  Curl_setup_transfer(data, -1, -1, FALSE, -1);

  return result;
}

#define CURL_SB_TERM(x)                                 \
  do {                                                  \
    x->subend = x->subpointer;                          \
    CURL_SB_CLEAR(x);                                   \
  } while(0)

#define CURL_SB_ACCUM(x,c)                                      \
  do {                                                          \
    if(x->subpointer < (x->subbuffer + sizeof(x->subbuffer)))   \
      *x->subpointer++ = (c);                                   \
  } while(0)

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}


void set_remote_option(struct Curl_easy *data, int option, int newstate)
{
  struct TELNET *tn = data->req.p.telnet;
  if(newstate == CURL_YES) {
    switch(tn->him[option]) {
    case CURL_NO:
      tn->him[option] = CURL_WANTYES;
      send_negotiation(data, CURL_DO, option);
      break;

    case CURL_YES:
      /* Already enabled */
      break;

    case CURL_WANTNO:
      switch(tn->himq[option]) {
      case CURL_EMPTY:
        /* Already negotiating for CURL_YES, queue the request */
        tn->himq[option] = CURL_OPPOSITE;
        break;
      case CURL_OPPOSITE:
        /* Error: already queued an enable request */
        break;
      }
      break;

    case CURL_WANTYES:
      switch(tn->himq[option]) {
      case CURL_EMPTY:
        /* Error: already negotiating for enable */
        break;
      case CURL_OPPOSITE:
        tn->himq[option] = CURL_EMPTY;
        break;
      }
      break;
    }
  }
  else { /* NO */
    switch(tn->him[option]) {
    case CURL_NO:
      /* Already disabled */
      break;

    case CURL_YES:
      tn->him[option] = CURL_WANTNO;
      send_negotiation(data, CURL_DONT, option);
      break;

    case CURL_WANTNO:
      switch(tn->himq[option]) {
      case CURL_EMPTY:
        /* Already negotiating for NO */
        break;
      case CURL_OPPOSITE:
        tn->himq[option] = CURL_EMPTY;
        break;
      }
      break;

    case CURL_WANTYES:
      switch(tn->himq[option]) {
      case CURL_EMPTY:
        tn->himq[option] = CURL_OPPOSITE;
        break;
      case CURL_OPPOSITE:
        break;
      }
      break;
    }
  }
}

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

#define CURL_NO          0

#define CURL_YES         1

#define CURL_WANTYES     2

#define CURL_WANTNO      3

#define CURL_EMPTY       0

#define CURL_OPPOSITE    1

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}

const struct Curl_handler Curl_handler_telnet = {
  "TELNET",                             /* scheme */
  ZERO_NULL,                            /* setup_connection */
  telnet_do,                            /* do_it */
  telnet_done,                          /* done */
  ZERO_NULL,                            /* do_more */
  ZERO_NULL,                            /* connect_it */
  ZERO_NULL,                            /* connecting */
  ZERO_NULL,                            /* doing */
  ZERO_NULL,                            /* proto_getsock */
  ZERO_NULL,                            /* doing_getsock */
  ZERO_NULL,                            /* domore_getsock */
  ZERO_NULL,                            /* perform_getsock */
  ZERO_NULL,                            /* disconnect */
  ZERO_NULL,                            /* readwrite */
  ZERO_NULL,                            /* connection_check */
  ZERO_NULL,                            /* attach connection */
  PORT_TELNET,                          /* defport */
  CURLPROTO_TELNET,                     /* protocol */
  CURLPROTO_TELNET,                     /* family */
  PROTOPT_NONE | PROTOPT_NOURLQUERY     /* flags */
};


void rec_will(struct Curl_easy *data, int option)
{
  struct TELNET *tn = data->req.p.telnet;
  switch(tn->him[option]) {
  case CURL_NO:
    if(tn->him_preferred[option] == CURL_YES) {
      tn->him[option] = CURL_YES;
      send_negotiation(data, CURL_DO, option);
    }
    else
      send_negotiation(data, CURL_DONT, option);

    break;

  case CURL_YES:
    /* Already enabled */
    break;

  case CURL_WANTNO:
    switch(tn->himq[option]) {
    case CURL_EMPTY:
      /* Error: DONT answered by WILL */
      tn->him[option] = CURL_NO;
      break;
    case CURL_OPPOSITE:
      /* Error: DONT answered by WILL */
      tn->him[option] = CURL_YES;
      tn->himq[option] = CURL_EMPTY;
      break;
    }
    break;

  case CURL_WANTYES:
    switch(tn->himq[option]) {
    case CURL_EMPTY:
      tn->him[option] = CURL_YES;
      break;
    case CURL_OPPOSITE:
      tn->him[option] = CURL_WANTNO;
      tn->himq[option] = CURL_EMPTY;
      send_negotiation(data, CURL_DONT, option);
      break;
    }
    break;
  }
}


static CURLcode telnet_done(struct Curl_easy *data,
                            CURLcode status, bool premature)
{
  struct TELNET *tn = data->req.p.telnet;
  (void)status; /* unused */
  (void)premature; /* not used */

  if(!tn)
    return CURLE_OK;

  curl_slist_free_all(tn->telnet_vars);
  tn->telnet_vars = NULL;

  Curl_safefree(data->req.p.telnet);

  return CURLE_OK;
}

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}

const struct Curl_handler Curl_handler_telnet = {
  "TELNET",                             /* scheme */
  ZERO_NULL,                            /* setup_connection */
  telnet_do,                            /* do_it */
  telnet_done,                          /* done */
  ZERO_NULL,                            /* do_more */
  ZERO_NULL,                            /* connect_it */
  ZERO_NULL,                            /* connecting */
  ZERO_NULL,                            /* doing */
  ZERO_NULL,                            /* proto_getsock */
  ZERO_NULL,                            /* doing_getsock */
  ZERO_NULL,                            /* domore_getsock */
  ZERO_NULL,                            /* perform_getsock */
  ZERO_NULL,                            /* disconnect */
  ZERO_NULL,                            /* readwrite */
  ZERO_NULL,                            /* connection_check */
  ZERO_NULL,                            /* attach connection */
  PORT_TELNET,                          /* defport */
  CURLPROTO_TELNET,                     /* protocol */
  CURLPROTO_TELNET,                     /* family */
  PROTOPT_NONE | PROTOPT_NOURLQUERY     /* flags */
};


static void suboption(struct Curl_easy *data)
{
  struct curl_slist *v;
  unsigned char temp[2048];
  ssize_t bytes_written;
  size_t len;
  int err;
  char varname[128] = "";
  char varval[128] = "";
  struct TELNET *tn = data->req.p.telnet;
  struct connectdata *conn = data->conn;

  printsub(data, '<', (unsigned char *)tn->subbuffer, CURL_SB_LEN(tn) + 2);
  switch(CURL_SB_GET(tn)) {
    case CURL_TELOPT_TTYPE:
      len = strlen(tn->subopt_ttype) + 4 + 2;
      msnprintf((char *)temp, sizeof(temp),
                "%c%c%c%c%s%c%c", CURL_IAC, CURL_SB, CURL_TELOPT_TTYPE,
                CURL_TELQUAL_IS, tn->subopt_ttype, CURL_IAC, CURL_SE);
      bytes_written = swrite(conn->sock[FIRSTSOCKET], temp, len);
      if(bytes_written < 0) {
        err = SOCKERRNO;
        failf(data,"Sending data failed (%d)",err);
      }
      printsub(data, '>', &temp[2], len-2);
      break;
    case CURL_TELOPT_XDISPLOC:
      len = strlen(tn->subopt_xdisploc) + 4 + 2;
      msnprintf((char *)temp, sizeof(temp),
                "%c%c%c%c%s%c%c", CURL_IAC, CURL_SB, CURL_TELOPT_XDISPLOC,
                CURL_TELQUAL_IS, tn->subopt_xdisploc, CURL_IAC, CURL_SE);
      bytes_written = swrite(conn->sock[FIRSTSOCKET], temp, len);
      if(bytes_written < 0) {
        err = SOCKERRNO;
        failf(data,"Sending data failed (%d)",err);
      }
      printsub(data, '>', &temp[2], len-2);
      break;
    case CURL_TELOPT_NEW_ENVIRON:
      msnprintf((char *)temp, sizeof(temp),
                "%c%c%c%c", CURL_IAC, CURL_SB, CURL_TELOPT_NEW_ENVIRON,
                CURL_TELQUAL_IS);
      len = 4;

      for(v = tn->telnet_vars; v; v = v->next) {
        size_t tmplen = (strlen(v->data) + 1);
        /* Add the variable only if it fits */
        if(len + tmplen < (int)sizeof(temp)-6) {
          if(sscanf(v->data, "%127[^,],%127s", varname, varval) == 2) {
            msnprintf((char *)&temp[len], sizeof(temp) - len,
                      "%c%s%c%s", CURL_NEW_ENV_VAR, varname,
                      CURL_NEW_ENV_VALUE, varval);
            len += tmplen;
          }
        }
      }
      msnprintf((char *)&temp[len], sizeof(temp) - len,
                "%c%c", CURL_IAC, CURL_SE);
      len += 2;
      bytes_written = swrite(conn->sock[FIRSTSOCKET], temp, len);
      if(bytes_written < 0) {
        err = SOCKERRNO;
        failf(data,"Sending data failed (%d)",err);
      }
      printsub(data, '>', &temp[2], len-2);
      break;
  }
  return;
}

#define CURL_SB_CLEAR(x)  x->subpointer = x->subbuffer

#define CURL_SB_ACCUM(x,c)                                      \
  do {                                                          \
    if(x->subpointer < (x->subbuffer + sizeof(x->subbuffer)))   \
      *x->subpointer++ = (c);                                   \
  } while(0)

#define  CURL_SB_GET(x) ((*x->subpointer++)&0xff)

#define  CURL_SB_LEN(x) (x->subend - x->subpointer)

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}


static void negotiate(struct Curl_easy *data)
{
  int i;
  struct TELNET *tn = data->req.p.telnet;

  for(i = 0; i < CURL_NTELOPTS; i++) {
    if(i == CURL_TELOPT_ECHO)
      continue;

    if(tn->us_preferred[i] == CURL_YES)
      set_local_option(data, i, CURL_YES);

    if(tn->him_preferred[i] == CURL_YES)
      set_remote_option(data, i, CURL_YES);
  }
}

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

#define CURL_YES         1

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}

const struct Curl_handler Curl_handler_telnet = {
  "TELNET",                             /* scheme */
  ZERO_NULL,                            /* setup_connection */
  telnet_do,                            /* do_it */
  telnet_done,                          /* done */
  ZERO_NULL,                            /* do_more */
  ZERO_NULL,                            /* connect_it */
  ZERO_NULL,                            /* connecting */
  ZERO_NULL,                            /* doing */
  ZERO_NULL,                            /* proto_getsock */
  ZERO_NULL,                            /* doing_getsock */
  ZERO_NULL,                            /* domore_getsock */
  ZERO_NULL,                            /* perform_getsock */
  ZERO_NULL,                            /* disconnect */
  ZERO_NULL,                            /* readwrite */
  ZERO_NULL,                            /* connection_check */
  ZERO_NULL,                            /* attach connection */
  PORT_TELNET,                          /* defport */
  CURLPROTO_TELNET,                     /* protocol */
  CURLPROTO_TELNET,                     /* family */
  PROTOPT_NONE | PROTOPT_NOURLQUERY     /* flags */
};


set_local_option(struct Curl_easy *data, int option, int newstate)
{
  struct TELNET *tn = data->req.p.telnet;
  if(newstate == CURL_YES) {
    switch(tn->us[option]) {
    case CURL_NO:
      tn->us[option] = CURL_WANTYES;
      send_negotiation(data, CURL_WILL, option);
      break;

    case CURL_YES:
      /* Already enabled */
      break;

    case CURL_WANTNO:
      switch(tn->usq[option]) {
      case CURL_EMPTY:
        /* Already negotiating for CURL_YES, queue the request */
        tn->usq[option] = CURL_OPPOSITE;
        break;
      case CURL_OPPOSITE:
        /* Error: already queued an enable request */
        break;
      }
      break;

    case CURL_WANTYES:
      switch(tn->usq[option]) {
      case CURL_EMPTY:
        /* Error: already negotiating for enable */
        break;
      case CURL_OPPOSITE:
        tn->usq[option] = CURL_EMPTY;
        break;
      }
      break;
    }
  }
  else { /* NO */
    switch(tn->us[option]) {
    case CURL_NO:
      /* Already disabled */
      break;

    case CURL_YES:
      tn->us[option] = CURL_WANTNO;
      send_negotiation(data, CURL_WONT, option);
      break;

    case CURL_WANTNO:
      switch(tn->usq[option]) {
      case CURL_EMPTY:
        /* Already negotiating for NO */
        break;
      case CURL_OPPOSITE:
        tn->usq[option] = CURL_EMPTY;
        break;
      }
      break;

    case CURL_WANTYES:
      switch(tn->usq[option]) {
      case CURL_EMPTY:
        tn->usq[option] = CURL_OPPOSITE;
        break;
      case CURL_OPPOSITE:
        break;
      }
      break;
    }
  }
}


static void sendsuboption(struct Curl_easy *data, int option)
{
  ssize_t bytes_written;
  int err;
  unsigned short x, y;
  unsigned char *uc1, *uc2;
  struct TELNET *tn = data->req.p.telnet;
  struct connectdata *conn = data->conn;

  switch(option) {
  case CURL_TELOPT_NAWS:
    /* We prepare data to be sent */
    CURL_SB_CLEAR(tn);
    CURL_SB_ACCUM(tn, CURL_IAC);
    CURL_SB_ACCUM(tn, CURL_SB);
    CURL_SB_ACCUM(tn, CURL_TELOPT_NAWS);
    /* We must deal either with little or big endian processors */
    /* Window size must be sent according to the 'network order' */
    x = htons(tn->subopt_wsx);
    y = htons(tn->subopt_wsy);
    uc1 = (unsigned char *)&x;
    uc2 = (unsigned char *)&y;
    CURL_SB_ACCUM(tn, uc1[0]);
    CURL_SB_ACCUM(tn, uc1[1]);
    CURL_SB_ACCUM(tn, uc2[0]);
    CURL_SB_ACCUM(tn, uc2[1]);

    CURL_SB_ACCUM(tn, CURL_IAC);
    CURL_SB_ACCUM(tn, CURL_SE);
    CURL_SB_TERM(tn);
    /* data suboption is now ready */

    printsub(data, '>', (unsigned char *)tn->subbuffer + 2,
             CURL_SB_LEN(tn)-2);

    /* we send the header of the suboption... */
    bytes_written = swrite(conn->sock[FIRSTSOCKET], tn->subbuffer, 3);
    if(bytes_written < 0) {
      err = SOCKERRNO;
      failf(data, "Sending data failed (%d)", err);
    }
    /* ... then the window size with the send_telnet_data() function
       to deal with 0xFF cases ... */
    send_telnet_data(data, (char *)tn->subbuffer + 3, 4);
    /* ... and the footer */
    bytes_written = swrite(conn->sock[FIRSTSOCKET], tn->subbuffer + 7, 2);
    if(bytes_written < 0) {
      err = SOCKERRNO;
      failf(data, "Sending data failed (%d)", err);
    }
    break;
  }
}

#define CURL_SB_CLEAR(x)  x->subpointer = x->subbuffer

#define CURL_SB_TERM(x)                                 \
  do {                                                  \
    x->subend = x->subpointer;                          \
    CURL_SB_CLEAR(x);                                   \
  } while(0)

#define CURL_SB_ACCUM(x,c)                                      \
  do {                                                          \
    if(x->subpointer < (x->subbuffer + sizeof(x->subbuffer)))   \
      *x->subpointer++ = (c);                                   \
  } while(0)

#define  CURL_SB_LEN(x) (x->subend - x->subpointer)

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}


static CURLcode check_telnet_options(struct Curl_easy *data)
{
  struct curl_slist *head;
  struct curl_slist *beg;
  char option_keyword[128] = "";
  char option_arg[256] = "";
  struct TELNET *tn = data->req.p.telnet;
  struct connectdata *conn = data->conn;
  CURLcode result = CURLE_OK;
  int binary_option;

  /* Add the user name as an environment variable if it
     was given on the command line */
  if(conn->bits.user_passwd) {
    msnprintf(option_arg, sizeof(option_arg), "USER,%s", conn->user);
    beg = curl_slist_append(tn->telnet_vars, option_arg);
    if(!beg) {
      curl_slist_free_all(tn->telnet_vars);
      tn->telnet_vars = NULL;
      return CURLE_OUT_OF_MEMORY;
    }
    tn->telnet_vars = beg;
    tn->us_preferred[CURL_TELOPT_NEW_ENVIRON] = CURL_YES;
  }

  for(head = data->set.telnet_options; head; head = head->next) {
    if(sscanf(head->data, "%127[^= ]%*[ =]%255s",
              option_keyword, option_arg) == 2) {

      /* Terminal type */
      if(strcasecompare(option_keyword, "TTYPE")) {
        strncpy(tn->subopt_ttype, option_arg, 31);
        tn->subopt_ttype[31] = 0; /* String termination */
        tn->us_preferred[CURL_TELOPT_TTYPE] = CURL_YES;
        continue;
      }

      /* Display variable */
      if(strcasecompare(option_keyword, "XDISPLOC")) {
        strncpy(tn->subopt_xdisploc, option_arg, 127);
        tn->subopt_xdisploc[127] = 0; /* String termination */
        tn->us_preferred[CURL_TELOPT_XDISPLOC] = CURL_YES;
        continue;
      }

      /* Environment variable */
      if(strcasecompare(option_keyword, "NEW_ENV")) {
        beg = curl_slist_append(tn->telnet_vars, option_arg);
        if(!beg) {
          result = CURLE_OUT_OF_MEMORY;
          break;
        }
        tn->telnet_vars = beg;
        tn->us_preferred[CURL_TELOPT_NEW_ENVIRON] = CURL_YES;
        continue;
      }

      /* Window Size */
      if(strcasecompare(option_keyword, "WS")) {
        if(sscanf(option_arg, "%hu%*[xX]%hu",
                  &tn->subopt_wsx, &tn->subopt_wsy) == 2)
          tn->us_preferred[CURL_TELOPT_NAWS] = CURL_YES;
        else {
          failf(data, "Syntax error in telnet option: %s", head->data);
          result = CURLE_TELNET_OPTION_SYNTAX;
          break;
        }
        continue;
      }

      /* To take care or not of the 8th bit in data exchange */
      if(strcasecompare(option_keyword, "BINARY")) {
        binary_option = atoi(option_arg);
        if(binary_option != 1) {
          tn->us_preferred[CURL_TELOPT_BINARY] = CURL_NO;
          tn->him_preferred[CURL_TELOPT_BINARY] = CURL_NO;
        }
        continue;
      }

      failf(data, "Unknown telnet option %s", head->data);
      result = CURLE_UNKNOWN_OPTION;
      break;
    }
    failf(data, "Syntax error in telnet option: %s", head->data);
    result = CURLE_TELNET_OPTION_SYNTAX;
    break;
  }

  if(result) {
    curl_slist_free_all(tn->telnet_vars);
    tn->telnet_vars = NULL;
  }

  return result;
}

#define CURL_SB_ACCUM(x,c)                                      \
  do {                                                          \
    if(x->subpointer < (x->subbuffer + sizeof(x->subbuffer)))   \
      *x->subpointer++ = (c);                                   \
  } while(0)

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

#define CURL_NO          0

#define CURL_YES         1

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}

const struct Curl_handler Curl_handler_telnet = {
  "TELNET",                             /* scheme */
  ZERO_NULL,                            /* setup_connection */
  telnet_do,                            /* do_it */
  telnet_done,                          /* done */
  ZERO_NULL,                            /* do_more */
  ZERO_NULL,                            /* connect_it */
  ZERO_NULL,                            /* connecting */
  ZERO_NULL,                            /* doing */
  ZERO_NULL,                            /* proto_getsock */
  ZERO_NULL,                            /* doing_getsock */
  ZERO_NULL,                            /* domore_getsock */
  ZERO_NULL,                            /* perform_getsock */
  ZERO_NULL,                            /* disconnect */
  ZERO_NULL,                            /* readwrite */
  ZERO_NULL,                            /* connection_check */
  ZERO_NULL,                            /* attach connection */
  PORT_TELNET,                          /* defport */
  CURLPROTO_TELNET,                     /* protocol */
  CURLPROTO_TELNET,                     /* family */
  PROTOPT_NONE | PROTOPT_NOURLQUERY     /* flags */
};


CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count)              /* Number of bytes received */
{
  unsigned char c;
  CURLcode result;
  int in = 0;
  int startwrite = -1;
  struct TELNET *tn = data->req.p.telnet;

#define startskipping()                                       \
  if(startwrite >= 0) {                                       \
    result = Curl_client_write(data,                          \
                               CLIENTWRITE_BODY,              \
                               (char *)&inbuf[startwrite],    \
                               in-startwrite);                \
    if(result)                                                \
      return result;                                          \
  }                                                           \
  startwrite = -1

#define writebyte() \
    if(startwrite < 0) \
      startwrite = in

#define bufferflush() startskipping()

  while(count--) {
    c = inbuf[in];

    switch(tn->telrcv_state) {
    case CURL_TS_CR:
      tn->telrcv_state = CURL_TS_DATA;
      if(c == '\0') {
        startskipping();
        break;   /* Ignore \0 after CR */
      }
      writebyte();
      break;

    case CURL_TS_DATA:
      if(c == CURL_IAC) {
        tn->telrcv_state = CURL_TS_IAC;
        startskipping();
        break;
      }
      else if(c == '\r')
        tn->telrcv_state = CURL_TS_CR;
      writebyte();
      break;

    case CURL_TS_IAC:
    process_iac:
      DEBUGASSERT(startwrite < 0);
      switch(c) {
      case CURL_WILL:
        tn->telrcv_state = CURL_TS_WILL;
        break;
      case CURL_WONT:
        tn->telrcv_state = CURL_TS_WONT;
        break;
      case CURL_DO:
        tn->telrcv_state = CURL_TS_DO;
        break;
      case CURL_DONT:
        tn->telrcv_state = CURL_TS_DONT;
        break;
      case CURL_SB:
        CURL_SB_CLEAR(tn);
        tn->telrcv_state = CURL_TS_SB;
        break;
      case CURL_IAC:
        tn->telrcv_state = CURL_TS_DATA;
        writebyte();
        break;
      case CURL_DM:
      case CURL_NOP:
      case CURL_GA:
      default:
        tn->telrcv_state = CURL_TS_DATA;
        printoption(data, "RCVD", CURL_IAC, c);
        break;
      }
      break;

      case CURL_TS_WILL:
        printoption(data, "RCVD", CURL_WILL, c);
        tn->please_negotiate = 1;
        rec_will(data, c);
        tn->telrcv_state = CURL_TS_DATA;
        break;

      case CURL_TS_WONT:
        printoption(data, "RCVD", CURL_WONT, c);
        tn->please_negotiate = 1;
        rec_wont(data, c);
        tn->telrcv_state = CURL_TS_DATA;
        break;

      case CURL_TS_DO:
        printoption(data, "RCVD", CURL_DO, c);
        tn->please_negotiate = 1;
        rec_do(data, c);
        tn->telrcv_state = CURL_TS_DATA;
        break;

      case CURL_TS_DONT:
        printoption(data, "RCVD", CURL_DONT, c);
        tn->please_negotiate = 1;
        rec_dont(data, c);
        tn->telrcv_state = CURL_TS_DATA;
        break;

      case CURL_TS_SB:
        if(c == CURL_IAC)
          tn->telrcv_state = CURL_TS_SE;
        else
          CURL_SB_ACCUM(tn, c);
        break;

      case CURL_TS_SE:
        if(c != CURL_SE) {
          if(c != CURL_IAC) {
            /*
             * This is an error.  We only expect to get "IAC IAC" or "IAC SE".
             * Several things may have happened.  An IAC was not doubled, the
             * IAC SE was left off, or another option got inserted into the
             * suboption are all possibilities.  If we assume that the IAC was
             * not doubled, and really the IAC SE was left off, we could get
             * into an infinite loop here.  So, instead, we terminate the
             * suboption, and process the partial suboption if we can.
             */
            CURL_SB_ACCUM(tn, CURL_IAC);
            CURL_SB_ACCUM(tn, c);
            tn->subpointer -= 2;
            CURL_SB_TERM(tn);

            printoption(data, "In SUBOPTION processing, RCVD", CURL_IAC, c);
            suboption(data);   /* handle sub-option */
            tn->telrcv_state = CURL_TS_IAC;
            goto process_iac;
          }
          CURL_SB_ACCUM(tn, c);
          tn->telrcv_state = CURL_TS_SB;
        }
        else {
          CURL_SB_ACCUM(tn, CURL_IAC);
          CURL_SB_ACCUM(tn, CURL_SE);
          tn->subpointer -= 2;
          CURL_SB_TERM(tn);
          suboption(data);   /* handle sub-option */
          tn->telrcv_state = CURL_TS_DATA;
        }
        break;
    }
    ++in;
  }
  bufferflush();
  return CURLE_OK;
}

#define SUBBUFSIZE 512

#define CURL_SB_CLEAR(x)  x->subpointer = x->subbuffer

#define CURL_SB_TERM(x)                                 \
  do {                                                  \
    x->subend = x->subpointer;                          \
    CURL_SB_CLEAR(x);                                   \
  } while(0)

#define CURL_SB_ACCUM(x,c)                                      \
  do {                                                          \
    if(x->subpointer < (x->subbuffer + sizeof(x->subbuffer)))   \
      *x->subpointer++ = (c);                                   \
  } while(0)

#define  CURL_SB_GET(x) ((*x->subpointer++)&0xff)

#define  CURL_SB_LEN(x) (x->subend - x->subpointer)

#define printoption(a,b,c,d)  Curl_nop_stmt

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

#define CURL_NO          0

#define CURL_YES         1

#define CURL_WANTYES     2

#define CURL_WANTNO      3

#define CURL_EMPTY       0

#define CURL_OPPOSITE    1

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}


static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread)
{
  ssize_t escapes, i, outlen;
  unsigned char *outbuf = NULL;
  CURLcode result = CURLE_OK;
  ssize_t bytes_written, total_written;
  struct connectdata *conn = data->conn;

  /* Determine size of new buffer after escaping */
  escapes = 0;
  for(i = 0; i < nread; i++)
    if((unsigned char)buffer[i] == CURL_IAC)
      escapes++;
  outlen = nread + escapes;

  if(outlen == nread)
    outbuf = (unsigned char *)buffer;
  else {
    ssize_t j;
    outbuf = malloc(nread + escapes + 1);
    if(!outbuf)
      return CURLE_OUT_OF_MEMORY;

    j = 0;
    for(i = 0; i < nread; i++) {
      outbuf[j++] = buffer[i];
      if((unsigned char)buffer[i] == CURL_IAC)
        outbuf[j++] = CURL_IAC;
    }
    outbuf[j] = '\0';
  }

  total_written = 0;
  while(!result && total_written < outlen) {
    /* Make sure socket is writable to avoid EWOULDBLOCK condition */
    struct pollfd pfd[1];
    pfd[0].fd = conn->sock[FIRSTSOCKET];
    pfd[0].events = POLLOUT;
    switch(Curl_poll(pfd, 1, -1)) {
      case -1:                    /* error, abort writing */
      case 0:                     /* timeout (will never happen) */
        result = CURLE_SEND_ERROR;
        break;
      default:                    /* write! */
        bytes_written = 0;
        result = Curl_write(data, conn->sock[FIRSTSOCKET],
                            outbuf + total_written,
                            outlen - total_written,
                            &bytes_written);
        total_written += bytes_written;
        break;
    }
  }

  /* Free malloc copy if escaped */
  if(outbuf != (unsigned char *)buffer)
    free(outbuf);

  return result;
}


void rec_dont(struct Curl_easy *data, int option)
{
  struct TELNET *tn = data->req.p.telnet;
  switch(tn->us[option]) {
  case CURL_NO:
    /* Already disabled */
    break;

  case CURL_YES:
    tn->us[option] = CURL_NO;
    send_negotiation(data, CURL_WONT, option);
    break;

  case CURL_WANTNO:
    switch(tn->usq[option]) {
    case CURL_EMPTY:
      tn->us[option] = CURL_NO;
      break;

    case CURL_OPPOSITE:
      tn->us[option] = CURL_WANTYES;
      tn->usq[option] = CURL_EMPTY;
      send_negotiation(data, CURL_WILL, option);
      break;
    }
    break;

  case CURL_WANTYES:
    switch(tn->usq[option]) {
    case CURL_EMPTY:
      tn->us[option] = CURL_NO;
      break;
    case CURL_OPPOSITE:
      tn->us[option] = CURL_NO;
      tn->usq[option] = CURL_EMPTY;
      break;
    }
    break;
  }
}


static void send_negotiation(struct Curl_easy *data, int cmd, int option)
{
  unsigned char buf[3];
  ssize_t bytes_written;
  struct connectdata *conn = data->conn;

  buf[0] = CURL_IAC;
  buf[1] = (unsigned char)cmd;
  buf[2] = (unsigned char)option;

  bytes_written = swrite(conn->sock[FIRSTSOCKET], buf, 3);
  if(bytes_written < 0) {
    int err = SOCKERRNO;
    failf(data,"Sending data failed (%d)",err);
  }

  printoption(data, "SENT", cmd, option);
}


CURLcode init_telnet(struct Curl_easy *data)
{
  struct TELNET *tn;

  tn = calloc(1, sizeof(struct TELNET));
  if(!tn)
    return CURLE_OUT_OF_MEMORY;

  data->req.p.telnet = tn; /* make us known */

  tn->telrcv_state = CURL_TS_DATA;

  /* Init suboptions */
  CURL_SB_CLEAR(tn);

  /* Set the options we want by default */
  tn->us_preferred[CURL_TELOPT_SGA] = CURL_YES;
  tn->him_preferred[CURL_TELOPT_SGA] = CURL_YES;

  /* To be compliant with previous releases of libcurl
     we enable this option by default. This behavior
         can be changed thanks to the "BINARY" option in
         CURLOPT_TELNETOPTIONS
  */
  tn->us_preferred[CURL_TELOPT_BINARY] = CURL_YES;
  tn->him_preferred[CURL_TELOPT_BINARY] = CURL_YES;

  /* We must allow the server to echo what we sent
         but it is not necessary to request the server
         to do so (it might forces the server to close
         the connection). Hence, we ignore ECHO in the
         negotiate function
  */
  tn->him_preferred[CURL_TELOPT_ECHO] = CURL_YES;

  /* Set the subnegotiation fields to send information
    just after negotiation passed (do/will)

     Default values are (0,0) initialized by calloc.
     According to the RFC1013 it is valid:
     A value equal to zero is acceptable for the width (or height),
         and means that no character width (or height) is being sent.
         In this case, the width (or height) that will be assumed by the
         Telnet server is operating system specific (it will probably be
         based upon the terminal type information that may have been sent
         using the TERMINAL TYPE Telnet option). */
  tn->subnegotiation[CURL_TELOPT_NAWS] = CURL_YES;
  return CURLE_OK;
}

#define CURL_SB_CLEAR(x)  x->subpointer = x->subbuffer

#define CURL_SB_TERM(x)                                 \
  do {                                                  \
    x->subend = x->subpointer;                          \
    CURL_SB_CLEAR(x);                                   \
  } while(0)

#define CURL_SB_ACCUM(x,c)                                      \
  do {                                                          \
    if(x->subpointer < (x->subbuffer + sizeof(x->subbuffer)))   \
      *x->subpointer++ = (c);                                   \
  } while(0)

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

#define CURL_YES         1

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}


void rec_do(struct Curl_easy *data, int option)
{
  struct TELNET *tn = data->req.p.telnet;
  switch(tn->us[option]) {
  case CURL_NO:
    if(tn->us_preferred[option] == CURL_YES) {
      tn->us[option] = CURL_YES;
      send_negotiation(data, CURL_WILL, option);
      if(tn->subnegotiation[option] == CURL_YES)
        /* transmission of data option */
        sendsuboption(data, option);
    }
    else if(tn->subnegotiation[option] == CURL_YES) {
      /* send information to achieve this option*/
      tn->us[option] = CURL_YES;
      send_negotiation(data, CURL_WILL, option);
      sendsuboption(data, option);
    }
    else
      send_negotiation(data, CURL_WONT, option);
    break;

  case CURL_YES:
    /* Already enabled */
    break;

  case CURL_WANTNO:
    switch(tn->usq[option]) {
    case CURL_EMPTY:
      /* Error: DONT answered by WILL */
      tn->us[option] = CURL_NO;
      break;
    case CURL_OPPOSITE:
      /* Error: DONT answered by WILL */
      tn->us[option] = CURL_YES;
      tn->usq[option] = CURL_EMPTY;
      break;
    }
    break;

  case CURL_WANTYES:
    switch(tn->usq[option]) {
    case CURL_EMPTY:
      tn->us[option] = CURL_YES;
      if(tn->subnegotiation[option] == CURL_YES) {
        /* transmission of data option */
        sendsuboption(data, option);
      }
      break;
    case CURL_OPPOSITE:
      tn->us[option] = CURL_WANTNO;
      tn->himq[option] = CURL_EMPTY;
      send_negotiation(data, CURL_WONT, option);
      break;
    }
    break;
  }
}


static void printsub(struct Curl_easy *data,
                     int direction,             /* '<' or '>' */
                     unsigned char *pointer,    /* where suboption data is */
                     size_t length)             /* length of suboption data */
{
  if(data->set.verbose) {
    unsigned int i = 0;
    if(direction) {
      infof(data, "%s IAC SB ", (direction == '<')? "RCVD":"SENT");
      if(length >= 3) {
        int j;

        i = pointer[length-2];
        j = pointer[length-1];

        if(i != CURL_IAC || j != CURL_SE) {
          infof(data, "(terminated by ");
          if(CURL_TELOPT_OK(i))
            infof(data, "%s ", CURL_TELOPT(i));
          else if(CURL_TELCMD_OK(i))
            infof(data, "%s ", CURL_TELCMD(i));
          else
            infof(data, "%u ", i);
          if(CURL_TELOPT_OK(j))
            infof(data, "%s", CURL_TELOPT(j));
          else if(CURL_TELCMD_OK(j))
            infof(data, "%s", CURL_TELCMD(j));
          else
            infof(data, "%d", j);
          infof(data, ", not IAC SE!) ");
        }
      }
      length -= 2;
    }
    if(length < 1) {
      infof(data, "(Empty suboption?)");
      return;
    }

    if(CURL_TELOPT_OK(pointer[0])) {
      switch(pointer[0]) {
      case CURL_TELOPT_TTYPE:
      case CURL_TELOPT_XDISPLOC:
      case CURL_TELOPT_NEW_ENVIRON:
      case CURL_TELOPT_NAWS:
        infof(data, "%s", CURL_TELOPT(pointer[0]));
        break;
      default:
        infof(data, "%s (unsupported)", CURL_TELOPT(pointer[0]));
        break;
      }
    }
    else
      infof(data, "%d (unknown)", pointer[i]);

    switch(pointer[0]) {
    case CURL_TELOPT_NAWS:
      if(length > 4)
        infof(data, "Width: %d ; Height: %d", (pointer[1]<<8) | pointer[2],
              (pointer[3]<<8) | pointer[4]);
      break;
    default:
      switch(pointer[1]) {
      case CURL_TELQUAL_IS:
        infof(data, " IS");
        break;
      case CURL_TELQUAL_SEND:
        infof(data, " SEND");
        break;
      case CURL_TELQUAL_INFO:
        infof(data, " INFO/REPLY");
        break;
      case CURL_TELQUAL_NAME:
        infof(data, " NAME");
        break;
      }

      switch(pointer[0]) {
      case CURL_TELOPT_TTYPE:
      case CURL_TELOPT_XDISPLOC:
        pointer[length] = 0;
        infof(data, " \"%s\"", &pointer[2]);
        break;
      case CURL_TELOPT_NEW_ENVIRON:
        if(pointer[1] == CURL_TELQUAL_IS) {
          infof(data, " ");
          for(i = 3; i < length; i++) {
            switch(pointer[i]) {
            case CURL_NEW_ENV_VAR:
              infof(data, ", ");
              break;
            case CURL_NEW_ENV_VALUE:
              infof(data, " = ");
              break;
            default:
              infof(data, "%c", pointer[i]);
              break;
            }
          }
        }
        break;
      default:
        for(i = 2; i < length; i++)
          infof(data, " %.2x", pointer[i]);
        break;
      }
    }
    if(direction)
      infof(data, "\n");
  }
}

static
CURLcode telrcv(struct Curl_easy *data,
                const unsigned char *inbuf, /* Data received from socket */
                ssize_t count);

static void printoption(struct Curl_easy *data,
                        const char *direction,
                        int cmd, int option);

static void negotiate(struct Curl_easy *data);

static void send_negotiation(struct Curl_easy *data, int cmd, int option);

static void set_local_option(struct Curl_easy *data,
                             int option, int newstate);

static void set_remote_option(struct Curl_easy *data,
                              int option, int newstate);

static void printsub(struct Curl_easy *data,
                     int direction, unsigned char *pointer,
                     size_t length);

static void suboption(struct Curl_easy *data);

static void sendsuboption(struct Curl_easy *data, int option);

static CURLcode telnet_do(struct Curl_easy *data, bool *done);

static CURLcode telnet_done(struct Curl_easy *data,
                                 CURLcode, bool premature);

static CURLcode send_telnet_data(struct Curl_easy *data,
                                 char *buffer, ssize_t nread);

typedef enum
{
   CURL_TS_DATA = 0,
   CURL_TS_IAC,
   CURL_TS_WILL,
   CURL_TS_WONT,
   CURL_TS_DO,
   CURL_TS_DONT,
   CURL_TS_CR,
   CURL_TS_SB,   /* sub-option collection */
   CURL_TS_SE   /* looking for sub-option end */
} TelnetReceive;

struct TELNET {
  int please_negotiate;
  int already_negotiated;
  int us[256];
  int usq[256];
  int us_preferred[256];
  int him[256];
  int himq[256];
  int him_preferred[256];
  int subnegotiation[256];
  char subopt_ttype[32];             /* Set with suboption TTYPE */
  char subopt_xdisploc[128];         /* Set with suboption XDISPLOC */
  unsigned short subopt_wsx;         /* Set with suboption NAWS */
  unsigned short subopt_wsy;         /* Set with suboption NAWS */
  TelnetReceive telrcv_state;
  struct curl_slist *telnet_vars;    /* Environment variables */

  /* suboptions */
  unsigned char subbuffer[SUBBUFSIZE];
  unsigned char *subpointer, *subend;      /* buffer for sub-options */
}

const struct Curl_handler Curl_handler_telnet = {
  "TELNET",                             /* scheme */
  ZERO_NULL,                            /* setup_connection */
  telnet_do,                            /* do_it */
  telnet_done,                          /* done */
  ZERO_NULL,                            /* do_more */
  ZERO_NULL,                            /* connect_it */
  ZERO_NULL,                            /* connecting */
  ZERO_NULL,                            /* doing */
  ZERO_NULL,                            /* proto_getsock */
  ZERO_NULL,                            /* doing_getsock */
  ZERO_NULL,                            /* domore_getsock */
  ZERO_NULL,                            /* perform_getsock */
  ZERO_NULL,                            /* disconnect */
  ZERO_NULL,                            /* readwrite */
  ZERO_NULL,                            /* connection_check */
  ZERO_NULL,                            /* attach connection */
  PORT_TELNET,                          /* defport */
  CURLPROTO_TELNET,                     /* protocol */
  CURLPROTO_TELNET,                     /* family */
  PROTOPT_NONE | PROTOPT_NOURLQUERY     /* flags */
};


void rec_wont(struct Curl_easy *data, int option)
{
  struct TELNET *tn = data->req.p.telnet;
  switch(tn->him[option]) {
  case CURL_NO:
    /* Already disabled */
    break;

  case CURL_YES:
    tn->him[option] = CURL_NO;
    send_negotiation(data, CURL_DONT, option);
    break;

  case CURL_WANTNO:
    switch(tn->himq[option]) {
    case CURL_EMPTY:
      tn->him[option] = CURL_NO;
      break;

    case CURL_OPPOSITE:
      tn->him[option] = CURL_WANTYES;
      tn->himq[option] = CURL_EMPTY;
      send_negotiation(data, CURL_DO, option);
      break;
    }
    break;

  case CURL_WANTYES:
    switch(tn->himq[option]) {
    case CURL_EMPTY:
      tn->him[option] = CURL_NO;
      break;
    case CURL_OPPOSITE:
      tn->him[option] = CURL_NO;
      tn->himq[option] = CURL_EMPTY;
      break;
    }
    break;
  }
}


static int amd_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
				unsigned long *configs, unsigned num_configs)
{
	int i;
	u32 arg;
	int ret = 0;
	u32 pin_reg;
	unsigned long flags;
	enum pin_config_param param;
	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctldev);

	spin_lock_irqsave(&gpio_dev->lock, flags);
	for (i = 0; i < num_configs; i++) {
		param = pinconf_to_config_param(configs[i]);
		arg = pinconf_to_config_argument(configs[i]);
		pin_reg = readl(gpio_dev->base + pin*4);

		switch (param) {
		case PIN_CONFIG_INPUT_DEBOUNCE:
			pin_reg &= ~DB_TMR_OUT_MASK;
			pin_reg |= arg & DB_TMR_OUT_MASK;
			break;

		case PIN_CONFIG_BIAS_PULL_DOWN:
			pin_reg &= ~BIT(PULL_DOWN_ENABLE_OFF);
			pin_reg |= (arg & BIT(0)) << PULL_DOWN_ENABLE_OFF;
			break;

		case PIN_CONFIG_BIAS_PULL_UP:
			pin_reg &= ~BIT(PULL_UP_SEL_OFF);
			pin_reg |= (arg & BIT(0)) << PULL_UP_SEL_OFF;
			pin_reg &= ~BIT(PULL_UP_ENABLE_OFF);
			pin_reg |= ((arg>>1) & BIT(0)) << PULL_UP_ENABLE_OFF;
			break;

		case PIN_CONFIG_DRIVE_STRENGTH:
			pin_reg &= ~(DRV_STRENGTH_SEL_MASK
					<< DRV_STRENGTH_SEL_OFF);
			pin_reg |= (arg & DRV_STRENGTH_SEL_MASK)
					<< DRV_STRENGTH_SEL_OFF;
			break;

		default:
			dev_err(&gpio_dev->pdev->dev,
				"Invalid config param %04x\n", param);
			ret = -ENOTSUPP;
		}

		writel(pin_reg, gpio_dev->base + pin*4);
	}
	spin_unlock_irqrestore(&gpio_dev->lock, flags);

	return ret;
}

pinctrl: amd: Add support for additional GPIO

This patch adds support for new Bank and adds IRQCHIP_SKIP_SET_WAKE flag.

Reviewed-by: S-k, Shyam-sundar <Shyam-sundar.S-k@amd.com>
Signed-off-by: Nehal Shah <Nehal-bakulchandra.Shah@amd.com>
Signed-off-by: Linus Walleij <linus.walleij@linaro.org>


static int amd_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
{
	unsigned long flags;
	u32 pin_reg;
	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);

	spin_lock_irqsave(&gpio_dev->lock, flags);
	pin_reg = readl(gpio_dev->base + offset * 4);
	pin_reg &= ~BIT(OUTPUT_ENABLE_OFF);
	writel(pin_reg, gpio_dev->base + offset * 4);
	spin_unlock_irqrestore(&gpio_dev->lock, flags);

	return 0;
}


static void amd_gpio_irq_eoi(struct irq_data *d)
{
	u32 reg;
	unsigned long flags;
	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);

	spin_lock_irqsave(&gpio_dev->lock, flags);
	reg = readl(gpio_dev->base + WAKE_INT_MASTER_REG);
	reg |= EOI_MASK;
	writel(reg, gpio_dev->base + WAKE_INT_MASTER_REG);
	spin_unlock_irqrestore(&gpio_dev->lock, flags);
}