├── dcp.o ├── pnio.o ├── MainApp.o ├── getMyIP.o ├── mqtt_pub.o ├── mqtt_sub.o ├── pnet_client ├── profinet.o ├── Device_list.o ├── mqtt_msg_hdlr.o ├── prepareBlock.o ├── config ├── Makefile ├── notes ├── profinet.c ├── getMyIP.c ├── MainApp.c ├── mqtt_pub.c ├── prepareBlock.c ├── Device_list.c ├── mqtt_sub.c ├── mqtt_msg_hdlr.c ├── profinet.h ├── pnio.c └── dcp.c /dcp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/dcp.o -------------------------------------------------------------------------------- /pnio.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/pnio.o -------------------------------------------------------------------------------- /MainApp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/MainApp.o -------------------------------------------------------------------------------- /getMyIP.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/getMyIP.o -------------------------------------------------------------------------------- /mqtt_pub.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/mqtt_pub.o -------------------------------------------------------------------------------- /mqtt_sub.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/mqtt_sub.o -------------------------------------------------------------------------------- /pnet_client: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/pnet_client -------------------------------------------------------------------------------- /profinet.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/profinet.o -------------------------------------------------------------------------------- /Device_list.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/Device_list.o -------------------------------------------------------------------------------- /mqtt_msg_hdlr.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/mqtt_msg_hdlr.o -------------------------------------------------------------------------------- /prepareBlock.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vipulsaini2327/profinet-io-communication/HEAD/prepareBlock.o -------------------------------------------------------------------------------- /config: -------------------------------------------------------------------------------- 1 | #This is the configuration file... 2 | IP_RANGE_MIN 192.168.0.1 3 | IP_RANGE_MAX 192.168.0.254 4 | IDENT_REQ_TIMEOUT 5 5 | INTERFACE_NAME enp2s0 6 | #End of Configuration file. 7 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | src = $(wildcard *.c) 2 | obj = $(src:.c=.o) 3 | 4 | LDFLAGS = -lpthread -ljson-c -lpaho-mqtt3a -lpaho-mqtt3cs 5 | 6 | pnet_client: $(obj) 7 | $(CC) -o $@ $^ $(LDFLAGS) 8 | 9 | .PHONY: clean 10 | clean: 11 | rm -f $(obj) pnet_client 12 | -------------------------------------------------------------------------------- /notes: -------------------------------------------------------------------------------- 1 | mosquitto_pub -t /profinet/request -m "{\"op\":\"read\", \"ip\": \"172.16.1.70\", \"slot\":\"1\", \"sub_slot\":\"1\", \"api\":\"0\"}" 2 | 3 | mosquitto_pub -t /profinet/request -m "{\"op\":\"write\", \"ip\": \"172.16.1.70\", \"slot\":\"1\", \"sub_slot\":\"1\", \"api\":\"0\", \"data\":\"02 01 00 11 22 33 44 55\"}" 4 | 5 | -------------------------------------------------------------------------------- /profinet.c: -------------------------------------------------------------------------------- 1 | /* 2 | Purpose: supervisor for all profinet devices over the local network 3 | Author: Vipul Saini 4 | Date: 4 jan 2019 5 | 6 | */ 7 | 8 | /**************************************headers*******************************************************************/ 9 | 10 | #include"profinet.h" 11 | extern void*create_mqtt_task(void*); 12 | 13 | CC_STATIC_ASSERT(PF_DCP_BLOCK_HDR_SIZE == sizeof(pf_dcp_block_hdr_t)); 14 | 15 | static int request_interval=0; 16 | void read_configurations() 17 | { 18 | FILE *file=fopen("config", "r"); 19 | char line[256]; 20 | int linenum=0; 21 | while(fgets(line, 256, file) != NULL) 22 | { 23 | char key[256], value[256]; 24 | 25 | linenum++; 26 | if(line[0] == '#') continue; 27 | if(sscanf(line, "%s %s", key, value) != 2) 28 | { 29 | fprintf(stderr, "Syntax error, line %d\n", linenum); 30 | continue; 31 | } 32 | if(!strcmp(key, "IDENT_REQ_TIMEOUT")) 33 | { 34 | request_interval=atoi(value); 35 | } 36 | else 37 | if(!strcmp(key, "INTERFACE_NAME")) 38 | { 39 | interface_name=malloc(50); 40 | memcpy(interface_name,value, sizeof(value)); 41 | } 42 | printf("%s %s\n", key, value); 43 | } 44 | } 45 | void *discover(void * arg) 46 | { 47 | while(1) 48 | { 49 | discover_devices(); 50 | discover_devices(); 51 | sleep(request_interval); 52 | } 53 | } 54 | void *recieve(void * arg) 55 | { 56 | while(1) 57 | { 58 | recieve_profinet_packets(); 59 | } 60 | } 61 | int main() 62 | { 63 | start=NULL; 64 | read_configurations(); 65 | pthread_t dis, rec, mqtt; 66 | 67 | pthread_create(&mqtt, NULL, create_mqtt_task,0); 68 | pthread_create(&dis, NULL, discover,0); 69 | pthread_create(&rec, NULL, recieve,0); 70 | while(1) 71 | { 72 | sleep(10); 73 | } 74 | //recieve_profinet_packets(); 75 | //check_ip_address(); 76 | } 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /getMyIP.c: -------------------------------------------------------------------------------- 1 | #include "profinet.h" 2 | 3 | // Returns hostname for the local computer 4 | void checkHostName(int hostname) 5 | { 6 | if (hostname == -1) 7 | { 8 | perror("gethostname"); 9 | exit(1); 10 | } 11 | } 12 | 13 | // Returns host information corresponding to host name 14 | void checkHostEntry(struct hostent * hostentry) 15 | { 16 | if (hostentry == NULL) 17 | { 18 | perror("gethostbyname"); 19 | exit(1); 20 | } 21 | } 22 | 23 | // Converts space-delimited IPv4 addresses 24 | // to dotted-decimal format 25 | void checkIPbuffer(char *IPbuffer) 26 | { 27 | if (NULL == IPbuffer) 28 | { 29 | perror("inet_ntoa"); 30 | exit(1); 31 | } 32 | } 33 | 34 | void getMyIP(char *ip_address) 35 | { 36 | int fd; 37 | struct ifreq ifr; 38 | fd = socket(AF_INET, SOCK_DGRAM, 0); 39 | 40 | ifr.ifr_addr.sa_family = AF_INET; 41 | 42 | memcpy(ifr.ifr_name, "enp2s0", IFNAMSIZ-1); 43 | 44 | ioctl(fd, SIOCGIFADDR, &ifr); 45 | close(fd); 46 | 47 | strcpy(ip_address,inet_ntoa(((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr)); 48 | 49 | } 50 | int check_ip_address() 51 | { 52 | int sockfd = socket(AF_INET, SOCK_STREAM, 0); 53 | 54 | struct sockaddr_in sin; 55 | sin.sin_family = AF_INET; 56 | sin.sin_port = htons(65432); // Could be anything 57 | inet_pton(AF_INET, "172.16.0.70", &sin.sin_addr); 58 | 59 | if (connect(sockfd, (struct sockaddr *) &sin, sizeof(sin)) == -1) 60 | { 61 | printf("Error connecting 172.16.0.70: %d (%s)\n", errno, strerror(errno)); 62 | } 63 | 64 | sin.sin_family = AF_INET; 65 | sin.sin_port = htons(65432); // Could be anything 66 | inet_pton(AF_INET, "172.16.0.7", &sin.sin_addr); 67 | 68 | if (connect(sockfd, (struct sockaddr *) &sin, sizeof(sin)) == -1) 69 | { 70 | printf("Error connecting 172.16.0.7: %d (%s)\n", errno, strerror(errno)); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /MainApp.c: -------------------------------------------------------------------------------- 1 | #include "profinet.h" 2 | 3 | static device *top=NULL; 4 | int number_of_devices=0; 5 | int addNewDevice(device *node) 6 | { 7 | if(NULL ==node) 8 | { 9 | return 0; 10 | } 11 | else 12 | { 13 | device *node=malloc(sizeof(device)); 14 | if(NULL == top) 15 | { 16 | top=node; 17 | } 18 | else 19 | { 20 | device *n=top; 21 | while(n) 22 | { 23 | n=n->next; 24 | } 25 | n->next=node; 26 | } 27 | number_of_devices++; 28 | return 1; 29 | } 30 | } 31 | device* findDeviceByName(char *name) 32 | { 33 | if(NULL != name) 34 | { 35 | if(NULL == top) 36 | { 37 | return 0; 38 | } 39 | else 40 | { 41 | int found=0; 42 | device *node=top; 43 | while(node) 44 | { 45 | if(strcmp(node->device_name, name)==0) 46 | { 47 | found++; 48 | break; 49 | } 50 | node=node->next; 51 | } 52 | if(found) 53 | return node; 54 | } 55 | } 56 | else 57 | return 0; 58 | } 59 | device* findDeviceByIP(uint8_t ip[4]) 60 | { 61 | if(NULL != ip) 62 | { 63 | if(NULL == top) 64 | { 65 | return 0; 66 | } 67 | else 68 | { 69 | int found=0; 70 | device *node=top; 71 | while(node) 72 | { 73 | if((node->ip)[0]==ip[0] && (node->ip)[1]==ip[1] && (node->ip)[2]==ip[2] && (node->ip)[3]==ip[3]) 74 | { 75 | found++; 76 | break; 77 | } 78 | node=node->next; 79 | } 80 | if(found) 81 | return node; 82 | } 83 | } 84 | else 85 | return 0; 86 | } 87 | device* findDeviceByMAC(uint8_t mac[6]) 88 | { 89 | if(NULL != mac) 90 | { 91 | if(NULL == top) 92 | { 93 | return 0; 94 | } 95 | else 96 | { 97 | int found=0; 98 | device *node=top; 99 | while(node) 100 | { 101 | if((node->mac)[0]==mac[0] && (node->mac)[1]==mac[1] && (node->mac)[2]==mac[2] && (node->mac)[3]==mac[3] && (node->mac)[4]==mac[4] && (node->mac)[5]==mac[5]) 102 | { 103 | found++; 104 | break; 105 | } 106 | node=node->next; 107 | } 108 | if(found) 109 | return node; 110 | } 111 | } 112 | else 113 | return 0; 114 | } 115 | int checkDEVICEINLIST(device *node) 116 | { 117 | if(NULL == node) 118 | { 119 | return -1; 120 | } 121 | else 122 | { 123 | 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /mqtt_pub.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "MQTTClient.h" 5 | 6 | #define ADDRESS "tcp://localhost:1883" 7 | #define CLIENTID "ExampleClientPub" 8 | #define TOPIC "/profinet/response" 9 | #define QOS 1 10 | #define TIMEOUT 10000L 11 | 12 | static volatile MQTTClient_deliveryToken deliveredtoken; 13 | static void delivered(void *context, MQTTClient_deliveryToken dt) 14 | { 15 | printf("Message with token value %d delivery confirmed\n", dt); 16 | deliveredtoken = dt; 17 | } 18 | 19 | static int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message) 20 | { 21 | int i; 22 | char* payloadptr; 23 | printf("Message arrived\n"); 24 | printf(" topic: %s\n", topicName); 25 | printf(" message: "); 26 | payloadptr = message->payload; 27 | for(i=0; ipayloadlen; i++) 28 | { 29 | putchar(*payloadptr++); 30 | } 31 | putchar('\n'); 32 | MQTTClient_freeMessage(&message); 33 | MQTTClient_free(topicName); 34 | return 1; 35 | } 36 | 37 | static void connlost(void *context, char *cause) 38 | { 39 | printf("\nConnection lost\n"); 40 | printf(" cause: %s\n", cause); 41 | } 42 | 43 | static int mqtt_pub(char *addr, char *topic, char *payload, unsigned int len) 44 | { 45 | MQTTClient client; 46 | MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; 47 | MQTTClient_message pubmsg = MQTTClient_message_initializer; 48 | MQTTClient_deliveryToken token; 49 | int rc; 50 | MQTTClient_create(&client, addr, CLIENTID, 51 | MQTTCLIENT_PERSISTENCE_NONE, NULL); 52 | conn_opts.keepAliveInterval = 20; 53 | conn_opts.cleansession = 1; 54 | MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered); 55 | if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) 56 | { 57 | printf("Failed to connect, return code %d\n", rc); 58 | exit(EXIT_FAILURE); 59 | } 60 | pubmsg.payload = payload; 61 | pubmsg.payloadlen = len; 62 | pubmsg.qos = QOS; 63 | pubmsg.retained = 0; 64 | deliveredtoken = 0; 65 | MQTTClient_publishMessage(client, topic, &pubmsg, &token); 66 | printf("Waiting for publication of %s\n" 67 | "on topic %s for client with ClientID: %s\n", 68 | payload, topic, CLIENTID); 69 | while(deliveredtoken != token); 70 | MQTTClient_disconnect(client, 10000); 71 | MQTTClient_destroy(&client); 72 | return rc; 73 | } 74 | 75 | int send_mqtt_pub(char *addr, char *topic, char *payload, unsigned int len) 76 | { 77 | return mqtt_pub(addr, topic, payload, len); 78 | } 79 | 80 | int send_mqtt_pub_default(char *payload, unsigned int len) 81 | { 82 | return mqtt_pub(ADDRESS, TOPIC, payload, len); 83 | } 84 | 85 | -------------------------------------------------------------------------------- /prepareBlock.c: -------------------------------------------------------------------------------- 1 | #include "profinet.h" 2 | 3 | void pf_put_mem(const void *p_src, uint16_t src_size, /* Bytes to copy */ uint16_t res_len,/* Sizeof p_bytes buf */ uint8_t *p_bytes, uint16_t *p_pos) 4 | { 5 | if (((*p_pos) + src_size) >= res_len) 6 | { 7 | p_bytes = NULL; 8 | } 9 | 10 | if (p_bytes != NULL) 11 | { 12 | memcpy(&p_bytes[*p_pos], p_src, src_size); 13 | (*p_pos) += src_size; 14 | } 15 | } 16 | 17 | void pf_put_byte(uint8_t val, uint16_t res_len, uint8_t *p_bytes, uint16_t *p_pos) 18 | { 19 | if (*p_pos >= res_len) 20 | { 21 | p_bytes = NULL; 22 | } 23 | 24 | if (p_bytes != NULL) 25 | { 26 | p_bytes[*p_pos] = val; 27 | (*p_pos)++; 28 | } 29 | } 30 | void pf_put_uint16(bool is_big_endian, uint16_t val, uint16_t res_len, uint8_t *p_bytes, uint16_t *p_pos) 31 | { 32 | if (is_big_endian) 33 | { 34 | pf_put_byte((val >> 8) & 0xff, res_len, p_bytes, p_pos); 35 | pf_put_byte(val & 0xff, res_len, p_bytes, p_pos); 36 | } 37 | else 38 | { 39 | pf_put_byte(val & 0xff, res_len, p_bytes, p_pos); 40 | pf_put_byte((val >> 8) & 0xff, res_len, p_bytes, p_pos); 41 | } 42 | } 43 | void pf_put_uint32(bool is_big_endian, uint32_t val, uint16_t res_len, uint8_t *p_bytes, uint16_t *p_pos) 44 | { 45 | if (is_big_endian) 46 | { 47 | pf_put_byte((val >> 24) & 0xff, res_len, p_bytes, p_pos); 48 | pf_put_byte((val >> 16) & 0xff, res_len, p_bytes, p_pos); 49 | pf_put_byte((val >> 8) & 0xff, res_len, p_bytes, p_pos); 50 | pf_put_byte(val & 0xff, res_len, p_bytes, p_pos); 51 | } 52 | else 53 | { 54 | pf_put_byte(val & 0xff, res_len, p_bytes, p_pos); 55 | pf_put_byte((val >> 8) & 0xff, res_len, p_bytes, p_pos); 56 | pf_put_byte((val >> 16) & 0xff, res_len, p_bytes, p_pos); 57 | pf_put_byte((val >> 24) & 0xff, res_len, p_bytes, p_pos); 58 | } 59 | } 60 | int pf_dcp_put_block(uint8_t *p_dst, uint16_t *p_dst_pos, uint16_t dst_max, uint8_t opt, uint8_t sub, bool with_block_info, uint16_t block_info, uint16_t value_length, const void *p_value) 61 | { 62 | int ret = -1; 63 | uint16_t b_len; 64 | 65 | if ((*p_dst_pos + value_length) < dst_max) 66 | { 67 | b_len = value_length;//Adjust written block length if block_info is included 68 | 69 | pf_put_byte(opt, dst_max, p_dst, p_dst_pos); 70 | pf_put_byte(sub, dst_max, p_dst, p_dst_pos); 71 | pf_put_uint16(true, b_len, dst_max, p_dst, p_dst_pos); 72 | 73 | if ((p_value != NULL) && (value_length > 0)) 74 | { 75 | pf_put_mem(p_value, value_length, dst_max, p_dst, p_dst_pos); 76 | } 77 | 78 | 79 | while ((*p_dst_pos) & 1)// Add padding to align on uint16_t 80 | { 81 | pf_put_byte(0, dst_max, p_dst, p_dst_pos); 82 | } 83 | } 84 | 85 | ret = 0; // Skip excess data silently !! 86 | 87 | return ret; 88 | } 89 | 90 | os_buf_t * os_buf_alloc(uint16_t length) 91 | { 92 | os_buf_t *p = malloc(sizeof(os_buf_t) + length); 93 | 94 | if (p != NULL) 95 | { 96 | memset(p, 0x0, sizeof(os_buf_t) + length); 97 | #if 1 98 | p->payload = (void *)((uint8_t *)p + sizeof(os_buf_t)); /* Payload follows header struct */ 99 | p->len = length; 100 | #endif 101 | } 102 | else 103 | { 104 | printf("malloc() failed\n"); 105 | } 106 | return p; 107 | } 108 | 109 | -------------------------------------------------------------------------------- /Device_list.c: -------------------------------------------------------------------------------- 1 | /* 2 | Purpose: maintain a Linked List of all devices and their properties 3 | Author: Vipul Saini 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | typedef struct Device 12 | { 13 | char *name; 14 | uint8_t mac[6]; 15 | uint8_t ip[4]; 16 | uint8_t subnet[4]; 17 | uint8_t gateway[4]; 18 | struct Device *next; 19 | }devices; 20 | 21 | 22 | uint8_t gateway[4]={192,168,1,7}; 23 | uint8_t ip_range_min[4]={192,168,1,1}; 24 | uint8_t ip_range_max[4]={192,168,1,254}; 25 | uint8_t subnet[4]={255,255,255,0}; 26 | 27 | 28 | 29 | devices *create_device() 30 | { 31 | devices *node=malloc(sizeof(devices)); 32 | return node; 33 | } 34 | 35 | int add_device(devices **head, uint8_t mac[6]) 36 | { 37 | if(mac) 38 | { 39 | devices *node=create_device(); 40 | memcpy(node->mac,mac, 6); 41 | if(head) 42 | { 43 | node->next=*head; 44 | } 45 | *head=node; 46 | return 1; 47 | } 48 | else 49 | return 0; 50 | } 51 | 52 | int add_ip_to_device(devices **head, uint8_t mac[6]) 53 | { 54 | devices *node; 55 | int found=0; 56 | if(head) 57 | { 58 | node=*head; 59 | while(node) 60 | { 61 | if(memcmp(node->mac, mac, 6)==0) 62 | { 63 | found=1; 64 | break; 65 | } 66 | node=node->next; 67 | } 68 | if(found==1) 69 | { 70 | if(ip_range_min[3]ip, ip_range_min, 4); 73 | memcpy(node->subnet, subnet, 4); 74 | memcpy(node->gateway, gateway, 4); 75 | (ip_range_min[3])+=1; 76 | printf("assigned an ip :%d.%d.%d.%d\n", ip_range_min[0], ip_range_min[1], ip_range_min[2], ip_range_min[3]); 77 | printf("assigned a gateway :%d.%d.%d.%d\n", gateway[0], gateway[1], gateway[2], gateway[3]); 78 | printf("assigned a subnet mask :%d.%d.%d.%d\n", subnet[0], subnet[1], subnet[2], subnet[3]); 79 | } 80 | else 81 | { 82 | return 0; 83 | // to do 84 | if(subnet[3]==0) 85 | { 86 | 87 | } 88 | } 89 | } 90 | } 91 | else 92 | return 0; 93 | } 94 | devices *find_device_by_mac(devices **head, uint8_t mac[6]) 95 | { 96 | int found=0; 97 | if(*head) 98 | { 99 | devices *node=*head; 100 | node=*head; 101 | while(node) 102 | { 103 | if(memcmp(mac, node->mac, 6)==0) 104 | { 105 | found =1; 106 | break; 107 | } 108 | node=node->next; 109 | } 110 | if(found) 111 | return node; 112 | else 113 | return 0; 114 | } 115 | else 116 | { 117 | return 0; 118 | } 119 | } 120 | 121 | devices *find_device_by_ip(devices **head, uint8_t ip[4]) 122 | { 123 | if(*head) 124 | { 125 | int found=0; 126 | devices *node=*head; 127 | node=*head; 128 | while(node) 129 | { 130 | if(memcmp(ip, node->ip, 4)==0) 131 | { 132 | found=1; 133 | break; 134 | } 135 | node=node->next; 136 | } 137 | if(found) 138 | return node; 139 | else 140 | return 0; 141 | } 142 | else 143 | { 144 | return 0; 145 | } 146 | } 147 | 148 | void show_all_devices(devices **head) 149 | { 150 | if(*head) 151 | { 152 | devices *node=*head; 153 | node=*head; 154 | int count=0; 155 | while(node) 156 | { 157 | printf("Device %d:\nIP- %d.%d.%d.%d\nSubnet Mask- %d.%d.%d.%d\nGateway- %d.%d.%d.%d\nMAC- %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n\n",++count, (node->ip)[0],(node->ip)[1],(node->ip)[2],(node->ip)[3], (node->subnet)[0], (node->subnet)[1], (node->subnet)[2], (node->subnet)[3], (node->gateway)[0], (node->gateway)[1], (node->gateway)[2], (node->gateway)[3], (node->mac)[0], (node->mac)[1], (node->mac)[2], (node->mac)[3], (node->mac)[4], (node->mac)[5]); 158 | node=node->next; 159 | } 160 | } 161 | else 162 | { 163 | printf("nothing to show, list is empty\n"); 164 | } 165 | } 166 | 167 | /* 168 | void main() 169 | { 170 | devices *head=NULL; 171 | uint8_t mac[6]={0xac,0xb9,0xff,0x21,0xaf,0xac}; 172 | uint8_t ip[4]={192,168,1,2}; 173 | add_device(&head, mac); 174 | printf("Device with mac %.2x:%.2x:%.2x:%.2x:%.2x:%.2x added to the list \n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); 175 | add_ip_to_device(&head, mac); 176 | show_all_devices(&head); 177 | devices *node=0; 178 | node=find_device_by_mac(&head, mac); 179 | if(node) 180 | printf("device found\n"); 181 | else 182 | printf("device not found\n"); 183 | node=0; 184 | node=find_device_by_ip(&head, ip); 185 | if(node) 186 | printf("device found\n"); 187 | else 188 | printf("device not found\n"); 189 | 190 | free(head); 191 | } 192 | */ 193 | -------------------------------------------------------------------------------- /mqtt_sub.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "MQTTAsync.h" 6 | 7 | #define ADDRESS "tcp://localhost:1883" 8 | #define CLIENTID "ExampleClientSub" 9 | #define TOPIC "/profinet/request" 10 | #define QOS 1 11 | #define TIMEOUT 10000L 12 | 13 | extern int mqtt_msg_hdlr(char *,int); 14 | 15 | volatile MQTTAsync_token deliveredtoken; 16 | int disc_finished = 0; 17 | int subscribed = 0; 18 | int finished = 0; 19 | void connlost(void *context, char *cause) 20 | { 21 | MQTTAsync client = (MQTTAsync)context; 22 | MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer; 23 | int rc; 24 | printf("\nConnection lost\n"); 25 | printf(" cause: %s\n", cause); 26 | printf("Reconnecting\n"); 27 | conn_opts.keepAliveInterval = 20; 28 | conn_opts.cleansession = 1; 29 | if ((rc = MQTTAsync_connect(client, &conn_opts)) != MQTTASYNC_SUCCESS) 30 | { 31 | printf("Failed to start connect, return code %d\n", rc); 32 | finished = 1; 33 | } 34 | } 35 | int msgarrvd(void *context, char *topicName, int topicLen, MQTTAsync_message *message) 36 | { 37 | int i; 38 | char* payloadptr; 39 | printf("Message arrived\n"); 40 | printf(" topic: %s\n", topicName); 41 | printf(" message: "); 42 | payloadptr = message->payload; 43 | for(i=0; ipayloadlen; i++) 44 | { 45 | putchar(*payloadptr++); 46 | } 47 | putchar('\n'); 48 | 49 | mqtt_msg_hdlr(message->payload, message->payloadlen); 50 | 51 | MQTTAsync_freeMessage(&message); 52 | MQTTAsync_free(topicName); 53 | return 1; 54 | } 55 | void onDisconnect(void* context, MQTTAsync_successData* response) 56 | { 57 | printf("Successful disconnection\n"); 58 | disc_finished = 1; 59 | } 60 | void onSubscribe(void* context, MQTTAsync_successData* response) 61 | { 62 | printf("Subscribe succeeded\n"); 63 | subscribed = 1; 64 | } 65 | void onSubscribeFailure(void* context, MQTTAsync_failureData* response) 66 | { 67 | printf("Subscribe failed, rc %d\n", response ? response->code : 0); 68 | finished = 1; 69 | } 70 | void onConnectFailure(void* context, MQTTAsync_failureData* response) 71 | { 72 | printf("Connect failed, rc %d\n", response ? response->code : 0); 73 | finished = 1; 74 | } 75 | void onConnect(void* context, MQTTAsync_successData* response) 76 | { 77 | MQTTAsync client = (MQTTAsync)context; 78 | MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer; 79 | MQTTAsync_message pubmsg = MQTTAsync_message_initializer; 80 | int rc; 81 | printf("Successful connection\n"); 82 | printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n" 83 | "Press Q to quit\n\n", TOPIC, CLIENTID, QOS); 84 | opts.onSuccess = onSubscribe; 85 | opts.onFailure = onSubscribeFailure; 86 | opts.context = client; 87 | deliveredtoken = 0; 88 | if ((rc = MQTTAsync_subscribe(client, TOPIC, QOS, &opts)) != MQTTASYNC_SUCCESS) 89 | { 90 | printf("Failed to start subscribe, return code %d\n", rc); 91 | exit(EXIT_FAILURE); 92 | } 93 | } 94 | 95 | #ifdef _MAIN_ 96 | int main(int argc, char* argv[]) 97 | #else 98 | void *create_mqtt_task(void*args) 99 | #endif 100 | { 101 | MQTTAsync client; 102 | MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer; 103 | MQTTAsync_disconnectOptions disc_opts = MQTTAsync_disconnectOptions_initializer; 104 | MQTTAsync_message pubmsg = MQTTAsync_message_initializer; 105 | MQTTAsync_token token; 106 | int rc; 107 | int ch; 108 | MQTTAsync_create(&client, ADDRESS, CLIENTID, MQTTCLIENT_PERSISTENCE_NONE, NULL); 109 | MQTTAsync_setCallbacks(client, NULL, connlost, msgarrvd, NULL); 110 | conn_opts.keepAliveInterval = 20; 111 | conn_opts.cleansession = 1; 112 | conn_opts.onSuccess = onConnect; 113 | conn_opts.onFailure = onConnectFailure; 114 | conn_opts.context = client; 115 | if ((rc = MQTTAsync_connect(client, &conn_opts)) != MQTTASYNC_SUCCESS) 116 | { 117 | printf("Failed to start connect, return code %d\n", rc); 118 | exit(EXIT_FAILURE); 119 | } 120 | while (!subscribed) 121 | #if defined(WIN32) || defined(WIN64) 122 | Sleep(100); 123 | #else 124 | usleep(10000L); 125 | #endif 126 | if (finished) 127 | goto exit; 128 | do 129 | { 130 | sleep(10); 131 | } while (1); 132 | disc_opts.onSuccess = onDisconnect; 133 | if ((rc = MQTTAsync_disconnect(client, &disc_opts)) != MQTTASYNC_SUCCESS) 134 | { 135 | printf("Failed to start disconnect, return code %d\n", rc); 136 | exit(EXIT_FAILURE); 137 | } 138 | while (!disc_finished) 139 | #if defined(WIN32) || defined(WIN64) 140 | Sleep(100); 141 | #else 142 | usleep(10000L); 143 | #endif 144 | exit: 145 | MQTTAsync_destroy(&client); 146 | return NULL; 147 | } 148 | 149 | -------------------------------------------------------------------------------- /mqtt_msg_hdlr.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | extern int send_mqtt_pub_default(char*,int); 6 | 7 | typedef struct 8 | { 9 | char ip[32]; 10 | char op[18]; 11 | char api[64]; 12 | char slot[16]; 13 | char sub_slot[16]; 14 | char data[128]; 15 | }profinet_req_t; 16 | 17 | int json_to_struct(char *buffer, profinet_req_t *out) 18 | { 19 | struct json_object *parsed_json= NULL; 20 | struct json_object *ip = NULL, *op= NULL, *api= NULL, *slot= NULL, *sub_slot = NULL, *data=NULL; 21 | size_t i; 22 | 23 | parsed_json = json_tokener_parse(buffer); 24 | 25 | if(!parsed_json) 26 | { 27 | return -1; 28 | } 29 | json_object_object_get_ex(parsed_json, "ip", &ip); 30 | json_object_object_get_ex(parsed_json, "op", &op); 31 | json_object_object_get_ex(parsed_json, "api", &api); 32 | json_object_object_get_ex(parsed_json, "slot", &slot); 33 | json_object_object_get_ex(parsed_json, "sub_slot", &sub_slot); 34 | json_object_object_get_ex(parsed_json, "data", &data); 35 | 36 | if(ip) 37 | { 38 | printf("IP: %s\n", json_object_get_string(ip)); 39 | strcpy(out->ip, json_object_get_string(ip)); 40 | json_object_put(ip); 41 | } 42 | if(op) 43 | { 44 | printf("OP: %s\n", json_object_get_string(op)); 45 | strcpy(out->op, json_object_get_string(op)); 46 | json_object_put(op); 47 | } 48 | if(api) 49 | { 50 | printf("API: %s\n", json_object_get_string(api)); 51 | strcpy(out->api, json_object_get_string(api)); 52 | json_object_put(api); 53 | } 54 | if(slot) 55 | { 56 | printf("SLOT: %s\n", json_object_get_string(slot)); 57 | strcpy(out->slot, json_object_get_string(slot)); 58 | json_object_put(slot); 59 | } 60 | if(sub_slot) 61 | { 62 | printf("SUB Slot: %s\n", json_object_get_string(sub_slot)); 63 | strcpy(out->sub_slot, json_object_get_string(sub_slot)); 64 | json_object_put(sub_slot); 65 | } 66 | if(data) 67 | { 68 | printf("Data: %s\n", json_object_get_string(data)); 69 | strcpy(out->data, json_object_get_string(data)); 70 | json_object_put(data); 71 | } 72 | 73 | return 0; 74 | } 75 | 76 | 77 | int HexToInt( char* Hex ) 78 | { 79 | int nLen = strlen( Hex ); 80 | int nDigitMult = 1; 81 | int nResult = 0; 82 | for( int i = nLen - 1; i >= 0; i-- ) 83 | { 84 | char ch = Hex[ i ]; 85 | if( '0' <= ch && ch <= '9' ) 86 | nResult += (ch - '0') * nDigitMult; 87 | else if( 'a' <= ch && ch <= 'f' ) 88 | nResult += (ch - 'a' + 10) * nDigitMult; 89 | else if( 'A' <= ch && ch <= 'F' ) 90 | nResult += (ch - 'A' + 10) * nDigitMult; 91 | nDigitMult *= 16; 92 | } 93 | return nResult; 94 | } 95 | 96 | int mqtt_msg_hdlr(char *payload, int len) 97 | { 98 | profinet_req_t out; 99 | 100 | memset(&out, 0x0, sizeof(out)); 101 | json_to_struct(payload, &out); 102 | printf("IP - %s, OP - %s\n", out.ip, out.op); 103 | if(strcasecmp(out.op, "read") == 0) 104 | { 105 | /* call read */ 106 | int i = 0, len = 0; 107 | char response[2048]={0}, mqtt_res[2048] = {0}; 108 | 109 | send_read_IO_Packet(out.ip, ntohl(atoi(out.api)), ntohs(atoi(out.slot)), ntohs(atoi(out.sub_slot)), response, &len); 110 | 111 | printf("response = ["); 112 | for(i=0;i 82 | 83 | static inline void cc_assert (int exp) 84 | { 85 | assert (exp); // LCOV_EXCL_LINE 86 | } 87 | 88 | #define CC_PACKED_BEGIN 89 | #define CC_PACKED_END 90 | #define CC_PACKED __attribute__((packed)) 91 | 92 | #define CC_FORMAT(str,arg) __attribute__((format (printf, str, arg))) 93 | 94 | #if BYTE_ORDER == LITTLE_ENDIAN 95 | #define CC_TO_LE16(x) (x) 96 | #define CC_TO_LE32(x) (x) 97 | #define CC_TO_LE64(x) (x) 98 | #define CC_FROM_LE16(x) (x) 99 | #define CC_FROM_LE32(x) (x) 100 | #define CC_FROM_LE64(x) (x) 101 | #else 102 | #define CC_TO_LE16(x) __builtin_bswap16 (x) 103 | #define CC_TO_LE32(x) __builtin_bswap32 (x) 104 | #define CC_TO_LE64(x) __builtin_bswap64 (x) 105 | #define CC_FROM_LE16(x) __builtin_bswap16 (x) 106 | #define CC_FROM_LE32(x) __builtin_bswap32 (x) 107 | #define CC_FROM_LE64(x) __builtin_bswap64 (x) 108 | #endif 109 | 110 | #define CC_ATOMIC_GET8(p) __atomic_load_n ((p), __ATOMIC_SEQ_CST) 111 | #define CC_ATOMIC_GET16(p) __atomic_load_n ((p), __ATOMIC_SEQ_CST) 112 | #define CC_ATOMIC_GET32(p) __atomic_load_n ((p), __ATOMIC_SEQ_CST) 113 | #define CC_ATOMIC_GET64(p) __atomic_load_n ((p), __ATOMIC_SEQ_CST) 114 | 115 | #define CC_ATOMIC_SET8(p, v) __atomic_store_n ((p), (v), __ATOMIC_SEQ_CST) 116 | #define CC_ATOMIC_SET16(p, v) __atomic_store_n ((p), (v), __ATOMIC_SEQ_CST) 117 | #define CC_ATOMIC_SET32(p, v) __atomic_store_n ((p), (v), __ATOMIC_SEQ_CST) 118 | #define CC_ATOMIC_SET64(p, v) __atomic_store_n ((p), (v), __ATOMIC_SEQ_CST) 119 | 120 | #define CC_ASSERT(exp) cc_assert (exp) 121 | #ifdef __cplusplus 122 | #define CC_STATIC_ASSERT(exp) static_assert (exp, "") 123 | #else 124 | #define CC_STATIC_ASSERT(exp) _Static_assert (exp, "") 125 | #endif 126 | 127 | #ifdef __cplusplus 128 | } 129 | #endif 130 | 131 | #endif 132 | 133 | #include 134 | #include 135 | #include 136 | #include 137 | #include 138 | #include 139 | #include 140 | #include 141 | #include 142 | #include 143 | #include 144 | #include 145 | #include 146 | #include 147 | #include 148 | #include 149 | #include 150 | #include 151 | #include 152 | #include 153 | #include 154 | #include 155 | #include 156 | #include 157 | #include 158 | 159 | /**************************************structures*******************************************************************/ 160 | typedef struct os_ethaddr 161 | { 162 | uint8_t addr[6]; 163 | } os_ethaddr_t; 164 | 165 | typedef struct os_ethhdr 166 | { 167 | os_ethaddr_t dest; 168 | os_ethaddr_t src; 169 | uint16_t type; 170 | } os_ethhdr_t; 171 | 172 | typedef struct os_buf 173 | { 174 | void * payload; 175 | uint16_t len; 176 | }os_buf_t; 177 | 178 | struct pack 179 | { 180 | uint8_t source_mac[6]; 181 | uint8_t dest_mac[6]; 182 | uint8_t source[6]; 183 | uint16_t packet_type; 184 | uint16_t frame_id; 185 | uint8_t service_id; 186 | uint8_t service_type; 187 | uint32_t xid; 188 | uint16_t reserve_or_delay; 189 | uint16_t ddl; 190 | }; 191 | struct block 192 | { 193 | uint8_t option; 194 | uint8_t suboption; 195 | int block_length; 196 | uint16_t block_info; 197 | }; 198 | 199 | CC_PACKED_BEGIN 200 | typedef struct CC_PACKED pf_dcp_header 201 | { 202 | uint8_t service_id; 203 | uint8_t service_type; 204 | uint32_t xid; 205 | uint16_t response_delay_factor; 206 | uint16_t data_length; 207 | } pf_dcp_header_t; 208 | CC_PACKED_END 209 | CC_STATIC_ASSERT(PF_DCP_HEADER_SIZE == sizeof(pf_dcp_header_t)); 210 | 211 | CC_PACKED_BEGIN 212 | typedef struct CC_PACKED pf_dcp_block_hdr 213 | { 214 | uint8_t option; 215 | uint8_t sub_option; 216 | uint16_t block_length; 217 | } pf_dcp_block_hdr_t; 218 | CC_PACKED_END 219 | 220 | CC_PACKED_BEGIN 221 | typedef struct CC_PACKED os_ipaddr 222 | { 223 | uint8_t addr[4]; 224 | } os_ipaddr_t; 225 | CC_PACKED_END 226 | 227 | CC_PACKED_BEGIN 228 | typedef struct CC_PACKED os_iphdr 229 | { 230 | uint8_t vrsnANDheadLen; 231 | uint8_t DiffServField; 232 | uint16_t length; 233 | uint16_t identification; 234 | uint16_t flags; 235 | uint8_t ttl; 236 | uint8_t protocol; 237 | uint16_t checksum; 238 | os_ipaddr_t src; 239 | os_ipaddr_t dest; 240 | 241 | } os_iphdr_t; 242 | CC_PACKED_END 243 | 244 | CC_PACKED_BEGIN 245 | typedef struct CC_PACKED DCEorRPC 246 | { 247 | uint8_t version;//1 248 | uint8_t packet_type;//1 249 | uint16_t flag;//1 250 | uint8_t data_rep[3];//3 251 | uint8_t serial_high;//1 252 | uint8_t object_uuid[16];//16 253 | uint8_t interface[16];//16 254 | uint8_t activity[16];//16 255 | uint32_t server_boot;//4 256 | uint32_t interface_version;//4 257 | uint32_t sequence;//4 258 | uint16_t option;//2 259 | uint16_t interface_hint;//2 260 | uint16_t activity_hint;//2 261 | uint16_t fragment_length;//2 262 | uint16_t fragment_num;//2 263 | uint8_t auth_proto;//1 264 | uint8_t serial_low;//1 265 | } os_DCEorRPC_t; 266 | CC_PACKED_END 267 | 268 | CC_PACKED_BEGIN 269 | typedef struct CC_PACKED block_hdr 270 | { 271 | uint16_t block_type; 272 | uint16_t length; 273 | uint8_t version_high; 274 | uint8_t version_low; 275 | } os_blockhdr_t; 276 | CC_PACKED_END 277 | 278 | CC_PACKED_BEGIN 279 | typedef struct CC_PACKED IOD_read_reqhdr 280 | { 281 | os_blockhdr_t block; 282 | uint16_t seq_nbr; 283 | uint8_t ar_uuid[16]; 284 | uint32_t api; 285 | uint16_t slot; 286 | uint16_t subslot; 287 | uint16_t padding; 288 | uint16_t index; 289 | uint32_t record_data_length; 290 | uint8_t target_ar_uuid[16]; 291 | uint32_t padding2[2]; 292 | } os_read_reqhdr_t; 293 | CC_PACKED_END 294 | 295 | CC_PACKED_BEGIN 296 | typedef struct CC_PACKED IOD_write_reqhdr 297 | { 298 | os_blockhdr_t block; 299 | uint16_t seq_nbr; 300 | uint8_t ar_uuid[16]; 301 | uint32_t api; 302 | uint16_t slot; 303 | uint16_t subslot; 304 | uint16_t padding; 305 | uint16_t index; 306 | uint32_t record_data_length; 307 | // uint8_t target_ar_uuid[16]; 308 | uint8_t dataValue[8]; 309 | } os_write_reqhdr_t; 310 | CC_PACKED_END 311 | 312 | CC_PACKED_BEGIN 313 | typedef struct CC_PACKED PN_IO 314 | { 315 | uint32_t max_arg; 316 | uint32_t arg_length; 317 | uint32_t maxcount; 318 | uint32_t offset; 319 | uint32_t actual_count; 320 | os_read_reqhdr_t read_req_header; 321 | } os_pnio_t; 322 | CC_PACKED_END 323 | 324 | CC_PACKED_BEGIN 325 | typedef struct CC_PACKED os_udphdr 326 | { 327 | uint16_t src_port; 328 | uint16_t dst_port; 329 | uint16_t length; 330 | uint16_t checksum; 331 | } os_udphdr_t; 332 | CC_PACKED_END 333 | 334 | typedef struct device_list 335 | { 336 | char *name; 337 | uint8_t *mac; 338 | uint8_t ip[4]; 339 | uint8_t subnet[4]; 340 | uint8_t gateway[4]; 341 | struct device_list *next; 342 | }device_list; 343 | 344 | typedef struct profinet_device 345 | { 346 | char *device_name; 347 | uint8_t mac[6]; 348 | uint8_t ip[4]; 349 | uint8_t subnet[4]; 350 | uint8_t gateway[4]; 351 | struct profinet_device *next; 352 | }device; 353 | 354 | 355 | static device_list *head=NULL; 356 | struct Device *start; 357 | /************************************global variables**************************************************************/ 358 | 359 | static os_ethaddr_t dcp_src_address= 360 | { 361 | {0xc8, 0x1f, 0x66, 0x43, 0x6a, 0x45} 362 | }; 363 | static os_ethaddr_t dcp_mc_address = { {0x01, 0x0e, 0xcf, 0x00, 0x00, 0x00 } }; 364 | 365 | static char *if_name="enp2s0"; 366 | static uint8_t devices[][6]={0}; 367 | static int device_count=0; 368 | char *interface_name; 369 | 370 | /************************************functions declarations*********************************************************/ 371 | void pf_put_mem(const void *, uint16_t , /* Bytes to copy */ uint16_t ,/* Sizeof p_bytes buf */ uint8_t *, uint16_t *); 372 | void pf_put_byte(uint8_t , uint16_t , uint8_t *, uint16_t *); 373 | void pf_put_uint16(bool , uint16_t , uint16_t , uint8_t *, uint16_t *); 374 | void pf_put_uint32(bool , uint32_t , uint16_t , uint8_t *, uint16_t *); 375 | int pf_dcp_put_block(uint8_t *, uint16_t *, uint16_t , uint8_t , uint8_t , bool , uint16_t , uint16_t , const void *); 376 | void send_set_req(uint8_t []); 377 | void send_get_req(uint8_t []); 378 | void discover_devices(); 379 | int check(uint8_t []); 380 | void addDevice(uint8_t []); 381 | int extract_blocks_from_packet(char *, int , device *); 382 | void testIO(uint8_t []); 383 | void recieve_profinet_packets(); 384 | void *discover(void *); 385 | void *recieve(void *); 386 | void saveDevice(device_list *); 387 | device_list* createNewDevice(char *, uint8_t *, uint8_t *, uint8_t *, uint8_t *); 388 | void set_ip_config(uint8_t []); 389 | os_buf_t * os_buf_alloc(uint16_t ); 390 | void getMyIP(char *); 391 | int send_write_IO_Packet(char *ip, uint32_t api, uint16_t slot, uint16_t subslot, uint8_t *data, uint32_t len, uint8_t *response, int *size); 392 | int send_read_IO_Packet(char *ip, uint32_t api, uint16_t slot, uint16_t subslot, uint8_t *response, int *size); 393 | void checkHostName(int); 394 | void checkHostEntry(struct hostent *); 395 | void checkIPbuffer(char *); 396 | int check_ip_address(); 397 | device* findDeviceByMAC(uint8_t [6]); 398 | device* findDeviceByIP(uint8_t [4]); 399 | device* findDeviceByName(char *); 400 | 401 | -------------------------------------------------------------------------------- /pnio.c: -------------------------------------------------------------------------------- 1 | 2 | #include"profinet.h" 3 | 4 | 5 | #define RESERVED_FOR_IMPL_FLAG 0X0001 6 | #define NO_FACK_FLAG 0X0008 7 | #define ACTIVITY_HINT 0xFFFF 8 | #define INTERFACE_HINT 0XFFFF 9 | #define AUTHORISED_PROTOCOL 0x00 10 | #define SERIAL_HIGH 0X00 11 | #define FRAGMENT_LENGTH 0X54 12 | #define FRAGMENT_NUMBER 0X00 13 | #define SERIAL_LOW 0X00 14 | 15 | int send_read_IO_Packet(char *ip, uint32_t api, uint16_t slot, uint16_t subslot, uint8_t *response, int *size) 16 | { 17 | os_buf_t * buf=NULL; 18 | buf=os_buf_alloc(2048); 19 | 20 | uint16_t dst_pos=0; 21 | 22 | //uint8_t ip[4]={*(dev->ip), *(dev->ip+1),*(dev->ip+2),*(dev->ip+3)}; 23 | //uint32_t addr=((uint32_t)ip[0])<<24|((uint32_t)ip[1])<<16|((uint32_t)ip[2])<<8|(uint32_t)ip[3]; 24 | 25 | // Driver code 26 | char hostbuffer[256]; 27 | char IPbuffer[15]; 28 | getMyIP(IPbuffer); 29 | struct hostent *host_entry; 30 | int hostname; 31 | char *nibble=NULL; 32 | int i=0; 33 | uint8_t myIP[4]={0}; 34 | os_ipaddr_t *source_ip=NULL; 35 | //os_ipaddr_t *dest_ip=malloc(4); 36 | 37 | os_ethhdr_t *p_ethhdr; 38 | os_iphdr_t *iphdr; 39 | os_udphdr_t *udphdr; 40 | os_DCEorRPC_t *dceorrpc; 41 | os_pnio_t *pnio_t; 42 | os_read_reqhdr_t *readreqhdr; 43 | os_blockhdr_t *ioblock; 44 | 45 | uint8_t *p_dst; 46 | p_dst=(uint8_t *)buf->payload; 47 | 48 | long port=34964; 49 | uint8_t objuuid[16]={0}; 50 | dceorrpc = (os_DCEorRPC_t *)&p_dst[dst_pos]; 51 | dceorrpc->version=4; 52 | dceorrpc->packet_type=0x00; 53 | dceorrpc->flag=NO_FACK_FLAG; 54 | dceorrpc->data_rep[0]=0x10; 55 | dceorrpc->data_rep[1]=0x00; 56 | dceorrpc->data_rep[2]= 0x00; 57 | dceorrpc->serial_high=SERIAL_HIGH; 58 | 59 | objuuid[0]=0x00; 60 | objuuid[1]=0x00; 61 | objuuid[2]=0xa0; 62 | objuuid[3]=0xde; 63 | objuuid[4]=0x97; 64 | objuuid[5]=0x6c; 65 | objuuid[6]=0xd1; 66 | objuuid[7]=0x11; 67 | objuuid[8]=0x82; 68 | objuuid[9]=0x71; 69 | 70 | memcpy(dceorrpc->object_uuid, objuuid, 16); 71 | 72 | uint8_t interface[16]; 73 | interface[0]= 0x02; 74 | interface[1]= 0x00; 75 | interface[2]= 0xa0; 76 | interface[3]= 0xde; 77 | interface[4]= 0x97; 78 | interface[5]= 0x6c; 79 | interface[6]= 0xd1; 80 | interface[7]= 0x11; 81 | interface[8]= 0x82; 82 | interface[9]= 0x71; 83 | interface[10]= 0x00; 84 | interface[11]= 0xa0; 85 | interface[12]= 0x24; 86 | interface[13]= 0x42; 87 | interface[14]= 0xdf; 88 | interface[15]= 0x7d; 89 | 90 | memcpy(dceorrpc->interface, interface, 16); 91 | 92 | uint8_t activity[16]; 93 | activity[0] = 0xdb; 94 | activity[1] = 0xab; 95 | activity[2] = 0xba; 96 | activity[3] = 0xec; 97 | activity[4] = 0x1d; 98 | activity[5] = 0x00; 99 | activity[6] = 0x54; 100 | activity[7] = 0x43; 101 | activity[8] = 0x50; 102 | activity[9] = 0xb2; 103 | activity[10] = 0x0b; 104 | activity[11] = 0x01; 105 | activity[12] = 0x63; 106 | activity[13] = 0x0a; 107 | activity[14] = 0xba; 108 | activity[15] = 0xfd; 109 | 110 | memcpy(dceorrpc->activity,activity,16); 111 | 112 | dceorrpc->server_boot=0x00; 113 | dceorrpc->interface_version=(dceorrpc->interface_version | 0x01); 114 | dceorrpc->sequence=0; 115 | dceorrpc->option=0x05; 116 | dceorrpc->interface_hint=INTERFACE_HINT; 117 | dceorrpc->activity_hint=ACTIVITY_HINT; 118 | dceorrpc->fragment_length=FRAGMENT_LENGTH; 119 | dceorrpc->fragment_num=FRAGMENT_NUMBER; 120 | dceorrpc->auth_proto=AUTHORISED_PROTOCOL; 121 | dceorrpc->serial_low=SERIAL_LOW; 122 | 123 | dst_pos += sizeof(os_DCEorRPC_t); 124 | 125 | 126 | pnio_t=(os_pnio_t *)&p_dst[dst_pos]; 127 | pnio_t->max_arg=0x8040; 128 | pnio_t->arg_length=0x00000040; 129 | pnio_t->maxcount=0x8040; 130 | pnio_t->offset=htonl(0); 131 | pnio_t->actual_count=0x00000040; 132 | readreqhdr=&(pnio_t->read_req_header); 133 | 134 | ioblock=&(readreqhdr->block); 135 | ioblock->block_type=0x0900; 136 | ioblock->length=0x3c00; 137 | ioblock->version_high=1; 138 | ioblock->version_low=htons(0); 139 | 140 | 141 | readreqhdr->seq_nbr=0x0a00; 142 | readreqhdr->api=api; 143 | readreqhdr->slot=slot; 144 | readreqhdr->subslot=subslot; 145 | readreqhdr->padding=0x0000; 146 | readreqhdr->index=0x40;// 147 | readreqhdr->index=(readreqhdr->index)<<8; 148 | readreqhdr->index=readreqhdr->index|0xf8;// 149 | readreqhdr->record_data_length=0x80; 150 | readreqhdr->record_data_length=(readreqhdr->record_data_length)<<16; 151 | readreqhdr->padding2[0]=0; 152 | readreqhdr->padding2[1]=0; 153 | 154 | dst_pos+= sizeof(os_pnio_t); 155 | 156 | //char plcip[12]={0}; 157 | //memcpy(dest_ip->addr,ip, 4); 158 | //sprintf(plcip, "%d.%d.%d.%d", dest_ip->addr[0],dest_ip->addr[1],dest_ip->addr[2],dest_ip->addr[3]); 159 | //printf(plcip, "%d.%d.%d.%d", dest_ip->addr[0],dest_ip->addr[1],dest_ip->addr[2],dest_ip->addr[3]); 160 | int portno=34964; 161 | unsigned char *payload=(char *)p_dst; 162 | size_t len=sizeof(p_dst); 163 | 164 | int sockfd, n; 165 | 166 | int serverlen; 167 | 168 | struct sockaddr_in serveraddr; 169 | 170 | struct hostent *server; 171 | 172 | 173 | 174 | /* socket: create the socket */ 175 | 176 | sockfd = socket(AF_INET, SOCK_DGRAM, 0); 177 | 178 | if (sockfd < 0) 179 | perror("ERROR opening socket"); 180 | 181 | /* gethostbyname: get the server's DNS entry */ 182 | server = gethostbyname(ip); 183 | 184 | if (server == NULL) 185 | { 186 | 187 | //fprintf(stderr,"ERROR, no such host as %2d . %2d . %2d . %2d\n", dest_ip->addr[0], dest_ip->addr[1], dest_ip->addr[2], dest_ip->addr[3]); 188 | return -1; 189 | } 190 | 191 | /* build the server's Internet address */ 192 | 193 | bzero((char *) &serveraddr, sizeof(serveraddr)); 194 | serveraddr.sin_family = AF_INET; 195 | bcopy((char *)server->h_addr,(char *)&serveraddr.sin_addr.s_addr, server->h_length); 196 | serveraddr.sin_port = htons(portno); 197 | 198 | /* send the message to the server */ 199 | 200 | serverlen = sizeof(serveraddr); 201 | n = sendto(sockfd, payload, dst_pos, 0,(struct sockaddr *) &serveraddr, serverlen); 202 | if (n < 0) 203 | perror("ERROR in sendto"); 204 | else 205 | { 206 | printf("send ret: %d\n", n); 207 | struct timeval waittime; 208 | socklen_t waitize=sizeof(waittime); 209 | waittime.tv_sec=5; 210 | setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, &waittime, waitize); 211 | int length=2048; 212 | uint8_t buffer[length]; 213 | n=recvfrom(sockfd, (void *)buffer, length, 0, (struct sockaddr *) &serveraddr, (socklen_t *)&serverlen); 214 | if(n<0) 215 | printf("nothing recieved%s\n", buffer); 216 | else 217 | { 218 | printf("%d bytes recieved: \n", n); 219 | memcpy(response, buffer, n); 220 | *size = n; 221 | } 222 | } 223 | 224 | //free(dest_ip); 225 | close(sockfd); 226 | return 0; 227 | } 228 | 229 | int send_write_IO_Packet(char *ip, uint32_t api, uint16_t slot, uint16_t subslot, uint8_t *data, uint32_t len, uint8_t *response, int *size) 230 | { 231 | os_buf_t * buf=NULL; 232 | buf=os_buf_alloc(2048); 233 | 234 | 235 | uint16_t dst_pos=0; 236 | 237 | //uint8_t ip[4]={*(dev->ip), *(dev->ip+1),*(dev->ip+2),*(dev->ip+3)}; 238 | //uint32_t addr=((uint32_t)ip[0])<<24|((uint32_t)ip[1])<<16|((uint32_t)ip[2])<<8|(uint32_t)ip[3]; 239 | 240 | char hostbuffer[256]; 241 | char IPbuffer[15]; 242 | getMyIP(IPbuffer); 243 | struct hostent *host_entry; 244 | int hostname; 245 | char *nibble=NULL; 246 | int i=0; 247 | uint8_t myIP[4]={0}; 248 | os_ipaddr_t *source_ip=NULL; 249 | os_ipaddr_t *dest_ip=malloc(4); 250 | 251 | os_ethhdr_t *p_ethhdr; 252 | os_iphdr_t *iphdr; 253 | os_udphdr_t *udphdr; 254 | os_DCEorRPC_t *dceorrpc; 255 | os_pnio_t *pnio_t; 256 | os_write_reqhdr_t *writereqhdr; 257 | os_blockhdr_t *ioblock; 258 | 259 | uint8_t *p_dst; 260 | p_dst=(uint8_t *)buf->payload; 261 | 262 | long port=34964; 263 | 264 | uint8_t objuuid[16]={0}; 265 | dceorrpc = (os_DCEorRPC_t *)&p_dst[dst_pos]; 266 | dceorrpc->version=4; 267 | dceorrpc->packet_type=0x00; 268 | dceorrpc->flag=NO_FACK_FLAG; 269 | dceorrpc->data_rep[0]=0x10; 270 | dceorrpc->data_rep[1]=0x00; 271 | dceorrpc->data_rep[2]= 0x00; 272 | dceorrpc->serial_high=SERIAL_HIGH; 273 | 274 | objuuid[0]=0x00; 275 | objuuid[1]=0x00; 276 | objuuid[2]=0xa0; 277 | objuuid[3]=0xde; 278 | objuuid[4]=0x97; 279 | objuuid[5]=0x6c; 280 | objuuid[6]=0xd1; 281 | objuuid[7]=0x11; 282 | objuuid[8]=0x82; 283 | objuuid[9]=0x71; 284 | objuuid[10]=0x00; 285 | objuuid[11]=0x00; 286 | objuuid[12]=0x00; 287 | objuuid[13]=0x00; 288 | objuuid[14]=0x00; 289 | objuuid[15]=0x00; 290 | 291 | memcpy(dceorrpc->object_uuid, objuuid, 16); 292 | 293 | uint8_t interface[16]; 294 | 295 | interface[0]= 0x02; 296 | interface[1]= 0x00; 297 | interface[2]= 0xa0; 298 | interface[3]= 0xde; 299 | interface[4]= 0x97; 300 | interface[5]= 0x6c; 301 | interface[6]= 0xd1; 302 | interface[7]= 0x11; 303 | interface[8]= 0x82; 304 | interface[9]= 0x71; 305 | interface[10]= 0x00; 306 | interface[11]= 0xa0; 307 | interface[12]= 0x24; 308 | interface[13]= 0x42; 309 | interface[14]= 0xdf; 310 | interface[15]= 0x7d; 311 | 312 | memcpy(dceorrpc->interface, interface, 16); 313 | 314 | uint8_t activity[16]; 315 | activity[0] = 0xdb; 316 | activity[1] = 0xab; 317 | activity[2] = 0xba; 318 | activity[3] = 0xec; 319 | activity[4] = 0x1d; 320 | activity[5] = 0x00; 321 | activity[6] = 0x54; 322 | activity[7] = 0x43; 323 | activity[8] = 0x50; 324 | activity[9] = 0xb2; 325 | activity[10] = 0x0b; 326 | activity[11] = 0x01; 327 | activity[12] = 0x63; 328 | activity[13] = 0x0a; 329 | activity[14] = 0xba; 330 | activity[15] = 0xfd; 331 | 332 | memcpy(dceorrpc->activity,activity,16); 333 | 334 | dceorrpc->server_boot=0x00; 335 | dceorrpc->interface_version=0x01; 336 | dceorrpc->sequence=0; 337 | dceorrpc->option=0x03; 338 | dceorrpc->interface_hint=INTERFACE_HINT; 339 | dceorrpc->activity_hint=ACTIVITY_HINT; 340 | dceorrpc->fragment_length=FRAGMENT_LENGTH; 341 | dceorrpc->fragment_num=FRAGMENT_NUMBER; 342 | dceorrpc->auth_proto=AUTHORISED_PROTOCOL; 343 | dceorrpc->serial_low=SERIAL_LOW; 344 | 345 | dst_pos += sizeof(os_DCEorRPC_t); 346 | 347 | 348 | pnio_t=(os_pnio_t *)&p_dst[dst_pos]; 349 | pnio_t->max_arg=0x8040; 350 | pnio_t->arg_length=0x00000040; 351 | pnio_t->maxcount=0x8040; 352 | pnio_t->offset=htonl(0); 353 | pnio_t->actual_count=0x00000040; 354 | writereqhdr=(os_write_reqhdr_t *)&(pnio_t->read_req_header); 355 | 356 | ioblock=&(writereqhdr->block); 357 | ioblock->block_type=0x0800; 358 | ioblock->length=0x3f00; 359 | ioblock->version_high=1; 360 | ioblock->version_low=htons(0); 361 | 362 | 363 | writereqhdr->seq_nbr=0x0110; 364 | 365 | uint8_t ar_uuid[16]; 366 | ar_uuid[0] = 0x10; 367 | ar_uuid[1] = 0x01; 368 | ar_uuid[2] = 0x00; 369 | ar_uuid[3] = 0x00; 370 | ar_uuid[4] = 0x00; 371 | ar_uuid[5] = 0x00; 372 | ar_uuid[6] = 0x00; 373 | ar_uuid[7] = 0x00; 374 | ar_uuid[8] = 0x00; 375 | ar_uuid[9] = 0x00; 376 | ar_uuid[10] = 0x00; 377 | ar_uuid[11] = 0x00; 378 | ar_uuid[12] = 0x00; 379 | ar_uuid[13] = 0x00; 380 | ar_uuid[14] = 0x00; 381 | ar_uuid[15] = 0x00; 382 | 383 | memcpy(writereqhdr->ar_uuid,ar_uuid,16); 384 | 385 | writereqhdr->api=api; 386 | writereqhdr->slot=slot; 387 | writereqhdr->subslot=subslot; 388 | writereqhdr->padding=0x0000; 389 | writereqhdr->index=0x40; 390 | writereqhdr->index=(writereqhdr->index)<<8; 391 | writereqhdr->index=writereqhdr->index|0xf8; 392 | //writereqhdr->record_data_length=0x08000000; 393 | writereqhdr->record_data_length=htonl(len); 394 | printf("write len = %d\n", len); 395 | memcpy(writereqhdr->dataValue, data, len); 396 | /*writereqhdr->dataValue[0]=0x01; 397 | writereqhdr->dataValue[1]=0x02; 398 | writereqhdr->dataValue[2]=0x03; 399 | writereqhdr->dataValue[3]=0x04; 400 | writereqhdr->dataValue[4]=0x05; 401 | writereqhdr->dataValue[5]=0x06; 402 | writereqhdr->dataValue[6]=0x07; 403 | writereqhdr->dataValue[7]=0x08;*/ 404 | 405 | 406 | dst_pos+= sizeof(os_pnio_t); 407 | 408 | 409 | 410 | //char plcip[12]={0}; 411 | //memcpy(dest_ip->addr,dev->ip, 4); 412 | //sprintf(plcip, "%d.%d.%d.%d", dest_ip->addr[0],dest_ip->addr[1],dest_ip->addr[2],dest_ip->addr[3]); 413 | //printf(plcip, "%d.%d.%d.%d", dest_ip->addr[0],dest_ip->addr[1],dest_ip->addr[2],dest_ip->addr[3]); 414 | int portno=34964; 415 | unsigned char *payload=(char *)p_dst; 416 | //size_t len=sizeof(p_dst); 417 | 418 | int sockfd, n; 419 | 420 | int serverlen; 421 | 422 | struct sockaddr_in serveraddr; 423 | 424 | struct hostent *server; 425 | 426 | 427 | 428 | /* socket: create the socket */ 429 | 430 | sockfd = socket(AF_INET, SOCK_DGRAM, 0); 431 | 432 | if (sockfd < 0) 433 | perror("ERROR opening socket"); 434 | 435 | /* gethostbyname: get the server's DNS entry */ 436 | server = gethostbyname(ip); 437 | 438 | if (server == NULL) 439 | { 440 | 441 | //fprintf(stderr,"ERROR, no such host as %2d . %2d . %2d . %2d\n", dest_ip->addr[0], dest_ip->addr[1], dest_ip->addr[2], dest_ip->addr[3]); 442 | return -1; 443 | } 444 | 445 | /* build the server's Internet address */ 446 | 447 | bzero((char *) &serveraddr, sizeof(serveraddr)); 448 | serveraddr.sin_family = AF_INET; 449 | bcopy((char *)server->h_addr,(char *)&serveraddr.sin_addr.s_addr, server->h_length); 450 | serveraddr.sin_port = htons(portno); 451 | 452 | /* send the message to the server */ 453 | 454 | serverlen = sizeof(serveraddr); 455 | n = sendto(sockfd, payload, dst_pos, 0,(struct sockaddr *) &serveraddr, serverlen); 456 | if (n < 0) 457 | perror("ERROR in sendto"); 458 | else 459 | { 460 | printf("send ret: %d\n", n); 461 | struct timeval waittime; 462 | socklen_t waitize=sizeof(waittime); 463 | waittime.tv_sec=5; 464 | setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, &waittime, waitize); 465 | int length=2048; 466 | uint8_t buffer[length]; 467 | n=recvfrom(sockfd, (void *)buffer, length, 0, (struct sockaddr *) &serveraddr, (socklen_t *)&serverlen); 468 | if(n<0) 469 | printf("nothing recieved%s\n", buffer); 470 | else 471 | { 472 | printf("%d bytes recieved: \n", n); 473 | memcpy(response, buffer, n); 474 | *size = n; 475 | } 476 | } 477 | 478 | free(dest_ip); 479 | close(sockfd); 480 | return 0; 481 | } 482 | -------------------------------------------------------------------------------- /dcp.c: -------------------------------------------------------------------------------- 1 | #include"profinet.h" 2 | 3 | 4 | 5 | int extract_blocks_from_packet(char *payload, int length, device *device) 6 | { 7 | char *pl=malloc(length); 8 | memcpy(pl, payload, length); 9 | for(int i=0; iip,ip, 4); 39 | memcpy(device->subnet,sn, 4); 40 | memcpy(device->gateway,gw, 4); 41 | break; 42 | case PF_DCP_SUB_IP_SUITE: 43 | printf("get IP SUITE address of PLC\n"); 44 | i=i+b.block_length+4; 45 | break; 46 | } 47 | break; 48 | case PF_DCP_OPT_DEVICE_PROPERTIES: 49 | switch(b.suboption) 50 | { 51 | case PF_DCP_SUB_DEV_PROP_VENDOR: /* Read */ 52 | printf("device vendor:"); 53 | for(int j=6; j<=b.block_length+5; j++) 54 | printf("%c", (uint8_t)pl[j]); 55 | printf("\n"); 56 | i=i+b.block_length+4; 57 | break; 58 | case PF_DCP_SUB_DEV_PROP_NAME: /* Read/Write */ 59 | printf("device name:"); 60 | for(int j=6; j<=b.block_length+5; j++) 61 | printf("%c", (uint8_t)pl[j]); 62 | printf("\n"); 63 | i=i+b.block_length+4; 64 | break; 65 | case PF_DCP_SUB_DEV_PROP_ID: /* Read */ 66 | printf("Vendor ID: "); 67 | printf("%.2X%.2X\n", (uint8_t)pl[6],(uint8_t)pl[7]); 68 | printf("Device ID: "); 69 | printf("%.2X%.2X\n", (uint8_t)pl[8],(uint8_t)pl[9]); 70 | i=i+b.block_length+4; 71 | break; 72 | case PF_DCP_SUB_DEV_PROP_ROLE: /* Read */ 73 | printf("device role: %.2X\n", (uint8_t)pl[6]); 74 | i=i+b.block_length+4; 75 | break; 76 | case PF_DCP_SUB_DEV_PROP_OPTIONS: /* Read */ 77 | printf("options available, optional\n"); 78 | break; 79 | } 80 | break; 81 | case PF_DCP_OPT_DHCP: 82 | i=i+b.block_length+4; 83 | break; 84 | case PF_DCP_OPT_RESERVED_4: 85 | i=i+b.block_length+4; 86 | break; 87 | case PF_DCP_OPT_CONTROL: 88 | switch(b.suboption) 89 | { 90 | case PF_DCP_SUB_CONTROL_START: /* Write */ 91 | printf("get device options of PLC\n"); 92 | i=i+b.block_length+4; 93 | break; 94 | case PF_DCP_SUB_CONTROL_STOP: /* Write */ 95 | printf("get control stop of PLC\n"); 96 | i=i+b.block_length+4; 97 | break; 98 | case PF_DCP_SUB_CONTROL_SIGNAL: /* Write */ 99 | printf("get control signal of PLC\n"); 100 | i=i+b.block_length+4; 101 | break; 102 | case PF_DCP_SUB_CONTROL_RESPONSE: 103 | printf("get control response of PLC\n"); 104 | i=i+b.block_length+4; 105 | break; 106 | case PF_DCP_SUB_CONTROL_FACTORY_RESET: /* Optional, Write */ 107 | printf("get control factory reset of PLC\n"); 108 | i=i+b.block_length+4; 109 | break; 110 | } 111 | break; 112 | case PF_DCP_OPT_DEVICE_INITIATIVE: 113 | i=i+b.block_length+4; 114 | break; 115 | 116 | } 117 | pl=pl+(b.block_length+4); 118 | } 119 | else 120 | { 121 | pl=pl+1;//ignoring padding(if any) 122 | i++; 123 | } 124 | } 125 | return 1; 126 | printf("\n\n\n"); 127 | 128 | } 129 | void recieve_profinet_packets() 130 | { 131 | int sock_r; 132 | sock_r=socket(AF_PACKET, SOCK_RAW, htons(0x8892)); 133 | if(sock_r<0) 134 | { 135 | printf("rcv error in socket\n"); 136 | } 137 | else 138 | { 139 | unsigned char *buffer = (unsigned char *) malloc(65536); //to receive data 140 | memset(buffer,0,65536); 141 | struct sockaddr socketaddr; 142 | int socketaddr_len = sizeof (socketaddr); 143 | 144 | struct pack p; 145 | int buflen=-1; 146 | int traversed=0; 147 | //printf("socket listening\n"); 148 | if((buflen=recvfrom(sock_r,buffer,65536,0,&socketaddr,(socklen_t *)&socketaddr_len))<0) 149 | { 150 | //will not come into running ever 151 | } 152 | else 153 | { 154 | 155 | // checking the packet to have DCP identify response frame id, DCP Identify service id and DCP success service type 156 | uint8_t mac[6]={(uint8_t)buffer[traversed], (uint8_t)buffer[traversed+1], (uint8_t)buffer[traversed+2], (uint8_t)buffer[traversed+3], (uint8_t)buffer[traversed+4], (uint8_t)buffer[traversed+5]}; 157 | memcpy(p.dest_mac, mac, sizeof(p.dest_mac)); 158 | traversed+=6; 159 | 160 | uint8_t smac[6]={(uint8_t)buffer[traversed], (uint8_t)buffer[traversed+1], (uint8_t)buffer[traversed+2], (uint8_t)buffer[traversed+3], (uint8_t)buffer[traversed+4], (uint8_t)buffer[traversed+5]}; 161 | memcpy(p.source_mac, smac, sizeof(p.source_mac)); 162 | traversed+=6; 163 | 164 | if((uint16_t)buffer[traversed]==0x81 && (uint16_t)buffer[traversed+1]==0x00)//if packet has VLAN as type it has type=1800 and next 2 bytes will be reserved 165 | { 166 | traversed+=4; 167 | } 168 | printf("test: 0x%.2x 0x%.2x 0x%.2x 0x%.2x\n", buffer[traversed],buffer[traversed+1],buffer[traversed+2],buffer[traversed+3]); 169 | p.packet_type=(((uint16_t)buffer[traversed])<<8)|(uint16_t)buffer[traversed+1]; 170 | traversed+=2; 171 | p.frame_id=((uint16_t)buffer[traversed]<<8)|(uint16_t)buffer[traversed+1]; 172 | traversed+=2; 173 | p.service_id=(uint8_t)buffer[traversed++]; 174 | p.service_type=(uint8_t)buffer[traversed++]; 175 | p.xid=((uint32_t)buffer[traversed]<<24)|((uint32_t)buffer[traversed+1]<<16)|((uint32_t)buffer[traversed+2]<<8)|((uint32_t)buffer[traversed+3]); 176 | traversed+=4; 177 | p.reserve_or_delay=((uint16_t)buffer[traversed]<<8)|((uint16_t)buffer[traversed+1]); 178 | traversed+=2; 179 | p.ddl=((uint16_t)buffer[traversed]<<8)|((uint16_t)buffer[traversed+1]); 180 | traversed+=2; 181 | 182 | int payload_length=(int)p.ddl; 183 | char *payload=buffer+(traversed); 184 | 185 | if(ETHTYPE_PROFINET == p.packet_type | printf(" packet type: %.4x\n", p.packet_type)) 186 | { 187 | // for a new device that has responded to identify request with identify response 188 | if( PF_DCP_ID_RES_FRAME_ID == p.frame_id && PF_DCP_SERVICE_IDENTIFY == p.service_id && PF_DCP_SERVICE_TYPE_SUCCESS == p.service_type && !check(p.source_mac)) 189 | { 190 | printf("new device found adding to devices\n"); 191 | device *dev=findDeviceByMAC(smac); 192 | if(!dev) 193 | { 194 | dev=(device *)malloc(sizeof(device)); 195 | (dev->mac)[0]=smac[0];(dev->mac)[1]=smac[1];(dev->mac)[2]=smac[2];(dev->mac)[3]=smac[3];(dev->mac)[4]=smac[4];(dev->mac)[5]=smac[5]; 196 | } 197 | 198 | //device_list *dev=malloc(sizeof(device_list)); 199 | extract_blocks_from_packet(payload, (int) p.ddl, dev); 200 | //addDevice(smac); 201 | memcpy(dev->mac,smac, 6); 202 | //addDevice(smac); 203 | 204 | /*int sop=2048; 205 | uint8_t *response=malloc(sop); 206 | uint8_t *response1=malloc(sop); 207 | 208 | send_read_IO_Packet(dev, response, sop); 209 | send_write_IO_Packet(dev, response1, sop); 210 | printf("%.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x\n", response[0], response[1], response[2], response[3], response[4], response[5], response[6], response[7], response[8], response[9], response[10], response[11], response[12], response[13], response[14], response[15]); 211 | //set_ip_config(smac); 212 | printf("set ip config sent\n"); 213 | */ 214 | } 215 | 216 | // for an old device that went off power and has come online again will pass a hello request and we add it to the list too {0x01, 0x0e, 0xcf, 0x00, 0x00, 0x00 } 217 | if( PF_DCP_SERVICE_HELLO == p.frame_id & PF_DCP_SERVICE_HELLO == p.service_id & PF_DCP_SERVICE_TYPE_SUCCESS == p.service_type & !memcmp(p.source_mac, dcp_mc_address.addr, 6) & !check(p.source_mac) ) 218 | { 219 | printf("device came back to power\n"); 220 | printf("source mac:%.2X:%.2X:%.2X:%.2X:%.2X:%.2X\n", (int)smac[0], (int)smac[1], (int)smac[2], (int)smac[3], (int)smac[4], (int)smac[5]); 221 | printf("destination mac:%.2X:%.2X:%.2X:%.2X:%.2X:%.2X\n", (int)mac[0], (int)mac[1], (int)mac[2], (int)mac[3], (int)mac[4], (int)mac[5]); 222 | device *dev=findDeviceByMAC(smac); 223 | if(!dev) 224 | { 225 | dev=(device *)malloc(sizeof(device)); 226 | (dev->mac)[0]=smac[0];(dev->mac)[1]=smac[1];(dev->mac)[2]=smac[2];(dev->mac)[3]=smac[3];(dev->mac)[4]=smac[4];(dev->mac)[5]=smac[5]; 227 | 228 | } 229 | extract_blocks_from_packet(payload, (int) p.ddl, dev); 230 | 231 | addDevice(smac); 232 | memcpy(dev->mac,smac, 6); 233 | /*int sop=1000; 234 | uint8_t *response=malloc(sop); 235 | uint8_t *response1=malloc(sop); 236 | send_read_IO_Packet(dev, response, sop); 237 | send_write_IO_Packet(dev, response1, sop); 238 | //printf("%.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x\n", response[0], response[1], response[2], response[3], response[4], response[5], response[6], response[7], response[8], response[9], response[10], response[11], response[12], response[13], response[14], response[15]); 239 | */ 240 | } 241 | } 242 | } 243 | } 244 | close(sock_r); 245 | } 246 | void send_set_req(uint8_t dest_mac[]) 247 | { 248 | os_buf_t * buf=NULL; 249 | uint8_t *p_dst; 250 | uint16_t dst_pos; 251 | uint16_t dst_start_pos; 252 | os_ethhdr_t *p_ethhdr; 253 | pf_dcp_header_t *p_dcphdr; 254 | uint8_t *p_value=0; 255 | 256 | int * sock; 257 | int i; 258 | struct ifreq ifr; 259 | struct sockaddr_ll sll; 260 | int ifindex; 261 | struct timeval timeout; 262 | 263 | sock = calloc (1, sizeof(int)); 264 | *sock = -1; 265 | 266 | *sock = socket(PF_PACKET, SOCK_RAW, htons(ETHTYPE_PROFINET)); 267 | 268 | timeout.tv_sec = 0; 269 | timeout.tv_usec = 1; 270 | setsockopt(*sock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); 271 | 272 | i = 1; 273 | setsockopt(*sock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i)); 274 | 275 | 276 | strcpy(ifr.ifr_name, interface_name); 277 | ioctl(*sock, SIOCGIFINDEX, &ifr); 278 | 279 | ifindex = ifr.ifr_ifindex; 280 | strcpy(ifr.ifr_name, interface_name); 281 | ifr.ifr_flags = 0; 282 | /* reset flags of NIC interface */ 283 | ioctl(*sock, SIOCGIFFLAGS, &ifr); 284 | 285 | /* set flags of NIC interface, here promiscuous and broadcast */ 286 | ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; 287 | ioctl(*sock, SIOCSIFFLAGS, &ifr); 288 | 289 | /* bind socket to protocol, in this case RAW EtherCAT */ 290 | sll.sll_family = AF_PACKET; 291 | sll.sll_ifindex = ifindex; 292 | sll.sll_protocol = htons(ETHTYPE_PROFINET); 293 | bind(*sock, (struct sockaddr *)&sll, sizeof(sll)); 294 | if (*sock > -1) 295 | { 296 | /******************************sending a set packet**************************************************************/ 297 | 298 | buf=os_buf_alloc(60); 299 | 300 | if (buf != NULL) 301 | { 302 | p_dst = (uint8_t *)buf->payload; 303 | if (p_dst != NULL) 304 | { 305 | dst_pos = 0; 306 | p_ethhdr = (os_ethhdr_t *)&p_dst[dst_pos]; 307 | memcpy(p_ethhdr->dest.addr, dest_mac, sizeof(p_ethhdr->dest.addr));//add multicast address to packet 308 | memcpy(p_ethhdr->src.addr,dcp_src_address.addr, sizeof(p_ethhdr->src.addr));//add source mac address to packet 309 | 310 | p_ethhdr->type = htons(ETHTYPE_PROFINET);//packet type profinet 311 | dst_pos += sizeof(os_ethhdr_t); 312 | //vipul 313 | /* Insert FrameId */ 314 | p_dst[dst_pos++] = (PF_DCP_GET_SET_FRAME_ID & 0xff00) >> 8; 315 | p_dst[dst_pos++] = PF_DCP_GET_SET_FRAME_ID & 0x00ff; 316 | 317 | p_dcphdr = (pf_dcp_header_t *)&p_dst[dst_pos]; 318 | p_dcphdr->service_id = PF_DCP_SERVICE_SET; 319 | p_dcphdr->service_type = PF_DCP_SERVICE_TYPE_REQUEST; 320 | p_dcphdr->xid = htonl(2); 321 | p_dcphdr->response_delay_factor = htons(0); 322 | p_dcphdr->data_length = htons(0); /* At the moment */ 323 | dst_pos += sizeof(pf_dcp_header_t); 324 | 325 | dst_start_pos = dst_pos; 326 | //(void)pf_dcp_put_block(p_dst, &dst_pos, 44, PF_DCP_OPT_CONTROL, PF_DCP_SUB_CONTROL_START, true, 0,4, p_value); 327 | //(void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_CONTROL, PF_DCP_SUB_CONTROL_SIGNAL, true, 0,4, p_value); 328 | (void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_IP, PF_DCP_SUB_IP_PAR, true, 0,4, p_value); 329 | //(void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_CONTROL, PF_DCP_SUB_CONTROL_FACTORY_RESET, true, 0,4, p_value); 330 | //(void)pf_dcp_put_block(p_dst, &dst_pos, 44, PF_DCP_OPT_CONTROL, PF_DCP_SUB_CONTROL_STOP, true, 0,4, p_value); 331 | //(void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_ALL, PF_DCP_SUB_ALL, true, 0,0, p_value); 332 | //(void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_ALL, PF_DCP_SUB_ALL, true, 0,0, p_value); 333 | 334 | /* Insert final response length and ship it! */ 335 | p_dcphdr->data_length = htons(dst_pos - dst_start_pos); 336 | buf->len = dst_pos; 337 | buf->payload = (void *)((uint8_t *)buf + sizeof(os_buf_t)); /* Payload follows header struct */ 338 | buf->len = 60; 339 | 340 | int ret = send (*sock, buf->payload, buf->len, 0); 341 | 342 | if (ret>=0) 343 | { 344 | fflush(stdout); 345 | // printf("transmission successful\n"); 346 | } 347 | else 348 | printf("not transmitted: %d \n", ret); 349 | } 350 | else 351 | { 352 | printf("buffer payload is null\n"); 353 | } 354 | } 355 | else 356 | { 357 | printf("buffer is null"); 358 | } 359 | close(*sock); 360 | free(sock); 361 | } 362 | else 363 | printf("socket not created"); 364 | } 365 | void send_get_req(uint8_t dest_mac[]) 366 | { 367 | os_buf_t * buf=NULL; 368 | uint8_t *p_dst; 369 | uint16_t dst_pos; 370 | uint16_t dst_start_pos; 371 | os_ethhdr_t *p_ethhdr; 372 | pf_dcp_header_t *p_dcphdr; 373 | uint8_t *p_value=0; 374 | 375 | int * sock; 376 | int i; 377 | struct ifreq ifr; 378 | struct sockaddr_ll sll; 379 | int ifindex; 380 | struct timeval timeout; 381 | 382 | sock = calloc (1, sizeof(int)); 383 | *sock = -1; 384 | 385 | *sock = socket(PF_PACKET, SOCK_RAW, htons(ETHTYPE_PROFINET)); 386 | 387 | timeout.tv_sec = 0; 388 | timeout.tv_usec = 1; 389 | setsockopt(*sock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); 390 | 391 | i = 1; 392 | setsockopt(*sock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i)); 393 | 394 | 395 | strcpy(ifr.ifr_name, interface_name); 396 | ioctl(*sock, SIOCGIFINDEX, &ifr); 397 | 398 | ifindex = ifr.ifr_ifindex; 399 | strcpy(ifr.ifr_name, interface_name); 400 | ifr.ifr_flags = 0; 401 | /* reset flags of NIC interface */ 402 | ioctl(*sock, SIOCGIFFLAGS, &ifr); 403 | 404 | /* set flags of NIC interface, here promiscuous and broadcast */ 405 | ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; 406 | ioctl(*sock, SIOCSIFFLAGS, &ifr); 407 | 408 | /* bind socket to protocol, in this case RAW EtherCAT */ 409 | sll.sll_family = AF_PACKET; 410 | sll.sll_ifindex = ifindex; 411 | sll.sll_protocol = htons(ETHTYPE_PROFINET); 412 | bind(*sock, (struct sockaddr *)&sll, sizeof(sll)); 413 | if (*sock > -1) 414 | { 415 | /******************************sending a packet**************************************************************/ 416 | 417 | buf=os_buf_alloc(60); 418 | 419 | if (buf != NULL) 420 | { 421 | p_dst = (uint8_t *)buf->payload; 422 | if (p_dst != NULL) 423 | { 424 | dst_pos = 0; 425 | p_ethhdr = (os_ethhdr_t *)&p_dst[dst_pos]; 426 | memcpy(p_ethhdr->dest.addr, dest_mac, sizeof(p_ethhdr->dest.addr));//add multicast address to packet 427 | memcpy(p_ethhdr->src.addr,dcp_src_address.addr, sizeof(p_ethhdr->src.addr));//add source mac address to packet 428 | 429 | p_ethhdr->type = htons(ETHTYPE_PROFINET);//packet type profinet 430 | dst_pos += sizeof(os_ethhdr_t); 431 | 432 | /* Insert FrameId */ 433 | p_dst[dst_pos++] = (PF_DCP_GET_SET_FRAME_ID & 0xff00) >> 8; 434 | p_dst[dst_pos++] = PF_DCP_GET_SET_FRAME_ID & 0x00ff; 435 | 436 | p_dcphdr = (pf_dcp_header_t *)&p_dst[dst_pos]; 437 | p_dcphdr->service_id = PF_DCP_SERVICE_GET; 438 | p_dcphdr->service_type = PF_DCP_SERVICE_TYPE_REQUEST; 439 | p_dcphdr->xid = htonl(2); 440 | p_dcphdr->response_delay_factor = htons(1); 441 | p_dcphdr->data_length = htons(0); /* At the moment */ 442 | dst_pos += sizeof(pf_dcp_header_t); 443 | 444 | dst_start_pos = dst_pos; 445 | (void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_ALL, PF_DCP_SUB_ALL, true, 0,0, p_value); 446 | 447 | /* Insert final response length and ship it! */ 448 | p_dcphdr->data_length = htons(dst_pos - dst_start_pos); 449 | buf->len = dst_pos; 450 | buf->payload = (void *)((uint8_t *)buf + sizeof(os_buf_t)); /* Payload follows header struct */ 451 | buf->len = 60; 452 | 453 | int ret = send (*sock, buf->payload, buf->len, 0); 454 | 455 | if (ret>=0) 456 | { 457 | fflush(stdout); 458 | printf("set/ connect and disconnect sent\n"); 459 | // printf("transmission successful\n"); 460 | } 461 | else 462 | printf("not transmitted: %d \n", ret); 463 | } 464 | else 465 | { 466 | printf("buffer payload is null\n"); 467 | } 468 | } 469 | else 470 | { 471 | printf("buffer is null"); 472 | } 473 | close(*sock); 474 | free(sock); 475 | } 476 | else 477 | printf("socket not created"); 478 | } 479 | void discover_devices() 480 | { 481 | /******************************var declarations for packet**************************************************************/ 482 | //printf("started discovering\n"); 483 | os_buf_t * buf=NULL; 484 | uint8_t *p_dst; 485 | uint16_t dst_pos; 486 | uint16_t dst_start_pos; 487 | os_ethhdr_t *p_ethhdr; 488 | pf_dcp_header_t *p_dcphdr; 489 | uint8_t *p_value=0; 490 | 491 | /******************************building a socket**************************************************************/ 492 | int * sock;//socket address 493 | int i; 494 | struct ifreq ifr; 495 | struct sockaddr_ll sll; 496 | int ifindex; 497 | struct timeval timeout; 498 | 499 | sock = calloc (1, sizeof(int)); 500 | *sock = -1; 501 | 502 | *sock = socket(PF_PACKET, SOCK_RAW, htons(ETHTYPE_PROFINET));//raw socket with protocol type profinet 503 | 504 | timeout.tv_sec = 0; 505 | timeout.tv_usec = 1; 506 | setsockopt(*sock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); 507 | 508 | i = 1; 509 | setsockopt(*sock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i)); 510 | 511 | 512 | strcpy(ifr.ifr_name, interface_name); 513 | ioctl(*sock, SIOCGIFINDEX, &ifr); 514 | 515 | ifindex = ifr.ifr_ifindex; 516 | strcpy(ifr.ifr_name, interface_name); 517 | ifr.ifr_flags = 0; 518 | /* reset flags of NIC interface */ 519 | ioctl(*sock, SIOCGIFFLAGS, &ifr); 520 | 521 | /* set flags of NIC interface, here promiscuous and broadcast */ 522 | ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; 523 | ioctl(*sock, SIOCSIFFLAGS, &ifr); 524 | 525 | /* bind socket to protocol, in this case RAW EtherCAT */ 526 | sll.sll_family = AF_PACKET; 527 | sll.sll_ifindex = ifindex; 528 | sll.sll_protocol = htons(ETHTYPE_PROFINET); 529 | bind(*sock, (struct sockaddr *)&sll, sizeof(sll)); 530 | if (*sock > -1) 531 | { 532 | /******************************sending a packet**************************************************************/ 533 | buf=os_buf_alloc(60); 534 | 535 | if (buf != NULL) 536 | { 537 | p_dst = (uint8_t *)buf->payload; 538 | if (p_dst != NULL) 539 | { 540 | dst_pos = 0; 541 | p_ethhdr = (os_ethhdr_t *)&p_dst[dst_pos]; 542 | memcpy(p_ethhdr->dest.addr, dcp_mc_address.addr, sizeof(p_ethhdr->dest.addr));//add multicast address to packet 543 | //discover source mac address, if equal to a default dummy MAC, read a current machine MAC and save it for future use 544 | if(dcp_src_address.addr[0]==0xc8 && dcp_src_address.addr[1]==0x1f && dcp_src_address.addr[2]==0x66 && dcp_src_address.addr[3]==0x43 && dcp_src_address.addr[4]==0x6a && dcp_src_address.addr[5]==0x45) 545 | { 546 | struct ifreq s; 547 | int fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP); 548 | strcpy(s.ifr_name, interface_name); 549 | if (0 == ioctl(fd, SIOCGIFHWADDR, &s)) 550 | { 551 | 552 | memcpy(dcp_src_address.addr,s.ifr_addr.sa_data,sizeof(dcp_src_address.addr)); 553 | } 554 | } 555 | /*else 556 | { 557 | printf(); 558 | }*/ 559 | 560 | memcpy(p_ethhdr->src.addr,dcp_src_address.addr, sizeof(p_ethhdr->src.addr));//add source mac address to packet 561 | 562 | p_ethhdr->type = htons(ETHTYPE_PROFINET);//packet type profinet 563 | dst_pos += sizeof(os_ethhdr_t); 564 | 565 | /* Insert FrameId */ 566 | p_dst[dst_pos++] = (PF_DCP_ID_REQ_FRAME_ID & 0xff00) >> 8; 567 | p_dst[dst_pos++] = PF_DCP_ID_REQ_FRAME_ID & 0x00ff; 568 | 569 | p_dcphdr = (pf_dcp_header_t *)&p_dst[dst_pos]; 570 | p_dcphdr->service_id = PF_DCP_SERVICE_IDENTIFY; 571 | p_dcphdr->service_type = PF_DCP_SERVICE_TYPE_REQUEST; 572 | p_dcphdr->xid = htonl(1); 573 | p_dcphdr->response_delay_factor = htons(1); 574 | p_dcphdr->data_length = htons(0); /* At the moment */ 575 | dst_pos += sizeof(pf_dcp_header_t); 576 | 577 | dst_start_pos = dst_pos; 578 | (void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_ALL, PF_DCP_SUB_ALL, true, 0,0, p_value); 579 | 580 | /* Insert final response length and ship it! */ 581 | p_dcphdr->data_length = htons(dst_pos - dst_start_pos); 582 | buf->len = dst_pos; 583 | buf->payload = (void *)((uint8_t *)buf + sizeof(os_buf_t)); /* Payload follows header struct */ 584 | buf->len = 60; 585 | 586 | int ret = send (*sock, buf->payload, buf->len, 0); 587 | 588 | if (ret>=0) 589 | { 590 | fflush(stdout); 591 | // printf("transmission successful\n"); 592 | } 593 | else 594 | printf("not transmitted: %d %s\n", ret, interface_name); 595 | } 596 | else 597 | { 598 | printf("buffer payload is null\n"); 599 | } 600 | } 601 | else 602 | { 603 | printf("buffer is null"); 604 | } 605 | free(sock); 606 | } 607 | else 608 | printf("socket not created"); 609 | } 610 | void set_ip_config(uint8_t mac[]) 611 | { 612 | os_buf_t * buf=NULL; 613 | uint8_t *p_dst; 614 | uint16_t dst_pos; 615 | uint16_t dst_start_pos; 616 | os_ethhdr_t *p_ethhdr; 617 | pf_dcp_header_t *p_dcphdr; 618 | uint8_t *p_value=0; 619 | 620 | uint8_t ip[4]; 621 | uint8_t subnet[4]; 622 | uint8_t gateway[4]; 623 | 624 | int * sock; 625 | int i; 626 | struct ifreq ifr; 627 | struct sockaddr_ll sll; 628 | int ifindex; 629 | struct timeval timeout; 630 | 631 | sock = calloc (1, sizeof(int)); 632 | *sock = -1; 633 | 634 | *sock = socket(PF_PACKET, SOCK_RAW, htons(ETHTYPE_PROFINET)); 635 | 636 | timeout.tv_sec = 0; 637 | timeout.tv_usec = 1; 638 | setsockopt(*sock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); 639 | 640 | i = 1; 641 | setsockopt(*sock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i)); 642 | 643 | 644 | strcpy(ifr.ifr_name, interface_name); 645 | ioctl(*sock, SIOCGIFINDEX, &ifr); 646 | 647 | ifindex = ifr.ifr_ifindex; 648 | strcpy(ifr.ifr_name, interface_name); 649 | ifr.ifr_flags = 0; 650 | /* reset flags of NIC interface */ 651 | ioctl(*sock, SIOCGIFFLAGS, &ifr); 652 | 653 | /* set flags of NIC interface, here promiscuous and broadcast */ 654 | ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; 655 | ioctl(*sock, SIOCSIFFLAGS, &ifr); 656 | 657 | /* bind socket to protocol, in this case RAW EtherCAT */ 658 | sll.sll_family = AF_PACKET; 659 | sll.sll_ifindex = ifindex; 660 | sll.sll_protocol = htons(ETHTYPE_PROFINET); 661 | bind(*sock, (struct sockaddr *)&sll, sizeof(sll)); 662 | if (*sock > -1) 663 | { 664 | /******************************sending a set packet**************************************************************/ 665 | 666 | buf=os_buf_alloc(60); 667 | 668 | if (buf != NULL) 669 | { 670 | p_dst = (uint8_t *)buf->payload; 671 | if (p_dst != NULL) 672 | { 673 | dst_pos = 0; 674 | p_ethhdr = (os_ethhdr_t *)&p_dst[dst_pos]; 675 | memcpy(p_ethhdr->dest.addr, mac, sizeof(p_ethhdr->dest.addr));//add multicast address to packet 676 | memcpy(p_ethhdr->src.addr,dcp_src_address.addr, sizeof(p_ethhdr->src.addr));//add source mac address to packet 677 | 678 | p_ethhdr->type = htons(ETHTYPE_PROFINET);//packet type profinet 8892 679 | dst_pos += sizeof(os_ethhdr_t); 680 | 681 | /* Insert FrameId */ 682 | p_dst[dst_pos++] = (PF_DCP_GET_SET_FRAME_ID & 0xff00) >> 8; 683 | p_dst[dst_pos++] = PF_DCP_GET_SET_FRAME_ID & 0x00ff; 684 | 685 | p_dcphdr = (pf_dcp_header_t *)&p_dst[dst_pos]; 686 | p_dcphdr->service_id = PF_DCP_SERVICE_SET; 687 | p_dcphdr->service_type = PF_DCP_SERVICE_TYPE_REQUEST; 688 | p_dcphdr->xid = htonl(2); 689 | p_dcphdr->response_delay_factor = htons(0); 690 | p_dcphdr->data_length = htons(0); /* At the moment */ 691 | dst_pos += sizeof(pf_dcp_header_t); 692 | int ip1, ip2, ip3, ip4,sn1, sn2,sn3,sn4,gw1,gw2,gw3,gw4; 693 | 694 | printf("please enter an IP Address to be assigned to the new found device"); 695 | scanf("%d.%d.%d.%d", &ip1, &ip2, &ip3, &ip4); 696 | printf("please enter its subnet Mask "); 697 | scanf("%d.%d.%d.%d", &sn1, &sn2, &sn3, &sn4); 698 | printf("please enter its Gateway "); 699 | scanf("%d.%d.%d.%d", &gw1, &gw2, &gw3, &gw4); 700 | ip[0]=ip1;ip[1]=ip2;ip[2]=ip3;ip[3]=ip4; 701 | subnet[0]=sn1;subnet[1]=sn2;subnet[2]=sn3;subnet[3]=sn4; 702 | gateway[0]=gw1;gateway[1]=gw2;gateway[2]=gw3;gateway[3]=gw4; 703 | uint8_t perm_or_temp[2]={0x00, 0x0e}; 704 | uint8_t ip_params[14]={perm_or_temp[0],perm_or_temp[1],ip[0], ip[1], ip[2], ip[3], subnet[0], subnet[1], subnet[2], subnet[3], gateway[0], gateway[1], gateway[2], gateway[3]}; 705 | dst_start_pos = dst_pos; 706 | 707 | (void)pf_dcp_put_block(p_dst, &dst_pos, 60, PF_DCP_OPT_IP, PF_DCP_SUB_IP_PAR, true, 0,14, ip_params); 708 | 709 | 710 | 711 | 712 | /* Insert final response length and ship it! */ 713 | p_dcphdr->data_length = htons(dst_pos - dst_start_pos); 714 | buf->len = dst_pos; 715 | buf->payload = (void *)((uint8_t *)buf + sizeof(os_buf_t)); /* Payload follows header struct */ 716 | buf->len = 60; 717 | 718 | int ret = send (*sock, buf->payload, buf->len, 0); 719 | 720 | if (ret>=0) 721 | { 722 | fflush(stdout); 723 | // printf("transmission successful\n"); 724 | } 725 | else 726 | printf("not transmitted: %d\n", ret); 727 | } 728 | else 729 | { 730 | printf("buffer payload is null\n"); 731 | } 732 | } 733 | else 734 | { 735 | printf("buffer is null"); 736 | } 737 | free(sock); 738 | } 739 | else 740 | printf("socket not created"); 741 | } 742 | device_list* createNewDevice(char *name, uint8_t mac[6], uint8_t ip[4], uint8_t subnet[4], uint8_t gateway[4]) 743 | { 744 | device_list *node=malloc(sizeof(device_list)); 745 | node->next=NULL; 746 | node->name=name; 747 | memcpy(node->mac,mac,6); 748 | memcpy(node->ip,ip, 4); 749 | memcpy(node->subnet,subnet,4); 750 | memcpy(node->gateway,gateway, 4); 751 | return node; 752 | } 753 | 754 | int check(uint8_t mac[]) 755 | { 756 | int found=0; 757 | for(int i=0; i