Annotation of ansh/src/utils.c, revision 1.1
1.1 ! misho 1: /*************************************************************************
! 2: * (C) 2011 AITNET - Sofia/Bulgaria - <office@aitnet.org>
! 3: * by Michael Pounov <misho@elwix.org>
! 4: *
! 5: * $Author: misho $
! 6: * $Id: global.h,v 1.2 2011/06/08 12:45:40 misho Exp $
! 7: *
! 8: *************************************************************************/
! 9: #include "global.h"
! 10:
! 11:
! 12: void
! 13: Get1stEth(char *psDev, int devlen)
! 14: {
! 15: struct ifaddrs *ifa;
! 16:
! 17: assert(psDev);
! 18: assert(devlen > 0);
! 19:
! 20: getifaddrs(&ifa);
! 21: strlcpy(psDev, ifa->ifa_name, devlen);
! 22: freeifaddrs(ifa);
! 23: }
! 24:
! 25: int
! 26: PrepareL2(const char *psDev, int *bpflen)
! 27: {
! 28: int h, n = 1;
! 29: register int i;
! 30: char szStr[STRSIZ];
! 31: struct ifreq ifr;
! 32:
! 33: FTRACE(3);
! 34: assert(psDev);
! 35:
! 36: for (i = 0; i < 10; i++) {
! 37: memset(szStr, 0, sizeof szStr);
! 38: snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
! 39: h = open(szStr, O_RDWR);
! 40: if (h > 2)
! 41: break;
! 42: }
! 43: if (h < 3) {
! 44: printf("Error:: open bpf %s #%d - %s\n", szStr, errno, strerror(errno));
! 45: return -1;
! 46: }
! 47:
! 48: strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name);
! 49: if (ioctl(h, BIOCSETIF, &ifr) == -1) {
! 50: printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
! 51: close(h);
! 52: return -1;
! 53: }
! 54: if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {
! 55: printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
! 56: close(h);
! 57: return -1;
! 58: }
! 59: if (ioctl(h, BIOCGBLEN, bpflen) == -1) {
! 60: printf("Error:: get interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
! 61: close(h);
! 62: return -1;
! 63: }
! 64:
! 65: VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);
! 66: return h;
! 67: }
! 68:
! 69: int
! 70: PrepareL3(const struct sockaddr *sa, int *bpflen)
! 71: {
! 72: int h, n = 1;
! 73:
! 74: FTRACE(3);
! 75: assert(sa);
! 76:
! 77: h = socket(sa->sa_family, SOCK_RAW, IPPROTO_ICMP);
! 78: if (h == -1) {
! 79: printf("Error:: Cant open raw socket #%d - %s\n", errno, strerror(errno));
! 80: return -1;
! 81: }
! 82: /*
! 83: if (setsockopt(h, SOL_SOCKET, SO_REUSEADDR, &n, sizeof n) == -1) {
! 84: printf("Error:: Cant set raw socket #%d - %s\n", errno, strerror(errno));
! 85: close(h);
! 86: return -1;
! 87: }
! 88: */
! 89: if (bind(h, sa, sizeof(struct sockaddr)) == -1) {
! 90: printf("Error:: Cant bind to raw socket #%d - %s\n", errno, strerror(errno));
! 91: close(h);
! 92: return -1;
! 93: }
! 94:
! 95: n = fcntl(h, F_GETFL);
! 96: fcntl(h, F_SETFL, n | O_NONBLOCK);
! 97:
! 98: *bpflen = USHRT_MAX;
! 99: VERB(3) LOG("Openned socket handle %d", h);
! 100: return h;
! 101: }
! 102:
! 103: char
! 104: icmpRecv(int s, u_short * __restrict id, u_char * __restrict data,
! 105: int * __restrict datlen, struct sockaddr *sa, socklen_t *salen)
! 106: {
! 107: int ret = 0;
! 108: struct icmp *icmp;
! 109: struct ansh_hdr *hdr;
! 110: u_char buf[USHRT_MAX] = { 0 };
! 111: u_int crc;
! 112:
! 113: ret = recvfrom(s, buf, sizeof buf, 0, sa, salen);
! 114: if (ret == -1) {
! 115: ERR("Receive recvfrom() #%d - %s", errno, strerror(errno));
! 116: return ANSH_FLG_ERR;
! 117: } else
! 118: VERB(4) LOG("Get packet with len=%d", ret);
! 119:
! 120: /* check header len */
! 121: if (ret < (sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr))) {
! 122: VERB(1) LOG("Discard packet too short %d ...", ret);
! 123: return ANSH_FLG_ERR;
! 124: } else
! 125: icmp = (struct icmp*) (buf + sizeof(struct ip));
! 126:
! 127: /* check echo magic ansh code */
! 128: if (icmp->icmp_type != ICMP_ECHOREPLY || icmp->icmp_code != ANSH_CODE) {
! 129: VERB(3) LOG("Packet isnt for me %d ... icmp_code=%d", ret, icmp->icmp_code);
! 130: return ANSH_FLG_ERR;
! 131: } else
! 132: hdr = (struct ansh_hdr*) (buf + sizeof(struct ip) + sizeof(struct icmp));
! 133:
! 134: /* check version and total size of packet */
! 135: if (hdr->ansh_ver != ANSH_VERSION) {
! 136: VERB(3) LOG("Packet with wrong version ...");
! 137: return ANSH_FLG_ERR;
! 138: }
! 139:
! 140: /* check crc of packet */
! 141: crc = hdr->ansh_crc;
! 142: hdr->ansh_crc ^= hdr->ansh_crc;
! 143: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
! 144: if (crc != hdr->ansh_crc) {
! 145: VERB(3) LOG("Packet with wrong crc ...");
! 146: return ANSH_FLG_ERR;
! 147: }
! 148:
! 149: /* copy data */
! 150: if (data && datlen) {
! 151: memset(data, 0, *datlen);
! 152: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
! 153: memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);
! 154: }
! 155:
! 156: if (id)
! 157: *id = ntohs(icmp->icmp_id);
! 158: return hdr->ansh_flg;
! 159: }
! 160:
! 161: int
! 162: icmpSend(int s, u_short id, char flg, u_char *data, int datlen, struct sockaddr *sa, socklen_t salen)
! 163: {
! 164: u_char *pos, buf[USHRT_MAX] = { 0 };
! 165: struct icmp *icmp;
! 166: struct ansh_hdr *hdr;
! 167: int ret = 0;
! 168:
! 169: assert(data);
! 170: if ((sizeof buf - sizeof(struct icmp) + sizeof(struct ansh_hdr)) < datlen)
! 171: return ANSH_FLG_ERR;
! 172:
! 173: icmp = (struct icmp*) buf;
! 174: hdr = (struct ansh_hdr*) (buf + sizeof(struct icmp));
! 175: pos = buf + sizeof(struct icmp) + sizeof(struct ansh_hdr);
! 176:
! 177: memcpy(pos, data, datlen);
! 178:
! 179: hdr->ansh_ver = ANSH_VERSION;
! 180: hdr->ansh_flg = flg;
! 181: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
! 182: hdr->ansh_crc = 0;
! 183: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
! 184:
! 185: icmp->icmp_type = ICMP_ECHOREPLY;
! 186: icmp->icmp_code = ANSH_CODE;
! 187: icmp->icmp_id = htons(id);
! 188: icmp->icmp_seq = htons(datlen);
! 189: icmp->icmp_cksum = 0;
! 190: icmp->icmp_cksum = crcIP(buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
! 191:
! 192: if ((ret = sendto(s, buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen,
! 193: 0, sa, salen)) == -1) {
! 194: ERR("Send sendto() #%d - %s", errno, strerror(errno));
! 195: return ANSH_FLG_ERR;
! 196: } else
! 197: VERB(4) LOG("Put packet with len=%d", ret);
! 198: if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {
! 199: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
! 200: sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
! 201: return ANSH_FLG_ERR;
! 202: }
! 203:
! 204: return ret;
! 205: }
! 206:
! 207: int
! 208: pktSend(int s, u_short id, char flg, u_char *data, int datlen, struct ether_addr *ea)
! 209: {
! 210: u_char *pos, buf[USHRT_MAX] = { 0 };
! 211: struct ether_header *e = (struct ether_header*) buf;
! 212: struct ansh_hdr *hdr;
! 213: int ret = 0;
! 214:
! 215: assert(data);
! 216: if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
! 217: return ANSH_FLG_ERR;
! 218:
! 219: e->ether_type = htons(id);
! 220: memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);
! 221: hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
! 222: pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);
! 223:
! 224: memcpy(pos, data, datlen);
! 225:
! 226: hdr->ansh_ver = ANSH_VERSION;
! 227: hdr->ansh_flg = flg;
! 228: hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
! 229: hdr->ansh_crc = 0;
! 230: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
! 231:
! 232: if ((ret = write(s, buf, ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)) == -1) {
! 233: ERR("Send packet() #%d - %s", errno, strerror(errno));
! 234: return ANSH_FLG_ERR;
! 235: } else
! 236: VERB(4) LOG("Put packet with len=%d", ret);
! 237: if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
! 238: VERB(3) LOG("Sended data %d is different from source data len %d", ret,
! 239: ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen);
! 240: return ANSH_FLG_ERR;
! 241: }
! 242:
! 243: return ret;
! 244: }
! 245:
! 246: char
! 247: pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth)
! 248: {
! 249: int ret = 0;
! 250: struct bpf_hdr *bpf;
! 251: struct ether_header *e;
! 252: struct ansh_hdr *hdr;
! 253: u_char *buf;
! 254: u_int crc;
! 255:
! 256: if (!eth || !datlen)
! 257: return ANSH_FLG_ERR;
! 258:
! 259: if (!(buf = malloc(*datlen))) {
! 260: ERR("malloc() #%d - %s", errno, strerror(errno));
! 261: return ANSH_FLG_ERR;
! 262: }
! 263:
! 264: ret = read(s, buf, *datlen);
! 265: if (ret == -1) {
! 266: ERR("Receive packet() #%d - %s", errno, strerror(errno));
! 267: free(buf);
! 268: return ANSH_FLG_ERR;
! 269: } else
! 270: VERB(4) LOG("Get packet with len=%d", ret);
! 271:
! 272: /* check header len */
! 273: if (ret < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
! 274: VERB(1) LOG("Discard packet too short %d ...", ret);
! 275: free(buf);
! 276: return ANSH_FLG_ERR;
! 277: } else {
! 278: bpf = (struct bpf_hdr*) buf;
! 279: e = (struct ether_header*) (buf + bpf->bh_hdrlen);
! 280: memcpy(eth, e, ETHER_HDR_LEN);
! 281: hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
! 282: }
! 283:
! 284: /* check version and total size of packet */
! 285: if (hdr->ansh_ver != ANSH_VERSION) {
! 286: VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
! 287: free(buf);
! 288: return ANSH_FLG_ERR;
! 289: }
! 290: /* check crc of packet */
! 291: crc = hdr->ansh_crc;
! 292: hdr->ansh_crc ^= hdr->ansh_crc;
! 293: hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
! 294: if (crc != hdr->ansh_crc) {
! 295: VERB(3) LOG("Packet with wrong crc ...");
! 296: free(buf);
! 297: return ANSH_FLG_ERR;
! 298: }
! 299:
! 300: /* copy data */
! 301: if (data) {
! 302: memset(data, 0, *datlen);
! 303: *datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
! 304: memcpy(data, hdr + sizeof(struct ansh_hdr), *datlen);
! 305: }
! 306:
! 307: ret = (char) hdr->ansh_flg;
! 308: free(buf);
! 309: return (char) ret;
! 310: }
! 311:
! 312: void *
! 313: TOfunc(sched_task_t *task)
! 314: {
! 315: struct tagProc *proc;
! 316:
! 317: FTRACE(3);
! 318:
! 319: /* not found argument, drop data */
! 320: if (!(proc = TASK_ARG(task)))
! 321: return (void*) -1;
! 322:
! 323: if (proc->proc_pid)
! 324: kill(proc->proc_pid, SIGTERM);
! 325:
! 326: return NULL;
! 327: }
! 328:
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>
![[BACK]](/icons/cvsweb/back.gif){kind=icon}
Return to utils.c CVS log ![[TXT]](/icons/cvsweb/text.gif){kind=icon}
![[DIR]](/icons/cvsweb/dir.gif){kind=icon}
Up to [ELWIX - Embedded LightWeight unIX -] / ansh / src