Ticket #2897: nftp.c

File nftp.c, 3.7 KB (added by matteo, 9 years ago)

tool to upload firmware to the DG834 (just a stub at the moment)

Line 
1#include <stdio.h>
2#include <stdlib.h>
3#include <string.h>
4#include <sys/ioctl.h>
5#include <sys/socket.h>
6#include <linux/if_packet.h>
7#include <linux/if_ether.h>
8#include <linux/if_arp.h>
9#include <arpa/inet.h>
10
11#define NFTP_PROBE_LEN          0x40
12#define NFTP_PROBE_RESP_LEN     0x66
13#define ETH_P_NFTP              0x8888
14
15#define DEBUG
16
17#ifdef DEBUG
18        #define D(x, ...) fprintf(stderr, x"\n", __VA_ARGS__)
19#else
20        #define D(...)
21#endif
22
23void usage(char *arg0)
24{
25        fprintf(stderr, "Usage: %s iface file.img\n"
26                        "Example:\n\t %s eth0 fiermware.img\n", arg0, arg0);
27        exit(1);
28}
29
30int main(int argc, char *argv[])
31{
32        if(argc < 2)
33                usage(argv[0]);
34        int sockfd;
35
36        sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
37        if (sockfd == -1) {
38                perror("socket");
39                return 1;
40        }
41
42        /*target address*/
43        struct sockaddr_ll socket_address;
44
45        /*buffer for ethernet frame*/
46        unsigned char send_buffer[NFTP_PROBE_LEN] = { 0 };
47
48        /*pointer to ethenet header*/
49        unsigned char* etherhead = send_buffer;
50
51        /*userdata in ethernet frame*/
52        unsigned char* data = send_buffer + 14;
53
54        /*another pointer to ethernet header*/
55        struct ethhdr *eh = (struct ethhdr *)etherhead;
56
57        int send_result = 0;
58
59        struct ifreq iface;
60        strncpy(iface.ifr_name, argv[1], IFNAMSIZ);
61
62        int res = ioctl(sockfd, SIOCGIFHWADDR, &iface);
63        if(res < 0){
64                perror("ioctl");
65                exit(1);
66        }
67
68        /*our MAC address*/
69        unsigned char src_mac[ETH_ALEN] = { 0 };
70        memcpy(src_mac, iface.ifr_hwaddr.sa_data, ETH_ALEN);
71        D("Sending frame on %s (%x:%x:%x:%x:%x:%x)", iface.ifr_name, src_mac[0], src_mac[1], src_mac[2], src_mac[3], src_mac[4], src_mac[5]);
72
73        /*other host MAC address*/
74        unsigned char dest_mac[ETH_ALEN] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
75
76        /*prepare sockaddr_ll*/
77
78        /*RAW communication*/
79        socket_address.sll_family   = PF_PACKET;
80        /*we don't use a protocoll above ethernet layer
81        *   ->just use anything here*/
82        socket_address.sll_protocol = htons(ETH_P_NFTP);
83
84        /*index of the network device
85        * see full code later how to retrieve it*/
86        res = ioctl(sockfd, SIOCGIFINDEX, &iface);
87        if(res < 0){
88                perror("ioctl");
89                exit(1);
90        }
91        socket_address.sll_ifindex  =iface.ifr_ifindex;
92
93        /*ARP hardware identifier is ethernet*/
94        socket_address.sll_hatype   = ARPHRD_ETHER;
95
96        /*target is another host*/
97        socket_address.sll_pkttype  = PACKET_OTHERHOST;
98
99        /*address length*/
100        socket_address.sll_halen    = ETH_ALEN;
101        /*MAC - begin*/
102        socket_address.sll_addr[0]  = 0xFF;
103        socket_address.sll_addr[1]  = 0xFF;
104        socket_address.sll_addr[2]  = 0xFF;
105        socket_address.sll_addr[3]  = 0xFF;
106        socket_address.sll_addr[4]  = 0xFF;
107        socket_address.sll_addr[5]  = 0xFF;
108        /*MAC - end*/
109        socket_address.sll_addr[6]  = 0x00;/*not used*/
110        socket_address.sll_addr[7]  = 0x00;/*not used*/
111
112        /*set the frame header*/
113        memcpy((void*)send_buffer, (void*)dest_mac, ETH_ALEN);
114        memcpy((void*)(send_buffer+ETH_ALEN), (void*)src_mac, ETH_ALEN);
115        eh->h_proto = ETH_P_NFTP;
116
117        /*send the packet*/
118        send_result = sendto(sockfd, send_buffer, NFTP_PROBE_LEN, 0,
119                (struct sockaddr*)&socket_address, sizeof(socket_address));
120        if (send_result == -1) {
121                perror("sendto");
122                return 1;
123        }
124
125        unsigned char recv_buffer[NFTP_PROBE_RESP_LEN]; /*Buffer for ethernet frame*/
126        int length = 0; /*length of the received frame*/
127
128        length = recvfrom(sockfd, recv_buffer, NFTP_PROBE_RESP_LEN, 0, NULL, NULL);
129        if (length == -1) {
130                perror("recvfrom");
131                return 1;
132        }
133        memcpy(dest_mac, recv_buffer + ETH_ALEN, ETH_ALEN);
134        D("Found a bricked router at %x:%x:%x:%x:%x:%x", dest_mac[0], dest_mac[1], dest_mac[2], dest_mac[3], dest_mac[4], dest_mac[5]);
135        D("Router is a %s", recv_buffer + 0x1C);
136        D("Current version is %x%x", recv_buffer[0x4A], recv_buffer[0x4B]);
137        D("Max upgrade size is %d kb", (recv_buffer[0x16] | (recv_buffer[0x17] << 8) | (recv_buffer[0x18] << 16)) - 20);
138        return 0;
139}