svr4/ppp_ahdlc.c

   1 #include <sys/types.h>
   2 #include <sys/param.h>
   3 #include <sys/stream.h>
   4 #include <sys/modctl.h>
   5 #include <sys/conf.h>
   6 #include <sys/kmem.h>
   7 #include <sys/ddi.h>
   8 #include <sys/sunddi.h>
   9 #include <net/ppp_defs.h>
  10 #include <net/pppio.h>
  11
  12 #define IFRAME_BSIZE    512     /* Block size to allocate for input */
  13 #define OFRAME_BSIZE    4096    /* Don't allocb more than this for output */
  14
  15 static int ahdlc_open __P((queue_t *, dev_t *, int, int, cred_t *));
  16 static int ahdlc_close __P((queue_t *, int, cred_t *));
  17 static int ahdlc_wput __P((queue_t *, mblk_t *));
  18 static int ahdlc_rput __P((queue_t *, mblk_t *));
  19 static void stuff_frame __P((queue_t *, mblk_t *));
  20 static void unstuff_chars __P((queue_t *, mblk_t *));
  21
  22 static struct module_info minfo = {
  23     0x7d23, "ppp_ahdl", 0, INFPSZ, 4096, 128
  24 };
  25
  26 static struct qinit rinit = {
  27     ahdlc_rput, NULL, ahdlc_open, ahdlc_close, NULL, &minfo, NULL
  28 };
  29
  30 static struct qinit winit = {
  31     ahdlc_wput, NULL, NULL, NULL, NULL, &minfo, NULL
  32 };
  33
  34 static struct streamtab ahdlc_info = {
  35     &rinit, &winit, NULL, NULL
  36 };
  37
  38 static struct fmodsw fsw = {
  39     "ppp_ahdl",
  40     &ahdlc_info,
  41     D_NEW | D_MP | D_MTQPAIR
  42 };
  43
  44 extern struct mod_ops mod_strmodops;
  45
  46 static struct modlstrmod modlstrmod = {
  47     &mod_strmodops,
  48     "PPP async HDLC module",
  49     &fsw
  50 };
  51
  52 static struct modlinkage modlinkage = {
  53     MODREV_1,
  54     (void *) &modlstrmod,
  55     NULL
  56 };
  57
  58 struct ahdlc_state {
  59     int flags;
  60     mblk_t *cur_frame;
  61     mblk_t *cur_blk;
  62     int inlen;
  63     ushort infcs;
  64     u_int32_t xaccm[8];
  65     u_int32_t raccm;
  66     int mtu;
  67     int mru;
  68 };
  69
  70 /* Values for flags */
  71 #define ESCAPED         1       /* last saw escape char on input */
  72 #define IFLUSH          2       /* flushing input due to error */
  73
  74 /*
  75  * FCS lookup table as calculated by genfcstab.
  76  */
  77 static u_short fcstab[256] = {
  78         0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
  79         0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
  80         0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
  81         0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
  82         0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
  83         0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
  84         0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
  85         0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
  86         0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
  87         0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
  88         0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
  89         0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
  90         0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
  91         0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
  92         0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
  93         0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
  94         0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
  95         0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
  96         0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
  97         0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
  98         0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
  99         0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
 100         0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
 101         0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
 102         0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
 103         0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
 104         0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
 105         0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
 106         0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
 107         0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
 108         0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
 109         0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
 110 };
 111
 112 /*
 113  * Entry points for modloading.
 114  */
 115 int
 116 _init(void)
 117 {
 118     return mod_install(&modlinkage);
 119 }
 120
 121 int
 122 _fini(void)
 123 {
 124     return mod_remove(&modlinkage);
 125 }
 126
 127 int
 128 _info(mip)
 129     struct modinfo *mip;
 130 {
 131     return mod_info(&modlinkage, mip);
 132 }
 133
 134 /*
 135  * STREAMS module entry points.
 136  */
 137 static int
 138 ahdlc_open(q, devp, flag, sflag, credp)
 139     queue_t *q;
 140     dev_t *devp;
 141     int flag, sflag;
 142     cred_t *credp;
 143 {
 144     struct ahdlc_state *sp;
 145
 146     if (q->q_ptr == 0) {
 147         sp = (struct ahdlc_state *) kmem_zalloc(sizeof(struct ahdlc_state),
 148                                                 KM_SLEEP);
 149         if (sp == 0)
 150             return ENOSR;
 151         q->q_ptr = sp;
 152         WR(q)->q_ptr = sp;
 153         sp->xaccm[0] = ~0;
 154         sp->xaccm[3] = 0x60000000;
 155         sp->mru = 1500;
 156         qprocson(q);
 157     }
 158     return 0;
 159 }
 160
 161 static int
 162 ahdlc_close(q, flag, credp)
 163     queue_t *q;
 164     int flag;
 165     cred_t *credp;
 166 {
 167     struct ahdlc_state *state;
 168
 169     qprocsoff(q);
 170     if (q->q_ptr != 0) {
 171         state = (struct ahdlc_state *) q->q_ptr;
 172         if (state->cur_frame != 0) {
 173             freemsg(state->cur_frame);
 174             state->cur_frame = 0;
 175         }
 176         kmem_free(q->q_ptr, sizeof(struct ahdlc_state));
 177     }
 178     return 0;
 179 }
 180
 181 static int
 182 ahdlc_wput(q, mp)
 183     queue_t *q;
 184     mblk_t *mp;
 185 {
 186     struct ahdlc_state *state;
 187     struct iocblk *iop;
 188
 189     state = (struct ahdlc_state *) q->q_ptr;
 190     switch (mp->b_datap->db_type) {
 191     case M_DATA:
 192         /*
 193          * A data packet - do character-stuffing and FCS, and
 194          * send it onwards.
 195          */
 196         stuff_frame(q, mp);
 197         freemsg(mp);
 198         break;
 199
 200     case M_IOCTL:
 201         iop = (struct iocblk *) mp->b_rptr;
 202         switch (iop->ioc_cmd) {
 203         case PPPIO_XACCM:
 204             if (iop->ioc_count != sizeof(ext_accm))
 205                 goto iocnak;
 206             bcopy(mp->b_cont->b_rptr, (caddr_t)state->xaccm, sizeof(ext_accm));
 207             state->xaccm[2] &= 0x40000000;      /* don't escape 0x5e */
 208             state->xaccm[3] |= 0x60000000;      /* do escape 0x7d, 0x7e */
 209             goto iocack;
 210
 211         case PPPIO_RACCM:
 212             if (iop->ioc_count != sizeof(u_int32_t))
 213                 goto iocnak;
 214             state->raccm = *(u_int32_t *)mp->b_cont->b_rptr;
 215             goto iocack;
 216
 217         default:
 218             putnext(q, mp);
 219             break;
 220
 221         iocack:
 222             iop->ioc_count = 0;
 223             mp->b_datap->db_type = M_IOCACK;
 224             qreply(q, mp);
 225             break;
 226
 227         iocnak:
 228             mp->b_datap->db_type = M_IOCNAK;
 229             qreply(q, mp);
 230             break;
 231         }
 232
 233     default:
 234         putnext(q, mp);
 235     }
 236     return 0;
 237 }
 238
 239 static int
 240 ahdlc_rput(q, mp)
 241     queue_t *q;
 242     mblk_t *mp;
 243 {
 244     mblk_t *np;
 245     uchar_t *cp;
 246     struct ahdlc_state *state;
 247
 248     switch (mp->b_datap->db_type) {
 249     case M_DATA:
 250         unstuff_chars(q, mp);
 251         freemsg(mp);
 252         break;
 253
 254     case M_HANGUP:
 255         state = (struct ahdlc_state *) q->q_ptr;
 256         if (state->cur_frame != 0) {
 257             /* XXX would like to send this up for debugging */
 258             freemsg(state->cur_frame);
 259             state->cur_frame = 0;
 260             state->cur_blk = 0;
 261         }
 262         state->inlen = 0;
 263         state->flags = IFLUSH;
 264         putnext(q, mp);
 265         break;
 266
 267     default:
 268         putnext(q, mp);
 269     }
 270     return 0;
 271 }
 272
 273 /* Extract bit c from map m, to determine if c needs to be escaped. */
 274 #define ESCAPE(c, m)    ((m)[(c) >> 5] & (1 << ((c) & 0x1f)))
 275
 276 static void
 277 stuff_frame(q, mp)
 278     queue_t *q;
 279     mblk_t *mp;
 280 {
 281     struct ahdlc_state *state;
 282     int ilen, olen, c, extra;
 283     mblk_t *omsg, *np, *op;
 284     uchar_t *sp, *sp0, *dp, *dp0, *spend;
 285     ushort_t fcs;
 286     u_int32_t *xaccm, lcp_xaccm[8];
 287
 288     /*
 289      * We estimate the length of the output packet as
 290      * 1.25 * input length + 16 (for initial flag, FCS, final flag, slop).
 291      */
 292     state = (struct ahdlc_state *) q->q_ptr;
 293     ilen = msgdsize(mp);
 294     olen = ilen + (ilen >> 2) + 16;
 295     if (olen > OFRAME_BSIZE)
 296         olen = OFRAME_BSIZE;
 297     omsg = op = allocb(olen, BPRI_MED);
 298     if (omsg == 0)
 299         return;
 300
 301     /*
 302      * Put in an initial flag for now.  We'll remove it later
 303      * if we decide we don't need it.
 304      */
 305     dp = op->b_wptr;
 306     *dp++ = PPP_FLAG;
 307     --olen;
 308
 309     /*
 310      * For LCP packets, we must escape all control characters.
 311      */
 312     if (proto == PPP_LCP) {
 313         bcopy(state->xaccm, lcp_xaccm, sizeof(lcp_xaccm));
 314         lcp_xaccm[0] = ~0;
 315         xaccm = lcp_xaccm;
 316     } else
 317         xaccm = state->xaccm;
 318
 319     sp = mp->b_rptr;
 320     fcs = PPP_INITFCS;
 321     for (;;) {
 322         spend = mp->b_wptr;
 323         extra = sp + olen - spend;
 324         if (extra < 0) {
 325             spend = sp + olen;
 326             extra = 0;
 327         }
 328         /*
 329          * We can safely process the input up to `spend'
 330          * without overrunning the output, provided we don't
 331          * hit more than `extra' characters which need to be escaped.
 332          */
 333         sp0 = sp;
 334         dp0 = dp;
 335         while (sp < spend) {
 336             c = *sp;
 337             if (ESCAPE(c, xaccm)) {
 338                 if (extra > 0)
 339                     --extra;
 340                 else if (sp < spend - 1)
 341                     --spend;
 342                 else
 343                     break;
 344                 fcs = PPP_FCS(fcs, c);
 345                 *dp++ = PPP_ESCAPE;
 346                 c ^= PPP_TRANS;
 347             } else
 348                 fcs = PPP_FCS(fcs, c);
 349             *dp++ = c;
 350             ++sp;
 351         }
 352         ilen -= sp - sp0;
 353         olen -= dp - dp0;
 354
 355         /*
 356          * At this point, we have emptied an input block
 357          * and/or filled an output block.
 358          */
 359         if (sp >= mp->b_wptr) {
 360             /*
 361              * We've emptied an input block.  Advance to the next.
 362              */
 363             mp = mp->b_cont;
 364             if (mp == 0)
 365                 break;          /* all done */
 366             sp = mp->b_rptr;
 367         }
 368         if (olen < 2) {
 369             /*
 370              * The output block is full.  Allocate a new one.
 371              */
 372             op->b_wptr = dp;
 373             olen = 2 * ilen + 5;
 374             if (olen > OFRAME_BSIZE)
 375                 olen = OFRAME_BSIZE;
 376             np = allocb(olen, BPRI_MED);
 377             if (np == 0) {
 378                 freemsg(omsg);
 379                 return;
 380             }
 381             op->b_cont = np;
 382             op = np;
 383             dp = op->b_wptr;
 384         }
 385     }
 386
 387     /*
 388      * Append the FCS and closing flag.
 389      * This could require up to 5 characters.
 390      */
 391     if (olen < 5) {
 392         /* Sigh.  Need another block. */
 393         op->b_wptr = dp;
 394         np = allocb(5, BPRI_MED);
 395         if (np == 0) {
 396             freemsg(omsg);
 397             return;
 398         }
 399         op->b_cont = np;
 400         op = np;
 401         dp = op->b_wptr;
 402     }
 403     c = ~fcs & 0xff;
 404     if (ESCAPE(c, xaccm)) {
 405         *dp++ = PPP_ESCAPE;
 406         c ^= PPP_TRANS;
 407     }
 408     *dp++ = c;
 409     c = (~fcs >> 8) & 0xff;
 410     if (ESCAPE(c, xaccm)) {
 411         *dp++ = PPP_ESCAPE;
 412         c ^= PPP_TRANS;
 413     }
 414     *dp++ = c;
 415     *dp++ = PPP_FLAG;
 416     op->b_wptr = dp;
 417
 418     /*
 419      * Remove the initial flag, if possible.
 420      */
 421     if (qsize(q->q_next) > 0)
 422         ++omsg->b_rptr;
 423
 424     putnext(q, omsg);
 425 }
 426
 427 static void
 428 unstuff_chars(q, mp)
 429     queue_t *q;
 430     mblk_t *mp;
 431 {
 432     struct ahdlc_state *state;
 433     mblk_t *om;
 434     uchar_t *cp, *cpend, *dp, *dp0;
 435     int c, len, extra, offset;
 436     ushort_t fcs;
 437
 438     state = (struct ahdlc_state *) q->q_ptr;
 439     cp = mp->b_rptr;
 440     for (;;) {
 441         /*
 442          * Advance to next input block if necessary.
 443          */
 444         if (cp >= mp->b_wptr) {
 445             mp = mp->b_cont;
 446             if (mp == 0)
 447                 break;
 448             cp = mp->b_rptr;
 449             continue;
 450         }
 451
 452         if ((state->flags & (IFLUSH|ESCAPED)) == 0
 453             && state->inlen >= PPP_HDRLEN
 454             && (om = state->cur_blk) != 0) {
 455             /*
 456              * Process bulk chars as quickly as possible.
 457              */
 458             dp = om->b_wptr;
 459             len = om->b_datap->db_lim - dp; /* max # output bytes */
 460             extra = (cpend - cp) - len;     /* #input chars - #output bytes */
 461             if (extra < 0) {
 462                 len += extra;               /* we'll run out of input first */
 463                 extra = 0;
 464             }
 465             cpend = cp + len;
 466             dp0 = dp;
 467             fcs = state->infcs;
 468             while (cp < cpend) {
 469                 c = *cp;
 470                 if (c == PPP_FLAG)
 471                     break;
 472                 ++cp;
 473                 if (c == PPP_ESCAPE) {
 474                     if (extra > 0)
 475                         --extra;
 476                     else if (len > 1)
 477                         --len;
 478                     else {
 479                         state->flags |= ESCAPED;
 480                         break;
 481                     }
 482                     c = *cp;
 483                     if (c == PPP_FLAG)
 484                         break;
 485                     ++cp;
 486                     c ^= PPP_TRANS;
 487                 }
 488                 *dp++ = c;
 489                 fcs = PPP_FCS(fcs, c);
 490             }
 491             state->inlen += dp - dp0;
 492             state->infcs = fcs;
 493             om->b_wptr = dp;
 494             if (len <= 0)
 495                 continue;       /* go back and check cp again */
 496         }
 497
 498         c = *cp++;
 499         if (c == PPP_FLAG) {
 500             /*
 501              * End of a frame.
 502              * If the ESCAPE flag is set, the frame ended with
 503              * the frame abort sequence "}~".
 504              */
 505             om = state->cur_frame;
 506             len = state->inlen;
 507             state->cur_frame = 0;
 508             state->inlen = 0;
 509             if (om == 0)
 510                 continue;
 511             if (state->flags & (IFLUSH|ESCAPED) || len < PPP_FCSLEN) {
 512                 /* XXX should send up ctl message to notify VJ decomp */
 513                 freemsg(om);
 514                 continue;
 515             }
 516             if (state->infcs != PPP_GOODFCS) {
 517                 /* incr bad fcs stats */
 518                 /* would like to be able to pass this up for debugging */
 519                 /* XXX should send up ctl message to notify VJ decomp */
 520                 freemsg(om);
 521                 continue;
 522             }
 523             adjmsg(om, -PPP_FCSLEN);    /* chop off fcs */
 524             putnext(q, om);             /* bombs away! */
 525             continue;
 526         }
 527
 528         if (state->flags & IFLUSH)
 529             continue;
 530         if (state->flags & ESCAPED) {
 531             c ^= PPP_TRANS;
 532         } else if (c == PPP_ESCAPE) {
 533             state->flags |= ESCAPED;
 534             continue;
 535         }
 536         if (state->inlen == 0) {
 537             /*
 538              * First byte of the frame: allocate the first message block.
 539              */
 540             om = allocb(IFRAME_BSIZE, BPRI_MED);
 541             if (om == 0) {
 542                 state->flags |= IFLUSH;
 543                 continue;
 544             }
 545             state->cur_frame = om;
 546             state->cur_blk = om;
 547             state->infcs = PPP_INITFCS;
 548         } else {
 549             om = state->cur_blk;
 550             if (om->b_wptr >= om->b_datap->db_lim) {
 551                 /*
 552                  * Current message block is full.  Allocate another one.
 553                  */
 554                 om = allocb(IFRAME_BSIZE, BPRI_MED);
 555                 if (om == 0) {
 556                     freemsg(state->cur_frame);
 557                     state->flags |= IFLUSH;
 558                     continue;
 559                 }
 560                 state->cur_blk->b_cont = om;
 561                 state->cur_blk = om;
 562             }
 563         }
 564         *om->b_wptr++ = c;
 565         ++state->inlen;
 566         state->infcs = PPP_FCS(state->infcs, c);
 567
 568         if (state->inlen == PPP_HDRLEN) {
 569             /*
 570              * We don't do address/control & protocol decompression here,
 571              * but we do leave space for the decompressed fields and
 572              * arrange for the info field to start on a word boundary.
 573              */
 574             cp = om->b_rptr;
 575             if (*cp == PPP_ALLSTATIONS)
 576                 cp += 2;
 577             if ((*cp & 1) == 0)
 578                 ++cp;
 579             /* cp is now pointing at the last byte of the ppp protocol field */
 580             offset = 3 - ((unsigned)cp & 3);
 581             if (offset > 0) {
 582                 do {
 583                     cp[offset] = cp[0];
 584                     --cp;
 585                 } while (cp >= om->b_rptr);
 586                 om->b_rptr += offset;
 587                 om->b_wptr += offset;
 588             }
 589         }
 590     }
 591 }
 592