package/network/utils/maccalc/src/main.c

   1 /*
   2  * MAC address manupulation utility
   3  *
   4  * Copyright (C) 2011 Gabor Juhos <juhosg@openwrt.org>
   5  *
   6  * This program is free software; you can redistribute it and/or modify it
   7  * under the terms of the GNU General Public License version 2 as published
   8  * by the Free Software Foundation.
   9  *
  10  */
  11
  12 #include <errno.h>
  13 #include <stdlib.h>
  14 #include <stdio.h>
  15 #include <stdint.h>
  16 #include <string.h>
  17 #include <unistd.h>
  18
  19 #define MAC_ADDRESS_LEN         6
  20
  21 #define ERR_INVALID             1
  22 #define ERR_IO                  2
  23
  24 static void usage(void);
  25
  26 char *maccalc_name;
  27
  28 static int parse_mac(const char *mac_str, unsigned char *buf)
  29 {
  30         int t;
  31
  32         t = sscanf(mac_str, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx",
  33                    &buf[0], &buf[1], &buf[2], &buf[3], &buf[4], &buf[5]);
  34
  35         if (t != MAC_ADDRESS_LEN)
  36                 return ERR_INVALID;
  37
  38         return 0;
  39 }
  40
  41 static void print_mac(unsigned char *buf)
  42 {
  43         printf("%02x:%02x:%02x:%02x:%02x:%02x\n",
  44                buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
  45 }
  46
  47 static int maccalc_do_add(int argc, const char *argv[])
  48 {
  49         unsigned char mac[MAC_ADDRESS_LEN];
  50         uint32_t t;
  51         int err;
  52         int i;
  53
  54         if (argc != 2) {
  55                 usage();
  56                 return ERR_INVALID;
  57         }
  58
  59         err = parse_mac(argv[0], mac);
  60         if (err)
  61                 return err;
  62
  63         i = atoi(argv[1]);
  64
  65         t = (mac[3] << 16) | (mac[4] << 8) | mac[5];
  66         t += i;
  67         mac[3] = (t >> 16) & 0xff;
  68         mac[4] = (t >> 8) & 0xff;
  69         mac[5] = t & 0xff;
  70
  71         print_mac(mac);
  72         return 0;
  73 }
  74
  75 static int maccalc_do_logical(int argc, const char *argv[],
  76                               unsigned char (*op)(unsigned char n1,
  77                                                   unsigned char n2))
  78 {
  79         unsigned char mac1[MAC_ADDRESS_LEN];
  80         unsigned char mac2[MAC_ADDRESS_LEN];
  81         int err;
  82         int i;
  83
  84         if (argc != 2) {
  85                 usage();
  86                 return ERR_INVALID;
  87         }
  88
  89         err = parse_mac(argv[0], mac1);
  90         if (err)
  91                 return err;
  92
  93         err = parse_mac(argv[1], mac2);
  94         if (err)
  95                 return err;
  96
  97         for (i = 0; i < MAC_ADDRESS_LEN; i++)
  98                 mac1[i] = op(mac1[i],mac2[i]);
  99
 100         print_mac(mac1);
 101         return 0;
 102 }
 103
 104 static int maccalc_do_mac2bin(int argc, const char *argv[])
 105 {
 106         unsigned char mac[MAC_ADDRESS_LEN];
 107         ssize_t c;
 108         int err;
 109
 110         if (argc != 1) {
 111                 usage();
 112                 return ERR_INVALID;
 113         }
 114
 115         err = parse_mac(argv[0], mac);
 116         if (err)
 117                 return err;
 118
 119         c = write(STDOUT_FILENO, mac, sizeof(mac));
 120         if (c != sizeof(mac)) {
 121                 fprintf(stderr, "failed to write to stdout\n");
 122                 return ERR_IO;
 123         }
 124
 125         return 0;
 126 }
 127
 128 static ssize_t read_safe(int fd, void *buf, size_t count)
 129 {
 130         ssize_t total = 0;
 131         ssize_t r;
 132
 133         while(count > 0) {
 134                 r = read(fd, buf, count);
 135                 if (r == 0)
 136                         /* EOF */
 137                         break;
 138                 if (r < 0) {
 139                         if (errno == EINTR)
 140                                 /* interrupted by a signal, restart */
 141                                 continue;
 142                         /* error */
 143                         total = -1;
 144                         break;
 145                 }
 146
 147                 /* ok */
 148                 total += r;
 149                 count -= r;
 150                 buf += r;
 151         }
 152
 153         return total;
 154 }
 155
 156 static int maccalc_do_bin2mac(int argc, const char *argv[])
 157 {
 158         unsigned char mac[MAC_ADDRESS_LEN];
 159         ssize_t c;
 160
 161         if (argc != 0) {
 162                 usage();
 163                 return ERR_INVALID;
 164         }
 165
 166         c = read_safe(STDIN_FILENO, mac, sizeof(mac));
 167         if (c != sizeof(mac)) {
 168                 fprintf(stderr, "failed to read from stdin\n");
 169                 return ERR_IO;
 170         }
 171
 172         print_mac(mac);
 173         return 0;
 174 }
 175
 176 static unsigned char op_or(unsigned char n1, unsigned char n2)
 177 {
 178         return n1 | n2;
 179 }
 180
 181 static int maccalc_do_or(int argc, const char *argv[])
 182 {
 183         return maccalc_do_logical(argc, argv, op_or);
 184 }
 185
 186 static unsigned char op_and(unsigned char n1, unsigned char n2)
 187 {
 188         return n1 & n2;
 189 }
 190
 191 static int maccalc_do_and(int argc, const char *argv[])
 192 {
 193         return maccalc_do_logical(argc, argv, op_and);
 194 }
 195
 196 static unsigned char op_xor(unsigned char n1, unsigned char n2)
 197 {
 198         return n1 ^ n2;
 199 }
 200
 201 static int maccalc_do_xor(int argc, const char *argv[])
 202 {
 203         return maccalc_do_logical(argc, argv, op_xor);
 204 }
 205
 206 static void usage(void)
 207 {
 208         fprintf(stderr,
 209                 "Usage: %s <command>\n"
 210                 "valid commands:\n"
 211                 "  add <mac> <number>\n"
 212                 "  and|or|xor <mac1> <mac2>\n"
 213                 "  mac2bin <mac>\n"
 214                 "  bin2mac\n",
 215                 maccalc_name);
 216 }
 217
 218 int main(int argc, const char *argv[])
 219 {
 220         int (*op)(int argc, const char *argv[]);
 221         int ret;
 222
 223         maccalc_name = (char *) argv[0];
 224
 225         if (argc < 2) {
 226                 usage();
 227                 return EXIT_FAILURE;
 228         }
 229
 230         if (strcmp(argv[1], "add") == 0) {
 231                 op = maccalc_do_add;
 232         } else if (strcmp(argv[1], "and") == 0) {
 233                 op = maccalc_do_and;
 234         } else if (strcmp(argv[1], "or") == 0) {
 235                 op = maccalc_do_or;
 236         } else if (strcmp(argv[1], "xor") == 0) {
 237                 op = maccalc_do_xor;
 238         } else if (strcmp(argv[1], "mac2bin") == 0) {
 239                 op = maccalc_do_mac2bin;
 240         } else if (strcmp(argv[1], "bin2mac") == 0) {
 241                 op = maccalc_do_bin2mac;
 242         } else {
 243                 fprintf(stderr, "unknown command '%s'\n", argv[1]);
 244                 usage();
 245                 return EXIT_FAILURE;
 246         }
 247
 248         argc -= 2;
 249         argv += 2;
 250
 251         ret = op(argc, argv);
 252         if (ret)
 253                 return EXIT_FAILURE;
 254
 255         return EXIT_SUCCESS;
 256 }