+// process Multilink PPP packet received
+void processmpin(sessionidt s, tunnelidt t, uint8_t *p, uint16_t l)
+{
+ bundleidt b = session[s].bundle;
+ uint8_t begin_frame;
+ uint8_t end_frame;
+ uint32_t seq_num;
+ uint32_t offset;
+
+ if (!b)
+ {
+ LOG(3, s, t, "MPPP: Invalid bundle id: 0\n");
+ return;
+ }
+
+ begin_frame = (*p & 0x80);
+ end_frame = (*p & 0x40);
+ if (session[s].mssf)
+ {
+ // Get 12 bit for seq number
+ uint16_t short_seq_num = ntohs((*(uint16_t *) p) & 0xFF0F);
+ uint16_t short_seq_num2 = short_seq_num >> 4;
+ p += 2;
+ l -= 2;
+ seq_num = short_seq_num2;
+ // After this point the pointer should be advanced 2 bytes
+ LOG(3, s, t, "MPPP: 12 bits, sequence number: %d, short1: %d, short2: %d\n",seq_num, short_seq_num, short_seq_num2);
+ }
+ else
+ {
+ // Get 24 bit for seq number
+ p++;
+ seq_num = ntohl((*(uint32_t *) p) & 0xFFFFFF00);
+ seq_num = seq_num >> 8;
+ p += 3;
+ l -= 4;
+ // After this point the pointer should be advanced 4 bytes
+ LOG(4, s, t, "MPPP: 24 bits sequence number:%d\n",seq_num);
+ }
+
+ if (seq_num - bundle[b].offset < 0)
+ {
+ bundle[b].offset = 0;
+ bundle[b].pending_frag = 0;
+ }
+
+ offset = bundle[b].offset;
+ if (begin_frame)
+ {
+ // Check for previous non-assembled frames
+ int error = 0;
+ if (bundle[b].pending_frag)
+ {
+ uint32_t fn = bundle[b].seq_num_m - offset;
+ uint16_t cur_len;
+ bundle[b].pending_frag = 0;
+ // Check for array indexes
+ if (fn < 0 || fn > MAXFRAGNUM)
+ {
+ LOG(2, s, t, "ERROR: Index out of range fn:%d, bundle:%d\n",fn,b);
+ return;
+ }
+
+ if (seq_num-offset < 0 || seq_num-offset > MAXFRAGNUM)
+ {
+ LOG(2, s, t, "ERROR: Index out of range fn(last):%d, bundle:%d\n",fn,b);
+ return;
+ }
+ /////////////////////////////////////////////////////
+ cur_len = 4; // This is set to 4 to leave 4 bytes for function processipin
+ for (fn = bundle[b].seq_num_m - offset; fn < seq_num - offset; fn++)
+ {
+ if (!frag[b].fragment[fn].length)
+ {
+ LOG(4, s, t, "MPPP: Found lost fragment while reassembling frame %d in (%d,%d)\n",fn, bundle[b].seq_num_m-offset, seq_num-offset);
+ error = 1;
+ break;
+ }
+
+ if (cur_len + frag[b].fragment[fn].length > MAXETHER)
+ {
+ LOG(2, s, t, "MPPP: ERROR: very long frame after assembling %d\n", frag[b].fragment[fn].length+cur_len);
+ error = 1;
+ break;
+ }
+
+ memcpy(frag[b].reassembled_frame+cur_len, frag[b].fragment[fn].data, frag[b].fragment[fn].length);
+ cur_len += frag[b].fragment[fn].length;
+ frag[b].fragment[fn].length = 0; // Indicates that this fragment has been consumed
+ // This is usefull for compression
+ memset(frag[b].fragment[fn].data, 0, sizeof(frag[b].fragment[fn].data));
+ }
+
+ if (!error)
+ {
+ frag[b].re_frame_len = cur_len;
+ // Process the resassembled frame
+ LOG(4, s, t, "MPPP: Process the reassembled frame, len=%d\n",cur_len);
+ processmpframe(s, t, frag[b].reassembled_frame, frag[b].re_frame_len, 1);
+ // Set reassembled frame length to zero after processing it
+ frag[b].re_frame_len = 0;
+ memset(frag[b].reassembled_frame, 0, sizeof(frag[b].reassembled_frame));
+ }
+ }
+ //////////////////////////////////////////
+ bundle[b].seq_num_m = seq_num;
+ if (end_frame)
+ {
+ // Both bits are set
+ LOG(4, s, t, "MPPP: Both bits are set (Begin and End).\n");
+ processmpframe(s, t, p, l, 0);
+ // The maximum number of fragments is 1500
+ if (seq_num - bundle[b].offset >= 1400)
+ {
+ bundle[b].offset = seq_num;
+ LOG(4, s, t, "MPPP: Setting offset to: %d\n",bundle[b].offset);
+ }
+ }
+ else
+ {
+ bundle[b].pending_frag = 1;
+ // End bit is clear
+ LOG(4, s, t, "MPPP: Push to receive buffer\n");
+ // Push to the receive buffer
+ // Array indexes checking
+ if (seq_num-offset < 0 || seq_num-offset >= MAXFRAGNUM)
+ {
+ LOG(2, s, t, "ERROR: Index out of range, push to receive buffer(1) seq:%d, offset:%d, bundle:%d\n",seq_num,offset,b);
+ return;
+ }
+ // Perform length checking
+ if (l > MAXFRAGLEN)
+ {
+ LOG(2, s, t, "MPPP: ERROR: very long fragment length (1)\n");
+ return;
+ }
+ frag[b].fragment[seq_num - offset].length = l;
+ memcpy(frag[b].fragment[seq_num - offset].data, p, l);
+ }
+ }
+ else
+ {
+ LOG(4, s, t, "MPPP: Push to receive buffer\n");
+ // Push to the receive buffer
+ // Array indexes checking
+ if (seq_num-offset < 0 || seq_num-offset >= MAXFRAGNUM)
+ {
+ LOG(2, s, t, "ERROR: Index out of range, push to receive buffer(2) seq:%d, offset:%d, bundle:%d\n",seq_num,offset,b);
+ return;
+ }
+ // Perform length checking
+ if (l > MAXFRAGLEN)
+ {
+ LOG(2, s, t, "MPPP: ERROR: very long fragment length (2).\n");
+ return;
+ }
+ frag[b].fragment[seq_num - offset].length = l;
+ memcpy(frag[b].fragment[seq_num - offset].data, p, l);
+ }
+}
+
projects / l2tpns.git / commitdiff