You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
433 lines
9.8 KiB
433 lines
9.8 KiB
/*
|
|
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2016
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
#include "trx_dv.h"
|
|
#include <eth_ar/eth_ar.h>
|
|
#include <dml/dml.h>
|
|
#include "alaw.h"
|
|
#include <eth_ar/ulaw.h>
|
|
|
|
#include <arpa/inet.h>
|
|
#include <linux/if_packet.h>
|
|
#include <net/ethernet.h>
|
|
#include <net/if.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
|
|
#include <codec2/codec2.h>
|
|
|
|
static int dv_sock = -1;
|
|
static GIOChannel *dv_io = NULL;
|
|
static char *dv_dev = NULL;
|
|
static uint8_t dv_mac[6] = { 0 };
|
|
static void (*dv_mac_cb)(uint8_t *mac) = NULL;
|
|
|
|
#define TRX_DV_WATCHDOG 5
|
|
|
|
static int (*in_cb)(void *arg, uint8_t from[6], uint8_t to[6], uint8_t *dv, size_t size, int mode, uint8_t level) = NULL;
|
|
static int (*ctrl_cb)(void *arg, uint8_t from[6], uint8_t to[6], char *ctrl, size_t size) = NULL;
|
|
static int (*fprs_cb)(void *arg, uint8_t from[6], uint8_t *fprs, size_t size) = NULL;
|
|
static void *in_cb_arg = NULL;
|
|
|
|
static gboolean trx_dv_in_cb(GIOChannel *source, GIOCondition condition, gpointer arg)
|
|
{
|
|
uint8_t dv_frame[6 + 6 + 2 + 1500];
|
|
ssize_t ret;
|
|
|
|
ret = recv(dv_sock, dv_frame, sizeof(dv_frame), 0);
|
|
if (ret >= 16) {
|
|
uint16_t type = (dv_frame[12] << 8) | dv_frame[13];
|
|
int mode;
|
|
size_t datasize = ret - 16;
|
|
switch (type) {
|
|
case ETH_P_CODEC2_3200:
|
|
mode = CODEC2_MODE_3200;
|
|
break;
|
|
case ETH_P_CODEC2_2400:
|
|
mode = CODEC2_MODE_2400;
|
|
break;
|
|
case ETH_P_CODEC2_1600:
|
|
mode = CODEC2_MODE_1600;
|
|
break;
|
|
case ETH_P_CODEC2_1400:
|
|
mode = CODEC2_MODE_1400;
|
|
break;
|
|
case ETH_P_CODEC2_1300:
|
|
mode = CODEC2_MODE_1300;
|
|
break;
|
|
case ETH_P_CODEC2_1200:
|
|
mode = CODEC2_MODE_1200;
|
|
break;
|
|
#if defined(CODEC2_MODE_700)
|
|
case ETH_P_CODEC2_700:
|
|
mode = CODEC2_MODE_700;
|
|
break;
|
|
#endif
|
|
#if defined(CODEC2_MODE_700B)
|
|
case ETH_P_CODEC2_700B:
|
|
mode = CODEC2_MODE_700B;
|
|
break;
|
|
#endif
|
|
case ETH_P_CODEC2_700C:
|
|
mode = CODEC2_MODE_700C;
|
|
break;
|
|
#if defined(CODEC2_MODE_WB)
|
|
case ETH_P_CODEC2_WB:
|
|
mode = CODEC2_MODE_WB;
|
|
break;
|
|
#endif
|
|
case ETH_P_CODEC2_450:
|
|
mode = CODEC2_MODE_450;
|
|
break;
|
|
case ETH_P_CODEC2_450PWB:
|
|
mode = CODEC2_MODE_450PWB;
|
|
break;
|
|
case ETH_P_ALAW:
|
|
mode = 'A';
|
|
break;
|
|
case ETH_P_ULAW:
|
|
mode = 'U';
|
|
break;
|
|
case ETH_P_LE16:
|
|
mode = 's';
|
|
break;
|
|
case ETH_P_BE16:
|
|
mode = 'S';
|
|
break;
|
|
case ETH_P_AR_CONTROL:
|
|
ctrl_cb(in_cb_arg, dv_frame + 6, dv_frame, (char *)dv_frame + 16, ret - 16);
|
|
return TRUE;
|
|
case ETH_P_FPRS:
|
|
fprs_cb(in_cb_arg, dv_frame + 6, dv_frame + 14, ret - 14);
|
|
return TRUE;
|
|
default:
|
|
return TRUE;
|
|
}
|
|
if (ret >= datasize + 14) {
|
|
in_cb(in_cb_arg, dv_frame + 6, dv_frame, dv_frame + 16, datasize, mode, dv_frame[15]);
|
|
}
|
|
} else {
|
|
printf("frame not the right size: %zd: \n", ret);
|
|
int i;
|
|
for (i = 0; i < ret; i++) {
|
|
printf("%02x ", dv_frame[i]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
int trx_dv_send(uint8_t from[6], uint8_t to[6], int mode, uint8_t *dv, size_t size, uint8_t level)
|
|
{
|
|
uint16_t type;
|
|
ssize_t max_size = 0;
|
|
|
|
switch (mode) {
|
|
case CODEC2_MODE_3200:
|
|
type = htons(ETH_P_CODEC2_3200);
|
|
max_size = 8;
|
|
break;
|
|
case CODEC2_MODE_2400:
|
|
type = htons(ETH_P_CODEC2_2400);
|
|
max_size = 6;
|
|
break;
|
|
case CODEC2_MODE_1600:
|
|
type = htons(ETH_P_CODEC2_1600);
|
|
max_size = 8;
|
|
break;
|
|
case CODEC2_MODE_1400:
|
|
type = htons(ETH_P_CODEC2_1400);
|
|
max_size = 7;
|
|
break;
|
|
case CODEC2_MODE_1300:
|
|
type = htons(ETH_P_CODEC2_1300);
|
|
max_size = 7;
|
|
break;
|
|
case CODEC2_MODE_1200:
|
|
type = htons(ETH_P_CODEC2_1200);
|
|
max_size = 6;
|
|
break;
|
|
#if defined(CODEC2_MODE_700)
|
|
case CODEC2_MODE_700:
|
|
type = htons(ETH_P_CODEC2_700);
|
|
max_size = 4;
|
|
break;
|
|
#endif
|
|
#if defined(CODEC2_MODE_700B)
|
|
case CODEC2_MODE_700B:
|
|
type = htons(ETH_P_CODEC2_700B);
|
|
max_size = 4;
|
|
break;
|
|
#endif
|
|
case CODEC2_MODE_700C:
|
|
type = htons(ETH_P_CODEC2_700C);
|
|
max_size = 4;
|
|
break;
|
|
#if defined(CODEC2_MODE_WB)
|
|
case CODEC2_MODE_WB:
|
|
type = htons(ETH_P_CODEC2_WB);
|
|
max_size = 8;
|
|
break;
|
|
#endif
|
|
case CODEC2_MODE_450:
|
|
type = htons(ETH_P_CODEC2_450);
|
|
max_size = 3;
|
|
break;
|
|
case CODEC2_MODE_450PWB:
|
|
type = htons(ETH_P_CODEC2_450PWB);
|
|
max_size = 3;
|
|
break;
|
|
case 'A':
|
|
type = htons(ETH_P_ALAW);
|
|
max_size = 320;
|
|
break;
|
|
case 'U':
|
|
type = htons(ETH_P_ULAW);
|
|
max_size = 320;
|
|
break;
|
|
case 's':
|
|
type = htons(ETH_P_LE16);
|
|
max_size = 640;
|
|
break;
|
|
case 'S':
|
|
type = htons(ETH_P_BE16);
|
|
max_size = 640;
|
|
break;
|
|
default:
|
|
return -1;
|
|
}
|
|
|
|
while (size) {
|
|
uint8_t dv_frame[6 + 6 + 2 + 1 + 1 + max_size];
|
|
size_t out_size = size;
|
|
if (out_size > max_size)
|
|
out_size = max_size;
|
|
memcpy(dv_frame + 0, to, 6);
|
|
memcpy(dv_frame + 6, from, 6);
|
|
memcpy(dv_frame + 12, &type, 2);
|
|
dv_frame[14] = from[0] ^ from[1] ^ from[2] ^ from[3] ^ from[4] ^ from[5];
|
|
dv_frame[15] = level;
|
|
memcpy(dv_frame + 16, dv, out_size);
|
|
|
|
ssize_t ret = send(dv_sock, dv_frame, 16 + out_size, 0);
|
|
if (ret == 16 + out_size) {
|
|
size -= out_size;
|
|
dv += out_size;
|
|
}
|
|
}
|
|
if (size == 0)
|
|
return 0;
|
|
|
|
return -1;
|
|
}
|
|
|
|
int trx_dv_send_control(uint8_t from[6], uint8_t to[6], char *control)
|
|
{
|
|
size_t control_size = strlen(control);
|
|
uint16_t type = htons(ETH_P_AR_CONTROL);
|
|
|
|
uint8_t dv_frame[6 + 6 + 2 + 1 + 1 + control_size];
|
|
memcpy(dv_frame + 0, to, 6);
|
|
memcpy(dv_frame + 6, from, 6);
|
|
memcpy(dv_frame + 12, &type, 2);
|
|
dv_frame[14] = 0;
|
|
dv_frame[15] = 1;
|
|
memcpy(dv_frame + 16, control, control_size);
|
|
|
|
ssize_t ret = send(dv_sock, dv_frame, 16 + control_size, 0);
|
|
if (ret == 14 + control_size)
|
|
return 0;
|
|
|
|
return -1;
|
|
}
|
|
|
|
int trx_dv_send_fprs(uint8_t from[6], uint8_t to[6], uint8_t *data, size_t size)
|
|
{
|
|
uint16_t type = htons(ETH_P_FPRS);
|
|
|
|
uint8_t dv_frame[6 + 6 + 2 + size];
|
|
memcpy(dv_frame + 0, to, 6);
|
|
memcpy(dv_frame + 6, from, 6);
|
|
memcpy(dv_frame + 12, &type, 2);
|
|
memcpy(dv_frame + 14, data, size);
|
|
|
|
ssize_t ret = send(dv_sock, dv_frame, 14 + size, 0);
|
|
if (ret == 14 + size)
|
|
return 0;
|
|
|
|
return -1;
|
|
}
|
|
|
|
int trx_dv_duration(size_t size, int mode)
|
|
{
|
|
switch (mode) {
|
|
case CODEC2_MODE_3200:
|
|
return (size * 20) / 8;
|
|
case CODEC2_MODE_2400:
|
|
return (size * 20) / 6;
|
|
case CODEC2_MODE_1600:
|
|
return (size * 40) / 8;
|
|
case CODEC2_MODE_1400:
|
|
return (size * 40) / 7;
|
|
case CODEC2_MODE_1300:
|
|
return (size * 40) / 7;
|
|
case CODEC2_MODE_1200:
|
|
return (size * 40) / 6;
|
|
#if defined(CODEC2_MODE_700)
|
|
case CODEC2_MODE_700:
|
|
return (size * 40) / 4;
|
|
#endif
|
|
#if defined(CODEC2_MODE_700B)
|
|
case CODEC2_MODE_700B:
|
|
return (size * 40) / 4;
|
|
#endif
|
|
case CODEC2_MODE_700C:
|
|
return (size * 40) / 4;
|
|
case CODEC2_MODE_450:
|
|
return (size * 40) / 3;
|
|
case CODEC2_MODE_450PWB:
|
|
return (size * 40) / 3;
|
|
#if defined(CODEC2_MODE_WB)
|
|
case CODEC2_MODE_WB:
|
|
return (size * 20) / 8;
|
|
#endif
|
|
case 'A':
|
|
case 'U':
|
|
return size / 8;
|
|
case 's':
|
|
case 'S':
|
|
return size / 16;
|
|
default:
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
static int trx_dv_bind_if(void)
|
|
{
|
|
short protocol = htons(ETH_P_ALL);
|
|
uint8_t mac[6];
|
|
struct ifreq ifr;
|
|
|
|
size_t if_name_len = strlen(dv_dev);
|
|
if (if_name_len >= sizeof(ifr.ifr_name))
|
|
goto err_len;
|
|
strcpy(ifr.ifr_name, dv_dev);
|
|
|
|
if (ioctl(dv_sock, SIOCGIFINDEX, &ifr) < 0)
|
|
goto err_ioctl;
|
|
|
|
int ifindex = ifr.ifr_ifindex;
|
|
|
|
struct sockaddr_ll sll = { 0 };
|
|
|
|
sll.sll_family = AF_PACKET;
|
|
sll.sll_ifindex = ifindex;
|
|
sll.sll_protocol = protocol;
|
|
if(bind(dv_sock, (struct sockaddr *)&sll , sizeof(sll)) < 0)
|
|
goto err_bind;
|
|
|
|
|
|
struct ifreq if_mac;
|
|
|
|
memset(&if_mac, 0, sizeof(struct ifreq));
|
|
strcpy(if_mac.ifr_name, dv_dev);
|
|
if (ioctl(dv_sock, SIOCGIFHWADDR, &if_mac) < 0)
|
|
goto err_ioctl_mac;
|
|
memcpy(mac, (uint8_t *)&if_mac.ifr_hwaddr.sa_data, 6);
|
|
|
|
if (memcmp(mac, dv_mac, 6)) {
|
|
memcpy(dv_mac, mac, 6);
|
|
dv_mac_cb(dv_mac);
|
|
}
|
|
|
|
return 0;
|
|
|
|
err_ioctl_mac:
|
|
err_len:
|
|
err_ioctl:
|
|
err_bind:
|
|
return -1;
|
|
}
|
|
|
|
static bool dv_bound = false;
|
|
|
|
static gboolean trx_dv_watchdog(void *arg)
|
|
{
|
|
bool bound = !trx_dv_bind_if();
|
|
|
|
if (!bound && dv_bound) {
|
|
printf("Lost interface\n");
|
|
|
|
g_source_remove_by_user_data(trx_dv_init);
|
|
}
|
|
if (bound && !dv_bound) {
|
|
printf("Bound to interface\n");
|
|
g_io_add_watch(dv_io, G_IO_IN, trx_dv_in_cb, trx_dv_init);
|
|
}
|
|
dv_bound = bound;
|
|
|
|
g_timeout_add_seconds(TRX_DV_WATCHDOG, trx_dv_watchdog, trx_dv_init);
|
|
return G_SOURCE_REMOVE;
|
|
}
|
|
|
|
int trx_dv_init(char *dev,
|
|
int (*new_in_cb)(void *arg, uint8_t from[6], uint8_t to[6], uint8_t *dv, size_t size, int mode, uint8_t level),
|
|
int (*new_ctrl_cb)(void *arg, uint8_t from[6], uint8_t to[6], char *ctrl, size_t size),
|
|
int (*new_fprs_cb)(void *arg, uint8_t from[6], uint8_t *fprs, size_t size),
|
|
void *arg,
|
|
void (*new_mac_cb)(uint8_t *mac))
|
|
{
|
|
int sock;
|
|
short protocol = htons(ETH_P_ALL);
|
|
|
|
free(dv_dev);
|
|
dv_dev = strdup(dev);
|
|
|
|
in_cb = new_in_cb;
|
|
in_cb_arg = arg;
|
|
ctrl_cb = new_ctrl_cb;
|
|
fprs_cb = new_fprs_cb;
|
|
dv_mac_cb = new_mac_cb;
|
|
|
|
sock = socket(AF_PACKET, SOCK_RAW, protocol);
|
|
if (sock < 0)
|
|
goto err_socket;
|
|
|
|
dv_sock = sock;
|
|
|
|
if (trx_dv_bind_if()) {
|
|
printf("Failed to connect to interface, trying again later\n");
|
|
dv_bound = false;
|
|
|
|
g_source_remove_by_user_data(trx_dv_init);
|
|
} else {
|
|
dv_bound = true;
|
|
dv_io = g_io_channel_unix_new(sock);
|
|
g_io_channel_set_encoding(dv_io, NULL, NULL);
|
|
g_io_add_watch(dv_io, G_IO_IN, trx_dv_in_cb, trx_dv_init);
|
|
}
|
|
g_timeout_add_seconds(TRX_DV_WATCHDOG, trx_dv_watchdog, trx_dv_init);
|
|
|
|
return 0;
|
|
|
|
err_socket:
|
|
return -1;
|
|
}
|