static void hci_status_param(uint16_t ocf, int plen, uint8_t *data)
{
read_local_amp_info_rp ai;
const uint16_t ogf = OGF_STATUS_PARAM;
switch (ocf) {
case OCF_READ_LOCAL_AMP_INFO:
memset(&ai, 0, sizeof(ai));
/* BT only */
ai.amp_status = 0x01;
ai.max_pdu_size = htobl(L2CAP_DEFAULT_MTU);
ai.controller_type = HCI_AMP;
ai.max_amp_assoc_length = htobl(HCI_MAX_ACL_SIZE);
/* No flushing at all */
ai.max_flush_timeout = 0xFFFFFFFF;
ai.best_effort_flush_timeout = 0xFFFFFFFF;
command_complete(ogf, ocf, sizeof(ai), &ai);
break;
default:
command_status(ogf, ocf, 0x01);
break;
}
}
u8archive* U8_packdir(u8archive * arc, U8Dir dir, u32 recursion)
{
u32 i, oldnumnodes;
U8File curfile;
U8Dir curdir;
u8node * curnode;
oldnumnodes = arc->numnodes; //save the old number of nodes, for updating the directory node later
arc->nodes = realloc(arc->nodes, (arc->numnodes + 1) * sizeof(u8node)); //expand buffer
curnode = &arc->nodes[arc->numnodes]; //get pointer to node to work with
curnode->type = 0x0001; // Is a folder.
curnode->string_offset = htobl(arc->stringsz); //save string table offset
arc->string_table = realloc(arc->string_table, arc->stringsz + strlen(curdir.name) + 1); //expand sting buffer
memcpy(arc->string_table, curdir.name, strlen(curdir.name) + arc->stringsz + 1); //copy over string
arc->stringsz += strlen(curdir.name) + 1;
arc->numnodes++; //we added another node
for(i = 0; i < dir.filecnt; i++) { //Handle files.
curfile = dir.files[i];
arc->nodes = realloc(arc->nodes, (arc->numnodes + 1) * sizeof(u8node)); //expand buffer
curnode = &arc->nodes[arc->numnodes]; //get pointer to node to work with
curnode->type = 0; //file
curnode->size = htobl(curfile.size); //save file size
curnode->data_offset = htobl(arc->datasz); //store data offset (updated later)
curnode->string_offset = htobl(arc->stringsz); //save string table offset
arc->string_table = realloc(arc->string_table, arc->stringsz + strlen(curfile.dst) + 1); //expand sting buffer
memcpy(arc->string_table + arc->stringsz, curfile.dst, strlen(curfile.dst) + 1); //copy over string
arc->stringsz += strlen(curfile.dst) + 1;
arc->data = realloc(arc->data, arc->datasz + inlinepad(curfile.size, 32)); //expand data buffer
memset(arc->data + arc->datasz, 0, inlinepad(curfile.size, 32)); //clear buffer
memcpy(arc->data + arc->datasz, curfile.data, curfile.size); //copy over data
arc->datasz += inlinepad(curfile.size, 32);
arc->numnodes++; //we added another node
}
for(i = 0; i < dir.dircnt; i++) { //Handle folders.
curdir = dir.dirs[i]; //get the dir to work with
arc = U8_packdir(arc, curdir, ++recursion); //recursive call
}
arc->nodes[oldnumnodes].size = htobl(arc->numnodes) + 1; //add one for root node
return arc; //for the calling function
}
tBleStatus aci_updater_calc_crc(uint32_t address,
uint8_t num_sectors,
uint32_t *crc)
{
struct hci_request rq;
updater_calc_crc_cp cp;
updater_calc_crc_rp resp;
memset(&resp, 0, sizeof(resp));
cp.address = htobl(address);
cp.num_sectors = num_sectors;
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = OCF_UPDATER_CALC_CRC;
rq.cparam = &cp;
rq.clen = UPDATER_CALC_CRC_CP_SIZE;
rq.rparam = &resp;
rq.rlen = UPDATER_CALC_CRC_RP_SIZE;
if (hci_send_req(&rq) < 0)
return -1;
*crc = btohl(resp.crc);
return resp.status;
}
tBleStatus aci_updater_read_data_block(uint32_t address,
uint16_t data_len,
uint8_t *data)
{
struct hci_request rq;
updater_read_data_block_cp cp;
uint8_t buffer[HCI_MAX_PACKET_SIZE];
if((data_len+1) > HCI_MAX_PACKET_SIZE)
return BLE_STATUS_INVALID_PARAMS;
cp.address = htobl(address);
cp.data_len = htobs(data_len);
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = OCF_UPDATER_READ_DATA_BLOCK;
rq.cparam = &cp;
rq.clen = UPDATER_READ_DATA_BLOCK_CP_SIZE;
rq.rparam = buffer;
rq.rlen = data_len + 1;
if (hci_send_req(&rq) < 0)
return -1;
// First byte is status
memcpy(data, buffer+1, data_len);
return buffer[0];
}
IMD5* LaichMeta_IMD5(u8* file, u32 size)
{
IMD5* header = malloc(sizeof(IMD5));
memset(header, 0, sizeof(IMD5));
header->imd5[0] = 'I';
header->imd5[1] = 'M';
header->imd5[2] = 'D';
header->imd5[3] = '5';
header->filesize = htobl(size);
//MD5(file, size, header->crypto);
return header;
}
IMET* LaichMeta_IMET(char** names, \
u32 iconsize, u32 bannersize, u32 soundsize)
{
IMET* header = malloc(sizeof(IMET));
memset(header, 0, sizeof(IMET));
header->imet[0] = 'I';
header->imet[1] = 'M';
header->imet[2] = 'E';
header->imet[3] = 'T';
header->unk = htobll(0x0000060000000003);
header->sizes[0] = htobl(iconsize);
header->sizes[1] = htobl(bannersize);
header->sizes[2] = htobl(soundsize);
header->flag1 = 0x00;
int i;
for(i = 0; i < 8; i++)
memcpy(header->names[i], names[i], 2 * 42);
//MD5(((u8*)header) + 0x40, 0x600, (u8*)header->crypto);
return header;
}
u8* LaichMeta_CompileU8(U8* input, u32* size)
{
u8archive* arc;
u32 offset = 0, data_offset, i;
u8* outbuf;
arc = calloc(1, sizeof(u8archive));
arc->string_table = calloc(1, 1); //Initial NULL
arc->stringsz = 1; //Initial NULL
arc->data = NULL;
arc->datasz = 0;
arc->nodes = NULL;
arc->numnodes = 0;
arc = U8_packdir(arc, input->rootdir, 0);
arc->rootnode.type = htobs(0x0100);
arc->rootnode.string_offset = 0;
arc->rootnode.data_offset = 0;
arc->rootnode.size = htobl(arc->numnodes);
arc->header.tag = htobl(0x55AA382D);
arc->header.root_offset = htobl(0x20);
arc->header.header_size = htobl((sizeof(u8node) * arc->numnodes) + arc->stringsz);
data_offset = inlinepad(0x20 + (sizeof(u8node) * arc->numnodes) + arc->stringsz, 0x40);
arc->header.data_offset = htobl(data_offset);
memset(arc->header.zeroes, 0, 16);
for(i = 0; i < arc->numnodes; i++) // fix the data offset
if (htobs(arc->nodes[i].type) == 0) // Checking if it is a file
arc->nodes[i].data_offset = htobl(btohl(arc->nodes[i].data_offset) + data_offset);
outbuf = calloc(1, sizeof(u8header) + (sizeof(u8node) * (arc->numnodes + 1)) + arc->stringsz + arc->datasz);
memcpy(outbuf + offset, &arc->header, sizeof(u8header));
offset += sizeof(u8header);
memcpy(outbuf + offset, &arc->rootnode, sizeof(u8node));
offset += sizeof(u8node);
memcpy(outbuf + offset, arc->nodes, sizeof(u8node) * arc->numnodes);
offset += sizeof(u8node) * arc->numnodes;
memcpy(outbuf + offset, arc->string_table, arc->stringsz);
offset += arc->stringsz;
memcpy(outbuf + offset, arc->data, arc->datasz);
offset += arc->datasz;
*size = offset;
free(arc);
return outbuf;
}
tBleStatus aci_updater_erase_sector(uint32_t address)
{
struct hci_request rq;
updater_erase_sector_cp cp;
uint8_t status;
cp.address = htobl(address);
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = OCF_UPDATER_ERASE_SECTOR;
rq.cparam = &cp;
rq.clen = UPDATER_ERASE_SECTOR_CP_SIZE;
rq.rparam = &status;
rq.rlen = 1;
if (hci_send_req(&rq) < 0)
return -1;
return status;
}
static void send_mode(char *svr)
{
struct sco_options so;
socklen_t len;
uint32_t seq;
int i, sk;
if ((sk = do_connect(svr)) < 0) {
syslog(LOG_ERR, "Can't connect to the server: %s (%d)",
strerror(errno), errno);
exit(1);
}
len = sizeof(so);
if (getsockopt(sk, SOL_SCO, SCO_OPTIONS, &so, &len) < 0) {
syslog(LOG_ERR, "Can't get SCO options: %s (%d)",
strerror(errno), errno);
exit(1);
}
syslog(LOG_INFO,"Sending ...");
for (i = 6; i < so.mtu; i++)
buf[i] = 0x7f;
seq = 0;
while (1) {
*(uint32_t *) buf = htobl(seq);
*(uint16_t *) (buf + 4) = htobs(data_size);
seq++;
if (send(sk, buf, so.mtu, 0) <= 0) {
syslog(LOG_ERR, "Send failed: %s (%d)",
strerror(errno), errno);
exit(1);
}
usleep(1);
}
}
tBleStatus aci_gap_set_auth_requirement(uint8_t mitm_mode,
uint8_t oob_enable,
uint8_t oob_data[16],
uint8_t min_encryption_key_size,
uint8_t max_encryption_key_size,
uint8_t use_fixed_pin,
uint32_t fixed_pin,
uint8_t bonding_mode)
{
struct hci_request rq;
gap_set_auth_requirement_cp cp;
uint8_t status;
cp.mitm_mode = mitm_mode;
cp.oob_enable = oob_enable;
Osal_MemCpy(cp.oob_data, oob_data, 16);
cp.min_encryption_key_size = min_encryption_key_size;
cp.max_encryption_key_size = max_encryption_key_size;
cp.use_fixed_pin = use_fixed_pin;
cp.fixed_pin = htobl(fixed_pin);
cp.bonding_mode = bonding_mode;
Osal_MemSet(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = OCF_GAP_SET_AUTH_REQUIREMENT;
rq.cparam = &cp;
rq.clen = sizeof(cp);
rq.rparam = &status;
rq.rlen = 1;
if (hci_send_req(&rq, FALSE) < 0)
return BLE_STATUS_TIMEOUT;
if (status) {
return status;
}
return 0;
}
tBleStatus aci_gap_pass_key_response(uint16_t conn_handle, uint32_t passkey)
{
struct hci_request rq;
gap_passkey_response_cp cp;
uint8_t status;
cp.conn_handle = htobs(conn_handle);
cp.passkey = htobl(passkey);
Osal_MemSet(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = OCF_GAP_PASSKEY_RESPONSE;
rq.cparam = &cp;
rq.clen = sizeof(cp);
rq.event = EVT_CMD_STATUS;
rq.rparam = &status;
rq.rlen = 1;
if (hci_send_req(&rq, FALSE) < 0)
return BLE_STATUS_TIMEOUT;
return status;
}
tBleStatus aci_updater_program_data_block(uint32_t address,
uint16_t len,
const uint8_t *data)
{
struct hci_request rq;
uint8_t status;
uint8_t buffer[HCI_MAX_PACKET_SIZE];
uint8_t indx = 0;
if((len+6) > HCI_MAX_PACKET_SIZE)
return BLE_STATUS_INVALID_PARAMS;
address = htobl(address);
memcpy(buffer + indx, &address, 4);
indx += 4;
len = htobs(len);
memcpy(buffer + indx, &len, 2);
indx += 2;
memcpy(buffer + indx, data, len);
indx += len;
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_VENDOR_CMD;
rq.ocf = OCF_UPDATER_PROG_DATA_BLOCK;
rq.cparam = (void *)buffer;
rq.clen = indx;
rq.rparam = &status;
rq.rlen = 1;
if (hci_send_req(&rq) < 0)
return -1;
return status;
}
static int process_frames(int dev, int sock, int fd, unsigned long flags)
{
struct cmsghdr *cmsg;
struct msghdr msg;
struct iovec iv;
struct hcidump_hdr *dh;
struct btsnoop_pkt *dp;
struct frame frm;
struct pollfd fds[2];
int nfds = 0;
char *buf, *ctrl;
int len, hdr_size = HCIDUMP_HDR_SIZE;
if (sock < 0)
return -1;
if (snap_len < SNAP_LEN)
snap_len = SNAP_LEN;
if (flags & DUMP_BTSNOOP)
hdr_size = BTSNOOP_PKT_SIZE;
buf = malloc(snap_len + hdr_size);
if (!buf) {
perror("Can't allocate data buffer");
return -1;
}
dh = (void *) buf;
dp = (void *) buf;
frm.data = buf + hdr_size;
ctrl = malloc(100);
if (!ctrl) {
free(buf);
perror("Can't allocate control buffer");
return -1;
}
if (dev == HCI_DEV_NONE)
printf("system: ");
else
printf("device: hci%d ", dev);
printf("snap_len: %d filter: 0x%lx\n", snap_len, parser.filter);
memset(&msg, 0, sizeof(msg));
fds[nfds].fd = sock;
fds[nfds].events = POLLIN;
fds[nfds].revents = 0;
nfds++;
while (1) {
int i, n = poll(fds, nfds, -1);
if (n <= 0)
continue;
for (i = 0; i < nfds; i++) {
if (fds[i].revents & (POLLHUP | POLLERR | POLLNVAL)) {
if (fds[i].fd == sock)
printf("device: disconnected\n");
else
printf("client: disconnect\n");
return 0;
}
}
iv.iov_base = frm.data;
iv.iov_len = snap_len;
msg.msg_iov = &iv;
msg.msg_iovlen = 1;
msg.msg_control = ctrl;
msg.msg_controllen = 100;
len = recvmsg(sock, &msg, MSG_DONTWAIT);
if (len < 0) {
if (errno == EAGAIN || errno == EINTR)
continue;
perror("Receive failed");
return -1;
}
/* Process control message */
frm.data_len = len;
frm.dev_id = dev;
frm.in = 0;
frm.pppdump_fd = parser.pppdump_fd;
frm.audio_fd = parser.audio_fd;
cmsg = CMSG_FIRSTHDR(&msg);
while (cmsg) {
int dir;
switch (cmsg->cmsg_type) {
case HCI_CMSG_DIR:
memcpy(&dir, CMSG_DATA(cmsg), sizeof(int));
frm.in = (uint8_t) dir;
break;
case HCI_CMSG_TSTAMP:
memcpy(&frm.ts, CMSG_DATA(cmsg),
sizeof(struct timeval));
break;
}
cmsg = CMSG_NXTHDR(&msg, cmsg);
}
frm.ptr = frm.data;
frm.len = frm.data_len;
switch (mode) {
case WRITE:
/* Save or send dump */
if (flags & DUMP_BTSNOOP) {
uint64_t ts;
uint8_t pkt_type = ((uint8_t *) frm.data)[0];
dp->size = htobe32(frm.data_len);
dp->len = dp->size;
dp->flags = be32toh(frm.in & 0x01);
dp->drops = 0;
ts = (frm.ts.tv_sec - 946684800ll) * 1000000ll + frm.ts.tv_usec;
dp->ts = htobe64(ts + 0x00E03AB44A676000ll);
if (pkt_type == HCI_COMMAND_PKT ||
pkt_type == HCI_EVENT_PKT)
dp->flags |= be32toh(0x02);
} else {
dh->len = htobs(frm.data_len);
dh->in = frm.in;
dh->ts_sec = htobl(frm.ts.tv_sec);
dh->ts_usec = htobl(frm.ts.tv_usec);
}
if (write_n(fd, buf, frm.data_len + hdr_size) < 0) {
perror("Write error");
return -1;
}
break;
default:
/* Parse and print */
parse(&frm);
break;
}
}
return 0;
}
static int do_connect(const char *svr)
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, opt;
if (uuid != 0x0000)
channel = get_channel(svr, uuid);
if (channel == 0) {
syslog(LOG_ERR, "Can't get channel number");
return -1;
}
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
return -1;
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
bacpy(&addr.rc_bdaddr, &bdaddr);
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
goto error;
}
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encrypt)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Connect to remote device */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
str2ba(svr, &addr.rc_bdaddr);
addr.rc_channel = channel;
if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't connect: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//goto error;
}
syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]",
conn.hci_handle,
conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
return sk;
error:
close(sk);
return -1;
}
static void do_listen(void (*handler)(int sk))
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, nsk, opt;
char ba[18];
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
exit(1);
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
bacpy(&addr.rc_bdaddr, &bdaddr);
addr.rc_channel = channel;
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encrypt)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Enable deferred setup */
opt = defer_setup;
if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
&opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)",
strerror(errno), errno);
goto error;
}
/* Listen for connections */
if (listen(sk, 10)) {
syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Check for socket address */
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
syslog(LOG_ERR, "Can't get socket name: %s (%d)",
strerror(errno), errno);
goto error;
}
channel = addr.rc_channel;
syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel);
while (1) {
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
if (nsk < 0) {
syslog(LOG_ERR,"Accept failed: %s (%d)",
strerror(errno), errno);
goto error;
}
if (fork()) {
/* Parent */
close(nsk);
continue;
}
/* Child */
close(sk);
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//close(nsk);
//goto error;
}
ba2str(&addr.rc_bdaddr, ba);
syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
ba, conn.hci_handle,
conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
close(nsk);
goto error;
}
}
/* Handle deferred setup */
if (defer_setup) {
syslog(LOG_INFO, "Waiting for %d seconds",
abs(defer_setup) - 1);
sleep(abs(defer_setup) - 1);
if (defer_setup < 0) {
close(nsk);
goto error;
}
}
handler(nsk);
syslog(LOG_INFO, "Disconnect: %m");
exit(0);
}
return;
error:
close(sk);
exit(1);
}
static void dump_mode(int sk)
{
int len;
syslog(LOG_INFO, "Receiving ...");
while ((len = read(sk, buf, data_size)) > 0)
syslog(LOG_INFO, "Recevied %d bytes", len);
}
static void recv_mode(int sk)
{
struct timeval tv_beg, tv_end, tv_diff;
char ts[30];
long total;
uint32_t seq;
syslog(LOG_INFO, "Receiving ...");
memset(ts, 0, sizeof(ts));
seq = 0;
while (1) {
gettimeofday(&tv_beg,NULL);
total = 0;
while (total < data_size) {
//uint32_t sq;
//uint16_t l;
int r;
if ((r = recv(sk, buf, data_size, 0)) < 0) {
if (r < 0)
syslog(LOG_ERR, "Read failed: %s (%d)",
strerror(errno), errno);
return;
}
if (timestamp) {
struct timeval tv;
if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
timestamp = 0;
memset(ts, 0, sizeof(ts));
} else {
sprintf(ts, "[%ld.%ld] ",
tv.tv_sec, tv.tv_usec);
}
}
#if 0
/* Check sequence */
sq = btohl(*(uint32_t *) buf);
if (seq != sq) {
syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
seq = sq;
}
seq++;
/* Check length */
l = btohs(*(uint16_t *) (buf + 4));
if (r != l) {
syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
continue;
}
/* Verify data */
for (i = 6; i < r; i++) {
if (buf[i] != 0x7f)
syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
}
#endif
total += r;
}
gettimeofday(&tv_end,NULL);
timersub(&tv_end,&tv_beg,&tv_diff);
syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
}
}
static void do_send(int sk)
{
uint32_t seq;
int i, fd, len;
syslog(LOG_INFO,"Sending ...");
if (filename) {
fd = open(filename, O_RDONLY);
if (fd < 0) {
syslog(LOG_ERR, "Open failed: %s (%d)",
strerror(errno), errno);
exit(1);
}
len = read(fd, buf, data_size);
send(sk, buf, len, 0);
return;
} else {
for (i = 6; i < data_size; i++)
buf[i] = 0x7f;
}
seq = 0;
while ((num_frames == -1) || (num_frames-- > 0)) {
*(uint32_t *) buf = htobl(seq);
*(uint16_t *) (buf + 4) = htobs(data_size);
seq++;
if (send(sk, buf, data_size, 0) <= 0) {
syslog(LOG_ERR, "Send failed: %s (%d)",
strerror(errno), errno);
exit(1);
}
if (num_frames && delay && count && !(seq % count))
usleep(delay);
}
}
