src/gsmtap_util.c

   1 /* GSMTAP support code in libmsomcore */
   2 /*
   3  * (C) 2010-2011 by Harald Welte <laforge@gnumonks.org>
   4  *
   5  * All Rights Reserved
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License along
  18  * with this program; if not, write to the Free Software Foundation, Inc.,
  19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  20  *
  21  */
  22
  23 #include "../config.h"
  24
  25 #include <osmocom/core/gsmtap_util.h>
  26 #include <osmocom/core/logging.h>
  27 #include <osmocom/core/gsmtap.h>
  28 #include <osmocom/core/msgb.h>
  29 #include <osmocom/core/talloc.h>
  30 #include <osmocom/core/select.h>
  31 #include <osmocom/core/socket.h>
  32 #include <osmocom/gsm/protocol/gsm_04_08.h>
  33 #include <osmocom/gsm/rsl.h>
  34
  35 #include <sys/types.h>
  36
  37 #include <arpa/inet.h>
  38
  39 #include <stdio.h>
  40 #include <unistd.h>
  41 #include <stdint.h>
  42 #include <string.h>
  43 #include <errno.h>
  44
  45 uint8_t chantype_rsl2gsmtap(uint8_t rsl_chantype, uint8_t link_id)
  46 {
  47         uint8_t ret = GSMTAP_CHANNEL_UNKNOWN;
  48
  49         switch (rsl_chantype) {
  50         case RSL_CHAN_Bm_ACCHs:
  51                 ret = GSMTAP_CHANNEL_TCH_F;
  52                 break;
  53         case RSL_CHAN_Lm_ACCHs:
  54                 ret = GSMTAP_CHANNEL_TCH_H;
  55                 break;
  56         case RSL_CHAN_SDCCH4_ACCH:
  57                 ret = GSMTAP_CHANNEL_SDCCH4;
  58                 break;
  59         case RSL_CHAN_SDCCH8_ACCH:
  60                 ret = GSMTAP_CHANNEL_SDCCH8;
  61                 break;
  62         case RSL_CHAN_BCCH:
  63                 ret = GSMTAP_CHANNEL_BCCH;
  64                 break;
  65         case RSL_CHAN_RACH:
  66                 ret = GSMTAP_CHANNEL_RACH;
  67                 break;
  68         case RSL_CHAN_PCH_AGCH:
  69                 /* it could also be AGCH... */
  70                 ret = GSMTAP_CHANNEL_PCH;
  71                 break;
  72         }
  73
  74         if (link_id & 0x40)
  75                 ret |= GSMTAP_CHANNEL_ACCH;
  76
  77         return ret;
  78 }
  79
  80 /* receive a message from L1/L2 and put it in GSMTAP */
  81 struct msgb *gsmtap_makemsg(uint16_t arfcn, uint8_t ts, uint8_t chan_type,
  82                             uint8_t ss, uint32_t fn, int8_t signal_dbm,
  83                             uint8_t snr, const uint8_t *data, unsigned int len)
  84 {
  85         struct msgb *msg;
  86         struct gsmtap_hdr *gh;
  87         uint8_t *dst;
  88
  89         msg = msgb_alloc(sizeof(*gh) + len, "gsmtap_tx");
  90         if (!msg)
  91                 return NULL;
  92
  93         gh = (struct gsmtap_hdr *) msgb_put(msg, sizeof(*gh));
  94
  95         gh->version = GSMTAP_VERSION;
  96         gh->hdr_len = sizeof(*gh)/4;
  97         gh->type = GSMTAP_TYPE_UM;
  98         gh->timeslot = ts;
  99         gh->sub_slot = ss;
 100         gh->arfcn = htons(arfcn);
 101         gh->snr_db = snr;
 102         gh->signal_dbm = signal_dbm;
 103         gh->frame_number = htonl(fn);
 104         gh->sub_type = chan_type;
 105         gh->antenna_nr = 0;
 106
 107         dst = msgb_put(msg, len);
 108         memcpy(dst, data, len);
 109
 110         return msg;
 111 }
 112
 113 #ifdef HAVE_SYS_SOCKET_H
 114
 115 #include <sys/socket.h>
 116 #include <netinet/in.h>
 117
 118 /* Open a GSMTAP source (sending) socket, conncet it to host/port and
 119  * return resulting fd */
 120 int gsmtap_source_init_fd(const char *host, uint16_t port)
 121 {
 122         if (port == 0)
 123                 port = GSMTAP_UDP_PORT;
 124         if (host == NULL)
 125                 host = "localhost";
 126
 127         return osmo_sock_init(AF_UNSPEC, SOCK_DGRAM, IPPROTO_UDP, host, port,
 128                                 OSMO_SOCK_F_CONNECT);
 129 }
 130
 131 int gsmtap_source_add_sink_fd(int gsmtap_fd)
 132 {
 133         struct sockaddr_storage ss;
 134         socklen_t ss_len = sizeof(ss);
 135         int rc;
 136
 137         rc = getpeername(gsmtap_fd, (struct sockaddr *)&ss, &ss_len);
 138         if (rc < 0)
 139                 return rc;
 140
 141         if (osmo_sockaddr_is_local((struct sockaddr *)&ss, ss_len) == 1) {
 142                 rc = osmo_sock_init_sa((struct sockaddr *)&ss, SOCK_DGRAM,
 143                                         IPPROTO_UDP, OSMO_SOCK_F_BIND);
 144                 if (rc >= 0)
 145                         return rc;
 146         }
 147
 148         return -ENODEV;
 149 }
 150
 151 int gsmtap_sendmsg(struct gsmtap_inst *gti, struct msgb *msg)
 152 {
 153         if (!gti)
 154                 return -ENODEV;
 155
 156         if (gti->ofd_wq_mode)
 157                 return osmo_wqueue_enqueue(&gti->wq, msg);
 158         else {
 159                 /* try immediate send and return error if any */
 160                 int rc;
 161
 162                 rc = write(gsmtap_inst_fd(gti), msg->data, msg->len);
 163                 if (rc <= 0) {
 164                         return rc;
 165                 } else if (rc >= msg->len) {
 166                         msgb_free(msg);
 167                         return 0;
 168                 } else {
 169                         /* short write */
 170                         return -EIO;
 171                 }
 172         }
 173 }
 174
 175 /* receive a message from L1/L2 and put it in GSMTAP */
 176 int gsmtap_send(struct gsmtap_inst *gti, uint16_t arfcn, uint8_t ts,
 177                 uint8_t chan_type, uint8_t ss, uint32_t fn,
 178                 int8_t signal_dbm, uint8_t snr, const uint8_t *data,
 179                 unsigned int len)
 180 {
 181         struct msgb *msg;
 182
 183         if (!gti)
 184                 return -ENODEV;
 185
 186         msg = gsmtap_makemsg(arfcn, ts, chan_type, ss, fn, signal_dbm,
 187                              snr, data, len);
 188         if (!msg)
 189                 return -ENOMEM;
 190
 191         return gsmtap_sendmsg(gti, msg);
 192 }
 193
 194 /* Callback from select layer if we can write to the socket */
 195 static int gsmtap_wq_w_cb(struct osmo_fd *ofd, struct msgb *msg)
 196 {
 197         int rc;
 198
 199         rc = write(ofd->fd, msg->data, msg->len);
 200         if (rc < 0) {
 201                 perror("writing msgb to gsmtap fd");
 202                 return rc;
 203         }
 204         if (rc != msg->len) {
 205                 perror("short write to gsmtap fd");
 206                 return -EIO;
 207         }
 208
 209         return 0;
 210 }
 211
 212 /* Callback from select layer if we can read from the sink socket */
 213 static int gsmtap_sink_fd_cb(struct osmo_fd *fd, unsigned int flags)
 214 {
 215         int rc;
 216         uint8_t buf[4096];
 217
 218         if (!(flags & BSC_FD_READ))
 219                 return 0;
 220
 221         rc = read(fd->fd, buf, sizeof(buf));
 222         if (rc < 0) {
 223                 perror("reading from gsmtap sink fd");
 224                 return rc;
 225         }
 226         /* simply discard any data arriving on the socket */
 227
 228         return 0;
 229 }
 230
 231 /* Add a local sink to an existing GSMTAP source instance */
 232 int gsmtap_source_add_sink(struct gsmtap_inst *gti)
 233 {
 234         int fd;
 235
 236         fd = gsmtap_source_add_sink_fd(gsmtap_inst_fd(gti));
 237         if (fd < 0)
 238                 return fd;
 239
 240         if (gti->ofd_wq_mode) {
 241                 struct osmo_fd *sink_ofd;
 242
 243                 sink_ofd = &gti->sink_ofd;
 244                 sink_ofd->fd = fd;
 245                 sink_ofd->when = BSC_FD_READ;
 246                 sink_ofd->cb = gsmtap_sink_fd_cb;
 247
 248                 osmo_fd_register(sink_ofd);
 249         }
 250
 251         return fd;
 252 }
 253
 254 /* like gsmtap_init2() but integrated with libosmocore select.c */
 255 struct gsmtap_inst *gsmtap_source_init(const char *host, uint16_t port,
 256                                         int ofd_wq_mode)
 257 {
 258         struct gsmtap_inst *gti;
 259         int fd;
 260
 261         fd = gsmtap_source_init_fd(host, port);
 262         if (fd < 0)
 263                 return NULL;
 264
 265         gti = talloc_zero(NULL, struct gsmtap_inst);
 266         gti->ofd_wq_mode = ofd_wq_mode;
 267         gti->wq.bfd.fd = fd;
 268         gti->sink_ofd.fd = -1;
 269
 270         if (ofd_wq_mode) {
 271                 osmo_wqueue_init(&gti->wq, 64);
 272                 gti->wq.write_cb = &gsmtap_wq_w_cb;
 273
 274                 osmo_fd_register(&gti->wq.bfd);
 275         }
 276
 277         return gti;
 278 }
 279
 280 #endif /* HAVE_SYS_SOCKET_H */