pogmommy/nmrpflash
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Use msg_opt_add

Browse source
This commit is contained in:
Joseph C. Lehner 2016-02-18 19:36:20 +01:00
parent a0d202c45d
commit ae55407452
1 changed files with 36 additions and 56 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

92
nmrp.c
View file

@ -33,6 +33,8 @@
#define NMRP_MAX_OPT_SIZE 12 #define NMRP_MAX_OPT_SIZE 12
#define NMRP_MAX_OPT_NUM 3 #define NMRP_MAX_OPT_NUM 3
#define NMRP_OPT_NEXT(x) ((struct nmrp_opt*)(((char*)x) + x->len))
#define ETH_P_NMRP 0x0912 #define ETH_P_NMRP 0x0912
#define IP_LEN 4 #define IP_LEN 4
#define MAX_LOOP_RECV 1024 #define MAX_LOOP_RECV 1024
@ -116,15 +118,6 @@ static const char *msg_code_str(uint16_t code)
#undef CASE_CODE #undef CASE_CODE
} }
static void msg_update_len(struct nmrp_msg *msg)
{
uint32_t i = 0;
msg->len = NMRP_HDR_LEN;
for (; i != msg->num_opts; ++i) {
msg->len += msg->opts[i].len;
}
}
static void msg_dump(struct nmrp_msg *msg, int dump_opts) static void msg_dump(struct nmrp_msg *msg, int dump_opts)
{ {
struct nmrp_opt *opt; struct nmrp_opt *opt;
@ -153,7 +146,7 @@ static void msg_dump(struct nmrp_msg *msg, int dump_opts)
fprintf(stderr, "\n"); fprintf(stderr, "\n");
} }
remain_len -= len; remain_len -= len;
opt = (struct nmrp_opt*)(((char*)opt) + len); opt = NMRP_OPT_NEXT(opt);
} }
} }
} }
@ -161,13 +154,16 @@ static void msg_dump(struct nmrp_msg *msg, int dump_opts)
static void msg_hton(struct nmrp_msg *msg) static void msg_hton(struct nmrp_msg *msg)
{ {
uint32_t i = 0; uint32_t i = 0;
struct nmrp_opt *opt = msg->opts, *next;
msg->reserved = htons(msg->reserved); msg->reserved = htons(msg->reserved);
msg->len = htons(msg->len); msg->len = htons(msg->len);
for (; i != msg->num_opts; ++i) { for (; i != msg->num_opts; ++i) {
msg->opts[i].len = htons(msg->opts[i].len); next = NMRP_OPT_NEXT(opt);
msg->opts[i].type = htons(msg->opts[i].type); opt->len = htons(opt->len);
opt->type = htons(opt->type);
opt = next;
} }
} }
@ -200,7 +196,7 @@ static int msg_ntoh(struct nmrp_msg *msg)
} }
remaining -= opt->len; remaining -= opt->len;
++opt; opt = NMRP_OPT_NEXT(opt);
} }
if (!remaining) { if (!remaining) {
@ -232,7 +228,7 @@ static void *msg_opt_data(struct nmrp_msg *msg, uint16_t type, uint16_t *len)
} }
remaining -= opt->len; remaining -= opt->len;
opt = (struct nmrp_opt*)((char*)opt) + opt->len; opt = NMRP_OPT_NEXT(opt);
} }
return NULL; return NULL;
@ -249,8 +245,8 @@ static void msg_opt_add(struct nmrp_msg *msg, uint16_t type, void *data,
fprintf(stderr, "Invalid option - this is a bug.\n"); fprintf(stderr, "Invalid option - this is a bug.\n");
} }
for (; i != msg->num_opts; ++i) { for (; i <= msg->num_opts; ++i) {
opt = (struct nmrp_opt*)(((char*)opt) + msg->len); opt = NMRP_OPT_NEXT(opt);
} }
opt->len = NMRP_OPT_HDR_LEN + len; opt->len = NMRP_OPT_HDR_LEN + len;
@ -260,9 +256,15 @@ static void msg_opt_add(struct nmrp_msg *msg, uint16_t type, void *data,
memcpy(&opt->val, data, len); memcpy(&opt->val, data, len);
} }
msg->len += opt->len;
++msg->num_opts; ++msg->num_opts;
}
return true; static inline void msg_init(struct nmrp_msg *msg, uint16_t code)
{
memset(msg, 0, sizeof(*msg));
msg->len = NMRP_HDR_LEN;
msg->code = code;
} }
static int pkt_send(struct ethsock *sock, struct nmrp_pkt *pkt) static int pkt_send(struct ethsock *sock, struct nmrp_pkt *pkt)
@ -376,11 +378,14 @@ int nmrp_do(struct nmrpd_args *args)
uint8_t *src, dest[6]; uint8_t *src, dest[6];
uint16_t len; uint16_t len;
char *filename; char *filename;
struct in_addr ipaddr, ipmask;
time_t beg; time_t beg;
int i, status, ulreqs, expect, upload_ok; int i, status, ulreqs, expect, upload_ok;
struct ethsock *sock; struct ethsock *sock;
void (*sigh_orig)(int); void (*sigh_orig)(int);
struct {
struct in_addr addr;
struct in_addr mask;
} PACKED ipconf;
if (args->op != NMRP_UPLOAD_FW) { if (args->op != NMRP_UPLOAD_FW) {
fprintf(stderr, "Operation not implemented.\n"); fprintf(stderr, "Operation not implemented.\n");
@ -392,12 +397,12 @@ int nmrp_do(struct nmrpd_args *args)
return 1; return 1;
} }
if ((ipaddr.s_addr = inet_addr(args->ipaddr)) == INADDR_NONE) { if ((ipconf.addr.s_addr = inet_addr(args->ipaddr)) == INADDR_NONE) {
fprintf(stderr, "Invalid IP address '%s'.\n", args->ipaddr); fprintf(stderr, "Invalid IP address '%s'.\n", args->ipaddr);
return 1; return 1;
} }
if ((ipmask.s_addr = inet_addr(args->ipmask)) == INADDR_NONE) { if ((ipconf.mask.s_addr = inet_addr(args->ipmask)) == INADDR_NONE) {
fprintf(stderr, "Invalid subnet mask '%s'.\n", args->ipmask); fprintf(stderr, "Invalid subnet mask '%s'.\n", args->ipmask);
return 1; return 1;
} }
@ -422,7 +427,7 @@ int nmrp_do(struct nmrpd_args *args)
return 1; return 1;
} }
status = is_valid_ip(sock, &ipaddr, &ipmask); status = is_valid_ip(sock, &ipconf.addr, &ipconf.mask);
if (status <= 0) { if (status <= 0) {
if (!status) { if (!status) {
fprintf(stderr, "Address %s/%s cannot be used on interface %s.\n", fprintf(stderr, "Address %s/%s cannot be used on interface %s.\n",
@ -447,19 +452,8 @@ int nmrp_do(struct nmrpd_args *args)
memcpy(tx.eh.ether_dhost, dest, 6); memcpy(tx.eh.ether_dhost, dest, 6);
tx.eh.ether_type = htons(ETH_P_NMRP); tx.eh.ether_type = htons(ETH_P_NMRP);
tx.msg.reserved = 0; msg_init(&tx.msg, NMRP_C_ADVERTISE);
tx.msg.code = NMRP_C_ADVERTISE; msg_opt_add(&tx.msg, NMRP_O_MAGIC_NO, "NTGR", 4);
tx.msg.id = 0;
tx.msg.num_opts = 0;
tx.msg.opts[0].type = NMRP_O_MAGIC_NO;
tx.msg.opts[0].len = NMRP_OPT_HDR_LEN + 4;
tx.msg.opts[0].val.magic[0] = 'N';
tx.msg.opts[0].val.magic[1] = 'T';
tx.msg.opts[0].val.magic[2] = 'G';
tx.msg.opts[0].val.magic[3] = 'R';
msg_update_len(&tx.msg);
msg_hton(&tx.msg); msg_hton(&tx.msg);
i = 0; i = 0;
@ -502,11 +496,7 @@ int nmrp_do(struct nmrpd_args *args)
msg_code_str(rx.msg.code), msg_code_str(expect)); msg_code_str(rx.msg.code), msg_code_str(expect));
} }
tx.msg.code = NMRP_C_NONE; msg_init(&tx.msg, NMRP_C_NONE);
tx.msg.reserved = 0;
tx.msg.id = 0;
tx.msg.num_opts = 0;
tx.msg.len = 0;
status = 1; status = 1;
@ -518,23 +508,14 @@ int nmrp_do(struct nmrpd_args *args)
goto out; goto out;
case NMRP_C_CONF_REQ: case NMRP_C_CONF_REQ:
tx.msg.code = NMRP_C_CONF_ACK; tx.msg.code = NMRP_C_CONF_ACK;
tx.msg.num_opts = 2;
tx.msg.opts[0].type = NMRP_O_DEV_IP; msg_opt_add(&tx.msg, NMRP_O_DEV_IP, &ipconf, 8);
tx.msg.opts[0].len = NMRP_OPT_HDR_LEN + 2 * 4; msg_opt_add(&tx.msg, NMRP_O_FW_UP, NULL, 0);
memcpy(tx.msg.opts[0].val.ip.addr, &ipaddr, 4);
memcpy(tx.msg.opts[0].val.ip.mask, &ipmask, 4);
tx.msg.opts[1].type = NMRP_O_FW_UP;
tx.msg.opts[1].len = NMRP_OPT_HDR_LEN;
#ifdef NMRPFLASH_SET_REGION #ifdef NMRPFLASH_SET_REGION
tx.msg.num_opts = 3; if (args->region) {
msg_opt_add(&tx.msg, NMRP_O_DEV_REGION, &args->region, 2);
tx.msg.opts[2].type = NMRP_O_DEV_REGION; }
tx.msg.opts[2].len = NMRP_OPT_HDR_LEN + 2;
tx.msg.opts[2].val.region = args->region;
#endif #endif
expect = NMRP_C_TFTP_UL_REQ; expect = NMRP_C_TFTP_UL_REQ;
@ -575,7 +556,7 @@ int nmrp_do(struct nmrpd_args *args)
len, filename); len, filename);
} else if (!args->file_remote) { } else if (!args->file_remote) {
args->file_remote = args->file_local; args->file_remote = args->file_local;
printf("Received upload request with empty filename."); printf("Received upload request with empty filename.\n");
} }
status = 0; status = 0;
@ -592,7 +573,7 @@ int nmrp_do(struct nmrpd_args *args)
} }
if (!status && args->file_local) { if (!status && args->file_local) {
status = is_valid_ip(sock, &ipaddr, &ipmask); status = is_valid_ip(sock, &ipconf.addr, &ipconf.mask);
if (status < 0) { if (status < 0) {
goto out; goto out;
} else if (!status) { } else if (!status) {
@ -647,7 +628,6 @@ int nmrp_do(struct nmrpd_args *args)
} }
if (tx.msg.code != NMRP_C_NONE) { if (tx.msg.code != NMRP_C_NONE) {
msg_update_len(&tx.msg);
msg_hton(&tx.msg); msg_hton(&tx.msg);
if (pkt_send(sock, &tx) < 0) { if (pkt_send(sock, &tx) < 0) {