Pfp Cybersecurity (Aka Power Fingerprinting, Inc.)
/
pfp-emon-nxp
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FXAS21002 FXOS8700Q
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // ---------------------------------------------------------------------------- 00002 // Copyright 2019 PFP Cybersecurity. 00003 // 00004 // SPDX-License-Identifier: Apache-2.0 00005 // 00006 // Licensed under the Apache License, Version 2.0 (the "License"); 00007 // you may not use this file except in compliance with the License. 00008 // You may obtain a copy of the License at 00009 // 00010 // http://www.apache.org/licenses/LICENSE-2.0 00011 // 00012 // Unless required by applicable law or agreed to in writing, software 00013 // distributed under the License is distributed on an "AS IS" BASIS, 00014 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00015 // See the License for the specific language governing permissions and 00016 // limitations under the License. 00017 // ---------------------------------------------------------------------------- 00018 #ifndef MBED_TEST_MODE 00019 00020 #include "mbed.h" 00021 00022 // Default network interface object. Don't forget to change the WiFi SSID/password in mbed_app.json if you're using WiFi. 00023 NetworkInterface *net = NetworkInterface::get_default_instance(); 00024 00025 AnalogIn ain(A0); 00026 00027 /////////////////////////////////////////////////////////////////////////////////////// 00028 /// 00029 /// PFP Main code start 00030 /// 00031 ////////////////////////////////////////////////////////////////////////////////////// 00032 uint8_t PFP_REGISTER_DIGITIZER = 87; 00033 uint8_t PFP_CLK_FREQ = 11; 00034 uint8_t PFP_ADC_GET_RAW = 3; 00035 uint8_t PFP_ADC_INIT = 0; 00036 uint8_t PFP_TRIG_CONFIG = 26; 00037 uint8_t PFP_TRIG_RECEIVED = 88; 00038 00039 struct DeviceData { 00040 int channel; 00041 00042 int numberOfTraces; 00043 int traceLength; 00044 00045 int sampleRate; 00046 int digitizer; 00047 00048 int trigMode; 00049 int trigSource; 00050 00051 float trigLevel; 00052 float trigHyst; 00053 00054 int trigPercentage; 00055 int gain; 00056 }; 00057 00058 struct DeviceData deviceData; 00059 00060 int bytesToInt(int8_t b0, int8_t b1, int8_t b2, int8_t b3) 00061 { 00062 int value = (b3 << 24) & 0xff000000 00063 | (b2 << 16) & 0x00ff0000 00064 | (b1 << 8) & 0x0000ff00 00065 | (b0 << 0) & 0x000000ff; 00066 return value; 00067 } 00068 00069 float bytesToFloat(int8_t b0, int8_t b1, int8_t b2, int8_t b3) 00070 { 00071 00072 float val = 0; 00073 unsigned long result = 0; 00074 result |= ((unsigned long) (b0) << 0x18); 00075 result |= ((unsigned long) (b1) << 0x10); 00076 result |= ((unsigned long) (b2) << 0x08); 00077 result |= ((unsigned long) (b3)); 00078 memcpy(&val, &result, 4); 00079 00080 return val; 00081 } 00082 00083 int8_t * intToBytes(int value) 00084 { 00085 int8_t *b = (int8_t*) calloc(4, sizeof(int8_t)); 00086 00087 b[0] = ((int8_t) (value >> 0)); 00088 b[1] = ((int8_t) (value >> 8)); 00089 b[2] = ((int8_t) (value >> 16)); 00090 b[3] = ((int8_t) (value >> 24)); 00091 return b; 00092 } 00093 00094 int8_t * longToBytes(long value) 00095 { 00096 static int8_t b[8]; 00097 b[0] = (int8_t) ((value >> 0) & 0xFF); 00098 b[1] = (int8_t) ((value >> 8) & 0xFF); 00099 b[2] = (int8_t) ((value >> 16) & 0xFF); 00100 b[3] = (int8_t) ((value >> 24) & 0xFF); 00101 b[4] = (int8_t) ((value >> 32) & 0xFF); 00102 b[5] = (int8_t) ((value >> 40) & 0xFF); 00103 b[6] = (int8_t) ((value >> 48) & 0xFF); 00104 b[7] = (int8_t) ((value >> 56) & 0xFF); 00105 return b; 00106 } 00107 00108 int8_t * pfp_emon_create_ack_for_client(int commandType, int numberOfBytes) 00109 { 00110 static int8_t b[64]; 00111 00112 int8_t *totalBytes = intToBytes(numberOfBytes); 00113 int8_t *successBytes = intToBytes(3); 00114 int8_t *returnBytes = intToBytes(commandType); 00115 int8_t *totaTrace = intToBytes(numberOfBytes / 2); 00116 00117 // EMON HEADER 00118 b[0] = 69; 00119 b[1] = 77; 00120 b[2] = 79; 00121 b[3] = 78; 00122 00123 // NUMBER OF BYTES 00124 b[4] = totalBytes[0]; 00125 b[5] = totalBytes[1]; 00126 b[6] = totalBytes[2]; 00127 b[7] = totalBytes[3]; 00128 00129 // ERROR 00130 b[8] = 0; 00131 b[9] = 0; 00132 b[10] = 0; 00133 b[11] = 0; 00134 00135 // SKIP BYTES 00136 b[12] = 0; 00137 b[13] = 0; 00138 b[14] = 0; 00139 b[15] = 0; 00140 00141 // SUCCESS COMMAND 00142 b[16] = successBytes[0]; 00143 b[17] = successBytes[1]; 00144 b[18] = successBytes[2]; 00145 b[19] = successBytes[3]; 00146 00147 // RETURN COMMAND 00148 b[20] = returnBytes[0]; 00149 b[21] = returnBytes[1]; 00150 b[22] = returnBytes[2]; 00151 b[23] = returnBytes[3]; 00152 00153 // SKIP BYTES 00154 b[24] = 0; 00155 b[25] = 0; 00156 b[26] = 0; 00157 b[27] = 0; 00158 00159 // TOTAL TRACE 00160 b[28] = totaTrace[0]; 00161 b[29] = totaTrace[1]; 00162 b[30] = totaTrace[2]; 00163 b[31] = totaTrace[3]; 00164 00165 00166 free(totalBytes); 00167 free(successBytes); 00168 free(returnBytes); 00169 free(totaTrace); 00170 00171 00172 return b; 00173 } 00174 00175 00176 void startEmonThread(void const *args) 00177 { 00178 00179 printf("The target IP address is '%s'\r\n", net->get_ip_address()); 00180 00181 TCPServer srv; 00182 TCPSocket clt_sock; 00183 SocketAddress clt_addr; 00184 00185 /* Open the server on ethernet stack */ 00186 srv.open(net); 00187 00188 /* Bind the HTTP port (TCP 80) to the server */ 00189 srv.bind(net->get_ip_address(), 7001); 00190 00191 long traceTrack = 0; 00192 float sleepCore = 0.001; 00193 00194 00195 //srv.set_blocking(false); 00196 while (true) { 00197 /* Can handle 5 simultaneous connections */ 00198 int err= srv.listen(1); 00199 printf("server listening error : %d\r\n",err); 00200 00201 while(1) { 00202 00203 printf("waiting for client connection\r\n"); 00204 err = srv.accept(&clt_sock, &clt_addr); 00205 if(err == 0) { 00206 printf("client connected :%s:%d\r\n", clt_addr.get_ip_address(), clt_addr.get_port()); 00207 int MAX_LEN = 80; 00208 int8_t bytes[MAX_LEN]; 00209 int32_t rawDataLen = 2048; 00210 int16_t *rawData = (int16_t*)calloc(rawDataLen, sizeof(int16_t)); 00211 int16_t MAX_TRANSMIT_DATA = 500; 00212 int16_t data[MAX_TRANSMIT_DATA]; 00213 traceTrack = 0; 00214 00215 bool debug = false; 00216 00217 while(1) { 00218 00219 //////////////// 00220 int len = clt_sock.recv(bytes, MAX_LEN); 00221 if(debug) { 00222 printf("====================> len = %i <======================\n",len); 00223 } 00224 00225 if (len < 1) { 00226 printf("Connection is closed....\n"); 00227 break; 00228 } 00229 00230 int commandType = 0; 00231 int commandLength = 0; 00232 int indexOffset = 0; 00233 00234 int8_t *sendBytes = NULL; 00235 00236 if (len < 0) { 00237 return; 00238 } 00239 if (len == 8) { 00240 commandType = bytesToInt(bytes[0], bytes[1], bytes[2], bytes[3]); 00241 } else if (len == 12) { 00242 commandType = bytesToInt(bytes[0], bytes[1], bytes[2], bytes[3]); 00243 commandLength = bytesToInt(bytes[4], bytes[5], bytes[6], bytes[7]); 00244 if (commandType == PFP_REGISTER_DIGITIZER) { 00245 00246 deviceData.digitizer = bytesToInt(bytes[8], bytes[9], bytes[10], 00247 bytes[11]); 00248 00249 // send_PFP_REGISTER_DIGITIZER(clt_sock); 00250 00251 } else if (commandType == PFP_CLK_FREQ) { 00252 deviceData.sampleRate = bytesToInt(bytes[8], bytes[9], 00253 bytes[10], bytes[11]); 00254 // send_PFP_CLK_FREQ(clt_sock); 00255 } else if (commandType == PFP_ADC_GET_RAW) { 00256 deviceData.channel = bytesToInt(bytes[8], bytes[9], bytes[10], 00257 bytes[11]); 00258 } 00259 if(debug) { 00260 printf("====================> len 12 commandType= %i <======================\n",commandType); 00261 printf("====================> len 12 commandLength= %i <======================\n",commandLength); 00262 } 00263 00264 } else { 00265 commandType = bytesToInt(bytes[0], bytes[1], bytes[2], bytes[3]); 00266 commandLength = bytesToInt(bytes[4], bytes[5], bytes[6], bytes[7]); 00267 if (commandLength == 8) { 00268 if ((len == 60) || (len == 72)) { 00269 commandType =bytesToInt(bytes[8], bytes[9], bytes[10], bytes[11]); 00270 commandLength = bytesToInt(bytes[12], bytes[13], bytes[14], bytes[15]); 00271 indexOffset = 8; 00272 if(debug) { 00273 printf("====================> inside commandType= %i <======================\n",commandType); 00274 printf("====================> inside commandLength= %i <======================\n",commandLength); 00275 } 00276 } 00277 } else { 00278 if(debug) { 00279 printf("====================> outside commandType= %i <======================\n",commandType); 00280 printf("====================> outside commandLength= %i <======================\n",commandLength); 00281 } 00282 } 00283 } 00284 00285 // Got command form client. Send back header 00286 if (commandType == PFP_ADC_INIT) { 00287 sendBytes = pfp_emon_create_ack_for_client(commandType, 0); 00288 00289 if (sendBytes != NULL) { 00290 clt_sock.send(sendBytes,64); 00291 wait(sleepCore); 00292 } 00293 } 00294 00295 if (commandType == PFP_REGISTER_DIGITIZER) { 00296 sendBytes = pfp_emon_create_ack_for_client(commandType, 0); 00297 00298 if (sendBytes != NULL) { 00299 clt_sock.send(sendBytes,64); 00300 wait(sleepCore); 00301 } 00302 } 00303 if (commandType == PFP_TRIG_CONFIG) { 00304 00305 if ((commandLength > 12) && (commandLength < 1000)) { 00306 00307 deviceData.channel = bytesToInt(bytes[indexOffset+8], bytes[indexOffset+9], bytes[indexOffset+10], 00308 bytes[indexOffset+11]); 00309 deviceData.traceLength = bytesToInt(bytes[indexOffset+12], bytes[indexOffset+13], 00310 bytes[indexOffset+14], bytes[indexOffset+15]); 00311 00312 deviceData.trigMode = bytesToInt(bytes[indexOffset+16], bytes[indexOffset+17], 00313 bytes[indexOffset+18], bytes[indexOffset+19]); 00314 deviceData.trigSource = bytesToInt(bytes[indexOffset+20], bytes[indexOffset+21], 00315 bytes[indexOffset+22], bytes[indexOffset+23]); 00316 00317 deviceData.trigLevel = bytesToFloat(bytes[indexOffset+24], bytes[indexOffset+25], 00318 bytes[indexOffset+26], bytes[indexOffset+27]); 00319 deviceData.trigHyst = bytesToFloat(bytes[indexOffset+28], bytes[indexOffset+29], 00320 bytes[indexOffset+30], bytes[indexOffset+31]); 00321 00322 deviceData.trigPercentage = bytesToInt(bytes[indexOffset+32], bytes[indexOffset+33], 00323 bytes[indexOffset+34], bytes[indexOffset+35]); 00324 deviceData.gain = bytesToInt(bytes[indexOffset+36], bytes[indexOffset+37], bytes[indexOffset+38], 00325 bytes[indexOffset+39]); 00326 if(debug) { 00327 printf("channel is %i\n", deviceData.channel); 00328 printf("traceLength is %i\n", deviceData.traceLength); 00329 printf("trigMode is %f\n", deviceData.trigLevel); 00330 printf("trigSource is %i\n", deviceData.trigSource); 00331 printf("trigLevel is %f\n", deviceData.trigLevel); 00332 printf("trigHyst is %f\n", deviceData.trigHyst); 00333 printf("trigPercentage is %i\n", deviceData.trigPercentage); 00334 printf("gain is %i\n", deviceData.gain); 00335 printf("sample rate is %i\n", deviceData.sampleRate); 00336 printf("gain is %i\n", deviceData.gain); 00337 } 00338 00339 commandType = PFP_TRIG_RECEIVED; 00340 sendBytes = pfp_emon_create_ack_for_client(commandType, 00341 deviceData.traceLength * 2); 00342 00343 if (sendBytes != NULL) { 00344 clt_sock.send(sendBytes,64); 00345 } 00346 00347 int numOfInteration = (len - (indexOffset+40))/4; 00348 00349 for(int i=0; i<numOfInteration; i++) { 00350 int index2 = i*4; 00351 int b0 = indexOffset+40+index2; 00352 int b1 = indexOffset+41+index2; 00353 int b2 = indexOffset+42+index2; 00354 int b3 = indexOffset+43+index2; 00355 int v = bytesToInt(bytes[b0], bytes[b1], bytes[b2],bytes[b3]); 00356 if(debug) { 00357 printf("Trailing value %i\n", v); 00358 } 00359 } 00360 00361 if(numOfInteration==6) { 00362 00363 commandType = bytesToInt(bytes[indexOffset+40], bytes[indexOffset+41], bytes[indexOffset+42],bytes[indexOffset+43]); 00364 commandLength = bytesToInt(bytes[indexOffset+44], bytes[indexOffset+45], bytes[indexOffset+46],bytes[indexOffset+47]); 00365 int sampleRate = bytesToInt(bytes[indexOffset+48], bytes[indexOffset+49], bytes[indexOffset+50],bytes[indexOffset+51]); 00366 deviceData.sampleRate = sampleRate; 00367 00368 if (commandType == PFP_CLK_FREQ) { 00369 sendBytes = pfp_emon_create_ack_for_client(commandType, 0); 00370 if (sendBytes != NULL) { 00371 clt_sock.send(sendBytes,64); 00372 } 00373 } 00374 wait(sleepCore); 00375 00376 commandType = bytesToInt(bytes[indexOffset+52], bytes[indexOffset+53], bytes[indexOffset+54],bytes[indexOffset+55]); 00377 commandLength = bytesToInt(bytes[indexOffset+56], bytes[indexOffset+57], bytes[indexOffset+58],bytes[indexOffset+59]); 00378 // int sampleRate = bytesToInt(bytes[indexOffset+40], bytes[indexOffset+41], bytes[indexOffset+42],bytes[indexOffset+43]); 00379 } 00380 00381 00382 } 00383 } 00384 if (commandType == PFP_CLK_FREQ) { 00385 sendBytes = pfp_emon_create_ack_for_client(commandType, 0); 00386 if (sendBytes != NULL) { 00387 clt_sock.send(sendBytes,64); 00388 wait(sleepCore); 00389 } 00390 00391 if(len==24) { 00392 commandType = PFP_ADC_GET_RAW; 00393 } 00394 00395 } 00396 00397 00398 if (commandType == PFP_ADC_GET_RAW) { 00399 sendBytes = pfp_emon_create_ack_for_client(PFP_TRIG_RECEIVED, 00400 deviceData.traceLength * 2); 00401 00402 if (sendBytes != NULL) { 00403 clt_sock.send(sendBytes,64); 00404 } 00405 00406 sendBytes = pfp_emon_create_ack_for_client(commandType, 00407 deviceData.traceLength * 2); 00408 if (sendBytes != NULL) { 00409 clt_sock.send(sendBytes,64); 00410 } 00411 00412 if (deviceData.traceLength != rawDataLen) { 00413 printf("Data size change to %i===============>\n",rawDataLen); 00414 free(rawData); 00415 rawDataLen = deviceData.traceLength; 00416 rawData = (int16_t*)calloc(rawDataLen, sizeof(int16_t)); 00417 } 00418 00419 // Store raw data 00420 00421 int val=0; 00422 00423 for (int i = 0; i < rawDataLen; i++) { 00424 val = ain.read_u16() - 32768; 00425 if(val>32762) { 00426 val = 32762; 00427 } else if(val<-32762) { 00428 val = -32762; 00429 } 00430 00431 rawData[i] =val; 00432 } 00433 00434 int num = rawDataLen/MAX_TRANSMIT_DATA; 00435 int startIndex = 0; 00436 for(int k =0; k<num; k++) { 00437 startIndex = k*MAX_TRANSMIT_DATA; 00438 for(int i=0; i<MAX_TRANSMIT_DATA; i++) { 00439 data[i] = rawData[i + startIndex]; 00440 } 00441 clt_sock.send(data,MAX_TRANSMIT_DATA*sizeof(int16_t)); 00442 wait(sleepCore); 00443 } 00444 00445 int leftOver = rawDataLen - num*MAX_TRANSMIT_DATA; 00446 00447 if(leftOver>0) { 00448 startIndex = num*MAX_TRANSMIT_DATA; 00449 00450 for(int j=startIndex; j<rawDataLen; j++) { 00451 int i = j-startIndex; 00452 data[i] = rawData[j]; 00453 } 00454 clt_sock.send(data,leftOver * sizeof(int16_t)); 00455 } 00456 traceTrack++; 00457 if(traceTrack>100000000) { 00458 traceTrack = 0; 00459 } 00460 if(debug) { 00461 printf("<================== Trace Count is %ld ===============>\n",traceTrack); 00462 } 00463 } 00464 } 00465 00466 free(rawData); 00467 } 00468 } 00469 } 00470 } 00471 00472 /////////////////////////////////////////////////////////////////////////////////////// 00473 /// 00474 /// PFP Main code end 00475 /// 00476 ////////////////////////////////////////////////////////////////////////////////////// 00477 00478 int main(void) 00479 { 00480 printf("Connecting to the network using the default network interface...\n"); 00481 net = NetworkInterface::get_default_instance(); 00482 00483 nsapi_error_t net_status = NSAPI_ERROR_NO_CONNECTION; 00484 while ((net_status = net->connect()) != NSAPI_ERROR_OK) { 00485 printf("Unable to connect to network (%d). Retrying...\n", net_status); 00486 } 00487 00488 Thread thread(startEmonThread); 00489 00490 while (true) { 00491 wait(0.001); 00492 } 00493 } 00494 00495 #endif /* MBED_TEST_MODE */
Generated on Tue Jul 12 2022 11:18:21 by
1.7.2