1/*-
2 * Copyright (c) 1996 - 2001 Brian Somers <brian@Awfulhak.org>
3 *          based on work by Toshiharu OHNO <tony-o@iij.ad.jp>
4 *                           Internet Initiative Japan, Inc (IIJ)
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 *    notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 *    notice, this list of conditions and the following disclaimer in the
14 *    documentation and/or other materials provided with the distribution.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26 * SUCH DAMAGE.
27 *
28 * $FreeBSD$
29 */
30
31#include <sys/types.h>
32
33#include <stdarg.h>
34#include <stdio.h>
35#include <stdlib.h>
36#include <string.h>
37#include <sysexits.h>
38#include <termios.h>
39
40#include "defs.h"
41#include "command.h"
42#include "mbuf.h"
43#include "log.h"
44#include "descriptor.h"
45#include "prompt.h"
46#include "main.h"
47
48#define BUCKET_CHUNK	20
49#define BUCKET_HASH	256
50
51struct mbucket;
52
53struct mfree {
54  struct mbucket *next;
55  size_t count;
56};
57
58static struct mbucket {
59  union {
60    struct mbuf m;
61    struct mfree f;
62  } u;
63} *bucket[(M_MAXLEN + sizeof(struct mbuf)) / BUCKET_HASH];
64
65#define M_BINDEX(sz)	(((sz) + sizeof(struct mbuf) - 1) / BUCKET_HASH)
66#define M_BUCKET(sz)	(bucket + M_BINDEX(sz))
67#define M_ROUNDUP(sz)	((M_BINDEX(sz) + 1) * BUCKET_HASH)
68
69static struct memmap {
70  struct mbuf *queue;
71  size_t fragments;
72  size_t octets;
73} MemMap[MB_MAX + 1];
74
75static unsigned long long mbuf_Mallocs, mbuf_Frees;
76
77size_t
78m_length(struct mbuf *bp)
79{
80  size_t len;
81
82  for (len = 0; bp; bp = bp->m_next)
83    len += bp->m_len;
84  return len;
85}
86
87static const char *
88mbuftype(int type)
89{
90  static const char * const mbufdesc[MB_MAX] = {
91    "ip in", "ip out", "ipv6 in", "ipv6 out", "nat in", "nat out",
92    "mp in", "mp out", "vj in", "vj out", "icompd in", "icompd out",
93    "compd in", "compd out", "lqr in", "lqr out", "echo in", "echo out",
94    "proto in", "proto out", "acf in", "acf out", "sync in", "sync out",
95    "hdlc in", "hdlc out", "async in", "async out", "cbcp in", "cbcp out",
96    "chap in", "chap out", "pap in", "pap out", "ccp in", "ccp out",
97    "ipcp in", "ipcp out", "ipv6cp in", "ipv6cp out", "lcp in", "lcp out"
98  };
99
100  return type < 0 || type >= MB_MAX ? "unknown" : mbufdesc[type];
101}
102
103struct mbuf *
104m_get(size_t m_len, int type)
105{
106  struct mbucket **mb;
107  struct mbuf *bp;
108  size_t size;
109
110  if (type > MB_MAX) {
111    log_Printf(LogERROR, "Bad mbuf type %d\n", type);
112    type = MB_UNKNOWN;
113  }
114
115  if (m_len > M_MAXLEN || m_len == 0) {
116    log_Printf(LogERROR, "Request for mbuf size %lu (\"%s\") denied !\n",
117               (u_long)m_len, mbuftype(type));
118    AbortProgram(EX_OSERR);
119  }
120
121  mb = M_BUCKET(m_len);
122  size = M_ROUNDUP(m_len);
123
124  if (*mb) {
125    /* We've got some free blocks of the right size */
126    bp = &(*mb)->u.m;
127    if (--(*mb)->u.f.count == 0)
128      *mb = (*mb)->u.f.next;
129    else {
130      ((struct mbucket *)((char *)*mb + size))->u.f.count = (*mb)->u.f.count;
131      *mb = (struct mbucket *)((char *)*mb + size);
132      (*mb)->u.f.next = NULL;
133    }
134  } else {
135    /*
136     * Allocate another chunk of mbufs, use the first and put the rest on
137     * the free list
138     */
139    *mb = (struct mbucket *)malloc(BUCKET_CHUNK * size);
140    if (*mb == NULL) {
141      log_Printf(LogALERT, "Failed to allocate memory (%lu)\n",
142                 (unsigned long)BUCKET_CHUNK * size);
143      AbortProgram(EX_OSERR);
144    }
145    bp = &(*mb)->u.m;
146    *mb = (struct mbucket *)((char *)*mb + size);
147    (*mb)->u.f.count = BUCKET_CHUNK - 1;
148    (*mb)->u.f.next = NULL;
149  }
150
151  mbuf_Mallocs++;
152
153  memset(bp, '\0', sizeof(struct mbuf));
154  bp->m_size = size - sizeof *bp;
155  bp->m_len = m_len;
156  bp->m_type = type;
157
158  MemMap[type].fragments++;
159  MemMap[type].octets += bp->m_size;
160
161  return bp;
162}
163
164struct mbuf *
165m_free(struct mbuf *bp)
166{
167  struct mbucket **mb, *f;
168  struct mbuf *nbp;
169
170  if ((f = (struct mbucket *)bp) != NULL) {
171    MemMap[bp->m_type].fragments--;
172    MemMap[bp->m_type].octets -= bp->m_size;
173
174    nbp = bp->m_next;
175    mb = M_BUCKET(bp->m_size);
176    f->u.f.next = *mb;
177    f->u.f.count = 1;
178    *mb = f;
179
180    mbuf_Frees++;
181    bp = nbp;
182  }
183
184  return bp;
185}
186
187void
188m_freem(struct mbuf *bp)
189{
190  while (bp)
191    bp = m_free(bp);
192}
193
194struct mbuf *
195mbuf_Read(struct mbuf *bp, void *v, size_t len)
196{
197  int nb;
198  u_char *ptr = v;
199
200  while (bp && len > 0) {
201    if (len > bp->m_len)
202      nb = bp->m_len;
203    else
204      nb = len;
205    if (nb) {
206      memcpy(ptr, MBUF_CTOP(bp), nb);
207      ptr += nb;
208      bp->m_len -= nb;
209      len -= nb;
210      bp->m_offset += nb;
211    }
212    if (bp->m_len == 0)
213      bp = m_free(bp);
214  }
215
216  while (bp && bp->m_len == 0)
217    bp = m_free(bp);
218
219  return bp;
220}
221
222size_t
223mbuf_View(struct mbuf *bp, void *v, size_t len)
224{
225  size_t nb, l = len;
226  u_char *ptr = v;
227
228  while (bp && l > 0) {
229    if (l > bp->m_len)
230      nb = bp->m_len;
231    else
232      nb = l;
233    memcpy(ptr, MBUF_CTOP(bp), nb);
234    ptr += nb;
235    l -= nb;
236    bp = bp->m_next;
237  }
238
239  return len - l;
240}
241
242struct mbuf *
243m_prepend(struct mbuf *bp, const void *ptr, size_t len, u_short extra)
244{
245  struct mbuf *head;
246
247  if (bp && bp->m_offset) {
248    if (bp->m_offset >= len) {
249      bp->m_offset -= len;
250      bp->m_len += len;
251      if (ptr)
252        memcpy(MBUF_CTOP(bp), ptr, len);
253      return bp;
254    }
255    len -= bp->m_offset;
256    if (ptr)
257      memcpy(bp + 1, (const char *)ptr + len, bp->m_offset);
258    bp->m_len += bp->m_offset;
259    bp->m_offset = 0;
260  }
261
262  head = m_get(len + extra, bp ? bp->m_type : MB_UNKNOWN);
263  head->m_offset = extra;
264  head->m_len -= extra;
265  if (ptr)
266    memcpy(MBUF_CTOP(head), ptr, len);
267  head->m_next = bp;
268
269  return head;
270}
271
272struct mbuf *
273m_adj(struct mbuf *bp, ssize_t n)
274{
275  if (n > 0) {
276    while (bp) {
277      if ((size_t)n < bp->m_len) {
278        bp->m_len = n;
279        bp->m_offset += n;
280        return bp;
281      }
282      n -= bp->m_len;
283      bp = m_free(bp);
284    }
285  } else {
286    if ((n = m_length(bp) + n) <= 0) {
287      m_freem(bp);
288      return NULL;
289    }
290    for (; bp; bp = bp->m_next, n -= bp->m_len)
291      if ((size_t)n < bp->m_len) {
292        bp->m_len = n;
293        m_freem(bp->m_next);
294        bp->m_next = NULL;
295        break;
296      }
297  }
298
299  return bp;
300}
301
302void
303mbuf_Write(struct mbuf *bp, const void *ptr, size_t m_len)
304{
305  size_t plen;
306  int nb;
307
308  plen = m_length(bp);
309  if (plen < m_len)
310    m_len = plen;
311
312  while (m_len > 0) {
313    nb = (m_len < bp->m_len) ? m_len : bp->m_len;
314    memcpy(MBUF_CTOP(bp), ptr, nb);
315    m_len -= bp->m_len;
316    bp = bp->m_next;
317  }
318}
319
320int
321mbuf_Show(struct cmdargs const *arg)
322{
323  int i;
324
325  prompt_Printf(arg->prompt, "Fragments (octets) in use:\n");
326  for (i = 0; i < MB_MAX; i += 2)
327    prompt_Printf(arg->prompt, "%10.10s: %04lu (%06lu)\t"
328                  "%10.10s: %04lu (%06lu)\n",
329	          mbuftype(i), (u_long)MemMap[i].fragments,
330                  (u_long)MemMap[i].octets, mbuftype(i+1),
331                  (u_long)MemMap[i+1].fragments, (u_long)MemMap[i+1].octets);
332
333  if (i == MB_MAX)
334    prompt_Printf(arg->prompt, "%10.10s: %04lu (%06lu)\n",
335                  mbuftype(i), (u_long)MemMap[i].fragments,
336                  (u_long)MemMap[i].octets);
337
338  prompt_Printf(arg->prompt, "Mallocs: %llu,   Frees: %llu\n",
339                mbuf_Mallocs, mbuf_Frees);
340
341  return 0;
342}
343
344struct mbuf *
345m_dequeue(struct mqueue *q)
346{
347  struct mbuf *bp;
348
349  log_Printf(LogDEBUG, "m_dequeue: queue len = %lu\n", (u_long)q->len);
350  bp = q->top;
351  if (bp) {
352    q->top = q->top->m_nextpkt;
353    q->len--;
354    if (q->top == NULL) {
355      q->last = q->top;
356      if (q->len)
357	log_Printf(LogERROR, "m_dequeue: Not zero (%lu)!!!\n",
358                   (u_long)q->len);
359    }
360    bp->m_nextpkt = NULL;
361  }
362
363  return bp;
364}
365
366void
367m_enqueue(struct mqueue *queue, struct mbuf *bp)
368{
369  if (bp != NULL) {
370    if (queue->last) {
371      queue->last->m_nextpkt = bp;
372      queue->last = bp;
373    } else
374      queue->last = queue->top = bp;
375    queue->len++;
376    log_Printf(LogDEBUG, "m_enqueue: len = %lu\n", (unsigned long)queue->len);
377  }
378}
379
380struct mbuf *
381m_pullup(struct mbuf *bp)
382{
383  /* Put it all in one contigous (aligned) mbuf */
384
385  if (bp != NULL) {
386    if (bp->m_next != NULL) {
387      struct mbuf *nbp;
388      u_char *cp;
389
390      nbp = m_get(m_length(bp), bp->m_type);
391
392      for (cp = MBUF_CTOP(nbp); bp; bp = m_free(bp)) {
393        memcpy(cp, MBUF_CTOP(bp), bp->m_len);
394        cp += bp->m_len;
395      }
396      bp = nbp;
397    }
398#ifndef __i386__	/* Do any other archs not care about alignment ? */
399    else if ((bp->m_offset & (sizeof(long) - 1)) != 0) {
400      bcopy(MBUF_CTOP(bp), bp + 1, bp->m_len);
401      bp->m_offset = 0;
402    }
403#endif
404  }
405
406  return bp;
407}
408
409void
410m_settype(struct mbuf *bp, int type)
411{
412  for (; bp; bp = bp->m_next)
413    if (type != bp->m_type) {
414      MemMap[bp->m_type].fragments--;
415      MemMap[bp->m_type].octets -= bp->m_size;
416      bp->m_type = type;
417      MemMap[type].fragments++;
418      MemMap[type].octets += bp->m_size;
419    }
420}
421
422struct mbuf *
423m_append(struct mbuf *bp, const void *v, size_t sz)
424{
425  struct mbuf *m = bp;
426
427  if (m) {
428    while (m->m_next)
429      m = m->m_next;
430    if (m->m_size - m->m_len >= sz) {
431      if (v)
432        memcpy((char *)(m + 1) + m->m_len, v, sz);
433      m->m_len += sz;
434    } else
435      m->m_next = m_prepend(NULL, v, sz, 0);
436  } else
437    bp = m_prepend(NULL, v, sz, 0);
438
439  return bp;
440}
441