knet_tx.c (17549B)
1 #include <signal.h> 2 #include <inttypes.h> 3 #include <stdio.h> 4 #include <stdlib.h> 5 #include <unistd.h> 6 #include <string.h> 7 #include <sys/socket.h> 8 #include <features.h> 9 #include <linux/if_packet.h> 10 #include <linux/if_ether.h> 11 #include <errno.h> 12 #include <sys/ioctl.h> 13 #include <net/if.h> 14 #include <net/ethernet.h> 15 #include <netinet/in.h> 16 #include <netinet/ether.h> 17 18 #define SRC_ETHER_ADDR "10:11:12:13:14:15" 19 #define DST_ETHER_ADDR "00:01:02:03:04:05" 20 21 #define RCPU_SIGNATURE 0xB840 22 #define TX_MAX 1514 /* Assuming no jumbo */ 23 24 #define INT_MAX 10 25 26 #define L2HEADER_SIZE 18 27 /* Starts at packet byte 0 */ 28 struct L2header { 29 uint8_t dest_mac[6]; 30 uint8_t src_mac[6]; 31 uint32_t tag; 32 uint16_t ether_type; 33 }; 34 35 #define RCPUHEADER_SIZE 14 36 /* Starts at packet byte 18 */ 37 struct RCPUheader { 38 uint16_t signature; 39 uint8_t opcode; 40 uint8_t flags; 41 uint16_t trans_id; 42 uint16_t data_len; 43 uint16_t reply_len; 44 uint32_t reserved; 45 }; 46 47 #define RCPUMETADATA_SIZE 32 48 49 /* 50 * The RCPU Metadata is hardware/chip specific. This example was derived 51 * from soc_pbsmh_v5_hdr_t in "include/soc/pgsmh.h". Not all fields have 52 * been tested with this particular utility. 53 */ 54 struct RCPUmetadata { 55 /* Byte Position */ 56 uint32_t start:8; /* 0 Start of frame indicator (must be set to 0xFF for SOBMH packets). */ 57 uint32_t _rsvd0:24; /* 1-3 */ 58 uint32_t ipcf_ptr:8; /* 4 Pointer to cell buffer of this SOBMH cell. (Set by hardware) */ 59 uint32_t spid_override:1; /* 5.0 For PBI.SPID_Override */ 60 uint32_t _rsvd2:3; /* 5.1-3 */ 61 uint32_t tx_ts:1; /* 5.4 Indicates for TS packet transmitted from CPU into IP that the outgoing packet needs to have its transmit timestamp captured by the port." */ 62 uint32_t _rsvd1:3; /* 5.5-7 */ 63 uint32_t unicast:1; /* 6.0 Indicates that PBI.UNICAST should be set to queue as unicast packet */ 64 uint32_t set_l2bm:1; /* 6.1 Indicates that PBI.L2_BITMAP should be set (to queue as L2MC packet) */ 65 uint32_t set_l3bm:1; /* 6.2 Indicates that PBI.L3_BITMAP should be set (to queue as IPMC packet). */ 66 uint32_t _rsvd3:1; /* 6.3 */ 67 uint32_t spap:2; /* 6.4-5 For PBI.SPAP - Service Pool Priority (color) */ 68 uint32_t spid:2; /* 6.6-7 For PBI.SPID - Service Pool ID */ 69 uint32_t src_mod:8; /* 7 Source module ID, must be programmed to MY_MODID */ 70 uint32_t input_pri:4; /* 8.0-3 Traffic priority to be applied to MMU via PBI.INPUT_PRIORITY. */ 71 uint32_t _rsvd4:4; /* 8.4-7 */ 72 uint32_t queue_num:7; /* 9.0-6 Queue number to be used for unicast queuing and CPU queue (CPU_COS). */ 73 uint32_t _rsvd5:1; /* 9.7 */ 74 uint32_t cos:4; /* 10.0-3 Class of service for MMU queueing for this packet - sets COS values, PBI.UC_COS, PBI.MC_COS1, and PBI.MC_COS2. */ 75 uint32_t _rsvd6:4; /* 10.4-7 */ 76 uint32_t dst_port:7; /* 11.0-6 Indicates the local port to send a SOBMH packet out. */ 77 uint32_t _rsvd7:1; /* 11.7 */ 78 }; 79 80 #define RCPUENCAP_SIZE (L2HEADER_SIZE + RCPUHEADER_SIZE + RCPUMETADATA_SIZE) 81 82 #define MAX_PAYLOAD (TX_MAX - sizeof(struct ethhdr) - RCPUENCAP_SIZE) 83 #define MIN_PAYLOAD (50) 84 85 static void 86 packL2header(uint8_t * const pkt, struct L2header *header) 87 { 88 uint8_t *vbuf; 89 int ix; 90 91 vbuf = &pkt[0]; 92 for (ix = 0; ix < 6; ix++) { 93 vbuf[ix] = header->dest_mac[ix]; 94 } 95 vbuf = &pkt[6]; 96 for (ix = 0; ix < 6; ix++) { 97 vbuf[ix] = header->src_mac[ix]; 98 } 99 vbuf = &pkt[12]; 100 vbuf[0] = (header->tag >> 24) & 0xFF; 101 vbuf[1] = (header->tag >> 16) & 0xFF; 102 vbuf[2] = (header->tag >> 8) & 0xFF; 103 vbuf[3] = header->tag & 0xFF; 104 vbuf = &pkt[16]; 105 vbuf[0] = (header->ether_type >> 8) & 0xFF; 106 vbuf[1] = header->ether_type & 0xFF; 107 } 108 109 static void 110 packRCPUheader(uint8_t * const pkt, struct RCPUheader *header) 111 { 112 uint8_t *vbuf; 113 114 vbuf = &pkt[18]; 115 116 vbuf[0] = (header->signature >> 8) & 0xFF; 117 vbuf[1] = header->signature & 0xFF; 118 vbuf[2] = header->opcode; 119 vbuf[3] = header->flags; 120 vbuf[4] = (header->trans_id >> 8) & 0xFF; 121 vbuf[5] = header->trans_id & 0xFF; 122 vbuf[6] = (header->data_len >> 8) & 0xFF; 123 vbuf[7] = header->data_len & 0xFF; 124 vbuf[8] = (header->reply_len >> 8) & 0xFF; 125 vbuf[9] = header->reply_len & 0xFF; 126 vbuf[10] = (header->reserved >> 24) & 0xFF; 127 vbuf[11] = (header->reserved >> 16) & 0xFF; 128 vbuf[12] = (header->reserved >> 8) & 0xFF; 129 vbuf[13] = header->reserved & 0xFF; 130 } 131 132 static void 133 packMetadata(uint8_t * const pkt, struct RCPUmetadata *metadata) 134 { 135 uint8_t *vbuf; 136 137 vbuf = &pkt[L2HEADER_SIZE + RCPUHEADER_SIZE]; 138 /* Add meta data */ 139 memset(vbuf, 0, RCPUMETADATA_SIZE); 140 141 vbuf[0] = metadata->start; /* 0 */ 142 vbuf[4] = metadata->ipcf_ptr; /* 4 */ 143 vbuf[5] = metadata->spid_override; /* 5.0 */ 144 vbuf[5] |= metadata->tx_ts << 4; /* 5.4 */ 145 vbuf[6] = metadata->unicast; /* 6.0 */ 146 vbuf[6] |= metadata->set_l2bm << 1; /* 6.1 */ 147 vbuf[6] |= metadata->set_l3bm << 2; /* 6.2 */ 148 vbuf[6] |= metadata->spap << 4; /* 6.4-5 */ 149 vbuf[6] |= metadata->spid << 6; /* 6.6-7 */ 150 vbuf[7] = metadata->src_mod; /* 7 */ 151 vbuf[8] = metadata->input_pri; /* 8 */ 152 vbuf[9] = metadata->queue_num; /* 9 */ 153 vbuf[10] = metadata->cos; /* 10 */ 154 vbuf[11] = metadata->dst_port; /* 11 */ 155 } 156 157 static void 158 buildEthernetHeader(uint8_t const *const pkt, char *src_mac, char *dst_mac, 159 int protocol) 160 { 161 struct ethhdr *ethernet_header = (struct ethhdr *) pkt; 162 163 /* copy the Src mac addr */ 164 memcpy(ethernet_header->h_source, (void *) ether_aton(src_mac), 6); 165 166 /* copy the Dst mac addr */ 167 memcpy(ethernet_header->h_dest, (void *) ether_aton(dst_mac), 6); 168 169 /* copy the protocol */ 170 ethernet_header->h_proto = htons(protocol); 171 } 172 173 static void 174 buildL2Header(struct L2header *const l2header, const char *const src_mac, 175 const char *const dst_mac, const int tag, const int ether_type) 176 { 177 memcpy(l2header->dest_mac, (void *) ether_aton(DST_ETHER_ADDR), 6); 178 memcpy(l2header->src_mac, (void *) ether_aton(SRC_ETHER_ADDR), 6); 179 180 l2header->tag = tag; 181 l2header->ether_type = ether_type; 182 } 183 184 static void 185 buildRCPUheader(struct RCPUheader *const rcpuHeader, const int signature, 186 const int opcode, const int flags, const int trans_id, 187 const int data_len, const int reply_len, const int reserved) 188 { 189 rcpuHeader->signature = signature; 190 rcpuHeader->opcode = opcode; 191 rcpuHeader->flags = flags; 192 rcpuHeader->trans_id = trans_id; 193 rcpuHeader->data_len = data_len; 194 rcpuHeader->reply_len = reply_len; 195 rcpuHeader->reserved = reserved; 196 } 197 198 static void 199 buildRCPUmetadata(struct RCPUmetadata *metadata, int port) 200 { 201 memset(metadata, 0, sizeof(struct RCPUmetadata)); 202 203 metadata->start = 0xFF; /* set SOBMH start */ 204 metadata->unicast = 1; /* set SOBMH: unicast */ 205 metadata->dst_port = port; /* SOBMH: dst_port */ 206 } 207 208 static void 209 relocateEthernetHeader(uint8_t * const pkt, const int payload_len) 210 { 211 int ix; 212 213 /* Move Ethernet header to encapsulated frame */ 214 memcpy(&pkt[RCPUENCAP_SIZE], pkt, 12); 215 ix = RCPUENCAP_SIZE + 12; 216 /* Add VLAN tag */ 217 pkt[ix++] = 0x81; 218 pkt[ix++] = 0x00; 219 pkt[ix++] = 0x00; 220 pkt[ix++] = 0x01; 221 /* Add length */ 222 pkt[ix++] = payload_len >> 8; 223 pkt[ix++] = payload_len & 0xff; 224 } 225 226 static int 227 create_raw_socket(int protocol_to_sniff) 228 { 229 int rawsock; 230 231 if ((rawsock = socket(PF_PACKET, SOCK_RAW, htons(protocol_to_sniff))) < 0) { 232 perror("Error creating raw socket: "); 233 return rawsock; 234 } 235 236 return rawsock; 237 } 238 239 static int 240 bind_raw_socket(char *device, int rawsock, int protocol) 241 { 242 243 struct sockaddr_ll sll; 244 struct ifreq ifr; 245 246 bzero(&sll, sizeof(sll)); 247 bzero(&ifr, sizeof(ifr)); 248 249 /* First Get the Interface Index */ 250 strncpy((char *) ifr.ifr_name, device, IFNAMSIZ); 251 if ((ioctl(rawsock, SIOCGIFINDEX, &ifr)) == -1) { 252 printf("Error getting interface index for %s\n", device); 253 return -1; 254 } 255 256 /* Bind our raw socket to this interface */ 257 sll.sll_family = AF_PACKET; 258 sll.sll_ifindex = ifr.ifr_ifindex; 259 sll.sll_protocol = htons(protocol); 260 261 if ((bind(rawsock, (struct sockaddr *) &sll, sizeof(sll))) == -1) { 262 perror("Error binding raw socket to interface\n"); 263 return -1; 264 } 265 266 return 0; 267 } 268 269 int 270 send_raw_packet(int rawsock, unsigned char *pkt, int pkt_len) 271 { 272 int sent = 0; 273 274 /* A simple write on the socket ..thats all it takes ! */ 275 if ((sent = write(rawsock, pkt, pkt_len)) != pkt_len) { 276 /* Error */ 277 if (sent < 0) { 278 printf("Send (%d bytes) failed\n", pkt_len); 279 return 1; 280 } else { 281 printf("Could only send %d bytes of packet of length %d\n", sent, pkt_len); 282 return 2; 283 } 284 } 285 return 0; 286 } 287 288 static void 289 dump_packet(unsigned char *pkt, int len) 290 { 291 int ix; 292 293 printf("Dump Packet: %d bytes\n", len); 294 for (ix = 0; ix < len; ix++) { 295 if ((ix & 0xf) == 0) { 296 printf("%04x: ", ix); 297 } else if ((ix & 0xf) == 8) { 298 printf("- "); 299 } 300 printf("%02x ", pkt[ix]); 301 if ((ix & 0xf) == 0xf) { 302 printf("\n"); 303 } 304 } 305 if ((ix & 0xf) != 0) { 306 printf("\n"); 307 } 308 } 309 310 static int rawsock = -1; 311 static int packet_count = 0; 312 static char *ifname = NULL; 313 314 static void 315 sigint(int signum) 316 { 317 /*Clean up....... */ 318 319 struct ifreq ifr; 320 321 if ((rawsock == -1) || (ifname == NULL)) { 322 return; 323 } 324 325 strncpy(ifr.ifr_name, ifname, IFNAMSIZ); 326 ioctl(rawsock, SIOCGIFFLAGS, &ifr); 327 ifr.ifr_flags &= ~IFF_PROMISC; 328 ioctl(rawsock, SIOCSIFFLAGS, &ifr); 329 close(rawsock); 330 331 printf("TX terminating ...\n"); 332 333 printf("Total packets transmitted: %d\n", packet_count); 334 exit(0); 335 } 336 337 static void 338 usage(const char *const prog) 339 { 340 printf 341 ("Usage %s [-v] [-vv] [-p port] [-ipd delay] [-s size] interface [maxTxPackets]\n", 342 prog); 343 printf("-r ; Interface is in RCPU mode\n"); 344 printf("-p <port> ; Tx on specified port\n"); 345 printf("-ipd <delay> ; inter packet delay (usecs)\n"); 346 printf("-s <psize> ; Payload size in bytes or MIN, MAX, RAND\n"); 347 printf("-v ; Be verbose\n"); 348 printf("-vv ; Be very verbose\n"); 349 printf("interface ; ethernet interface (eth5)\n"); 350 printf("maxTxPackets ; Transmit specified number of packets\n"); 351 } 352 353 int 354 main(int argc, char **argv) 355 { 356 int ax; 357 int inter_packet_delay = 0; 358 int max_packets = -1; 359 int payload_len = 110; 360 int port = 1; 361 int random_packet_sizes = 0; 362 int rcpu = 0; 363 int rcpu_len = 0; 364 int tag_len; 365 int tx_len; 366 int verbose = 0; 367 int vverbose = 0; 368 uint8_t tx_packet[TX_MAX]; 369 370 memset(tx_packet, 0xA5, sizeof(tx_packet)); 371 for (ax = 1; ax < argc; ax++) { 372 if (argv[ax][0] == '-') { 373 char *flag = &argv[ax][1]; 374 375 if (strcmp(flag, "v") == 0) { 376 verbose = 1; 377 } else if (strcmp(flag, "vv") == 0) { 378 vverbose = 1; 379 verbose = 1; 380 } else if (strcmp(flag, "r") == 0) { 381 rcpu = 1; 382 } else if (strcmp(flag, "ipd") == 0) { 383 ax++; 384 if (ax == argc) { 385 fprintf(stderr, "Missing inter packet delay\n"); 386 usage(argv[0]); 387 return -2; 388 } 389 if (argv[ax] != NULL) { 390 char *end; 391 392 inter_packet_delay = strtol(argv[ax], &end, 10); 393 if ((*end != 0) || (inter_packet_delay < 0) 394 || (inter_packet_delay > 1000000)) { 395 fprintf(stderr, "Invalid inter packet delay: %s\n", argv[ax]); 396 usage(argv[0]); 397 return -2; 398 } 399 } 400 } else if (strcmp(flag, "p") == 0) { 401 ax++; 402 if (ax == argc) { 403 fprintf(stderr, "Missing port\n"); 404 usage(argv[0]); 405 return -2; 406 } 407 if (argv[ax] != NULL) { 408 char *end; 409 410 port = strtol(argv[ax], &end, 10); 411 if ((*end != 0) || (port < 0) || (port > 128)) { 412 fprintf(stderr, "Invalid port: %s\n", argv[ax]); 413 usage(argv[0]); 414 return -2; 415 } 416 } 417 } else if (strcmp(flag, "s") == 0) { 418 ax++; 419 if (ax == argc) { 420 fprintf(stderr, "Missing payload size\n"); 421 usage(argv[0]); 422 return -2; 423 } 424 if (argv[ax] != NULL) { 425 if (strcmp(argv[ax], "MAX") == 0) { 426 payload_len = MAX_PAYLOAD; 427 } else if (strcmp(argv[ax], "MIN") == 0) { 428 payload_len = MIN_PAYLOAD; 429 } else if (strcmp(argv[ax], "RAND") == 0) { 430 random_packet_sizes = 1; 431 srandom(MAX_PAYLOAD); /* Any number will do */ 432 payload_len = MAX_PAYLOAD; 433 } else { 434 char *end; 435 436 payload_len = strtol(argv[ax], &end, 10); 437 if ((*end != 0) || (payload_len < MIN_PAYLOAD) 438 || (payload_len > MAX_PAYLOAD)) { 439 fprintf(stderr, "Invalid payload size: %s\n", argv[ax]); 440 usage(argv[0]); 441 return -2; 442 } 443 } 444 } 445 } else { 446 fprintf(stderr, "ERROR: unknown switch %s\n", argv[ax]); 447 usage(argv[0]); 448 return -2; 449 } 450 } else { 451 if (ifname == NULL) { 452 ifname = argv[ax]; 453 } else if (max_packets < 0) { 454 if (strcmp(argv[ax], "MAX") == 0) { 455 max_packets = INT_MAX; 456 } else { 457 char *end; 458 459 max_packets = strtol(argv[ax], &end, 10); 460 if ((*end != 0) || (max_packets <= 0) || (max_packets > INT_MAX)) { 461 fprintf(stderr, "Invalid packet count: %s\n", argv[ax]); 462 usage(argv[0]); 463 return -2; 464 } 465 } 466 } else { 467 fprintf(stderr, "ERROR: unknown argument %s\n", argv[ax]); 468 usage(argv[0]); 469 return -2; 470 } 471 } 472 } 473 474 if (ifname == NULL) { 475 usage(argv[0]); 476 return -1; 477 } 478 479 /*establish signal handler */ 480 if (SIG_ERR == signal(SIGINT, sigint)) { 481 return 2; 482 } 483 484 if (max_packets <= 0) { 485 max_packets = 100; 486 } 487 488 /* RCPU encapsulation */ 489 rcpu_len = rcpu ? RCPUENCAP_SIZE : 0; 490 491 tag_len = rcpu ? 4 : 0; 492 493 tx_len = sizeof(struct ethhdr) + payload_len; 494 495 /* Create the raw socket */ 496 rawsock = create_raw_socket(ETH_P_ALL); 497 498 if (rawsock < 0) { 499 return 1; 500 } 501 502 /* Bind raw socket to interface */ 503 if (bind_raw_socket(ifname, rawsock, ETH_P_ALL) < 0) { 504 return 2; 505 } 506 507 buildEthernetHeader(tx_packet, SRC_ETHER_ADDR, DST_ETHER_ADDR, 508 payload_len + rcpu_len); 509 510 if (rcpu) { 511 struct L2header l2Header; 512 struct RCPUheader rcpuHeader; 513 struct RCPUmetadata metadata; 514 515 relocateEthernetHeader(tx_packet, payload_len); 516 buildL2Header(&l2Header, SRC_ETHER_ADDR, DST_ETHER_ADDR, 0x81000001, 0xDE08); 517 buildRCPUheader(&rcpuHeader, RCPU_SIGNATURE, 0x20, (port >= 0) ? 0x04 : 0x00, 0, 518 tx_len, 0, 0); 519 buildRCPUmetadata(&metadata, port); 520 521 packL2header(tx_packet, &l2Header); 522 packRCPUheader(tx_packet, &rcpuHeader); 523 packMetadata(tx_packet, &metadata); 524 525 tx_len += RCPUENCAP_SIZE; 526 } 527 if (verbose) { 528 dump_packet(tx_packet, tx_len); 529 } 530 if (verbose) { 531 printf("Transmitting %d %d byte packets; payload len=%d; port=%d\n", 532 max_packets, tx_len, payload_len, port); 533 } 534 for (packet_count = 0; packet_count < max_packets; packet_count++) { 535 int this_tx_len = 536 random_packet_sizes ? (tx_len - ((random() % (MAX_PAYLOAD - MIN_PAYLOAD)) + 537 MIN_PAYLOAD)) : tx_len; 538 tx_packet[22 + tag_len + rcpu_len] = 0x80 + (packet_count & 0x7f); 539 if ((send_raw_packet(rawsock, tx_packet, this_tx_len) == 0) && (vverbose)) { 540 printf("Packet #%d (%d bytes) sent successfully\n", packet_count + 1, 541 this_tx_len); 542 } 543 if (inter_packet_delay) { 544 usleep(inter_packet_delay); 545 } 546 } 547 printf("Transmitted %d packets, DONE\n", max_packets); 548 549 close(rawsock); 550 551 return 0; 552 }