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); } }