monitor/ellisys.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2011-2014  Intel Corporation
   7  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *
   9  *
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <stdio.h>
  17 #include <unistd.h>
  18 #include <string.h>
  19 #include <time.h>
  20 #include <sys/time.h>
  21 #include <sys/socket.h>
  22 #include <sys/uio.h>
  23
  24 #include <netdb.h>
  25 #include <arpa/inet.h>
  26
  27 #include "src/shared/btsnoop.h"
  28 #include "ellisys.h"
  29
  30 static int ellisys_fd = -1;
  31 static uint16_t ellisys_index = 0xffff;
  32
  33 void ellisys_enable(const char *server, uint16_t port)
  34 {
  35         struct sockaddr_in addr;
  36         int fd;
  37
  38         if (ellisys_fd >= 0) {
  39                 fprintf(stderr, "Ellisys injection already enabled\n");
  40                 return;
  41         }
  42
  43         fd = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0);
  44         if (fd < 0) {
  45                 perror("Failed to open UDP injection socket");
  46                 return;
  47         }
  48
  49         memset(&addr, 0, sizeof(addr));
  50         addr.sin_family = AF_INET;
  51         addr.sin_addr.s_addr = inet_addr(server);
  52         addr.sin_port = htons(port);
  53
  54         if (connect(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
  55                 perror("Failed to connect UDP injection socket");
  56                 close(fd);
  57                 return;
  58         }
  59
  60         ellisys_fd = fd;
  61 }
  62
  63 void ellisys_inject_hci(struct timeval *tv, uint16_t index, uint16_t opcode,
  64                                         const void *data, uint16_t size)
  65 {
  66         uint8_t msg[] = {
  67                 /* HCI Injection Service, Version 1 */
  68                 0x02, 0x00, 0x01,
  69                 /* DateTimeNs Object */
  70                 0x02, 0x00, 0x00, 0x00, 0x00,
  71                                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  72                 /* Bitrate Object, 12000000 bps */
  73                 0x80, 0x00, 0x1b, 0x37, 0x4b,
  74                 /* HCI Packet Type Object */
  75                 0x81, 0x00,
  76                 /* HCI Packet Data Object */
  77                 0x82
  78         };
  79         long nsec;
  80         time_t t;
  81         struct tm tm;
  82         struct iovec iov[2];
  83         int iovcnt;
  84
  85         if (!tv)
  86                 return;
  87
  88         if (ellisys_fd < 0)
  89                 return;
  90
  91         if (ellisys_index == 0xffff)
  92                 ellisys_index = index;
  93
  94         if (index != ellisys_index)
  95                 return;
  96
  97         t = tv->tv_sec;
  98         localtime_r(&t, &tm);
  99
 100         nsec = ((tm.tm_sec + (tm.tm_min * 60) +
 101                         (tm.tm_hour * 3600)) * 1000000l + tv->tv_usec) * 1000l;
 102
 103         msg[4]  = (1900 + tm.tm_year) & 0xff;
 104         msg[5]  = (1900 + tm.tm_year) >> 8;
 105         msg[6]  = (tm.tm_mon + 1) & 0xff;
 106         msg[7]  = tm.tm_mday & 0xff;
 107         msg[8]  = (nsec & 0x0000000000ffl);
 108         msg[9]  = (nsec & 0x00000000ff00l) >> 8;
 109         msg[10] = (nsec & 0x000000ff0000l) >> 16;
 110         msg[11] = (nsec & 0x0000ff000000l) >> 24;
 111         msg[12] = (nsec & 0x00ff00000000l) >> 32;
 112         msg[13] = (nsec & 0xff0000000000l) >> 40;
 113
 114         switch (opcode) {
 115         case BTSNOOP_OPCODE_COMMAND_PKT:
 116                 msg[20] = 0x01;
 117                 break;
 118         case BTSNOOP_OPCODE_EVENT_PKT:
 119                 msg[20] = 0x84;
 120                 break;
 121         case BTSNOOP_OPCODE_ACL_TX_PKT:
 122                 msg[20] = 0x02;
 123                 break;
 124         case BTSNOOP_OPCODE_ACL_RX_PKT:
 125                 msg[20] = 0x82;
 126                 break;
 127         case BTSNOOP_OPCODE_SCO_TX_PKT:
 128                 msg[20] = 0x03;
 129                 break;
 130         case BTSNOOP_OPCODE_SCO_RX_PKT:
 131                 msg[20] = 0x83;
 132                 break;
 133         case BTSNOOP_OPCODE_ISO_TX_PKT:
 134                 msg[20] = 0x05;
 135                 break;
 136         case BTSNOOP_OPCODE_ISO_RX_PKT:
 137                 msg[20] = 0x85;
 138                 break;
 139         default:
 140                 return;
 141         }
 142
 143         iov[0].iov_base = msg;
 144         iov[0].iov_len  = sizeof(msg);
 145
 146         if (size > 0) {
 147                 iov[1].iov_base = (void *) data;
 148                 iov[1].iov_len  = size;
 149                 iovcnt = 2;
 150         } else
 151                 iovcnt = 1;
 152
 153         if (writev(ellisys_fd, iov, iovcnt) < 0)
 154                 perror("Failed to send Ellisys injection packet");
 155 }