static uint8_t send_packet(struct net_buf *buf,
mac_callback_t sent_callback, void *ptr)
{
bool is_broadcast, ack_required;
uint8_t attempts;
uint8_t retries;
int ret;
#ifdef SIMPLERDC_802154_ACK_REQ
packetbuf_set_attr(buf, PACKETBUF_ATTR_MAC_ACK, 1);
#endif
retries = prepare_packet(buf);
if (!retries) {
return MAC_TX_ERR_FATAL;
}
ack_required = prepare_for_ack(buf);
is_broadcast = !!packetbuf_holds_broadcast(buf);
attempts = 0;
while (retries) {
attempts++;
retries--;
ret = NETSTACK_RADIO.transmit(buf, packetbuf_totlen(buf));
if (ret == RADIO_TX_COLLISION) {
continue;
}
ret = wait_for_ack(is_broadcast, ack_required);
if (ret == MAC_TX_OK) {
break;
}
}
mac_call_sent_callback(buf, sent_callback, ptr, ret, attempts);
if (ret == MAC_TX_OK) {
return 1;
}
return 0;
}
int init_monitor(int fd)
{
int res;
uint8_t init = CMD_INIT;
/* Skip in i2c mode */
if (i2c_adapter != INVALID_I2C_ADAPTER)
return 0;
printf("Waiting for the monitor startup ...");
fflush(stdout);
while (1) {
/* Send the command index */
res = write(fd, &init, 1);
if (res <= 0) {
perror("Failed to write command");
return -1;
}
/* Wait for the ACK */
res = wait_for_ack(fd);
if (res == 0)
break;
if (res == -EINVAL) {
/* we got NACK'ed, the loader might be already started
* let's ping it to check
*/
if (command_get_id(fd)) {
printf("Monitor already started.\n");
return 0;
}
}
if (res < 0 && res != -ETIMEDOUT)
return -1;
fflush(stdout);
}
printf("Done.\n");
/* read trailing chars */
discard_input(fd);
return 0;
}
int command_go(int fd, uint32_t address)
{
int res;
uint32_t addr_be = htonl(address);
payload_t load = { 4, (uint8_t *)&addr_be };
res = send_command(fd, CMD_GO, &load, 1, NULL, 0, 1);
if (res < 0)
return -EIO;
#if 0 /* this ACK should exist according to the documentation ... */
/* Wait for the ACK */
if (wait_for_ack(fd) < 0) {
fprintf(stderr, "Failed to get GO ACK\n");
return -EINVAL;
}
#endif
printf("Program started at 0x%08x.\n", address);
return 0;
}
int command_read_unprotect(int fd)
{
int res;
res = send_command(fd, CMD_RU, NULL, 0, NULL, 0, 1);
if (res < 0)
return -EIO;
/* Wait for the ACK */
if (wait_for_ack(fd) < 0) {
fprintf(stderr, "Failed to get read-protect ACK\n");
return -EINVAL;
}
printf("Flash read unprotected.\n");
/* This commands triggers a reset */
if (init_monitor(fd) < 0) {
fprintf(stderr, "Cannot recover after RP reset\n");
return -EIO;
}
return 0;
}
/**
* Send a message verdict including the payload
* @arg nlh netlink messsage header
* @arg msg queue msg
* @arg payload_data packet payload data
* @arg payload_len payload length
* @return 0 on OK or error code
*/
int nfnl_queue_msg_send_verdict_payload(struct nl_sock *nlh,
const struct nfnl_queue_msg *msg,
const void *payload_data, unsigned payload_len)
{
struct nl_msg *nlmsg;
int err;
struct iovec iov[3];
struct nlattr nla;
nlmsg = nfnl_queue_msg_build_verdict(msg);
if (nlmsg == NULL)
return -NLE_NOMEM;
memset(iov, 0, sizeof(iov));
iov[0].iov_base = (void *) nlmsg_hdr(nlmsg);
iov[0].iov_len = nlmsg_hdr(nlmsg)->nlmsg_len;
nla.nla_type = NFQA_PAYLOAD;
nla.nla_len = payload_len + sizeof(nla);
nlmsg_hdr(nlmsg)->nlmsg_len += nla.nla_len;
iov[1].iov_base = (void *) &nla;
iov[1].iov_len = sizeof(nla);
iov[2].iov_base = (void *) payload_data;
iov[2].iov_len = NLA_ALIGN(payload_len);
nl_complete_msg(nlh, nlmsg);
err = nl_send_iovec(nlh, nlmsg, iov, 3);
nlmsg_free(nlmsg);
if (err < 0)
return err;
return wait_for_ack(nlh);
}
static int build_rule_msg(struct rtnl_rule *tmpl, int cmd, int flags,
struct nl_msg **result)
{
struct nl_msg *msg;
struct fib_rule_hdr frh = {
.family = tmpl->r_family,
.table = tmpl->r_table,
.action = tmpl->r_action,
.flags = tmpl->r_flags,
.tos = tmpl->r_dsfield,
};
if (!(tmpl->ce_mask & RULE_ATTR_FAMILY))
return -NLE_MISSING_ATTR;
msg = nlmsg_alloc_simple(cmd, flags);
if (!msg)
return -NLE_NOMEM;
if (nlmsg_append(msg, &frh, sizeof(frh), NLMSG_ALIGNTO) < 0)
goto nla_put_failure;
if (tmpl->ce_mask & RULE_ATTR_SRC) {
frh.src_len = nl_addr_get_prefixlen(tmpl->r_src);
NLA_PUT_ADDR(msg, FRA_SRC, tmpl->r_src);
}
if (tmpl->ce_mask & RULE_ATTR_DST) {
frh.dst_len = nl_addr_get_prefixlen(tmpl->r_dst);
NLA_PUT_ADDR(msg, FRA_DST, tmpl->r_dst);
}
if (tmpl->ce_mask & RULE_ATTR_IIFNAME)
NLA_PUT_STRING(msg, FRA_IIFNAME, tmpl->r_iifname);
if (tmpl->ce_mask & RULE_ATTR_OIFNAME)
NLA_PUT_STRING(msg, FRA_OIFNAME, tmpl->r_oifname);
if (tmpl->ce_mask & RULE_ATTR_PRIO)
NLA_PUT_U32(msg, FRA_PRIORITY, tmpl->r_prio);
if (tmpl->ce_mask & RULE_ATTR_MARK)
NLA_PUT_U32(msg, FRA_FWMARK, tmpl->r_mark);
if (tmpl->ce_mask & RULE_ATTR_MASK)
NLA_PUT_U32(msg, FRA_FWMASK, tmpl->r_mask);
if (tmpl->ce_mask & RULE_ATTR_GOTO)
NLA_PUT_U32(msg, FRA_GOTO, tmpl->r_goto);
if (tmpl->ce_mask & RULE_ATTR_FLOW)
NLA_PUT_U32(msg, FRA_FLOW, tmpl->r_flow);
*result = msg;
return 0;
nla_put_failure:
nlmsg_free(msg);
return -NLE_MSGSIZE;
}
/**
* Build netlink request message to add a new rule
* @arg tmpl template with data of new rule
* @arg flags additional netlink message flags
*
* Builds a new netlink message requesting a addition of a new
* rule. The netlink message header isn't fully equipped with
* all relevant fields and must thus be sent out via nl_send_auto_complete()
* or supplemented as needed. \a tmpl must contain the attributes of the new
* address set via \c rtnl_rule_set_* functions.
*
* @return The netlink message
*/
int rtnl_rule_build_add_request(struct rtnl_rule *tmpl, int flags,
struct nl_msg **result)
{
return build_rule_msg(tmpl, RTM_NEWRULE, NLM_F_CREATE | flags,
result);
}
/**
* Add a new rule
* @arg sk Netlink socket.
* @arg tmpl template with requested changes
* @arg flags additional netlink message flags
*
* Builds a netlink message by calling rtnl_rule_build_add_request(),
* sends the request to the kernel and waits for the next ACK to be
* received and thus blocks until the request has been fullfilled.
*
* @return 0 on sucess or a negative error if an error occured.
*/
int rtnl_rule_add(struct nl_sock *sk, struct rtnl_rule *tmpl, int flags)
{
struct nl_msg *msg;
int err;
if ((err = rtnl_rule_build_add_request(tmpl, flags, &msg)) < 0)
return err;
err = nl_send_auto_complete(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return wait_for_ack(sk);
}
/** @} */
/**
* @name Rule Deletion
* @{
*/
/**
* Build a netlink request message to delete a rule
* @arg rule rule to delete
* @arg flags additional netlink message flags
*
* Builds a new netlink message requesting a deletion of a rule.
* The netlink message header isn't fully equipped with all relevant
* fields and must thus be sent out via nl_send_auto_complete()
* or supplemented as needed. \a rule must point to an existing
* address.
*
* @return The netlink message
*/
int rtnl_rule_build_delete_request(struct rtnl_rule *rule, int flags,
struct nl_msg **result)
{
return build_rule_msg(rule, RTM_DELRULE, flags, result);
}
/**
* Delete a rule
* @arg sk Netlink socket.
* @arg rule rule to delete
* @arg flags additional netlink message flags
*
* Builds a netlink message by calling rtnl_rule_build_delete_request(),
* sends the request to the kernel and waits for the next ACK to be
* received and thus blocks until the request has been fullfilled.
*
* @return 0 on sucess or a negative error if an error occured.
*/
int rtnl_rule_delete(struct nl_sock *sk, struct rtnl_rule *rule, int flags)
{
struct nl_msg *msg;
int err;
if ((err = rtnl_rule_build_delete_request(rule, flags, &msg)) < 0)
return err;
err = nl_send_auto_complete(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return wait_for_ack(sk);
}
/** @} */
/**
* @name Attribute Modification
* @{
*/
void rtnl_rule_set_family(struct rtnl_rule *rule, int family)
{
rule->r_family = family;
rule->ce_mask |= RULE_ATTR_FAMILY;
}
int main (int argc, char **argv)
{
char *in_file_name = NULL;
char *out_file_name = OUT_FILE_NAME;
struct termios orig_termios, cur_termios;
/* the general buffer for config read from file */
unsigned char data[65536] = { SYNC_CHAR_1, SYNC_CHAR_2 };
/* this packet saves the just sent CFG commands */
unsigned char cfg_cfg[CFG_CFG_LENGTH] = {
SYNC_CHAR_1, SYNC_CHAR_2, CFG, CFG_CFG, 0x0C, 0x00,
0x00, 0x00, 0x00, 0x00,
0xFF, 0xFF, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00 };
unsigned char data_temp;
FILE *in_file;
int serial_fd;
int count, i;
int br;
int baud;
int save_perm = SAVE_PERMANENT;
int baudrate = DEFAULT_BAUDRATE;
int ublox_port = UBLOX_PORT;
while ((i=getopt(argc, argv, "p:b:s:d:")) != -1)
{
switch(i)
{
case'p':
ublox_port = strtoul(optarg, 0, 0);
break;
case'b':
baudrate = strtoul(optarg, 0, 0);
break;
case's':
save_perm = strtoul(optarg, 0, 0);
break;
case'd':
out_file_name = optarg;
break;
}
}
if (optind == argc - 1)
{
in_file_name = argv[ optind ];
++optind;
}
if (in_file_name == NULL)
{
// fprintf(stderr, usage_str);
fprintf(stderr,
"ublox_conf\nConfigures u-blox GPS receivers\n");
fprintf(stderr,
" <file> : configuration file name (example: %s)\n",
IN_FILE_NAME);
fprintf(stderr,
" -d <dev> : GPS device name (default: %s)\n",
OUT_FILE_NAME);
fprintf(stderr,
" -b <baud> : initial GPS receiver baud rate (default: %d)\n",
DEFAULT_BAUDRATE);
fprintf(stderr,
" -s <0|1> : save config to Flash/battery backed RAM (default: %d)\n",
SAVE_PERMANENT);
fprintf(stderr,
" -p <port> : internal u-blox receiver port (default: %d)\n",
UBLOX_PORT);
exit(2);
}
if ((br = int_to_baud(baudrate)) < 0 )
{
printf("invalid baudrate %d\n", baudrate);
return( -1 );
}
if ( (in_file = fopen( in_file_name, "r" )) == NULL )
{
perror("could not open in file");
return( -1 );
}
if ( (serial_fd = open( out_file_name, O_RDWR )) == 0 )
{
perror("could not open out file");
return( -1 );
}
if (tcgetattr(serial_fd, &orig_termios))
{
perror("getting modem serial device attr");
return( -1 );
}
cur_termios = orig_termios;
/* input modes */
cur_termios.c_iflag &= ~(IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK|ISTRIP|INLCR|IGNCR
|ICRNL |IXON|IXANY|IXOFF|IMAXBEL);
/* IGNCR does not pass 0x0D */
cur_termios.c_iflag |= BRKINT;
/* output_flags */
cur_termios.c_oflag &=~(OPOST|ONLCR|OCRNL|ONOCR|ONLRET);
/* control modes */
cur_termios.c_cflag &= ~(CSIZE|CSTOPB|CREAD|PARENB|PARODD|HUPCL|CLOCAL|CRTSCTS);
cur_termios.c_cflag |= CREAD|CS8|CLOCAL;
/* local modes */
cur_termios.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN);
cur_termios.c_lflag |= NOFLSH;
if (cfsetispeed(&cur_termios, B0))
{
perror("setting input modem serial device speed");
return( -1 );
}
if (cfsetospeed(&cur_termios, br))
{
perror("setting modem serial device speed");
return( -1 );
}
if (tcsetattr(serial_fd, TCSADRAIN, &cur_termios))
{
perror("setting modem serial device attr");
return( -1 );
}
while (!feof(in_file))
{
/* search for first ' ' character */
while( ((data_temp = fgetc(in_file)) != BLANK_CHAR) &&
(!feof(in_file)) )
{
printf("%c", data_temp);
}
/* check for minus sign */
if ( (!feof(in_file)) &&
(fgetc(in_file) != MINUS_CHAR) )
{
printf("format error (no minus)\n");
return(-1);
}
/* search for second ' ' character */
while( (fgetc(in_file) != BLANK_CHAR) &&
(!feof(in_file)) ){}
if (feof(in_file)) break;
printf(" ");
/* read message */
count = 2;
do
{
data_temp = fgetc(in_file);
if ( (data_temp <= '9') && (data_temp >= '0') )
{
data[count] = (data_temp - '0') << 4;
}
else
if ( (data_temp <= 'f') && (data_temp >= 'a') )
{
data[count] = (data_temp - 'a' + 10) << 4;
}
else
if ( (data_temp <= 'F') && (data_temp >= 'A') )
{
data[count] = (data_temp - 'A' + 10) << 4;
}
else
{
printf("format error (letters a)\n");
return(-1);
}
data_temp = fgetc(in_file);
if ( (data_temp <= '9') && (data_temp >= '0') )
{
data[count] += (data_temp - '0');
}
else
if ( (data_temp <= 'f') && (data_temp >= 'a') )
{
data[count] += (data_temp - 'a' + 10);
}
else
if ( (data_temp <= 'F') && (data_temp >= 'A') )
{
data[count] += (data_temp - 'A' + 10);
}
else
{
printf("format error (letters b)\n");
return(-1);
}
count++;
}
while( (fgetc(in_file) == BLANK_CHAR) &&
(!feof(in_file)) );
if ( (data[4] + (data[5] << 8) + 4) != count-2)
{
printf("format error (length)");
return(-1);
}
/* calc checksum */
data[count] = 0;
data[count+1] = 0;
for (i=2; i < count; i++)
{
data[count] = data[count] + data[i];
data[count+1] = data[count+1] + data[count];
}
/* send data */
write(serial_fd, data, count+2);
/* did we configure something: check for acknowledge */
if (data[2] == CFG)
{
/* did we change baudrate: change PC as well */
if (data[3] == CFG_PRT)
{
baud = (data[6+8]) +
(data[6+9] << 8) +
(data[6+10] << 16) +
(data[6+11] << 24);
if (data[6] == ublox_port)
{
sleep(1);
printf("port in use, ");
if ((br = int_to_baud(baud)) < 0 )
{
printf("invalid baudrate %d\n", baud);
break;
}
if (cfsetospeed(&cur_termios, br))
{
perror("setting modem serial device speed");
return( -1 );
}
else
{
if (tcsetattr(serial_fd, TCSADRAIN, &cur_termios))
{
perror("setting modem serial device attr");
return( -1 );
}
else
{
printf("baudrate set to %d ", baud);
}
}
}
}
wait_for_ack( data, serial_fd );
}
/* monitor command? */
else if (data[2] == MON)
{
/* monitor version command? */
if (data[3] == MON_VER)
{
/* print it */
for (i=6; i < count; i++)
{
printf("%c", data[i]);
}
printf("\n");
}
}
else
{
printf("-\n");
}
}
/* save it forever? */
if (save_perm != 0)
{
printf("CFG-CFG writing config to permanent memory ");
count = CFG_CFG_LENGTH-2;
for (i=2; i < count; i++)
{
cfg_cfg[count] = cfg_cfg[count] + cfg_cfg[i];
cfg_cfg[count+1] = cfg_cfg[count+1] + cfg_cfg[count];
}
/* send data */
write(serial_fd, cfg_cfg, count+2);
wait_for_ack( cfg_cfg, serial_fd );
}
fclose(in_file);
close(serial_fd);
return(0);
}
/**
* Send netlink message.
* @arg sk Netlink socket.
* @arg msg Netlink message to be sent.
* @arg iov iovec to be sent.
* @arg iovlen number of struct iovec to be sent.
* @see nl_sendmsg()
* @return Number of characters sent on success or a negative error code.
*/
int nl_send_iovec(struct nl_sock *sk, struct nl_msg *msg, struct iovec *iov, unsigned iovlen)
{
struct sockaddr_nl *dst;
struct ucred *creds;
struct msghdr hdr = {
.msg_name = (void *) &sk->s_peer,
.msg_namelen = sizeof(struct sockaddr_nl),
.msg_iov = iov,
.msg_iovlen = iovlen,
};
/* Overwrite destination if specified in the message itself, defaults
* to the peer address of the socket.
*/
dst = nlmsg_get_dst(msg);
if (dst->nl_family == AF_NETLINK)
hdr.msg_name = dst;
/* Add credentials if present. */
creds = nlmsg_get_creds(msg);
if (creds != NULL) {
char buf[CMSG_SPACE(sizeof(struct ucred))];
struct cmsghdr *cmsg;
hdr.msg_control = buf;
hdr.msg_controllen = sizeof(buf);
cmsg = CMSG_FIRSTHDR(&hdr);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SCM_CREDENTIALS;
cmsg->cmsg_len = CMSG_LEN(sizeof(struct ucred));
memcpy(CMSG_DATA(cmsg), creds, sizeof(struct ucred));
}
return nl_sendmsg(sk, msg, &hdr);
}
/**
* Send netlink message.
* @arg sk Netlink socket.
* @arg msg Netlink message to be sent.
* @see nl_sendmsg()
* @return Number of characters sent on success or a negative error code.
*/
int nl_send(struct nl_sock *sk, struct nl_msg *msg)
{
struct iovec iov = {
.iov_base = (void *) nlmsg_hdr(msg),
.iov_len = nlmsg_hdr(msg)->nlmsg_len,
};
return nl_send_iovec(sk, msg, &iov, 1);
}
void nl_complete_msg(struct nl_sock *sk, struct nl_msg *msg)
{
struct nlmsghdr *nlh;
nlh = nlmsg_hdr(msg);
if (nlh->nlmsg_pid == 0)
nlh->nlmsg_pid = sk->s_local.nl_pid;
if (nlh->nlmsg_seq == 0)
nlh->nlmsg_seq = sk->s_seq_next++;
if (msg->nm_protocol == -1)
msg->nm_protocol = sk->s_proto;
nlh->nlmsg_flags |= NLM_F_REQUEST;
if (!(sk->s_flags & NL_NO_AUTO_ACK))
nlh->nlmsg_flags |= NLM_F_ACK;
}
void nl_auto_complete(struct nl_sock *sk, struct nl_msg *msg)
{
nl_complete_msg(sk, msg);
}
/**
* Automatically complete and send a netlink message
* @arg sk Netlink socket.
* @arg msg Netlink message to be sent.
*
* This function takes a netlink message and passes it on to
* nl_auto_complete() for completion.
*
* Checks the netlink message \c nlh for completness and extends it
* as required before sending it out. Checked fields include pid,
* sequence nr, and flags.
*
* @see nl_send()
* @return Number of characters sent or a negative error code.
*/
int nl_send_auto(struct nl_sock *sk, struct nl_msg *msg)
{
struct nl_cb *cb = sk->s_cb;
nl_complete_msg(sk, msg);
if (cb->cb_send_ow)
return cb->cb_send_ow(sk, msg);
else
return nl_send(sk, msg);
}
int nl_send_auto_complete(struct nl_sock *sk, struct nl_msg *msg)
{
return nl_send_auto(sk, msg);
}
/**
* Send netlink message and wait for response (sync request-response)
* @arg sk Netlink socket
* @arg msg Netlink message to be sent
*
* This function takes a netlink message and sends it using nl_send_auto().
* It will then wait for the response (ACK or error message) to be
* received. Threfore this function will block until the operation has
* been completed.
*
* @note Disabling auto-ack (nl_socket_disable_auto_ack()) will cause
* this function to return immediately after sending. In this case,
* it is the responsibility of the caller to handle any eventual
* error messages returned.
*
* @see nl_send_auto().
*
* @return 0 on success or a negative error code.
*/
int nl_send_sync(struct nl_sock *sk, struct nl_msg *msg)
{
int err;
err = nl_send_auto(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return wait_for_ack(sk);
}
static int route_request_update(struct nl_cache *c, struct nl_sock *h)
{
struct rtmsg rhdr = {
.rtm_family = c->c_iarg1,
};
if (c->c_iarg2 & ROUTE_CACHE_CONTENT)
rhdr.rtm_flags |= RTM_F_CLONED;
return nl_send_simple(h, RTM_GETROUTE, NLM_F_DUMP, &rhdr, sizeof(rhdr));
}
/**
* @name Cache Management
* @{
*/
/**
* Build a route cache holding all routes currently configured in the kernel
* @arg sk Netlink socket.
* @arg family Address family of routes to cover or AF_UNSPEC
* @arg flags Flags
*
* Allocates a new cache, initializes it properly and updates it to
* contain all routes currently configured in the kernel.
*
* @note The caller is responsible for destroying and freeing the
* cache after using it.
* @return The cache or NULL if an error has occured.
*/
int rtnl_route_alloc_cache(struct nl_sock *sk, int family, int flags,
struct nl_cache **result)
{
struct nl_cache *cache;
int err;
if (!(cache = nl_cache_alloc(&rtnl_route_ops)))
return -NLE_NOMEM;
cache->c_iarg1 = family;
cache->c_iarg2 = flags;
if (sk && (err = nl_cache_refill(sk, cache)) < 0) {
free(cache);
return err;
}
*result = cache;
return 0;
}
/** @} */
/**
* @name Route Addition
* @{
*/
static int build_route_msg(struct rtnl_route *tmpl, int cmd, int flags,
struct nl_msg **result)
{
struct nl_msg *msg;
int err;
if (!(msg = nlmsg_alloc_simple(cmd, flags)))
return -NLE_NOMEM;
if ((err = rtnl_route_build_msg(msg, tmpl)) < 0) {
nlmsg_free(msg);
return err;
}
*result = msg;
return 0;
}
int rtnl_route_build_add_request(struct rtnl_route *tmpl, int flags,
struct nl_msg **result)
{
return build_route_msg(tmpl, RTM_NEWROUTE, NLM_F_CREATE | flags,
result);
}
int rtnl_route_add(struct nl_sock *sk, struct rtnl_route *route, int flags)
{
struct nl_msg *msg;
int err;
if ((err = rtnl_route_build_add_request(route, flags, &msg)) < 0)
return err;
err = nl_send_auto_complete(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return wait_for_ack(sk);
}
int rtnl_route_build_del_request(struct rtnl_route *tmpl, int flags,
struct nl_msg **result)
{
return build_route_msg(tmpl, RTM_DELROUTE, flags, result);
}
int rtnl_route_delete(struct nl_sock *sk, struct rtnl_route *route, int flags)
{
struct nl_msg *msg;
int err;
if ((err = rtnl_route_build_del_request(route, flags, &msg)) < 0)
return err;
err = nl_send_auto_complete(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return wait_for_ack(sk);
}
/** @} */
static struct nl_af_group route_groups[] = {
{ AF_INET, RTNLGRP_IPV4_ROUTE },
{ AF_INET6, RTNLGRP_IPV6_ROUTE },
{ AF_DECnet, RTNLGRP_DECnet_ROUTE },
{ END_OF_GROUP_LIST },
};
static struct nl_cache_ops rtnl_route_ops = {
.co_name = "route/route",
.co_hdrsize = sizeof(struct rtmsg),
.co_msgtypes = {
{ RTM_NEWROUTE, NL_ACT_NEW, "new" },
{ RTM_DELROUTE, NL_ACT_DEL, "del" },
{ RTM_GETROUTE, NL_ACT_GET, "get" },
END_OF_MSGTYPES_LIST,
},
.co_protocol = NETLINK_ROUTE,
.co_groups = route_groups,
.co_request_update = route_request_update,
.co_msg_parser = route_msg_parser,
.co_obj_ops = &route_obj_ops,
};
static void __init route_init(void)
{
nl_cache_mngt_register(&rtnl_route_ops);
}
static void __exit route_exit(void)
{
nl_cache_mngt_unregister(&rtnl_route_ops);
}
int
UBX::configure(unsigned &baudrate)
{
_configured = false;
/* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
int baud_i;
for (baud_i = 0; baud_i < 5; baud_i++) {
baudrate = baudrates_to_try[baud_i];
set_baudrate(_fd, baudrate);
/* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK from this
*/
type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
/* Set everything else of the packet to 0, otherwise the module wont accept it */
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
_message_class_needed = UBX_CLASS_CFG;
_message_id_needed = UBX_MESSAGE_CFG_PRT;
/* Define the package contents, don't change the baudrate */
cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = baudrate;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time change the baudrate */
cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
wait_for_ack(UBX_CONFIG_TIMEOUT);
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
}
/* at this point we have correct baudrate on both ends */
break;
}
if (baud_i >= 5) {
return 1;
}
/* send a CFG-RATE message to define update rate */
type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
_message_class_needed = UBX_CLASS_CFG;
_message_id_needed = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("CFG FAIL: RATE");
return 1;
}
/* send a NAV5 message to set the options for the internal filter */
type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
_message_class_needed = UBX_CLASS_CFG;
_message_id_needed = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.clsID = UBX_CLASS_CFG;
cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH;
cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK;
cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("CFG FAIL: NAV5");
return 1;
}
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV VELNED");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SVINFO");
return 1;
}
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: MON HW");
return 1;
}
_configured = true;
return 0;
}
int
UBX::configure(unsigned &baudrate)
{
_configured = false;
/* try different baudrates */
const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};
unsigned baud_i;
for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
baudrate = baudrates[baud_i];
set_baudrate(_fd, baudrate);
/* flush input and wait for at least 20 ms silence */
decode_init();
receive(20);
decode_init();
/* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK for this */
memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
_buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID;
_buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE;
_buf.payload_tx_cfg_prt.baudRate = baudrate;
_buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK;
_buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK;
send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time change the baudrate */
memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
_buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID;
_buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE;
_buf.payload_tx_cfg_prt.baudRate = UBX_TX_CFG_PRT_BAUDRATE;
_buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK;
_buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK;
send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE);
baudrate = UBX_TX_CFG_PRT_BAUDRATE;
}
/* at this point we have correct baudrate on both ends */
break;
}
if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
return 1; // connection and/or baudrate detection failed
}
/* Send a CFG-RATE message to define update rate */
memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
_buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL;
_buf.payload_tx_cfg_rate.navRate = UBX_TX_CFG_RATE_NAVRATE;
_buf.payload_tx_cfg_rate.timeRef = UBX_TX_CFG_RATE_TIMEREF;
send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate));
if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
/* send a NAV5 message to set the options for the internal filter */
memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5));
_buf.payload_tx_cfg_nav5.mask = UBX_TX_CFG_NAV5_MASK;
_buf.payload_tx_cfg_nav5.dynModel = UBX_TX_CFG_NAV5_DYNMODEL;
_buf.payload_tx_cfg_nav5.fixMode = UBX_TX_CFG_NAV5_FIXMODE;
send_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5));
if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#ifdef UBX_CONFIGURE_SBAS
/* send a SBAS message to set the SBAS options */
memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
_buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE;
send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#endif
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
/* try to set rate for NAV-PVT */
/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
configure_message_rate(UBX_MSG_NAV_PVT, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
_use_nav_pvt = false;
} else {
_use_nav_pvt = true;
}
UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
if (!_use_nav_pvt) {
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_SOL, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_VELNED, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
}
configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_MON_HW, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
/* request module version information by sending an empty MON-VER message */
send_message(UBX_MSG_MON_VER, nullptr, 0);
_configured = true;
return 0;
}
/**
* Transmit Netlink message (taking IO vector)
* @arg sk Netlink socket (required)
* @arg msg Netlink message to be sent (required)
* @arg iov IO vector to be sent (required)
* @arg iovlen Number of struct iovec to be sent (required)
*
* This function is identical to nl_send() except that instead of taking a
* `struct nl_msg` object it takes an IO vector. Please see the description
* of `nl_send()`.
*
* @callback This function triggers the `NL_CB_MSG_OUT` callback.
*
* @see nl_send()
*
* @return Number of bytes sent on success or a negative error code.
*
* @lowlevel
*/
int nl_send_iovec(struct nl_sock *sk, struct nl_msg *msg, struct iovec *iov, unsigned iovlen)
{
struct sockaddr_nl *dst;
struct ucred *creds;
struct msghdr hdr = {
.msg_name = (void *) &sk->s_peer,
.msg_namelen = sizeof(struct sockaddr_nl),
.msg_iov = iov,
.msg_iovlen = iovlen,
};
/* Overwrite destination if specified in the message itself, defaults
* to the peer address of the socket.
*/
dst = nlmsg_get_dst(msg);
if (dst->nl_family == AF_NETLINK)
hdr.msg_name = dst;
/* Add credentials if present. */
creds = nlmsg_get_creds(msg);
if (creds != NULL) {
char buf[CMSG_SPACE(sizeof(struct ucred))];
struct cmsghdr *cmsg;
hdr.msg_control = buf;
hdr.msg_controllen = sizeof(buf);
cmsg = CMSG_FIRSTHDR(&hdr);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SCM_CREDENTIALS;
cmsg->cmsg_len = CMSG_LEN(sizeof(struct ucred));
memcpy(CMSG_DATA(cmsg), creds, sizeof(struct ucred));
}
return nl_sendmsg(sk, msg, &hdr);
}
/**
* Transmit Netlink message
* @arg sk Netlink socket (required)
* @arg msg Netlink message (required)
*
* Transmits the Netlink message `msg` over the Netlink socket using the
* `sendmsg()` system call. This function is based on `nl_send_iovec()` but
* takes care of initializing a `struct iovec` based on the `msg` object.
*
* The message is addressed to the peer as specified in the socket by either
* the nl_socket_set_peer_port() or nl_socket_set_peer_groups() function.
* The peer address can be overwritten by specifying an address in the `msg`
* object using nlmsg_set_dst().
*
* If present in the `msg`, credentials set by the nlmsg_set_creds() function
* are added to the control buffer of the message.
*
* @par Overwriting Capability:
* Calls to this function can be overwritten by providing an alternative using
* the nl_cb_overwrite_send() function.
*
* @callback This function triggers the `NL_CB_MSG_OUT` callback.
*
* @attention
* Unlike `nl_send_auto()`, this function does *not* finalize the message in
* terms of automatically adding needed flags or filling out port numbers.
*
* @see nl_send_auto()
* @see nl_send_iovec()
* @see nl_socket_set_peer_port()
* @see nl_socket_set_peer_groups()
* @see nlmsg_set_dst()
* @see nlmsg_set_creds()
* @see nl_cb_overwrite_send()
*
* @return Number of bytes sent on success or a negative error code.
*/
int nl_send(struct nl_sock *sk, struct nl_msg *msg)
{
struct nl_cb *cb = sk->s_cb;
if (cb->cb_send_ow)
return cb->cb_send_ow(sk, msg);
else {
struct iovec iov = {
.iov_base = (void *) nlmsg_hdr(msg),
.iov_len = nlmsg_hdr(msg)->nlmsg_len,
};
return nl_send_iovec(sk, msg, &iov, 1);
}
}
/**
* Finalize Netlink message
* @arg sk Netlink socket (required)
* @arg msg Netlink message (required)
*
* This function finalizes a Netlink message by completing the message with
* desirable flags and values depending on the socket configuration.
*
* - If not yet filled out, the source address of the message (`nlmsg_pid`)
* will be set to the local port number of the socket.
* - If not yet specified, the next available sequence number is assigned
* to the message (`nlmsg_seq`).
* - If not yet specified, the protocol field of the message will be set to
* the protocol field of the socket.
* - The `NLM_F_REQUEST` Netlink message flag will be set.
* - The `NLM_F_ACK` flag will be set if Auto-ACK mode is enabled on the
* socket.
*/
void nl_complete_msg(struct nl_sock *sk, struct nl_msg *msg)
{
struct nlmsghdr *nlh;
nlh = nlmsg_hdr(msg);
if (nlh->nlmsg_pid == NL_AUTO_PORT)
nlh->nlmsg_pid = nl_socket_get_local_port(sk);
if (nlh->nlmsg_seq == NL_AUTO_SEQ)
nlh->nlmsg_seq = sk->s_seq_next++;
if (msg->nm_protocol == -1)
msg->nm_protocol = sk->s_proto;
nlh->nlmsg_flags |= NLM_F_REQUEST;
if (!(sk->s_flags & NL_NO_AUTO_ACK))
nlh->nlmsg_flags |= NLM_F_ACK;
}
/**
* Finalize and transmit Netlink message
* @arg sk Netlink socket (required)
* @arg msg Netlink message (required)
*
* Finalizes the message by passing it to `nl_complete_msg()` and transmits it
* by passing it to `nl_send()`.
*
* @callback This function triggers the `NL_CB_MSG_OUT` callback.
*
* @see nl_complete_msg()
* @see nl_send()
*
* @return Number of bytes sent or a negative error code.
*/
int nl_send_auto(struct nl_sock *sk, struct nl_msg *msg)
{
nl_complete_msg(sk, msg);
return nl_send(sk, msg);
}
/**
* Finalize and transmit Netlink message and wait for ACK or error message
* @arg sk Netlink socket (required)
* @arg msg Netlink message (required)
*
* Passes the `msg` to `nl_send_auto()` to finalize and transmit it. Frees the
* message and waits (sleeps) for the ACK or error message to be received.
*
* @attention
* Disabling Auto-ACK (nl_socket_disable_auto_ack()) will cause this function
* to return immediately after transmitting the message. However, the peer may
* still be returning an error message in response to the request. It is the
* responsibility of the caller to handle such messages.
*
* @callback This function triggers the `NL_CB_MSG_OUT` callback.
*
* @attention
* This function frees the `msg` object after transmitting it by calling
* `nlmsg_free()`.
*
* @see nl_send_auto().
* @see nl_wait_for_ack()
*
* @return 0 on success or a negative error code.
*/
int nl_send_sync(struct nl_sock *sk, struct nl_msg *msg)
{
int err;
err = nl_send_auto(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return wait_for_ack(sk);
}
void
inventory(object *pack, unsigned short mask)
{
object *obj;
short i, j, maxlen, n;
short row, col;
char *p;
char *msg = " ＝スペースを押してください＝";
short len = 30;
if (!(obj = pack->next_object)) {
message(mesg[26], 0);
return;
}
#define Protected(obj) ((obj->what_is & ARMOR) && obj->is_protected)
nextpage:
i = 0;
maxlen = len;
while (obj && i < ROGUE_LINES - 2) {
if (obj->what_is & mask) {
p = descs[i];
*p++ = ' ';
*p++ = obj->ichar;
*p++ = Protected(obj) ? '}' : ')';
*p++ = ' ';
get_desc(obj, p, 0);
if ((n = strlen(descs[i])) > maxlen) {
maxlen = n;
}
i++;
}
obj = obj->next_object;
}
(void) strcpy(descs[i++], msg);
if (i == 0) {
return;
}
col = ROGUE_COLUMNS - (maxlen + 2);
for (row = 0; row < i; row++) {
if (row > 0) {
for (j = col; j < ROGUE_COLUMNS; j++) {
descs[row - 1][j - col] = mvinch_rogue(row, j);
}
descs[row - 1][j - col] = 0;
}
mvaddstr_rogue(row, col, descs[row]);
clrtoeol();
}
refresh();
wait_for_ack();
move(0, 0);
clrtoeol();
#if defined( COLOR )
for (j = 1; j < i; j++) {
move(j, col);
for (p = descs[j - 1]; *p; p++) {
addch_rogue(*p);
}
}
#else /* not COLOR */
for (j = 1; j < i; j++) { /* by Yasha */
move(j, col); /* by Yasha */
clrtoeol(); /* by Yasha */
addstr_rogue(descs[j - 1]); /* by Yasha */
} /* by Yasha */
move(ROGUE_LINES - 1, 0); /* by Yasha */
clrtoeol(); /* by Yasha */
print_stats(STAT_ALL); /* by Yasha */
#endif /* not COLOR */
if (obj) {
goto nextpage;
}
}
int send_command(int fd, uint8_t cmd, payload_t *loads, int cnt,
uint8_t *resp, int resp_size, int ack_requested)
{
int res, i, c;
payload_t *p;
int readcnt = 0;
uint8_t cmd_frame[] = { cmd, 0xff ^ cmd }; /* XOR checksum */
/* Send the command index */
res = write(fd, cmd_frame, 2);
if (res <= 0) {
perror("Failed to write command frame");
return -1;
}
/* Wait for the ACK */
if (wait_for_ack(fd) < 0) {
fprintf(stderr, "Failed to get command 0x%02x ACK\n", cmd);
return -1;
}
/* Send the command payloads */
for (p = loads, c = 0; c < cnt; c++, p++) {
uint8_t crc = 0;
int size = p->size;
uint8_t *data = malloc(size + 1), *data_ptr;
if (data == NULL) {
fprintf(stderr,
"Failed to allocate memory for load %d\n", c);
return -ENOMEM;
}
memcpy(data, p->data, size);
for (i = 0; i < size; i++)
crc ^= data[i];
if (size == 1)
crc = 0xff ^ crc;
data[size] = crc;
size++;
data_ptr = data;
while (size) {
res = write(fd, data_ptr, size);
if (res < 0) {
perror("Failed to write command payload");
free(data);
return -1;
}
size -= res;
data_ptr += res;
}
/* Wait for the ACK */
if (wait_for_ack(fd) < 0) {
fprintf(stderr, "payload %d ACK failed for CMD%02x\n",
c, cmd);
free(data);
return -1;
}
free(data);
}
/* Read the answer payload */
if (resp) {
while ((resp_size > 0) && (res = read(fd, resp, resp_size))) {
if (res < 0) {
perror("Failed to read payload");
return -1;
}
readcnt += res;
resp += res;
resp_size -= res;
}
/* Wait for the ACK */
if (ack_requested) {
if (wait_for_ack(fd) < 0) {
fprintf(stderr,
"Failed to get response to command 0x%02x ACK\n",
cmd);
return -1;
}
}
}
return readcnt;
}
//the meat of the client. performs all sending of data to the server, as well as calling methods
//to build the various request types.
void client_operations(int sockfd, host* host, hfs_entry* files, char* auth_token, char* username, int num_files) {
int seq_num = 0;
hfs_entry* current_file = &files[0];
int current_file_index = 1;
while(current_file) {
//get length of file in bytes
FILE* fp = fopen(current_file->abs_path, "r");
fseek(fp, 0L, SEEK_END);
if(!fp) {
syslog(LOG_ERR, "Error reading file %s, could not include in request.", current_file->abs_path);
fclose(fp);
continue;
}
uint32_t f_size = ftell(fp);
fseek(fp, 0L, SEEK_SET);
fclose(fp);
msg* control_msg = build_control_init_request(seq_num, strlen(current_file->rel_path),
f_size, current_file->crc32, auth_token, current_file->rel_path);
//something is wrong with network order here.
// control_msg->length = htonl(control_msg->length);
char* msg_type_str = get_msg_type(control_msg->buffer[0]);
syslog(LOG_DEBUG, "Sending %s msg. Sequence # %d. Data size: %d",
msg_type_str, control_msg->buffer[1], control_msg->length);
free(msg_type_str);
send_msg(sockfd, control_msg, host);
int errcode = wait_for_ack(control_msg, sockfd, seq_num, host);
//make sure no error!
if(errcode) {
syslog(LOG_EMERG, "Received error code from HFTP server. Likely bad auth token.");
errx(EXIT_FAILURE, "Received error code from HFTP server. Likely bad auth token.");
}
// free(control_msg->buffer);
free(control_msg);
int index = 0, prev_index = 0, interval_counter = 0;
//while file is still being sent
while(index < f_size+1) {
seq_num = !seq_num;
//data request - index is out param to say where in the file has already been processed
msg* data_msg = data_from_file(current_file->abs_path, f_size, seq_num, &index);
msg_type_str = get_msg_type(data_msg->buffer[0]);
//not necessarily whole file; check with index < f_size+1
syslog(LOG_DEBUG, "Sending %s msg. Sequence # %d. Data size: %d",
msg_type_str, data_msg->buffer[1], data_msg->length);
free(msg_type_str);
send_msg(sockfd, data_msg, host);
//need error check here?
wait_for_ack(data_msg, sockfd, seq_num, host);
// free(data_msg->buffer);
free(data_msg);
interval_counter += index - prev_index;
prev_index = index;
if(interval_counter >= OUTPUT_INTERVAL) {
interval_counter = 0;
if(f_size != 0) {
syslog(LOG_INFO, "Transferring file %s, %d of %d. %d bytes successfully transferred of %d, %.1lf%% complete.",
current_file->rel_path, current_file_index, num_files, index, f_size, ((double)index/(double)f_size)*100);
}
}
}
syslog(LOG_INFO, "Finished transferring %s, %d of %d.", current_file->rel_path, current_file_index, num_files);
seq_num = !seq_num;
current_file = current_file->next;
current_file_index++;
}
msg_control* term_msg_control = (msg_control*)calloc(1, sizeof(msg_control));
term_msg_control->type = TYPE_CONTROL_TERM;
term_msg_control->seq = seq_num;
term_msg_control->length = sizeof(term_msg_control->type + term_msg_control->seq);
msg* term_msg = (msg*)term_msg_control;
send_msg(sockfd, term_msg, host);
wait_for_ack(term_msg, sockfd, seq_num, host);
free(term_msg_control);
}