drivers/isdn/hisax/st5481_hdlc.c

   1 /*
   2  * Driver for ST5481 USB ISDN modem
   3  *
   4  * Author       Frode Isaksen
   5  * Copyright    2001 by Frode Isaksen      <fisaksen@bewan.com>
   6  *              2001 by Kai Germaschewski  <kai.germaschewski@gmx.de>
   7  *
   8  * This software may be used and distributed according to the terms
   9  * of the GNU General Public License, incorporated herein by reference.
  10  *
  11  */
  12
  13 #include "st5481_hdlc.h"
  14
  15 static const unsigned short int crc16_tab[] = {
  16         0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf,
  17         0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7,
  18         0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e,
  19         0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876,
  20         0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd,
  21         0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5,
  22         0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c,
  23         0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974,
  24         0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb,
  25         0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3,
  26         0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a,
  27         0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72,
  28         0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9,
  29         0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1,
  30         0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738,
  31         0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70,
  32         0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7,
  33         0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff,
  34         0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036,
  35         0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e,
  36         0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5,
  37         0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd,
  38         0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134,
  39         0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c,
  40         0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3,
  41         0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb,
  42         0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232,
  43         0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a,
  44         0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1,
  45         0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9,
  46         0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330,
  47         0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78
  48 };
  49
  50
  51
  52 enum {
  53         HDLC_FAST_IDLE,HDLC_GET_FLAG_B0,HDLC_GETFLAG_B1A6,HDLC_GETFLAG_B7,
  54         HDLC_GET_DATA,HDLC_FAST_FLAG
  55 };
  56
  57 enum {
  58         HDLC_SEND_DATA,HDLC_SEND_CRC1,HDLC_SEND_FAST_FLAG,
  59         HDLC_SEND_FIRST_FLAG,HDLC_SEND_CRC2,HDLC_SEND_CLOSING_FLAG,
  60         HDLC_SEND_IDLE1,HDLC_SEND_FAST_IDLE,HDLC_SENDFLAG_B0,
  61         HDLC_SENDFLAG_B1A6,HDLC_SENDFLAG_B7,STOPPED
  62 };
  63
  64 void
  65 hdlc_rcv_init(struct hdlc_vars *hdlc, int do_adapt56)
  66 {
  67         hdlc->bit_shift = 0;
  68         hdlc->hdlc_bits1 = 0;
  69         hdlc->data_bits = 0;
  70         hdlc->ffbit_shift = 0;
  71         hdlc->data_received = 0;
  72         hdlc->state = HDLC_GET_DATA;
  73         hdlc->do_adapt56 = do_adapt56;
  74         hdlc->dchannel = 0;
  75         hdlc->crc = 0;
  76         hdlc->cbin = 0;
  77         hdlc->shift_reg = 0;
  78         hdlc->ffvalue = 0;
  79         hdlc->dstpos = 0;
  80 }
  81
  82 void
  83 hdlc_out_init(struct hdlc_vars *hdlc, int is_d_channel, int do_adapt56)
  84 {
  85         hdlc->bit_shift = 0;
  86         hdlc->hdlc_bits1 = 0;
  87         hdlc->data_bits = 0;
  88         hdlc->ffbit_shift = 0;
  89         hdlc->data_received = 0;
  90         hdlc->do_closing = 0;
  91         hdlc->ffvalue = 0;
  92         if (is_d_channel) {
  93                 hdlc->dchannel = 1;
  94                 hdlc->state = HDLC_SEND_FIRST_FLAG;
  95         } else {
  96                 hdlc->dchannel = 0;
  97                 hdlc->state = HDLC_SEND_FAST_FLAG;
  98                 hdlc->ffvalue = 0x7e;
  99         }
 100         hdlc->cbin = 0x7e;
 101         hdlc->bit_shift = 0;
 102         if(do_adapt56){
 103                 hdlc->do_adapt56 = 1;
 104                 hdlc->data_bits = 0;
 105                 hdlc->state = HDLC_SENDFLAG_B0;
 106         } else {
 107                 hdlc->do_adapt56 = 0;
 108                 hdlc->data_bits = 8;
 109         }
 110         hdlc->shift_reg = 0;
 111 }
 112
 113 /*
 114   hdlc_decode - decodes HDLC frames from a transparent bit stream.
 115
 116   The source buffer is scanned for valid HDLC frames looking for
 117   flags (01111110) to indicate the start of a frame. If the start of
 118   the frame is found, the bit stuffing is removed (0 after 5 1's).
 119   When a new flag is found, the complete frame has been received
 120   and the CRC is checked.
 121   If a valid frame is found, the function returns the frame length
 122   excluding the CRC with the bit HDLC_END_OF_FRAME set.
 123   If the beginning of a valid frame is found, the function returns
 124   the length.
 125   If a framing error is found (too many 1s and not a flag) the function
 126   returns the length with the bit HDLC_FRAMING_ERROR set.
 127   If a CRC error is found the function returns the length with the
 128   bit HDLC_CRC_ERROR set.
 129   If the frame length exceeds the destination buffer size, the function
 130   returns the length with the bit HDLC_LENGTH_ERROR set.
 131
 132   src - source buffer
 133   slen - source buffer length
 134   count - number of bytes removed (decoded) from the source buffer
 135   dst _ destination buffer
 136   dsize - destination buffer size
 137   returns - number of decoded bytes in the destination buffer and status
 138   flag.
 139  */
 140 int hdlc_decode(struct hdlc_vars *hdlc, const unsigned char *src,
 141                 int slen, int *count, unsigned char *dst, int dsize)
 142 {
 143         int status=0;
 144
 145         static const unsigned char fast_flag[]={
 146                 0x00,0x00,0x00,0x20,0x30,0x38,0x3c,0x3e,0x3f
 147         };
 148
 149         static const unsigned char fast_flag_value[]={
 150                 0x00,0x7e,0xfc,0xf9,0xf3,0xe7,0xcf,0x9f,0x3f
 151         };
 152
 153         static const unsigned char fast_abort[]={
 154                 0x00,0x00,0x80,0xc0,0xe0,0xf0,0xf8,0xfc,0xfe,0xff
 155         };
 156
 157         *count = slen;
 158
 159         while(slen > 0){
 160                 if(hdlc->bit_shift==0){
 161                         hdlc->cbin = *src++;
 162                         slen--;
 163                         hdlc->bit_shift = 8;
 164                         if(hdlc->do_adapt56){
 165                                 hdlc->bit_shift --;
 166                         }
 167                 }
 168
 169                 switch(hdlc->state){
 170                 case STOPPED:
 171                         return 0;
 172                 case HDLC_FAST_IDLE:
 173                         if(hdlc->cbin == 0xff){
 174                                 hdlc->bit_shift = 0;
 175                                 break;
 176                         }
 177                         hdlc->state = HDLC_GET_FLAG_B0;
 178                         hdlc->hdlc_bits1 = 0;
 179                         hdlc->bit_shift = 8;
 180                         break;
 181                 case HDLC_GET_FLAG_B0:
 182                         if(!(hdlc->cbin & 0x80)) {
 183                                 hdlc->state = HDLC_GETFLAG_B1A6;
 184                                 hdlc->hdlc_bits1 = 0;
 185                         } else {
 186                                 if(!hdlc->do_adapt56){
 187                                         if(++hdlc->hdlc_bits1 >=8 ) if(hdlc->bit_shift==1)
 188                                                 hdlc->state = HDLC_FAST_IDLE;
 189                                 }
 190                         }
 191                         hdlc->cbin<<=1;
 192                         hdlc->bit_shift --;
 193                         break;
 194                 case HDLC_GETFLAG_B1A6:
 195                         if(hdlc->cbin & 0x80){
 196                                 hdlc->hdlc_bits1++;
 197                                 if(hdlc->hdlc_bits1==6){
 198                                         hdlc->state = HDLC_GETFLAG_B7;
 199                                 }
 200                         } else {
 201                                 hdlc->hdlc_bits1 = 0;
 202                         }
 203                         hdlc->cbin<<=1;
 204                         hdlc->bit_shift --;
 205                         break;
 206                 case HDLC_GETFLAG_B7:
 207                         if(hdlc->cbin & 0x80) {
 208                                 hdlc->state = HDLC_GET_FLAG_B0;
 209                         } else {
 210                                 hdlc->state = HDLC_GET_DATA;
 211                                 hdlc->crc = 0xffff;
 212                                 hdlc->shift_reg = 0;
 213                                 hdlc->hdlc_bits1 = 0;
 214                                 hdlc->data_bits = 0;
 215                                 hdlc->data_received = 0;
 216                         }
 217                         hdlc->cbin<<=1;
 218                         hdlc->bit_shift --;
 219                         break;
 220                 case HDLC_GET_DATA:
 221                         if(hdlc->cbin & 0x80){
 222                                 hdlc->hdlc_bits1++;
 223                                 switch(hdlc->hdlc_bits1){
 224                                 case 6:
 225                                         break;
 226                                 case 7:
 227                                         if(hdlc->data_received) {
 228                                                 // bad frame
 229                                                 status = -HDLC_FRAMING_ERROR;
 230                                         }
 231                                         if(!hdlc->do_adapt56){
 232                                                 if(hdlc->cbin==fast_abort[hdlc->bit_shift+1]){
 233                                                         hdlc->state = HDLC_FAST_IDLE;
 234                                                         hdlc->bit_shift=1;
 235                                                         break;
 236                                                 }
 237                                         } else {
 238                                                 hdlc->state = HDLC_GET_FLAG_B0;
 239                                         }
 240                                         break;
 241                                 default:
 242                                         hdlc->shift_reg>>=1;
 243                                         hdlc->shift_reg |= 0x80;
 244                                         hdlc->data_bits++;
 245                                         break;
 246                                 }
 247                         } else {
 248                                 switch(hdlc->hdlc_bits1){
 249                                 case 5:
 250                                         break;
 251                                 case 6:
 252                                         if(hdlc->data_received){
 253                                                 if (hdlc->dstpos < 2) {
 254                                                         status = -HDLC_FRAMING_ERROR;
 255                                                 } else if (hdlc->crc != 0xf0b8){
 256                                                         // crc error
 257                                                         status = -HDLC_CRC_ERROR;
 258                                                 } else {
 259                                                         // remove CRC
 260                                                         hdlc->dstpos -= 2;
 261                                                         // good frame
 262                                                         status = hdlc->dstpos;
 263                                                 }
 264                                         }
 265                                         hdlc->crc = 0xffff;
 266                                         hdlc->shift_reg = 0;
 267                                         hdlc->data_bits = 0;
 268                                         if(!hdlc->do_adapt56){
 269                                                 if(hdlc->cbin==fast_flag[hdlc->bit_shift]){
 270                                                         hdlc->ffvalue = fast_flag_value[hdlc->bit_shift];
 271                                                         hdlc->state = HDLC_FAST_FLAG;
 272                                                         hdlc->ffbit_shift = hdlc->bit_shift;
 273                                                         hdlc->bit_shift = 1;
 274                                                 } else {
 275                                                         hdlc->state = HDLC_GET_DATA;
 276                                                         hdlc->data_received = 0;
 277                                                 }
 278                                         } else {
 279                                                 hdlc->state = HDLC_GET_DATA;
 280                                                 hdlc->data_received = 0;
 281                                         }
 282                                         break;
 283                                 default:
 284                                         hdlc->shift_reg>>=1;
 285                                         hdlc->data_bits++;
 286                                         break;
 287                                 }
 288                                 hdlc->hdlc_bits1 = 0;
 289                         }
 290                         if (status) {
 291                                 hdlc->dstpos = 0;
 292                                 *count -= slen;
 293                                 hdlc->cbin <<= 1;
 294                                 hdlc->bit_shift--;
 295                                 return status;
 296                         }
 297                         if(hdlc->data_bits==8){
 298                                 unsigned cval;
 299
 300                                 hdlc->data_bits = 0;
 301                                 hdlc->data_received = 1;
 302                                 cval = (hdlc->crc^hdlc->shift_reg) & 0xff;
 303                                 hdlc->crc = (hdlc->crc>>8)^crc16_tab[cval];
 304                                 // good byte received
 305                                 if (dsize--) {
 306                                         dst[hdlc->dstpos++] = hdlc->shift_reg;
 307                                 } else {
 308                                         // frame too long
 309                                         status = -HDLC_LENGTH_ERROR;
 310                                         hdlc->dstpos = 0;
 311                                 }
 312                         }
 313                         hdlc->cbin <<= 1;
 314                         hdlc->bit_shift--;
 315                         break;
 316                 case HDLC_FAST_FLAG:
 317                         if(hdlc->cbin==hdlc->ffvalue){
 318                                 hdlc->bit_shift = 0;
 319                                 break;
 320                         } else {
 321                                 if(hdlc->cbin == 0xff){
 322                                         hdlc->state = HDLC_FAST_IDLE;
 323                                         hdlc->bit_shift=0;
 324                                 } else if(hdlc->ffbit_shift==8){
 325                                         hdlc->state = HDLC_GETFLAG_B7;
 326                                         break;
 327                                 } else {
 328                                         hdlc->shift_reg = fast_abort[hdlc->ffbit_shift-1];
 329                                         hdlc->hdlc_bits1 = hdlc->ffbit_shift-2;
 330                                         if(hdlc->hdlc_bits1<0)hdlc->hdlc_bits1 = 0;
 331                                         hdlc->data_bits = hdlc->ffbit_shift-1;
 332                                         hdlc->state = HDLC_GET_DATA;
 333                                         hdlc->data_received = 0;
 334                                 }
 335                         }
 336                         break;
 337                 default:
 338                         break;
 339                 }
 340         }
 341         *count -= slen;
 342         return 0;
 343 }
 344
 345 /*
 346   hdlc_encode - encodes HDLC frames to a transparent bit stream.
 347
 348   The bit stream starts with a beginning flag (01111110). After
 349   that each byte is added to the bit stream with bit stuffing added
 350   (0 after 5 1's).
 351   When the last byte has been removed from the source buffer, the
 352   CRC (2 bytes is added) and the frame terminates with the ending flag.
 353   For the dchannel, the idle character (all 1's) is also added at the end.
 354   If this function is called with empty source buffer (slen=0), flags or
 355   idle character will be generated.
 356
 357   src - source buffer
 358   slen - source buffer length
 359   count - number of bytes removed (encoded) from source buffer
 360   dst _ destination buffer
 361   dsize - destination buffer size
 362   returns - number of encoded bytes in the destination buffer
 363 */
 364 int hdlc_encode(struct hdlc_vars *hdlc, const unsigned char *src,
 365                 unsigned short slen, int *count,
 366                 unsigned char *dst, int dsize)
 367 {
 368         static const unsigned char xfast_flag_value[] = {
 369                 0x7e,0x3f,0x9f,0xcf,0xe7,0xf3,0xf9,0xfc,0x7e
 370         };
 371
 372         int len = 0;
 373
 374         *count = slen;
 375
 376         while (dsize > 0) {
 377                 if(hdlc->bit_shift==0){
 378                         if(slen && !hdlc->do_closing){
 379                                 hdlc->shift_reg = *src++;
 380                                 slen--;
 381                                 if (slen == 0)
 382                                         hdlc->do_closing = 1;  /* closing sequence, CRC + flag(s) */
 383                                 hdlc->bit_shift = 8;
 384                         } else {
 385                                 if(hdlc->state == HDLC_SEND_DATA){
 386                                         if(hdlc->data_received){
 387                                                 hdlc->state = HDLC_SEND_CRC1;
 388                                                 hdlc->crc ^= 0xffff;
 389                                                 hdlc->bit_shift = 8;
 390                                                 hdlc->shift_reg = hdlc->crc & 0xff;
 391                                         } else if(!hdlc->do_adapt56){
 392                                                 hdlc->state = HDLC_SEND_FAST_FLAG;
 393                                         } else {
 394                                                 hdlc->state = HDLC_SENDFLAG_B0;
 395                                         }
 396                                 }
 397
 398                         }
 399                 }
 400
 401                 switch(hdlc->state){
 402                 case STOPPED:
 403                         while (dsize--)
 404                                 *dst++ = 0xff;
 405
 406                         return dsize;
 407                 case HDLC_SEND_FAST_FLAG:
 408                         hdlc->do_closing = 0;
 409                         if(slen == 0){
 410                                 *dst++ = hdlc->ffvalue;
 411                                 len++;
 412                                 dsize--;
 413                                 break;
 414                         }
 415                         if(hdlc->bit_shift==8){
 416                                 hdlc->cbin = hdlc->ffvalue>>(8-hdlc->data_bits);
 417                                 hdlc->state = HDLC_SEND_DATA;
 418                                 hdlc->crc = 0xffff;
 419                                 hdlc->hdlc_bits1 = 0;
 420                                 hdlc->data_received = 1;
 421                         }
 422                         break;
 423                 case HDLC_SENDFLAG_B0:
 424                         hdlc->do_closing = 0;
 425                         hdlc->cbin <<= 1;
 426                         hdlc->data_bits++;
 427                         hdlc->hdlc_bits1 = 0;
 428                         hdlc->state = HDLC_SENDFLAG_B1A6;
 429                         break;
 430                 case HDLC_SENDFLAG_B1A6:
 431                         hdlc->cbin <<= 1;
 432                         hdlc->data_bits++;
 433                         hdlc->cbin++;
 434                         if(++hdlc->hdlc_bits1 == 6)
 435                                 hdlc->state = HDLC_SENDFLAG_B7;
 436                         break;
 437                 case HDLC_SENDFLAG_B7:
 438                         hdlc->cbin <<= 1;
 439                         hdlc->data_bits++;
 440                         if(slen == 0){
 441                                 hdlc->state = HDLC_SENDFLAG_B0;
 442                                 break;
 443                         }
 444                         if(hdlc->bit_shift==8){
 445                                 hdlc->state = HDLC_SEND_DATA;
 446                                 hdlc->crc = 0xffff;
 447                                 hdlc->hdlc_bits1 = 0;
 448                                 hdlc->data_received = 1;
 449                         }
 450                         break;
 451                 case HDLC_SEND_FIRST_FLAG:
 452                         hdlc->data_received = 1;
 453                         if(hdlc->data_bits==8){
 454                                 hdlc->state = HDLC_SEND_DATA;
 455                                 hdlc->crc = 0xffff;
 456                                 hdlc->hdlc_bits1 = 0;
 457                                 break;
 458                         }
 459                         hdlc->cbin <<= 1;
 460                         hdlc->data_bits++;
 461                         if(hdlc->shift_reg & 0x01)
 462                                 hdlc->cbin++;
 463                         hdlc->shift_reg >>= 1;
 464                         hdlc->bit_shift--;
 465                         if(hdlc->bit_shift==0){
 466                                 hdlc->state = HDLC_SEND_DATA;
 467                                 hdlc->crc = 0xffff;
 468                                 hdlc->hdlc_bits1 = 0;
 469                         }
 470                         break;
 471                 case HDLC_SEND_DATA:
 472                         hdlc->cbin <<= 1;
 473                         hdlc->data_bits++;
 474                         if(hdlc->hdlc_bits1 == 5){
 475                                 hdlc->hdlc_bits1 = 0;
 476                                 break;
 477                         }
 478                         if(hdlc->bit_shift==8){
 479                                 unsigned cval;
 480
 481                                 cval = (hdlc->crc^hdlc->shift_reg) & 0xff;
 482                                 hdlc->crc = (hdlc->crc>>8)^crc16_tab[cval];
 483                         }
 484                         if(hdlc->shift_reg & 0x01){
 485                                 hdlc->hdlc_bits1++;
 486                                 hdlc->cbin++;
 487                                 hdlc->shift_reg >>= 1;
 488                                 hdlc->bit_shift--;
 489                         } else {
 490                                 hdlc->hdlc_bits1 = 0;
 491                                 hdlc->shift_reg >>= 1;
 492                                 hdlc->bit_shift--;
 493                         }
 494                         break;
 495                 case HDLC_SEND_CRC1:
 496                         hdlc->cbin <<= 1;
 497                         hdlc->data_bits++;
 498                         if(hdlc->hdlc_bits1 == 5){
 499                                 hdlc->hdlc_bits1 = 0;
 500                                 break;
 501                         }
 502                         if(hdlc->shift_reg & 0x01){
 503                                 hdlc->hdlc_bits1++;
 504                                 hdlc->cbin++;
 505                                 hdlc->shift_reg >>= 1;
 506                                 hdlc->bit_shift--;
 507                         } else {
 508                                 hdlc->hdlc_bits1 = 0;
 509                                 hdlc->shift_reg >>= 1;
 510                                 hdlc->bit_shift--;
 511                         }
 512                         if(hdlc->bit_shift==0){
 513                                 hdlc->shift_reg = (hdlc->crc >> 8);
 514                                 hdlc->state = HDLC_SEND_CRC2;
 515                                 hdlc->bit_shift = 8;
 516                         }
 517                         break;
 518                 case HDLC_SEND_CRC2:
 519                         hdlc->cbin <<= 1;
 520                         hdlc->data_bits++;
 521                         if(hdlc->hdlc_bits1 == 5){
 522                                 hdlc->hdlc_bits1 = 0;
 523                                 break;
 524                         }
 525                         if(hdlc->shift_reg & 0x01){
 526                                 hdlc->hdlc_bits1++;
 527                                 hdlc->cbin++;
 528                                 hdlc->shift_reg >>= 1;
 529                                 hdlc->bit_shift--;
 530                         } else {
 531                                 hdlc->hdlc_bits1 = 0;
 532                                 hdlc->shift_reg >>= 1;
 533                                 hdlc->bit_shift--;
 534                         }
 535                         if(hdlc->bit_shift==0){
 536                                 hdlc->shift_reg = 0x7e;
 537                                 hdlc->state = HDLC_SEND_CLOSING_FLAG;
 538                                 hdlc->bit_shift = 8;
 539                         }
 540                         break;
 541                 case HDLC_SEND_CLOSING_FLAG:
 542                         hdlc->cbin <<= 1;
 543                         hdlc->data_bits++;
 544                         if(hdlc->hdlc_bits1 == 5){
 545                                 hdlc->hdlc_bits1 = 0;
 546                                 break;
 547                         }
 548                         if(hdlc->shift_reg & 0x01){
 549                                 hdlc->cbin++;
 550                         }
 551                         hdlc->shift_reg >>= 1;
 552                         hdlc->bit_shift--;
 553                         if(hdlc->bit_shift==0){
 554                                 hdlc->ffvalue = xfast_flag_value[hdlc->data_bits];
 555                                 if(hdlc->dchannel){
 556                                         hdlc->ffvalue = 0x7e;
 557                                         hdlc->state = HDLC_SEND_IDLE1;
 558                                         hdlc->bit_shift = 8-hdlc->data_bits;
 559                                         if(hdlc->bit_shift==0)
 560                                                 hdlc->state = HDLC_SEND_FAST_IDLE;
 561                                 } else {
 562                                         if(!hdlc->do_adapt56){
 563                                                 hdlc->state = HDLC_SEND_FAST_FLAG;
 564                                                 hdlc->data_received = 0;
 565                                         } else {
 566                                                 hdlc->state = HDLC_SENDFLAG_B0;
 567                                                 hdlc->data_received = 0;
 568                                         }
 569                                         // Finished with this frame, send flags
 570                                         if (dsize > 1) dsize = 1;
 571                                 }
 572                         }
 573                         break;
 574                 case HDLC_SEND_IDLE1:
 575                         hdlc->do_closing = 0;
 576                         hdlc->cbin <<= 1;
 577                         hdlc->cbin++;
 578                         hdlc->data_bits++;
 579                         hdlc->bit_shift--;
 580                         if(hdlc->bit_shift==0){
 581                                 hdlc->state = HDLC_SEND_FAST_IDLE;
 582                                 hdlc->bit_shift = 0;
 583                         }
 584                         break;
 585                 case HDLC_SEND_FAST_IDLE:
 586                         hdlc->do_closing = 0;
 587                         hdlc->cbin = 0xff;
 588                         hdlc->data_bits = 8;
 589                         if(hdlc->bit_shift == 8){
 590                                 hdlc->cbin = 0x7e;
 591                                 hdlc->state = HDLC_SEND_FIRST_FLAG;
 592                         } else {
 593                                 *dst++ = hdlc->cbin;
 594                                 hdlc->bit_shift = hdlc->data_bits = 0;
 595                                 len++;
 596                                 dsize = 0;
 597                         }
 598                         break;
 599                 default:
 600                         break;
 601                 }
 602                 if(hdlc->do_adapt56){
 603                         if(hdlc->data_bits==7){
 604                                 hdlc->cbin <<= 1;
 605                                 hdlc->cbin++;
 606                                 hdlc->data_bits++;
 607                         }
 608                 }
 609                 if(hdlc->data_bits==8){
 610                         *dst++ = hdlc->cbin;
 611                         hdlc->data_bits = 0;
 612                         len++;
 613                         dsize--;
 614                 }
 615         }
 616         *count -= slen;
 617
 618         return len;
 619 }
 620