| /* |
| * OsmoGGSN - Gateway GPRS Support Node |
| * Copyright (C) 2002 Mondru AB. |
| * |
| * The contents of this file may be used under the terms of the GNU |
| * General Public License Version 2, provided that the above copyright |
| * notice and this permission notice is included in all copies or |
| * substantial portions of the software. |
| * |
| */ |
| |
| /* |
| * gtpie.c: Contains functions to encapsulate and decapsulate GTP |
| * information elements |
| * |
| * |
| * Encapsulation |
| * - gtpie_tlv, gtpie_tv0, gtpie_tv1, gtpie_tv2 ... Adds information |
| * elements to a buffer. |
| * |
| * Decapsulation |
| * - gtpie_decaps: Returns array with pointers to information elements. |
| * - getie_getie: Returns the pointer of a particular element. |
| * - gtpie_gettlv: Copies tlv information element. Return 0 on success. |
| * - gtpie_gettv: Copies tv information element. Return 0 on success. |
| * |
| */ |
| |
| #include <../config.h> |
| |
| #ifdef HAVE_STDINT_H |
| #include <stdint.h> |
| #endif |
| |
| #include <stdio.h> |
| #include <sys/types.h> |
| #include <netinet/in.h> |
| #include <string.h> |
| |
| #include "gtpie.h" |
| |
| int gtpie_tlv(void *p, unsigned int *length, unsigned int size, uint8_t t, |
| int l, void *v) |
| { |
| if ((*length + 3 + l) >= size) |
| return 1; |
| ((union gtpie_member *)(p + *length))->tlv.t = hton8(t); |
| ((union gtpie_member *)(p + *length))->tlv.l = hton16(l); |
| memcpy((void *)(p + *length + 3), v, l); |
| *length += 3 + l; |
| return 0; |
| } |
| |
| int gtpie_tv0(void *p, unsigned int *length, unsigned int size, uint8_t t, |
| int l, uint8_t * v) |
| { |
| if ((*length + 1 + l) >= size) |
| return 1; |
| ((union gtpie_member *)(p + *length))->tv0.t = hton8(t); |
| memcpy((void *)(p + *length + 1), v, l); |
| *length += 1 + l; |
| return 0; |
| } |
| |
| int gtpie_tv1(void *p, unsigned int *length, unsigned int size, uint8_t t, |
| uint8_t v) |
| { |
| if ((*length + 2) >= size) |
| return 1; |
| ((union gtpie_member *)(p + *length))->tv1.t = hton8(t); |
| ((union gtpie_member *)(p + *length))->tv1.v = hton8(v); |
| *length += 2; |
| return 0; |
| } |
| |
| int gtpie_tv2(void *p, unsigned int *length, unsigned int size, uint8_t t, |
| uint16_t v) |
| { |
| if ((*length + 3) >= size) |
| return 1; |
| ((union gtpie_member *)(p + *length))->tv2.t = hton8(t); |
| ((union gtpie_member *)(p + *length))->tv2.v = hton16(v); |
| *length += 3; |
| return 0; |
| } |
| |
| int gtpie_tv4(void *p, unsigned int *length, unsigned int size, uint8_t t, |
| uint32_t v) |
| { |
| if ((*length + 5) >= size) |
| return 1; |
| ((union gtpie_member *)(p + *length))->tv4.t = hton8(t); |
| ((union gtpie_member *)(p + *length))->tv4.v = hton32(v); |
| *length += 5; |
| return 0; |
| } |
| |
| int gtpie_tv8(void *p, unsigned int *length, unsigned int size, uint8_t t, |
| uint64_t v) |
| { |
| if ((*length + 9) >= size) |
| return 1; |
| ((union gtpie_member *)(p + *length))->tv8.t = hton8(t); |
| ((union gtpie_member *)(p + *length))->tv8.v = hton64(v); |
| *length += 9; |
| return 0; |
| } |
| |
| int gtpie_getie(union gtpie_member *ie[], int type, int instance) |
| { |
| int j; |
| for (j = 0; j < GTPIE_SIZE; j++) { |
| if ((ie[j] != 0) && (ie[j]->t == type)) { |
| if (instance-- == 0) |
| return j; |
| } |
| } |
| return -1; |
| } |
| |
| int gtpie_exist(union gtpie_member *ie[], int type, int instance) |
| { |
| int j; |
| for (j = 0; j < GTPIE_SIZE; j++) { |
| if ((ie[j] != 0) && (ie[j]->t == type)) { |
| if (instance-- == 0) |
| return 1; |
| } |
| } |
| return 0; |
| } |
| |
| int gtpie_gettlv(union gtpie_member *ie[], int type, int instance, |
| unsigned int *length, void *dst, unsigned int size) |
| { |
| int ien; |
| ien = gtpie_getie(ie, type, instance); |
| if (ien >= 0) { |
| *length = ntoh16(ie[ien]->tlv.l); |
| if (*length <= size) |
| memcpy(dst, ie[ien]->tlv.v, *length); |
| else |
| return EOF; |
| } |
| return 0; |
| } |
| |
| int gtpie_gettv0(union gtpie_member *ie[], int type, int instance, |
| void *dst, unsigned int size) |
| { |
| int ien; |
| ien = gtpie_getie(ie, type, instance); |
| if (ien >= 0) |
| memcpy(dst, ie[ien]->tv0.v, size); |
| else |
| return EOF; |
| return 0; |
| } |
| |
| int gtpie_gettv1(union gtpie_member *ie[], int type, int instance, |
| uint8_t * dst) |
| { |
| int ien; |
| ien = gtpie_getie(ie, type, instance); |
| if (ien >= 0) |
| *dst = ntoh8(ie[ien]->tv1.v); |
| else |
| return EOF; |
| return 0; |
| } |
| |
| int gtpie_gettv2(union gtpie_member *ie[], int type, int instance, |
| uint16_t * dst) |
| { |
| int ien; |
| ien = gtpie_getie(ie, type, instance); |
| if (ien >= 0) |
| *dst = ntoh16(ie[ien]->tv2.v); |
| else |
| return EOF; |
| return 0; |
| } |
| |
| int gtpie_gettv4(union gtpie_member *ie[], int type, int instance, |
| uint32_t * dst) |
| { |
| int ien; |
| ien = gtpie_getie(ie, type, instance); |
| if (ien >= 0) |
| *dst = ntoh32(ie[ien]->tv4.v); |
| else |
| return EOF; |
| return 0; |
| } |
| |
| int gtpie_gettv8(union gtpie_member *ie[], int type, int instance, |
| uint64_t * dst) |
| { |
| int ien; |
| ien = gtpie_getie(ie, type, instance); |
| if (ien >= 0) |
| *dst = ntoh64(ie[ien]->tv8.v); |
| else |
| return EOF; |
| return 0; |
| } |
| |
| int gtpie_decaps(union gtpie_member *ie[], int version, void *pack, |
| unsigned len) |
| { |
| int i; |
| int j = 0; |
| unsigned char *p; |
| unsigned char *end; |
| |
| end = (unsigned char *)pack + len; |
| p = pack; |
| |
| memset(ie, 0, sizeof(union gtpie_member *) * GTPIE_SIZE); |
| |
| while ((p < end) && (j < GTPIE_SIZE)) { |
| if (GTPIE_DEBUG) { |
| printf("The packet looks like this:\n"); |
| for (i = 0; i < (end - p); i++) { |
| printf("%02x ", |
| (unsigned char)*(char *)(p + i)); |
| if (!((i + 1) % 16)) |
| printf("\n"); |
| }; |
| printf("\n"); |
| } |
| |
| switch (*p) { |
| case GTPIE_CAUSE: /* TV GTPIE types with value length 1 */ |
| case GTPIE_REORDER: |
| case GTPIE_MAP_CAUSE: |
| case GTPIE_MS_VALIDATED: |
| case GTPIE_RECOVERY: |
| case GTPIE_SELECTION_MODE: |
| case GTPIE_TEARDOWN: |
| case GTPIE_NSAPI: |
| case GTPIE_RANAP_CAUSE: |
| case GTPIE_RP_SMS: |
| case GTPIE_RP: |
| case GTPIE_MS_NOT_REACH: |
| case GTPIE_BCM: |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE TV1 found. Type %d, value %d\n", |
| ie[j]->tv1.t, ie[j]->tv1.v); |
| p += 1 + 1; |
| j++; |
| } |
| break; |
| case GTPIE_FL_DI: /* TV GTPIE types with value length 2 or 4 */ |
| case GTPIE_FL_C: |
| if (version != 0) { |
| if (j < GTPIE_SIZE) { /* GTPIE_TEI_DI & GTPIE_TEI_C with length 4 */ |
| /* case GTPIE_TEI_DI: gtp1 */ |
| /* case GTPIE_TEI_C: gtp1 */ |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE TV 4 found. Type %d, value %d\n", |
| ie[j]->tv4.t, |
| ie[j]->tv4.v); |
| p += 1 + 4; |
| j++; |
| } |
| break; |
| } |
| case GTPIE_PFI: /* TV GTPIE types with value length 2 */ |
| case GTPIE_CHARGING_C: |
| case GTPIE_TRACE_REF: |
| case GTPIE_TRACE_TYPE: |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE TV2 found. Type %d, value %d\n", |
| ie[j]->tv2.t, ie[j]->tv2.v); |
| p += 1 + 2; |
| j++; |
| } |
| break; |
| case GTPIE_QOS_PROFILE0: /* TV GTPIE types with value length 3 */ |
| case GTPIE_P_TMSI_S: |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE TV 3 found. Type %d, value %d, %d, %d\n", |
| ie[j]->tv0.t, ie[j]->tv0.v[0], |
| ie[j]->tv0.v[1], ie[j]->tv0.v[2]); |
| p += 1 + 3; |
| j++; |
| } |
| break; |
| case GTPIE_TLLI: /* TV GTPIE types with value length 4 */ |
| case GTPIE_P_TMSI: |
| case GTPIE_CHARGING_ID: |
| /* case GTPIE_TEI_DI: Handled by GTPIE_FL_DI */ |
| /* case GTPIE_TEI_C: Handled by GTPIE_FL_DI */ |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE TV 4 found. Type %d, value %d\n", |
| ie[j]->tv4.t, ie[j]->tv4.v); |
| p += 1 + 4; |
| j++; |
| } |
| break; |
| case GTPIE_TEI_DII: /* TV GTPIE types with value length 5 */ |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf("GTPIE TV 5 found. Type %d\n", |
| ie[j]->tv0.t); |
| p += 1 + 5; |
| j++; |
| } |
| break; |
| case GTPIE_RAB_CONTEXT: /* TV GTPIE types with value length 7 */ |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf("GTPIE TV 7 found. Type %d\n", |
| ie[j]->tv0.t); |
| p += 1 + 7; |
| j++; |
| } |
| break; |
| case GTPIE_IMSI: /* TV GTPIE types with value length 8 */ |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE_IMSI - GTPIE TV 8 found. Type %d, value 0x%llx\n", |
| ie[j]->tv0.t, ie[j]->tv8.v); |
| p += 1 + 8; |
| j++; |
| } |
| break; |
| case GTPIE_RAI: /* TV GTPIE types with value length 6 */ |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE_RAI - GTPIE TV 6 found. Type %d, value 0x%llx\n", |
| ie[j]->tv0.t, ie[j]->tv8.v); |
| p += 1 + 6; |
| j++; |
| } |
| break; |
| case GTPIE_AUTH_TRIPLET: /* TV GTPIE types with value length 28 */ |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf("GTPIE TV 28 found. Type %d\n", |
| ie[j]->tv0.t); |
| p += 1 + 28; |
| j++; |
| } |
| break; |
| case GTPIE_EXT_HEADER_T: /* GTP extension header */ |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf |
| ("GTPIE GTP extension header found. Type %d\n", |
| ie[j]->ext.t); |
| p += 2 + ntoh8(ie[j]->ext.l); |
| j++; |
| } |
| break; |
| case GTPIE_EUA: /* TLV GTPIE types with variable length */ |
| case GTPIE_MM_CONTEXT: |
| case GTPIE_PDP_CONTEXT: |
| case GTPIE_APN: |
| case GTPIE_PCO: |
| case GTPIE_GSN_ADDR: |
| case GTPIE_MSISDN: |
| case GTPIE_QOS_PROFILE: |
| case GTPIE_AUTH_QUINTUP: |
| case GTPIE_TFT: |
| case GTPIE_TARGET_INF: |
| case GTPIE_UTRAN_TRANS: |
| case GTPIE_RAB_SETUP: |
| case GTPIE_TRIGGER_ID: |
| case GTPIE_OMC_ID: |
| case GTPIE_CHARGING_ADDR: |
| case GTPIE_RAT_TYPE: |
| case GTPIE_USER_LOC: |
| case GTPIE_MS_TZ: |
| case GTPIE_IMEI_SV: |
| case GTPIE_PRIVATE: |
| if (j < GTPIE_SIZE) { |
| ie[j] = (union gtpie_member *)p; |
| if (GTPIE_DEBUG) |
| printf("GTPIE TLV found. Type %d\n", |
| ie[j]->tlv.t); |
| p += 3 + ntoh16(ie[j]->tlv.l); |
| j++; |
| } |
| break; |
| default: |
| if (GTPIE_DEBUG) |
| printf("GTPIE something unknown. Type %d\n", |
| *p); |
| return EOF; /* We received something unknown */ |
| } |
| } |
| if (p == end) { |
| if (GTPIE_DEBUG) |
| printf("GTPIE normal return. %lx %lx\n", |
| (unsigned long)p, (unsigned long)end); |
| return 0; /* We landed at the end of the packet: OK */ |
| } else if (!(j < GTPIE_SIZE)) { |
| if (GTPIE_DEBUG) |
| printf("GTPIE too many elements.\n"); |
| return EOF; /* We received too many information elements */ |
| } else { |
| if (GTPIE_DEBUG) |
| printf("GTPIE exceeded end of packet. %lx %lx\n", |
| (unsigned long)p, (unsigned long)end); |
| return EOF; /* We exceeded the end of the packet: Error */ |
| } |
| } |
| |
| int gtpie_encaps(union gtpie_member *ie[], void *pack, unsigned *len) |
| { |
| int i; |
| unsigned char *p; |
| unsigned char *end; |
| int iesize; |
| |
| p = pack; |
| |
| memset(pack, 0, GTPIE_MAX); |
| end = p + GTPIE_MAX; |
| for (i = 1; i < GTPIE_SIZE; i++) |
| if (ie[i] != 0) { |
| if (GTPIE_DEBUG) |
| printf("gtpie_encaps. Type %d\n", i); |
| switch (i) { |
| case GTPIE_CAUSE: /* TV GTPIE types with value length 1 */ |
| case GTPIE_REORDER: |
| case GTPIE_MAP_CAUSE: |
| case GTPIE_MS_VALIDATED: |
| case GTPIE_RECOVERY: |
| case GTPIE_SELECTION_MODE: |
| case GTPIE_TEARDOWN: |
| case GTPIE_NSAPI: |
| case GTPIE_RANAP_CAUSE: |
| case GTPIE_RP_SMS: |
| case GTPIE_RP: |
| case GTPIE_MS_NOT_REACH: |
| case GTPIE_BCM: |
| iesize = 2; |
| break; |
| case GTPIE_FL_DI: /* TV GTPIE types with value length 2 */ |
| case GTPIE_FL_C: |
| case GTPIE_PFI: |
| case GTPIE_CHARGING_C: |
| case GTPIE_TRACE_REF: |
| case GTPIE_TRACE_TYPE: |
| iesize = 3; |
| break; |
| case GTPIE_QOS_PROFILE0: /* TV GTPIE types with value length 3 */ |
| case GTPIE_P_TMSI_S: |
| iesize = 4; |
| break; |
| case GTPIE_TLLI: /* TV GTPIE types with value length 4 */ |
| case GTPIE_P_TMSI: |
| /* case GTPIE_TEI_DI: only in gtp1 */ |
| /* case GTPIE_TEI_C: only in gtp1 */ |
| case GTPIE_CHARGING_ID: |
| iesize = 5; |
| break; |
| case GTPIE_TEI_DII: /* TV GTPIE types with value length 5 */ |
| iesize = 6; |
| break; |
| case GTPIE_RAB_CONTEXT: /* TV GTPIE types with value length 7 */ |
| iesize = 8; |
| break; |
| case GTPIE_IMSI: /* TV GTPIE types with value length 8 */ |
| case GTPIE_RAI: |
| iesize = 9; |
| break; |
| case GTPIE_AUTH_TRIPLET: /* TV GTPIE types with value length 28 */ |
| iesize = 29; |
| break; |
| case GTPIE_EXT_HEADER_T: /* GTP extension header */ |
| iesize = 2 + hton8(ie[i]->ext.l); |
| break; |
| case GTPIE_EUA: /* TLV GTPIE types with length length 2 */ |
| case GTPIE_MM_CONTEXT: |
| case GTPIE_PDP_CONTEXT: |
| case GTPIE_APN: |
| case GTPIE_PCO: |
| case GTPIE_GSN_ADDR: |
| case GTPIE_MSISDN: |
| case GTPIE_QOS_PROFILE: |
| case GTPIE_AUTH_QUINTUP: |
| case GTPIE_TFT: |
| case GTPIE_TARGET_INF: |
| case GTPIE_UTRAN_TRANS: |
| case GTPIE_RAB_SETUP: |
| case GTPIE_TRIGGER_ID: |
| case GTPIE_OMC_ID: |
| case GTPIE_CHARGING_ADDR: |
| case GTPIE_PRIVATE: |
| iesize = 3 + hton16(ie[i]->tlv.l); |
| break; |
| default: |
| return 2; /* We received something unknown */ |
| } |
| if (p + iesize < end) { |
| memcpy(p, ie[i], iesize); |
| p += iesize; |
| *len += iesize; |
| } else |
| return 2; /* Out of space */ |
| } |
| return 0; |
| } |
| |
| int gtpie_encaps2(union gtpie_member ie[], unsigned int size, |
| void *pack, unsigned *len) |
| { |
| unsigned int i, j; |
| unsigned char *p; |
| unsigned char *end; |
| int iesize; |
| |
| p = pack; |
| |
| memset(pack, 0, GTPIE_MAX); |
| end = p + GTPIE_MAX; |
| for (j = 0; j < GTPIE_SIZE; j++) |
| for (i = 0; i < size; i++) |
| if (ie[i].t == j) { |
| if (GTPIE_DEBUG) |
| printf |
| ("gtpie_encaps. Number %d, Type %d\n", |
| i, ie[i].t); |
| switch (ie[i].t) { |
| case GTPIE_CAUSE: /* TV GTPIE types with value length 1 */ |
| case GTPIE_REORDER: |
| case GTPIE_MAP_CAUSE: |
| case GTPIE_MS_VALIDATED: |
| case GTPIE_RECOVERY: |
| case GTPIE_SELECTION_MODE: |
| case GTPIE_TEARDOWN: |
| case GTPIE_NSAPI: |
| case GTPIE_RANAP_CAUSE: |
| case GTPIE_RP_SMS: |
| case GTPIE_RP: |
| case GTPIE_MS_NOT_REACH: |
| case GTPIE_BCM: |
| iesize = 2; |
| break; |
| case GTPIE_PFI: /* TV GTPIE types with value length 2 */ |
| case GTPIE_CHARGING_C: |
| case GTPIE_TRACE_REF: |
| case GTPIE_TRACE_TYPE: |
| iesize = 3; |
| break; |
| case GTPIE_QOS_PROFILE0: /* TV GTPIE types with value length 3 */ |
| case GTPIE_P_TMSI_S: |
| iesize = 4; |
| break; |
| case GTPIE_TLLI: /* TV GTPIE types with value length 4 */ |
| case GTPIE_P_TMSI: |
| case GTPIE_TEI_DI: |
| case GTPIE_TEI_C: |
| iesize = 5; |
| break; |
| case GTPIE_TEI_DII: /* TV GTPIE types with value length 5 */ |
| iesize = 6; |
| break; |
| case GTPIE_RAB_CONTEXT: /* TV GTPIE types with value length 7 */ |
| iesize = 8; |
| break; |
| case GTPIE_IMSI: /* TV GTPIE types with value length 8 */ |
| case GTPIE_RAI: |
| iesize = 9; |
| break; |
| case GTPIE_AUTH_TRIPLET: /* TV GTPIE types with value length 28 */ |
| iesize = 29; |
| break; |
| case GTPIE_EXT_HEADER_T: /* GTP extension header */ |
| iesize = 2 + hton8(ie[i].ext.l); |
| break; |
| case GTPIE_CHARGING_ID: /* TLV GTPIE types with length length 2 */ |
| case GTPIE_EUA: |
| case GTPIE_MM_CONTEXT: |
| case GTPIE_PDP_CONTEXT: |
| case GTPIE_APN: |
| case GTPIE_PCO: |
| case GTPIE_GSN_ADDR: |
| case GTPIE_MSISDN: |
| case GTPIE_QOS_PROFILE: |
| case GTPIE_AUTH_QUINTUP: |
| case GTPIE_TFT: |
| case GTPIE_TARGET_INF: |
| case GTPIE_UTRAN_TRANS: |
| case GTPIE_RAB_SETUP: |
| case GTPIE_TRIGGER_ID: |
| case GTPIE_OMC_ID: |
| case GTPIE_CHARGING_ADDR: |
| case GTPIE_PRIVATE: |
| iesize = 3 + hton16(ie[i].tlv.l); |
| break; |
| default: |
| return 2; /* We received something unknown */ |
| } |
| if (p + iesize < end) { |
| memcpy(p, &ie[i], iesize); |
| p += iesize; |
| *len += iesize; |
| } else |
| return 2; /* Out of space */ |
| } |
| return 0; |
| } |